考慮速度約束的無人機Dubins路徑規(guī)劃*

劉 流，梁曉龍，何呂龍，朱創(chuàng)創(chuàng)

（空軍工程大學空管領航學院，西安 710051）

0 引言

隨著無人機的飛速發(fā)展，及其應用越來越廣泛，無人機的路徑規(guī)劃問題也受到了廣泛關注。為無人機設計可飛性高的飛行路徑并對無人機實施有效控制，對于無人機的編隊構成和保持具有重要意義[1-4]。

當前無人機路徑規(guī)劃技術多從地面機器人領域借鑒而來，并未較好考慮無人機的性能約束，常規(guī)的解決辦法是先由路徑規(guī)劃算法生成一系列直線段連接的路徑，隨后進行光滑處理得到可飛路徑。Dubins[5]考慮到了轉彎半徑對于車輛（飛行器）運動的影響，使用幾何方法首先討論了運動初始狀態(tài)和終止狀態(tài)之間的最短曲線問題，并在1957年給出了相應數(shù)學證明。梁勇等[6]將Dubins路徑應用到了導彈的時間與角度控制中，利用Dubins路徑轉彎特性完成角度控制，采用微調(diào)轉彎半徑的方法實現(xiàn)了精確時間控制；關震宇等[7]則基于Dubins路徑提出了無人機的避撞規(guī)劃算法，首先分析了單障礙模型，并利用遺傳算法推廣到了多障礙情況中，實現(xiàn)了無人機的實時避撞。Yeol J W等[8]提出了基于Dubins路徑獲取二維最短路徑的方法；Shanmugavel[9]基于Dubins路徑提出了多無人機協(xié)同路徑產(chǎn)生的方法，并利用Clothoid曲線有效解決了Dubins路徑曲率不連續(xù)的問題。

目前針對二維、三維空間內(nèi)的基于Dubins路徑的路徑規(guī)劃研究較多，能夠實現(xiàn)無人機以期望速度方向及最短路徑到達指定位姿點，但并未考慮無人機在運動過程中的速度大小問題，以一定的速度大小到達位姿點對于無人機編隊隊形的保持以及后續(xù)任務的執(zhí)行具有重要意義，無需進行另外的速度大小調(diào)整，便可立即執(zhí)行下一任務，提高了無人機任務執(zhí)行的連續(xù)性。

1 基本問題描述

無人機執(zhí)行任務的過程可看作對一系列任務點Pi（i=1，2，…，n）進行跟蹤的過程[10]，本文對無人機在二維平面內(nèi)，從起始點Ps（Xs，Ys）運動至終止點Pf（Xf，Yf）的過程進行分析，如圖1所示。

圖1 無人機執(zhí)行任務過程

在二維坐標中，無人機的運動學模型為

2 Dubins路徑概述

2.1 Dubins路徑基本概念

對于無人機的控制，除路徑點的坐標外，定義無人機在該點的方向也十分重要，對于無人機后續(xù)任務的執(zhí)行具有重要意義。故本文中所提到的路徑點均為位姿點（帶有坐標與方向）。兩個位姿點間的最短路徑是Dubins路徑[5]。Dubins路徑可簡單定義為，在曲率的限制下，平面內(nèi)兩個位姿點間的最短可行路徑是CSC路徑或CCC路徑，或是它們的子集，其中C表示圓弧段，S表示與C相切的直線段，從Dubins路徑的定義可以看出，其實質(zhì)便是無人機的直線運動模型與勻速轉彎運動模型的交互運動路徑。根據(jù)實際任務的需要以及無人機運動性能的約束，無人機的飛行航程通常會遠大于無人機的最小轉彎半徑，本文僅考慮CSC路徑情況，即時，此時Dubins路徑一定存在。

由于在給定位姿點下，無人機可以向左轉彎和向右轉彎，路徑可以以順時針方向（右）或以逆時針方向（左）開始或結束，此時便存在多條可飛Dubins路徑，Andrei M Shkel[11]給出了最短 Dubins路徑的分類結果，針對起始位姿點與終止位姿點方向角所在象限的不同，以及根據(jù)角度的具體差值確定了不同情況下的最短Dubins路徑，具體過程不再贅述。

2.2 解析幾何Dubins路徑設計方法

