基于多傳感器融合的陸空兩棲機(jī)器人移動控制系統(tǒng)設(shè)計

洪向共, 鐘地長, 趙慶敏

(南昌大學(xué)信息工程學(xué)院，南昌 330031)

陸空兩棲機(jī)器人是指既可在地面行駛又可在空中飛行的機(jī)動機(jī)器人，同時兼具無人車和無人機(jī)的機(jī)動能力，可為安防、勘探、救援等領(lǐng)域提供多種優(yōu)勢，或?qū)ξ磥聿筷犠鲬?zhàn)帶來重大影響[1]。文獻(xiàn)[1]提到美國“黑騎士”軍用陸空兩棲平臺在部隊作戰(zhàn)的裝備投送、傷員救援、急救物資補(bǔ)給等方面具有較大作用，其經(jīng)濟(jì)性高于直升機(jī)，但采用的是兩套驅(qū)動系統(tǒng)控制。文獻(xiàn)[2]總結(jié)了當(dāng)前新型陸空兩棲平臺的技術(shù)特點(diǎn)和發(fā)展趨勢，對兩棲平臺功能、用途分類闡述清晰。文獻(xiàn)[3]提出使用履帶式機(jī)械結(jié)構(gòu)設(shè)計，具有很好的機(jī)動性，但系統(tǒng)在陸空兩種情況下運(yùn)動控制闡述不夠清晰。新型平臺對陸空兩棲移動控制算法和傳感器數(shù)據(jù)融合進(jìn)行了清晰的描述，系統(tǒng)采用多傳感器數(shù)據(jù)融合處理方式實(shí)現(xiàn)兩種驅(qū)動模式控制一體化，具有成本低、效益高、集成度高、可擴(kuò)展性強(qiáng)等特點(diǎn)，可用于地質(zhì)勘探、巡視偵查、物資投送等領(lǐng)域。

1 系統(tǒng)總體構(gòu)建

主要從機(jī)械結(jié)構(gòu)到硬件電路設(shè)計，再到軟件算法控制實(shí)現(xiàn)來完成系統(tǒng)的功能。

1.1 系統(tǒng)設(shè)計方案

考慮到兩棲機(jī)器人移動控制的安全性、可靠性，以及控制系統(tǒng)移動的便捷性等因素，同時在進(jìn)行機(jī)械結(jié)構(gòu)設(shè)計時需要考慮到機(jī)器人自身質(zhì)量、承載能力和控制難易度，而輪式控制結(jié)合了無人車的設(shè)計特點(diǎn)，減震、易控制，載重能力也比較好，在不平地面上的越障能力較強(qiáng)，因此將其作為地面行駛運(yùn)動機(jī)構(gòu)。旋翼具有可垂直起降，可懸停，而且結(jié)構(gòu)簡單，機(jī)動性好，可操縱性較好，可作為空中飛行的運(yùn)動機(jī)構(gòu)[4]。最終，兩棲機(jī)器人結(jié)構(gòu)采用兩驅(qū)四輪式和四旋翼式結(jié)構(gòu)設(shè)計作為該控制系統(tǒng)的測試樣機(jī)。各部分結(jié)構(gòu)如圖1所示。

圖1 陸空兩棲機(jī)器人系統(tǒng)結(jié)構(gòu)Fig.1 Structure of air-ground amphibious robot system

1.2 系統(tǒng)控制電路設(shè)計

1.2.1 主控制器

為了更好地實(shí)現(xiàn)陸空兩棲機(jī)器人的控制效果，并考慮到系統(tǒng)的集成化、降低成本、可操作性、穩(wěn)定性等，將飛行控制和行駛控制集成到一塊主控制板，兼容使用相同的傳感器數(shù)據(jù)的思路去設(shè)計。控制主板的具體硬件電路如圖2所示。設(shè)計選用了意法半導(dǎo)體的STM32作為CPU。使用STM32F405RGT6型號芯片作為CPU控制器，32位高性能ARM Cortex-M4處理器，時鐘高達(dá)168 MHz。強(qiáng)大的時鐘系統(tǒng)使其具有高效的運(yùn)行效率，能滿足陸空兩棲系統(tǒng)的穩(wěn)定運(yùn)行。

