楊秀霞，劉小偉，張毅

(海軍航空工程學(xué)院控制工程系，山東煙臺264001)

0 引言

目前，無人機(jī)在軍事及民用領(lǐng)域已得到越來越廣泛的應(yīng)用。如何使無人機(jī)能夠安全導(dǎo)引到目標(biāo)并順利避開動態(tài)障礙物是眾多學(xué)者研究的重點(diǎn)課題。

最早對這一問題開始研究的是 Khatib［1］，Erdmann等［2］。后來，學(xué)者們把在靜態(tài)環(huán)境下得到成功應(yīng)用的路徑規(guī)劃及避障方法推廣到動態(tài)環(huán)境中。如在文獻(xiàn)［3］中提到了確定及隨機(jī)的路徑地圖方法;在文獻(xiàn)［4］中考慮機(jī)器人及障礙物的速度，并加入到勢能函數(shù)的構(gòu)造中，得到適用于動態(tài)環(huán)境的勢場法;在文獻(xiàn)［5］中通過動態(tài)窗口方法進(jìn)行路徑規(guī)劃及避障。這些方法在靜態(tài)環(huán)境下得到了較好的結(jié)果，但是在動態(tài)環(huán)境下，尤其是應(yīng)用于無人機(jī)高速運(yùn)動這一特殊情況下，避障效果并不理想。其原因主要是在動態(tài)環(huán)境下，碰撞判斷及避障情況下對速度的控制與靜態(tài)環(huán)境相比，存在較大的區(qū)別。

為解決動態(tài)環(huán)境下的碰撞判斷問題，文獻(xiàn)［6-7］提出了基于運(yùn)動學(xué)方程的碰撞判斷方法。文獻(xiàn)［6］定義了碰撞錐的概念，依據(jù)相對速度角度與障礙圓的切線來確定碰撞。在文獻(xiàn)［7］中基于視線角速度方程，對直接碰撞和間接碰撞進(jìn)行了比較。

同時，考慮到動態(tài)環(huán)境下障礙物速度變化的問題，學(xué)者們提出了速度障礙的概念。例如文獻(xiàn)［8］在考慮運(yùn)動障礙物速度的基礎(chǔ)上進(jìn)行碰撞錐的構(gòu)建，在此基礎(chǔ)上，通過使機(jī)器人的速度轉(zhuǎn)移到碰撞錐之外達(dá)到避障目的，但是對障礙物的速度要求必須是線性的。為解決障礙物速度為不定值的避障問題，文獻(xiàn)［9］通過對某一時刻，在考慮無人機(jī)及障礙物相對位置情況下，對可能的碰撞速度進(jìn)行時間積分，得到非線性速度障礙集進(jìn)行避障。同時考慮到多無人機(jī)之間的避碰問題，文獻(xiàn)［10］提出了互惠速度障礙的概念，即兩個即將相碰的無人機(jī)之間互相主動避碰，達(dá)到避碰目的。

但上面所有的方法均未考慮避障過程中避障完成時間的問題?，F(xiàn)實(shí)情況下，只有避障完成時間小于無人機(jī)到達(dá)避障點(diǎn)時間，才能確保無人機(jī)成功避障。因此，基于對避障完成時間的考慮，本文提出了滑模變結(jié)構(gòu)有限時間收斂制導(dǎo)律的避障算法，使無人機(jī)相對障礙物的速度方向在有限時間內(nèi)轉(zhuǎn)移到碰撞錐之外達(dá)到避障目的。本文首先在極坐標(biāo)系下建立無人機(jī)與障礙物的相對運(yùn)動學(xué)方程，通過引入滑模變結(jié)構(gòu)有限時間收斂制導(dǎo)律進(jìn)行避障算法設(shè)計。最后對有限收斂時間進(jìn)行分析，得到了滿足要求的參數(shù)范圍，可以使無人機(jī)的收斂時間小于到達(dá)避障點(diǎn)時間，確保避障完成。最后利用Matlab對設(shè)計的避障算法進(jìn)行了仿真驗(yàn)證。仿真結(jié)果表明，在選擇合適的參數(shù)下，無人機(jī)相對障礙物速度方向收斂到避障切線方向的時間小于無人機(jī)到達(dá)避障點(diǎn)的時間，驗(yàn)證了算法的有效性。

