朱梅　賴(lài)日?！×_昭明　樊中奎

摘? 要：基于機(jī)器人大賽制作競(jìng)技機(jī)器人的需要，提出一種機(jī)器人軟/硬件控制方案。該方案以STM32F407IGX開(kāi)發(fā)板作為主控，由M3508無(wú)刷電機(jī)構(gòu)成底盤(pán)驅(qū)動(dòng)模塊、M6020無(wú)刷電機(jī)構(gòu)成云臺(tái)驅(qū)動(dòng)模塊。根據(jù)機(jī)器人各個(gè)通信模塊的數(shù)據(jù)傳輸特點(diǎn)及造價(jià)，采用CAN、SPI、IIC及UART等多信號(hào)融合組成通信系統(tǒng)。底盤(pán)模塊設(shè)計(jì)側(cè)重于移動(dòng)功能，通過(guò)矢量分解公式實(shí)現(xiàn)全向移動(dòng)，通過(guò)位置式PID算法閉環(huán)控制底盤(pán)電機(jī)轉(zhuǎn)速；云臺(tái)模塊設(shè)計(jì)側(cè)重Yaw軸電機(jī)和Pitch軸電機(jī)的控制及其他模塊和主控間的通信，結(jié)合IMU姿態(tài)解算、Kalman濾波及串級(jí)PID算法控制云臺(tái)電機(jī)，通過(guò)搭載于云臺(tái)的圖傳模塊得到第一人稱(chēng)視角控制機(jī)器人。

關(guān)鍵詞：嵌入式算法；STM32F407；通信協(xié)議；無(wú)刷電機(jī)

中圖分類(lèi)號(hào)：TP242? 文獻(xiàn)標(biāo)識(shí)碼：A? 文章編號(hào)：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? 引? 言

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

控制系統(tǒng)包括硬件系統(tǒng)和軟件系統(tǒng)，硬件部分是嵌入式系統(tǒng)的基礎(chǔ)，對(duì)嵌入式系統(tǒng)的性能有決定性影響，也決定了平臺(tái)的功能和拓展性[3]。主控制板是機(jī)器人的控制中樞，選用合適的主控一方面可以降低成本費(fèi)用，另一方面可以最大程度地滿(mǎn)足機(jī)器人對(duì)算力的需求；通信系統(tǒng)是機(jī)器人協(xié)調(diào)各個(gè)模塊正常運(yùn)動(dòng)的關(guān)鍵，針對(duì)機(jī)器人內(nèi)部不同的通信線(xiàn)路，選擇合適的通信外設(shè)可以提高通信效率、匹配該模塊的要求響應(yīng)速度以及降低開(kāi)發(fā)成本。機(jī)器人的軟件設(shè)計(jì)方案質(zhì)量的優(yōu)劣也會(huì)直接影響到機(jī)器人應(yīng)用層質(zhì)量的優(yōu)劣與模塊穩(wěn)定性，其中各重要模塊電機(jī)的控制方案更是軟件設(shè)計(jì)方案中的關(guān)鍵問(wèn)題之一。為了保證電機(jī)控制的精確性和穩(wěn)定性，即電機(jī)能夠快速準(zhǔn)確地響應(yīng)輸入，不會(huì)出現(xiàn)抖動(dòng)、瘋轉(zhuǎn)或失控等異?，F(xiàn)象，需選取和設(shè)計(jì)一套合適的電機(jī)控制方案，其控制邏輯能夠很好地滿(mǎn)足比賽需求、易于維護(hù)和拓展，其控制算法能夠在運(yùn)用中很好地發(fā)揮出作用。

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

1? 硬件設(shè)計(jì)

1.1? 硬件設(shè)計(jì)思路

