面向四旋翼飛行器的運動捕捉系統實時測姿方法技術方案

本發明專利技術公開了面向四旋翼飛行器的運動捕捉系統實時測姿方法，利用運動捕捉系統獲取安裝在四旋翼飛行器上五個標記點的位置信息，并結合四旋翼飛行器運動特性建立機體坐標系，由坐標系間的轉化關系求解姿態。相對于傳統的MEMS?IMU測姿方法，本發明專利技術公開的方法測姿精度更高，并且能夠實現動態實時測姿，為機載低成本MEMS慣性導航系統提供校正信息，提高其姿態估計精度，具有精度高、實時性強、易于實現等特點。

Real time attitude determination method for motion capture system of four rotor aircraft

The present invention discloses the four rotor aircraft motion capture system real-time attitude determination method using motion capture system to get installed on the four rotor aircraft on the five marks the location information, and combined with the motion characteristics of four rotor aircraft built body coordinate system, coordinate transformation relation between attitude by solving. Compared with the traditional MEMS IMU attitude determination method, the invention discloses a method of attitude measurement accuracy, and can realize real-time dynamic measurement, provide correction information for low cost MEMS airborne inertial navigation system, improve the attitude estimation accuracy, which has the advantages of high precision, real-time and easy to realize etc..

【技術實現步驟摘要】
面向四旋翼飛行器的運動捕捉系統實時測姿方法
本專利技術屬于多旋翼飛行器
，特別涉及了一種面向四旋翼飛行器的測姿方法。
技術介紹
旋翼飛行器具有體積小、重量輕且能耗低的特點，能夠較好地在室內無GPS(GlobalPositioningSystem)環境中完成搜索、環境監測和救援等任務，是目前無人機領域的研究熱點之一。導航系統為旋翼飛行器提供其飛行控制系統所必須的導航信息，是其執行各種復雜飛行任務的必要保障。受旋翼飛行器體積、載重和成本的條件限制，其通常選用低成本、小型化的低精度機載導航傳感器，如MEMS(Micro-Electro-MechanicalSystem)慣性導航系統。而在室內環境中，無法使用衛星導航系統為MEMS慣性導航系統提供校正信息，其解算的導航信息很快就會發散，無法保證旋翼飛行器室內穩定飛行所需的導航精度要求。運動捕捉系統可以完成在室內指定的捕捉區域內對標記點(特定光點)的捕捉，從而得到標記點的高精度位置信息。將標記點固定在旋翼飛行器上，根據固定點的位置信息能夠得到該飛行器的位置、速度和姿態信息。
技術實現思路
為了解決上述
技術介紹
提出的技術問題，本專利技術旨在提供面向四旋翼飛行器的運動捕捉系統實時測姿方法，提高實時姿態測量的精度，解決多旋翼飛行器在室內環境中衛星導航系統不可用時的實時測姿不精確的問題。為了實現上述技術目的，本專利技術的技術方案為：面向四旋翼飛行器的運動捕捉系統實時測姿方法，包括以下步驟：(1)在四旋翼飛行器上安裝5個標記點，分別記為M1、M2、M3、M4和G，其中標記點M1、M2、M3、M4依次安裝于飛行器的4個機臂，這4個標記點近似共面，且該平面近似平行于飛行器的橫截面，線段M1M2和線段M3M4的中點連線平行于飛行器的機頭方向，點G位于四邊形M1M2M3M4內，且點G到M1、M2、M3、M4的距離互不相等；(2)由運動捕捉系統獲取5個標記點在導航系下的位置坐標，根據幾何原理以及5個標記點的相對位置關系，辨識出5個標記點；(3)根據辨識出的5個標記點的位置坐標，建立機體系；(4)計算導航系到機體系的坐標系轉換矩陣，根據坐標系轉換矩陣中各元素與姿態角的對應關系，求解出飛行器的姿態角。進一步地，在步驟(2)中，辨識5個標記點的過程如下：首先將5個標記點的位置坐標相加求平均，得到的位置坐標記為點分別求出點與5個標記點的距離，則其中最短距離對應的標記點必然是G，再根據G與其余4個標記點的距離，辨識出標記點M1、M2、M3、M4。進一步地，在步驟(3)中，建立機體系的過程如下：首先由M1、M2、M3、M4四點擬合平面作為機體系的X-O-Y平面，擬合平面記為S：a*x+b*y+c*z+1＝0，將點Mi的坐標代入平面S的方程，i＝1,2,3,4，求得其殘差為記殘差平方和解下列方程組：由方程組解得a、b、c的值后，即求出擬合平面S；然后按照下列公式計算機體系的X軸、Y軸、Z軸的正方向單位向量其中，H為線段M1M2的中點，W為四邊形M1M2M3M4的中心，上標“'”表示各點在擬合平面S的垂直投影的投影點。進一步地，在步驟(4)中，所述坐標系轉換矩陣的計算公式如下：上式中，上標“b”和“n”分別表示各向量在機體系和導航系下的投影，T表示轉置。進一步地，在步驟(4)中，設ψ為航向角，θ為俯仰角，γ為橫滾角，則：即，其中，cij表示坐標系轉換矩陣中第i行第j列的元素。采用上述技術方案帶來的有益效果：(1)本專利技術根據固定在四旋翼飛行器上的標記點位置信息，并結合四旋翼飛行器的運動特性，建立機體坐標系，根據機體坐標系和導航系的轉化關系，對其姿態進行估計，可有效提高四旋翼飛行器在無GNSS環境下飛行時的姿態角測量精度；(2)本專利技術可以有效地減小標記點的安裝誤差給測姿精度帶來的影響，測姿精度高且實用性強，能夠為旋翼飛行器室內動態飛行時的姿態測量提供實時參考基準，是一種較好的四旋翼飛行器測姿方法。附圖說明圖1是本專利技術的基本流程圖；圖2是本專利技術標記點的安裝示意圖；圖3是實施例的測姿誤差示意圖；圖4是實施例的四旋翼飛行器的飛行航跡示意圖；圖5是實施例的采用本專利技術后四旋翼飛行器的實時姿態示意圖。具體實施方式以下將結合附圖，對本專利技術的技術方案進行詳細說明。面向四旋翼飛行器的運動捕捉系統實時測姿方法，如圖1所示，步驟如下。步驟一：按照如下方法在多旋翼飛行器上安裝標記點：安裝在四旋翼飛行器上的五個標記點分別記為M1、M2、M3、M4和G，其中標記點M1、M2、M3、M4依次安裝于飛行器的4個機臂，這4個標記點近似共面，且該平面近似平行于飛行器的橫截面，線段M1M2和線段M3M4的中點連線平行于飛行器的機頭方向，點G位于四邊形M1M2M3M4內，5個標記點的距離關系為|M1G|＜|M2G|＜|M4G|＜|M3G|，如圖1所示。步驟二：按照如下方法辨識標記點順序：由運動捕捉系統得到5個標記點坐標(運動捕捉系統坐標系下的位置坐標)，記為(xi,yi,zi)T(i＝1,2,3,4,5)，將這五個標記點的位置坐標相加求平均，得到的位置坐標記為點與5個標記點的距離分別記為Li(i＝1,2,3,4,5)，與數值最小的Li相對應的標記點為G，然后根據G和其它4個標記點的距離約束關系，即|M1G|＜|M2G|＜|M4G|＜|M3G|的距離關系辨識出M1、M2、M3、M4四點。步驟三：按照如下方法，由M1、M2、M3、M4四點擬合平面作為機體橫截面(機體系X-O-Y平面)：擬合平面方程記為S：a*x+b*y+c*z+1＝0，將點Mi(i＝1,2,3,4)的坐標代入平面S的方程，求得其殘差為記殘差平方和解下列方程組：由方程組解得a、b、c的值后，即求出擬合平面S的方程。步驟四：分別為所建立的機體系(記為b系)Xb軸、Yb軸、Zb軸的正方向單位向量(列向量)，按照如下公式計算：其中，H為線段M1M2的中點，W為四邊形M1M2M3M4的中心，上標“'”表示各點在擬合平面S的垂直投影的投影點。步驟五：由導航系(運動捕捉系統坐標系)到機體系的坐標系轉換矩陣中各元素與姿態角的對應關系求飛行器的姿態角。按照如下公式計算其中，上標“b”和“n”分別表示各向量在機體系和導航系下的投影，上標“T”表示向量的轉置。本專利技術中采用右前上坐標系，ψ為航向角，θ為俯仰角，γ為橫滾角，由下列公式計算：其中，cij表示中第i行第j列的元素(i,j＝1,2,3)，航向角ψ的定義域為0°～360°，橫滾角γ的定義域為-180°～180°，俯仰角θ的定義域為：-90°～90°，各姿態角的象限可由cij的正負判斷出。實施例：采用實驗的形式，通過室內靜態和動態實驗，對本專利技術方法后的測姿精度與實時測量進行驗證。靜態實驗中，將標記點安裝在三軸小型機械轉臺的轉臺平面上，通過轉臺平面(相當于機體系XOY平面)的變化模擬四旋翼飛行器姿態角變化。圖3為測姿誤差示意圖，表1為其測姿均方根誤差(RMSE)，其中參考基準為轉臺轉動的姿態角。可以看出其姿態測量誤差都小于0.2度，其中俯仰角的測姿誤差小于0.1度，該精度可滿足四旋翼飛行器的室內飛行需求。表1航向角RMSE(°)俯仰角RMSE(°)橫滾角RMSE(°)0.16460.06670.0665在室內運動捕捉系統中，將本專利技術方法應用于四本文檔來自技高網...

