基于約束跟隨理論的消防無人機水槍噴嘴跟蹤控制

張新榮 ，康龍 ，張東升 ，張軍

（1.長安大學 道路施工技術與裝備教育部重點實驗室，陜西 西安 710064；2.西安交通大學 機械工程學院，陜西 西安 710049）

隨著經濟的發展和人口的增長，城市規模變得越來越大，為了滿足日益增長的空間需求，出現了越來越多的高層建筑甚至摩天大樓，高層建筑的消防工作已成為一個新的城市問題.傳統的消防車和消防梯只能用于較低的建筑物，而對于10 層以上的更高層建筑很難實施滅火措施［1］.

近年來，無人機（Unmanned Aerial Vehicle，UAV）在民用、軍事等領域發揮著重要作用，因此，受到了工業界和學術界的廣泛關注［2］.由于單純的UAV 無法適應復雜的工作需求，因此UAV 與配備水槍的云臺緊密結合被應用于高層建筑消防中［3-5］.UAV下懸掛的云臺裝置主要用于控制水或其他滅火材料的噴灑方向.在很多情況下，找到火點后，為了達到良好的滅火效果，水槍噴嘴必須沿著期望軌跡運動，如瞄準一個固定點水平或垂直擺動、圓形或橢圓形擺動等.水槍噴嘴的運動具有兩個自由度，并由兩臺無刷直流電機（Brushless Direct Current Motors，BLDCMs）通過諧波減速器驅動.

軌跡跟蹤任務是工業自動化與控制領域經常遇到的問題.軌跡跟蹤控制一般分為兩種，一種是通過控制策略來實現，主要處理跟蹤中的非線性、不確定性和跟蹤誤差等干擾，目前，比較常見的有魯棒控制［6-7］、模型預測控制（Model Predictive Control，MPC）［8-10］、滑模控制（Sliding Mode Control，SMC）［11-12］和自適應控制［13-15］等智能控制算法.另一種是通過施加物理約束實現，也就是將期望的運動軌跡通過合適的物理結構設計來達到.本文采用的方法是這兩種方法的結合，將運動軌跡需求作為約束對待，但這個約束不是來自環境，而是在機械系統動力學的框架下計算相應的約束力，并通過控制系統施加這個需要的約束力來實現，稱其為約束跟隨理論.

動力學建模與控制是機械系統最傳統的研究方向，在眾多的動力學建模方法中，基于Udwadia-Kalaba 方程［16］的建模方法的提出是近一個世紀以來動力學建模方向非常重要的進展.相比之下，該方法不出現任何輔助變量（如拉格朗日乘數）或偽變量（如廣義速度），可以求得滿足高斯最小原理和達朗貝爾原理的約束解析解，即可以得到封閉形式的解.該優勢可用于建模與控制領域，如最優跟蹤控制［17］、動態建模［18］、家用智能車控制［19］、車輛運動控制［20］、列車虛擬聯掛控制［21］等.

根據這一點，基于Udwadia-Kalaba 方程，將期望軌跡作為約束條件引入約束跟隨控制方法［22-24］，這里的約束不是真實的，而是虛擬的.與傳統的跟蹤控制相似，Chen［25］研究了機械系統穩定與最優設計中的約束跟隨伺服控制.Zhao 等［26］將該方法應用于人工蟻群跟蹤控制.對于具有不確定性的機械系統，Chen 和Zhang［27］提出了一種自適應魯棒近似約束跟隨方法.

本文提出一種基于Udwadia-Kalaba 方程的約束跟隨理論來實現云臺水槍噴嘴的跟蹤控制問題.將消防無人機下固定的云臺裝置驅動的水槍噴嘴運動軌跡作為需要跟隨的約束，利用Udwadia-Kalaba 方程得到實現該跟隨約束所需的實時驅動力.在此基礎上，建立水槍噴嘴運動軌跡跟蹤控制方法.利用Adams 軟件建立云臺系統的物理模型，并在MATLAB/Simulink 環境下編制控制算法，通過聯合仿真驗證了該方法的有效性.同時，針對實際應用中的云臺系統，設計了一種基于數字信號處理器（Digital Signal Processor，DSP）的驅動與控制集成系統，利用兩臺BLDCMs 對系統進行驅動.仿真和實驗驗證了該方法的有效性和實用性.