如圖1所示，硬件控制方案以云臺(tái)作為控制中樞，搭載主控制板通過(guò)CAN信號(hào)控制云臺(tái)和底盤(pán)的電機(jī)及通過(guò)UART信號(hào)其他模塊，主控內(nèi)部間的通信采用IIC、SPI通信，主控和外設(shè)間的通信采用UART通信，主控和電機(jī)間的通信采用CAN通信，底盤(pán)和云臺(tái)需要分別設(shè)計(jì)轉(zhuǎn)接控制板起擴(kuò)展、轉(zhuǎn)接信號(hào)和供電的作用。放置于底盤(pán)的電源通過(guò)該轉(zhuǎn)接控制板將電流輸送給主控，主控對(duì)底盤(pán)的控制信號(hào)經(jīng)過(guò)一條CAN線(xiàn)路，通過(guò)轉(zhuǎn)接控制板分散給四個(gè)M3508電機(jī)，云臺(tái)轉(zhuǎn)接板僅負(fù)責(zé)云臺(tái)電機(jī)的接口擴(kuò)展，在云臺(tái)電機(jī)和主控之間傳遞信號(hào)和供電。

1.2? 主控模塊選型及分析

鑒于比賽中對(duì)于機(jī)器人的操作要求和難度越來(lái)越高，即對(duì)于控制及響應(yīng)的要求越來(lái)越高，因此選用一款高性?xún)r(jià)比的開(kāi)發(fā)板是滿(mǎn)足機(jī)器人快速響應(yīng)和精確控制的重要硬件支持之一。如圖2所示，選用的主控開(kāi)發(fā)板采用高性能的STM32F407芯片，支持寬電壓輸入，集成專(zhuān)用的擴(kuò)展接口，通信接口以及高精度 IMU 傳感器。其具有如下外設(shè)：用戶(hù)自定義LED、5 V接口、BOOT配置接口、micro USB接口、SWD接口、按鍵、可配置I/O接口、UART接口、CAN總線(xiàn)接口、PWM接口、DBUS接口、數(shù)字?jǐn)z像頭FPC接口、蜂鳴器、電壓檢測(cè)ADC、六軸慣性測(cè)量單元和磁力計(jì)。

1.3? 通信配置及分析

實(shí)際開(kāi)發(fā)過(guò)程中有多種通信方式可供選擇，如何在眾多通信方式中選擇一種最優(yōu)解實(shí)現(xiàn)兩個(gè)模塊間的通信以達(dá)到更好的效果是通信設(shè)計(jì)的重要討論問(wèn)題之一。需要從機(jī)器人自身的機(jī)械結(jié)構(gòu)與通信線(xiàn)路的匹配程度、外設(shè)接口數(shù)量、數(shù)據(jù)傳輸內(nèi)容、軟件實(shí)現(xiàn)難易以及價(jià)格等多方面綜合考慮。

1.3.1? UART通信協(xié)議

UART通信協(xié)議是一種較古老的全雙工異步通信協(xié)議，由于價(jià)格便宜、方便使用且可靠性高仍廣泛應(yīng)用于嵌入式開(kāi)發(fā)，它支持一段字符的連續(xù)傳輸，因此特別適合用于一次性傳輸大量數(shù)據(jù)串的場(chǎng)景。UART數(shù)據(jù)傳輸主要通過(guò)中斷或直接內(nèi)存存取（direct memory access， DMA）的方式實(shí)現(xiàn)[4，5]，如果對(duì)于傳輸速度沒(méi)有很高的要求，UART通信無(wú)疑是十分合適的選擇。遙控器對(duì)機(jī)器人的控制是通過(guò)和UART通信極其相似的DBUS通信協(xié)議進(jìn)行控制信號(hào)的傳輸，每次傳輸?shù)囊粠?8個(gè)字節(jié)的數(shù)據(jù)，由于其傳輸數(shù)據(jù)量大的需要，采用UART協(xié)議作為控制信號(hào)傳輸方案，并配置DMA直接存取極大節(jié)約了CPU的讀取時(shí)間。

1.3.2? CAN通信協(xié)議

