[發(fā)明專利]一種實現(xiàn)輪式移動機器人實時避障的路徑規(guī)劃方法

有效

• 申請?zhí)枺?/td>: 202011348566.5
• 申請日: 2020-11-26
• 公開（公告）號: CN112378408B
• 公開（公告）日: 2023-07-25
• 發(fā)明（設計）人: 江濤;付文豪;蘇曉杰;黃江帥;文郁;李海
• 申請（專利權）人: 重慶大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G05D1/02
• 代理公司: 廣州市華學知識產(chǎn)權代理有限公司 44245
• 代理人: 張晨

1.一種實現(xiàn)輪式移動機器人實時避障的路徑規(guī)劃方法，其特征在于，包括如下步驟：

S1、利用車載傳感器獲取環(huán)境信息，實現(xiàn)移動機器人的自身定位，并構建柵格地圖，建立TF坐標轉換；

S2、利用改進的A*算法完成移動機器人的全局路徑規(guī)劃；

S3、利用改進A*算法采用隊列數(shù)據(jù)格式的特性，按照權值將點由小到大依次放入隊列；

S4、針對車載傳感器獲取未知障礙物信息，根據(jù)初始參數(shù)、最大速度Vm、線加速度v和角加速度ω，利用DWA算法結合全局路徑規(guī)劃信息做出路徑評價，選擇局部最優(yōu)的路徑；

其中，S2包括以下步驟：

S201、將輸入的柵格地圖，經(jīng)過map_sever節(jié)點處理，將處理過后的map的數(shù)據(jù)存入二維數(shù)組中，數(shù)組中的0和1分別代表可通行和不可通行；

S202、設定數(shù)組中的每一個數(shù)代表地圖上的一個柵格，每一個柵格有G(n)、H(n)兩個代價，其中，G(n)代表的是從初始位置A到節(jié)點n的移動代價，H(n)指定n到目標節(jié)點的估計代價；

S3包括如下步驟：

S301、所有節(jié)點移動評估函數(shù)為：F(n)＝(a)G(n)+(b)H(n)，其中，a，b為權值，當a＝0，b＝1，此時，是貪心算法，當a＝1,b＝0,是迪杰斯特拉，當a＝1,b＝1,是A*算法；

S302、H(n)為啟發(fā)函數(shù)并采用傳統(tǒng)的歐式距離；

S303、在實際工程中，改進A*算法采用Tie?Breaker方法，路徑中的點都有相同的f值，通過改變h值使得f有細微的差別；

其中，h＝h(1.0+p)，P每一步的最代價/柵格總數(shù)的期望最大路徑代價；

S304、通過加入可視函數(shù)，當更新節(jié)點時，實現(xiàn)擴展點與該點的父節(jié)點之間是否可視進行判定；

S305、改進A*算法打開一個節(jié)點s，并更新周圍的節(jié)點s'時，會檢查s'與s父節(jié)點的可見性，若可見，則把s'的父節(jié)點設置成s的父節(jié)點，實現(xiàn)只保留必要的拐點，當搜索到目標點后，調用回溯函數(shù)，找到一條無冗余點的直線段集合；

S4包括如下步驟：

S401、機器人運動模型的建立：機器人相鄰時刻內，運動時間很短，運動距離也短，將兩相鄰點之間的運動軌跡看成直線，即沿機器人坐標系x軸移動了υ_t*Δt，將該段距離分別投影在世界坐標系x軸和y軸上得到t+1時刻相對于t時刻機器人在世界坐標系中坐標移動的位移Δx和Δy：

Δx＝υΔtcos(θ_t)

Δy＝υΔtsin(θ_t)

一定時間內位移增量，如下：

x＝x+υΔtcos(θ_t)

y＝y(tǒng)+υΔt?sin(θ_t)

θ_t＝θ_t+ωΔt

若機器人是全向運動的，即機器人坐標系的y軸有速度表達，將機器人在機器人坐標y軸移動的距離投影到世界坐標系即可：

最終運動軌跡模型如下：

x＝x+υΔtcos(θ_t)-υ_yΔtsin(θ_t)

y＝y(tǒng)+υΔtsin(θ_t)+υ_yΔtcos(θ_t)

θ_t＝θ_t+ωΔt；

S402、速度采樣模型的建立：移動機器人的移動受自身最大速度和最小速度的限制：

υ_m＝{υ∈[υ_min,υ_max],ω∈[ω_min,ω_max]}

其中，υ_m(υ,ω)為組合速度，υ表示線速度，ω表示角速度；

移動機器人軌跡前向模擬的周期內，存在一個動態(tài)窗口，在該窗口內的速度是機器人能夠實際達到的速度：

其中，υ_d為可達速度，υ_curr為當前線速度，ω_curr為當前角速度；

整個機器人軌跡假設是由n個時段的n個小圓弧組成，從安全性考慮，根據(jù)簡單的運動學推算，在某一時刻，不發(fā)生碰撞的允許速度υ_a為：

其中，dist(υ,ω)為速度為(υ,ω)時對應軌跡上離最近障礙物的距；

最終速度采樣模型為：υ_r＝υ_m∩υ_a∩υ_d；

S403、預測軌跡評價模型的建立：采用評價函數(shù)的方式為每條軌跡進行評價，從中選擇滿足約束條件的最優(yōu)速度，評價函數(shù)綜合考慮了機器人運動速度、航向和避障，其定義如下：

G(υ,ω)＝α·to_goal_cost(υ,ω)+β·obstacle_cost(υ,ω)+γ·speed_cost(υ,ω)

其中，to_goal_cost表示機器人航向和到目標點的距離的得分；

obstacle表示預測軌跡中的點距離障礙物的最小距離；

speed_cost表示圓弧軌跡中t+1時刻的線速度，該項將選擇動態(tài)窗口中線速度大的值；

α、β、γ為3個權值參數(shù)，為避免其中一項值占比過高，在總的評價前，將評價函數(shù)的3項參數(shù)歸一化處理為[0,1]之間的參數(shù)；

goal_cost(υ,ω)是用于評價機器人在當前設定的采樣速度下，達到模擬軌跡末端時的朝向和目標之間的角度差距以及達到模擬軌跡末端后到目標的距離，角度差與距離越小，得分越低；

obstacle_cost(υ,ω)是用于規(guī)避障礙物的評價項，計算預測軌跡上每個點到附近每個障礙物的距離，比較其中最小距離與機器人與障礙物之間的容忍距離，若小于容忍距離則舍棄該預測軌跡，通過對機器人到障礙物的最小距離取反，距離越大，則得分越低；

speed_cost(υ,ω)是用于評估機器人移動速度的評價項，在速度窗口內，預測軌跡所對應的線速度與期望速度的差值越小，則得分越低；

通過歸一化處理，給每一個評價項分配不同的權重，得到每一條預測軌跡的最終得分，選擇其中得分最低的預測軌跡，并將其對應的速度發(fā)送給移動機器人地盤。