1 云臺水槍噴嘴驅動結構設計

1.1 水槍噴嘴運動需求

如圖1 所示，安裝在UAV 下的云臺裝置可以驅動水槍噴嘴將水或滅火劑沿正確方向噴灑到著火點.在滅火作業中，高度方向的保持由UAV 自身高度保證，但要求水槍噴嘴必須在一定范圍內擺動，使水盡可能覆蓋著火點.因此，水槍噴嘴驅動機構應滿足相應的運動要求，即水平和垂直運動或兩者組合運動.

圖1 配有云臺的消防UAVFig.1 Firefighting UAV with pan/tilt

采用兩自由度云臺裝置分別實現橫擺和俯仰運動，滿足水槍噴嘴的運動要求.考慮到高層建筑消防的實際需要，橫擺和俯仰的運動范圍分別為±170°和±17°.通過運動組合，可以獲得水槍噴嘴的期望軌跡，從而為實際滅火提供更加復雜的運動跟蹤需求.

1.2 云臺的結構

圖2為云臺結構，云臺通過基座固定在UAV 下，通過橫擺電機和俯仰電機實現水槍兩個自由度的運動.兩臺BLDCMs 帶動相應的諧波減速齒輪，兩個諧波齒輪的減速比都是100.橫擺電機和俯仰電機具有相同的結構和參數，其中電機額定功率均為100 W.此外，由于UAV 的負載限制，云臺必須盡可能地減少其自身的重量.為此，該裝置的主要元件采用PA6材料制造，該材料具有較好的機械強度、耐磨性、耐腐蝕性，且重量較輕.

圖2 云臺結構Fig.2 The pan/tilt structure

2 系統建模

2.1 坐標系建立

描述系統的首要任務就是建立合適的坐標系.針對無人機云臺的結構和運動特點，可以通過機器人技術中常用的Denavit-Hartenberg（D-H）方法來實現，即將該系統視為兩自由度的機械臂，建立的連桿坐標系如圖3 所示.橫擺電機驅動的運動部件稱為橫擺運動部件，俯仰電機驅動的運動部件稱為俯仰運動部件.如前面所述，云臺系統有兩個轉動關節，分別是橫擺關節和俯仰關節.

圖3 云臺坐標系Fig.3 Coordinate systems of pan/tilt

橫擺和俯仰運動部件的質量分別為m1和m2.由此建立了三種坐標系：基坐標系O-X0Y0Z0，橫擺坐標系O-X1Y1Z1、俯仰坐標系O-X2Y2Z2.由于兩個轉動關節的軸線相互垂直并相交，因此，根據經驗選擇模型的三個坐標系的原點重合并固定在俯仰關節的中心位置.軸Z1和軸Z2的方向分別是橫擺關節軸和俯仰關節軸的方向，X0、X1、X2軸方向為水槍噴嘴方向，軸Y0、Y1、Y2的方向是根據右手定則確定的.P是水槍噴嘴的末端，l為線段OP的長度，θ為線段OP與水槍所在直線之間的夾角，且l和θ均為常值.

2.2 運動學方程

云臺的連桿參數如表1所示.

由坐標變換關系，可以得到不同坐標系之間的變換矩陣.基坐標系0到俯仰坐標系2的變換矩陣可以表示為：

式中：s1=sinθ1，s2=sinθ2，c1=cosθ1，c2=cosθ2；θ1是橫擺關節的旋轉角；θ2是俯仰關節的旋轉角.