1 幾何關(guān)系及運(yùn)動學(xué)方程

1.1 問題描述

當(dāng)無人機(jī)與障礙物在慣性空間運(yùn)動時，建立無人機(jī)與障礙物的幾何關(guān)系示意圖，如圖1所示。無人機(jī)的速度大小為vuav，方向?yàn)棣譽(yù)av;障礙物的速度大小為vobs，方向?yàn)棣護(hù)bs。無人機(jī)相對障礙物的速度大小為vrel，方向?yàn)棣譺el，其中vrel=vuav－ vobs，－ vobs為障礙物速度相反速度。由速度障礙原理，通過擴(kuò)大障礙圓半徑為RP，則把無人機(jī)簡化為質(zhì)點(diǎn)U，基于質(zhì)點(diǎn)U及障礙圓切線BU和AU，建立碰撞區(qū)域UAB。若vrel位于UAB內(nèi)，則存在碰撞危險，因此需要調(diào)整無人機(jī)的速度大小及方向，使vrel調(diào)整到UAB之外。調(diào)整時，一般考慮vrel角度變化最小即避障完成時間短、偏離原目標(biāo)軌跡短原則。結(jié)合圖1，選取避障切線為BU，即避障點(diǎn)為B，其中避障切線BU的長度為rBU，方向?yàn)?θBU。

本文提出的避障算法是基于滑模變結(jié)構(gòu)有限時間收斂制導(dǎo)律，使無人機(jī)能夠在有限時間內(nèi)到達(dá)避障點(diǎn)B，且相對速度方向ψrel與切線BU方向θBU相等，確保避障完成。

圖1 無人機(jī)與障礙物幾何關(guān)系示意圖Fig.1 Schematic diagram for geometric relation of UAV and obstacle

1.2 建立運(yùn)動學(xué)方程

由于假設(shè)障礙圓是剛性圓，因此B點(diǎn)的速度大小及航向角與圓心一致。結(jié)合圖1，在極坐標(biāo)系下可得無人機(jī)U與B點(diǎn)之間的距離rBU及視線角θBU的變化率為:

為研究方便，建立無人機(jī)與障礙物的相對運(yùn)動方程，此時無人機(jī)以相對速度vrel飛向避障點(diǎn)B:

式(3)給出了無人機(jī)與避障點(diǎn)B之間的距離變化關(guān)系，式(4)給出了無人機(jī)與避障點(diǎn)B之間的視線角的變化方程。

對式(4)求導(dǎo)，并將式(3)代入，整理可得:

式中:urel為相對速度vrel的加速度在視線法向上的分量。

選取無人機(jī)在發(fā)現(xiàn)障礙物開始避障的初始時刻為t=t0，初始狀態(tài)為rBU(t0)和，初始視線角為θBU(t0)，在t時刻的狀態(tài)為rBU(t)和，令x，則式(5)可寫為:

2 滑模變結(jié)構(gòu)有限時間收斂避障算法設(shè)計

無人機(jī)在避障過程中控制系統(tǒng)容易受到參數(shù)變化和外界環(huán)境干擾，因此制導(dǎo)律的設(shè)計必須考慮系統(tǒng)的魯棒性。滑模變結(jié)構(gòu)控制對參數(shù)攝動和外界干擾具有不變性，使系統(tǒng)具有良好的抗干擾能力，因此基于滑模變結(jié)構(gòu)控制理論設(shè)計有限時間收斂避障算法是有實(shí)際意義的。下面將基于滑?？刂评碚撛O(shè)計有限時間收斂制導(dǎo)律，通過選擇合適的參數(shù)，使避障完成時間小于到達(dá)避障點(diǎn)時間，完成避障。

2.1 制導(dǎo)律設(shè)計

由初始時刻x1(t0)=0，x2(t0)≠0設(shè)計非奇異滑模面為:

式中:β＞0，p和q(p＞q)為正奇數(shù)。

設(shè)計非奇異滑?？刂破鳛?

式中:1＜p/q＜2，η＞0。

