基于多信號融合的競技機器人控制研究

朱梅　賴日海　羅昭明　樊中奎

摘? 要：基于機器人大賽制作競技機器人的需要，提出一種機器人軟/硬件控制方案。該方案以STM32F407IGX開發板作為主控，由M3508無刷電機構成底盤驅動模塊、M6020無刷電機構成云臺驅動模塊。根據機器人各個通信模塊的數據傳輸特點及造價，采用CAN、SPI、IIC及UART等多信號融合組成通信系統。底盤模塊設計側重于移動功能，通過矢量分解公式實現全向移動，通過位置式PID算法閉環控制底盤電機轉速；云臺模塊設計側重Yaw軸電機和Pitch軸電機的控制及其他模塊和主控間的通信，結合IMU姿態解算、Kalman濾波及串級PID算法控制云臺電機，通過搭載于云臺的圖傳模塊得到第一人稱視角控制機器人。

關鍵詞：嵌入式算法；STM32F407；通信協議；無刷電機

中圖分類號：TP242? 文獻標識碼：A? 文章編號：2096-4706（2023）20-0022-06

Research on Control for Competitive Robots Based on Multi-signal Fusion

ZHU Mei1， LAI Rihai1， LUO Zhaoming2， FAN Zhongkui1

（1.School of Software Engineering， Jiangxi University of Science and Technology， Nanchang? 330013， China;

2.School of Energy and Mechanical Engineering， Jiangxi University of Science and Technology， Nanchang? 330013， China）

Abstract： Based on the need of making competitive robots in robot competition， a software/hardware control scheme of robot is proposed. In this scheme， STM32F407IGX development board is used as the main control， M3508 brushless motor constitutes the chassis drive module， and M6020 brushless motor constitutes the cradle head drive module. According to the data transmission characteristics and cost of each communication module of the robot， the Controller Area Network （CAN）， Serial Peripheral Interface （SPI）， Inter-Integrated Circuit （IIC）， and Universal Asynchronous Receiver/Transmitter （UART） and other signals fuse together to form a communication system. The chassis module design focuses on mobile function， the omnidirectional movement is realized by vector decomposition formula， and the speed of chassis motor is controlled by position-type PID （Proportion Integral Differential） algorithm in closed loop. The design of cradle head module focuses on the control of Yaw shaft motor and Pitch shaft motor and the communication between other modules and main control. The cradle head motor is controlled by combining IMU attitude calculation， Kalman filtering and cascade PID algorithm. The robot is controlled from the first person perspective through the picture transmission module attached to the cradle head.

Keywords： embedded algorithm; STM32F407; communication protocol; brushless motor

0? 引? 言

機器人競賽是推動機器人技術研發、培養機器人技術工程師的重要方式[1]。近年來，隨著各種機器人競賽的興起，各參賽團體為了能夠在比賽中獲得良好的名次展開了激烈的競爭，對于機器人各方面的研究在不斷地深入，對于相關技術也在不斷地升級突破。機器人控制技術一直是機器人領域的重點研究方向，好的機器人控制方案的使用，不僅是機器人可以正常運行的保障，更是團隊取得優異成績的關鍵[2]。然而在實際設計過程中，很難找到匹配的機器人控制方案，對于不同的任務需求機器人的制作設計常常有很大差異，存在運行性能和開發成本的矛盾。如何設計一種合適的控制系統方案，在既能滿足競賽任務需求的同時花費最小代價，既是機器人競賽的主題，也是競賽團體研究的關鍵。

控制系統包括硬件系統和軟件系統，硬件部分是嵌入式系統的基礎，對嵌入式系統的性能有決定性影響，也決定了平臺的功能和拓展性[3]。主控制板是機器人的控制中樞，選用合適的主控一方面可以降低成本費用，另一方面可以最大程度地滿足機器人對算力的需求；通信系統是機器人協調各個模塊正常運動的關鍵，針對機器人內部不同的通信線路，選擇合適的通信外設可以提高通信效率、匹配該模塊的要求響應速度以及降低開發成本。機器人的軟件設計方案質量的優劣也會直接影響到機器人應用層質量的優劣與模塊穩定性，其中各重要模塊電機的控制方案更是軟件設計方案中的關鍵問題之一。為了保證電機控制的精確性和穩定性，即電機能夠快速準確地響應輸入，不會出現抖動、瘋轉或失控等異常現象，需選取和設計一套合適的電機控制方案，其控制邏輯能夠很好地滿足比賽需求、易于維護和拓展，其控制算法能夠在運用中很好地發揮出作用。

