基于IMU及UWB的微型彈跳機(jī)器人傳感器節(jié)點(diǎn)位姿檢測研究?

江朝軍倪江生張 軍李 漢

(東南大學(xué)儀器科學(xué)與工程學(xué)院，江蘇 南京210000)

當(dāng)今世界，機(jī)器人的應(yīng)用日益廣泛，其面臨的作業(yè)環(huán)境愈發(fā)惡劣，諸如災(zāi)害救援、地質(zhì)勘測中都期望具備感知能力的機(jī)器人替代人員，作為傳感器節(jié)點(diǎn)，完成信息收集和輔助作業(yè)等任務(wù)。這類環(huán)境往往地形崎嶇多變，多障礙物和溝壑地形，相較于傳統(tǒng)微型輪式機(jī)器人節(jié)點(diǎn)，具有更靈活的運(yùn)動性能的微型彈跳機(jī)器人(MJR)節(jié)點(diǎn)更能勝任此類任務(wù)。

彈跳機(jī)器人具有較強(qiáng)的越障能力，但同時也有難以控制以及姿態(tài)調(diào)節(jié)復(fù)雜等問題需要解決。目前彈跳機(jī)器人從運(yùn)動形式上分為兩類:一類是作連續(xù)性彈跳運(yùn)動[1]，即彈跳腿碰到地面后在很短的時間內(nèi)再次起跳，彈跳機(jī)構(gòu)的蓄能、姿態(tài)調(diào)整等都是動態(tài)完成的。而另一類作間隙性運(yùn)動[2－4]，即落地后需要短暫時間用于姿態(tài)調(diào)整和彈跳機(jī)構(gòu)蓄能等，為后續(xù)的彈跳做準(zhǔn)備，相對簡化了控制。

無論是連續(xù)性還是間歇性彈跳運(yùn)動，要實(shí)現(xiàn)長距離運(yùn)動，都需要進(jìn)行位姿檢測與調(diào)節(jié)。美國噴氣推進(jìn)實(shí)驗(yàn)室(JPL)在彈跳式機(jī)器人的研究方面獲得較多的成果，其中較出名的是其設(shè)計(jì)的三代間隙性彈跳式機(jī)器人[5－8]。其中文章[8]中提出了一種針對彈跳機(jī)器人的融合相機(jī)、慣性和加速度數(shù)據(jù)的定位算法，但僅進(jìn)行了仿真分析，并未實(shí)現(xiàn)樣機(jī)測試。瑞士聯(lián)邦理工學(xué)院Kovac等人結(jié)合蚱蜢的運(yùn)動方式，研制出一款微型彈跳機(jī)器人[9－11]，其利用三軸加速度計(jì)檢測姿態(tài)，并通過籠架結(jié)構(gòu)實(shí)現(xiàn)被動式復(fù)位設(shè)計(jì)。密歇根州立大學(xué)的趙建國等人受蜥蜴空中操縱能力的啟發(fā)，開展了微型尾巴跳躍機(jī)器人的機(jī)構(gòu)設(shè)計(jì)與控制研究[12－14]，其利用加速度計(jì)檢測自由落體，采用陀螺儀檢測空中機(jī)體角度并加以控制。國內(nèi)機(jī)構(gòu)在彈跳式機(jī)器人方面的研究也在不斷深入，并取得了不少成績。上海交通大學(xué)楊煜普教授等人提出了一種翻轉(zhuǎn)跳躍單腿機(jī)器人模型[15]。東南大學(xué)研制了一款仿蝗蟲微型彈跳式機(jī)器人，彈跳高度能達(dá)到0.8 m左右[2－4]。