則在控制器式(10)的作用下，系統(tǒng)將克服外界干擾，在有限時間內(nèi)到達(dá)滑模面式(9)，并且狀態(tài)量x1和x2沿滑模面運(yùn)動，將在有限時間內(nèi)收斂到零。

2.2 穩(wěn)定性分析

對于滑模到達(dá)階段，選取Lyapunov函數(shù):

對式(11)求導(dǎo)，可得:

結(jié)合式(8)和式(9)可得:

把式(10)代入式(13)得:

因此可得:

由于1＜p/q＜2，則0＜p/q－1＜1，又由于β ＞0，p和q(p＞q)為正奇數(shù)，則0時)。因此可得:

可見，控制器滿足Lyapunov穩(wěn)定條件，系統(tǒng)狀態(tài)在有限時間內(nèi)收斂到滑模面。

2.3 有限收斂時間分析

基于滑模變結(jié)構(gòu)控制律，系統(tǒng)運(yùn)動分為滑模到達(dá)和在滑模上運(yùn)動到平衡點(diǎn)兩個階段。因此，收斂時間分為相應(yīng)的兩部分。針對滑模到達(dá)階段，設(shè)s(t0)≠0到s(t)=0的時間為tr。當(dāng)t=t0+tr時，s(t0+tr)=0。由得:

當(dāng) s≥0時，有:

對式(17)兩邊積分得:

則:

即:

同理，當(dāng)s≤0時，有:

下一步計算狀態(tài)變量沿滑模面運(yùn)動到平衡點(diǎn)的時間ts。考慮到系統(tǒng)初始條件的奇異性，設(shè)計滑模面為:

當(dāng)狀態(tài)變量在滑模上運(yùn)動時，滿足s=0，即:

同時式(21)等價于

則:

因此總的制導(dǎo)系統(tǒng)的收斂時間可表示為:

綜上可知，所設(shè)計的制導(dǎo)律能夠使制導(dǎo)系統(tǒng)狀態(tài)在有限時間內(nèi)收斂。由式(20)及式(23)可以看出，系統(tǒng)狀態(tài)到達(dá)滑模面的時間與選取的參數(shù)η成反比，即η越大，系統(tǒng)狀態(tài)收斂越快，收斂時間就越短;當(dāng)系統(tǒng)狀態(tài)到達(dá)滑模面后，收斂到原點(diǎn)的時間與選取參數(shù)p，q及β有關(guān)。因此可以通過調(diào)節(jié)參數(shù)η，p，q及β來控制系統(tǒng)的收斂速度，使總的制導(dǎo)系統(tǒng)的收斂時間t小于到達(dá)避障點(diǎn)時間trea=rBU(t0)/vrel，完成避障。

3 無人機(jī)航向角控制

由上述避障算法設(shè)計可求出控制urel。假設(shè)相對速度大小不變，即，則由式(6)可知:

進(jìn)而可知:

假設(shè)障礙物為勻速運(yùn)動，則arel全部由無人機(jī)提供，因此可得無人機(jī)速度大小及方向角變化率為:

由式(27)及式(28)就可求出無人機(jī)的運(yùn)動軌跡。

4 仿真結(jié)果及分析

為驗(yàn)證上述避障算法的有效性，對避障算法進(jìn)行仿真。

假設(shè)障礙物為勻速運(yùn)動，vobs=43 m/s，ψo(hù)bs=0 rad，初始坐標(biāo)為 xobs0= －10 km，yobs0=10 km，無人機(jī)初始速度大小為vuav0=43 m/s，初始方向角為ψo(hù)bs0=(π/2)rad，初始坐標(biāo)為xuav0=0 km，yuav0=0 km，假設(shè)無人機(jī)和障礙物圓半徑RP=250 m。

通過計算，可求得無人機(jī)到達(dá)避障點(diǎn)B的時間為:

因此，通過選擇合適的參數(shù)η，p，q及β，可使無人機(jī)在到達(dá)避障點(diǎn)前相對速度角度收斂到期望角度。

通過計算，選擇η=0.002，p=8，q=7，β=0.17，仿真結(jié)果見圖2～圖7。