針對競技機器人的控制方案展開研究，主要將從硬件層面和軟件層面進行介紹和分析，包括主控制板的選型、機器人通信系統的設計；以及機器人的整體控制邏輯，并對其中關鍵模塊展開討論，介紹使用的嵌入式算法，并結合實際情況探討可行性。

1? 硬件設計

1.1? 硬件設計思路

如圖1所示，硬件控制方案以云臺作為控制中樞，搭載主控制板通過CAN信號控制云臺和底盤的電機及通過UART信號其他模塊，主控內部間的通信采用IIC、SPI通信，主控和外設間的通信采用UART通信，主控和電機間的通信采用CAN通信，底盤和云臺需要分別設計轉接控制板起擴展、轉接信號和供電的作用。放置于底盤的電源通過該轉接控制板將電流輸送給主控，主控對底盤的控制信號經過一條CAN線路，通過轉接控制板分散給四個M3508電機，云臺轉接板僅負責云臺電機的接口擴展，在云臺電機和主控之間傳遞信號和供電。

1.2? 主控模塊選型及分析

鑒于比賽中對于機器人的操作要求和難度越來越高，即對于控制及響應的要求越來越高，因此選用一款高性價比的開發板是滿足機器人快速響應和精確控制的重要硬件支持之一。如圖2所示，選用的主控開發板采用高性能的STM32F407芯片，支持寬電壓輸入，集成專用的擴展接口，通信接口以及高精度 IMU 傳感器。其具有如下外設：用戶自定義LED、5 V接口、BOOT配置接口、micro USB接口、SWD接口、按鍵、可配置I/O接口、UART接口、CAN總線接口、PWM接口、DBUS接口、數字攝像頭FPC接口、蜂鳴器、電壓檢測ADC、六軸慣性測量單元和磁力計。

1.3? 通信配置及分析

實際開發過程中有多種通信方式可供選擇，如何在眾多通信方式中選擇一種最優解實現兩個模塊間的通信以達到更好的效果是通信設計的重要討論問題之一。需要從機器人自身的機械結構與通信線路的匹配程度、外設接口數量、數據傳輸內容、軟件實現難易以及價格等多方面綜合考慮。

1.3.1? UART通信協議

UART通信協議是一種較古老的全雙工異步通信協議，由于價格便宜、方便使用且可靠性高仍廣泛應用于嵌入式開發，它支持一段字符的連續傳輸，因此特別適合用于一次性傳輸大量數據串的場景。UART數據傳輸主要通過中斷或直接內存存取（direct memory access， DMA）的方式實現[4，5]，如果對于傳輸速度沒有很高的要求，UART通信無疑是十分合適的選擇。遙控器對機器人的控制是通過和UART通信極其相似的DBUS通信協議進行控制信號的傳輸，每次傳輸的一幀有18個字節的數據，由于其傳輸數據量大的需要，采用UART協議作為控制信號傳輸方案，并配置DMA直接存取極大節約了CPU的讀取時間。

1.3.2? CAN通信協議

CAN通信是一種串行通信總線方式，其傳輸速度高達1 M/s，CAN總線因具有實時性強、傳輸距離較遠、抗電磁干擾能力好、成本低、檢錯能力強等優點，在現場總線中應用廣泛[6]。總線一幀攜帶的數據達8個字節，擁有較高數據承載能力。機器人的驅動需要多個電機的同時配合、快速響應和精確控制，并且能夠承受機器人在運動過程中可能受到的各種干擾信號，電機反饋給主控的數據較多，根據CAN協議的主要特點，故而對于主控模塊與電機的通信使用CAN協議。

1.3.3? SPI通信協議

SPI是一種高速全雙工同步通信總線，由設備選擇線（CS）、時鐘線（SCK）、串行輸出數據線（MOSI）、串行輸入數據線（MISO）構成，主要是在SCK的控制下進行雙向數據交換[7]。SPI通信總線一次能傳輸一個字節的數據，適用于對數據傳輸量小但速度要求高的通信電路。BMI088高性能慣性測量單元是本方案采納的主控制板中內置的一款傳感器，它需要實時將測量的角度數據發送給主控以便調整姿態，由于其傳輸量小但速度要求高的特點，故而采用SPI協議。

1.3.4? IIC通信協議