CAN通信是一種串行通信總線(xiàn)方式，其傳輸速度高達(dá)1 M/s，CAN總線(xiàn)因具有實(shí)時(shí)性強(qiáng)、傳輸距離較遠(yuǎn)、抗電磁干擾能力好、成本低、檢錯(cuò)能力強(qiáng)等優(yōu)點(diǎn)，在現(xiàn)場(chǎng)總線(xiàn)中應(yīng)用廣泛[6]。總線(xiàn)一幀攜帶的數(shù)據(jù)達(dá)8個(gè)字節(jié)，擁有較高數(shù)據(jù)承載能力。機(jī)器人的驅(qū)動(dòng)需要多個(gè)電機(jī)的同時(shí)配合、快速響應(yīng)和精確控制，并且能夠承受機(jī)器人在運(yùn)動(dòng)過(guò)程中可能受到的各種干擾信號(hào)，電機(jī)反饋給主控的數(shù)據(jù)較多，根據(jù)CAN協(xié)議的主要特點(diǎn)，故而對(duì)于主控模塊與電機(jī)的通信使用CAN協(xié)議。

1.3.3? SPI通信協(xié)議

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

1.3.4? IIC通信協(xié)議

IIC協(xié)議是一種半雙工串行同步通信總線(xiàn)協(xié)議，具有兩根信號(hào)線(xiàn)，一根是雙向的數(shù)據(jù)傳輸線(xiàn)SDA，另一個(gè)是時(shí)鐘線(xiàn)SCL，通過(guò)對(duì)SCL和SDA線(xiàn)高低電平時(shí)序的控制，產(chǎn)生IIC協(xié)議所需要的信號(hào)進(jìn)行數(shù)據(jù)的傳遞[8]，具有接口少，控制方式簡(jiǎn)單，通信速率較高等優(yōu)點(diǎn)[9]。機(jī)器人主控內(nèi)置IST8310磁力計(jì)模塊，需要發(fā)送給CPU磁偏角，并且主控模塊內(nèi)置溫度傳感器及ADC采集等監(jiān)控機(jī)器人狀態(tài)。此類(lèi)傳感器的數(shù)據(jù)傳輸量小、速度低，但數(shù)量較多，使用IIC協(xié)議既能夠滿(mǎn)足其正常的數(shù)據(jù)傳輸需求，又能減小其他性能高的外設(shè)的承載負(fù)擔(dān)。

2? 軟件設(shè)計(jì)

2.1? 軟件整體設(shè)計(jì)

軟件設(shè)計(jì)的主要任務(wù)是搭建框架、完成邏輯設(shè)計(jì)以及算法設(shè)計(jì)。框架的搭建借助軟件快速生成實(shí)現(xiàn)底層以及中間層的工程目錄，從而極大提高軟件開(kāi)發(fā)效率；基于此框架編寫(xiě)應(yīng)用層代碼，即完成邏輯設(shè)計(jì)，機(jī)器人需要根據(jù)控制信號(hào)完成各種操作，不僅僅是給電機(jī)固定不變的電流，對(duì)每個(gè)電機(jī)根據(jù)其需要實(shí)現(xiàn)的功能設(shè)計(jì)相應(yīng)的一套控制邏輯；僅僅依靠邏輯程序很難做到如臂使指地控制電機(jī)，這勢(shì)必會(huì)影響機(jī)器人速度的穩(wěn)定性，或?qū)е略婆_(tái)轉(zhuǎn)動(dòng)無(wú)力及高頻振蕩等影響機(jī)器人正常運(yùn)行并損害電機(jī)的現(xiàn)象發(fā)生，故機(jī)器人的控制層需配合算法以保證機(jī)器人的正常穩(wěn)定運(yùn)行。軟件總體結(jié)構(gòu)如圖3所示。

2.2? 底盤(pán)控制方案

2.2.1? 移動(dòng)方案設(shè)計(jì)

機(jī)器人移動(dòng)功能由四個(gè)M3508電機(jī)完成。遙控器給機(jī)器人發(fā)送一個(gè)向任意方向的某個(gè)速度的控制信號(hào)后，主控不會(huì)直接將該信號(hào)解析后發(fā)送給底盤(pán)電機(jī)讓其以該數(shù)值轉(zhuǎn)動(dòng)，而是需要事先完成發(fā)送給機(jī)器人的原生信號(hào)到對(duì)單個(gè)電機(jī)控制信號(hào)的轉(zhuǎn)換。由于四個(gè)全向輪的所處位置、安裝方式的差異，每個(gè)車(chē)輪加上驅(qū)動(dòng)電機(jī)都是一個(gè)獨(dú)立的控制單元，每個(gè)單元需要對(duì)應(yīng)的速度才能讓機(jī)器人按指定速度移動(dòng)。而電機(jī)的速度對(duì)應(yīng)給電機(jī)的電流，因此可以把實(shí)際上的速度控制轉(zhuǎn)變?yōu)榻o輸出電流控制，如何從遙控器實(shí)時(shí)傳回的數(shù)據(jù)中分別計(jì)算出四個(gè)電機(jī)的電流值是該模塊軟件設(shè)計(jì)重點(diǎn)，具體實(shí)現(xiàn)流程如圖4所示。

