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

有效

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

1.一種無人駕駛軌跡跟蹤中實(shí)時判別當(dāng)前參考軌跡點(diǎn)的算法，其特征在于，包括如下步驟：

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

2)將車輛在全局坐標(biāo)系下的實(shí)時x-y坐標(biāo)代入角平分線的直線表達(dá)式得到實(shí)時位置的結(jié)果值，并將第三個參考軌跡點(diǎn)的x-y坐標(biāo)代入角平分線的直線表達(dá)式得到第三個參考軌跡點(diǎn)的結(jié)果值；

3)若實(shí)時位置的結(jié)果值和第三個參考軌跡點(diǎn)的結(jié)果值異號，則說明車輛的當(dāng)前坐標(biāo)與第三個參考軌跡點(diǎn)位于角平分線的兩側(cè)，即認(rèn)定為車輛尚未通過第二個參考軌跡點(diǎn)，返回步驟2)；

若實(shí)時位置的結(jié)果值和第三個參考軌跡點(diǎn)的結(jié)果值同號，則說明車輛的當(dāng)前坐標(biāo)與第三個參考軌跡點(diǎn)位于角平分線的同側(cè)，即認(rèn)定為車輛已經(jīng)通過第二個參考軌跡點(diǎn)，進(jìn)入步驟4)；

4)對后三個連續(xù)的參考軌跡點(diǎn)的進(jìn)行步驟1)至步驟3)的判斷，實(shí)時地得到車輛當(dāng)前經(jīng)過的參考軌跡點(diǎn)，直到到達(dá)最后一個參考軌跡點(diǎn)為止。

2.根據(jù)權(quán)利要求1所述的無人駕駛軌跡跟蹤中實(shí)時判別當(dāng)前參考軌跡點(diǎn)的算法，其特征在于，所述全局坐標(biāo)系的構(gòu)建方法為：在參考軌跡附近任取一個原點(diǎn)并建立x-y坐標(biāo)系，將車輛實(shí)時位置點(diǎn)及所有參考軌跡點(diǎn)的經(jīng)緯度坐標(biāo)轉(zhuǎn)換到該x-y坐標(biāo)系下。

3.根據(jù)權(quán)利要求1所述的無人駕駛軌跡跟蹤中實(shí)時判別當(dāng)前參考軌跡點(diǎn)的算法，其特征在于，當(dāng)前判斷目標(biāo)為參考軌跡的出發(fā)點(diǎn)時，取出發(fā)點(diǎn)和出發(fā)點(diǎn)后一個參考軌跡點(diǎn)所連成直線在出發(fā)點(diǎn)處的垂線，并得到垂線在全局坐標(biāo)系下的直線表達(dá)式；

然后將車輛在全局坐標(biāo)系下的實(shí)時x-y坐標(biāo)代入垂線的直線表達(dá)式得到實(shí)時位置的結(jié)果值，并將出發(fā)點(diǎn)后一個參考軌跡點(diǎn)的x-y坐標(biāo)代入垂線的直線表達(dá)式得到出發(fā)點(diǎn)后一個參考軌跡點(diǎn)的結(jié)果值；

若實(shí)時位置的結(jié)果值和出發(fā)點(diǎn)后一個參考軌跡點(diǎn)的結(jié)果值異號，即判定為車輛尚未通過出發(fā)點(diǎn)；若實(shí)時位置的結(jié)果值和出發(fā)點(diǎn)后一個參考軌跡點(diǎn)的結(jié)果值同號，則判定為車輛已經(jīng)通過出發(fā)點(diǎn)。

4.根據(jù)權(quán)利要求1所述的無人駕駛軌跡跟蹤中實(shí)時判別當(dāng)前參考軌跡點(diǎn)的算法，其特征在于，當(dāng)前判斷目標(biāo)為參考軌跡的終點(diǎn)時，取終點(diǎn)和終點(diǎn)前一個參考軌跡點(diǎn)所連成直線在終點(diǎn)處的垂線，并得到垂線在全局坐標(biāo)系下的直線表達(dá)式；

然后將車輛在全局坐標(biāo)系下的實(shí)時x-y坐標(biāo)代入垂線的直線表達(dá)式得到實(shí)時位置的結(jié)果值，并將終點(diǎn)前一個參考軌跡點(diǎn)的x-y坐標(biāo)代入垂線的直線表達(dá)式得到終點(diǎn)前一個參考軌跡點(diǎn)的結(jié)果值；

若實(shí)時位置的結(jié)果值和終點(diǎn)前一個參考軌跡點(diǎn)的結(jié)果值同號，即判定為車輛尚未抵達(dá)終點(diǎn)；若實(shí)時位置的結(jié)果值和終點(diǎn)前一個參考軌跡點(diǎn)的結(jié)果值異號，則判定為車輛已經(jīng)抵達(dá)終點(diǎn)。

5.根據(jù)權(quán)利要求1所述的無人駕駛軌跡跟蹤中實(shí)時判別當(dāng)前參考軌跡點(diǎn)的算法，其特征在于，從參考軌跡的出發(fā)點(diǎn)開始，按照參考軌跡的行進(jìn)路線，對每一個當(dāng)前參考軌跡點(diǎn)只做車輛是否經(jīng)過當(dāng)前參考軌跡點(diǎn)的判斷，并只在判斷得到車輛經(jīng)過當(dāng)前參考軌跡點(diǎn)時，才對后一個參考軌跡點(diǎn)進(jìn)行判斷。

6.根據(jù)權(quán)利要求1所述的無人駕駛軌跡跟蹤中實(shí)時判別當(dāng)前參考軌跡點(diǎn)的算法，其特征在于，在車輛軌跡跟蹤時，按照參考軌跡經(jīng)過的順序?qū)⑺袇⒖架壽E點(diǎn)進(jìn)行排序，每經(jīng)過一個參考軌跡點(diǎn)就將該參考軌跡點(diǎn)的序號記錄下來；在軌跡跟蹤調(diào)試時，若出現(xiàn)問題需要重新運(yùn)行程序，程序運(yùn)行伊始便會從記錄的最后一個參考軌跡點(diǎn)的序號開始進(jìn)行判斷，實(shí)現(xiàn)從中斷點(diǎn)繼續(xù)測試。

7.根據(jù)權(quán)利要求6所述的無人駕駛軌跡跟蹤中實(shí)時判別當(dāng)前參考軌跡點(diǎn)的算法，其特征在于，參考軌跡的起始參考點(diǎn)序號可通過手動輸入，重新運(yùn)行程序時便會從手動輸入的起始參考點(diǎn)開始進(jìn)行判斷。