多傳感器數(shù)據(jù)融合下機(jī)器人路徑分段規(guī)劃仿真

王紅梅，陳 冬

(1.山西工程技術(shù)學(xué)院大數(shù)據(jù)與信息工程科學(xué)系，山西 陽(yáng)泉 045000；2.山西大同大學(xué)數(shù)學(xué)與計(jì)算機(jī)科學(xué)學(xué)院 山西 大同 037009)

1 引言

隨著軍事、工業(yè)、航空航天領(lǐng)域?qū)κ褂脵C(jī)器人的需求日益增多，大力研發(fā)機(jī)器人技術(shù)[1]成為了熱門(mén)課題。因機(jī)器人憑借較好的靈活性與獨(dú)立性等優(yōu)勢(shì)，還被廣泛應(yīng)用于服務(wù)業(yè)、醫(yī)療業(yè)、日常生活等各個(gè)領(lǐng)域，使人類(lèi)在生產(chǎn)生活等諸多方面都得到極大程度的改善與革新。為滿(mǎn)足當(dāng)今的應(yīng)用需求，機(jī)器人既要能夠很好地適應(yīng)環(huán)境，還需要有理想的智能控制系統(tǒng)，而機(jī)器人的路徑規(guī)劃作為該研究方向的一個(gè)關(guān)鍵課題，受到眾多相關(guān)學(xué)者的深入探討。

比如，文獻(xiàn)[2]張杰等人在研究水下群機(jī)器人路徑規(guī)劃算法形式化的過(guò)程中，構(gòu)建出一種基于定理證明器HOL4的遺傳算法形式化模型，該模型具有合理性與有效性；李靖等人[3]在灰狼優(yōu)化算法中引入Logistic混沌映射進(jìn)行優(yōu)化，將其與執(zhí)行大任務(wù)量監(jiān)測(cè)任務(wù)的機(jī)器人相結(jié)合，實(shí)現(xiàn)大規(guī)模任務(wù)量的有效路徑規(guī)劃；李娟等人[4]針對(duì)蟻群系統(tǒng)的種群多樣性與收斂速度問(wèn)題，利用動(dòng)態(tài)混沌算子構(gòu)建動(dòng)態(tài)混沌蟻群系統(tǒng)，應(yīng)用于機(jī)器人路徑規(guī)劃中，加快規(guī)劃效率。

影響機(jī)器人路徑規(guī)劃效果因素較多，上述文獻(xiàn)方法極易造成時(shí)間與空間信息上的缺失與冗余，因此，本文引用多傳感器數(shù)據(jù)融合技術(shù)，提出一種機(jī)器人路徑分段規(guī)劃方法。從多級(jí)別、多層次、多方面處理多個(gè)傳感器數(shù)據(jù)的技術(shù)就叫做多傳感器信息融合技術(shù)[5]，其應(yīng)用領(lǐng)域有工業(yè)機(jī)器人、金融系統(tǒng)、船舶避碰、空中交通管制、軍事戰(zhàn)略等。該項(xiàng)技術(shù)可有效補(bǔ)充缺失信息，優(yōu)化冗余數(shù)據(jù)，使環(huán)境得到極大程度認(rèn)知，強(qiáng)化傳感器性能，既提升機(jī)器人路徑規(guī)劃的適用范圍，也降低傳感器元件生成的累積誤差，且在機(jī)器人移動(dòng)定位、障礙物躲避、路徑規(guī)劃等重要環(huán)節(jié)中得到了理想的仿真效果；通過(guò)平滑處理規(guī)劃路徑，消除因路徑生成較多折線(xiàn)與拐點(diǎn)而產(chǎn)生的鋸齒效應(yīng)，解決機(jī)器人碰撞問(wèn)題；利用轉(zhuǎn)彎策略，抑制機(jī)器人本體寬度影響，防止其突然變向、發(fā)生碰撞；在簡(jiǎn)化運(yùn)算方面，分別采取了做正切圓、將機(jī)器人車(chē)輪寬度算入整體寬度中等策略。

2 機(jī)器人路徑狀態(tài)變量估計(jì)值分析

