無人直升機基于視覺的靜止點目標跟蹤
2009-01-01 00:00:00范才智劉云輝吳亮宋寶泉
計算機應用研究 2009年2期
（國防科學技術大學 電子科學與工程學院，長沙 410073）
摘 要：針對帶有攝像機的直升機進行靜止點目標跟蹤的情況，提出了一種層級控制器。該控制器共有三個回路：內回路采用四個獨立的PD控制器控制直升機的高度和姿態；中間回路利用兩個Mamdani型模糊控制器控制直升機的位置；外回路利用視覺反饋獲得直升機下一步的期望位置，其不需要已知攝像機的內參數和平移外參數以及目標點的坐標，只需已知粗略標定的旋轉外參數。仿真結果表明了該控制器的可行性。
關鍵詞：無人直升機；PD控制器；模糊控制器；目標跟蹤
中圖分類號：TP24 文獻標志碼：A
文章編號：10013695(2009)02049204
Visionbased static point target tracking of unmanned helicopter
FAN Caizhi，LIU Yunhui，WU Liang，SONG Baoquan
(College of Electric Science Engineering， National University of Defense Technology， Changsha 410073， China)
Abstract:An cascade controller for unmanned helicopter on which a camera was mounted static point target tracking was presented. It was constituted by three loops. The inner loop controled altitude and attitude using four PD controllers， and the medial loop controled lateral and longitudinal translation using two Mamdani type fuzzy controllers， and the outer loop attained the next desire location of helicopter by using visual feedback. When computing the desire location only the camera extrinsic rotation parameters， which were calibrated roughly，were needed， the other parameters such as camera intrinsic parameters， extrinsic translation parameters and the 3D coordinates of the feature needn’t know. The effectiveness of the proposed controller is verified by simulation results.
Key words:unmanned helicopter; PD controller; fuzzy controller; target tracking
無人直升機具有任意方向機動飛行、懸停和垂直起降等特點，因此在軍事、民用以及學術研究方面都有很大的應用前景[1，2]。
計算機視覺對直升機感知周圍的環境非常有用，在實現無人直升機導航、路徑規劃和目標跟蹤方面可以彌補GPS在某些區域不能使用和精度較低的不足。利用視覺引導直升機的自動著陸是目前研究的一個熱點，著陸的一個重要環節就是著陸目標的跟蹤，加州大學伯克利分校[3~6]和南加州大學[7，8]在這方面的做了很多的工作。南加州大學在移動目標上的著陸上也進行了一些研究[9]。Hamel和Mahony在無人直升機視覺伺服方面利用剛體運動的無源化特性提出了一種新穎的基于圖像的方法[10，11]，在文獻[12，13]中，利用帶有攝像機的無人直升機實現了對市內環境下房屋窗戶和墻上標記的自動跟蹤。
為了簡化問題分析，只討論點特征目標跟蹤，而暫不考慮其他形狀特征的目標跟蹤。目前大多數目標跟蹤的研究工作中，有的需要對攝像機的內外參數進行標定或者對攝像機的安裝角有嚴格的要求；有的需要已知目標點的三維位置坐標。針對靜止目標的跟蹤，提出了一種層級控制算法，其與目前已有的工作主要有兩個不同之處：一是算法中只需要用到粗略標定的旋轉外參數，而不需要平移外參數和攝像機的內參數以及目標點的位置坐標，在保證目標能在可視范圍內的前提下任意安裝攝像機即可；二是將直升機的控制和視覺反饋完全分開來考慮，即先控制好直升機，待直升機穩定后，由視覺反饋提供直升機下一步的期望位置，然后再控制直升機移動到該期望位置，依此類推，直到目標點成像在期望點。
1 直升機動力學模型
直升機可在空間六自由度方向上產生運動，即三個平移方向和三個轉動方向。如圖 1 所示，慣性坐標系ODXDYDZD滿足右手法則，YD垂直指向上。在直升機質心處建立機體坐標系OXYZ，X軸沿著機體前向方向，指向前為正，與XDZD的夾角為俯仰角θ，抬頭為正，與XDYD的夾角為偏航角ψ，向左偏轉為正；Y軸垂直X向上，XY為機體縱向對稱面，該面與YD的夾角為側傾角γ，向右傾斜為正；Z軸按右手法則確定。T為旋翼拉力，垂直槳盤平面，Q為旋翼旋轉引起的反扭矩，與旋翼的旋轉方向相反，下標“M”代表主槳，下標“T”代表尾槳，a1為主槳槳盤縱向傾斜，向后傾斜為正，b1為主槳槳盤橫向傾斜，向右傾斜為正；G表示地面ODXDZD中被跟蹤的目標點。
在機體坐標系里表示的剛體運動的牛頓—歐拉方程[14]為
mE 00I
+ω×mvω×I ω=fτ(1)
其中：速度v=(vx，vy，vz)T；角速度ω=(ωx，ωy，ωz)T；m為直升機質量；E為3×3的單位矩陣；I為在機體坐標系里表示的慣量矩陣。
外力和力矩在機體坐標系里的表示[15]分別如下：
f=TMxTMyTMz+TT+RT0-mg0
τ=RMMMNM+NT+TMzrOMy-TMyrOMz+TTrOTy
TMxrOMz-TMzrOMx+TTrOTx
TMyrOMx-TMxrOMy
-Kg0ωy0(2)
其中：力矩最后一項是由電子陀螺引入的阻尼，將其建模為簡單線性模型[16]；Kg為常系數。其他參數求法如下：
TMx=-TM sin a1 RM=Kβb1
TMy=TM cos a1 cos b1 MM=QM
TMz=TM sin b1 NM=Kβa1
NT=-QT(3)
根據文獻[17] ，QM和QT的利用如下近似公式計算，CQ和DQ為常系數。
QM=CMQTM1.5+DMQ
QT=CTQTT1.5+DTQ(4)
R為機體坐標系到慣性坐標系的轉換矩陣，其為正交矩陣，具體表達如下：
R=c θ c ψ-c γ s θc ψ+s γ s ψs γ s θ c ψ+c γ sψ
s θc γ cθ-s γ cθ
-c θ s ψc γ s θ s ψ-s γ s θ s ψ+c γ c ψ(5)
其中s θ和c θ分別是sin θ和cos θ的縮寫。
把剛體運動的牛頓—歐拉方程重寫為
P#8226;DR#8226;=
vDRf/mRsk(ω)I-1(τ-sk(ω)Iω)(6)
其中：P=(x， y， z)T是在慣性坐標系中表示的直升機位置；下標D表示在慣性坐標系中的向量；sk:R3→R3×3為向量到斜對稱矩陣的映射，具體計算為sk(v)w=v×w。
2 控制器結構
控制器采用層級控制的思想，控制器結構如圖 2 所示。內回路的高度/姿態控制器采用PD控制器對直升機的高度和姿態進行控制，中間回路采用模糊控制器對直升機的水平位置進行控制[18]，PD控制器和模糊控制器都具有結構簡單、魯棒性好的優點。但是由于狀態之間的耦合導致參數調節有一定的難度，參數調節時應該先將外回路的PD控制器調節好，讓高度/姿態控制器正常工作，然后再調節中間回路的模糊控制器的參數。外回路利用視覺信息獲得直升機的下一步運動的期望位置，以實現靜止目標跟蹤。其中上標d表示變量的期望值。
2.1 高度/姿態控制器
在高度/姿態控制器中，一個很重要的問題是配平點的計算，令式（7）中的總外力f和總力矩τ等于零，然后解非線性方程組，可得到配平點參數。忽略各個通道之間的耦合，各通道分別采用PD控制器進行控制，其中高度y是直升機在慣性坐標系里的垂直位置，總共有四個獨立的PD控制器，一個用來控制直升機的高度y，另外三個分別用來控制姿態角θ、γ和ψ，需要注意的是機構的最大行程，所以對每個輸入需要有飽和限位器。當未飽和時如式（7）計算，當飽和時，輸入分別為+TMmax或-TMmax，+a1max或-a1max，+TTmax或-TTmax，+b1max或-b1max來計算。
TMa1TTb1=
TMtrim-KPTMΔy-KDTMvy
a1trim-KPa1Δθ-KDa1ωz
TTtrim-KPTTΔψ-KDTTωy
b1trim-KPb1Δγ-KDb1ωx(7)
其中：上標trim表示配平點參數；K為PD控制器的系數；下標P表示比例項系數；下標D表示微分項系數。
2.2 位置控制器
采用Mamdani型模糊控制器來對直升機的x和z方向的位置進行控制[18，19]，其反映了遙控操作者的經驗。控制器的隸屬度函數如圖 3 所示。
Mamdani模糊控制器在x和z方向分別有九個推理規則， ED表示在慣性坐標系中直升機位置的偏差，EC表示在慣性坐標系中直升機的速度，Y表示輸出期望的姿態角偏差。例如x方向，ED=x-xd，EC=vx，Y=Δθ，θd=θtrim+Δθ，一個推理規則的例子如下：
如果ED為N且 EC為N，則Y為 N。
2.3 視覺反饋生成期望位置
如圖4所示，M為攝像機圖像平面，N為地平面，直線n是面M和面N的交線，點O1是圖像平面中心點，點O2是攝像機光軸與地平面P的交點，XcYcO2是滿足右手法則的攝像機坐標系，直線Xc1通過點O1而且與線n平行，直線Yc1通過光心O1而且與線Xc1垂直，面N中的線X、Y、X1和Y1的在面M中的成像分別對應Xc、Yc、Xc1和Yc1，面N由X、Y分為四個象限P1、P2、P3和P4，其分別對應圖像平面M中的象限Q1、Q2、Q3和Q4，目標點G點是N中的點，點C是目標點G在圖像平面中的成像點，點D為Yc1和線n的交點，點A為線O1C、線O2G和線n的交點，線AB為線AO1在面N中的投影，線DO2為O1O2在面N中的投影， AB和AO2的夾角為α，AD和AB的夾角為β，O2D和O2O1的夾角為λ。
目標是對目標點G進行跟蹤，即移動攝像機讓G點的像點C位于期望的位置，比如讓C點位于圖像平面的中點O1，在地面N中表現為將點O2移動到G點，因為攝像機和直升機之間是固連的，而且兩者坐標系之間的旋轉矩陣Rc是已知的，所以移動攝像機和移動直升機是等價的。
移動的策略是：
a）當攝像機光軸與地面之間的夾角λ≥π/4 rad時，采取等高度運動來跟蹤目標；如果λ＜π/4 rad時，采取X坐標恒定的運動來跟蹤目標。
b）任意給定初始步長K0，將攝像機沿方向等高度移動距離K，然后直升機姿態穩定后，假設此時G點在圖像平面M中的成像點為C′，判斷成像點C′和上一步成像點C是否位于同一個象限內，如果還處于同一個象限，則步長保持不變；如果不在同一個象限內，則步長K1變為K0的一半，即K1=K0/2，下一步沿O1C’在面N內的投影方向等高度移動K1，后面的運動規律依此類推。
攝像機坐標系相對直升機坐標系的旋轉矩陣Rc可表示為歐拉角x、y、z的函數。可以證明，其各個方向的角度x、y、z的最大誤差容限有0.192 rad（約11°），即標定誤差滿足在最大的誤差容限內時，根據上面的策略運動就能實現目標跟蹤，而這個條件非常寬松，很容易滿足。限于篇幅，這里略去證明過程。
3 仿真結果
3.1 高度/姿態控制器和位置控制器的性能
在仿真實例中，所求得的配平參數為：TMtrim=81.806 6 N， a1trim=0 rad，TTtrim=-2.272 4 N， b1trim=0.003 9 rad ， θtrim=0， γtrim=0.023 9 rad。直升機質心在世界坐標系中的初始位置為（13，45，-50），將其移動到（15，35，-45）。圖 5 是移動過程中的速度、角速度和姿態角的變化曲線，可以看出，直升機最終能保持穩定狀態。圖 6 是移動過程中位移變化曲線，可以看出，直升機能準確地運動到指定的位置。
3.2 Rc不存在標定誤差時的跟蹤性能
假設當Rc不存在標定誤差時，在仿真中取Rc=[0 0.707 10.707 1;00.707 1 -0.7071;-100]，直升機在慣性坐標系中的初始位置為Xi=（13 45-50），目標點的位置為XG=（50 0 -50）。圖7是在目標跟蹤過程中，目標在圖像平面中的像點和期望點之間的像素偏差，從圖中可以看出，最終目標點成像在期望的位置上。圖8是跟蹤過程中速度、角速度和姿態角的變化曲線，中間的波動起伏是因為每次直升機穩定下來后再通過圖像誤差計算下一點期望位置，又要重新破壞平衡來移動直升機所導致的。圖9是移動過程中直升機的位置變化曲線，在本仿真中采取的是等高移動，可以看出y方向位移變化很小。圖10是移動過程中直升機位置在xz方向的變化。其中圓點是直升機的起始位置，方點是直升機的最終穩定的位置，實線和虛線表示相鄰的步數，可以看出經過6步以后，實現了目標跟蹤。
3.3 Rc存在標定誤差時的跟蹤性能
圖11~14是當Rc存在標定誤差時的目標跟蹤性能，在仿真中取存在標定誤差的Rce=[0 0.331 4 0.191 3;0 0.5-0.866 0;-0.382 7 0 0]，其他參數與3.2節的仿真相同。從圖11可以看出，目標點的成像點仍然能趨向期望位置，即仍然能實現目標跟蹤。圖12是跟蹤過程中的速度、角速度和姿態角的變化曲線，可以看出，直升機最后保持穩定。圖13是跟蹤過程中的直升機位置的變化曲線，由于采取的是等高運動，也可看出，y方向位置基本上保持恒定。圖14是跟蹤過程中xz方向的位置變化，圓點是直升機起始位置，方點是最終位置，可以看出，起始點和最終點的位置與圖10中是一致的，不同的是最終實現目標跟蹤時的步數增多，也就是需要的時間變長了，這正是因為Rc的存在誤差所導致的。
4 結束語
針對帶有攝像機的無人直升機的目標跟蹤提出了一種層級控制算法，利用PD控制器和模糊控制器分別來控制直升機的高度/姿態和水平位置，利用視覺反饋來計算直升機下一步運動的期望位置。通過仿真結果可以看出，直升機可成功地實現對點目標的跟蹤。視覺反饋中無須知道攝像機的平移外參數、內參數和目標點的三維坐標，只需要粗略標定后的旋轉外參數，這種視覺反饋的方法，不只可以應用于直升機的基于視覺的目標跟蹤，對于任何可在三維空間中自由運動的帶有攝像機的載體，如機械手等，利用攝像機對靜止點目標進行跟蹤的情形同樣適用。
參考文獻：
［1］
BORTOFF S A.The university of toronto RC helicopter:a test bed for nonlinear control[C]//Proc of IEEE International Conference on Control Applications. Hawaii:[s.n.]，1999:333338.
[2]GAVEILETS V A，SHTERENBERG M A，FERON D E.Avionics system for a small unmanned helicopter performing aggressive maneuvers[C]//Proc of the 19th Digital Avionics Systems Conferences. Philadelphia:[s.n.]， 2000:1E2/11E2/7.
[3]SHARP C S，SHAKERNIA O，SASTRY S S.A vision system for lan-ding an unmanned aerial vehicle [C]//Proc of IEEE International Conference on Robotics and Automation. Seoul， Korea:[s.n.]， 2001:17201727.
[4]SHAKERNIA O，MA Yi，KOO T J，et al.Vision guided landing of an unmanned air vehicle[C]//Proc of the 38th Conference on Decision Control. Phoenix， Arizona:[s.n.]，1999:41434148.
[5]SHAKERNIA O，VIDAL R，SHARP C S，et al.Multiple view estimation and control for landing an unmanned aerial vehicle [C]//Proc of IEEE Conference on Robotics and Automation. Washington DC:[s.n.].2002:27932798.
[6]SHAKERNIA O，MA Yi，KOO T J，et al.Landing an unmanned air vehicle: vision based motion estimation and nonlinear control[J].Asian Journal of Control，1999，1(3):128145.
[7]SARIPALLI S，MONTGOMERY J F，SUKHATME G S.Visually guided landing of an unmanned aerial vehicle[J].IEEE Trans on Robotics and Automation，2003，19(3):371380.
[8]SARIPALLI S，MONTGOMERY J F，SUKHATME G S.Vision based autonomous landing of an unmanned aerial vehicle[C]//Proc of IEEE International Conference on Robotics and Automation. Washington DC:[s.n.]， 2002:27992804.
[9]SARIPALLI S，SUKHATME G S.Landing on a moving target using an autonomous helicopter[C]//Proc of International Conference on Field and Service Robotics. Mt Fuji:[s.n.]，2003:277286.
[10] HAMEL T，MAHONY R.Visual servoing of an under actuated dyna-mic rigid body system: an image based approach[J].IEEE Trans on Robotics and Automation，2002，18(2):187198.
[11]CHRIETTE A，HAMEL T，MAHONY R.Visual servoing for a scale model autonomous helicopter[C]//Proc of IEEE International Confe-rence on Robotics and Automation. Seoul， Korea:[s.n.]，2001:2126.
[12]MEJIAS L，CAMPOY P，SARIPALLI S，et al.A visual servoing approach for tracking features in urban areas using an autonomous helicopter[C]//Proc of IEEE International Conference on Robotics and Automation. Orlando， Florida:[s.n.]， 2006:25032508.
[13]MEJIAS L，SARIPALLI S，SUKHATME G S，et al.Detection and tracking of external features in an urban environment using an autonomous helicopter[C]//Proc ofIEEE International Conference on Robotics and Automation. Barcelona， Spain:[s.n.]，2005:39833988.
[14]高正，陳仁良.直升機飛行動力學[M]．北京：科學出版社，2003.
[15]HAMEL T，MAHONY R，DZUL A.Hover control via lyapunov control for an autonomous model helicopter[C]//Proc of the 38th IEEE Conference on Decision and Control. Phoenix， Arizona:[s.n.]，1999:34903495.
[16]KIM S K，TILBURY D M.Mathematical modeling and experimental identification of an unmanned helicopter robot with flybar dynamics[J].Journal of Robotic System，2004，21(3):95116.
[17]KOO T J，SASTRY S.Output tracking control design of a helicopter model based on approximate linearization[C]//Proc of the 37th IEEE Conference on Decision and Control.Tampa， Florida:[s.n.]，1998:36353640.
[18]SANCHEZ E N，BECERRAH M，VELEZ C M.Combining fuzzy and PID control for an unmanned helicopter[C]//Proc of Annual Meeting of the North American Fuzzy Information Processing Society.2005:235240.
[19]王立新.自適應模糊系統與控制教程[M]．王迎軍，譯．北京：清華大學出版社，2003.