本文采用解析幾何方法生成Dubins路徑，僅對初始位姿點與終止位姿點方向角均在第一象限的情況進行分析，其他情況下的算法可參照此情況，本文不再一一分析。文獻[12]中給出了利用解析幾何方法產(chǎn)生Dubins路徑的方法，其步驟如圖2所示。

圖2 Dubins路徑產(chǎn)生步驟圖

首先根據(jù)起始位姿點與終止位姿點的信息以及無人機最小轉彎半徑確定起始、終止轉彎圓圓心，隨后確定起始、終止轉彎圓的切點，便可得出滿足條件的最短Dubins路徑。

本文直接利用此方法根據(jù)初始條件產(chǎn)生Dubins路徑，如下頁圖3所示。

圖3 Dubins路徑示意圖

圖3中Ps（Xs，Ys）為起始位姿點，Pf（Xf，Yf）為終止位姿點，φs，φf分別是起始航向角與終止航向角，Os，Of分別是起始轉彎圓與終止轉彎圓的圓心，PX，PN分別是起始轉彎圓與終止轉彎圓的切點坐標，圓Cs，圓Cf分別為起始和終止轉彎圓。文獻[6]中已經(jīng)證明在其他條件不變的情況下，轉彎半徑越小，總航程越小，故本文中取起始轉彎圓和終止轉彎圓半徑均為 rmin，圓Cff半徑為 2rmin。根據(jù)文獻[11]中的結論，圖4中的最短路徑應為LSR，即在初始點左轉彎加直線段加右轉彎到達終止點。

3 速度控制算法及虛擬位姿點設置

速度控制算法基本步驟如下：

Step2計算直線PXPN段速度；

Step4根據(jù)終止點的速度大小方向要求以及無人機性能約束，確定虛擬位姿點坐標；

Step5計算虛擬位姿點至終止任務點段速度。

起始轉彎圓心Os（xcs，ycs）的坐標為

OsOf與OsT之間的夾角為

圓心中心線OsOf的斜率為tanβ

圓Cs上切出點PX的坐標為

從初始點Ps轉彎至切出點PX所轉過的圓心角為

2）計算直線PXPN段速度

圓Cf上切入點PN的坐標

則在直線PXPN段無人機速度為

終止轉彎圓心Of（xcf，ycf）的坐標

從切入點PN轉彎至終止點Pf所轉過的圓心角為

4）確定虛擬位姿點

如圖4所示，無人機從起始點Ps以速度大小Vs出發(fā)，航向角為φs，現(xiàn)要求無人機以速度大小Vf，航向角φf到達任務點Pf，Pfx點即為所需設置的虛擬位姿點，無人機在到達Pfx點后在PfxPf段完成速度大小的調(diào)整。

圖4 虛擬位姿點設置

PfxPf段為速度調(diào)整段，所需時間為

調(diào)整路程長度為

此處amax為無人機最大縱向加速度大小，為標量。

點Pfx的坐標為

5）計算虛擬位姿點至終止點段速度

無人機的速度為

通過以上步驟便計算出在跟蹤Dubins路徑過程中無人機的速度，以此作為無人機的輸入對其實施控制可完成對Dubins路徑的有效跟蹤。

4 實例仿真

4.1 Dubins路徑速度控制仿真分析

現(xiàn)建立以（0，0）位坐標原點的平面直角坐標系，假設無人機起始位姿點坐標為（150，30）m，起始速度大小為1 m/s，方向角為30°，現(xiàn)要求無人機到達任務點（31.5，60）m，速度大小為 3 m/s，方向角為60°。無人機最大縱向加速度大小為0.5 m/s2，最大飛行速度為5 m/s，最小飛行速度為0.5 m/s，最小轉彎半徑為20 m。

根據(jù)Dubins路徑的解析幾何設計方法及第3節(jié)中速度算法進行模擬仿真，結果如圖5所示。

圖5 Dubins路徑仿真圖

在圖5（a）中，紅色箭頭表示初始位姿點速度方向，黑色箭頭表示終止位姿點速度方向，藍色路徑為Dubins路徑，紫色直線部分為速度大小調(diào)整段，可以看出根據(jù)Dubins路徑解析幾何設計算法，無人機從起始點Ps生成了一條到達虛擬位姿點Pfx的最短Dubins路徑，并在直線部分進行調(diào)整速度大小，最終到達終止任務點Pf。圖5（b）中的藍色虛線和紅色實線分別反映的是無人機在整個運動過程中在x軸和y軸方向上的速度大小隨時間的變化，由于Dubins路徑勻速轉彎的特性，導致速度在某些拐點處可能變化不連續(xù)，在實際中可能會出現(xiàn)一定的誤差，其中速度曲線的首尾曲線段分別為起始、終止轉彎圓弧的速度大小，直線段為切線段勻速運動，可以看出無人機始終以1 m/s的速度大小從起始位姿點運動至虛擬位姿點，隨后進行速度大小的調(diào)整，最終以指定速度大小到達終止任務點Pf。

