基于運動捕捉系統的多旋翼飛行器實時測姿算法

石　鵬，賴際舟，呂　品，包　勝

(南京航空航天大學自動化學院，南京 211106)

0　引言

旋翼飛行器具有體積小、質量小且能耗低的特點，能夠較好地在室內無GPS(Global Positioning System)環境中完成搜索、環境監測和救援等任務[1]，是目前無人機領域的研究熱點之一。

運動捕捉系統(Motion Capture System，MCS)在室內指定的區域內可以完成對標記點的捕捉，從而得到標記點的高精度位置信息[2]。將特定光點固定在旋翼飛行器上，根據固定點的位置信息能夠得到該飛行器的位置、速度和姿態信息。MCS是旋翼飛行器室內飛行的重要導航手段，可以為旋翼飛行器室內飛行提供高精度參考基準[3-6]，近年來受到了廣泛關注。目前,MCS測姿通常采用的三標記點方案對標記點的安裝有較為嚴格的幾何空間約束，要求利用3個標記點描述出機體系的兩個正交軸來建立機體系，導致標記點安裝誤差會引起較大的測姿誤差。對此，本文提出了一種MCS高精度五點測姿算法，該算法可以有效地減小標記點的安裝誤差給測姿精度帶來的影響，測姿精度高，能夠為旋翼飛行器室內動態飛行時的姿態測量提供實時參考基準。

1　MCS傳統三點測姿算法缺陷分析

MCS傳統三點測姿算法利用3個標記點安裝成直角三角形，以直角邊為坐標軸建立機體系，從空間幾何角度出發，根據線與線或線與面的夾角關系求解姿態角。

如圖1所示，3個標記點(黑色實心點)確定的平面(p0)為機體橫截面，且3點構成直角三角形，兩條直角邊記為L1和L2，導航坐標系(n系)的Xn軸、Yn軸和Zn軸兩兩構成的平面分別記為p1、p2和p3。該算法中以L1為機體縱軸，由L1與p1的夾角求俯仰角，由L1在p1上的投影與p3的夾角求航向角，由p0的法向量與p3法向量的夾角求橫滾角。

該算法不僅要求3個標記點滿足直角三角形的安裝約束，而且要求兩條直角邊(記為L1和L2)分別與真實機體系的X軸、Y軸方向一致；然而標記點的安裝常由人工完成，難以保證安裝精度。

因此，三點測姿算法在旋翼飛行器上應用時會存在以下不足：當標記點的安裝存在誤差時，L1與機體縱軸位置存在偏差，且p0與機體橫截面不重合，從而導致從測姿原理上產生較大測姿誤差。

2　MCS五點測姿算法設計與分析

針對MCS中三點測姿算法測姿誤差受標記點安裝誤差影響大的問題，本文設計了一種五點測姿算法。本節將對五點測姿算法流程進行闡述，并且對兩種測姿算法的測姿誤差進行對比分析。

2.1　五點測姿算法設計

本文中的四旋翼飛行器測姿算法一共采用5個標記點安裝在飛行器上，如圖2所示。其中，A、B、C、D4點分別代表安裝在機臂上的4個標記點，用來求解飛行器姿態。AB和CD中點的連線平行機頭方向，第5個標記點G為辨識點，位于四邊形ABCD內，用于辨識前4個標記點，其距離約束關系為AG

五點法標記點的安裝沒有角度約束要求，直接固定在四旋翼飛行器4個支架上，易于實現。采用旋轉矢量法對載體姿態進行求解[7-9]，其算法流程如圖3所示，原理示意圖如圖4所示。

由于MCS下捕捉標記點得到的位置信息順序是隨機的，因此該算法中首先根據標記點的位置關系得到辨識點G信息，然后利用G與其他4點的距離關系辨識出A、B、C、D4點。

