基于專家PID的帶臂四旋翼無人機控制方法

陳寶，周祖鵬，衛(wèi)歡，呂延釗，睢志成

（桂林電子科技大學機電工程學院，廣西桂林 541004）

0 引言

近年來，隨著科學技術的發(fā)展，導航和控制技術不斷成熟，飛行機器人不斷應用于航空拍攝、災難現場勘查、環(huán)境建模［1］及抓捕罪犯。一種具有作業(yè)能力的帶臂無人機（Unmanned Aerial Vehicle，UAV）成為了新的研究熱點［2］，帶機械臂的四旋翼無人機主要由四旋翼無人機和機械臂組成，并具有與環(huán)境交互的能力，通過與環(huán)境的“接觸式”應用，帶臂無人機具有廣泛的應用前景，如危險目標的抓取、飛行目標物的捕獲及抓取物品運送到指定地點等。

面向潛在的應用，國內外研究機構設計出多種飛行機械臂系統(tǒng)，德雷塞爾大學實驗室將四旋翼無人機配備機械臂并對機械臂進行動力學建模，以及搭建了測試平臺［3］。文獻［4］設計了一種應用于林冠采樣的航空機械手，用于解決在森林冠層復雜環(huán)境中的樣本采集，并借助回路中的運動捕捉以補償慣性傳感器的不足，使無人機和機械臂的移動抓取更加精準；文獻［5］提出了一種解決空中操縱器碰撞問題的方法，該方法能夠在空中操縱器碰撞期間和碰撞后保持穩(wěn)定；文獻［6］介紹了一種大工作空間并聯航空機械手的設計，該機械手能夠在側面以及空中機器人下方進行物理交互，以實現空中機械臂的無奇異工作空間和高靈巧度；文獻［7］設計了一種帶機械臂無人直升機控制系統(tǒng)，由無人直升機和機械臂組成，并使機械臂拔出固定在地面上的直桿；文獻［8］介紹了四旋翼無人機和單自由度機械臂組成的飛行機械臂控制系統(tǒng)，并使機械臂能夠抓取靜止目標；文獻［9］利用一種單自由度抓手或繩索實現目標物體的空中運輸；文獻［10］介紹了機械臂直升機系統(tǒng)的建模和分析，并設計了一種線性二次規(guī)劃控制器（Linear Quadratic Regulator，LQR），同時對系統(tǒng)閉環(huán)性能進行仿真實驗；文獻［11］針對帶臂飛行器飛行過程中的動態(tài)穩(wěn)定性，提出了一種組合控制法，將增益調度跟模型參考自適應控制（Model Reference Adaptive Control，MRAC）有機融合；文獻［12］設計了一種帶臂無人機的控制器，實現其穩(wěn)定飛行，該控制器采用基于反饋線性化和雙積分器相結合進行控制；文獻［13］設計了自適應魯棒阻抗控制器；文獻［14］為飛行機械臂系統(tǒng)設計了模型預測控制器，面向末端執(zhí)行器的軌跡進行跟蹤控制；文獻［15］建立了漂浮在空間中剛體之間接觸的運動方程，對于非合作目標的抓取，引入了阻抗控制，當機械臂與被動目標碰撞時，通過阻抗控制匹配的方法進行有效的調節(jié)。

本文針對帶二自由度的機械臂無人機控制系統(tǒng)進行設計，利用拉格朗日-歐拉方程對二自由度機械臂的運動學和動力學特性進行全狀態(tài)建模，并通過設計專家PID 控制器對帶臂無人機進行穩(wěn)定性控制，且與常規(guī)的PID 控制器進行對比。通過Matlab/Simulink 對帶臂無人機進行仿真，以驗證帶臂無人機在位置控制和姿態(tài)控制及二自由度機械臂在受干擾下的運動穩(wěn)定情況，其實物如圖1 所示。

圖1 帶臂四旋翼無人機系統(tǒng)Fig.1 System of quadrotor UAV with manipulator

1 帶臂無人機系統(tǒng)模型

1.1 帶臂無人機運動學模型

帶臂無人機控制系統(tǒng)主要是機械臂與無人機以鉸接方式進行連接的。通過拉格朗日-歐拉方程分析系統(tǒng)的內部運動關系，從機械臂末端的位姿及受力方式向前計算得到機械臂在無人機上的受力情況，進而分析無人機的位姿，簡易模型［16］如圖2 所示。

由圖2 模型坐標系可推導該系統(tǒng)的運動模型。其中：Σb表示機載坐標系；Σe表示機械臂末端坐標系；ΣI表示大地坐標系；Pb=[x y z]T∈R3和Θb=[φ θ ψ]T∈R3表示無人機在慣性框架中的位置和歐拉角，φ是繞Xb軸的滾轉角（roll），θ是繞Yb軸的俯仰角（pitch），ψ是繞Zb軸的偏航角（yaw）；Q=[q1q2]T表示機械臂關節(jié)角度。所有的廣義矢量的坐標變量可表示為L=[PTΘTQT]T∈R8×1。