IIC協議是一種半雙工串行同步通信總線協議，具有兩根信號線，一根是雙向的數據傳輸線SDA，另一個是時鐘線SCL，通過對SCL和SDA線高低電平時序的控制，產生IIC協議所需要的信號進行數據的傳遞[8]，具有接口少，控制方式簡單，通信速率較高等優點[9]。機器人主控內置IST8310磁力計模塊，需要發送給CPU磁偏角，并且主控模塊內置溫度傳感器及ADC采集等監控機器人狀態。此類傳感器的數據傳輸量小、速度低，但數量較多，使用IIC協議既能夠滿足其正常的數據傳輸需求，又能減小其他性能高的外設的承載負擔。

2? 軟件設計

2.1? 軟件整體設計

軟件設計的主要任務是搭建框架、完成邏輯設計以及算法設計。框架的搭建借助軟件快速生成實現底層以及中間層的工程目錄，從而極大提高軟件開發效率；基于此框架編寫應用層代碼，即完成邏輯設計，機器人需要根據控制信號完成各種操作，不僅僅是給電機固定不變的電流，對每個電機根據其需要實現的功能設計相應的一套控制邏輯；僅僅依靠邏輯程序很難做到如臂使指地控制電機，這勢必會影響機器人速度的穩定性，或導致云臺轉動無力及高頻振蕩等影響機器人正常運行并損害電機的現象發生，故機器人的控制層需配合算法以保證機器人的正常穩定運行。軟件總體結構如圖3所示。

2.2? 底盤控制方案

2.2.1? 移動方案設計

機器人移動功能由四個M3508電機完成。遙控器給機器人發送一個向任意方向的某個速度的控制信號后，主控不會直接將該信號解析后發送給底盤電機讓其以該數值轉動，而是需要事先完成發送給機器人的原生信號到對單個電機控制信號的轉換。由于四個全向輪的所處位置、安裝方式的差異，每個車輪加上驅動電機都是一個獨立的控制單元，每個單元需要對應的速度才能讓機器人按指定速度移動。而電機的速度對應給電機的電流，因此可以把實際上的速度控制轉變為給輸出電流控制，如何從遙控器實時傳回的數據中分別計算出四個電機的電流值是該模塊軟件設計重點，具體實現流程如圖4所示。

所制作的機器人是在地面移動的，因此可以將地面看作為一個以機器人為中心，以機器人正前方向為y軸，正右方向為x軸的動態二維坐標系，機器人朝某一方向的速度可以看作一個起點在中心的速度矢量，前面提到，每個電機都是一個獨立的控制單元，同樣分別需要四個獨立的坐標系及速度矢量。大坐標系的矢量速度，經過矢量分解得到四個小坐標系的速度矢量。圖5是底盤分解公式，假定給機器人一個方向上的速度，以其為例進行分析，首先通過式（1）得到機器人整體速度在x軸和y軸上的分速度：

Vx = V cosα，Vy = V sinα? ? ? ? ? ? ? ? ? （1）

其中，遙控器給機器人的速度為V，V與x軸的夾角為α，通過三角函數得到機器人在x軸分速度Vx和y軸分速度Vy。根據Vx和Vy分別求出四個電機速度：

Vleftup =? + Vx + Vy? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（2）

Vleftup是左上位置電機速度，由圖示電機安裝位置可知電機需要順時針轉動，因此Vx和Vy都是正方向上的矢量相加。

Vrightup =? + Vx - Vy? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? （3）

Vrightup是右上位置電機速度，由圖示電機安裝位置可知電機需要逆時針轉動，因此Vx是正方向，Vy是負方向。

Vleftdown =? - Vx + Vy? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（4）

Vleftdown是左下位置電機速度，由圖示電機安裝位置可知電機需要順時針轉動，因此Vx是負方向，Vy是正方向。

Vrightdown =? - Vx - Vy? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（5）

Vrightdown是右下位置電機速度，由圖示電機安裝位置可知電機需要逆時針轉動，因此Vx和Vy都是負方向上的矢量相加。

2.2.2? 底盤算法設計

得出各個電機的設定速度后，在實際測試中，如果僅僅依靠設定速度給電機一定的電流，通常達不到預期速度值，甚至速度會時快時滿，這是因為沒有考慮到地面阻力的影響。如何考慮上阻力因素的影響，讓實際速度保持在一個較恒定的、極其接近設定速度的范圍內，這就需要閉環控制，對此采取位置式PID算法，在給每個電機一個電流值前，需要先獲取到電機當前實際速度，再比對設定速度得出二者間的誤差，最后計算出應發送給電機的電流值，具體如何計算出電流值是PID算法的核心內容。

