基于前饋補償的PD四旋翼無人機軌跡跟蹤控制*

劉栩粼 胡德清

（四川信息職業技術學院電子與物聯網學院 廣元 628000）

1 引言

近年來，由于四旋翼無人機具有成本低廉、易操控等特征，特別是在許多高風險、高強度的任務中更具備獨特優勢，在災害救援、軍事、精細化巡檢等領域得到了廣泛應用，并且受到了越來越多的關注。眾所周知，自主飛行控制算法是保證無人機擁有良好的穩定性、良品率和適應性的關鍵技術，因此，注重研究其飛行控制方法一直是四旋翼無人機產業發展的重要熱點之一［1～4］。

經典PID控制算法是普遍應用于飛行控制系統中的研究方法之一［5～10］。PID控制雖是工程實踐應用最廣泛的控制技術，但它又存在著自身的缺陷，如參數整定的過程較繁復、缺乏可移植性、解耦控制和魯棒控制等［11～13］。正因為存在以上缺點，四旋翼UAV飛行控制性能難以得到提升。伴隨著新型材料、微機電及控制技術的發展，以及新技術與PID控制方法相融合的優良PID控制方法也在無人機飛控中得到了大批的應用［14］，比如，為了實現四旋翼無人機的靈活機動性和穩定性［15］，利用非線性控制方法來控制四旋翼無人機飛行位置的勘定及飛機的飛行姿態；又比如，針對無人機存在的動態調節不理想、控制器反應時間過長，調節量大等問題，范怡敏等人在文獻［16］的基礎上，引入神經網絡設計了控制方法，該方法不僅實現了無人機的自我控制，同時還縮短了無人機自我控制的過渡過程［17］；王祺等［18］基于模糊自適應控制方法，通過補償項消除干擾以及在線估計PID系數，設計并解決了無人機目標追蹤自適應控制。

本文主要以四旋翼無人機為研究對象，建立其非線性數學模型，再基于前饋補償的PD設計了四旋翼控制器，將無人機的軌跡跟蹤控制系統分為內外兩個控制環路，內外環均由PD控制，然后進行仿真驗證與分析。研究結果表明，基于前饋補償PD控制器不僅調節時間短，而且能補償系統所受到的內外干擾，提高系統的穩定性，較好地滿足了系統性能要求。

2 四旋翼無人機模型

四旋翼無人機［15～17］取地理坐標系RN(nx，ny，nz)和機體坐標系RB(bx，by，bz)，兩套坐標系均滿足右手定則且原點重合。四旋翼無人機從地理坐標系到機體坐標系下的轉換關系用姿態矩陣RNB表示，如式（1）。

式中，φ、θ和φ分別表示四旋翼無人機橫滾角、俯仰角和偏航角；ci表示的是cosi，si表示的是sini。

常態下，為了研究的便利性和有效性，會對數學模型增加約束條件的同時，也會忽略轉動慣量和空氣系數的物理效應對四旋翼無人機運動產生的影響。因此，根據拉格朗日方程［16～18］，四旋翼無人機的動力學模型如下所示：

其中：

其中，U1、U2、U3和U4分別代表飛行器的垂直升力、橫滾力矩、俯仰力矩、偏航力矩；Fi(N)表示四旋翼飛行器電機升力；K1、K2、K3為空氣阻力對位置坐標的影響系數；K4、K5、K6為陀螺效應對歐拉角度的影響系數；Ω1、Ω2、Ω3和Ω4分別為電機M1、電機M2、電機M3和電機M4上旋翼的旋轉速度。

表1給出了四旋翼無人機數學模型的相關參數信息。

表1 四旋翼參數表

3 四旋翼控制器設計

由于四旋翼無人機具有欠驅動的特質［19～20］，故而它不能同時對六個自由度進行跟進。因此，給出一個有效的控制實施方案為：跟蹤航跡[ ]x，y，z和橫滾角φ，同時保證俯仰角θ和偏航角φ的角度［13］。所設計的整個控制系統結構，如圖1所示。

圖1 基于補償的PD控制

接下來的設計過程中，采用二階系統Hurwitz判據，同時保證a0/a1/a2＞0和a0*a1＞0，便能讓系統具有穩定性。

3.1 位置控制律的設計

通過設計位置控制律u1，實現四旋翼無人機軌跡跟蹤［12～15］，即控制目標：x→0，y→0，z→0。

根據Hurwitz判據，式（2）、式（3）可得描述位置狀態的模型為

以第一個位置子系統為例，采用PD控制方法設計控制律為

同理，分別對第二個位置子系統設計PD控制律和第三個位置子系統設計基于前饋和重力補償的PD控制律：

因此，基于補償的PD位置控制各參數和控制方案如表2所示。

表2 PD位置控制參數表