為提升機(jī)器人路徑規(guī)劃的適用范圍，降低傳感器元件生成的累積誤差[6]，融合機(jī)器人超聲波測(cè)距儀等多個(gè)傳感器的測(cè)量數(shù)據(jù)。

(1)

假設(shè)機(jī)器人在[k，k+1]時(shí)段內(nèi)未發(fā)生軌跡，測(cè)距量i的權(quán)重為wi，則利用加權(quán)最小二乘法[8]構(gòu)建下列表達(dá)式

(2)

(3)

其中

(4)

(5)

(6)

(7)

(8)

(9)

式中，θ為機(jī)器人正前方方向與順時(shí)針旋轉(zhuǎn)90°方向所呈夾角，b為機(jī)器人履帶寬度，ΔSl、ΔSr分別為左右兩條履帶的移動(dòng)距離。

(10)

(11)

得到機(jī)器人運(yùn)行軌道偏方差為

S′(k+1)=H(k+1)P(k+1|k)H(k+1)T+R(k+1)

(12)

(13)

P(k+1|k+1)=P(k+1|k)-

K(k+1)S′(k+1)K(k+1)-1

(14)

K(k+1)=p(k+1|k)H(k+1)TS′(k+1)-1

(15)

通過(guò)更新機(jī)器人運(yùn)行路徑狀態(tài)，結(jié)合方差，對(duì)路徑數(shù)據(jù)進(jìn)行卡爾曼濾波[9]，得到濾波結(jié)果表達(dá)式為

(16)

(17)

(18)

給定輸入量與測(cè)量值的協(xié)方差數(shù)據(jù)信息后，通過(guò)融合多傳感器數(shù)據(jù)，得到機(jī)器人不同采樣時(shí)刻的狀態(tài)，獲取其路徑規(guī)劃結(jié)果。

3 機(jī)器人路徑分段規(guī)劃

機(jī)器人在需要躲避路徑障礙物時(shí)，會(huì)存在較大路徑曲率的路段，應(yīng)先減速慢行，確保平穩(wěn)過(guò)彎，待經(jīng)過(guò)該路段需加速移動(dòng)一段距離，之后再勻速通行。因此，以路徑軌跡曲率極值點(diǎn)[11]為依據(jù)，分段處理各段路徑上的機(jī)器人移動(dòng)速度。

3.1 平滑處理

為消除因路徑生成較多折線(xiàn)與拐點(diǎn)而產(chǎn)生的鋸齒效應(yīng)，解決機(jī)器人碰撞問(wèn)題，需平滑處理規(guī)劃路徑。根據(jù)規(guī)劃路徑各分段點(diǎn)，連接當(dāng)前分段點(diǎn)與下一個(gè)相鄰分段點(diǎn)，逐段展開(kāi)平滑處理至全路徑結(jié)束。具體執(zhí)行流程描述如下：

1)根據(jù)多傳感器數(shù)據(jù)融合過(guò)程中得到的機(jī)器人定位點(diǎn)，分析最終路徑規(guī)劃結(jié)果上各分段點(diǎn)；

2)假設(shè)當(dāng)前分段點(diǎn)是下一個(gè)路徑起始點(diǎn)，若存在當(dāng)前分段點(diǎn)的子分段點(diǎn)，則進(jìn)入下一流程；反之，則停止平滑處理；

3)遍歷起始點(diǎn)與下一個(gè)當(dāng)前分段點(diǎn)，若獲取的返回值是真值，則進(jìn)入下一流程；反之，則跳至第(5)步；

4)用下一個(gè)當(dāng)前分段點(diǎn)賦值當(dāng)前分段點(diǎn)，與此同時(shí)，去除之前的各當(dāng)前分段點(diǎn)，返回第(2)步；

5)用當(dāng)前分段點(diǎn)賦值起始點(diǎn)，用下一個(gè)當(dāng)前分段點(diǎn)賦值當(dāng)前分段點(diǎn)，返回第(2)步；

6)終止路徑平滑處理。

平滑處理工程中，需按照固定間隔進(jìn)行遍歷、采樣，依序處理各分段路徑。