圖2 帶臂無人機的坐標系模型Fig.2 Coordinate system model of UAV with manipulator

本文的運動學是旋翼平臺在慣性坐標系I 下的位姿變換以及機械臂的坐標系E 在慣性系統(tǒng)下位置與姿態(tài)方面的表達。而坐標系B 與大地坐標系I 之間的旋轉變換矩陣為Rb。

1.2 帶臂無人機動力學模型

帶臂無人機通過四個升力Fi(i=1，2，3，4)提升無人機并抓取作業(yè)，機械臂的交互使無人機的位姿發(fā)生改變，即無人機與機械臂形成整個控制系統(tǒng)。對帶臂無人機動力學建模是根據機械臂角度和無人機位置及偏航角得出帶臂無人機的力矩，并通過動力學的逆問題得出帶臂無人機的實際位姿及角度。

帶臂無人機動力學模型是通過拉格朗日-歐拉動力學方程分析系統(tǒng)內部的關系與運動情況。主要通過拉格朗日-歐拉方程［16-17］進行計算，拉格朗日-歐拉方程表達式為：

其中：κ=[x z θ q1q2]T；u主要由無人機在X和Z方向的力矩uf和無人機力矩uλ及機械臂力矩uτ組成；D是慣性矩陣，D∈R5×5；C是離心力和哥氏力構成的矩陣，C∈R5×1；G是重力項構成的矩陣，G∈R5×1。D、C、G表示無人機的X和Z方向的位置和俯仰角及機械臂通過拉格朗日-歐拉方程編寫的動力學結合矩陣。Y方向由無人機的總推力和總質量得到，偏航角及滾轉角取決于力矩和轉動慣量。

而帶臂四旋翼無人機在運動過程中四個旋翼會產生升力和旋轉力矩，關系可表示為：

為了實現對帶臂無人機的閉環(huán)控制需對帶臂四旋翼無人機動力學進行正逆運算，正動力學主要是為了計算出無人機位姿[x z θ]及二自由度機械臂的兩個轉角qi(i=1，2)。則方程為：

2 專家PID控制器設計

專家PID 控制主要利用受控對象和控制規(guī)律的知識進行控制，對被控制的對象不需要精確的模型，使用專家經驗對系統(tǒng)模型進行設計控制。對于系統(tǒng)模型的控制專家PID具有靈活性、適應性和魯棒性，可根據系統(tǒng)的工作狀態(tài)及誤差情況靈活地選擇相應的控制規(guī)律去控制，并根據專家知識和經驗，能動性地調整控制器的參數，適應對象特性及環(huán)境的變化，通過利用專家規(guī)則，控制系統(tǒng)模型在非線性、大偏差下可靠地工作，使帶臂旋翼無人機在專家PID 控制律下能夠可靠地使旋翼無人機穩(wěn)定飛行及平穩(wěn)地抓取物品。

專家PID 主要由五個控制律組成［18］，通過工作狀態(tài)及誤差去選擇相應的控制律去穩(wěn)定數據，并對參數進行調節(jié)達到對控制系統(tǒng)穩(wěn)定的作用。控制結構如圖3 所示。

圖3 帶臂無人機的控制系統(tǒng)結構Fig.3 Structure of control system of UAV with manipulator

專家PID 控制算法如下：

對期望值和實際值進行誤差分析，令e(k)表示當前的誤差值，e(k-1)、e(k-2)分別表示前一個和前兩個采樣的時刻誤差，表達式分別為：

設計專家控制規(guī)律，根據誤差及其變化進行定性分析，控制規(guī)律如下。

1）當|e(k)|＞M1時：誤差絕對值很大，應按控制器的輸出最大（或最?。┬问捷敵?，以達到調整誤差的作用，使誤差的最大速度絕對值減小，可以達到快速使系統(tǒng)控制穩(wěn)定的作用。其控制器輸出為：

其中：c表示系數。

2）當e(k)Δe(k) ＞0 時：

①如|e(k)|≥M2，應按控制器較大的輸出進行控制，以使絕對值的誤差往減小的方向變化，達到控制的目的。其控制輸出為：

②如|e(k)|＜M2，表示誤差往大的方向變化但誤差絕對值變化不明顯，可使用一般控制。其控制器輸出為：

3）當e(k)Δe(k) ＜0，Δe(k)Δe(k-1) ＞0 且e(k)=0 時，表示誤差值不是很大，或處于平衡狀態(tài)，其控制器輸出可保持不變，輸出為：

其中：u(k)表示在上一次輸出過程中輸出值很小，可利用上次的輸出值作為這一次的輸出。

