基于地圖定位能力的路徑規(guī)劃方法
【專利摘要】本發(fā)明提供了一種基于地圖定位能力的路徑規(guī)劃方法，通過將地圖定位能力加入路徑節(jié)點(diǎn)代價函數(shù)，以達(dá)到優(yōu)化規(guī)劃路徑的目的。本發(fā)明的規(guī)劃方法包括以下部分：首先計算概率柵格地圖各點(diǎn)的定位能力，其次將概率柵格地圖各點(diǎn)加入路徑節(jié)點(diǎn)代價函數(shù)，最后通過路徑規(guī)劃算法進(jìn)行搜索，計算出從初始點(diǎn)至目標(biāo)點(diǎn)的移動路徑。應(yīng)用本發(fā)明所提出的路徑規(guī)劃方法具有提高移動單位在環(huán)境中的定位能力、避免移動單位定位失敗等特點(diǎn)，適用于民用、工業(yè)、軍事等各個領(lǐng)域。
【專利說明】
基于地圖定位能力的路徑規(guī)劃方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及路徑規(guī)劃技術(shù)領(lǐng)域，具體地，涉及一種基于地圖定位能力的路徑規(guī)劃 方法。
【背景技術(shù)】
[0002] 在機(jī)器人領(lǐng)域中，移動機(jī)器人所處的環(huán)境情況是相對復(fù)雜的。移動機(jī)器人的基本 需求之一為在定位導(dǎo)航過程中規(guī)劃出一條避開所有障礙物的路徑。在環(huán)境各點(diǎn)中由于可觀 測信息的不同，導(dǎo)致機(jī)器人在不同點(diǎn)有不同的定位能力（或稱定位能力）。在進(jìn)行路徑規(guī)劃 時須考慮該問題，沿著定位能力相對較強(qiáng)的區(qū)域進(jìn)行規(guī)劃，并避免因?yàn)槁窂焦?jié)點(diǎn)的定位能 力過低而引起機(jī)器人定位失敗甚至丟失的情況。
[0003] Kaelbling[l]等人提出了一種通過使定位過程中的期望熵值最低來提高定位預(yù) 測信息的準(zhǔn)確性的方法，但這種方法在規(guī)劃路徑時存在局部最小點(diǎn)的問題。Fox[2]等人提 出了一種主動選擇高定位精度的路徑的策略，他們在代價函數(shù)中引入了表示定位不確定性 的熵值。應(yīng)用熵的策略通常存在計算復(fù)雜度過大的問題，該問題在針對較大環(huán)境時尤為明 顯。同時，使用熵的方法還存在不能離線計算環(huán)境的不確定信息的問題，以及只能對不確定 的觀測估計值進(jìn)行一個整體評估的問題。
[0004] 參考文獻(xiàn)：
[0005] [l]L.P.Kaelbling,M.L.Littman,and A.R.Cassandra.Planning and acting in partially observable stochastic domains . Technical report,Brown University, 1995.
[0006] [2]D.Fox,ff.Burgard and S.Thrun."Active markov localization for mobile robots，"Robotics and Autonomous Systems,1998

【發(fā)明內(nèi)容】

