软件篇-05-融合ORB_SLAM2和IMU闭环控制SLAM底盘运动轨迹

前面我们已经得到了当前底盘在世界坐标系中的位姿，这个位姿是通过融合ORB_SLAM2位姿和IMU积分得到的，在当前位姿已知的case下，给SLAM小车设置一个goal，我这里是通过上位机设置，然后使用上篇文章中的轨迹规划方法就可以得到一个路径Queue。

依次执行这个队列里的离散点，就可以到达设置的目标点了。本篇文章主要内容分享一下我是如何通过闭环控制使得小车在实际环境中从A点运动到目标点B点，才疏学浅，抛砖引玉。本文没有使用ROS里的Navigation包，而是自己封装了一个Navigaton类，集成了上篇文章里的轨迹规划和这篇里的运动控制，这样方便更好的自定义控制算法。

运动控制逻辑框架

在一个二维平面中，小车从A点运动到B点主要有两种方式，一是在A点原地旋转到车头朝向B点，然后径直走向B点；二是从A到B沿着弧线走。两者的区别就像是，图（1）是纯旋转和位移的解耦，图（2）则是两者的耦合。

红点是当前点，蓝色是目标点，图例标反了

Visio画得图有点丑，大家将就点看。这两种方案我之前纠结了一会要用哪个，一方面现实中的车辆都是如（2）中所示转弯的，这种方式也很自然，但是相比与（1）走的路径也会更长一点，另外如果以我这个小车底盘原地转向肯定是要有滑动摩擦的，但是我的小车是差速转向没有特殊的转向轮，这样一来也不是很好控制。最终还是决定使用图（1）中的方式。

闭环控制

其实系统在下位机那一层已经有一个速度环了。另外底层还有一个速度平滑队列，虽然不是但是可以抽象为电流环（速度整的越平滑，速度就增大或减小的越慢，也就是加速度越小，加速度直接和电机输出的扭矩挂钩，又有直流电机M = K*I）。

那么我们在TX2上要做的就是位置环了。

广义位置包括3个自由度的旋转和3个自由度的位移，旋转就是第一步中的先旋转底盘到小车朝向B点，位移就是沿着直线运动到B点了。我这里使用之前得到的小车位姿作为传感器反馈的真实实际值，使用位置式PID，实际上没有用到I，就叫PD控制器吧。

要控制的东西有两个：

旋转过程中旋转的角度；
直线运动中小车的朝向以及当前位置和B点的距离。

其中过程1很大的取决了最后的运动精度，过程2中虽然也在直线运动中不断矫正小车的朝向，但还是没有直接把过程1弄的更准确来的方便。

下层单片机中断接收TX2的速度命令cmd_vel，TX2和stm32通讯频率设置为25HZ。

bool fromPoseCmdvel(geometry_msgs::PoseStamped);
bool setTargetSpeed(float vLeft, float vRight, uint8_t direction);
bool setTargetOmega(float omega, float omegaBias);
bool posePidController(float targetAngle, float currentAngle);
bool posePidController(float target_x, float target_y, float current_x, float current_y, float targetAngle, float currentAngle);
float omegaPidController(float omegaTarget, float omegaActual);
float thetaPidController(float thetaTarget, float thetaActual);bool fromPoseCmdvel(geometry_msgs::PoseStamped);