【技術保護點】
面向四旋翼飛行器的運動捕捉系統實時測姿方法，其特征在于，包括以下步驟：(1)在四旋翼飛行器上安裝5個標記點，分別記為M

【技術特征摘要】
1.面向四旋翼飛行器的運動捕捉系統實時測姿方法，其特征在于，包括以下步驟：(1)在四旋翼飛行器上安裝5個標記點，分別記為M1、M2、M3、M4和G，其中標記點M1、M2、M3、M4依次安裝于飛行器的4個機臂，這4個標記點近似共面，且該平面近似平行于飛行器的橫截面，線段M1M2和線段M3M4的中點連線平行于飛行器的機頭方向，點G位于四邊形M1M2M3M4內，且點G到M1、M2、M3、M4的距離互不相等；(2)由運動捕捉系統獲取5個標記點在導航系下的位置坐標，根據幾何原理以及5個標記點的相對位置關系，辨識出5個標記點；(3)根據辨識出的5個標記點的位置坐標，建立機體系；(4)計算導航系到機體系的坐標系轉換矩陣，根據坐標系轉換矩陣中各元素與姿態角的對應關系，求解出飛行器的姿態角。2.根據權利要求1所述面向四旋翼飛行器的運動捕捉系統實時測姿方法，其特征在于：在步驟(2)中，辨識5個標記點的過程如下：首先將5個標記點的位置坐標相加求平均，得到的位置坐標記為點分別求出點與5個標記點的距離，則其中最短距離對應的標記點必然是G，再根據G與其余4個標記點的距離，辨識出標記點M1、M2、M3、M4。3.根據權利要求1或2所述面向四旋翼飛行器的運動捕捉系統實時測姿方法，其特征在于：在步驟(3)中，建立機體系的過程如下：首先由M1、M2、M3、M4四點擬合平面作為機體系的X-O-Y平面，擬合平面記為S：a*x+b*y+c*z+1＝0，將點Mi的坐標代入平面S的方程，i＝1,2,3,4，求得其殘差為記殘差平方和解下列方程組：由方程組解得a、b、c的值后，即求出擬合平面S；然后按照下列公式計算機體系的X軸、Y軸、Z軸的正方向單位向量其中，H為線段M...

【專利技術屬性】
技術研發人員：石鵬，賴際舟，呂品，張竣涵，包勝，
申請(專利權)人：南京航空航天大學，
類型：發明
國別省市：江蘇,32