3.2 虛姿態角度控制律的設計

下面針對三個姿態子系統設計PD控制律，實現φ→φd，θ→θd，φ→φd。

取φc=φ-φd、θc=θ-θd、φc=φ-φd，以第一個姿態角子系統為例描述PD控制律的設計過程：

同理，分別對第二個姿態子系統設計基于前饋補償的PD控制律和第三個姿態子系統設計PD控制律：

因此，基于補償的PD姿態控制各參數和控制方案如表3所示。

表3 PD姿態控制參數表

4 閉環系統設計

考慮到上節所設計的PD控制律能使四旋翼無人機達到理想的狀態，其整個控制系統結構圖如圖2所示。

圖2 閉環系統結構圖

上述閉環系統是由外環位置子系統和內環姿態子系統所構成的，而整個控制系統又采取了雙環控制的方法來進行設計飛行控制律。兩個中間姿態指令信號θd、φd，從外環系統傳遞給了內環，在內環中又通過控制方法來實現對θd、φd兩個姿態指令信號的跟蹤。

4.1 期望姿態角度控制律的設計

假設滿足式（6）、式（7）所需要的姿態角度為θd和φd，為了實現θ→θd和φ→φd，就需要對兩個虛擬姿態角（θd、φd）進行求解。

由式（5）可知：

1）當x＞1當時，取sinθd=1，即；

2）當x＜-1時，取 sinθd=-1，即；

3）當 |x|≥1時，即：

根據以上分析，若要使式（14）成立，那么將式（14）改寫成以下形式即可：

其中，ζ1x、ζ1x和ζ1x為時變值，ξ=[ζ1x，ζ1y，ζ1z]，當|x|≤1時，可取ζ=0；當|x|＞1時，ζ為了滿足上述成立的實數，相當于式加載控制輸入u1x、u1y和u1z上的擾動，可通過控制器的魯棒性來克服。

求解θd和φd后，便可得到位置控制律為

4.2 期望姿態角速度控制律的設計

上式中，待微分的輸入信號用v(t)表示；ζ=0.04；對期望姿態角度的跟蹤、對期望姿態角速度和期望姿態角加速度的估計分別用x1、x2和x3表示；初始化微分器：x1(0)=0，x2(0)=0，x3(0)=0。

鑒于解決非連續函數求導問題，可采用微分器。因此，一方面不要求期望姿態角指令信號θd和φd滿足連續性，另一方面在位置控制中可包含切換函數。本文采用的是積分鏈式結構式微分器，對含有干擾的信號進行求解導數時，干擾信號位于微分器最后一層，這樣通過積分作用后的信號中的干擾就能被足夠地抑制住。

5 仿真結果

為了展示PD控制器對四旋翼無人機的控制性能的有效性，我們在Matlab/Simulink環境下模擬四旋翼無人機系統的三個姿態角和三個位置的軌跡跟蹤。被控制對象位置初始狀態和目標狀態取[2，1，3，0，0，10]，姿態初始狀態和目標狀態取[0，0，0，0，0，0]。

再結合四旋翼無人機模型式（2）和參數表1，取K1=K2=K3=0.01、K4=K5=K6=0.012、I1=I2=1.25、I3=2.5、d4=d5=d6=0.1。采用內環收斂速度大于外環收斂速度的方法，保證閉環系統的穩定性。因此，取內環控制器增益遠遠大于外環控制器增益。采用位置控制律式（6～7）和姿態控制律式（9～10）。最后，仿真結果如圖3～5所示。

圖3 三個姿態角的收斂過程

圖4 三個位置狀態的收斂過程

圖5 四個控制輸入的變化過程

從圖3、圖4可知，橫滾角和偏航角在第4s時收斂到位，而俯仰角的收斂時間只有2.3s。但，三個位置狀態的收斂時間均用時4s。從上述收斂結果可以得出：本算法通過調整內環控制其增益系數，使得內環收斂速度大于外環收斂速度，保證了閉環系統的穩定性。

從圖5可知，僅垂直通道的控制力用時稍久、俯仰通道的控制力在初始時變化稍大，而其他兩個通道的控制力能在較短時間內收斂到位。

6 結語

在存在外部未知擾動的情況下，基于前饋補償的PD控制器，一方面較好地控制了內環增益系數，即在飛行姿態控制方法的設計過程中，采取了合適的PD增益系數，保證了內環收斂速度大于外環收斂速度，另一方面使得四旋翼無人機在姿態和位置調節時間上用時較少。從仿真結果中可知，相較于純PD飛行控制律，本文所設計的控制律使得四旋翼無人機飛行控制系統更加穩定，能夠按照期望的軌跡懸停。并且能較好地補償系統所受到的內外干擾，提高了系統的穩定性，較好地滿足了系統性能要求。