自動(dòng)駕駛車(chē)輛循跡路徑生成算法研究

李晉津 孫二鑫 陳宗娟

摘 要：在無(wú)人駕駛領(lǐng)域中 已經(jīng)實(shí)現(xiàn)了車(chē)輛按照已有路徑進(jìn)行循跡無(wú)人駕駛 通常已有路徑是由車(chē)輛實(shí)際采集獲得。文章采用Matlab編寫(xiě)了循跡路徑的生成算法 解決了循跡路線必須由車(chē)輛采集獲取的問(wèn)題 使車(chē)輛控制更加高效。再循跡路徑生成算法中結(jié)合了車(chē)輛的左右轉(zhuǎn)最小拐彎半徑約束和允許倒車(chē)的情況 得到了無(wú)人駕駛車(chē)輛可行駛的路徑。最后通過(guò)算法生成的路徑進(jìn)行了實(shí)車(chē)實(shí)驗(yàn) 車(chē)輛能夠按照算法生成的路徑進(jìn)行自動(dòng)駕駛 最后對(duì)算法生成的路徑和車(chē)輛實(shí)際路徑進(jìn)行了對(duì)比 驗(yàn)證了算法的可行性。

關(guān)鍵詞：路徑生成;倒車(chē);車(chē)輛轉(zhuǎn)彎半徑;實(shí)車(chē)實(shí)驗(yàn)

中圖分類(lèi)號(hào)：U495? 文獻(xiàn)標(biāo)識(shí)碼：A? 文章編號(hào)：1671-7988（2020）18-16-06

Abstract： In the field of unmanned driving， it has been realized that vehicles follow existing paths to track unmanned driving， Usually the existing path is obtained by actual vehicle collection. this paper uses Matlab to write the algorithm of generating the tracing path， which solves the problem that the tracing route must be acquired by vehicle collection and makes the vehicle control more efficient. The re-tracking path generation algorithm combines the vehicle's left-right tur -ning minimum turning radius constraint and the allowable reversing situation to obtain a path that can be driven by an unmanned vehicle. Finally， a real-vehicle experiment was performed on the path generated by the algorithm. The vehicle can drive automatically according to the path generated by the algorithm. Finally， the path generated by the algorithm is compared with the actual path of the vehicle to verify the feasibility of the algorithm.

Keywords： Path generation; Reversing; Vehicle turning radius; Real car experiment

CLC NO.： U495? Document Code： A? Article ID： 1671-7988（2020）18-16-06

前言

在無(wú)人駕駛領(lǐng)域中 軌跡規(guī)劃有多種算法 主要分為：1）基于搜索的規(guī)劃算法 如：Hybrid A 算法、DLite算法;2） 基于采樣的規(guī)劃算法 如概率路線圖（PRM）和快速搜索隨機(jī)樹(shù)（RRT）算法;3）人工勢(shì)場(chǎng)法;4）參數(shù)化曲線構(gòu)造法。本文中循跡路徑算法即采樣的參數(shù)化曲線構(gòu)造方法實(shí)現(xiàn)的。

參數(shù)化曲線構(gòu)造法又分為多項(xiàng)式曲線 貝塞爾曲線樣條曲線和Dubins（直線和圓弧）。Andrei M Shke等人對(duì)dubins曲線進(jìn)行了研究 對(duì)起始點(diǎn)的姿態(tài)進(jìn)行了坐標(biāo)變換 完成了各類(lèi)情況的路線計(jì)算[1];劉流等研究了帶速度約束的dubins路徑 研究表明 拐彎半徑是速度的函數(shù) 當(dāng)速度較大時(shí) 需要匹配更大的轉(zhuǎn)彎半徑 并進(jìn)行了無(wú)人機(jī)飛行驗(yàn)證[2];吳克風(fēng)等人針對(duì)起終點(diǎn)距離較近情況 進(jìn)行了不同情況下的分類(lèi) 推導(dǎo)了并驗(yàn)證了各類(lèi)情況下的最短路徑情況[3];楊秀霞等人 考慮了角度約束 即起始轉(zhuǎn)彎半徑不同情況下的路徑 并對(duì)不同角度下的路徑進(jìn)行了分析對(duì)比[4]。黃得剛等人，研究了三維Dubins路徑 即在位置中加入不同的高度z 其核心主要是對(duì)起終點(diǎn)不同高度進(jìn)行插值 并對(duì)三維Dubins路徑進(jìn)行了仿真實(shí)驗(yàn)[5]。