所制作的機(jī)器人是在地面移動(dòng)的，因此可以將地面看作為一個(gè)以機(jī)器人為中心，以機(jī)器人正前方向?yàn)閥軸，正右方向?yàn)閤軸的動(dòng)態(tài)二維坐標(biāo)系，機(jī)器人朝某一方向的速度可以看作一個(gè)起點(diǎn)在中心的速度矢量，前面提到，每個(gè)電機(jī)都是一個(gè)獨(dú)立的控制單元，同樣分別需要四個(gè)獨(dú)立的坐標(biāo)系及速度矢量。大坐標(biāo)系的矢量速度，經(jīng)過(guò)矢量分解得到四個(gè)小坐標(biāo)系的速度矢量。圖5是底盤(pán)分解公式，假定給機(jī)器人一個(gè)方向上的速度，以其為例進(jìn)行分析，首先通過(guò)式（1）得到機(jī)器人整體速度在x軸和y軸上的分速度：

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

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

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

Vleftup是左上位置電機(jī)速度，由圖示電機(jī)安裝位置可知電機(jī)需要順時(shí)針轉(zhuǎn)動(dòng)，因此Vx和Vy都是正方向上的矢量相加。

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

Vrightup是右上位置電機(jī)速度，由圖示電機(jī)安裝位置可知電機(jī)需要逆時(shí)針轉(zhuǎn)動(dòng)，因此Vx是正方向，Vy是負(fù)方向。

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

Vleftdown是左下位置電機(jī)速度，由圖示電機(jī)安裝位置可知電機(jī)需要順時(shí)針轉(zhuǎn)動(dòng)，因此Vx是負(fù)方向，Vy是正方向。

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

Vrightdown是右下位置電機(jī)速度，由圖示電機(jī)安裝位置可知電機(jī)需要逆時(shí)針轉(zhuǎn)動(dòng)，因此Vx和Vy都是負(fù)方向上的矢量相加。

2.2.2? 底盤(pán)算法設(shè)計(jì)

得出各個(gè)電機(jī)的設(shè)定速度后，在實(shí)際測(cè)試中，如果僅僅依靠設(shè)定速度給電機(jī)一定的電流，通常達(dá)不到預(yù)期速度值，甚至速度會(huì)時(shí)快時(shí)滿(mǎn)，這是因?yàn)闆](méi)有考慮到地面阻力的影響。如何考慮上阻力因素的影響，讓實(shí)際速度保持在一個(gè)較恒定的、極其接近設(shè)定速度的范圍內(nèi)，這就需要閉環(huán)控制，對(duì)此采取位置式PID算法，在給每個(gè)電機(jī)一個(gè)電流值前，需要先獲取到電機(jī)當(dāng)前實(shí)際速度，再比對(duì)設(shè)定速度得出二者間的誤差，最后計(jì)算出應(yīng)發(fā)送給電機(jī)的電流值，具體如何計(jì)算出電流值是PID算法的核心內(nèi)容。

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

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

error是設(shè)定量Inset和實(shí)際量Inrel的偏差量（誤差），即通過(guò)相減得出，反映了目標(biāo)設(shè)定量和實(shí)際測(cè)量值的差距。

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

Pout是比例輸出值，其和誤差呈線(xiàn)性關(guān)系，其值是一個(gè)比例系數(shù)P與error的乘積，比例輸出值體現(xiàn)了響應(yīng)的快慢程度。

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

Iout是積分輸出值，是積分輸出值和本次積分系數(shù)I與誤差間乘積的相加，程序中Iout的積分求和是離散的，每次執(zhí)行到該條語(yǔ)句進(jìn)行一次積分值相加。

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

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

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

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

