一種復(fù)雜地形下智能移動(dòng)機(jī)器人路徑規(guī)劃與控制研究

劉飛

關(guān)鍵詞：復(fù)雜地形；移動(dòng)機(jī)器人；自抗擾控制；隨機(jī)生成樹(shù)

0 引言

復(fù)雜地形下智能移動(dòng)機(jī)器人研究非常重要，這一研究領(lǐng)域已經(jīng)成了機(jī)器人技術(shù)的熱點(diǎn)之一[1-2]。隨著科技的不斷發(fā)展，越來(lái)越多的工作被機(jī)器人所代替[3]。在很多場(chǎng)景中，特別是在工業(yè)生產(chǎn)、軍事作戰(zhàn)、海洋勘探、醫(yī)療衛(wèi)生等領(lǐng)域，機(jī)器人已經(jīng)發(fā)揮了重要作用，而智能移動(dòng)機(jī)器人則是其中的重要一環(huán)[4]。因此，相關(guān)的理論研究需要進(jìn)一步提升[5-8]。在復(fù)雜地形下，智能移動(dòng)機(jī)器人需要具備一定的智能和自主學(xué)習(xí)能力，能夠通過(guò)感知和決策，完成任務(wù)并避免意外事故的發(fā)生[9]。同時(shí)，智能移動(dòng)機(jī)器人還需要具備一定的適應(yīng)性，能夠應(yīng)對(duì)不同的環(huán)境和任務(wù)需求[10]。因此，智能移動(dòng)機(jī)器人的研究需要綜合運(yùn)用規(guī)劃與運(yùn)籌、機(jī)器學(xué)習(xí)、控制理論等多學(xué)科知識(shí)，不斷提升機(jī)器人的智能和自主性[11]。在工業(yè)生產(chǎn)中，智能移動(dòng)機(jī)器人可以實(shí)現(xiàn)物流自動(dòng)化、裝配自動(dòng)化、巡檢自動(dòng)化等多種功能，提高制造效率和質(zhì)量[12]。在軍事作戰(zhàn)中，智能移動(dòng)機(jī)器人可以用于情報(bào)偵察、無(wú)人偵察、戰(zhàn)場(chǎng)救援等多個(gè)方面，提高作戰(zhàn)效率和安全性[13]。在海洋勘探中，智能移動(dòng)機(jī)器人可以用于深?？碧?、海洋環(huán)境監(jiān)測(cè)等領(lǐng)域，提高勘探效率和減少人員風(fēng)險(xiǎn)[14]。在醫(yī)療衛(wèi)生領(lǐng)域，智能移動(dòng)機(jī)器人可以用于手術(shù)輔助、康復(fù)訓(xùn)練、護(hù)理等方面，提高醫(yī)療水平和服務(wù)質(zhì)量[15]。但在實(shí)際應(yīng)用中，智能移動(dòng)機(jī)器人還需要考慮到安全性、可靠性、可操作性等多種因素。綜合以上應(yīng)用場(chǎng)景，智能移動(dòng)機(jī)器人的研究不僅僅是理論探討，更需要實(shí)際應(yīng)用與驗(yàn)證。只有在實(shí)際應(yīng)用中不斷優(yōu)化和完善，才能真正實(shí)現(xiàn)機(jī)器人技術(shù)的突破和發(fā)展[16-20]?？傊瑥?fù)雜地形下智能移動(dòng)機(jī)器人研究非常重要，會(huì)在很多領(lǐng)域帶來(lái)巨大的工程價(jià)值。隨著技術(shù)的不斷進(jìn)步和應(yīng)用的不斷拓展，智能移動(dòng)機(jī)器人的研究和發(fā)展將會(huì)成為機(jī)器人領(lǐng)域的重要發(fā)展方向之一。

1 移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃

針對(duì)移動(dòng)機(jī)器人執(zhí)行復(fù)雜地形下運(yùn)動(dòng)的特殊性，需要考慮到復(fù)雜地形的特殊性，才能夠真正地規(guī)劃出一條從任務(wù)起點(diǎn)到終點(diǎn)的最優(yōu)或次優(yōu)路徑[14]。通過(guò)對(duì)常見(jiàn)復(fù)雜地形的分析與抽象，設(shè)定以下的規(guī)劃約束條件。具體包括三個(gè)重要條件：