目前基于倒車(chē)和起始點(diǎn)轉(zhuǎn)彎半徑（曲率）不同的Dubins路徑研究較少 而加入倒車(chē)模式會(huì)得到更短的路徑并在某些特定的領(lǐng)域如避障 自動(dòng)泊車(chē)等方面大有所為。本文研究了加入倒車(chē)和選擇不同的起始轉(zhuǎn)彎半徑情況下的Dubins路徑 算法另辟蹊徑 解決了車(chē)輛循跡路徑生成問(wèn)題。

1 問(wèn)題描述

在一個(gè)二維平面中 尋找兩個(gè)帶方向的點(diǎn)間的最短光滑路徑。起點(diǎn)為A（xa，ya） A點(diǎn)頭車(chē)朝向?yàn)棣羇 車(chē)頭朝向與地理方位角一致 即正北為0度 范圍為[0-360];終點(diǎn)為B（xb，yb） 終點(diǎn)車(chē)頭朝向?yàn)棣羈。尋找從起點(diǎn)A到終點(diǎn)B的最短光滑路徑。車(chē)輛的轉(zhuǎn)彎半徑設(shè)置為R 受車(chē)輛最小轉(zhuǎn)彎半徑約束 R選取應(yīng)大于車(chē)的最小轉(zhuǎn)彎半徑。

2 約束條件

2.1 車(chē)輛的轉(zhuǎn)彎半徑約束

2.1.1 轉(zhuǎn)彎半徑

由于車(chē)輛的結(jié)構(gòu)特征存在最小拐彎半徑 有些車(chē)輛的左轉(zhuǎn)最小拐彎半徑和右轉(zhuǎn)最小轉(zhuǎn)彎半徑還可能不相同 如圖2所示 已知A點(diǎn)的位置及方位角αa 由一個(gè)特定的位置出發(fā) 車(chē)輛在下一步只能到達(dá)它可能到達(dá)的位置 受制于汽車(chē)最大轉(zhuǎn)彎半徑的限制。下圖中紅色箭頭表示車(chē)輛在最大拐彎半徑情況下的極限可行駛方向 藍(lán)色表示直行。車(chē)輛前方區(qū)域表示正向行駛的區(qū)域 范圍為極限右轉(zhuǎn)與極限左轉(zhuǎn)之間;車(chē)輛后方為倒車(chē)情況下的可行駛區(qū)域;范圍也為為極限右轉(zhuǎn)與極限左轉(zhuǎn)之間。那么只要給定車(chē)輛的拐彎半徑大于車(chē)輛的極限拐彎半徑 車(chē)輛是可以行駛的。

2.1.2 實(shí)車(chē)轉(zhuǎn)彎半徑參數(shù)

本次實(shí)驗(yàn)采用的是傳祺GE3改裝的自動(dòng)駕駛車(chē)輛 最大拐彎半徑根據(jù)實(shí)車(chē)采集數(shù)據(jù)獲取如圖3所示 車(chē)輛的左轉(zhuǎn)和右轉(zhuǎn)情況具有不同的最大轉(zhuǎn)彎半徑 左轉(zhuǎn)最大轉(zhuǎn)彎半徑為3.75m 右轉(zhuǎn)最大轉(zhuǎn)彎半徑為4.2m。

2.2 倒車(chē)約束約束

