考慮物理特征的避障路徑生成算法

羅月童， 王曉靜， 瞿德清， 季 浩， 劉曉平

（合肥工業(yè)大學(xué)計(jì)算機(jī)與信息學(xué)院VCC研究室，安徽 合肥 230009）

路徑生成算法是仿真、游戲等領(lǐng)域的重要研究內(nèi)容，避障路徑生成算法是其重要組成部分。文獻(xiàn)[1]的算法（如圖1 所示）是早期研究工作的一個(gè)代表，它首先依據(jù)障礙物的凸包確定路徑上的關(guān)鍵點(diǎn)，然后用直線依次連接關(guān)鍵點(diǎn)構(gòu)成避障路徑。這種算法不僅所生成的路徑不真實(shí)，而且它沒有考慮運(yùn)動(dòng)物體固有的物理屬性，如車輛的尺寸、速度、轉(zhuǎn)彎半徑等。

本文以車輛為對象，考慮車輛的自身尺寸、速度、轉(zhuǎn)彎、半徑等物理屬性，提出并實(shí)現(xiàn)了一種基于物理模型的避障路徑生成算法。

圖1 利用直線和障礙物凸包得到路徑

1 問題分析

避障路徑生成可分為兩大步驟：首先按某種規(guī)則確定一組關(guān)鍵點(diǎn)，然后采用某種插值算法將所有關(guān)鍵點(diǎn)連接生成一條路徑。路徑插值算法很多，如文獻(xiàn)[1]中線性插值算法、文獻(xiàn)[2]中的基于Hermite 樣條曲線的路徑生成算法。因?yàn)椋螀?shù)樣條曲線可以經(jīng)過每一個(gè)給定的型值點(diǎn)（路徑關(guān)鍵點(diǎn)），給定端點(diǎn)約束條件，便可生成一條C2連續(xù)的曲線，且具有幾何不變性及局部性等特 點(diǎn)[3-6]，本文基本采用三次參數(shù)樣條曲線擬合避障路徑。因此，問題的關(guān)鍵是確定避障路徑的關(guān)鍵點(diǎn)，本文的方法考慮以下因素：

· 障 礙 物 本文僅考慮障礙物的尺寸和形狀屬性；

· 運(yùn)動(dòng)車輛 本文考慮車輛尺寸屬性和轉(zhuǎn)彎半徑屬性；

· 原有路徑 為了保證避障路徑和原路徑之間的G1連續(xù)性，本文方法在確定避障路徑關(guān)鍵點(diǎn)時(shí)需要考慮原有路徑的屬性。

2 避障路徑生成算法

避障路徑生成過程如圖2 所示，首先需要根據(jù)某些條件來確定路徑關(guān)鍵點(diǎn)；對于給定的n 個(gè)關(guān)鍵點(diǎn)，只要知道各關(guān)鍵點(diǎn)處的空間坐標(biāo)和邊界條件，就可以計(jì)算得出各點(diǎn)處的切向量[5]，進(jìn)而得到各Hermite 曲線段的邊界條件，最終插值得到整個(gè)樣條曲線，即生成路徑。

圖2 避障算法流程

2.1 確定路徑關(guān)鍵點(diǎn)

作為算法的基礎(chǔ)，考慮到車輛的尺寸和障礙物尺寸等因素，本文提出了外擴(kuò)凸包的概念：設(shè)車輛的寬度為w，由障礙物凸包的各頂點(diǎn)，分別沿各自兩條鄰邊的外角平分線方向向外擴(kuò)展k 的距離(k＞w)，所形成的新的凸包多邊形，即為外擴(kuò)凸包，其中k 為外擴(kuò)系數(shù)。本文首先生成障礙物的外擴(kuò)凸包，然后基于外擴(kuò)凸包的頂點(diǎn)確定路徑關(guān)鍵點(diǎn)。

如圖3 所示，假設(shè)障礙物存在于原路徑L1上 pa、 pb之間。因?yàn)椋苷下窂街荒芪挥谠窂降耐粋?cè)，因此在生成一條避障路徑時(shí)，只需考慮位于原路徑一側(cè)的外擴(kuò)凸包頂點(diǎn)，現(xiàn)將位于L1的某一側(cè)的頂點(diǎn)記為 p1… pn。

如果將 p1… pn全作為路徑關(guān)鍵點(diǎn)，則生成的路徑如圖3 中路徑L2，過于彎曲，不符合實(shí)際。本文按下述方法對 p1… pn進(jìn)行挑選，使得所生成路徑如L3所示，更加自然逼真。