1） 機(jī)器人移動(dòng)時(shí)應(yīng)遵循最優(yōu)路線(xiàn)，包括路徑最短及運(yùn)動(dòng)時(shí)間最少；

2） 機(jī)器人所處的復(fù)雜地形存在障礙物，且地面不平整存在起伏現(xiàn)象；

3） 機(jī)器人與障礙物之間存在安全距離。

綜上，以上因素都是路徑規(guī)劃算法需要考慮到的約束條件。經(jīng)分析后需要考慮以下具體內(nèi)容。

1.1 空間坐標(biāo)系定義

移動(dòng)機(jī)器人在復(fù)雜地形中運(yùn)動(dòng)時(shí)，因?yàn)椴粌H存在起伏地形，能夠抽象為凹型及凸型地面。常規(guī)的位置及姿態(tài)描述一般只考慮OX、OY 及偏航角。但本文研究的機(jī)器人在復(fù)雜地形，特別是存在起伏地形，所以將常規(guī)的位姿描述拓展為六自由度模型，包括表示位置的三個(gè)向量和表示姿態(tài)的三個(gè)向量。

首先建立坐標(biāo)系，包括大地坐標(biāo)系OE XEYE ZE 和機(jī)體坐標(biāo)系OB XBYB ZB，坐標(biāo)系的定義滿(mǎn)足右手定則，如圖1所示。其中，大地坐標(biāo)系原點(diǎn)OE 綁定于地球質(zhì)心，OE XE 指向正北方向，OEYE 指向正西方向，OE ZE 指向地球質(zhì)心；移動(dòng)機(jī)器人幾何中心與質(zhì)心重合，并且作為機(jī)體坐標(biāo)系的原點(diǎn)OB，OB XB 指向機(jī)器人前進(jìn)方向。通過(guò)歐拉角形式表示移動(dòng)機(jī)器人的姿態(tài)，定義如下：繞OB XB 軸旋轉(zhuǎn)的角度定義為滾轉(zhuǎn)角φ，繞OBYB 軸旋轉(zhuǎn)的角度為俯仰角θ，繞OB ZB 軸旋轉(zhuǎn)的角度為偏航角ψ。

1.2 安全運(yùn)動(dòng)距離

移動(dòng)機(jī)器人的路徑規(guī)劃問(wèn)題是解決機(jī)器人在空間中最佳運(yùn)動(dòng)路徑的優(yōu)化問(wèn)題，需要設(shè)計(jì)出一個(gè)能夠快速有效處理搜索算法，能夠高效求解出最優(yōu)的空間運(yùn)動(dòng)路徑，同時(shí)確保機(jī)器人能與障礙物間保持最小安全運(yùn)動(dòng)距離，如圖2 所示。假設(shè)在大地坐標(biāo)系OE XEYE ZE 中，pi 表示為移動(dòng)機(jī)器人在復(fù)雜地形下的位置向量；obj是運(yùn)動(dòng)空間下的障礙物；d 是機(jī)器人與障礙物間的安全運(yùn)動(dòng)距離。則機(jī)器人在空間中的位置pi、障礙物obj、距離d滿(mǎn)足以下條件，如式（1） 所示。

Dis（ pi，obj ） ≥ d （1）

其中，Dis（ pi，obj ） 表示機(jī)器人在空間中的位置pi到障礙物obj之間的距離。

1.3 機(jī)身尺寸

在復(fù)雜地形存在地面不平整、狹小通道等狀態(tài)，需要將機(jī)器人的自身尺寸作為路徑規(guī)劃的先驗(yàn)條件。如圖3所示，表示移動(dòng)機(jī)器人的尺寸。W1、W2表示上視角度下的尺寸，W3表示側(cè)視角度下的尺寸。

2 移動(dòng)機(jī)器人控制算法設(shè)計(jì)

2.1 動(dòng)力學(xué)建模