如下圖所示 在某些情況下 正向行駛會(huì)走更長(zhǎng)的路程 例如終點(diǎn)在起點(diǎn)的正后方 且車(chē)頭朝向和初始車(chē)頭朝向一致 這樣車(chē)輛只需掛倒擋即可實(shí)現(xiàn)到達(dá)終點(diǎn)。又如當(dāng)車(chē)輛從A點(diǎn)出發(fā) 通過(guò)固定的拐彎半徑可以實(shí)現(xiàn)到達(dá)終點(diǎn)B且滿足到達(dá)B的車(chē)頭朝向 同時(shí)車(chē)輛也可以掛倒擋實(shí)現(xiàn)到達(dá)終點(diǎn)B且滿足B點(diǎn)車(chē)頭朝向。對(duì)比發(fā)現(xiàn) 掛倒擋走的路徑要遠(yuǎn)小于前進(jìn)走的道路。因此對(duì)于任意車(chē)輛可走的圓弧 均有兩種到達(dá)方式 一種時(shí)正向行駛 一種是后退行駛 走的弧段大于半圓稱(chēng)之為劣弧 走的弧段小于半圓稱(chēng)之為優(yōu)弧。因此車(chē)輛在加入倒車(chē)模式后 在某些情況下能夠得到更短的路徑。

文中以f（forward）表示向前行駛 以b（back）表示倒車(chē)行駛 故對(duì)應(yīng)于直線情況 將AB直線記為 前進(jìn)直線路徑記為 倒車(chē)直線路徑記為。對(duì)于圓弧路徑情況 將起終點(diǎn)圓弧記為 那么向前行駛弧AB記為 倒車(chē)行駛AB記為。

3 算法實(shí)現(xiàn)

3.1 算法構(gòu)思

對(duì)于任意位置任意車(chē)頭朝向的兩個(gè)起終車(chē)位點(diǎn) 無(wú)論情況怎樣復(fù)雜都可以將其劃分為如圖四種情況 即考慮實(shí)際情況 車(chē)輛可以左轉(zhuǎn)出發(fā)也可以右轉(zhuǎn)出發(fā) 到達(dá)終點(diǎn)可以是左轉(zhuǎn)到達(dá)或者右轉(zhuǎn)到達(dá)。

假設(shè)在不考慮避障及車(chē)輛速度情況下 認(rèn)為在任意位置車(chē)輛都可以按車(chē)頭反方向倒車(chē)行駛。

實(shí)際路線為車(chē)輛從起點(diǎn)A以拐彎半徑R走圓弧（向左或向右） 接著直行走起終點(diǎn)兩個(gè)圓弧的公切線 最后以拐彎半徑R走圓弧到達(dá)終點(diǎn)B 即路線為圓弧-直線-圓弧路線。可將所有情況都列出 最后對(duì)到達(dá)終點(diǎn)過(guò)程中是否有倒車(chē)情形及到達(dá)終點(diǎn)車(chē)頭的正反向情況進(jìn)行分類(lèi)。將行駛過(guò)程中有無(wú)倒車(chē)狀態(tài)記為'Back'或者'Noback';將到達(dá)終點(diǎn)B與目標(biāo)車(chē)頭朝向的正反狀態(tài)記為'Positive' 'Reverse';那么所有狀態(tài)可以為：'PositiveNoback' 'PositiveBack' 'ReverseNoback' 'Reverse Back'四種。四種狀態(tài)分別表示為：到達(dá)終點(diǎn)正向且行駛中無(wú)倒車(chē)情形;到達(dá)終點(diǎn)正向且行駛中有倒車(chē)情形;到達(dá)終點(diǎn)反向且行駛中無(wú)倒車(chē)情形;到達(dá)終點(diǎn)反向且行駛中有倒車(chē)情形。編寫(xiě)算法計(jì)算求出所有存在的路徑 并對(duì)路徑的狀態(tài)進(jìn)行標(biāo)注。

編寫(xiě)選擇路徑算法 一般情況下 更為關(guān)心的是行駛過(guò)程中無(wú)倒車(chē)情形且到達(dá)終點(diǎn)B頭車(chē)朝向一致的路徑。選擇最短的作為最終需求的路徑。最后按需提取不同狀態(tài)路徑即實(shí)現(xiàn)了路徑的生成。