由圖3 可知，水槍噴嘴末端P相對于坐標系2 即俯仰坐標系的坐標為2P=[lcosθlsinθ0]T，相對于云臺基坐標系的坐標為0P.假設在基坐標系下0P=[xyz]T，那么

因此，由關節變量描述的水槍噴嘴末端P相對于基坐標系的坐標可以表示為：

式中：l為線段OP的長度；cθ=cosθ；sθ=sinθ.

至此完成了云臺系統運動學方程的建立，為后續的伺服約束控制提供數學基礎.由式（3）的逆解可得：

3 機械系統約束跟隨

3.1 受約束的機械系統

考慮一個無約束的機械系統［28］

式中：t∈R是獨立變量時間；q∈Rn是廣義位置坐標；∈Rn 是廣義速 度；∈Rn是廣義加速度；M(q，t) ∈Rn×n是對稱正定慣性矩陣（在某些特殊情況下，其正定性并不總是正確的［29］，本文不考慮這種特殊情況）；Q(q，t) ∈Rn是沿著廣義坐標的廣義外力，包括重力、離心力以及控制輸入等.

假設系統（5）受以下m個約束

式中：φ(?)：Rn×Rn×R→R，φ(?)是光滑的.

需要指出的是，系統（5）所受的約束（6）除了物理約束，也可能是運動軌跡約束或者其他性能要求等虛擬約束.由于初始時刻偏差、數值計算誤差等影響，等式約束（6）有可能不成立，為了使得式（6）在一定的時間內收斂，約束（6）可以等效表示為［30］：

式中：μi為正實數，i=1，2，…，m.

將式（8）代入式（7）得：

將約束（7）用矩陣形式表示為：

約束（6）可能是完整約束或非完整約束.這里，我們指出滿足約束的所有q和都包含在集合q∈Ψ?Rn，∈Ω?Rn中.為了方便，我們將約束（10）表示為：

式 中：A(q，，t) ∈Rm×n被稱為約束矩陣；b(q，，t) ∈Rm是一個m× 1維向量.

式（11）是約束的二階表示形式，是進一步進行動力學分析與控制設計的最優形式.Chen［25］認為各種控制問題，如軌跡跟蹤、穩定性、最優控制等都可以把約束表示成式（11）的形式.對于給定的A和b，如果式（11）至少存在一個解，則稱約束（11）是一致的，本文假設約束（11）是一致的.約束（11）一致的充分必要條件為AA+b=b，其中A+表示A的Moore-Penrose 廣義逆矩陣.約束（11）的一致性假設的含義說明了這些期望約束是符合實際并不是互相矛盾的.

3.2 約束跟隨控制理論

具有約束的機械系統運動問題可以轉化為系統的軌跡跟蹤問題［31］，即在軌跡跟蹤控制問題中，區別于物理約束，可以把期望軌跡當作是一種無形的虛擬約束，如果用伺服控制的方式來實現，稱它為伺服約束.也就是說問題變為，求解滿足軌跡跟蹤控制所需要的控制力，保證系統（5）滿足約束（11），這就是約束跟隨控制問題.

根據牛頓力學，只有力才能改變機械系統的運動狀態.因此，要滿足軌跡跟蹤約束式（11），可以把受約束的機械系統寫為：

式中：Qc(q，，t) ∈Rn是系統（5）滿足約束（11）時所必須施加的伺服約束力.

根據達朗貝爾原理的拉格朗日形式，無約束系統（5）滿足約束（11）時所需要的伺服約束力Qc的解析形式為［16］：

式中：上標“+”表示Moore-Penrose 廣義逆；a=M-1Q表示無約束系統在t時刻的加速度.

聯立式（12）和式（13），得到受約束機械系統的動力學方程為：