彈跳機(jī)器人的位姿信息反映其實(shí)時的狀態(tài)，影響著彈跳機(jī)器人動作執(zhí)行的有效性與完成度。因此，獲取彈跳機(jī)器人的位姿信息以及信息的準(zhǔn)確度，對于彈跳機(jī)器人有效執(zhí)行動作具有突出的作用。然而MJR作為傳感器節(jié)點(diǎn)，存在體積小、負(fù)載能力弱等問題，難以搭載具有高精度而體積大、重量大的傳感器系統(tǒng)，存在位姿檢測難的問題，限制了其實(shí)際應(yīng)用。在團(tuán)隊(duì)早期研制的間歇性運(yùn)動的MJR基礎(chǔ)上，本文采用IMU對MJR姿態(tài)進(jìn)行檢測，實(shí)現(xiàn)了自復(fù)位以及自主航向調(diào)節(jié)功能。通過UWB對MJR進(jìn)行定位，并通過實(shí)驗(yàn)驗(yàn)證位姿檢測與控制的穩(wěn)定性和準(zhǔn)確性，為MJR節(jié)點(diǎn)自部署奠定了基礎(chǔ)。

1 基于IMU的機(jī)器人姿態(tài)檢測

本文采用一個9軸IMU模塊對MJR進(jìn)行姿態(tài)檢測，該模塊包含了3軸加速度計(jì)、3軸角速度計(jì)及3軸磁力計(jì)。MJR讀取IMU模塊返回的數(shù)據(jù)后加以處理得到姿態(tài)信息，并結(jié)合機(jī)械結(jié)構(gòu)執(zhí)行動作。

1.1 自復(fù)位中姿態(tài)角檢測

在本文的MJR設(shè)計(jì)中，利用機(jī)器人底部的調(diào)節(jié)電機(jī)帶動調(diào)節(jié)桿腿旋轉(zhuǎn)，以實(shí)現(xiàn)展開式自復(fù)位，展開式自復(fù)位原理如圖1所示。

圖1 MJR自復(fù)位示意圖

MJR的自復(fù)位功能主要分為姿態(tài)檢測與復(fù)位控制兩部分。MJR會首先檢測傾覆狀態(tài)，通過檢測到姿態(tài)角的正負(fù)判斷機(jī)身的著地面，然后進(jìn)行相應(yīng)復(fù)位操作。自復(fù)位過程中，通過檢測機(jī)身傾斜角度，并根據(jù)角度調(diào)節(jié)PWM輸出，控制調(diào)節(jié)電機(jī)扭矩與轉(zhuǎn)速。

當(dāng)MJR處于靜止時，三軸加速度傳感器只受到豎直向下的重力，而當(dāng)MJR傾斜時，重力加速度將在加速度傳感器的三個軸向產(chǎn)生加速度分量，如圖2所示。

圖2 MJR姿態(tài)角示意圖

機(jī)體坐標(biāo)系下，將加速度計(jì)測量到的數(shù)據(jù)歸一化為a＝[ax，ay，az]T。設(shè)繞X軸旋轉(zhuǎn)的橫滾角為?，繞Y軸旋轉(zhuǎn)的俯仰角為θ。IMU水平姿態(tài)繞Y軸旋轉(zhuǎn)后可以得到關(guān)系:

再將IMU當(dāng)前坐標(biāo)系繞X軸旋轉(zhuǎn)，可以得到IMU實(shí)際姿態(tài)下的輸出為:

由此，通過式(2)可以得到姿態(tài)角為:

當(dāng)檢測到橫滾角?較大時，說明機(jī)身傾覆角度偏大，此時將PWM占空比設(shè)置較大，確保桿腿能獲得足夠的力實(shí)現(xiàn)復(fù)位。隨著復(fù)位進(jìn)行，檢測到的?逐漸變小，此時MJR所需復(fù)位力矩變小，減小PWM占空比。當(dāng)檢測到?<β時，PWM占空比降為0，MJR會在慣性以及機(jī)身重力作用下完成復(fù)位。其中β為機(jī)身重心與地面支撐腿處于同一豎直線時的機(jī)身傾覆角度。隨后采集一段時間姿態(tài)角信息，當(dāng)檢測到?與θ均小于一定閾值時，則表明自復(fù)位成功。由于機(jī)身全高約12 cm，而重心高度約9 cm，且并不關(guān)于左右端面對稱，實(shí)驗(yàn)中要實(shí)現(xiàn)MJR的穩(wěn)定復(fù)位也存在一定困難?；贗MU姿態(tài)角檢測實(shí)現(xiàn)自復(fù)位功能的工作流程如圖3所示。