定義好復(fù)雜環(huán)境下的坐標(biāo)系之后，同時(shí)綜合考慮機(jī)身尺寸。根據(jù)機(jī)器人學(xué)有關(guān)理論及工程實(shí)現(xiàn)的目標(biāo)，通過(guò)牛頓-歐拉方法建立該機(jī)器人的動(dòng)力學(xué)模型。有關(guān)的位置和姿態(tài)由向量形式予以表示。其中，位置向量為（x，y，z ），姿態(tài)向量為（?，θ，ψ），采用歐拉角形式，單位為度（°，degree） 。同時(shí)定義機(jī)體坐標(biāo)系下的角速度向量為（ p，q，r ），所以根據(jù)牛頓-歐拉方法建立的動(dòng)力學(xué)模型有：

其中，fi 是動(dòng)力系統(tǒng)作用于整個(gè)移動(dòng)機(jī)器人上的總驅(qū)動(dòng)力?？傭?qū)動(dòng)力在機(jī)體坐標(biāo)系OB XBYB ZB 分解為三維分量，τi 是總驅(qū)動(dòng)力矩在機(jī)體坐標(biāo)系上的三維分量。M 是移動(dòng)機(jī)器人的質(zhì)量，IB是轉(zhuǎn)動(dòng)慣量矩陣。

2.2 控制器設(shè)計(jì)

為實(shí)現(xiàn)移動(dòng)機(jī)器人適應(yīng)復(fù)雜地形環(huán)境中的運(yùn)動(dòng)，需要機(jī)器人的控制器具備自適應(yīng)能力，同時(shí)也要對(duì)環(huán)境干擾具備一定的抗擾能力。本文研究是在高壓強(qiáng)電磁環(huán)境中使用，環(huán)境復(fù)雜且干擾因素多，很難得到精確模型，因此選擇使用采用無(wú)模型的自抗擾控制方法（Active Disturbance Rejection Control，ADRC） 。同時(shí)考慮到需要對(duì)環(huán)境具有自適應(yīng)能力，因此選擇BP神經(jīng)網(wǎng)絡(luò)對(duì)ADRC進(jìn)行參數(shù)自適應(yīng)改進(jìn)。

在無(wú)模型控制方法中，ADRC是與工程應(yīng)用廣泛的，且能夠有效抵抗環(huán)境干擾的一類(lèi)方法[15]。國(guó)內(nèi)學(xué)者韓京清于1995年提出，通過(guò)期望輸入與反饋輸出間的誤差作為控制器輸入信號(hào)，再通過(guò)算法模塊輸出控制量消除誤差。主要算法模塊是三個(gè)：跟蹤微分器（TD） 、擴(kuò)張狀態(tài)觀(guān)測(cè)器（ESO） 和狀態(tài)誤差反饋率（SEF） 。這三個(gè)模塊互相配合后，能夠獲得很好的控制效果[16–18]。圖4是ADRC的典型結(jié)構(gòu)。

移動(dòng)機(jī)器人控制器的設(shè)計(jì)方法需要考慮到復(fù)雜地形下不同因素造成的多類(lèi)型擾動(dòng)，比如風(fēng)的力學(xué)作用、起伏地面造成的姿態(tài)快速變化等。而ADRC將所有擾動(dòng)因素視為一個(gè)“總擾動(dòng)”，通過(guò)控制器中的典型算法模塊將總擾動(dòng)作為控制目標(biāo)，實(shí)現(xiàn)控制的快速收斂，以期達(dá)到良好的控制效果。通過(guò)BP（Back Propa?gation） 神經(jīng)網(wǎng)絡(luò)技術(shù)對(duì)擴(kuò)張狀態(tài)觀(guān)測(cè)器進(jìn)行在線(xiàn)參數(shù)整定，通過(guò)引入自適應(yīng)ESO的方法，提高ESO對(duì)擾動(dòng)估計(jì)的精度，進(jìn)而提升控制效果。BP網(wǎng)絡(luò)典型結(jié)構(gòu)如圖5所示。