PID控制器正如其名，有三個控制系數P：比例、I：積分、D：微分。相比于其他簡單的控制運算，PID控制器可根據歷史數據及差別的出現率來調整輸入值，這樣可使系統更加準確，更加穩定[10]。誤差在PID控制器中指設定目標值和實際目標值的查。比例系數的作用是根據誤差值大小，按一定比例調節控制量，使系統快速響應。但是比例作用不能消除穩態誤差。積分系數的作用是根據誤差的累積量調節控制量，使系統達到穩態，從而解決穩態誤差問題，但積分會導致超調和震蕩。微分作用是根據偏差的變化率，預測未來的趨勢，調節控制量，使系統更加平穩，但是微分作用會放大噪聲和干擾。PID控制算法的輸出結果是三個控制系數處理結果之和，故算法要解決的是如何得出比例輸出值、積分輸出值及微分輸出值。以下是算法核心式，首先通過式（6）求得誤差：

error = Inset - Inrel? ? ? ? ? ? ? ? ? ? ? （6）

error是設定量Inset和實際量Inrel的偏差量（誤差），即通過相減得出，反映了目標設定量和實際測量值的差距。

Pout = P×error? ? ? ? ? ? ? ? ? ? ? ? ?（7）

Pout是比例輸出值，其和誤差呈線性關系，其值是一個比例系數P與error的乘積，比例輸出值體現了響應的快慢程度。

Iout + = I×error? ? ? ? ? ? ? ? ? ? ? ? （8）

Iout是積分輸出值，是積分輸出值和本次積分系數I與誤差間乘積的相加，程序中Iout的積分求和是離散的，每次執行到該條語句進行一次積分值相加。

Dout = D（errorn - errorn-1）? ? ? ? ? ? ? ? ?（9）

設本次是第n次執行，Dout是微分輸出值，即微分系數D和此次運算誤差errorn與上一次誤差errorn-1間差的乘積。

out = Pout + Iout + Dout? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? （10）

out是PID算法的最終輸出，是式（7）（8）和（9）分別求得的Pout、Iout和Dout的求和。

2.3? 云臺控制方案

2.3.1? 轉動策略

云臺是機器人的控制中樞，并搭載視覺系統，通過客戶端連接到云臺圖傳模塊后，用戶能夠以第一人稱視角操縱機器人，云臺可以在水平上轉動，也可以抬頭和低頭，為展現給用戶穩定的視角，云臺驅動電機的角度控制相比于底盤的速度控制精度要求更高，設計中采用能反饋當前角度的M6020無刷電機作為機器人的云臺驅動模塊，具體控制流程如圖6所示。

機器人云臺可以在Yaw軸0-360度范圍內轉動，但礙于機械結構的限制，在Pitch軸只能在一個角度區間內轉動。用戶給機器人Yaw軸方向上的控制信號，云臺會根據值的大小以相應速度持續轉動；同樣，當用戶給機器人Pitch軸方向上的控制信號，云臺也會持續轉動直到達到限制邊界。這其中涉及兩個技術點：如何從360°轉到0°造成的角度突變問題以及如何限制電機在一定范圍內轉動。

角度突變問題在通過PID算法計算角度突變時的計算結果會有極大偏差，容易造成云臺失控快速轉動，其主要原因是從0°到360°的巨大誤差破壞了原本的連續性，故而將從本次的≤360°到下一次的＞0°或從本次的≥0°到下一次的＜360°的突變情況轉換成連續情況：添加一個虛擬設定角度參與到PID計算，如果設定角度＞0°，在PID計算中是從實際角度的≤360°到虛擬設定角度的＞360°，在軟件上云臺轉到了＞360°的位置，但實際上是轉回到＞0°的某個位置，從另一方向上的突變情況同理。而邊界限制問題是防止云臺在Pitch軸上持續轉而不動造成電機燒毀發生，故而需要設定俯仰區間保證電機只能在一定范圍內轉動，當持續施加給電機轉動信號時，如果設定目標值已經超出邊界值，則實際目標值等于邊界值，兩個邊界值根據實際測試結果得出。

2.3.2? 算法設計

云臺電機的輸出值融合三個控制算法：IMU姿態解算將獲取到的主控內置BMI088傳感器的角速度值和角加速度值，將BMI088傳感器的多軸數據從四元數形式變換為歐拉角進行計算[11]，計算得到云臺在Yaw軸、Pitch軸以及Roll軸的姿態信息。姿態角反映了云臺當前的絕對角度，通過姿態角控制云臺電機從而擺脫對底盤的依賴，即不再受地形起伏以及底盤轉動的影響。串行PID算法是以嵌套的方式在速度環的基礎上增加了角度環從而提高了電機的穩定性和響應速度，很適合對控制精度要求極高的云臺電機。