2.3? 云臺(tái)控制方案

2.3.1? 轉(zhuǎn)動(dòng)策略

云臺(tái)是機(jī)器人的控制中樞，并搭載視覺(jué)系統(tǒng)，通過(guò)客戶(hù)端連接到云臺(tái)圖傳模塊后，用戶(hù)能夠以第一人稱(chēng)視角操縱機(jī)器人，云臺(tái)可以在水平上轉(zhuǎn)動(dòng)，也可以抬頭和低頭，為展現(xiàn)給用戶(hù)穩(wěn)定的視角，云臺(tái)驅(qū)動(dòng)電機(jī)的角度控制相比于底盤(pán)的速度控制精度要求更高，設(shè)計(jì)中采用能反饋當(dāng)前角度的M6020無(wú)刷電機(jī)作為機(jī)器人的云臺(tái)驅(qū)動(dòng)模塊，具體控制流程如圖6所示。

機(jī)器人云臺(tái)可以在Yaw軸0-360度范圍內(nèi)轉(zhuǎn)動(dòng)，但礙于機(jī)械結(jié)構(gòu)的限制，在Pitch軸只能在一個(gè)角度區(qū)間內(nèi)轉(zhuǎn)動(dòng)。用戶(hù)給機(jī)器人Yaw軸方向上的控制信號(hào)，云臺(tái)會(huì)根據(jù)值的大小以相應(yīng)速度持續(xù)轉(zhuǎn)動(dòng)；同樣，當(dāng)用戶(hù)給機(jī)器人Pitch軸方向上的控制信號(hào)，云臺(tái)也會(huì)持續(xù)轉(zhuǎn)動(dòng)直到達(dá)到限制邊界。這其中涉及兩個(gè)技術(shù)點(diǎn)：如何從360°轉(zhuǎn)到0°造成的角度突變問(wèn)題以及如何限制電機(jī)在一定范圍內(nèi)轉(zhuǎn)動(dòng)。

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

2.3.2? 算法設(shè)計(jì)

云臺(tái)電機(jī)的輸出值融合三個(gè)控制算法：IMU姿態(tài)解算將獲取到的主控內(nèi)置BMI088傳感器的角速度值和角加速度值，將BMI088傳感器的多軸數(shù)據(jù)從四元數(shù)形式變換為歐拉角進(jìn)行計(jì)算[11]，計(jì)算得到云臺(tái)在Yaw軸、Pitch軸以及Roll軸的姿態(tài)信息。姿態(tài)角反映了云臺(tái)當(dāng)前的絕對(duì)角度，通過(guò)姿態(tài)角控制云臺(tái)電機(jī)從而擺脫對(duì)底盤(pán)的依賴(lài)，即不再受地形起伏以及底盤(pán)轉(zhuǎn)動(dòng)的影響。串行PID算法是以嵌套的方式在速度環(huán)的基礎(chǔ)上增加了角度環(huán)從而提高了電機(jī)的穩(wěn)定性和響應(yīng)速度，很適合對(duì)控制精度要求極高的云臺(tái)電機(jī)。

在用PID算法計(jì)算出結(jié)果前，需要先對(duì)設(shè)定角度進(jìn)行濾波，在此用到Kalman濾波器。卡爾曼濾波算法是一種利用線(xiàn)性系統(tǒng)狀態(tài)方程，通過(guò)系統(tǒng)輸入輸出觀測(cè)數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法，它是一種最優(yōu)化自回歸數(shù)據(jù)算法，是描述全部計(jì)算更新數(shù)據(jù)的過(guò)程，提供一組有效的遞歸推算方程組來(lái)估計(jì)過(guò)程的狀態(tài)量，使估計(jì)均方差誤差最小化[12]。Kalman濾波器通過(guò)對(duì)動(dòng)態(tài)系統(tǒng)建立一個(gè)模型，假設(shè)系統(tǒng)狀態(tài)是該模型內(nèi)的一個(gè)變量，通過(guò)實(shí)時(shí)獲取的測(cè)量值更新?tīng)顟B(tài)變量的概率分布，從而在含有噪聲干擾的測(cè)量值中得出動(dòng)態(tài)系統(tǒng)的最優(yōu)估計(jì)量。假設(shè)本次是第k-1次計(jì)算，根據(jù)式（11）獲取第k次系統(tǒng)狀態(tài)的預(yù)測(cè)值：