圖2 系統(tǒng)硬件電路框圖Fig.2 Hardware circuit block diagram of the system

1.2.2 內(nèi)部感知模塊

(1)姿態(tài)傳感器采用ICM20602型號的6軸運(yùn)動處理傳感器。它集成了微機(jī)電系統(tǒng)(microe-lectro mechanical system，MEMS)傳感器、3軸的陀螺儀、3軸的加速度計，更精確，噪聲更小。它還可以獲得32 kHz的陀螺讀數(shù)，相較MPU6500型號的芯片在性能上有很大提升，因此對于具有更高處理能力的未來飛行控制器將是一個很好的選擇，可以選用I2C(inter-integrated circuit)總線或SPI(serial peripheral interface)總線接口連接一個第三方的數(shù)字傳感器，比如磁力計。型號為AK8975的是一個3軸電子羅盤，是具有高靈敏度的霍爾傳感器。

(2)氣壓計選用歌爾推出的型號SPL06-001數(shù)字壓力傳感器，它是一款具備SPI總線接口的新一代高精度氣壓傳感器。

1.2.3 地面執(zhí)行模塊

(1)針對輪式機(jī)器人調(diào)控操作，選用了調(diào)速范圍大，過載、啟動、動轉(zhuǎn)矩大，易于控制，可靠性高的直流電機(jī)。

(2)直流電機(jī)驅(qū)動器采用了工作電流比較大，驅(qū)動能力較強(qiáng)的A4950電機(jī)驅(qū)動芯片。

1.2.4 外部感知模塊

為使系統(tǒng)能夠更好地運(yùn)行，采用主要定位和測距模塊作為外部感知模塊。

根據(jù)上述設(shè)計，最終設(shè)計的系統(tǒng)控制主板如圖3所示。

2 運(yùn)動學(xué)建模

針對陸空兩棲兩種驅(qū)動方式進(jìn)行運(yùn)動學(xué)建模，將進(jìn)行兩種模式下的受力分析，模型線性化，建立模型公式，建立坐標(biāo)系轉(zhuǎn)換，方便主控處理器更好地處理兩棲機(jī)器人的位姿[5]。

2.1 空中飛行

動力學(xué)建模在實(shí)際應(yīng)用中，模型復(fù)雜度的增加通常并不能帶來準(zhǔn)確性的提高，相反會導(dǎo)致算法實(shí)時性的降低，因此，模型簡化是一種非常重要的手段。

對兩棲無人機(jī)飛行時的氣流進(jìn)行受力分析，建立系統(tǒng)飛行模型，如圖4所示,并對實(shí)際控制情況進(jìn)行分析，確定各個環(huán)節(jié)的傳遞函數(shù)[6]。

圖3 系統(tǒng)主控制板Fig.3 System main control board

圖4 系統(tǒng)飛行模型Fig.4 System flight model establishment

四旋翼無人機(jī)的沿軸平移運(yùn)動需要在導(dǎo)航坐標(biāo)系n中分析,假設(shè)四旋翼除重力和升力外不受其他力，四旋翼外力僅有螺旋槳提供的升力為

(1)

式(1)中：F為4個槳葉的合力；Ω1、Ω2、Ω3、Ω4分別為螺旋槳1、2、3、4提供的升力；b為轉(zhuǎn)換系數(shù)。

(2)

通過升降、俯仰、橫滾、偏航4種運(yùn)動模型的計算，得出旋翼轉(zhuǎn)速與升降、俯仰、橫滾和偏航4種運(yùn)動的控制量之間的關(guān)系為

(3)

式(3)中：F1、F2、F3、F4分別是升降、俯仰、橫滾、偏航運(yùn)動方向的合力；U1、U2、U3、U4分別是升降、俯仰、橫滾、偏航4個輸出控制量，這4個控制量直接與程序算法輸出量相關(guān)聯(lián)；c、b為模型的轉(zhuǎn)換系數(shù)。

