UGV轨迹跟踪控制学习笔记

本篇主要调研如何实现无人车/无人机的轨迹跟踪控制，解决如下几个问题：

1. UGV/UAV中的控制系统的架构及实现流程

       在机器人、无人驾驶以及无人机的自主导航中，均包括有感知定位规划控制四个部分，其中规划在自主导航系统中扮演行为决策、路径生成、轨迹优化的角色，而控制则关注如何使机器人按照设定的轨迹移动。如果规划只考虑路径规划，该部分以各式各样的地图（感知结果）、目标（人为设定）、当前状态信息（定位结果）作为输入，以带有时间信息的轨迹或者期望的速度指令作为输出。输出的轨迹信息/速度指令作为控制模块的输入，在控制算法的作用下，结合系统底盘的运动学模型进行正解（将系统期望速度解算为每个车轮的速度，反之，逆解为将电机编码器得到的车轮速度计算为底盘速度——航迹推演）。

       在搭建一个能够实现自主导航的机器人系统时，在硬件上，最常使用的方案是使用一个上位机（如树莓派、jetson nano等）实现感知定位规划算法，并使用一个下位机（如DSP、单片机等）实现对底盘的底层控制，两者采用一定的通信方案。从软件架构上分析：

1. 底层对应底盘控制，该部分通过串口接受上位机输出的期望速度（或已经正解得到的左右轮速度），并根据控制算法对左右轮进行控制。此外，定时采样电机码盘值，并通过串口传给上位机（或在下位机完成逆解过程），以便形成反馈。速度指令的正/逆解放在上位机还是下位机取决于设计者，不同存放位置只是导致通信层传输的数据不同，不会影响最终效果。
2. 中间层为需要完成上位机与下位机的控制通信，且需要将其它传感器采集的信息发布给ROS。一方面，需要通过串口采集左右轮速度（机器人速度）并发布/odom话题，从而使ROS能订阅到，进行位置估计。另一方面，需要接收ROS发出的期望速度的控制指令，并通过串口传输给下位机。
3. 决策层即上位机，用于完成定位、建图、决策、规划等功能，并将规划结果发布。上下位机的系统方案不是绝对的，也可以仅使用主控完成。（据说，下位机用于满足机器人对控制实时性的要求，上位机实现不要需求实时性而追求性能的高级任务，且早期下位机承担电机驱动和运动学解算的任务。）

2. move_base中规划层和控制层的设计

        move_base是ROS官方自带的导航包navigation中处理路径规划的相关节点，从wiki官网中可以找到move_bose的工作流程，这里我们关注move_base对控制模块的接口设计。move_base有四个输入，分别是tf、odom、move_base_simple/goal、map、“sensor topics”，而输出只有一个cmd_vel。

        move_base节点接收人工指定的goal，将goal通过actionlib的客户端向服务器发送，服务器根据坐标系之间的tf关系和里程计信息odom不断计算机器人的状态。在move_base接收到指定的goal后，会调用回调函数MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal)将目标点goal的格式转换，随后将目标点goal传入函数MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)，将goal转换到全局坐标系下并publish可视化，唤醒线程planThread以一定频率更新代价地图，该线程调用makplan函数规划全局路径，并且会更新，全局路径会传递到局部路径规划器。原线程会继续执行函数MoveBase::executeCycle，首先获取机器人在world frame下的位姿，传递给feedback反馈和发布，随后调用setplan函数存储全局路径并进行局部路径规划，这里有一个状态机的机制，不展开解释了， 总之计算路径后调用函数computeVelocityCommands计算机器人的速度，输出期望的速度twist到话题cmd_vel上，geometry_msgs/Twist数据格式为

geometry_msgs/Vector3 linear
    float64 x
    float64 y
    float64 z
geometry_msgs/Vector3 angular
    float64 x
    float64 y
    float64 z

        twist中就分别包含了机器人的线速度和角速度。

3. Wheeltec小车的规划控制方案

        Wheeltec小车硬件架构上采用了jetson nano作为上位机，stm32作为下位机的设计，其导航算法采用的也是move_base，控制算法选择对电机速度进行PID控制。所以关键其实是对于规划与控制模块的接口设计。