4）當e(k)Δe(k) ＜0 且Δe(k)Δe(k-1) ＜0 時：

①如|e(k)|≥M2時，表示誤差較大處于極限狀態(tài)，應使控制器輸出較大，達到控制的目的。其控制器輸出為：

②如|e(k)|＜M2時，表示誤差比較小，可使控制器輸出較小，其控制器輸出為：

5）當|e(k)|≤ε時，表示誤差的絕對值很小，應加入積分作為控制輸出，以減小穩(wěn)態(tài)誤差。控制器輸出為：

其中：a、b是系數；em(k)為誤差e的第k個絕對值；u(k)為第k次控制器的輸出；u(k-1)為第k-1 次控制器的輸出；k1為增益放大系數，k1＞1；k2為抑制系數，0 ＜k2＜1；M1、M2為誤差界限，M1＞M2＞0；k為周期序數；ε為任意小的正實數。通過對帶臂無人機進行控制器設計，專家PID 能夠很好地對無人機進行穩(wěn)定控制，具有很好的魯棒性。

3 軌跡規(guī)劃

機械臂的軌跡規(guī)劃主要以點到點的運動方式進行，即規(guī)劃一條從初始位置q(t0)到最終位置q(tf)的軌跡，機械臂的軌跡有利于抓取物品的流暢性及規(guī)律性。本文采用五次多項式進行軌跡規(guī)劃，在t0初始時刻和tf最終時刻的角度、角速度、角加速度滿足如下：

五次多項式為：

其中：t=tf-t0；ai(i=0，1，2，3，4，5)為系數。

根據初始位置和最終位置的角度、角速度和角加速度可得方程為：

通過方程組（21）～（22）可得ai(i=0，1，2，3，4，5)系數，即機械臂關節(jié)期望的角度、角速度和角加速度可為：

根據式（23）可對帶臂無人機的機械臂進行軌跡規(guī)劃。

4 仿真實驗

為了驗證在專家PID 控制下帶臂四旋翼無人機的穩(wěn)定性和優(yōu)越性，使用Matlab/Simulink 對帶臂無人機在仿真環(huán)境下進行實驗，帶臂無人機在擾動的情況下仍然處于穩(wěn)定，以驗證算法的可行性。

經實際測量可知，帶臂四旋翼無人機系統(tǒng)的各項參數和指標［19］如表1 所示。

表1 帶臂四旋翼無人機系統(tǒng)仿真參數Tab.1 Simulation parameters of quadrotor UAV system with manipulator

表1 中：Ct和Cq分別是推力系數和扭矩系數；mv是無人機的質量；mi(i=1，2)分別是基座和各關節(jié)的質量；d是轉子到無人機中心的距離；H是無人機中心到地面的距離；Li(i=1，2)分別表示機械臂的臂長；Ixx、Iyy、Izz分別是無人機的轉動慣量。

對帶機械臂無人機的位置和姿態(tài)進行仿真，以驗證算法的可行性和穩(wěn)定性，在仿真之前設置專家PID 控制器的數值，采樣時間ts=0.001，誤差界限M1=0.8，M2=0.000 5，增益放大系數K1=20，抑制系數K2=0.6，任意小的正實數ε=0.000 1，系數a=0.5，b=0.01，c=0.2 等參數，則帶機械臂無人機在設計參數的專家PID 控制器下的仿真性能如圖4～10 所示。

從圖4 表示的帶臂無人機在專家PID 和常規(guī)PID 控制器下位置對比曲線可以看出：X方向專家PID 的收斂速度很快在t=5 s 時就已經達到穩(wěn)定而且超調量很小，而常規(guī)PID 在t=22 s 時才達到穩(wěn)定，超調量要更大一些；Y方向專家PID 在t=4.5 s 達到穩(wěn)定，常規(guī)PID 在t=30.5 s 達到穩(wěn)定；Z方向專家PID 在t=5.0 s 達到穩(wěn)定，常規(guī)PID 在t=19.0 s 達到穩(wěn)定。相較于常規(guī)PID，專家PID 收斂更快、穩(wěn)定性更好。圖5 表示帶臂旋翼無人機在偏航角ψ方面的控制，帶臂無人機以π/6 的角度偏航，專家PID 控制器在t=0.9 s 時就達到穩(wěn)定，而常規(guī)PID 在t=5.7 s 才達到穩(wěn)定。圖6 表示帶臂無人機二自由度機械臂關節(jié)角度對比曲線，專家PID 對機械臂的控制比常規(guī)PID 控制更好，機械臂關節(jié)收斂性更好、超調量更小。

圖4 位置對比曲線Fig.4 Curve comparison of position

圖5 偏航角ψ對比曲線Fig.5 Curve comparison of yaw angle ψ

圖6 機械臂關節(jié)角度對比曲線Fig.6 Curve comparison of joint angles of manipulator