2.2 地面行駛

在無人駕駛系統(tǒng)中，合理的車體模型的選擇對規(guī)劃、控制模塊的復(fù)雜度和性能有很大的影響。為了能夠盡可能準(zhǔn)確描述車體運(yùn)動，需要建立復(fù)雜的微分方程組，并用多個狀態(tài)變量來描述其運(yùn)動[7]。

系統(tǒng)對車體位姿(地面行駛的位置和航向)，首先在大地坐標(biāo)系下，設(shè)定[x,y,θ]為車體模型的狀態(tài)量(描述車體的位姿Vehiclepose)，[v,δ]為車體模型的控制量。v代表車體前進(jìn)車速，δ代表車體的前輪偏角。建立圖5所示的車體位姿信息模型。

Vehiclepose=[x,y,θ]

(4)

而后根據(jù)單自由度車體位姿模型計算得二自由度車體運(yùn)動學(xué)模型公式為

(5)

最后根據(jù)轉(zhuǎn)向位姿估計模型，如圖6所示，給定車體的初始定位信息。圖5、圖6中(XG,YG)為世界坐標(biāo)系，(XL,YL)為車體局部坐標(biāo)系，OL為車體后軸中心，OLXL為車體前進(jìn)方向。車體位姿用向量表示為(x0,y0,θ0)T，(x0,y0)為車體位置，θ0為車體航向角。根據(jù)車體后軸左、右輪的單位移動距離估計車體的位姿(位置和航向)信息，得出輪速計位姿估計表達(dá)式為

(6)

圖6 轉(zhuǎn)向位姿估計模型Fig.6 Steering pose estimation model

3 軟件算法控制

一般的比例積分微分(PID)控制算法是根據(jù)反饋后的控制效果來不斷整定合適的參數(shù)，而沒有對控制對象作細(xì)致的分析，因而對簡單的系統(tǒng)控制過程比較簡單[8]。但對于控制對象的滯后和時間常數(shù)很大、干擾作用強(qiáng)而頻繁的系統(tǒng)來說，很難通過調(diào)整最佳參數(shù)來獲得高質(zhì)量的控制效果。而串級PID控制目的在于通過設(shè)置副變量來提高對主變量的控制質(zhì)量，抗干擾作用強(qiáng)，改善控制質(zhì)量，提高系統(tǒng)工作頻率，且自適應(yīng)負(fù)荷能力較強(qiáng)。另外對于姿態(tài)測量傳感器進(jìn)行了數(shù)據(jù)融合，采用不同的融合算法能夠?qū)崿F(xiàn)在不同模式下反饋值給PID控制器，輸出不同驅(qū)動量驅(qū)動機(jī)體的槳葉和輪子。傳感器數(shù)據(jù)融合處理與兩種模式之間的聯(lián)系如圖7所示。

圖7 多傳感器數(shù)據(jù)融合后的反饋Fig.7 Feedback after multi-sensor data fusion

3.1 空中飛行控制算法

飛行過程中系統(tǒng)通過慣性導(dǎo)航單元(inertial measurement unit，IMU)數(shù)據(jù)融合，一般IMU包括加速度計、陀螺儀、磁力計，然后進(jìn)行解算實(shí)時的姿態(tài)角，反饋到飛行姿態(tài)控制系統(tǒng)中。根據(jù)PID的控制原理，輸入期望與反饋值作差，通過比例、積分、微分等運(yùn)算，輸出四路的脈沖寬度調(diào)制(pulse width modulation，PWM)波形信號來控制電機(jī)轉(zhuǎn)速，實(shí)現(xiàn)系統(tǒng)穩(wěn)定飛行[9]。

