本篇主要调研如何实现无人车/无人机的轨迹跟踪控制,解决如下几个问题:
1. UGV/UAV中的控制系统的架构及实现流程
在机器人、无人驾驶以及无人机的自主导航中,均包括有感知定位规划控制四个部分,其中规划在自主导航系统中扮演行为决策、路径生成、轨迹优化的角色,而控制则关注如何使机器人按照设定的轨迹移动。如果规划只考虑路径规划,该部分以各式各样的地图(感知结果)、目标(人为设定)、当前状态信息(定位结果)作为输入,以带有时间信息的轨迹或者期望的速度指令作为输出。输出的轨迹信息/速度指令作为控制模块的输入,在控制算法的作用下,结合系统底盘的运动学模型进行正解(将系统期望速度解算为每个车轮的速度,反之,逆解为将电机编码器得到的车轮速度计算为底盘速度——航迹推演)。
在搭建一个能够实现自主导航的机器人系统时,在硬件上,最常使用的方案是使用一个上位机(如树莓派、jetson nano等)实现感知定位规划算法,并使用一个下位机(如DSP、单片机等)实现对底盘的底层控制,两者采用一定的通信方案。从软件架构上分析:
- 底层对应底盘控制,该部分通过串口接受上位机输出的期望速度(或已经正解得到的左右轮速度),并根据控制算法对左右轮进行控制。此外,定时采样电机码盘值,并通过串口传给上位机(或在下位机完成逆解过程),以便形成反馈。速度指令的正/逆解放在上位机还是下位机取决于设计者,不同存放位置只是导致通信层传输的数据不同,不会影响最终效果。
- 中间层为需要完成上位机与下位机的控制通信,且需要将其它传感器采集的信息发布给ROS。一方面,需要通过串口采集左右轮速度(机器人速度)并发布/odom话题,从而使ROS能订阅到,进行位置估计。另一方面,需要接收ROS发出的期望速度的控制指令,并通过串口传输给下位机。
- 决策层即上位机,用于完成定位、建图、决策、规划等功能,并将规划结果发布。上下位机的系统方案不是绝对的,也可以仅使用主控完成。(据说,下位机用于满足机器人对控制实时性的要求,上位机实现不要需求实时性而追求性能的高级任务,且早期下位机承担电机驱动和运动学解算的任务。)
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控制。所以关键其实是对于规划与控制模块的接口设计。