3.2 路線說(shuō)明

3.2.1 基礎(chǔ)路徑

以圖5左上圖（從起點(diǎn)A右邊出發(fā) 左邊到達(dá)終點(diǎn)B情形）為例進(jìn)行說(shuō)明 從A到B有4種路線 分別是黃色 綠色 藍(lán)色 紫色四種路徑。1.黃色路徑為（圓弧 直線 圓?。?2.綠色路徑為（圓弧 直線 圓?。?3.藍(lán)色路徑為（圓弧 直線 圓弧）;4.紫色路徑為（圓弧 直線 圓?。?/p>

3.2.2 實(shí)際車(chē)輛行駛路徑

根據(jù)圓弧的可正向行駛和反向行駛的特征 到達(dá)一個(gè)圓弧處的終點(diǎn)B有兩種方式到達(dá) 根據(jù)上述路線需要走兩個(gè)圓弧 故每一條路線 可以有四種行駛方法。具體以圖6為例黃色路線為例進(jìn)行說(shuō)明車(chē)輛可能行駛路徑為①（）②（）③（ ）④（ ）。

3.2.3 路徑狀態(tài)標(biāo)注

對(duì)圖6路徑進(jìn)行分析可以發(fā)現(xiàn)該路線到達(dá)終點(diǎn)B的車(chē)頭朝向與需求的正好相反 其中只有路徑①?zèng)]有倒車(chē)行駛情況。那么路徑①的狀態(tài)為'ReverseNoback' 其他的路徑狀態(tài)為'ReverseBack'。圖5中第一種情況路徑狀態(tài)見(jiàn)表1 其它三類(lèi)情況類(lèi)似 均可求出每條路徑的狀態(tài) 故其他三類(lèi)情況不在贅述。

3.3 算法實(shí)現(xiàn)步驟

3.3.1 求圓心

以A點(diǎn)為例 輸入為（xa，ya，αa）及拐彎半徑R 可以求得左右共兩個(gè)圓的圓心位置。輸出為圓心CA（xi，yi）（i=1，2） 同樣終點(diǎn)B處可以求取兩個(gè)圓心CB（xj，yj）（j=1，2）。

式中 x y為圓心的橫縱坐標(biāo);xa ya為起點(diǎn)的橫縱坐標(biāo)位置;αa為起點(diǎn)車(chē)頭朝向;中+表示右側(cè)圓心位置的求法 -表示左側(cè)圓心位置的求法。

3.3.2 求兩圓公切點(diǎn)

根據(jù)兩圓相切公切線存在情況 如圖7 當(dāng)兩個(gè)圓的半徑相同時(shí)存在四種情況即：有四組公切線（兩圓相離） 三組公切線（兩圓外切） 兩組公切線（兩圓相交） 沒(méi)有公切線（同心圓 即兩圓重合）。

求取兩個(gè)圓相切的所有切點(diǎn)對(duì)。輸入?yún)?shù)為CA（xCA? yCA） RA CB（xCB yCB） RB;輸出參數(shù)為公切點(diǎn)對(duì) 即兩個(gè)切點(diǎn)組成的切點(diǎn)對(duì) 如圖8所示外公切線中的切點(diǎn)對(duì)[A1，B1] [A4，B4]及內(nèi)公切線中的切點(diǎn)對(duì)[A2，B2] [A4，B4]。計(jì)算公式如下：

3.3.3 路徑生成

只需將圓弧或者直線路徑求出 按照?qǐng)A弧-直線-圓弧路線生成軌跡路徑即可 三段路徑可用相同的算法進(jìn)行編寫(xiě)。輸入?yún)?shù)為：起點(diǎn)位置及車(chē)頭朝向A（xa，ya，αa） 終點(diǎn)位置A1（xa1，ya1） 向前或者向后行駛（forwardorback） 輸出為終點(diǎn)車(chē)頭朝向αa1 路徑長(zhǎng)度dAA1 離散路徑序列點(diǎn) （xi，yi，αi）。由于路徑為離散采樣 可以根據(jù)不同的需求進(jìn)行設(shè)計(jì) 如等長(zhǎng)度分割 等數(shù)量分割等。算法實(shí)現(xiàn)詳細(xì)如下圖所示。