圖3 基于姿態(tài)檢測的自復(fù)位工作流程圖

1.2 航向調(diào)節(jié)中航向角檢測

在論文[4]中，楊茜等人對MJR姿態(tài)調(diào)節(jié)中離散和連續(xù)運(yùn)動進(jìn)行建模分析和實(shí)驗(yàn)研究，其中連續(xù)式航向調(diào)節(jié)機(jī)構(gòu)由一個電機(jī)驅(qū)動，并利用單向軸承的特性進(jìn)行機(jī)械解耦設(shè)計(jì)。

如圖4所示，由調(diào)節(jié)電機(jī)帶動輪子旋轉(zhuǎn)以實(shí)現(xiàn)機(jī)身轉(zhuǎn)動，而桿腿也會隨之轉(zhuǎn)動至另一側(cè)。為了防止桿腿觸地使得MJR側(cè)翻，需要進(jìn)行及時回調(diào)，在單向軸承作用下，桿腿回轉(zhuǎn)而輪子保持不動，重復(fù)上述過程即可實(shí)現(xiàn)連續(xù)式航向調(diào)節(jié)。本文在團(tuán)隊(duì)研究基礎(chǔ)上進(jìn)行適當(dāng)改進(jìn)以及自主航向調(diào)節(jié)功能實(shí)現(xiàn)。在航向調(diào)節(jié)中發(fā)揮主要作用的傳感器是磁力計(jì)，通過磁力計(jì)計(jì)算出MJR當(dāng)前航向，結(jié)合目標(biāo)航向角以及機(jī)身轉(zhuǎn)動角度和電機(jī)轉(zhuǎn)動角度的關(guān)系，可以轉(zhuǎn)換為電機(jī)需要轉(zhuǎn)動的角度。

圖4 連續(xù)式航向調(diào)節(jié)機(jī)構(gòu)功能示意圖

①磁力計(jì)基本原理

磁力計(jì)能夠根據(jù)地磁場實(shí)時提供航向信息，通過測量地磁場矢量在各輸出軸的分量以計(jì)算偏航角。偏航角基本公式如下:

然而在磁力計(jì)非水平情況下，需要采用加速度計(jì)進(jìn)行傾角補(bǔ)償，將載體坐標(biāo)系的XOY平面旋轉(zhuǎn)到水平面，再計(jì)算航向角信息，解算過程如圖5所示。

圖5 非水平情況下航向角求解過程(傾角補(bǔ)償)

第一步將磁力計(jì)所在坐標(biāo)系繞X軸旋轉(zhuǎn)r度，將Y軸旋轉(zhuǎn)至水平面，如此可求得磁感應(yīng)強(qiáng)度沿旋轉(zhuǎn)后坐標(biāo)系各軸的投影:

第二步將旋轉(zhuǎn)后的坐標(biāo)系繞Y軸旋轉(zhuǎn)p度，如此已將原始磁力計(jì)坐標(biāo)系中XOY平面旋轉(zhuǎn)至水平面，旋轉(zhuǎn)之后的各軸投影為:

結(jié)合式(4)~式(6)，可得到一般情況下MJR航向角的表達(dá)式如(7)所示。

②磁力計(jì)校準(zhǔn)

MJR機(jī)身上裝備的各類元器件以及周圍環(huán)境中的磁性材料等，都會對磁力計(jì)周圍的磁場造成明顯的影響，因而需要先對實(shí)驗(yàn)環(huán)境下的磁力計(jì)進(jìn)行校準(zhǔn)。

