在我们先前的文章中提到多机器人协同算法（基于一致性算法的分布式时间协同制导律），理论很美好，现实却很骨感，无人机协同控制实验的调试过程中，某个环节出现问题就会导致炸机，且多无人机的集群测试还得去室外，受到天气、场地等诸多因素的阻碍。经常遇到实验无法进行的情况怎么办？不妨换个思路，从空中转移到地面，先用地面小车集群做实验，调试协同控制算法，验证成功后再应用到无人机集群上。小车，几乎是每一个无人机开发者的必经之路，本文将介绍小车系统的组成以及在协同控制实验中的应用。

▌ 小车系统的组成

1.小车的应用程序

小车的应用程序是整个小车系统最重要的一环，一般为前后台系统和实时操作系统RTOS两种方式：1.前后台系统是在主函数while（1）大循环中嵌套多个任务函数，按顺序循环运行，同时应用中断的方式完成一些突发任务的处理。前后台系统任务执行图如图1，中断服务函数作为前台程序，大循环while(1)作为后台程序，此种方式资源消耗少但实时性差，只能应用于简单的系统。

前后台系统还有一种方式是定义一个基准定时器，如10ms，以这个周期来运行不同的任务，比如IMU一个周期（10ms）运行一次，电机控制两个周期（20ms）运行一次，此种方式能够满足对周期性高的任务需求，但同样无法解决多任务的复杂系统。

2.稍微大一点复杂的嵌入式应用就需要移植实时操作系统RTOS，常见的操作系统有μs/os、FreeRTOS等。在使用 RTOS 的时候，一个实时应用可以作为一个独立的任务，每个任务都有自己的运行环境，不依赖于系统中其他的任务或者RTOS调度器。RTOS的任务执行如图2。

笔者选用FreeRTOS操作系统，FreeRTOS内核支持抢占式、协作式和混合式调度，在固定优先级抢占式调度中，具有相同优先级的任务使用循环策略（Round-Robin）进行调度，支持任意数量的任务且能够非常有效地完成任务间切换。FreeRTOS支持任务的静态和动态（在运行时更改）优先级，具有二进制、计数和递归信号量，用于资源保护和同步的互斥体，以及用于在任务之间传递消息的队列[3]。

2.麦克纳姆轮的全向运动

为了满足全向移动的控制需求，实验选择四麦轮小车。麦克纳姆轮(Mecanum Wheel)， 其由轮毂和固定在外周的许多小辊子构成，轮轴和辊轴之间的夹角通常为45°，如下图3所示。每个轮子具有三个自由度，分别是绕轮轴转动，沿垂直于与地面接触的辊子的辊轴方向移动，绕轮子和地面的接触点转动，理论上只要有3个这样的轮子组成的移动平台便可实现全向移动。麦克纳姆轮的运动几何学分析可参考论文Geometry and kinematics of the Mecanum wheel [4]，对四轮麦轮小车进行运动学分析，由四个轮子的实时速度求出三轴实时速度，最后转化成PWM输出到电机，部分代码如下：

3.PID控制

小车的控制采用PID算法，期望为上位机发过来的vicon速度数据，反馈为电机编码器的实时速度。笔者小车采用单极PID控制，飞控为PID控制的无人机算法一般采用的都是串级PID，即通过一个位置速度环的串级PID得到加速度（位置→速度→加速度），将加速度转换成姿态角再通过一个串级PID得到角加速度（姿态角→角速度→角加速度），最后转换成PWM输出到电机。PID算法如下：

▌小车在协同控制实验中的应用

1.通信与组网

笔者在实验中采用了两种通信方式来操控小车，分别是WiFi组网通信和PS2手柄遥控控制。根据实验需求，小车需要与上位机电脑保持实时通信，以及接收vicon定位的位置数据，采用WiFi通信最为合理。但应用中需要注意WiFi的带宽，采用UDP一对多的通信方式，一般不要超过10个节点,UDP通信原理如下图4所示[5]，建立socket套接字后，绑定地址和端口，通过sendto()和recvfrom()两个函数进行数据通信。

同时，在实验调试的过程中，为了便于操控小车，让小车回到原点或目标位置，使用PS2无线遥控手柄，其接收机通过SPI与小车微处理器进行数据通信，编写程序将PS2遥控手柄的控制优先级设置为最高，在小车失控的时候，PS2遥控手柄能够随时接管控制。2.功能性拓展为了提高实验的可观性，在每辆小车上安装30颗全彩LED灯串联的WS2812B灯环，用以显示小车的实时控制量，以及开机时的电量。该全彩LED灯的的通讯方式为单线归零码，像素点在上电复位以后，DIN端接收数据，第一个像素点提取前24bit数据，存放到内部的数据锁存器，剩余的数据经过内部整形处理电路整形放大后通过DO端口转发输出给下一个级联的像素点，每经过一个像素点的传输，信号减少24bit。像素点采用自动整形转发技术，使得该像素点的级联个数不受信号传送的限制，仅受限信号传输速度要求。为了模拟出控制LED灯的单线归零码，采用TIM+DMA+PWM的方式，通过定时器设置其周期T，再利用PWM模拟其0码和1码，通过DMA将数据传送出去。其效果如下图5。

由于编队实验需要保证每辆小车的朝向一致，笔者通过计算所有小车的偏移角进行姿态初始化校准，姿态初始化校准任务在小车启动后，往前车头方向行驶固定距离，全局坐标下会得到一个三角形，即期望行驶的位移X1、实际行驶的位移X2、偏移量Y，通过计算其夹角θ修正位姿的偏移，示意图如图6。

对小车系统组成及在集群控制中应用就介绍到这，有兴趣的同学可以在下方留言交流哦！

参考文献

[1] https://zhuanlan.zhihu.com/p/416392562

[2] https://blog.csdn.net/qq_44318582/article/details/120153001

[3] R. Inam, J. Mäki-Turja, M. Sjödin, S. M. H. Ashjaei and S. Afshar, "Support for hierarchical scheduling in FreeRTOS," ETFA2011, 2011, pp. 1-10, doi: 10.1109/ETFA.2011.6059016.

[4] A. Gfrerrer,Geometry and kinematics of the Mecanum wheel,Computer Aided Geometric Design,Volume 25, Issue 9,2008,Pages 784-791,ISSN 0167-8396

[5] Kiam Heong Ang, G. Chong and Yun Li, "PID control system analysis, design, and technology," in IEEE Transactions on Control Systems Technology, vol. 13, no. 4, pp. 559-576, July 2005, doi: 10.1109/TCST.2005.847331.

本文共2175字

由西湖大学智能无人系统实验室工程师李勇奇原创

申请文章授权请联系后台相关运营人员

▌微信公众号：空中机器人前沿

▌知乎：空中机器人前沿（本文链接：https://zhuanlan.zhihu.com/p/552404862）

▌Youtube：Aerial robotics @ Westlake University

▌实验室网站：https://shiyuzhao.westlake.edu.cn/