[發明專利]基于車輛運動模型的無人車輛野外路徑規劃方法

在審

• 申請號: 201710865274.0
• 申請日: 2017-09-22
• 公開（公告）號: CN107479558A
• 公開（公告）日: 2017-12-15
• 發明（設計）人: 朱會杰;邵立福;曹有輝;劉飛;傅幸民;宣惠平;王煥坤;卞斌華;陳高杰
• 申請（專利權）人: 中國人民解放軍63983部隊
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 總裝工程兵科研一所專利服務中心32002
• 代理人: 張婉

1.基于車輛運動模型的無人車輛野外路徑規劃方法，其特征在于，包括以下步驟：

步驟一，建立環境三維地圖

根據傳感器獲取的信息進行初步處理，刪除噪點，對臨近障礙進行合并、膨脹，最終確定障礙Obs_i(i＝1,2,3…)，每個Obs_i包含了該區域中的離散點坐標，確定起點S_pos，在目標航線上，按照目標航線的方向以地圖最遠處為起點向地圖內延伸，直到碰觸到障礙物為止，該線段定義為目標航線段；

步驟二，定義車輛行駛模型

車輛在行駛中有以下假設：每個控制周期與行駛速度為固定，因此在該控制周期車輛行駛距離為C，車輛行駛時，在每個控制周期內，車輛轉向角可以變化的最大值為S_angle，為了簡化計算，將車輪轉向角變化值離散為N個角度Δ_angle_i(i＝1,2,3…N)，在最小轉彎半徑下，車輛的轉彎角度最大，為Angle_range，在一個控制周期內，車輪的轉動變化值Δ_angle_i要小于S_angle，車輛當前轉向角為Angle，那么車輛的實際轉向角-Angle_range≤Angle+Δ_angle_i≤Angle_range，定義開集Open_set，每一個位置對應一個向量，該向量包括車輛中心位置，航向向量，上一個控制周期的車輛中心位置，車輛當前狀態下的車輪轉角，從起點到當前位置所行駛的路程Cost，啟發性值H，總代價Value＝Cost+H，啟發性信息一般為車輛當前位置與目標點的距離，為了保證航線一致，這里可以選用車頭與目標航線段的距離以及車尾與目標航線段距離之和，對各個參數進行初始化，其中將起點及其對應參數加入開集；

步驟三，車輛按控制周期行駛

按下面內容進行迭代循環：

(1)尋找Open_set中總代價Value最小的向量，所對應的車輛中心位置為車輛當前位置，第一次迭代時，當前位置為起點位置，閉集Closed_set為空；

(2)遍歷i＝1,2,3…N：

①如果車輛的轉向角變化量滿足-S_angle≤Δ_angle_i≤S_angle，并且車輛的實際轉向角滿足-Angle_range≤Angle+Δ_angle_i≤Angle_range，根據當前位置以及車輛方向向量，按照一個控制周期車輛行駛的距離，計算車輛可以行駛到的新位置、新的方向向量；

②判斷是否碰觸障礙，是否能夠到達目標航線段，如果不碰觸障礙且有可能到達目標航線段，則計算新位置的車輛坐標、航線、轉向角、Cost、H、Value等值，并加入到Open_set中；

③i＝i+1，進入下一循環，當計算完所有的轉向角對應的參數后，將當前位置所對應的開集向量加入到閉集Closed_set中；

(3)停止條件：

①檢驗車輛是否已經到達目標航線段，并且航線偏差在規定誤差內，如果滿足，則停止循環；

②判斷Open_set是否為空，如果為空，則說明找不到可以行駛的路線，程序結束，顯示無可行駛路線；

如果不滿足停止條件，則繼續迭代循環；

步驟四，輸出規劃方案

如果找到了規劃方案，將開集和閉集合并，首先找到終點位于的向量，保存該向量中的車輛當前位置、轉向、車輛方向向量到路徑矩陣中，并根據上一周期所對應的坐標尋找該坐標所對應的向量，保存相關信息到路徑矩陣中，按照如下進行迭代：

按照當前坐標與上一周期坐標依次循環直到起點，所對應的路線即為搜索到的路線。