系統(tǒng)使用串級PID控制策略能夠提高飛行系統(tǒng)的抗干擾性和適應(yīng)性。如圖8所示，根據(jù)串級PID的特性和文獻(xiàn)[10-11]的分析可知，串級PID分為外環(huán)和內(nèi)環(huán)兩個控制環(huán)，其中外環(huán)為角度環(huán)，其作用是對姿態(tài)角進(jìn)行控制，內(nèi)環(huán)為角速度環(huán)，其作用是增強(qiáng)系統(tǒng)的穩(wěn)定性，使系統(tǒng)的控制更加精確。外環(huán)期望姿態(tài)角與解算后反饋的實(shí)時姿態(tài)角作差，經(jīng)過外環(huán)比例控制器運(yùn)算輸出的是角速度值，并作內(nèi)環(huán)期望角速度，與由陀螺儀測得的實(shí)時角速度的偏差作為內(nèi)環(huán)的輸入，最終內(nèi)環(huán)輸出通過控制量轉(zhuǎn)換，得到不同的PWM波信號控制電機(jī)轉(zhuǎn)速，實(shí)現(xiàn)機(jī)器人的各種姿態(tài)變化運(yùn)動。

圖8 空中飛行姿態(tài)控制流程Fig.8 Process of attitude control in air flight

圖8中φ、θ、ψ為姿態(tài)解算后的3個姿態(tài)角(俯仰、橫滾、偏航)，φe、θe、ψe是期望的姿態(tài)角度，經(jīng)過外環(huán)PID控制器運(yùn)算，輸出3個姿態(tài)角速度pe、qe、re作為內(nèi)環(huán)PID的輸入，陀螺儀測量的角速度p、q、r作為反饋量。

3.2 地面行駛控制算法

系統(tǒng)地面行駛的控制難點(diǎn)在于對機(jī)器人運(yùn)動速度和方向控制的融合，其中方向的控制是系統(tǒng)控制策略的難點(diǎn)。根據(jù)文獻(xiàn)[7，12]的描述，采用最優(yōu)曲率法。其控制思路如下：模型車在一個控制周期內(nèi)的運(yùn)動軌跡可以近似為一段圓弧，整個運(yùn)動軌跡可以看作由小段圓弧連接成的曲線。在假設(shè)線速度恒定的前提下，給定路徑上的一個點(diǎn)就能確定出模型車到達(dá)該點(diǎn)所應(yīng)具有的舵機(jī)角度[12-13]。根據(jù)該點(diǎn)的信息得到車體實(shí)時位姿情況。

建立地面運(yùn)動坐標(biāo)系，以車體的兩后輪之間中點(diǎn)為坐標(biāo)系原點(diǎn)，車體的中軸前進(jìn)方向?yàn)閥軸正方向，設(shè)(cx,cy)為路徑上某一點(diǎn)在該坐標(biāo)系中的坐標(biāo)，則控制量之間的計算公式為

(7)

ω=arctan(L/rb)

(8)

R=ωm

(9)

式中：rb為后輪的轉(zhuǎn)彎半徑；L為車體的前橋與后橋的距離；m為將ω轉(zhuǎn)換為R的系數(shù)。

集合上述算法的控制思想和地面運(yùn)動學(xué)模型，設(shè)計出特定的串級PID控制算法，將控制器分為內(nèi)、外環(huán)控制，外環(huán)控制為小車行駛位置和方向角度PID控制，控制輸出角速度，再將角速度轉(zhuǎn)換成線速度，由運(yùn)動學(xué)方程進(jìn)行轉(zhuǎn)換后，作為內(nèi)環(huán)的期望值輸入，內(nèi)環(huán)控制為小車加速度PID控制，內(nèi)環(huán)控制輸出經(jīng)過線性映射轉(zhuǎn)換成控制電機(jī)的PWM波，如圖9所示。

圖9 地面行駛位姿控制流程Fig.9 Process of position control forground driving

4 系統(tǒng)測試與分析

