在我們先前的文章中提到多機器人協(xié)同算法（基于一致性算法的分布式時間協(xié)同制導律），理論很美好，現(xiàn)實卻很骨感，無人機協(xié)同控制實驗的調(diào)試過程中，某個環(huán)節(jié)出現(xiàn)問題就會導致炸機，且多無人機的集群測試還得去室外，受到天氣、場地等諸多因素的阻礙。經(jīng)常遇到實驗無法進行的情況怎么辦？不妨換個思路，從空中轉(zhuǎn)移到地面，先用地面小車集群做實驗，調(diào)試協(xié)同控制算法，驗證成功后再應用到無人機集群上。小車，幾乎是每一個無人機開發(fā)者的必經(jīng)之路，本文將介紹小車系統(tǒng)的組成以及在協(xié)同控制實驗中的應用。

▌ 小車系統(tǒng)的組成

1.小車的應用程序

小車的應用程序是整個小車系統(tǒng)最重要的一環(huán)，一般為前后臺系統(tǒng)和實時操作系統(tǒng)RTOS兩種方式：1.前后臺系統(tǒng)是在主函數(shù)while（1）大循環(huán)中嵌套多個任務函數(shù)，按順序循環(huán)運行，同時應用中斷的方式完成一些突發(fā)任務的處理。前后臺系統(tǒng)任務執(zhí)行圖如圖1，中斷服務函數(shù)作為前臺程序，大循環(huán)while(1)作為后臺程序，此種方式資源消耗少但實時性差，只能應用于簡單的系統(tǒng)。

前后臺系統(tǒng)還有一種方式是定義一個基準定時器，如10ms，以這個周期來運行不同的任務，比如IMU一個周期（10ms）運行一次，電機控制兩個周期（20ms）運行一次，此種方式能夠滿足對周期性高的任務需求，但同樣無法解決多任務的復雜系統(tǒng)。

2.稍微大一點復雜的嵌入式應用就需要移植實時操作系統(tǒng)RTOS，常見的操作系統(tǒng)有μs/os、FreeRTOS等。在使用 RTOS 的時候，一個實時應用可以作為一個獨立的任務，每個任務都有自己的運行環(huán)境，不依賴于系統(tǒng)中其他的任務或者RTOS調(diào)度器。RTOS的任務執(zhí)行如圖2。

筆者選用FreeRTOS操作系統(tǒng)，F(xiàn)reeRTOS內(nèi)核支持搶占式、協(xié)作式和混合式調(diào)度，在固定優(yōu)先級搶占式調(diào)度中，具有相同優(yōu)先級的任務使用循環(huán)策略（Round-Robin）進行調(diào)度，支持任意數(shù)量的任務且能夠非常有效地完成任務間切換。FreeRTOS支持任務的靜態(tài)和動態(tài)（在運行時更改）優(yōu)先級，具有二進制、計數(shù)和遞歸信號量，用于資源保護和同步的互斥體，以及用于在任務之間傳遞消息的隊列[3]。

2.麥克納姆輪的全向運動

為了滿足全向移動的控制需求，實驗選擇四麥輪小車。麥克納姆輪(Mecanum Wheel)， 其由輪轂和固定在外周的許多小輥子構(gòu)成，輪軸和輥軸之間的夾角通常為45°，如下圖3所示。每個輪子具有三個自由度，分別是繞輪軸轉(zhuǎn)動，沿垂直于與地面接觸的輥子的輥軸方向移動，繞輪子和地面的接觸點轉(zhuǎn)動，理論上只要有3個這樣的輪子組成的移動平臺便可實現(xiàn)全向移動。麥克納姆輪的運動幾何學分析可參考論文Geometry and kinematics of the Mecanum wheel [4]，對四輪麥輪小車進行運動學分析，由四個輪子的實時速度求出三軸實時速度，最后轉(zhuǎn)化成PWM輸出到電機，部分代碼如下：

3.PID控制

小車的控制采用PID算法，期望為上位機發(fā)過來的vicon速度數(shù)據(jù)，反饋為電機編碼器的實時速度。筆者小車采用單極PID控制，飛控為PID控制的無人機算法一般采用的都是串級PID，即通過一個位置速度環(huán)的串級PID得到加速度（位置→速度→加速度），將加速度轉(zhuǎn)換成姿態(tài)角再通過一個串級PID得到角加速度（姿態(tài)角→角速度→角加速度），最后轉(zhuǎn)換成PWM輸出到電機。PID算法如下：

▌小車在協(xié)同控制實驗中的應用

1.通信與組網(wǎng)

筆者在實驗中采用了兩種通信方式來操控小車，分別是WiFi組網(wǎng)通信和PS2手柄遙控控制。根據(jù)實驗需求，小車需要與上位機電腦保持實時通信，以及接收vicon定位的位置數(shù)據(jù)，采用WiFi通信最為合理。但應用中需要注意WiFi的帶寬，采用UDP一對多的通信方式，一般不要超過10個節(jié)點,UDP通信原理如下圖4所示[5]，建立socket套接字后，綁定地址和端口，通過sendto()和recvfrom()兩個函數(shù)進行數(shù)據(jù)通信。

同時，在實驗調(diào)試的過程中，為了便于操控小車，讓小車回到原點或目標位置，使用PS2無線遙控手柄，其接收機通過SPI與小車微處理器進行數(shù)據(jù)通信，編寫程序?qū)S2遙控手柄的控制優(yōu)先級設(shè)置為最高，在小車失控的時候，PS2遙控手柄能夠隨時接管控制。2.功能性拓展為了提高實驗的可觀性，在每輛小車上安裝30顆全彩LED燈串聯(lián)的WS2812B燈環(huán)，用以顯示小車的實時控制量，以及開機時的電量。該全彩LED燈的的通訊方式為單線歸零碼，像素點在上電復位以后，DIN端接收數(shù)據(jù)，第一個像素點提取前24bit數(shù)據(jù)，存放到內(nèi)部的數(shù)據(jù)鎖存器，剩余的數(shù)據(jù)經(jīng)過內(nèi)部整形處理電路整形放大后通過DO端口轉(zhuǎn)發(fā)輸出給下一個級聯(lián)的像素點，每經(jīng)過一個像素點的傳輸，信號減少24bit。像素點采用自動整形轉(zhuǎn)發(fā)技術(shù)，使得該像素點的級聯(lián)個數(shù)不受信號傳送的限制，僅受限信號傳輸速度要求。為了模擬出控制LED燈的單線歸零碼，采用TIM+DMA+PWM的方式，通過定時器設(shè)置其周期T，再利用PWM模擬其0碼和1碼，通過DMA將數(shù)據(jù)傳送出去。其效果如下圖5。

由于編隊實驗需要保證每輛小車的朝向一致，筆者通過計算所有小車的偏移角進行姿態(tài)初始化校準，姿態(tài)初始化校準任務在小車啟動后，往前車頭方向行駛固定距離，全局坐標下會得到一個三角形，即期望行駛的位移X1、實際行駛的位移X2、偏移量Y，通過計算其夾角θ修正位姿的偏移，示意圖如圖6。

對小車系統(tǒng)組成及在集群控制中應用就介紹到這，有興趣的同學可以在下方留言交流哦！

參考文獻

[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字

由西湖大學智能無人系統(tǒng)實驗室工程師李勇奇原創(chuàng)

申請文章授權(quán)請聯(lián)系后臺相關(guān)運營人員

▌微信公眾號：空中機器人前沿

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

▌Youtube：Aerial robotics @ Westlake University

▌實驗室網(wǎng)站：https://shiyuzhao.westlake.edu.cn/? ??