路徑規(guī)劃的可定位性研究

高 揚(yáng)，劉 江，李鹍鵬

長(zhǎng)安大學(xué) 汽車學(xué)院，西安 710054

1 引言

路徑規(guī)劃結(jié)果的實(shí)現(xiàn)通常高度依賴于機(jī)器人準(zhǔn)確的定位結(jié)果。已有對(duì)路徑規(guī)劃的研究中多集中于如何根據(jù)各種條件找到一條最優(yōu)的路徑，常用條件如：路徑最短、時(shí)間最優(yōu)、便于執(zhí)行、能夠避開障礙物等[1]。然而基于此類條件所規(guī)劃出來的路徑很少考慮到路徑的可定位性，這使得機(jī)器人在跟蹤路徑的過程中很有可能因定位誤差過大而導(dǎo)致路徑追蹤失敗，因此在進(jìn)行路徑規(guī)劃時(shí)將可定位性考慮進(jìn)來具有重要的意義。本文主要研究常見的雙輪差速機(jī)器人利用激光掃描儀，基于地圖匹配與航跡推算的定位方法，建立了兩者的定位誤差方程，分別討論了其可定位性對(duì)路徑規(guī)劃的影響[2]。

將定位的不確定性用分布的方差來描述在很多研究中都有被提到[3]。Roy最早在研究海岸線導(dǎo)航提出通過定位概率分布的熵值來反映機(jī)器人的可定位性[4]。Li等提出使用地圖來輔助測(cè)量定位系統(tǒng)提供的信息可信度[5]。Censi等人基于Cramer-Rao定理從理論上給出了基于地圖匹配定位的精度上限[6]。在Censi 等人的基礎(chǔ)上又發(fā)展出來一系列研究機(jī)器人可定位性估計(jì)的研究成果。Qian 等人提出了一種引入動(dòng)態(tài)障礙物影響因素的定位估計(jì)方法[2]。Wang 等人提出了一種基于可定位性的機(jī)器人動(dòng)作選擇機(jī)制，從而加速機(jī)器人全局定位的速度[7]。王煒等通過將概率柵格離散化，將Fisher 信息矩陣的應(yīng)用擴(kuò)展到概率柵格地圖中去[8]。Wang 等人基于概率柵格地圖和Cramer-Rao 邊界的激光測(cè)距模型，將先驗(yàn)地圖的不確定性考慮進(jìn)來，加快了機(jī)器人的定位速度[9]。孫自飛等人利用離散化Fisher信息矩陣在線估計(jì)觀測(cè)信息的可定位性矩陣，同時(shí)通過預(yù)測(cè)模型協(xié)方差矩陣評(píng)估里程計(jì)信息的可靠性，從而提高機(jī)器人的定位準(zhǔn)確性和可靠性[10]。

近年來，也有不少研究是將機(jī)器人的可定位性考慮到路徑規(guī)劃中去。Censi 等通過計(jì)算期望激光數(shù)據(jù)以及激光掃描到的環(huán)境表面斜率，得到定位Fisher 信息矩陣，該矩陣可以反映可定位性的整體性和方向性，然而沒有考慮地圖信息的不確定性對(duì)可定位性評(píng)估的影響[11]。還有一些研究嘗試使機(jī)器人在跟蹤路徑時(shí)位置不確定性最小，但是它們要求激光的掃描范圍是360°，這就限制了其使用范圍[12-15]。Irani等通過計(jì)算LM（Localizability Measure）來排除定位性差的區(qū)域，在此基礎(chǔ)上再進(jìn)行路徑規(guī)劃，可在一定程度上避免路徑經(jīng)過可定位性差的區(qū)域，但是需要通過閾值來調(diào)整通行區(qū)域，有可能因?yàn)殚撝档脑O(shè)置過大而大幅擴(kuò)大不可通行區(qū)域[16]。

已有研究中，可定位性是如何影響路徑規(guī)劃，并且如何去衡量路徑的可定位性需要進(jìn)一步地分析。因此，本文主要集中研究可定位性的影響并提出包含可定位性的新的路徑評(píng)價(jià)函數(shù)。

2 可定位性對(duì)路徑規(guī)劃的影響

2.1 基于地圖匹配和航跡推算的定位方差分析