以 pa點(diǎn)為原點(diǎn)，以原路徑L1在 pa的切向量方向?yàn)閤 軸正向建立局部坐標(biāo)系。設(shè)原路徑L1的曲線方程為 f ( x,y)=0，分別從 pa, pb向障礙物外擴(kuò)凸包的各頂點(diǎn)引切線，可得切線斜率kia和 kib, 其中1≤i ≤ n 。顯然，無論凸包形狀如何，都會(huì)存在這樣的兩個(gè)點(diǎn) ps和pt，它們在原路徑曲線的同一側(cè)， ps先于或等于 pt且斜率ksa, ktb的絕對值分別為最大。即滿足下面條件：

（1） s ≤ t；

（2） f ( xs,ys)? f(xt,yt)＞0；

將 ps、 pt兩點(diǎn)選入控制點(diǎn)集，若它們之間存在其它點(diǎn) pj,s ≤ j ≤ t，由凸包性可知，其一定在 ts pp 連線的外側(cè)（遠(yuǎn)離障礙物的一側(cè)），則 加入控制點(diǎn)集，直到控制點(diǎn)集為最大。依次以這些控制點(diǎn)為關(guān)鍵點(diǎn)，可以生成一條樣條曲線路徑。

圖3 路徑關(guān)鍵點(diǎn)的選擇

2.2 生成候選路徑

為了保證與原路徑的G1連續(xù)，本文以新路徑與原路徑連接點(diǎn)處的切向量 p'a、 p'b作為邊界條件，可以得到矩陣表達(dá)式[5]

從而得到各Hermite 曲線段的端點(diǎn)約束，即得到樣條曲線路徑。

三次Bézier 曲線需要4 個(gè)控制點(diǎn)，因?yàn)槿螀?shù)樣條曲線具有幾何不變性和局部性，本文用如下方法來確定控制點(diǎn)（如圖4 所示）：

圖4 第一段曲線的改進(jìn)

（1） 局部坐標(biāo)系的建立同2.1；

（3） 依次以 Pa、 A1、 A2、Ps為控制點(diǎn)，形成控制多邊形，則生成的Bézier 曲線不會(huì)超出控制多邊形的區(qū)域。

由Bézier 曲線的端點(diǎn)性質(zhì)可知， Pa、 Ps兩點(diǎn)處的切向量方向不會(huì)發(fā)生變化，故可以保證路徑的G1連續(xù)。bp 點(diǎn)可用同樣方法處理。

2.3 確定避障路徑

該算法未考慮避障路徑上存在障礙物的情況，針對該情況，可以將新障礙物的外擴(kuò)凸包頂點(diǎn)加入到預(yù)選頂點(diǎn)中來，然后再應(yīng)用該算法，找出避障路徑。

3 實(shí)驗(yàn)和應(yīng)用

本算法在某軍的三維戰(zhàn)場仿真決策系統(tǒng)中得到應(yīng)用，如圖5 所示。圖中原路徑C1上存在障礙物，C2為避障路徑，仿真效果良好。

4 總結(jié)與展望

本算法在基于物理模型的基礎(chǔ)上，利用三次參數(shù)樣條曲線生成避障路徑，可以保證新生成路徑本身的C2連續(xù)性和與原路徑的G1連續(xù)；在計(jì)算插值點(diǎn)三維坐標(biāo)時(shí)，只關(guān)心車輛的x 和y 坐標(biāo)，至于車輛的高度信息（z 坐標(biāo)值），可以在具體的應(yīng)用中實(shí)時(shí)獲取地面信息，保證車輛隨時(shí)緊貼地面運(yùn)動(dòng)即可；障礙物位于pa, pb中點(diǎn)位置附近時(shí)，避障路徑生成效果最好，所以本算法與合適的障礙物檢測算法配合能達(dá)到較好的仿真效果。

[1] 吳風(fēng)光, 叢 爽. 自動(dòng)避障中的一種路徑生成、選擇與實(shí)現(xiàn)[C]//自動(dòng)化理論、技術(shù)與應(yīng)用. 2002： 63-68.

[2] 劉曉平, 曹 力, 張 靜. 物體運(yùn)動(dòng)路線多樣化模擬[J]. 工程圖學(xué)學(xué)報(bào), 2007, 28(3)： 39-43.

[3] 包 曄. 樣條插值在運(yùn)動(dòng)模擬中的應(yīng)用[J]. 杭州師范學(xué)院學(xué)報(bào)(自然科學(xué)版), 2004, 3(5)： 373-377.

[4] 賴舜男, 吳學(xué)禮, 汪國平. G2三次Hermite 樣條曲線形狀的交互修改[J]. 計(jì)算機(jī)應(yīng)用研究, 2004, (10)： 106-109.

[5] 唐澤圣, 周嘉玉, 李新友. 計(jì)算機(jī)圖形學(xué)基礎(chǔ)[M]. 北京： 清華大學(xué)出版社, 1995. 78-87.

[6] 銀紅霞, 杜四春, 蔡力軍. 計(jì)算機(jī)圖形學(xué)[M]. 北京：中國水利水電出版社, 2005. 130-159.