4.2 飛行實驗驗證

4.2.1 實驗平臺簡介

本文采用單架四旋翼無人機作為實驗平臺，主要由機架、動力系統(tǒng)以及飛行控制核心構成的底層飛控系統(tǒng)和上層上位機系統(tǒng)、數(shù)傳以及自組網(wǎng)模塊組成，單架重量約1.5 kg，最大飛行時間約15 min。實物如下頁圖6所示。

圖6 無人機實驗平臺

無人機飛控系統(tǒng)采用Pixhawk系統(tǒng)，無人機與地面站間的通信協(xié)議采用mavlink通信協(xié)議，通信天線頻率為133 MHz，地面站采用聯(lián)想某型筆記本電腦，使用Mission planner作為飛行參數(shù)設置的軟件，同時選擇Qgroundcontrol作為控制軟件，根據(jù)需要實現(xiàn)無人機的起飛、降落。系統(tǒng)框圖如圖7所示。

圖7 實驗系統(tǒng)示意圖

4.2.2 實驗結果分析

本實驗采用速度作為控制變量，對無人機實施運動控制。首先生成Dubins路徑并通過速度算法得出速度控制輸入，通過地面站將控制指令實時發(fā)送至四旋翼無人機進行控制，同時無人機將自身速度位置信息實時回傳至地面，記錄實驗結果如圖8所示。

1）結果分析

圖8（a）、（b）反映的是無人機在實驗過程中在x軸及y軸方向上的速度大小對比圖，藍色曲線為實驗速度記錄數(shù)據(jù)，紅色曲線為控制輸入速度，可以看出在初始階段無人機對于速度控制輸入存在響應延遲。由于通信等原因，實驗過程中接收到的無人機速度數(shù)據(jù)存在丟包情況，但可以看出在實驗過程中無人機能夠按照期望速度運動。圖8（c）中藍色十字軌跡為無人機在實驗過程中的飛行軌跡，紅色曲線為期望Dubins路徑，從實驗過程中無人機的飛行軌跡可以看出實驗飛行軌跡與期望軌跡存在一定的誤差。圖8（d）中為實驗軌跡與期望軌跡的誤差變化曲線，由于無人機初始階段的響應延遲，誤差在初始階段快速增長，隨后在一定范圍內(nèi)波動。

圖8 實驗結果分析圖

2）誤差分析

從速度對比曲線可以看出，在實驗開始階段的10 s時間內(nèi)，無人機并未立即達到期望的速度，出現(xiàn)初始速度上的響應延遲，導致無人機在后續(xù)飛行過程中始終存在一定誤差。從軌跡誤差曲線也可以看出，在初始階段，由于無人機的響應延遲，誤差快速增長，在后續(xù)過程中，雖然由于GPS定位設備的誤差原因導致誤差仍存在一定的波動，但總體保持在一定范圍內(nèi)，實現(xiàn)了有效的控制。

3）解決方法

針對無人機初始階段的響應延遲，可以采取為無人機設置一段響應緩沖路徑，待無人機完成速度的調(diào)整，能夠穩(wěn)定地響應控制輸入后，方可按照所提出的速度控制方法對無人機實施有效控制。

5 結論

針對無人機的運動性能約束，本文選擇Dubins路徑作為無人機飛行路徑，利用了Dubins路徑可飛性高和二維平面內(nèi)路程最短的特點，對于無人機的運動控制采取了速度控制的方法，提出了Dubins路徑的速度算法，計算出無人機的飛行速度作為控制輸入，通過設置虛擬位姿點使無人機能夠完成速度大小的調(diào)整，按照指定速度大小到達任務點。飛行實驗結果表明采用Dubins路徑作為無人機的飛行路徑是簡便可行的，同時速度控制方法能夠對無人機進行有效控制。在下一步的研究工作中，將把Dubins路徑應用到無人機群的編隊構成、重構等方面中，更好地實現(xiàn)無人機群編隊控制。