機(jī)器人的定位結(jié)果一般來源于航跡推算和地圖匹配兩種定位方法的融合，這樣可以獲得更好的定位效果。設(shè)δm_s為基于地圖匹配得到的定位估計(jì)方差，δd_s為航跡推算所得的定位估計(jì)方差，δf_s為融合后的定位估計(jì)方差。本文建立了δd_s的估計(jì)模型如式（1）和（2）所示：

其中，f()為機(jī)器人運(yùn)動(dòng)方程，Sk(xk,yk,θk)為機(jī)器人當(dāng)前時(shí)刻位姿，ek為機(jī)器人運(yùn)動(dòng)過程中所受到的噪聲，假設(shè)ek服從均值為0，方差為δe的高斯分布。當(dāng)忽略機(jī)器人中不準(zhǔn)確的運(yùn)動(dòng)時(shí)，δd_s可以通過式（3）計(jì)算得到：

δm_s可依據(jù)文獻(xiàn)[7]中方法由Fisher 信息矩陣求得其下限，如式（4）、（5）所示。riE為激光測(cè)距儀所得第i個(gè)激光束到障礙物的距離，δp為激光的白噪聲。根據(jù)Cramer-Rao 理論，則可建立對(duì)δf_s的估計(jì)，即公式（5）。由于通常將航跡推算結(jié)果與基于地圖匹配的定位結(jié)果融合使用。假設(shè)采用常用的卡爾曼濾波算法進(jìn)行融合，則根據(jù)信息融合理論，δf_s可以根據(jù)公式（6）計(jì)算得到。

2.2 基于地圖匹配和航跡推算的定位方差分析

δf_s代表融合定位后的方差，假設(shè)定位結(jié)果服從高斯分布，則其在概率分布上呈現(xiàn)為一個(gè)橢球的形狀，橢球體積大小代表了機(jī)器人定位的不確定度。令δf_xy表示δf_s在X-Y平面的投影，它呈現(xiàn)為橢圓形狀，代表機(jī)器人位姿估計(jì)在X-Y平面上的可能的分布范圍。令δf_θ表示對(duì)機(jī)器人朝向角θ的估計(jì)。因此，δf_s可以表示為 (δf_xy,δf_θ)。如圖1 中所示，令rs為障礙物的膨脹半徑，ra為目標(biāo)點(diǎn)的膨脹半徑，R為機(jī)器人跟蹤的路徑，‖δf_D‖為δf_xy形成的橢圓度的長(zhǎng)軸半徑，三角形代表機(jī)器人的位姿，黑色方塊代表障礙物，藍(lán)色點(diǎn)為給定的路徑的預(yù)瞄點(diǎn)，紅色的點(diǎn)表示路徑終點(diǎn)。

圖1 定位不確定性的影響

2.3 δf_xy 和 δf_θ 對(duì)路徑規(guī)劃的影響

如圖1中所示，假設(shè)機(jī)器人真實(shí)位姿落入該橢圓形包含的范圍的概率不低于1-α，則該橢圓的長(zhǎng)軸半徑‖δf_D‖必然滿足公式（7）：

在路徑規(guī)劃中，機(jī)器人應(yīng)盡可能靠近障礙物以獲得一條最短的路徑。而‖δf_D‖越大，機(jī)器人撞到障礙物的可能性就越大，為了減少機(jī)器人碰撞的風(fēng)險(xiǎn)，‖δf_D‖應(yīng)該滿足式（8）。同時(shí)為了減少由于定位不確定性導(dǎo)致機(jī)器人不能到達(dá)目標(biāo)點(diǎn)的風(fēng)險(xiǎn)，‖δf_D‖還應(yīng)該滿足式（9）。

對(duì)于角度誤差δf_θ對(duì)路徑規(guī)劃帶來的影響，本文分析了不同δf_θ條件下，機(jī)器人跟蹤路徑形成的軌跡長(zhǎng)度L，如圖2所示。

圖2 不同的δf_θ 的路徑長(zhǎng)度

分析結(jié)果表明δf_θ與L間存在如式（10）的關(guān)系：

其中X0為機(jī)器人到目標(biāo)點(diǎn)間的起始距離，而參數(shù)p1=-6.375 ；p2=3.766E+04 ；q1=-1.436E+04 ；q2=2.669E+04。