兩棲機(jī)器人控制系統(tǒng)在陸空兩種模式下分別進(jìn)行了測驗(yàn)，其穩(wěn)定性和可操作性已實(shí)現(xiàn)，并且通過MATLAB對系統(tǒng)輸出數(shù)據(jù)進(jìn)行繪圖，更直觀地看到控制的效果。

在空中飛行模式下，以陀螺儀Z軸采集數(shù)據(jù)為例進(jìn)行傳感器數(shù)據(jù)融合實(shí)際測試。如圖10所示，紅色部分為傳感器數(shù)據(jù)融合后的值，藍(lán)色部分為傳感器采集的原始數(shù)據(jù)。由圖10可知，數(shù)據(jù)經(jīng)過本文系統(tǒng)的數(shù)據(jù)融合算法處理，明顯減少了大部分的噪聲干擾，使得數(shù)據(jù)對系統(tǒng)控制更加穩(wěn)定和可靠。如圖11所示，以加速度計y軸的數(shù)據(jù)融合前后對比為例，藍(lán)色是加速度原始采集的數(shù)據(jù)，紅色是融合處理后的數(shù)據(jù)。從圖11可以看出，融合的速度和原始數(shù)據(jù)幾乎貼合，而且對數(shù)據(jù)的尖峰做了一定的濾波限制處理，提高數(shù)據(jù)柔和性，避免飛行出現(xiàn)大幅度抖動。

圖10 陀螺儀z軸數(shù)據(jù)融合前后對比Fig.10 Comparison of z-axis data fusion for gyroscope

圖11 加速度計y軸數(shù)據(jù)融合前后對比Fig.11 Comparison of y-axis data fusion for accelerometer

傳感器數(shù)據(jù)作為串級PID閉環(huán)反饋控制量，是輸出飛行姿態(tài)角重要的數(shù)據(jù)來源。處理后的傳感器數(shù)據(jù)通過姿態(tài)解算輸出姿態(tài)角，如圖12所示，其中藍(lán)色為俯仰角，黑色為橫滾角。姿態(tài)角在實(shí)際測試過程中收斂較快，靈敏度高，具有較好的控制效果和操作性。

圖12 飛行姿態(tài)角度測試Fig.12 Flight attitude angle test

圖13 地面行駛速度控制測試Fig.13 Ground velocity control test

對系統(tǒng)地面行駛進(jìn)行測試，如圖13所示，藍(lán)色實(shí)線為期望的速度曲線，綠色實(shí)線為系統(tǒng)控制輸出的曲線，通過遙桿的作用測試，系統(tǒng)在地面行駛下的速度曲線和期望的曲線基本相近，綠色曲線出現(xiàn)的±0.01 m/s的毛刺誤差，是由于系統(tǒng)定時器計數(shù)測量引起的誤差。實(shí)際速度和理想速度控制效果基本相近，體現(xiàn)該兩棲機(jī)器人系統(tǒng)的地面運(yùn)動可控制性較強(qiáng)。

系統(tǒng)實(shí)物測試如圖14所示。

圖14 實(shí)物測試Fig.14 Physical testing

5 結(jié)語

設(shè)計的陸空兩棲無人機(jī)系統(tǒng)控制主板是以STM32為核心，集成了無人機(jī)和無人機(jī)車測量姿態(tài)傳感器，同時兼容外部測距、定位等傳感器。主控板設(shè)計使用有刷和無刷電機(jī)驅(qū)動，并預(yù)留可擴(kuò)展傳感器功能接口。代碼開源，算法清晰易懂，可靠性高，可操作性強(qiáng)，系統(tǒng)可根據(jù)測距模塊采集信息實(shí)現(xiàn)避障功能。系統(tǒng)的串級PID 算法雖然增加了控制系統(tǒng)的工作頻率，減少了時間常數(shù)，但達(dá)到了更佳的控制效果。最后制作了樣機(jī)，通過試驗(yàn)，驗(yàn)證了設(shè)計的合理性和可靠性，兩棲系統(tǒng)運(yùn)行正常。今后可在此基礎(chǔ)上展開水陸空三棲控制系統(tǒng)的研發(fā)。