本文采用基于最小二乘的橢球擬合法對磁力計(jì)進(jìn)行校準(zhǔn)[16－18]，將MJR在實(shí)驗(yàn)環(huán)境中進(jìn)行緩慢旋轉(zhuǎn)，并采集磁力計(jì)數(shù)據(jù)。再將采集到的數(shù)據(jù)導(dǎo)入MATLAB中，利用橢球擬合法進(jìn)行擬合，可以得到圓心坐標(biāo)為(x0，y0，z0)，三軸長度分別為lx，ly，lz。如此可得校準(zhǔn)后磁力計(jì)輸出如式(8)所示:

③磁力計(jì)輔助航向調(diào)節(jié)

本文采用帶編碼器的電機(jī)驅(qū)動航向調(diào)節(jié)，得到機(jī)身待轉(zhuǎn)動角度后需要轉(zhuǎn)換為電機(jī)轉(zhuǎn)動角度。如圖6所示，設(shè)MJR旋轉(zhuǎn)半徑為R，調(diào)節(jié)輪半徑為r，機(jī)身轉(zhuǎn)動角度為θ，那么電機(jī)轉(zhuǎn)動角度φ為:

圖6 航向調(diào)節(jié)平面示意圖

此外，設(shè)電機(jī)軸轉(zhuǎn)動一圈編碼器得到的脈沖數(shù)為Cppr，調(diào)節(jié)電機(jī)減速箱的減速比為i，那么電機(jī)輸出軸轉(zhuǎn)動角度與對應(yīng)脈沖數(shù)關(guān)系為:

因此結(jié)合式(8)與式(9)可以得到脈沖數(shù)與航向調(diào)整角的關(guān)系:

理論上通過編碼器即可實(shí)現(xiàn)精準(zhǔn)的航向調(diào)節(jié)，然而在航向調(diào)節(jié)過程中，由于地面粗糙度和坡度等影響，可能會導(dǎo)致旋轉(zhuǎn)中心的改變，如圖7所示。

圖7 航向調(diào)節(jié)中旋轉(zhuǎn)中心改變圖

此外，調(diào)節(jié)輪打滑以及電機(jī)未及時剎停等因素都會導(dǎo)致航向調(diào)節(jié)不精確。因此，單純依靠編碼器控制航向調(diào)節(jié)并不可靠，本文結(jié)合了磁力計(jì)修正的方式來提升航向調(diào)節(jié)的可靠性。此種調(diào)節(jié)方式中，調(diào)節(jié)電機(jī)每旋轉(zhuǎn)固定角度Dfixed(所需旋轉(zhuǎn)角度>Dfixed時)就進(jìn)行一次航向角檢測，并更新剩余需要旋轉(zhuǎn)的角度，不斷調(diào)節(jié)直到偏差角小于設(shè)定的誤差閾值，期間還要根據(jù)電機(jī)旋轉(zhuǎn)角度及時將桿腿回調(diào)。其控制流程圖如圖8所示，其中Nfixed為Dfixed對應(yīng)的脈沖數(shù)。

圖8 結(jié)合磁力計(jì)修正的航向調(diào)節(jié)流程圖

在航向調(diào)節(jié)過程中，由于機(jī)身重心較高，其可能會受到地面坡度等影響而跌倒。因此自主航向調(diào)節(jié)設(shè)計(jì)中，結(jié)合了姿態(tài)檢測，確保MJR意外傾覆時，能夠自主啟動復(fù)位功能復(fù)位，并繼續(xù)執(zhí)行航向調(diào)節(jié)。

2 基于UWB的機(jī)器人定位方法

2.1 空間三邊定位算法

在UWB定位技術(shù)[19－23]方面，目前常用的定位算法有直接計(jì)算法、Fang算法、Chan算法以及幾何法等各類方法。本文采用幾何法解算目標(biāo)點(diǎn)，設(shè)置4個UWB基站，根據(jù)機(jī)器人與各個基站的距離信息以及基站的坐標(biāo)，通過空間三邊定位算法可以解算出MJR節(jié)點(diǎn)當(dāng)前位置坐標(biāo)信息，如圖9所示。