3 定位不確定性評(píng)價(jià)函數(shù)

在路徑規(guī)劃中通常借助評(píng)價(jià)函數(shù)將優(yōu)化目標(biāo)映射到一個(gè)有限的值域，采用不同的路徑規(guī)劃方法，所采用的評(píng)價(jià)函數(shù)形式也不一樣。本文采用文獻(xiàn)[15]中提出的基于粒子濾波的全局路徑規(guī)劃方法，因而采用sigmoid函數(shù)建立可定位影響評(píng)價(jià)函數(shù)，將 ‖δf_D‖和δf_θ對(duì)路徑規(guī)劃帶來的影響映射到[0，1]。δf_s表示融合和后的定位方差，可由(δf_xy,δf_θ)表示，‖δf_D‖為δf_xy在X-Y平面投影上圓的直徑，代表了δf_s在X-Y平面上定位方差的大小，‖δf_D‖和δf_θ值越小，代表著定位方差越小，機(jī)器人的定位越準(zhǔn)確。

3.1 δf_θ 影響的評(píng)價(jià)函數(shù)

如圖2所示，δf_θ對(duì)路徑規(guī)劃具有負(fù)面影響，所以采用的評(píng)價(jià)函數(shù)Eθ(δf_θ),如公式（11）所示：

Eθ(δf_θ)的值越小，對(duì)路徑規(guī)劃的影響更小。θu2是一個(gè)可變參數(shù)，由于當(dāng)時(shí)，此時(shí)機(jī)器人到達(dá)不了終點(diǎn)，所以取參數(shù)k2表示評(píng)價(jià)函數(shù)值的變化速率，根據(jù)實(shí)際情況調(diào)整。其他參數(shù)本文取k2=14.1，θu2=2，因此Eθ(1)=0.5，Eθ(1.4)≈1。

3.2 ‖ δ f_D ‖影響的評(píng)價(jià)函數(shù)

由3.1 節(jié)分析可知 ‖δf_D‖對(duì)路徑安全性呈負(fù)面影響，且需滿足如式（8）、式（9）要求。故設(shè)計(jì)評(píng)價(jià)函數(shù)E(‖δf_D‖)如式（12）：

E(‖δf_D‖)的值越小，代表效果越好。其中由式（8）、（9）得到ru為由式（13）限制的最小值。

參數(shù)k1控制著評(píng)價(jià)函數(shù)值改變大小。圖3 展示了它們之間的關(guān)系，其中k1分別取0.2，0.6，1，2，3，4。k1的取值要使得E(ru)≈1。故本文取k1=1，rs=50 cm ，rR=30 cm，ra=50 cm。

圖3 在不同k 取值情況下E(‖ δ f _D ‖)和 ‖ δ f _D ‖的關(guān)系

3.3 基于可定位性的可定位性評(píng)價(jià)函數(shù)

在文獻(xiàn)[17]中，采用了Ferguson 樣條曲線來描述路徑R，并提出評(píng)價(jià)函數(shù)E(R,MC)來平衡路徑長(zhǎng)度和機(jī)器人與障礙物之間的距離，E(R,MC)可由式（14）計(jì)算得知。結(jié)合本文所提出的可定性的評(píng)價(jià)函數(shù)，E(R,MC)可以擴(kuò)展成新的評(píng)價(jià)函數(shù)Ep(R,MC)，如式（15）所示：

新的評(píng)價(jià)函數(shù)在路徑規(guī)劃的傳統(tǒng)指標(biāo)與可定位性指標(biāo)之間做了平衡。式（14）中MC表示環(huán)境的地圖，ELmax(R)是由式（16）求得的路徑R的最大可定位評(píng)價(jià)值，ES(S)是給定路徑點(diǎn)S的可定性評(píng)價(jià)值。L是由式（17）定義的路徑R長(zhǎng)度，lmin是起點(diǎn)P0到目標(biāo)點(diǎn)Ta的最短距離。dmin為路徑R到障礙物的最短距離，由式（18）求得，α1是一個(gè)權(quán)重系數(shù)。Xi(T)、Yi(T)為曲線i在X-Y平面的坐標(biāo)，Xi′(T)、Yi′(T)為對(duì)T的導(dǎo)數(shù)。objx和objy為X-Y坐標(biāo)系中距離原點(diǎn)最近的障礙物的坐標(biāo)。α1,α2,α3是影響dmin,‖δf_D‖以及δf_θ的三個(gè)權(quán)重參數(shù)。

