[發明專利]一種基于結構化數據的路徑規劃方法

有效

• 申請號: 202010607328.5
• 申請日: 2020-06-29
• 公開（公告）號: CN111895999B
• 公開（公告）日: 2022-03-15
• 發明（設計）人: 黃小春;李瑞峰;霍光磊;常騏川
• 申請（專利權）人: 福建（泉州）哈工大工程技術研究院
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/34
• 代理公司: 泉州市立航專利代理事務所(普通合伙) 35236
• 代理人: 姚婉莉

1.一種基于結構化數據的路徑規劃方法，其特征在于，方法過程如下

(1)、全局路徑規劃首先根據結構化數據進行路網構建；結構化路網數據主要包括車道線和路沿，車道線結構化路網數據的字段包括節點ID，基于點云地圖坐標系的X、Y、Z，前置節點的PID，后置節點的PID字段；路沿結構化路網數據的字段包括節點ID，基于點云地圖坐標系的X、Y、Z，前置節點的PID，后置節點的PID，寬度，高度字段；建立運動目標點的隊列,根據執行規則執行隊列中的目標點；

全局路徑規劃的工作過程：讀取結構化路網數據進行解析，并根據其節點類型，前置節點的PID和后置節點的PID關系，構建出在路網數據結構中的車道線和路沿；

修正重疊或距離較小的節點，平滑構建車道線和路沿；

接收機器人當前位姿，接收目標點，從目標點隊列緩存中彈出隊首元素為當前執行目標點G，

遍歷路網數據結構，得到與當前機器人位姿最接近的起始節點，得到與目標點G最近接近的終點節點，根據所述起始節點從該點出發，結合前置節點的PID和后置節點的PID，遞推至所述終點節點，得到當前目標點的全局路徑為初始全局路徑，均勻處理初始全局路徑的節點間距，重新生成全局路徑為二次全局路徑，平滑處理二次全局路徑得到最終全局路徑，發布最終全局路徑；

(2)、候選軌跡生成，接收最終全局路徑輸入，機器人當前位姿輸入，遍歷最終全局路徑，查找最接近機器人當前位姿的節點，并截取以機器人當前位姿前方為正方向，后方為負方向的最終全局路徑上的一段距離范圍區間內的路徑為全局路徑片段，以用于局部候選軌跡的規劃；

構造分布于機器人當前左右兩側的候選軌跡間距數組記為Density，當前位姿左側為正，右側為負，用于計算生成多條候選軌跡，所述候選軌跡包括過渡部分和平行部分，確定過渡部分節點數記為Count，截取的全局路徑片段記為PointPathSection，PointPathSection相對于當前位姿的側向偏移量記為HOffset；

計算各候選軌跡過渡部分側向增量：Diff[i]＝(Density[i]+HOffset)/Count，其中i取值范圍為[0，Density.size()-1]，

候選軌跡過渡部分節點計算：

PointTrajectory[i][j].y＝PointPathSection[j].y–(HOffset-Diff[i]*j)*sin(PointPathSection[j].yaw+Pi/2)；

PointTrajectory[i][j].x＝PointPathSection[j].x–(HOffset-Diff[i]*j)*cos(PointPathSection[j].yaw+Pi/2)；

其中j取值范圍[0,Count-1]，PointTrajectory[i][j]為第i條候選軌跡的第j個點，PointPathSection[j]為被規劃候選軌跡的全局路徑片段的第j個點，x、y、yaw分別表示節點的坐標和偏航；

候選軌跡平行部分節點計算：

PointTrajectory[i][k].y＝PointPathSection[k].y+Density[i]*sin(PointPathSection[k].yaw+Pi/2)；

PointTrajectory[i][k].x＝PointPathSection[k].x+Density[i]*cos(PointPathSection[k].yaw+Pi/2)；

其中k的取值范圍[Count,PathSection.size()]，PointTrajectory[i][k]為第i條候選軌跡的第k個點，PointPathSection[k]為被規劃候選軌跡的全局路徑片段的第k個點；

(3)、候選軌跡評價，接收全局路徑輸入，機器人當前位姿輸入，上述(2)生成的多條候選軌跡輸入，障礙物輪廓點集輸入；

遍歷各個障礙物輪廓點，計算機器人當前位姿與障礙物各個輪廓點沿最終全局路徑方向的距離；

遍歷各障礙物輪廓點與各候選軌跡，計算各障礙物輪廓點與各候選軌跡的側向距離；

根據機器人膨脹層的寬度進行候選軌跡的可通行性判定，比較候選軌跡與障礙物的間距是否大于機器人膨脹層寬度的一半，小于則進入阻塞狀態；

(4)、候選軌跡防抖濾波：初始化上次各候選軌跡阻塞狀態，障礙物與各候選軌跡最小距離；

遍歷各候選軌跡，跳過處于阻塞狀態的候選軌跡；

遍歷各候選軌跡，對處于非阻塞狀態的候選軌跡，其上次阻塞狀態為非阻塞，則跳過；

遍歷各候選軌跡，對處于非阻塞狀態的候選軌跡，其上次阻塞狀態為阻塞的，則檢查其與障礙物的最小距離，如果大于安全間距閾值，則跳過，否則修改為阻塞狀態。