圖9 UWB幾何法解算示意圖

設(shè)基站A1的坐標(biāo)為(xA1，yA1，zA1)，基站A2的坐標(biāo)為(xA2，yA2，zA2)，基站A3的坐標(biāo)為(xA3，yA3，zA3)，以及基站A4的坐標(biāo)為(xA4，yA4，zA4)，測距得到四個基站距離機(jī)器人的距離分別為l1，l2，l3，l4。設(shè)MJR當(dāng)前坐標(biāo)為(x，y，z)，則根據(jù)測得的與四個基站的距離信息，可以直接得到以下方程組(12):

理論上，通過解這個方程組即可獲得唯一解，作為MJR的位置坐標(biāo)。然而實(shí)際上，由于UWB測距誤差的原因，方程組很有可能是無解的，需要考慮對不同情況進(jìn)行特殊處理以獲得MJR位置的最優(yōu)估計(jì)。

對于幾何法實(shí)現(xiàn)球面定位解算的步驟基本如圖9所示，主要分為三步:

STEP 1 先選取其中A1與A2兩個基站，由于測距誤差的影響，兩個球面可能有相交、相切與分離三種情況，即方程組(13)會對應(yīng)存在無窮解、唯一解以及無解的情況:①對于一般相交的情況，交點(diǎn)的集合為一個圓。②而對于相切的情況，唯一解即為兩個球面的切點(diǎn)。③對于兩個球面分離的情況，我們需要逐步且適當(dāng)?shù)財(cái)U(kuò)大兩個球面的半徑，即基站A1與A2返回的測距值，直到兩球面相交或相切為止。

STEP 2 從STEP 1中我們解算得到的是一個空間中的圓，或者是一個點(diǎn):①若為一個點(diǎn)，那么判斷該點(diǎn)是否處于基站3的球面上，是那就返回該點(diǎn)作為結(jié)果，不是就返回失敗。②若得出的是一個圓，那么求解圓與基站3的球面的相交情況:如果相切，返回切點(diǎn)坐標(biāo)作為結(jié)果；如果相離，返回失?。蝗绻嘟?，計(jì)算出兩個交點(diǎn)用于下一步驟的計(jì)算。

STEP 3 從STEP 2中可以得出兩個交點(diǎn)，設(shè)STEP 2中求出的兩個交點(diǎn)坐標(biāo)分別為P1(xP1，yP1，zP1)，P2(xP2，yP2，zP2)，聯(lián)合直線方程與基站4球面方程(14):

將直線方程與基站4球面坐標(biāo)方程聯(lián)合求解，再根據(jù)交點(diǎn)分布情況以及P1P2離基站4球面的距離情況，進(jìn)行分情況討論處理:①假如過兩個交點(diǎn)的直線與基站4的球面無交點(diǎn)，那么不斷擴(kuò)大球面4的半徑；若是擴(kuò)大到一定值仍無交點(diǎn)，則直接取P1P2兩點(diǎn)中離球面4更近的那個點(diǎn)作為結(jié)果。②假如直線與球面4存在交點(diǎn)，如圖10幾大類情況，那么根據(jù)具體情況對P1和P2點(diǎn)進(jìn)行加權(quán)處理求得最優(yōu)估計(jì)位置坐標(biāo)。

圖10 交點(diǎn)相對關(guān)系示意圖

2.2 UWB測距標(biāo)定

由于周圍環(huán)境中的金屬以及障礙物等會對UWB信號產(chǎn)生吸收和阻擋等影響，使得UWB測距值出現(xiàn)較大偏差。因此，針對實(shí)驗(yàn)環(huán)境，需要事先進(jìn)行UWB測距標(biāo)定實(shí)驗(yàn)，對輸出測距信息進(jìn)行線性擬合以獲取更加精確的測距結(jié)果。

