SLAM机器人开发（一）系统框图

自主导航与控制模块（上位机）

SLAM机器人自主导航与控制模块是基于 Ubuntu 16.04 和 ROS Kinetic 系统而设计的。 Nav_Control 模块主要由十个节点构成，其中 RealSense node 和 STM32 node 作为 Linux 驱动分别 与 RealSense D415 和 STM32 单片机进行通讯。

VO node，即视觉综合里程计模块，是整个 Nav_Control 模块中最为重要的 node。VO node 从 RealSense node 获取相机数据，包括深度图和 彩色图，从轮速-惯导里程计 Ode node 获取里程计数据，包括物流机器人坐标系下的 XYZ轴线速度、 路程和绕 XYZ 轴转动的角度（yaw，roll，pitch）、角速度。
然后与回环检测模块 Loop_Detec node 一起根据获取的数据估计物流机器人的位姿和局部地图数据，将其发送给后端优化模块 BA node 和路径规划模块 Planner node。BA node 对前端得到的数据进行非线性优化，生成全局地图数据发送给图像转换模块 Pcmap2Octomap node，同时将更加精确的轨迹数据发送给 Planner node。 给定一个目标位姿，Planner node 将使用 3D-RRTstar 算法进行路径规划，从轨迹数据获得初始位姿，然后获取 Pcmap2Octomap node 的八叉树地图数据并在八叉树地图上进行规划出一条满意解。

最后 VP_Control node 获取物流机器人的实际位姿和预期位姿数据，通过四环串级控制算法使物流机器人达到预期位姿。其中，在 VP_Control node 中只实现位置环和速度环的控制算法， 另外两环是对电机的转速、电流双闭环控制，在 STM32 中实现。

自主导航与控制模块（下位机）

电机转速闭环控制根据单位时间内编码器获取的脉冲数测量电机的速度信息，并与目标值进行比较，得到控制偏差；然后通过对比偏差的比例、积分、微分进行控制，使偏差趋向于零。同时STM32将IMU信息处理后传输给上位机。