3.2 轉(zhuǎn)彎策略

為抑制機(jī)器人本體寬度影響，防止突然變向、發(fā)生碰撞，設(shè)計(jì)出一種合理的轉(zhuǎn)彎策略。假設(shè)機(jī)器人轉(zhuǎn)彎半徑是R，圓心是O，以起始點(diǎn)為參照的圓心方向是DO，起始點(diǎn)方向是Dstart，x、y表示各點(diǎn)對(duì)應(yīng)橫縱坐標(biāo)，則當(dāng)機(jī)器人右轉(zhuǎn)時(shí)，用下列表達(dá)式描述圓心O的轉(zhuǎn)動(dòng)方向與位置

(19)

(20)

已知點(diǎn)Q是加速區(qū)域的結(jié)束點(diǎn)，則基于三角定理可得該點(diǎn)與下一分段點(diǎn)的間距d′、距離LQ與半徑R所呈夾角θ′，計(jì)算公式如下所示

(21)

由此推導(dǎo)出下列點(diǎn)Q的轉(zhuǎn)動(dòng)方向與位置計(jì)算公式

(22)

當(dāng)機(jī)器人向左轉(zhuǎn)，將圓心O的方向加90°，點(diǎn)Q坐標(biāo)求解過(guò)程中把φQ+θ′改為φQ-θ′。

當(dāng)機(jī)器人在起始點(diǎn)P′1與目標(biāo)點(diǎn)P′2的轉(zhuǎn)彎方向均為順時(shí)針?lè)较驎r(shí)，則兩點(diǎn)間距的長(zhǎng)度LP′1P′2與斜率k′P′1P′2等于以?xún)牲c(diǎn)為圓心的圓外切線(xiàn)AB長(zhǎng)度LAB與斜率k′AB，即

(23)

式中，離開(kāi)第一段弧的角度是θ″arc1，抵達(dá)第二段弧的角度是θ″arc2。

若機(jī)器人在起始點(diǎn)P′1與目標(biāo)點(diǎn)P′2的轉(zhuǎn)彎方向是相對(duì)的，離開(kāi)時(shí)為順時(shí)針，抵達(dá)是逆時(shí)針，則對(duì)以目標(biāo)點(diǎn)為圓心的圓做正切圓，以降低運(yùn)算復(fù)雜度，設(shè)正切圓圓心為點(diǎn)P′3，因此，基于三角形P′1P′2P′3，推導(dǎo)出下列方程組

(24)

式中，兩目標(biāo)點(diǎn)間距為L(zhǎng)P′2P′3，∠P′1P′2P′3度數(shù)是θ′P′1P′2P′3，切線(xiàn)AB角度是θ′AB。根據(jù)得到的各分段路徑離開(kāi)與到達(dá)弧度，實(shí)現(xiàn)機(jī)器人移動(dòng)路徑的分段規(guī)劃，避免發(fā)生碰撞，跌倒。

4 機(jī)器人路徑分段規(guī)劃仿真

4.1 機(jī)器人

選取由驅(qū)動(dòng)控制模塊、傳感器模塊、GPS(Global Positioning System，全球定位系統(tǒng))定位模塊以及軟件控制模塊組成的機(jī)器人作為實(shí)驗(yàn)對(duì)象，規(guī)劃其移動(dòng)路徑。機(jī)器人具體參數(shù)信息如表1所示。

表1 實(shí)驗(yàn)用機(jī)器人參數(shù)統(tǒng)計(jì)表

開(kāi)啟軟件程序，進(jìn)入仿真界面，初始化處理機(jī)器人與路徑規(guī)劃環(huán)境，將其存儲(chǔ)于數(shù)據(jù)庫(kù)中，利用編程軟件執(zhí)行本文分段規(guī)劃方法，待機(jī)器人抵達(dá)指定目標(biāo)點(diǎn)后，終止路徑規(guī)劃，退出程序。方法運(yùn)行時(shí)，各傳感器持續(xù)讀取環(huán)境信息，不停往數(shù)據(jù)庫(kù)中存儲(chǔ)機(jī)器人位置信息。