在用PID算法計算出結果前，需要先對設定角度進行濾波，在此用到Kalman濾波器。卡爾曼濾波算法是一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法，它是一種最優化自回歸數據算法，是描述全部計算更新數據的過程，提供一組有效的遞歸推算方程組來估計過程的狀態量，使估計均方差誤差最小化[12]。Kalman濾波器通過對動態系統建立一個模型，假設系統狀態是該模型內的一個變量，通過實時獲取的測量值更新狀態變量的概率分布，從而在含有噪聲干擾的測量值中得出動態系統的最優估計量。假設本次是第k-1次計算，根據式（11）獲取第k次系統狀態的預測值：

其中x表示系統的狀態向量，F表示狀態轉移矩陣，B表示控制矩陣，u表示控制向量。之后通過式（12）估計預測誤差：

P表示誤差協方差矩陣，Q表示過程噪聲協方差矩陣，T表示轉置矩陣。通過式（13）計算卡爾曼增益：

其中K表示卡爾曼增益矩陣，H表示觀測矩陣，R表示觀測噪聲協方差矩陣。之后計算式（14）更新系統的狀態：

其中z表示觀測向量。最后通過式（15）更新誤差協方差矩陣：

其中I表示單位矩陣。至此求得第k次的誤差協方差。

3? 結? 論

提出的機器人控制設計方案詳細闡述了一種競技機器人的硬件與軟件設計過程和使用的策略，包括硬件重要模塊的選型、電路設計以和分模塊化設計控制邏輯、對嵌入式算法的分析及在機器人契合使用。機器人在場地測試和實際比賽過程中都能夠流暢無誤地接收命令并穩定地實現相應功能，充分體現了該機器人控制方案的可行性和可靠性，為機器人競賽提供了一種較為實用的控制方案。

參考文獻：

[1] 王旭.競技機器人比賽裁判原則與技術 [J].機器人技術與應用，2022（2）：43-48.

[2] 宋宇航，婁照坤，任雨婷，等.Robomaster比賽工程機器人控制系統 [J].中國科技信息，2022（15）：44-47.

[3] 劉放，劉蕾.模塊化機器人控制系統的設計與實現 [J].信息技術與信息化，2020（1）：34-36.

[4] 邵偉恒，顧昂，隋俊杰.基于STM32的便攜式恒流源設計 [J].自動化與儀表，2015，30（12）：29-32.

[5] 邢麗華，高志鵬，袁德殿.基于散列DMA的高速串口驅動設計 [J].單片機與嵌入式系統應用，2010（4）：40-42.

[6] 邱小軍，劉瀏，吳超，等.水下機器人通信系統的CAN總線冗余設計 [J].自動化儀表，2012，33（11）：23-26.

[7] 盛楊博嚴.基于SPI總線的Arduino顯示與控制模塊設計 [J].單片機與嵌入式系統應用，2020，20（3）：74-76+80.

[8] 董雪.基于FPGA的SPI與ⅡC總線通信系統的設計與實現 [D].銀川：寧夏大學，2017.

[9] 李斯祺，曾榮鑫.基于STM32的紅外傳感器的數據采集系統的設計 [J].工業控制計算機，2018，31（8）：30-31+33.

[10] 王露峰.基于PID控制的電動汽車直流驅動電機調速研究 [J].汽車實用技術，2020（10）：106-108.

[11] 劉曉宇，孟瑞鋒，賈偉.四旋翼飛行器姿態解算研究及最優化設計 [J].南方農機，2018，49（15）：40-41.

[12] 余爾標，陳宏華.一種有纜式四軸水下機器人的設計與制作 [J].江西電力職業技術學院學報，2022，35（10）：142-144.

作者簡介：朱梅（1980—），女，漢族，江蘇江陰人，中級實驗師，碩士研究生，研究方向：人工智能；賴日海（2001—），男，漢族，江西贛州人，本科在讀，研究方向：機器人控制、軟件開發；樊中奎（1980—），男，漢族，江西南昌人，講師，碩士研究生，研究方向：人工智能。

收稿日期：2023-04-10

基金項目：江西理工大學大學生創新創業訓練資助項目（202110407039）