圖2 視線角變化示意圖Fig.2 Schematic diagram for variation of the angle of sight

圖3 視線角速率變化示意圖Fig.3 Schematic diagram for variation of the angular rate of sight

圖4 相對速度角度變化示意圖Fig.4 Schematic diagram for variation of the angle of relative velocity

圖5 無人機(jī)速度變化示意圖Fig.5 Schematic diagram for velocity variation of UAV

圖6 無人機(jī)速度方向角變化示意圖Fig.6 Schematic diagram for variation of velocity and orientation angle

圖7 無人機(jī)避障過程示意圖Fig.7 Schematic diagram for UAV to avoid the obstacles

由圖2可知，當(dāng)無人機(jī)觀察避障點(diǎn)B的視線角穩(wěn)定時，t=20 s，小于trea=22 s，且穩(wěn)定視線角與初始時刻發(fā)現(xiàn)避障點(diǎn)B的切線角相等，保證無人機(jī)能夠順利避障。由圖3可知，在t=20 s時，視線角速率，視線穩(wěn)定，再次證明避障完成。由圖4可知，當(dāng)t=20 s時，相對速度角度ψrel=θBU=2.717 6 rad，即相對速度角度穩(wěn)定時，相對速度角度到達(dá)避障方向，避障完成。由圖5和圖6可知，無人機(jī)實(shí)際速度大小及方向角變化不劇烈，即過載不是很大，保證了過載要求。由圖7可知，障礙物圓與無人機(jī)圓相遇時兩圓相切，也就保證了避障完成。

5 結(jié)束語

本文基于無人機(jī)與障礙物之間的幾何關(guān)系，得到無人機(jī)與障礙物在極坐標(biāo)系下的運(yùn)動方程，在考慮避障完成時間的基礎(chǔ)上，通過引入滑模變結(jié)構(gòu)有限時間收斂導(dǎo)引律，設(shè)計避障算法，選擇合適的參數(shù)，可使避障完成時間小于無人機(jī)到達(dá)障礙物的時間，保證了避障完成。仿真結(jié)果證明了該算法的有效性。

本算法的實(shí)現(xiàn)僅需要知道障礙物及無人機(jī)的位置及速度這4個參數(shù)，因此，算法操作起來比較方便。下一步的工作是將該算法擴(kuò)展到三維空間，并對障礙物是變加速的情況進(jìn)行研究。

［1］ Khatib O.Real time collision avoidance for manipulators and mobile robots［J］.The International Journal of Robotics Research，1986，5(1):90-98.

［2］ Erdmann M，Lozano-Perez T.On multiple moving objects［J］.Algorithmica，1987，2(1):477-521.

［3］ Fujimura K，Samet H.Roadmap-based motion planning in dynamic environments［J］.IEEE Trans.Robot，2005，21(5):885-897.

［4］ Ge S，Cui Y.Dynamic motion planning for mobile robots using potential field method［J］.Auton Robots，2002，13(3):207-233.

［5］ Seder M，Petrovic I.Dynamic window based approach to mobile robot motion control in the presence of moving obstacles［C］//Conf.Robot.Italy:Autom，Roma，2007:1986-1992.

［6］ Chakravarthy A，Ghose D.Obstacle avoidance in dynamic environment:a collision cone approach［J］.IEEE Transactions Systems，Man Cybernetics:Systems，Humans，1998，28(5):562-574.

［7］ Belkhouche F，Belkhouche B.Kinematics based characterization of the collision course［J］.Robot Autom，2008，23(2):127-136.

［8］ 朱齊丹，鐘訓(xùn)昱，張智.基于速度變化空間的移動機(jī)器人動態(tài)避碰規(guī)劃［J］.機(jī)器人，2009，31(6):539-547.

［9］ Large F，Sckhavat S，Shiller Z，et al.Using non-linear velocity obstacles to plan motions in a dynamic environment［C］//ICARCV Control，Automation，Robot and Vision Conference.France:IEEE，2002，734-739.

［10］ Van den Berg J，Lin M，Manocha D.Reciprocal velocity obstacles for real-time multi-agent navigation［C］//ICRA Robotics and Automation Conference.CA:IEEE，2008，1928-1935.