在測距標(biāo)定實(shí)驗(yàn)中，將MJR置于地面，UWB基站則放置在三角架上。其中MJR裝備的信標(biāo)UWB模塊離地距離12.5 cm，基站UWB模塊離地120 cm，即兩者的垂直高度差為107.5 cm。在標(biāo)定實(shí)驗(yàn)中水平距離間隔50 cm進(jìn)行一次采集，從水平距離200 cm到800 cm一共采集13組。由于UWB在MJR上的安裝位置不具備對稱性，因此每組實(shí)驗(yàn)對MJR前后左右四個朝向分別進(jìn)行檢測記錄，得到測距值如表1所示。

表1 UWB測距標(biāo)定實(shí)驗(yàn)數(shù)據(jù)(4方向)單位:mm

將實(shí)驗(yàn)測距數(shù)據(jù)整理繪圖，得到如圖11結(jié)果，可以明顯地看出，輸出距離值基本呈現(xiàn)線性遞增關(guān)系。本文采用四個方向測距均值替代MJR的測距信息，并在MATLAB中對測距均值采用二次項(xiàng)擬合，得到擬合式(15):

圖11 UWB原始測距值變化曲線

式中:dm為UWB返回原始測距值，dAvg為擬合后輸出測距值。

采用擬合函數(shù)對原始數(shù)據(jù)進(jìn)行處理，并計(jì)算與實(shí)際距離值的偏差，得到效果如圖12所示。從圖12中可以明顯看出，經(jīng)過二次項(xiàng)擬合之后的UWB測距信息更加符合實(shí)際測距值，并且測距偏差顯著降低，基本穩(wěn)定在±5 cm之間，這為之后MJR的UWB定位效果提供了一定的基礎(chǔ)條件。

圖12 二次項(xiàng)擬合效果圖

3 實(shí)驗(yàn)測試

本部分首先對MJR的自復(fù)位姿態(tài)檢測和自主航向檢測效果進(jìn)行了實(shí)驗(yàn)測試，在此基礎(chǔ)上，通過MJR運(yùn)動部署實(shí)驗(yàn)，驗(yàn)證本文提出方法的有效性。

3.1 節(jié)點(diǎn)自復(fù)位姿態(tài)檢測實(shí)驗(yàn)

對MJR節(jié)點(diǎn)自主復(fù)位功能進(jìn)行實(shí)驗(yàn)測試，測試其基于IMU姿態(tài)檢測的自主復(fù)位功能的成功率。MJR節(jié)點(diǎn)可能一次復(fù)位成功，也可能首次復(fù)位不穩(wěn)導(dǎo)致傾覆而從另一側(cè)進(jìn)行復(fù)位，本文實(shí)驗(yàn)中將超過連續(xù)4次以上復(fù)位運(yùn)動以及其他未能復(fù)位的情況視為失敗。自主復(fù)位實(shí)驗(yàn)一共分為兩組，一組初始狀態(tài)為左側(cè)傾覆，另一組初始狀態(tài)為右側(cè)傾覆，對每組實(shí)驗(yàn)分別進(jìn)行40次重復(fù)測試，記錄復(fù)位結(jié)果如表2所示。

表2 兩種傾覆狀態(tài)下自復(fù)位實(shí)驗(yàn)結(jié)果

從表2中可以看出，針對兩種初始傾覆狀態(tài)下的自主復(fù)位實(shí)驗(yàn)均未出現(xiàn)復(fù)位失敗的情形，這說明了基于IMU的自主復(fù)位功能的穩(wěn)健性。

可以看出當(dāng)MJR基本都是從右側(cè)成功復(fù)位，出現(xiàn)該現(xiàn)象的原因是MJR樣機(jī)的重心偏向于機(jī)身右側(cè)，如圖13所示。

圖13 兩種傾覆狀態(tài)下重心位置示意圖