在復(fù)雜地形下，對(duì)環(huán)境運(yùn)動(dòng)中機(jī)器人的位置和姿態(tài)進(jìn)行控制?？刂破鲀?nèi)外環(huán)需要進(jìn)行設(shè)計(jì)，控內(nèi)環(huán)控制姿態(tài)變化，外環(huán)控制位置變化。同時(shí)，外環(huán)的位置控制輸出作為內(nèi)環(huán)姿態(tài)控制器的控制輸入。整體控制器設(shè)計(jì)如圖6所示：

位置控制器根據(jù)期望位置和實(shí)際位置計(jì)算出期望總拉力fd、期望滾轉(zhuǎn)角?d、期望俯仰角θd；姿態(tài)控制器根據(jù)位置控制器輸出的期望偏航角ψd、實(shí)際姿態(tài)以及位置控制器的輸出計(jì)算出期望力矩τd；然后，控制分配算法根據(jù)fd 和τd 計(jì)算出四個(gè)車(chē)輪的期望轉(zhuǎn)速ωd，i；最后，驅(qū)動(dòng)控制根據(jù)ωd，i 計(jì)算出每個(gè)驅(qū)動(dòng)電機(jī)的輸出功率σd，i。

3 移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃

機(jī)器人移動(dòng)的復(fù)雜空間能夠視為一個(gè)三維空間，特別是存在起伏型路面，并且移動(dòng)過(guò)程中會(huì)存在一些無(wú)法通過(guò)的障礙點(diǎn)。當(dāng)移動(dòng)機(jī)器人進(jìn)行巡檢路徑規(guī)劃時(shí)，需要將不平整的起伏點(diǎn)、無(wú)法通過(guò)的障礙點(diǎn)等非通行區(qū)域進(jìn)行整體規(guī)劃計(jì)算[19]。根據(jù)分析，首先選擇相關(guān)信息進(jìn)行全局規(guī)劃，生成多個(gè)路徑節(jié)點(diǎn)，將近鄰節(jié)點(diǎn)間做連線(xiàn)形成一條完整的運(yùn)動(dòng)軌跡，實(shí)現(xiàn)整體的規(guī)劃路徑求解。在規(guī)劃算法生成路徑以后，移動(dòng)機(jī)器人按照指規(guī)劃處的路徑從第一個(gè)節(jié)點(diǎn)出發(fā)，根據(jù)節(jié)點(diǎn)依次移動(dòng)，直至機(jī)器人到達(dá)最后節(jié)點(diǎn)。但是移動(dòng)機(jī)器人在執(zhí)行運(yùn)動(dòng)任務(wù)的過(guò)程中需要維持安全距離且要兼顧規(guī)避突發(fā)性障礙，因此需要同時(shí)結(jié)合局部動(dòng)態(tài)規(guī)劃來(lái)進(jìn)行實(shí)時(shí)規(guī)劃。

快速探索隨機(jī)樹(shù)（RRT）算法是運(yùn)動(dòng)規(guī)劃和機(jī)器人學(xué)中廣泛使用的算法。它最早是由Steve LaValle在1998年提出的，此后成為在靜態(tài)網(wǎng)絡(luò)中尋找最短路徑的流行方法。該算法的工作原理是在存儲(chǔ)所獲得的路徑數(shù)據(jù)的同時(shí)，向規(guī)劃空間增量地添加隨機(jī)樹(shù)節(jié)點(diǎn)。當(dāng)隨機(jī)樹(shù)節(jié)點(diǎn)滿(mǎn)足一定約束時(shí)，停止樹(shù)擴(kuò)展，從終點(diǎn)到起點(diǎn)進(jìn)行反向搜索，得到連接兩點(diǎn)的完整路徑。RRT的優(yōu)勢(shì)之一是能夠處理復(fù)雜和動(dòng)態(tài)的環(huán)境。該算法在解決高維狀態(tài)空間的問(wèn)題時(shí)尤其有效，在高維狀態(tài)空間中，傳統(tǒng)的搜索算法可能會(huì)因?yàn)榫S度災(zāi)難而失敗。此外，RRT可以處理機(jī)器人系統(tǒng)中常見(jiàn)的非完整約束。RRT已被應(yīng)用于多個(gè)領(lǐng)域，包括自動(dòng)駕駛、機(jī)器人和電腦游戲。在自動(dòng)駕駛中，RRT可以用來(lái)規(guī)劃自動(dòng)駕駛汽車(chē)的軌跡，避開(kāi)障礙物，安全到達(dá)目的地。在機(jī)器人科學(xué)中，RRT可以幫助機(jī)器人在復(fù)雜的環(huán)境中導(dǎo)航，并執(zhí)行抓取和操縱等任務(wù)。因此，RRT是一個(gè)有效的算法，可以在復(fù)雜和動(dòng)態(tài)的環(huán)境中高效地規(guī)劃路徑。憑借其多功能性和靈活性，它已成為運(yùn)動(dòng)規(guī)劃和機(jī)器人學(xué)中的基本工具，也非常適用于本文的研究?jī)?nèi)容。算法流程如圖8所示。