為了驗證專家PID 對擾動的抑制效果，使用階躍信號和正弦波信號分別模擬平均風和陣風的擾動。平均風表示風速不隨時間而變化的風，陣風表示風速隨時間而變化的風，其風速有突發(fā)的特性。用于描述在兩種風的干擾下，帶臂無人機的穩(wěn)定性及收斂性。以偏航角ψ為例，在t=2 s 至t=6 s 的階躍信號為w(t)=1(2 ＜t＜6)；在t=4 s 至t=24 s的正弦波信號為w(t)=0.6sin[π(t-t1)/2] (4 ＜t＜24)，其中：t1為起始時間，t為干擾時間。設偏航角ψd=0 rad 表示懸停效果，實驗結果如圖7 所示。

圖7（a）為階躍信號擾動下兩種控制器估計的結果，可以看出，在t=2 s 有突發(fā)擾動，而在擾動開始時專家PID 雖然有一點超調，但影響較小，且很快收斂到真實值，并保持穩(wěn)定；當t=6 s 擾動消失時專家PID 快速恢復到穩(wěn)定值。對于常規(guī)PID，當出現擾動時超調比較大持續(xù)時間長，擾動消失時不能快速收斂到穩(wěn)定值。圖7（b）為正弦波信號擾動下兩種控制器估計對比曲線，可以看出，t=4 s 時有擾動，而在擾動時專家PID 對真實擾動的跟隨性及準確性都比較好，偏差性較??；在t=24 s 擾動消失時雖然有一點超調但影響比較小，而常規(guī)PID 相對于真實擾動有一定的偏差，且擾動消失時超調比較大。

圖7 外力干擾對比曲線Fig.7 Curve comparison of external interference

由圖8 所示的帶臂無人機在兩個控制器控制下軌跡曲線對比可知：專家PID 控制的帶臂無人機開始起飛后就快速地到達了期望的目標軌跡，離期望的目標軌跡偏差比較小，最終能到達目標位置；而常規(guī)PID 控制的帶臂無人機開始起飛后需要飛一段時間才慢慢地接近期望目標軌跡，且離期望的目標軌跡有很大的偏差，不能到達最終的目標位置。即專家PID 優(yōu)于常規(guī)PID，收斂性更好。

圖8 帶臂無人機軌跡曲線對比Fig.8 Comparison of trajectory curves of UAV with manipulator

而在實際的應用中，可以通過分層控制分別控制無人機和機械臂，上層控制無人機，下層控制機械臂，無人機帶動機械臂到達指定位置，通過機械臂的運動執(zhí)行任務，即機械臂在五次多項式軌跡規(guī)劃下穩(wěn)定到達指定位置。設關節(jié)1 和關節(jié)2 的初始角度q1和q2分別為0，目標位置角度分別為π/6和π/4。機械臂關節(jié)角度如圖9 所示。

圖9 機械臂角度軌跡曲線對比Fig.9 Curve comparison of manipulator angle trajectories

由圖9 機械臂角度軌跡曲線知，帶臂無人機的機械臂在五次多項式規(guī)劃下到達指定位置，且在專家PID 控制下機械臂的軌跡跟隨比常規(guī)PID 更好，跟蹤誤差較小。由圖10 機械臂角速度可知，機械臂第1 個關節(jié)的初始角速度為0，然后速度慢慢增大到最高點，并在第5 秒時達到了最大值為0.098 rad/s，最后角速度逐漸減小到0；機械臂第2 個關節(jié)的初始角速度為0，然后慢慢增大到最高點，并在第5 秒時達到了最大值為0.147 rad/s，最后角速度逐漸減小到0?？梢钥闯鰴C械臂在專家PID 的控制下，關節(jié)角速度平穩(wěn)地從0 到最大值再逐漸減小到0，顯示了機械臂能夠快速平穩(wěn)地到達指定位置并執(zhí)行任務。

圖10 機械臂角速度Fig.10 Manipulator angular velocity

5 結語

本文對二自由度的帶臂無人機系統(tǒng)全狀態(tài)的運動學和動力學模型進行探討，并在此基礎上對帶臂無人機控制系統(tǒng)進行設計及算法研究，分析帶臂無人機的運動狀態(tài)穩(wěn)定性，并提出了利用專家PID 控制器智能整定帶臂無人機的穩(wěn)定參數，與常規(guī)PID 進行對比，專家PID 收斂速度更快、穩(wěn)定性更好、超調量更小，大幅提高了帶臂無人機的位姿跟蹤效果，使帶臂無人機在抓取物品時能更精準操作。仿真結果表明帶臂旋翼無人機在專家PID 的控制下有更好的抗擾性和魯棒性，改善性能更好，具有很好的優(yōu)越性和全局穩(wěn)定性，有很好的實用價值。