四旋翼飛行器的飛行動力由螺旋槳提供，通過A、B、C、D4點位置對旋翼所在位置進行全局平面擬合(S1)，并由S1作為機體橫截面建立機體坐標系，可以較好地與四旋翼飛行器的運動特性相對應[10]，這是三點法所不具備的特點之一。

導航坐標系(n)為運動捕捉系統坐標系，Yn軸指向北，以擬合的平面S1為機體系的X-O-Y平面建立機體坐標系(b系)。本文中提到的機體系為右前上坐標系，記點H為AB中點，O點為四邊形ABCD的中心。記O′、H′、A′和B′分別為O、H、A和B投影到平面S1的投影點，以O′點為b系原點Ob，O′點和A′B′中點(點H′)確定的射線為Yb軸。記ti、tj、tk分別為Xb軸、Yb軸、Zb軸上方向為正方向的單位向量，Yb軸對應機體的機頭方向。則：

(1)

根據向量O′A′和O′B′叉乘可以求得X-O-Y平面的法向量，作為Zb軸：

(2)

由Yb軸和Zb軸的關系可以建立Xb軸：

ti=tj×tk

(3)

(4)

(5)

2.2　MCS兩種測姿算法對比

五點測姿算法和三點測姿算法的測姿精度取決于其建立的機體系的準確度，因此從兩種測姿算法確定的機體系與真實機體系的偏差來分析測姿誤差大小。

圖5中，b0系為真實機體系。三點法中，由B、C、D3點確定機體系b1求姿態角，L1為Yb1軸，L2為Xb1軸；五點法中，由A、B、C、D4個標記點確定機體系b2求姿態角。

三點法中，理想情況為L1和L2分別平行于Yb0軸和Xb0軸。實際情況中，安裝誤差可以描述為：L1在Xb0軸和Zb0軸的投影長度為Δl1，L2在Yb0軸和Zb0軸的投影長度為Δl1。b0系繞Zb0反向轉動Δψi，再繞Xb0正向轉動Δθi，再繞Yb0正向轉動Δγi得到bi系(i=1,2)，有：

(6)

(7)

(8)

通過擬合四旋翼所在平面和4個標記點確定Xb0軸方向的方法，五點法在確定機體系的過程中能夠補償部分標記點安裝誤差帶來的誤差。考慮到對比的直觀性，在式(6)和式(7)中，五點法在三軸方向上的安裝誤差等價于Δl2(Δl2<Δl1)。

(9)

(10)

由此看出，本文提出的五點測姿算法可以降低安裝標記點的幾何空間約束，并減小安裝誤差對測姿誤差的影響，因此五點法的測姿誤差比三點法小。

3　算法驗證

為驗證提出的五點測姿法的測姿精度，將標記點安裝在三軸小型機械轉臺的轉臺平面上，通過轉臺平面(相當于機體系X-O-Y平面)的變化模擬四旋翼飛行器姿態角變化。實驗分為實驗1和實驗2，實驗中同時改變轉臺轉動的3個姿態角，將三點法與五點法的測姿結果與轉臺轉動角度進行對比，并分析其均方根誤差(RMSE)。實驗1中，主要針對標記點的安裝在與轉臺平面平行方向上存在誤差時，對比兩種算法的測姿精度。實驗2相對于實驗1，不僅考慮了標記點在與轉臺平面平行方向上的安裝誤差，還考慮了與轉臺平面的垂直方向上的安裝誤差。

實驗1中，標記點直接粘貼在轉臺平面上，所有標記點共面，且其所在平面與轉臺平面平行。由表1和圖6可知，將兩種測姿算法結果與轉臺對比，其俯仰角和橫滾角偏差相近，測姿精度相當；比較其航向角偏差，三點法大于五點法；比較其RMSE，五點法優于三點法。因此，當標記點安裝誤差主要存在于與轉臺平面平行方向上時，五點法優于三點法，主要體現于航向角的測量精度。

表1　實驗1測姿均方根誤差表