方程（14）也被稱為Udwadia-Kalaba 方程，它是進行動力學建模與控制的一種力學理論基礎.顯然，從Udwadia-Kalaba 方程可以封閉解的形式來描述約束力學系統，而不需要引入拉格朗日乘數等偽變量.控制運動的約束力可以通過式（13）得到.因此，Udwadia-Kalaba 方程可以用于伺服控制.將系統運動軌跡作為期望約束（約束跟隨），跟蹤控制的目的是找到合適的約束力來引導系統沿期望軌跡運動.

4 云臺水槍噴嘴軌跡跟蹤控制

4.1 無約束系統運動方程

基于約束跟隨控制理論的框架，無約束系統是指系統沒有考慮控制，即沒有軌跡運動要求的系統.根據圖3，將θ1和θ2作為廣義坐標，分別記為q1和q2.利用拉格朗日方程得到無約束條件下的云臺系統的動力學方程

式中：x1c、y1c表示橫擺運動部件質心位置在坐標系O-X1Y1Z1上的分量；x2c、y2c表示俯仰運動部件質心位置在坐標系O-X2Y2Z2上的分量；I1zz表示橫擺運動部件繞旋轉軸的轉動慣量；I2xx、I2yy分別表示俯仰運動部件繞其相應軸的轉動慣量；s1=sinθ1，s2=sinθ2，c1=cosθ1，c2=cosθ2；θ1是橫擺關節的旋轉角，θ2是俯仰關節的旋轉角；m1、m2分別表示橫擺和俯仰運動部件的質量；g表示重力加速度.

4.2 云臺運動約束條件

對于無約束云臺系統（15），要求其噴嘴實現期望的運動軌跡，該系統成為一個有約束機械系統.如前述，為實現這個期望約束，應增加額外的伺服約束力.這里將云臺水槍噴嘴末端的運動軌跡作為需要施加的約束條件.

由于云臺系統只有橫擺和俯仰2 個自由度的運動，故目標軌跡只對y、z方向上的位置有約束，對x方向并無要求.假設選取云臺系統的水槍噴嘴末端目標運動軌跡為y-z平面上半徑為0.06 m、圓心為（0，-0.09）的圓.若選取關節角速度為2 rad/s，則采用下列方程組描述云臺系統的軌跡約束方程：

根據以上討論，結合式（3），軌跡約束為：

式（17）對時間t求一階和二階導數，約束方程可以寫成式（11）的形式，其相關項A、b分別為：

4.3 伺服約束力求解

如圖4所示是求解伺服約束力的過程.針對無約束系統，如前所述，首先給出一組期望軌跡（約束方程）.為了解決初始誤差和計算誤差等原因可能產生的偏差，對約束采用漸進收斂（穩定）處理，并把它寫為式（11）的形式.為實現期望的運動軌跡（虛擬約束）要求，需要在無約束系統上增加額外的力或力矩，即伺服約束力，使系統沿期望軌跡（約束）運動.在此基礎上，根據式（13）計算伺服約束力的解析解，顯然這個伺服約束力隨著系統狀態的變化而在不斷變化.

圖4 伺服約束力求解Fig.4 Solution of servo constraint force

為了滿足水槍噴嘴運動軌跡的要求，即需要的約束條件，根據式（13）可以得到施加在電機上的控制力矩：

式中：τ=τ1、τ2分別表示橫擺關節和俯仰關節處所需要施加的伺服力矩.

5 軌跡跟蹤控制仿真

5.1 云臺系統物理模型

仿真的目的是驗證提出的約束跟隨理論對水槍噴嘴進行跟蹤控制的有效性.在式（18）得到的力矩下，水槍噴嘴末端的運動軌跡將是期望軌跡.此外，在進行物理實驗之前，從安全性和成本的角度對所提出的方法進行模擬也是有必要的.

由圖4 可知，作為控制對象，需要得到云臺系統的物理模型，才能完成仿真.采用 SolidWorks 軟件建立云臺系統三維模型并將其導入Adams 環境中，如圖5 所示.利用 Adams 模型還可以得到其位移、速度和加速度等物理參數.