4 算法仿真結(jié)果

4.1 路徑展示

4.1.1 路徑情形展示

對(duì)于任意位置 任意車(chē)頭朝向的兩個(gè)起終點(diǎn) 根據(jù)圖5所示 一種情況最多有16組路徑 4種情況共計(jì)最多有64種解。例如A點(diǎn)（0，0，0） B（15，15，315） 拐彎半徑R=5。如圖所示為存在64組解的所有情況。具體路徑圖見(jiàn)圖10-13。

4.1.2 路徑分類(lèi)統(tǒng)計(jì)

根據(jù)展示的所有路徑 對(duì)路徑分類(lèi)統(tǒng)計(jì)結(jié)果如下表所示。

4.2 路徑篩選結(jié)果對(duì)比

選擇符合要求的路徑進(jìn)行分析顯示 一般情況下 更關(guān)注正向的沒(méi)有倒擋情況下的路徑即'PositiveNoback' 此外在有倒車(chē)情形下的最短路徑'Positive'也是非常關(guān)注的 下面通過(guò)不同的參數(shù)對(duì)比兩者路徑中的最短路徑。

4.2.1 轉(zhuǎn)彎半徑相同時(shí) 各類(lèi)情況路徑對(duì)比

（1）對(duì)比AB直線距離不同情況下的路徑;設(shè)置起點(diǎn)A位置為（0，0，0） 車(chē)的拐彎半徑R=8 設(shè)置起終點(diǎn)車(chē)頭朝向αb=60° B點(diǎn)與A點(diǎn)距離分別為0 R 2R 3R情況下的路徑對(duì)比如下。

（2）B點(diǎn)與A點(diǎn)距離固定B點(diǎn)的車(chē)頭朝向不同情況下的路徑;設(shè)置起點(diǎn)A位置為（0，0，0） 車(chē)的拐彎半徑R=8 設(shè)置起終點(diǎn)B在A的某個(gè)象限 B點(diǎn)車(chē)頭朝向不同情況下的路徑對(duì)比如下。

（3）B點(diǎn)與A點(diǎn)距離固定 B點(diǎn)車(chē)頭朝向固定 B位于A不同象限情況下的路徑。設(shè)置起點(diǎn)A位置為（0，0，0） 車(chē)的拐彎半徑R=8 設(shè)置起終點(diǎn)車(chē)頭朝向αb=60° B點(diǎn)分別位于A的不同象限情況下的路徑對(duì)比如下。

綜合對(duì)比可以得出：算法可以找到任意復(fù)雜情況的最短路徑;加入倒車(chē)情形會(huì)得到更短的路徑。

4.2.2 轉(zhuǎn)彎半徑不同時(shí) 不同類(lèi)別路徑對(duì)比

設(shè)置起點(diǎn)A位置為（0，0，0） 車(chē)的起始拐彎半徑RA=8 設(shè)置起終點(diǎn)B位置為（0，0，0） 到達(dá)B點(diǎn)拐彎半徑分別設(shè)置為RB=[5，8，11，14] 三種不同類(lèi)型（'PositiveNoback';'Positive';' ReverseNoback'）情況下的路徑對(duì)比如下。

4.2.3 結(jié)論

對(duì)比曲線可以得出以下結(jié)論。1、不同轉(zhuǎn)彎半徑約束下 半徑越大會(huì)得到更長(zhǎng)的路徑里程;2、加入倒擋模式在某種情況下會(huì)得到更短的路徑 倒車(chē)路徑的影響范圍比沒(méi)有倒車(chē)情況更小 倒車(chē)路徑的控制復(fù)雜度要高于沒(méi)有倒車(chē)情形的路徑;3、所有路徑中存在到達(dá)B點(diǎn)但與預(yù)期的車(chē)頭朝向相反的路徑 且在某些情況下會(huì)有更短的路徑 當(dāng)某些場(chǎng)合不太關(guān)注車(chē)頭的正反向時(shí)有重要意義。