實驗2中，給一個標記點(相對位置與圖5中點C相同)在與轉臺平面的垂直方向上引入5mm的安裝誤差。由表2和圖7可知，通過比較其姿態角偏差和RMSE得出結論，當標記點安裝在轉臺系(機體系)三軸方向上都存在誤差時，五點法的3個姿態角測量精度都明顯優于三點法。

表2　實驗2測姿均方根誤差表

實驗1和實驗2的結果表明，針對實際應用時，標記點在機體系三軸方向上都存在安裝誤差，較三點測姿算法，五點測姿算法能夠通過補償標記點安裝誤差來減小測姿誤差，其測姿RMSE值小于0.2°,保證了測姿精度，測姿效果更好。

此外，為進一步驗證五點測姿法在多旋翼飛行器中的應用效果，進行了基于該方法的四旋翼飛行器航跡飛行實驗。圖8和圖9為其飛行航跡圖，圖10為利用五點測姿算法對其實時動態測姿圖。結果表明，應用五點測姿算法，能夠較好地完成旋翼飛行器實時動態測姿任務。

4　結論

本文提出的基于運動捕捉系統的多旋翼飛行器室內五點測姿算法相對于MCS中常用的三點測姿算法，減小了測姿誤差，且易于實現。五點測姿算法利用四點位置對旋翼所在位置進行全局平面擬合，較好地與其結構特性相對應，并通過擬合平面建立機體系，能夠有效地降低由標記點的安裝誤差產生的測姿誤差。室內實驗結果表明，本文提出的五點測姿算法的3個姿態角RMSE值均小于0.2°,較三點測姿算法，減小了標記點安裝誤差產生的測姿誤差，實際應用時該算法測姿精度更高。四旋翼飛行器的航跡飛行實驗結果表明，該算法能夠有效實現四旋翼飛行器室內動態實時姿態測量，具有良好的工程應用價值。

[1]Weiss S, Scaramuzza D, Siegwart R. Monocular-SLAM-based navigation for autonomous micro helicopters in GPS-denied environments[J]. Journal of Field Robotics, 2011, 28(6): 854-874.

[2]Park S W, Park H S, Kim J H, et al. 3D displacement measurement model for health monitoring of structures using a motion capture system[J].Measurement, 2015,59: 352-362.

[3]邵慧. 無人機高精度目標定位技術研究[D]. 南京航空航天大學, 2014.

SHAO Hui. Research on high precision target localization technology in UAV [D]. Nanjing University of Aeronautics and Astronautics, 2014.

[4]Lupashin S, Hehn M, Mueller M W, et al. A platform for aerial robotics research and demonstration: the flying machine arena[J]. Mechatronics, 2014, 24(1): 41-54.

[5]趙繼.基于運動捕捉系統的四旋翼飛行器導航控制技術研究[D].南京航空航天大學,2015.

ZHAO Ji. Research on navigation and control technology of quadrotor based on motion capture system [D]. Nanjing University of Aeronautics and Astronautics, 2015.

[6]Michael N, Mellinger D, Lindsey Q, et al. The GRASP multiple micro-UAV testbed[J]. Robotics & Automation Magazine, IEEE, 2010, 17(3): 56-65.

[7]Moutinho A, Figueira M, Azinheira J R. Attitude estimation in SO(3): a comparative UAV case study[J]. Journal of Intelligent & Robotic Systems, 2015, 80(3): 375-384.

[8]Hua M D. Attitude estimation for accelerated vehicles using GPS/INS measurements[J]. Control Engineering Practice, 2010, 18(7):723-732.

[9]Martin P, Salaün E. Generalized multiplicative extended Kalman filter for aided attitude and heading reference system[C]. AIAA Guidance, Navigation, & Control Conference, 2010.

[10]Bernardini G, Serafini J, Colella M M, et al. Analysis of a structural-aerodynamic fully-coupled formulation for aeroelastic response of rotorcraft[J]. Aerospace Science & Technology, 2013, 29(1): 175-184.