[0007] 針對現(xiàn)有技術(shù)中的缺陷，本發(fā)明的目的是提供一種基于地圖定位能力的路徑規(guī)劃 方法。
[0008] 根據(jù)本發(fā)明提供的基于地圖定位能力的路徑規(guī)劃方法，包括如下步驟：
[0009] 步驟1:計算概率柵格地圖各點(diǎn)的定位能力；
[0010]步驟2:將概率柵格地圖各點(diǎn)的定位能力加入路徑節(jié)點(diǎn)代價函數(shù)，使用路徑規(guī)劃算 法進(jìn)行搜索，計算出目標(biāo)移動路徑。
[0011]優(yōu)選地，所述步驟1包括：
[0012] 步驟1.1:獲取待進(jìn)行路徑規(guī)劃的概率柵格地圖；具體地，包括采用SLAM算法對待 進(jìn)行路徑規(guī)劃的環(huán)境進(jìn)行建圖，得到環(huán)境的概率柵格地圖；
[0013] 步驟1.2:針對概率柵格地圖上的多個點(diǎn)，分別計算每個點(diǎn)的定位能力。
[0014] 優(yōu)選地，所述步驟1.2包括：
[0015] 步驟1.2.1:將機(jī)器人設(shè)置在概率柵格地圖上，記錄機(jī)器人的位姿坐標(biāo)為
[0016] P =〖?xì)狻?外七,外,％為/]，以坐標(biāo)？為球心，且已知機(jī)器人的傳感器感知半徑為1， 從而能夠確定機(jī)器人的最大掃描邊界，以A r為間隔進(jìn)行掃描，得到n條掃描射線；其中Xp， yP，zP為三維空間直角坐標(biāo)系下的坐標(biāo)，％為,％分別為xP，y P，zP對應(yīng)坐標(biāo)軸上的角度，Ar 為相鄰的兩束傳感器射線的間隔；
[0017] 步驟1.2.2:對每條掃描射線i計算τι方向上機(jī)器人到障礙物柵格的期望距離rlE;
[0018] 步驟1.2.3:求取柵格的靜態(tài)定位能力矩陣，計算公式如下：
[0020] 式中：ΔΓιΕ表示期望距離的單位偏差，八知表示位姿P及沿X軸方向移動的單位距 離，Α ^表示位姿Ρ及沿y軸方向移動的單位距離，△ 2[)表示位姿Ρ及沿ζ軸方向移動的單位距 離，傘Pp表示位姿P及沿爐角方向旋轉(zhuǎn)的單位角度，Α θρ表示位姿P及沿Θ角方向旋轉(zhuǎn)的單位 角度，Α %表示位姿Ρ及沿φ角方向旋轉(zhuǎn)的單位角度；
·表示位姿P及沿X軸方向移動單位距 離處兩者期望距離rlE的偏差
表示位姿P及沿y軸方向移動單位距離處兩者期望距離rlE 的偏差
表示位姿P及沿z軸方向移動單位距離處兩者期望距離rlE的偏差
表示位姿 P及沿？^角方向旋轉(zhuǎn)單位角度后兩者期望距離rlE的偏差：
=表示位姿P及沿Θ角方向旋轉(zhuǎn)單 位角度后兩者期望距離rlE的偏差
表示位姿P及沿Φ角方向旋轉(zhuǎn)單位角度后兩者期望距 咼1^的偏差；
[0021] 其中：矩陣/:(7)的形式能夠根據(jù)實(shí)際使用的傳感器的不同轉(zhuǎn)化為三維全景、二維 全景、二維非全景中的任一種形式;三維全景是指機(jī)器人的位姿坐標(biāo)P只考慮含 Xp，yP，Zp項(xiàng)， 其余項(xiàng)為零;二維全景是指機(jī)器人的位姿坐標(biāo)P只考慮含&，^項(xiàng)，其余項(xiàng)為零;二維非全景 是指機(jī)器人的位姿坐標(biāo)P只考慮含&，7 [)，9[)項(xiàng)，其余項(xiàng)為零的形式;11為總射線數(shù)，0 2為傳感 器噪聲方差;所述單位距離和單位角度可根據(jù)實(shí)際需要確定；
[0022] 步驟1.2.4:計算?(Ρ)的行列式，并將結(jié)果作為反映柵格定位能力的代價L;對每個 柵格計算定位能力矩陣并計算行列式，行列式的計算結(jié)果即為該柵格的L值。
[0023] 優(yōu)選地，所述步驟2中根據(jù)不同的路徑規(guī)劃方法得到相應(yīng)的不同的路徑節(jié)點(diǎn)代價 函數(shù)，所述路徑節(jié)點(diǎn)代價函數(shù)均能夠加入定位能力作為規(guī)劃時的指標(biāo);每個柵格的定位能 力由每個柵格的L值反映。
[0024] 優(yōu)選地，所述步驟2中的路徑規(guī)劃算法包括:A*、Dijkstra、Fallback或者Floyd算 法。
[0025]優(yōu)選地，當(dāng)使用A*算法進(jìn)行路徑規(guī)劃時，計算公式如下：
[0026] G(n) =G(n_l )+Kc · C(n_l，η)+Κι · L(n)
[0027] F(n) =G(n)+H(n)
[0028] 其中F(n)為路徑節(jié)點(diǎn)n的綜合代價;G(n)為從初始節(jié)點(diǎn)移動至節(jié)點(diǎn)n的實(shí)際代價;H U)為從節(jié)點(diǎn)η移動至目標(biāo)點(diǎn)的啟發(fā)式估計代價;L(n)為路徑節(jié)點(diǎn)η所在位置柵格的定位能 力;C(n-l，n)為機(jī)器人由節(jié)點(diǎn)η-l移動至節(jié)點(diǎn)η的代價;K C和KL均為權(quán)重系數(shù)；當(dāng)權(quán)重系數(shù)KL 為零時，代價函數(shù)退化為A*算法。
[0029] 與現(xiàn)有技術(shù)相比，本發(fā)明具有如下的有益效果：
[0030] 本發(fā)明提供的基于地圖定位能力的路徑規(guī)劃方法，通過在路徑節(jié)點(diǎn)代價函數(shù)中考 慮地圖各處的定位能力，從而能夠有效優(yōu)化規(guī)劃路徑，使得機(jī)器人沿著定位能力相對較強(qiáng) 的區(qū)域進(jìn)行規(guī)劃，并避免因?yàn)槁窂焦?jié)點(diǎn)的定位能力過低而引起機(jī)器人定位失敗甚至丟失的 情況。本法的定位算法簡單，能夠離線計算環(huán)境中的不確定信息，定位效率高。應(yīng)用本發(fā)明 所提出的路徑規(guī)劃方法具有提尚移動單位在環(huán)境中的定位能力、避免移動單位定位失敗等 特點(diǎn)，適用于民用、工業(yè)、軍事等各個領(lǐng)域。
【附圖說明】
[0031] 通過閱讀參照以下附圖對非限制性實(shí)施例所作的詳細(xì)描述，本發(fā)明的其它特征、 目的和優(yōu)點(diǎn)將會變得更明顯：
[0032] 圖1為掃描方程示意圖；
[0033]圖2為傳感器觀測模型示意圖；
[0034]圖3為仿真環(huán)境概率柵格地圖；
[0035]圖4為仿真環(huán)境定位能力強(qiáng)度圖；
[0036]圖5為路徑規(guī)劃結(jié)果示例圖。
[0037]圖6為本發(fā)明的方法流程圖。
【具體實(shí)施方式】
[0038]下面結(jié)合具體實(shí)施例對本發(fā)明進(jìn)行詳細(xì)說明。以下實(shí)施例將有助于本領(lǐng)域的技術(shù) 人員進(jìn)一步理解本發(fā)明，但不以任何形式限制本發(fā)明。應(yīng)當(dāng)指出的是，對本領(lǐng)域的普通技術(shù) 人員來說，在不脫離本發(fā)明構(gòu)思的前提下，還可以做出若干變化和改進(jìn)。這些都屬于本發(fā)明 的保護(hù)范圍。
[0039]本發(fā)明的規(guī)劃方法包括（1)計算概率柵格地圖各點(diǎn)的定位能力；（2)使用路徑規(guī)劃 算法進(jìn)行搜索兩個部分，由以下步驟組成：
[0040] 步驟A1，獲取待進(jìn)行路徑規(guī)劃的環(huán)境的概率柵格地圖：
[0041] 待進(jìn)行路徑規(guī)劃的環(huán)境的建圖工作可通過SLAM等算法完成。建圖結(jié)束后可以得到 環(huán)境的概率柵格地圖。
[0042]步驟A2,計算概率柵格地圖各點(diǎn)的定位能力：
[0043] 對概率柵格地圖中各柵格進(jìn)行如下計算：
[0044] 步驟A2.1，設(shè)機(jī)器人位姿坐標(biāo)為= [V 3W ΘΡ, %]，以該坐標(biāo)為球心，根據(jù) 實(shí)際傳感器感知半徑r可得機(jī)器人最大掃描邊界：
[0045] (x-xp)2+(y-yp) 2+(z-zp)2 = r2 (1)
[0046] 以△ τ為掃描間隔，在式（1)的約束范圍內(nèi)，按照傳感器的掃描順序可以得到η條掃 描射線。
[0047]步驟Α2.2,對每條掃描射線i計算^方向上機(jī)器人到障礙物柵格的期望距離rlE:
[0048]圖示2為傳感器觀測模型示意圖，其中概率柵格地圖的灰度區(qū)域?yàn)橐阎系K物柵 格，μ為柵格被占據(jù)的概率值。其值越大，表示柵格被障礙物占有的可能性越大。第i條射線 到障礙物的期望長度rlE可表示為：
[0050] 其中^」為機(jī)器人沿著第i條掃描射線方向至第j個概率柵格的距離，為對應(yīng)的 柵格概率值。由于只需考慮該方向的最近已知障礙物，因此截止柵格序號m需滿足條件:μ ιη〈 Τ為概率閾值。當(dāng)柵格概率值小于Τ時，可近似認(rèn)為該柵格沒有被占據(jù)。
[0051] 步驟Α2.3，求取柵格的靜態(tài)定位能力矩陣：
[0053] 其中Ρ - ％?..，％，.約3:，％，.少ρ]為機(jī)器人位姿·坐標(biāo)；為傳感器數(shù)據(jù)尚斯白卩栄聲方 差;η為掃描射線的個數(shù);rlE為第i條射線到障礙物的期望長度。本矩陣根據(jù)實(shí)際使用的傳感 器的不同，可以轉(zhuǎn)化為三維全景（只考慮Xp，yP，zP信息）、二維全景（只考慮xP，y P信息)或二維 非全景（只考慮xp，yP，M言息)等形式。
[0054]步驟A2.4,計??:?(：Ρ：)的行列式，并將結(jié)果作為反映柵格定位能力的代價L，即 [0055] i(P) = det/(P) (4)
[0056]步驟A3,將各柵格的定位能力加入路徑節(jié)點(diǎn)代價函數(shù)：
[0057]賦予每個路徑節(jié)點(diǎn)的定位能力即是其所處柵格的定位能力。路徑節(jié)點(diǎn)代價函數(shù)的 形式可根據(jù)具體路徑規(guī)劃算法的不同而有不同的形式，以下以使用A*算法進(jìn)行路徑規(guī)劃的 情況為例
[0058] G(n) =G(n_l)+Kc · C(n_l，η)+Κι · L(n) (5)
[0059] F(n)=G(n)+H(n) (6)
[0060] 其中F(n)為路徑節(jié)點(diǎn)n的綜合代價;G(n)為從初始節(jié)點(diǎn)移動至節(jié)點(diǎn)n的實(shí)際代價;H U)為從節(jié)點(diǎn)η移動至目標(biāo)點(diǎn)的啟發(fā)式估計代價;L(n)為路徑節(jié)點(diǎn)η所在位置柵格的定位能 力;C(n-l，n)為機(jī)器人由節(jié)點(diǎn)n-Ι移動至節(jié)點(diǎn)η的代價;K C和KL均為權(quán)重系數(shù)。當(dāng)權(quán)重系數(shù)KL 為零時，代價函數(shù)退化為A*算法。
[0061 ]步驟A4，使用路徑規(guī)劃算法進(jìn)行路徑搜索，并計算出目標(biāo)移動路徑：
[0062] 本步驟可使用的路徑規(guī)劃算法包括A*、Di」1^1:瓜、？31]^3〇1^、？1〇7(1算法。
[0063]下面結(jié)合附圖和實(shí)施例對本發(fā)明作進(jìn)一步的詳細(xì)說明。此處以二維全景（只考慮 xP，yP信息)情況為例加以說明，在此情況公式⑶將退化為以下形式：
L〇〇65」本例使用R0S糸統(tǒng)仿真環(huán)境加以說明。該環(huán)境為一室內(nèi)辦公區(qū)域，包括兩條走廊及 若干間辦公室。其中一條走廊經(jīng)過辦公室門口，另外一條走廊兩側(cè)為墻面。步驟S1，在上述 仿真環(huán)境中使用激光測距儀作為傳感器進(jìn)行SLAM建圖，可以得到該環(huán)境的概率柵格地圖， 見圖示3。其中柵格顏色深淺代表其被障礙物占用的概率，顏色越深表示其為障礙物的概率 越高。柵格每一格尺寸為0.1X0.1米，激光傳感器的感知范圍為10米。
[0066] 對離線建立的概率柵格地圖上的各個柵格計算期望感知數(shù)據(jù)。機(jī)器人模型及掃描 示意圖見圖示1。設(shè)在位姿P=[xP，y P]下進(jìn)行掃描，在式（1)的約束范圍內(nèi)，按照傳感器的掃 描順序，可以得到η條掃描射線。對于每條激光射線i，在給定概率閾值T并確定截止柵格序 號m后，可以根據(jù)式(2)計算期望的激光掃描距離r lE。設(shè)定掃描范圍為360°，掃描距離r為10 米，概率閾值T為0.65。
[0067] 對概率柵格地圖上的各個柵格按照公式(7)計算其靜態(tài)定位能力矩陣。Δχ、Ay均 取柵格的分辨率，即0.1米。在計算該矩陣后，計算該矩陣的行列式，并作為該柵格最終的定 位能力。圖示4為仿真環(huán)境的定位能力計算結(jié)果。
[0068] 在柵格地圖上設(shè)定初始點(diǎn)和目標(biāo)點(diǎn)后使用路徑規(guī)劃算法進(jìn)行搜索，并計算出機(jī)器 人的目標(biāo)移動路徑。示例規(guī)劃結(jié)果如圖示5所示，其中綠色路徑為僅使用A*算法規(guī)劃的結(jié) 果，新規(guī)劃路徑為加入柵格定位能力后的規(guī)劃結(jié)果。
[0069] 以上對本發(fā)明的具體實(shí)施例進(jìn)行了描述。需要理解的是，本發(fā)明并不局限于上述 特定實(shí)施方式，本領(lǐng)域技術(shù)人員可以在權(quán)利要求的范圍內(nèi)做出各種變化或修改，這并不影 響本發(fā)明的實(shí)質(zhì)內(nèi)容。在不沖突的情況下，本申請的實(shí)施例和實(shí)施例中的特征可以任意相 互組合。
【主權(quán)項(xiàng)】
1. 一種基于地圖定位能力的路徑規(guī)劃方法，其特征在于，包括如下步驟： 步驟1:計算概率柵格地圖各點(diǎn)的定位能力； 步驟2:將概率柵格地圖各點(diǎn)的定位能力加入路徑節(jié)點(diǎn)代價函數(shù)，使用路徑規(guī)劃算法進(jìn) 行搜索，計算出目標(biāo)移動路徑。2. 根據(jù)權(quán)利要求1所述的基于地圖定位能力的路徑規(guī)劃方法，其特征在于，所述步驟1 包括： 步驟1.1:獲取待進(jìn)行路徑規(guī)劃的概率柵格地圖；具體地，包括采用SLAM算法對待進(jìn)行 路徑規(guī)劃的環(huán)境進(jìn)行建圖，得到環(huán)境的概率柵格地圖； 步驟1.2:針對概率柵格地圖上的多個點(diǎn)，分別計算每個點(diǎn)的定位能力。3. 根據(jù)權(quán)利要求1所述的基于地圖定位能力的路徑規(guī)劃方法，其特征在于，所述步驟2 中根據(jù)不同的路徑規(guī)劃方法得到相應(yīng)的不同的路徑節(jié)點(diǎn)代價函數(shù)，所述路徑節(jié)點(diǎn)代價函數(shù) 均能夠加入定位能力作為規(guī)劃時的指標(biāo);每個柵格的定位能力由每個柵格的L值反映。4. 根據(jù)權(quán)利要求1或3所述的基于地圖定位能力的路徑規(guī)劃方法，其特征在于，所述步 驟2中的路徑規(guī)劃算法包括:A*、Di jkstra、Fallback或者Floyd算法。5. 根據(jù)權(quán)利要求2所述的基于地圖定位能力的路徑規(guī)劃方法，其特征在于，所述步驟 1.2包括： 步驟1.2.1:將機(jī)器人設(shè)置在概率柵格地圖上，記錄機(jī)器人的位姿坐標(biāo)為,以坐標(biāo)P為球心，且已知機(jī)器人的傳感器感知半徑Sr，從而 能夠確定機(jī)器人的最大掃描邊界，以Ar為間隔進(jìn)行掃描，得到η條掃描射線;其中Xp，yp， Zp 為三維空間直角坐標(biāo)系下的坐標(biāo)，分別為xP，yP，zP對應(yīng)坐標(biāo)軸上的角度，A r為相 鄰的兩束傳感器射線的間隔； 步驟1.2.2:對每條掃描射線i計算T1方向上機(jī)器人到障礙物柵格的期望距離rlE; 步驟1.2.3:求取柵格的靜態(tài)定位能力矩陣，計算公式如下：式中：ΔΓιΕ表示期望距離的單位偏差，八&表示位姿P及沿X軸方向移動的單位距離，Δ 7[)表示位姿P及沿y軸方向移動的單位距離，△ 2[)表示位姿P及沿z軸方向移動的單位距離， 表示位姿P及沿貨角方向旋轉(zhuǎn)的單位角度，A θρ表示位姿P及沿Θ角方向旋轉(zhuǎn)的單位角 度，△%表示位姿P及沿Φ角方向旋轉(zhuǎn)的單位角度;^表示位姿P及沿X軸方向移動單位距離 處兩者期望距離rlE的偏差，^表示位姿P及沿y軸方向移動單位距離處兩者期望距離rlE的 偏差，^表示位姿P及沿z軸方向移動單位距離處兩者期望距離rlE的偏差，^表示位姿P 及沿中角方向旋轉(zhuǎn)單位角度后兩者期望距離rlE的偏差，^表示位姿P及沿Θ角方向旋轉(zhuǎn)單 位角度后兩者期望距離rlE的偏差，^表示位姿P及沿Φ角方向旋轉(zhuǎn)單位角度后兩者期望距 ^riE的偏差； 其中：矩陣的形式能夠根據(jù)實(shí)際使用的傳感器的不同轉(zhuǎn)化為三維全景、二維全景、 二維非全景中的任一種形式;三維全景是指機(jī)器人的位姿坐標(biāo)P只考慮含&，7[)，2[)項(xiàng)，其余 項(xiàng)為零;二維全景是指機(jī)器人的位姿坐標(biāo)P只考慮含&，yP項(xiàng)，其余項(xiàng)為零;二維非全景是指 機(jī)器人的位姿坐標(biāo)P只考慮含&，7 [)，9[)項(xiàng)，其余項(xiàng)為零的形式;11為總射線數(shù)，〇 2為傳感器噪 聲方差;所述單位距離和單位角度可根據(jù)實(shí)際需要確定； 步驟1.2.4:計算iGP)的行列式，并將結(jié)果作為反映柵格定位能力的代價L;對每個柵格 計算定位能力矩陣并計算行列式，行列式的計算結(jié)果即為該柵格的L值。6.根據(jù)權(quán)利要求4所述的基于地圖定位能力的路徑規(guī)劃方法，其特征在于，當(dāng)使用A*算 法進(jìn)行路徑規(guī)劃時，計算公式如下： G(n)=G(n_l)+Kc · C(n_l，n)+KL · L(n) F(n) =G(n)+H(n) 其中F (n)為路徑節(jié)點(diǎn)n的綜合代價;G (n)為從初始節(jié)點(diǎn)移動至節(jié)點(diǎn)n的實(shí)際代價;H (n) 為從節(jié)點(diǎn)η移動至目標(biāo)點(diǎn)的啟發(fā)式估計代價;L(n)為路徑節(jié)點(diǎn)η所在位置柵格的定位能力;C (η-1，η)為機(jī)器人由節(jié)點(diǎn)n-Ι移動至節(jié)點(diǎn)η的代價;Kc和Kl均為權(quán)重系數(shù)；當(dāng)權(quán)重系數(shù)Kl為零 時，代價函數(shù)退化為Α*算法。
【文檔編號】G01C21/34GK106017497SQ201610527990
【公開日】2016年10月12日
【申請日】2016年7月6日
【發(fā)明人】王景川, 陳衛(wèi)東, 劉成志
【申請人】上海交通大學(xué)