[發(fā)明專利]一種無人駕駛軌跡跟蹤中實時判別當前參考軌跡點的算法

有效

• 申請?zhí)枺?/td>: 201911095004.1
• 申請日: 2019-11-11
• 公開（公告）號: CN110703783B
• 公開（公告）日: 2021-07-27
• 發(fā)明（設計）人: 李夢揚;王亞飛;高瑞金;周志松;殷承良
• 申請（專利權）人: 上海交通大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 上海伯瑞杰知識產權代理有限公司 31227
• 代理人: 俞磊

本發(fā)明公開了一種無人駕駛軌跡跟蹤中實時判別當前參考軌跡點的算法，首先從參考軌跡的出發(fā)點開始，在每三個連續(xù)的參考軌跡點中的第二個參考軌跡點處，取前兩個參考軌跡點所連成的直線與后兩個參考軌跡點所連成直線的角平分線，并得到角平分線在全局坐標系下的直線表達式；然后通過實時位置的結果值和第三個參考軌跡點的結果值乘積來判斷車輛是否通過第二個參考軌跡點，實時地得到車輛當前經過的參考軌跡點，直到到達最后一個參考軌跡點為止。本發(fā)明能實時地判別車輛當前經過的參考軌跡點，并對后一個參考軌跡點進行跟蹤的功能，從而使得車輛可以得到當前的預瞄點及預瞄點的語意信息，進行局部軌跡規(guī)劃、軌跡跟蹤和避開障礙物的控制。

技術領域

本發(fā)明涉及無人駕駛領域，具體地說，特別涉及到一種無人駕駛軌跡跟蹤中實時判別當前參考軌跡點的算法。

背景技術

軌跡跟蹤，是無人駕駛的最基本環(huán)節(jié)。軌跡跟蹤，指的是無人車在得到一條參考軌跡(由一系列參考軌跡點組成)之后，對車輛進行實時有效地控制，使無人車安全地跟隨參考軌跡指定的路線進行自主行駛。

軌跡跟蹤需要車輛進行實時地精準地定位，判斷當前處于參考軌跡的哪個參考軌跡點，并選擇合適的預瞄模型，對車輛進行精準的縱橫向控制，從而達到軌跡跟蹤的效果。同時，如果車輛使用的是具有語義信息的語義拓撲地圖時，也需要通過判斷當前經過了哪個參考軌跡點來獲取當前軌跡點的語意，從而指導車輛執(zhí)行相關動作。因此，如何實時地魯棒地判斷當前車輛經過了參考軌跡的哪個參考軌跡點是非常重要的。

目前的判斷方法都存在各自的缺陷。比如以距離最近為原則找尋當前車輛位置距離最近的參考軌跡點，這種方法無法應對參考軌跡中出現(xiàn)軌跡重疊或交叉的情況，極有可能將當前經過軌跡點判斷為已經過或未經過的軌跡點，從而導致無人車發(fā)生跟隨錯誤軌跡行駛的情況發(fā)生。同時，若以車輛實時位置到參考軌跡點的距離大小方式進行判斷，則在剛剛經過當前軌跡點后，存在車輛位置到剛經過的參考軌跡點的距離小于到后一個參考軌跡點的距離時，則會判斷當前瞄準的參考軌跡點依然為上一個剛經過的點。因此，軌跡跟蹤中亟需一種可以在參考軌跡出現(xiàn)重疊或交叉的情況下，仍然能魯棒地依次判斷當前經過的跟蹤參考軌跡點的算法。

發(fā)明內容

本發(fā)明的目的在于針對現(xiàn)有技術中的不足，提供一種無人駕駛軌跡跟蹤中實時判別當前參考軌跡點的算法，在軌跡跟蹤時可以實時地判別當前經過了哪些參考軌跡點，并立即對后一個參考軌跡點進行跟蹤的功能，從而使得車輛可以得到當前的預瞄點及預瞄點的語意信息，從而進行局部軌跡規(guī)劃，進行軌跡跟蹤和避開障礙物的控制。

本發(fā)明所解決的技術問題可以采用以下技術方案來實現(xiàn)：

一種無人駕駛軌跡跟蹤中實時判別當前參考軌跡點的算法，包括如下步驟：

1)從參考軌跡的出發(fā)點開始，在每三個連續(xù)的參考軌跡點中的第二個參考軌跡點處，取第一個參考軌跡點和第二個參考軌跡點所連成的直線與第二個參考軌跡點和第三個參考軌跡點所連成直線的角平分線，并得到角平分線在全局坐標系下的直線表達式；

2)將車輛在全局坐標系下的實時x-y坐標代入角平分線的直線表達式得到實時位置的結果值，并將第三個參考軌跡點的x-y坐標代入角平分線的直線表達式得到第三個參考軌跡點的結果值；

3)若實時位置的結果值和第三個參考軌跡點的結果值異號，則說明車輛的當前坐標與第三個參考軌跡點位于角平分線的兩側，即認定為車輛尚未通過第二個參考軌跡點，返回步驟2)；

若實時位置的結果值和第三個參考軌跡點的結果值同號，則說明車輛的當前坐標與第三個參考軌跡點位于角平分線的同側，即認定為車輛已經通過第二個參考軌跡點，進入步驟4)；

4)對后三個連續(xù)的參考軌跡點的進行步驟1)至步驟3)的判斷，實時地得到車輛當前經過的參考軌跡點，直到到達最后一個參考軌跡點為止。