其中x表示系統(tǒng)的狀態(tài)向量，F(xiàn)表示狀態(tài)轉(zhuǎn)移矩陣，B表示控制矩陣，u表示控制向量。之后通過(guò)式（12）估計(jì)預(yù)測(cè)誤差：

P表示誤差協(xié)方差矩陣，Q表示過(guò)程噪聲協(xié)方差矩陣，T表示轉(zhuǎn)置矩陣。通過(guò)式（13）計(jì)算卡爾曼增益：

其中K表示卡爾曼增益矩陣，H表示觀測(cè)矩陣，R表示觀測(cè)噪聲協(xié)方差矩陣。之后計(jì)算式（14）更新系統(tǒng)的狀態(tài)：

其中z表示觀測(cè)向量。最后通過(guò)式（15）更新誤差協(xié)方差矩陣：

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

3? 結(jié)? 論

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

參考文獻(xiàn)：

[1] 王旭.競(jìng)技機(jī)器人比賽裁判原則與技術(shù) [J].機(jī)器人技術(shù)與應(yīng)用，2022（2）：43-48.

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

[3] 劉放，劉蕾.模塊化機(jī)器人控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn) [J].信息技術(shù)與信息化，2020（1）：34-36.

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

[5] 邢麗華，高志鵬，袁德殿.基于散列DMA的高速串口驅(qū)動(dòng)設(shè)計(jì) [J].單片機(jī)與嵌入式系統(tǒng)應(yīng)用，2010（4）：40-42.

[6] 邱小軍，劉瀏，吳超，等.水下機(jī)器人通信系統(tǒng)的CAN總線(xiàn)冗余設(shè)計(jì) [J].自動(dòng)化儀表，2012，33（11）：23-26.

[7] 盛楊博嚴(yán).基于SPI總線(xiàn)的Arduino顯示與控制模塊設(shè)計(jì) [J].單片機(jī)與嵌入式系統(tǒng)應(yīng)用，2020，20（3）：74-76+80.

[8] 董雪.基于FPGA的SPI與ⅡC總線(xiàn)通信系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn) [D].銀川：寧夏大學(xué)，2017.

[9] 李斯祺，曾榮鑫.基于STM32的紅外傳感器的數(shù)據(jù)采集系統(tǒng)的設(shè)計(jì) [J].工業(yè)控制計(jì)算機(jī)，2018，31（8）：30-31+33.

[10] 王露峰.基于PID控制的電動(dòng)汽車(chē)直流驅(qū)動(dòng)電機(jī)調(diào)速研究 [J].汽車(chē)實(shí)用技術(shù)，2020（10）：106-108.

[11] 劉曉宇，孟瑞鋒，賈偉.四旋翼飛行器姿態(tài)解算研究及最優(yōu)化設(shè)計(jì) [J].南方農(nóng)機(jī)，2018，49（15）：40-41.

[12] 余爾標(biāo)，陳宏華.一種有纜式四軸水下機(jī)器人的設(shè)計(jì)與制作 [J].江西電力職業(yè)技術(shù)學(xué)院學(xué)報(bào)，2022，35（10）：142-144.

作者簡(jiǎn)介：朱梅（1980—），女，漢族，江蘇江陰人，中級(jí)實(shí)驗(yàn)師，碩士研究生，研究方向：人工智能；賴(lài)日海（2001—），男，漢族，江西贛州人，本科在讀，研究方向：機(jī)器人控制、軟件開(kāi)發(fā)；樊中奎（1980—），男，漢族，江西南昌人，講師，碩士研究生，研究方向：人工智能。

收稿日期：2023-04-10

基金項(xiàng)目：江西理工大學(xué)大學(xué)生創(chuàng)新創(chuàng)業(yè)訓(xùn)練資助項(xiàng)目（202110407039）