圖5 云臺的Adams模型Fig.5 Adams model of the pan/tilt

5.2 約束跟隨的穩定性處理

約束跟隨理論基于系統模型和初始條件.根據系統模型和運動狀態，連續計算引導水槍噴嘴沿期望軌跡運動的力矩（18）.由于數字誤差、建模偏差、初始條件不相容、外界干擾等原因，跟蹤誤差不可避免.為了穩定約束跟隨，采用Baumgarte 約束違約穩定法［32-33］.也就是說，在加速度約束方程中加入位移和速度約束，將不穩定的二階約束方程=0 變為穩定的約束方程

式中：α，β是正常數，分別作為二階阻尼系統的阻尼系數和剛度.隨著時間的推移，Ψ中的約束將收斂到零并滿足式（11）.

實際上，考慮約束（11），在這種情況下，相比于不穩定的方法，在穩定過程中除了b外，沒有改變系統參數來處理跟蹤控制，即穩定后，A與前面相同，而b不同，用b'代替.

5.3 相容初始條件下的仿真

在初始時刻，如果期望軌跡中給定的初始位置和速度與系統實際是一致的，則稱為相容初始條件.在這種情況下，初始點剛好在期望軌跡上.

根據2.2節的運動學逆解，取相容初始條件為：

將Adams 模型導入Matlab/Simulink 仿真環境中，在MATLAB/Simulink 下進行仿真.在Matlab 中設置相關參數，并將初始條件（20）和控制（18）應用到從Adams 導入的云臺模型上，仿真時間為10 s.水槍噴嘴末端期望軌跡在相容初始條件下的仿真結果如圖6和圖7所示.

圖6 相容初始條件下的跟蹤控制Fig.6 Tracking control with consistent initial conditions

圖7 相容初始條件下y-z平面上的運動軌跡Fig.7 Motion trajectory in y-z plane with consistent initial conditions

圖6（a）、圖6（b）分別為在橫擺和俯仰關節施加的力矩，圖6（c）、圖6（d）分別為橫擺和俯仰關節的角度位置，圖6（e）、圖6（f）分別為水槍噴嘴末端在y、z方向上的誤差，圖7為相容初始條件下的運動軌跡.

由圖6 和圖7 可以看出，云臺水槍噴嘴在y-z平面上的軌跡為半徑為0.06 m、圓心為（0，-0.09）的圓，與預期一致.但是從圖6（e）和圖6（f）可以看出，誤差范圍在0～0.8 s.產生這種誤差的原因是建模偏差，由于采用了上述的穩定方法，誤差會隨著時間的推移收斂到零.

5.4 不相容初始條件下的仿真

基于Udwadia-Kalaba 方程的約束跟隨控制方法不適用于初始條件不相容的情況，即系統的初始位置或初始速度不在期望軌跡上，與系統不匹配.用Baumgarte 約束違約穩定法來修正這個不相容引起的偏差.

為了說明穩定的適用性，假設給定不相容初始條件為：

仿真時間為10 s，該工況的跟蹤控制仿真結果如圖8、圖9所示.

圖8 不相容初始條件下的跟蹤控制Fig.8 Tracking control with inconsistent initial conditions

圖9 不相容初始條件下y-z平面上的運動軌跡Fig.9 Motion trajectory in y-z plane with inconsistent initial conditions

圖8（a）和圖8（b）分別表示了橫擺和俯仰關節的力矩，圖8（c）和圖8（d）分別表示了橫擺和俯仰關節的角度位置，圖8（e）和圖8（f）分別表示了水槍噴嘴末端在y和z方向上的誤差，圖9 為不相容初始條件下的運動軌跡.

由于初始條件不相容，即初始位置和速度不在期望軌跡內，初始階段的誤差相對較大.但在約束違約穩定下，跟蹤誤差逐漸減小并趨近于零，即跟蹤控制是有效的.

6 實 驗

6.1 實驗目的