相同機(jī)身傾斜角度α下，左側(cè)傾斜下重心－旋轉(zhuǎn)軸－桿腿的夾角更大。而當(dāng)機(jī)身姿態(tài)接近水平時，是依靠慣性以及自身重力實(shí)現(xiàn)復(fù)位的，即從此時到機(jī)身水平，左側(cè)傾斜復(fù)位情況下重心位置繞旋轉(zhuǎn)軸轉(zhuǎn)過的角度更加大，也就更加容易出現(xiàn)再次傾覆的現(xiàn)象。

在自復(fù)位實(shí)驗(yàn)過程中采集機(jī)身傾角信息，取其中典型的幾組實(shí)驗(yàn)數(shù)據(jù)在MATLAB中繪制如圖14所示。從圖中可以看出，一次復(fù)位成功情況下，機(jī)身傾角絕對值從90°附近不斷減小至15°左右，隨后MJR可在慣性與重力作用下完成復(fù)位。而在二次復(fù)位或多次復(fù)位情況下，MJR均出現(xiàn)了復(fù)位立正后再次傾覆的情況，不過MJR檢測到再次傾覆后會繼續(xù)從另一側(cè)進(jìn)行復(fù)位，直到成功調(diào)整機(jī)身至水平穩(wěn)定。

圖14 自主復(fù)位過程中機(jī)身傾角變化示意圖

3.2 節(jié)點(diǎn)自主航向調(diào)節(jié)實(shí)驗(yàn)測試

針對單純依靠編碼器進(jìn)行航向調(diào)節(jié)以及磁力計(jì)修正的航向調(diào)節(jié)方法進(jìn)行對比實(shí)驗(yàn)，實(shí)驗(yàn)中控制MJR進(jìn)行固定航向調(diào)節(jié)(45°~360°，每隔45°設(shè)置一組實(shí)驗(yàn))。由于磁力計(jì)航向檢測以及航向調(diào)節(jié)均存在一定誤差，為了確保自主航向調(diào)節(jié)功能的效率，實(shí)驗(yàn)中MJR節(jié)點(diǎn)檢測到航向調(diào)節(jié)偏差處于5°以內(nèi)即視作調(diào)節(jié)成功，并自動停止該次航向調(diào)節(jié)運(yùn)動。對每一組航向角調(diào)節(jié)實(shí)驗(yàn)進(jìn)行5次，取均值作為結(jié)果，并計(jì)算每組調(diào)節(jié)實(shí)驗(yàn)的角度偏差以及方差，結(jié)果如圖15、圖16及表3所示。

表3 兩種航向調(diào)節(jié)方式的調(diào)節(jié)角度方差(單純編碼器，編碼器＋磁力計(jì)修正)

圖15 兩種航向調(diào)節(jié)方式對比圖

圖16 調(diào)節(jié)角度偏差與方差對比

通過自主航向調(diào)節(jié)實(shí)驗(yàn)得到的單純編碼器航向調(diào)節(jié)方式的平均調(diào)節(jié)角度偏差為4.64°，調(diào)節(jié)平均方差為13.25。編碼器結(jié)合磁力計(jì)修正的航向調(diào)節(jié)方式平均調(diào)節(jié)角度偏差為3.64°，調(diào)節(jié)平均方差為0.65?？梢悦黠@看出，采用磁力計(jì)修正航向調(diào)節(jié)的方式相比單純采用編碼器，有效地降低了旋轉(zhuǎn)中心改變以及調(diào)節(jié)輪打滑等對航向調(diào)節(jié)的影響，具備更高的準(zhǔn)確性和穩(wěn)定性。

3.3 節(jié)點(diǎn)運(yùn)動部署定位實(shí)驗(yàn)測試

本文基于上文MJR基礎(chǔ)功能的實(shí)現(xiàn)以及提出的定位方法，設(shè)計(jì)了直線和矩形軌跡跳躍的部署運(yùn)動驗(yàn)證MJR基礎(chǔ)功能的穩(wěn)定性與定位功能的準(zhǔn)確性。

