面向四旋翼飞行器的运动捕捉系统实时测姿方法技术方案

本发明专利技术公开了面向四旋翼飞行器的运动捕捉系统实时测姿方法，利用运动捕捉系统获取安装在四旋翼飞行器上五个标记点的位置信息，并结合四旋翼飞行器运动特性建立机体坐标系，由坐标系间的转化关系求解姿态。相对于传统的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