仿真結果表明，本文提出的約束跟隨控制方法用于由云臺裝置驅動的水槍噴嘴運動軌跡跟蹤控制具有很好的效果.在仿真中，采用了基于Adams 的系統模型和基于Matlab/Simulink 的仿真環境.為了進一步驗證約束跟隨理論的有效性，需要在實際的物理系統和控制器上進行實驗.為了與仿真有更好的對比，水槍噴嘴的期望軌跡采用和仿真同樣的要求［式（16）］.

6.2 實驗設備

搭建以TI TMS320F28069 DSP 控制器為核心的實驗平臺，通過兩臺無刷直流電機驅動云臺裝置，實驗所需的控制結構如圖10 所示.無刷直流電機具有成本低的優點，但是位置控制精度較低.在此實驗中，電機轉角參數采用嵌入電機中的霍爾位置傳感器來得到，并通過微分得到角速度.實驗中采用了電機控制算法的磁場定向控制（Field Oriented Control，FOC）.

圖10 實驗結構Fig.10 Experiment structure

根據式（18）對算法進行編程，在PC 上結合Matlab/Simulink 和C 語言，通過自動代碼生成技術將控制算法編譯并加載到DSP 控制器中.需要指出的是，我們使用了上面介紹的穩定方法，即在式（18）用b'代替b.

控制器輸出的驅動電機的電流與所需轉矩成正比，由兩臺電機驅動云臺使得水槍噴嘴運動，實驗設備如圖11所示.

圖11 實驗設備Fig.11 Experiment equipment

實驗結果及與仿真結果的對比如圖12 所示，分別為水槍噴嘴的橫擺角、俯仰角和水槍噴嘴末端的軌跡，實驗結果與仿真結果的吻合度較好.

圖12 水槍噴嘴末端跟蹤軌跡的比較Fig.12 Tracking trajectory comparison of water-jet nozzle end point

水槍噴嘴末端在y方向和z方向上實驗軌跡與期望軌跡的誤差如圖13所示.

水槍噴嘴末端跟蹤軌跡與預期基本吻合，但仍存在一定誤差，造成這種誤差的原因主要是：

1）約束跟隨跟蹤控制依賴于機械系統的建模精度，不可避免地存在參數誤差，如質量、質心、慣性矩等，從而導致建模偏差.

2）角度測量誤差.角位置和角速度在控制轉矩計算中非常重要，在云臺控制系統中，由于成本原因，沒有使用編碼器，由BLDCMs 所帶的霍爾傳感器來測量關節角度.根據BLDCMs 的特點可知，該傳感器的輸出信號存在一個扇區誤差（60°電角度），使得實驗結果存在一定的偏差.

針對消防無人機的實際需求，從實驗結果可以看出，水槍噴嘴運動誤差在允許范圍內，所采用的控制方法對云臺噴嘴運動軌跡的跟蹤控制是可行的.

7 結論

本文提出一種約束跟隨方法研究了無人機下固定的驅動水槍噴嘴運動的云臺跟蹤控制問題.該控制理論基于Udwadia-Kalaba 方程，約束跟隨方法沒有對被控對象進行線性化處理，也沒有引入偽變量或其他算子來得到控制，該方法可以直接得到控制的封閉解析解.針對消防無人機水槍噴嘴運動控制的要求，以水槍噴嘴的期望運動軌跡為約束條件，計算水槍噴嘴驅動電機所需的力矩.該方法所得到的力矩為封閉形式，這是約束跟隨控制的優點之一.仿真和實驗結果表明，采用基于約束跟隨的跟蹤方法對消防無人機云臺裝置進行運動控制是有效的.在實際應用過程中，由于外部擾動以及建模誤差等影響，很難獲得無人機云臺的準確模型，所以無人機云臺系統存在不確定性且是未知的.因此，在設計控制輸入時考慮無人機云臺系統的不確定性需要在今后的工作中完成.