5 實(shí)車(chē)驗(yàn)證

通過(guò)仿真結(jié)果可以給出任意情況下 起終點(diǎn)的連接路徑 算法在自動(dòng)駕駛場(chǎng)地進(jìn)行了實(shí)車(chē)測(cè)試 對(duì)比結(jié)果如下圖：

根據(jù)路徑展示情況可以看出 實(shí)車(chē)循跡路線與算法生成的路線基本吻合 僅在拐彎處存在一定的誤差;分析曲率變化情況可以看出 算法生成的曲率平滑 當(dāng)曲率為0表示直行 曲率為-0.1表示車(chē)輛以拐彎半徑為10m進(jìn)行左轉(zhuǎn) 實(shí)車(chē)循跡路線的曲率為緩慢增大再變?yōu)? 這是由于車(chē)輛控制算法導(dǎo)致的 車(chē)輛完成左轉(zhuǎn)首先需要逐步左打方向盤(pán) 待完成拐彎后方向盤(pán)回正 表現(xiàn)出來(lái)的是曲率的絕對(duì)值先增大后減小;點(diǎn)距為采樣點(diǎn) 算法生成的路線點(diǎn)是10cm一個(gè)點(diǎn) 實(shí)車(chē)采樣點(diǎn)距為10cm～20cm之間。

通過(guò)實(shí)車(chē)實(shí)驗(yàn) 驗(yàn)證了算法的可行性 大大提高了無(wú)人駕駛車(chē)輛的測(cè)試效率 解決了要進(jìn)行測(cè)試首先需要車(chē)輛去錄制軌跡數(shù)據(jù)這一“痛點(diǎn)” 在實(shí)踐中得到廣泛應(yīng)用。

最后算法在車(chē)輛遠(yuǎn)程控制中得到應(yīng)用 將算法集成在web端 通過(guò)遠(yuǎn)程路徑下發(fā)來(lái)控制無(wú)人駕駛車(chē)輛自動(dòng)駕駛至目的地。

6 結(jié)束語(yǔ)

本文通過(guò)詳細(xì)介紹了自動(dòng)駕駛循跡路線生成算法 通過(guò)該算法生成的路徑可指導(dǎo)自動(dòng)駕駛車(chē)輛按照路徑進(jìn)行自動(dòng)駕駛。解決了自動(dòng)駕駛車(chē)輛必須先錄制車(chē)輛軌跡路徑才能進(jìn)行自動(dòng)駕駛的弊端。極大的提升了控制的靈活性。

通過(guò)實(shí)車(chē)進(jìn)行了測(cè)試 在空曠的無(wú)人駕駛場(chǎng)地中自動(dòng)駕駛車(chē)輛按照算法生成的既定路線進(jìn)行自動(dòng)駕駛。

參考文獻(xiàn)

[1] Andrei M Shkel，Vladimir Lumelsky. Classification of Dubins set[J]. Robotics and Autonomous Systems 2001，34，179-202.

[2] 劉 流，梁曉龍，何呂龍，等.考慮速度約束的無(wú)人機(jī)Dubins路徑規(guī)劃[J].火力與指揮控制，2018，43（12）：50-58.

[3] 吳克風(fēng)，曹曉文，周其忠，等.任意距離下Dubins最短路徑研究[J].戰(zhàn)術(shù)導(dǎo)彈技術(shù)，2017，1：76-84.

[4] 楊秀霞，曹唯一，張毅.時(shí)間和角度約束下的雙圓弧路徑規(guī)劃[J].空間控制技術(shù)與應(yīng)用，2019，41（8）：1835-1843.

[5] 黃得剛，章衛(wèi)國(guó)，楊立本.無(wú)動(dòng)力無(wú)人機(jī)進(jìn)場(chǎng)階段三維Dubins路徑跟蹤方法[J].系統(tǒng)工程與電子技術(shù)，2016，38（3）：629-637.