直線跳躍部署運(yùn)動實(shí)驗(yàn)設(shè)置為8跳，共進(jìn)行2次實(shí)驗(yàn)。矩形跳躍部署運(yùn)動實(shí)驗(yàn)設(shè)置為16跳，約為2 m×2 m的正方形，共進(jìn)行1次實(shí)驗(yàn)。如圖17所示，實(shí)驗(yàn)場地設(shè)置為5 m×8 m，在場地的四個邊角處分別放置一個UWB定位基站，用對MJR運(yùn)動過程中每次的落地位置進(jìn)行定位。

圖17 戶外實(shí)驗(yàn)場景圖

在直線跳躍部署運(yùn)動中，將MJR置于起始點(diǎn)(250，200)，通過上位機(jī)向MJR發(fā)送跳躍方向以及單跳指令。MJR接收指令后，將自主進(jìn)行航向調(diào)節(jié)，調(diào)整至設(shè)定方向后進(jìn)行跳躍，落地后進(jìn)行自動復(fù)位，MJR姿態(tài)調(diào)整完畢之后利用UWB基站進(jìn)行定位，并將定位結(jié)果發(fā)送至上位機(jī)，每組實(shí)驗(yàn)共執(zhí)行8次連續(xù)單跳運(yùn)動。矩形跳躍中，MJR初始位置位于點(diǎn)(150，300)處，發(fā)送指令控制MJR自主按照順時針方向跳躍，每個方向進(jìn)行4次跳躍，使得運(yùn)行軌跡基本符合矩形。

測量每次彈跳落地后的實(shí)際位置以及UWB返回的位置信息，在MATLAB中繪制折線圖表征實(shí)驗(yàn)運(yùn)動軌跡，如圖18、圖19所示。

圖18 直線跳躍運(yùn)動及定位效果

圖19 矩形跳躍運(yùn)動及定位效果

通過直線以及矩形運(yùn)動軌跡可以看出，基于姿態(tài)檢測的MJR自復(fù)位以及航向調(diào)節(jié)功能具備一定的穩(wěn)定性。MJR在運(yùn)動過程中，可能出現(xiàn)某幾跳落點(diǎn)不夠均勻，其產(chǎn)生的原因主要是是MJR落地時二次起彈，此外MJR落地后自復(fù)位會導(dǎo)致一定的位置偏移。

通過分析跳躍運(yùn)動中實(shí)際落點(diǎn)與UWB定位落點(diǎn)位置的差距，得到UWB定位的平均偏差為9.08 cm，定位誤差基本能保持在23 cm以內(nèi)，這反映了MJR采用UWB定位方式具備一定的穩(wěn)定性與精確性。

4 結(jié)論

本文基于實(shí)驗(yàn)室早期研制的仿生MJR，利用IMU對機(jī)器人的姿態(tài)進(jìn)行檢測，實(shí)現(xiàn)了自主復(fù)位功能以及自主航向調(diào)節(jié)功能，并通過實(shí)驗(yàn)驗(yàn)證了穩(wěn)定性與精確性。此外，本文對基于UWB定位方式進(jìn)行研究，并通過實(shí)驗(yàn)驗(yàn)證了該定位方式的可行性與精確性?；谏鲜龌A(chǔ)功能設(shè)計(jì)以及UWB定位的實(shí)現(xiàn)，進(jìn)行了直線和矩形軌跡的部署運(yùn)動實(shí)驗(yàn)測試，對每次落點(diǎn)進(jìn)行定位測試，得到平均定位精度為9.08 cm。本文所提出的基于IMU和UWB的MJR位姿檢測方法，可為負(fù)載受限的移動傳感器節(jié)點(diǎn)位姿檢測提供借鑒；所實(shí)現(xiàn)的MJR自部署功能可提高復(fù)雜環(huán)境中移動傳感器網(wǎng)絡(luò)的網(wǎng)絡(luò)覆蓋率，實(shí)現(xiàn)環(huán)境監(jiān)測、災(zāi)害搜救中繼等功能，具有很好的應(yīng)用前景。