4 實(shí)驗(yàn)結(jié)果分析

為了驗(yàn)證本文所提出算法的效果，在圖4（a）所示的室內(nèi)環(huán)境進(jìn)行了實(shí)驗(yàn)，圖4（b）為實(shí)驗(yàn)所用概率柵格地圖。實(shí)驗(yàn)機(jī)器人采用先鋒機(jī)器人Pioneer 3-DX，如圖5所示配置了里程計(jì)和激光測(cè)距儀（UTM-30LX）。控制器采用戴爾電腦，系統(tǒng)為Ubantu16.04，內(nèi)存為8 GB。算法設(shè)置參數(shù)如下：激光掃描距離：Ld=3 m ，掃描范圍[0°，270°]，激光束數(shù)目n=1 080 ，掃描周期T=25 ms。

圖4 實(shí)驗(yàn)場(chǎng)地

圖5 Pioneer 3-DX機(jī)器人

圖6中紅色的實(shí)線是傳統(tǒng)的路徑規(guī)劃路線，藍(lán)色的實(shí)線是加入了本文提出的路徑評(píng)價(jià)函數(shù)生成的優(yōu)化路徑。在實(shí)驗(yàn)過程中機(jī)器人采用自適應(yīng)粒子濾波定位的方法實(shí)現(xiàn)自定位，圖6 中虛線代表機(jī)器人真實(shí)的軌跡。機(jī)器人在跟隨傳統(tǒng)路徑時(shí)，由于穿行的區(qū)域定位誤差過大，在實(shí)驗(yàn)過程中丟失了自己的位姿，不能成功地到達(dá)終點(diǎn)，而機(jī)器人在跟隨本文改進(jìn)后優(yōu)化的路徑，穿過的路徑區(qū)域可定位性比較好，成功地到達(dá)終點(diǎn)。

圖6 優(yōu)化后的路徑以及傳統(tǒng)路徑

圖7展示了機(jī)器人在跟隨優(yōu)化后的路徑時(shí)本文設(shè)計(jì)的四種可定位性評(píng)價(jià)指標(biāo)(‖δf_D‖,δf_θ,E(‖δf_D‖),Eθ(δf_θ))的變化，‖δf_D‖其中代表δf_xy形成的橢圓度的長(zhǎng)軸半徑，δf_θ代表朝向角的方差，E(‖δf_D‖)代表 ‖δf_D‖的評(píng)價(jià)函數(shù)值，Eθ(δf_θ)代表δf_θ評(píng)價(jià)函數(shù)值。

圖7 優(yōu)化路徑的四種評(píng)價(jià)指標(biāo)變化

圖8 展示了機(jī)器人在跟隨傳統(tǒng)路徑時(shí)四種評(píng)價(jià)指標(biāo)的變化。可以明顯看到在跟隨優(yōu)化后的路徑時(shí)四種評(píng)價(jià)指標(biāo)的值比較小，這使得機(jī)器人可以成功地到達(dá)目的地。相反機(jī)器人在跟隨傳統(tǒng)路徑時(shí)，從第75 處定位到115處定位，由于δf_θ比較大，使得機(jī)器人由于定位誤差比較大而不能達(dá)到終點(diǎn)。

5 結(jié)束語

本文主要研究定位不確定性對(duì)路徑規(guī)劃和路徑跟蹤的影響。通過將定位的不確定性分解為坐標(biāo)定位方差和朝向角估計(jì)方差降低了可定位性評(píng)估難度。分析了兩者對(duì)機(jī)器人路徑跟蹤的影響，在此基礎(chǔ)上，提出了一個(gè)評(píng)價(jià)函數(shù)來評(píng)價(jià)不確定性對(duì)路徑規(guī)劃影響。實(shí)驗(yàn)表明，評(píng)價(jià)函數(shù)成功地描述了不確定性的影響，使得機(jī)器人在進(jìn)行路徑規(guī)劃時(shí)可以在傳統(tǒng)指標(biāo)與可定位性指標(biāo)間取得平衡。

圖8 傳統(tǒng)路徑的四種評(píng)價(jià)指標(biāo)變化