4.2 機(jī)器人定位精度分析

超聲波傳感器的采樣時(shí)間是0.1秒，分別采用文獻(xiàn)[2]、[3]、[4]方法以及本文方法，融合一定時(shí)間里多個(gè)傳感器采集到的數(shù)據(jù)信息，整理得到表2。

表2 多傳感器數(shù)據(jù)融合測(cè)量值統(tǒng)計(jì)表

結(jié)合上表2中數(shù)據(jù)可知，本文方法經(jīng)過(guò)預(yù)測(cè)、觀(guān)測(cè)以及估計(jì)等一系列步驟，不斷更新?tīng)顟B(tài)變量與對(duì)應(yīng)協(xié)方差，獲取機(jī)器人不同采樣時(shí)刻狀態(tài)與最優(yōu)位置估計(jì)，所以，對(duì)比文獻(xiàn)方法的定位準(zhǔn)確度具有顯著優(yōu)越性，誤差均值不超過(guò)1厘米，機(jī)器人移動(dòng)路線(xiàn)與理想的預(yù)測(cè)軌跡較為趨近。因此，引入多傳感器數(shù)據(jù)融合為機(jī)器人路徑規(guī)劃奠定了良好的數(shù)據(jù)基礎(chǔ)。

4.3 路徑分段規(guī)劃仿真

為簡(jiǎn)化運(yùn)算復(fù)雜度，驗(yàn)證方法有效性，將機(jī)器人車(chē)輪寬度算入整體寬度中，并分別在簡(jiǎn)單障礙物與復(fù)雜障礙物兩種環(huán)境里展開(kāi)仿真。按照所得融合數(shù)據(jù)，規(guī)劃出機(jī)器人移動(dòng)路徑，分段規(guī)劃的最終路徑結(jié)果如圖1所示。

圖1 不同環(huán)境下路徑分段規(guī)劃仿真效果

根據(jù)上圖中得到的路徑結(jié)果可以看出，機(jī)器人不論是在簡(jiǎn)單障礙物還是更接近實(shí)際環(huán)境的復(fù)雜障礙物環(huán)境中，都規(guī)劃出了較為理想的路徑，不僅有效躲避了環(huán)境中的障礙物，而且規(guī)劃的路徑也比較合理；從機(jī)器人移動(dòng)狀態(tài)而言，未出現(xiàn)任何晃動(dòng)、跌倒趨勢(shì)。這是因?yàn)楸疚姆椒ǜ鶕?jù)路徑軌跡曲率極值點(diǎn)，分段處理了各段路徑上的機(jī)器人移動(dòng)速度，通過(guò)平滑處理與轉(zhuǎn)彎策略，有效實(shí)現(xiàn)了機(jī)器人移動(dòng)路徑的分段規(guī)劃，防止了碰撞與跌倒的發(fā)生。

5 結(jié)論

基于給定的環(huán)境信息程度，一般將路徑規(guī)劃方法劃分成兩種：全局、局部。前者常用于障礙物信息已知、固定、可預(yù)見(jiàn)等情況下，而后者則通過(guò)傳感器信息，在未知、動(dòng)態(tài)的環(huán)境中，展開(kāi)路徑規(guī)劃。將進(jìn)一步提升機(jī)器人障礙物躲避與環(huán)境識(shí)別能力、實(shí)時(shí)性與靈活性作為下一個(gè)研究重點(diǎn)；應(yīng)嘗試引用AS-R機(jī)器人視覺(jué)系統(tǒng)，增加其在三維空間中自由運(yùn)動(dòng)的能力；應(yīng)在完全未知的環(huán)境中仿真規(guī)劃方法，拓展方法適用性；僅以單機(jī)器人為研究對(duì)象具有一定的局限性，需在今后的工作中，深入探索多機(jī)器人的路徑分段規(guī)劃策略；路徑規(guī)劃應(yīng)用領(lǐng)域多種多樣，應(yīng)通過(guò)分析環(huán)境信息，采取對(duì)應(yīng)的規(guī)劃方法。