將整個(gè)移動(dòng)機(jī)器人規(guī)劃的地圖抽象為圖的表示形式：

其中，V 是隨機(jī)樹(shù)中節(jié)點(diǎn)的集合，E 是隨機(jī)樹(shù)中邊的集合。隨機(jī)樹(shù)邊的和就是不同節(jié)點(diǎn)間的距離和。通過(guò)RRT算法求解出最優(yōu)的航跡節(jié)點(diǎn)，可以作為前文所述控制器的路徑規(guī)劃生成器。這種方法可以使機(jī)器人在環(huán)境中進(jìn)行路徑規(guī)劃，避免了碰撞和障礙物，并且不需要提前建立地圖?？刂破鹘Y(jié)構(gòu)可以根據(jù)路徑規(guī)劃的需要進(jìn)行更新，圖7是更新后的控制器結(jié)構(gòu)。

4 系統(tǒng)驗(yàn)證

移動(dòng)機(jī)器人控制器性能需要設(shè)計(jì)合理的實(shí)驗(yàn)驗(yàn)證規(guī)劃算法與控制律的有效性，尤其需要測(cè)試存在障礙物及起伏路面條件下的控制器與路徑規(guī)劃的性能與有效性。因此，基于Matlab進(jìn)行了數(shù)值仿真，多個(gè)測(cè)試任務(wù)中的典型結(jié)果如圖9、10所示，圖9是位置軌跡變化，圖10是姿態(tài)變化。

從圖中可明顯地看到，軌跡是平滑并且穩(wěn)定的，快速經(jīng)過(guò)規(guī)劃后的目標(biāo)位置點(diǎn)，在經(jīng)過(guò)某些起伏路面時(shí)會(huì)發(fā)生小幅度姿態(tài)變化，總體上機(jī)器人的姿態(tài)始終是穩(wěn)定的。說(shuō)明控制器的控制效果達(dá)到了預(yù)期的目標(biāo)，能夠?qū)崿F(xiàn)復(fù)雜地形下的快速規(guī)劃軌跡及位置控制。

5 結(jié)論

針對(duì)復(fù)雜環(huán)境下移動(dòng)機(jī)器人任務(wù)規(guī)劃與控制，綜合了環(huán)境下的起伏地形及障礙物。首先，通過(guò)牛頓- 歐拉方法建立了機(jī)器人的動(dòng)力學(xué)模型，并分別建立了大地坐標(biāo)系與機(jī)體坐標(biāo)系。然后使用BP神經(jīng)網(wǎng)絡(luò)對(duì)ADRC控制器進(jìn)行修改，完成控制器的改進(jìn)，并且通過(guò)隨機(jī)生成樹(shù)算法開(kāi)發(fā)了路徑規(guī)劃器，實(shí)時(shí)產(chǎn)生最優(yōu)期望位置坐標(biāo)。然后通過(guò)Matlab仿真實(shí)驗(yàn)證明了系統(tǒng)設(shè)計(jì)有效性，能夠?qū)崿F(xiàn)復(fù)雜地形下的移動(dòng)任務(wù)。