CN106885573A - 面向四旋翼飞行器的运动捕捉系统实时测姿方法 - Google Patents

Abstract

本发明公开了面向四旋翼飞行器的运动捕捉系统实时测姿方法，利用运动捕捉系统获取安装在四旋翼飞行器上五个标记点的位置信息，并结合四旋翼飞行器运动特性建立机体坐标系，由坐标系间的转化关系求解姿态。相对于传统的MEMS‑IMU测姿方法，本发明公开的方法测姿精度更高，并且能够实现动态实时测姿，为机载低成本MEMS惯性导航系统提供校正信息，提高其姿态估计精度，具有精度高、实时性强、易于实现等特点。

Description

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

技术领域

本发明属于多旋翼飞行器技术领域，特别涉及了一种面向四旋翼飞行器的测姿方法。

背景技术

旋翼飞行器具有体积小、重量轻且能耗低的特点，能够较好地在室内无GPS(Global Positioning System)环境中完成搜索、环境监测和救援等任务，是目前无人机领域的研究热点之一。导航系统为旋翼飞行器提供其飞行控制系统所必须的导航信息，是其执行各种复杂飞行任务的必要保障。

受旋翼飞行器体积、载重和成本的条件限制，其通常选用低成本、小型化的低精度机载导航传感器，如MEMS(Micro-Electro-Mechanical System)惯性导航系统。而在室内环境中，无法使用卫星导航系统为MEMS惯性导航系统提供校正信息，其解算的导航信息很快就会发散，无法保证旋翼飞行器室内稳定飞行所需的导航精度要求。

运动捕捉系统可以完成在室内指定的捕捉区域内对标记点(特定光点)的捕捉，从而得到标记点的高精度位置信息。将标记点固定在旋翼飞行器上，根据固定点的位置信息能够得到该飞行器的位置、速度和姿态信息。

发明内容

为了解决上述背景技术提出的技术问题，本发明旨在提供面向四旋翼飞行器的运动捕捉系统实时测姿方法，提高实时姿态测量的精度，解决多旋翼飞行器在室内环境中卫星导航系统不可用时的实时测姿不精确的问题。

为了实现上述技术目的，本发明的技术方案为：

面向四旋翼飞行器的运动捕捉系统实时测姿方法，包括以下步骤：

(1)在四旋翼飞行器上安装5个标记点，分别记为M₁、M₂、M₃、M₄和G，其中标记点M₁、M₂、M₃、M₄依次安装于飞行器的4个机臂，这4个标记点近似共面，且该平面近似平行于飞行器的横截面，线段M₁M₂和线段M₃M₄的中点连线平行于飞行器的机头方向，点G位于四边形M₁M₂M₃M₄内，且点G到M₁、M₂、M₃、M₄的距离互不相等；

(2)由运动捕捉系统获取5个标记点在导航系下的位置坐标，根据几何原理以及5个标记点的相对位置关系，辨识出5个标记点；

(3)根据辨识出的5个标记点的位置坐标，建立机体系；

(4)计算导航系到机体系的坐标系转换矩阵，根据坐标系转换矩阵中各元素与姿态角的对应关系，求解出飞行器的姿态角。

进一步地，在步骤(2)中，辨识5个标记点的过程如下：

首先将5个标记点的位置坐标相加求平均，得到的位置坐标记为点分别求出点与5个标记点的距离，则其中最短距离对应的标记点必然是G，再根据G与其余4个标记点的距离，辨识出标记点M₁、M₂、M₃、M₄。

进一步地，在步骤(3)中，建立机体系的过程如下：

首先由M₁、M₂、M₃、M₄四点拟合平面作为机体系的X-O-Y平面，拟合平面记为S：a*x+b*y+c*z+1＝0，将点M_i的坐标代入平面S的方程，i＝1,2,3,4，求得其残差为记残差平方和解下列方程组：

由方程组解得a、b、c的值后，即求出拟合平面S；

然后按照下列公式计算机体系的X轴、Y轴、Z轴的正方向单位向量

其中，H为线段M₁M₂的中点，W为四边形M₁M₂M₃M₄的中心，上标“'”表示各点在拟合平面S的垂直投影的投影点。

进一步地，在步骤(4)中，所述坐标系转换矩阵的计算公式如下：

上式中，上标“b”和“n”分别表示各向量在机体系和导航系下的投影，T表示转置。

进一步地，在步骤(4)中，设ψ为航向角，θ为俯仰角，γ为横滚角，则：

即，

其中，c_ij表示坐标系转换矩阵中第i行第j列的元素。

采用上述技术方案带来的有益效果：

(1)本发明根据固定在四旋翼飞行器上的标记点位置信息，并结合四旋翼飞行器的运动特性，建立机体坐标系，根据机体坐标系和导航系的转化关系，对其姿态进行估计，可有效提高四旋翼飞行器在无GNSS环境下飞行时的姿态角测量精度；

(2)本发明可以有效地减小标记点的安装误差给测姿精度带来的影响，测姿精度高且实用性强，能够为旋翼飞行器室内动态飞行时的姿态测量提供实时参考基准，是一种较好的四旋翼飞行器测姿方法。

附图说明

图1是本发明的基本流程图；

图2是本发明标记点的安装示意图；

图3是实施例的测姿误差示意图；

图4是实施例的四旋翼飞行器的飞行航迹示意图；

图5是实施例的采用本发明后四旋翼飞行器的实时姿态示意图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

面向四旋翼飞行器的运动捕捉系统实时测姿方法，如图1所示，步骤如下。

步骤一：按照如下方法在多旋翼飞行器上安装标记点：

安装在四旋翼飞行器上的五个标记点分别记为M₁、M₂、M₃、M₄和G，其中标记点M₁、M₂、M₃、M₄依次安装于飞行器的4个机臂，这4个标记点近似共面，且该平面近似平行于飞行器的横截面，线段M₁M₂和线段M₃M₄的中点连线平行于飞行器的机头方向，点G位于四边形M₁M₂M₃M₄内，5个标记点的距离关系为|M₁G|＜|M₂G|＜|M₄G|＜|M₃G|，如图1所示。

步骤二：按照如下方法辨识标记点顺序：

由运动捕捉系统得到5个标记点坐标(运动捕捉系统坐标系下的位置坐标)，记为(x_i,y_i,z_i)^T(i＝1,2,3,4,5)，将这五个标记点的位置坐标相加求平均，得到的位置坐标记为点

与5个标记点的距离分别记为L_i(i＝1,2,3,4,5)，与数值最小的L_i相对应的标记点为G，然后根据G和其它4个标记点的距离约束关系，即|M₁G|＜|M₂G|＜|M₄G|＜|M₃G|的距离关系辨识出M₁、M₂、M₃、M₄四点。

步骤三：按照如下方法，由M₁、M₂、M₃、M₄四点拟合平面作为机体横截面(机体系X-O-Y平面)：

拟合平面方程记为S：a*x+b*y+c*z+1＝0，将点M_i(i＝1,2,3,4)的坐标代入平面S的方程，求得其残差为记残差平方和解下列方程组：

由方程组解得a、b、c的值后，即求出拟合平面S的方程。

步骤四：分别为所建立的机体系(记为b系)X^b轴、Y^b轴、Z^b轴的正方向单位向量(列向量)，按照如下公式计算：

其中，H为线段M₁M₂的中点，W为四边形M₁M₂M₃M₄的中心，上标“'”表示各点在拟合平面S的垂直投影的投影点。

步骤五：由导航系(运动捕捉系统坐标系)到机体系的坐标系转换矩阵中各元素与姿态角的对应关系求飞行器的姿态角。

按照如下公式计算

其中，上标“b”和“n”分别表示各向量在机体系和导航系下的投影，上标“T”表示向量的转置。

本发明中采用右前上坐标系，ψ为航向角，θ为俯仰角，γ为横滚角，由下列公式计算：

其中，c_ij表示中第i行第j列的元素(i,j＝1,2,3)，航向角ψ的定义域为0°～360°，横滚角γ的定义域为-180°～180°，俯仰角θ的定义域为：-90°～90°，各姿态角的象限可由c_ij的正负判断出。

实施例：

采用实验的形式，通过室内静态和动态实验，对本发明方法后的测姿精度与实时测量进行验证。

静态实验中，将标记点安装在三轴小型机械转台的转台平面上，通过转台平面(相当于机体系XOY平面)的变化模拟四旋翼飞行器姿态角变化。图3为测姿误差示意图，表1为其测姿均方根误差(RMSE)，其中参考基准为转台转动的姿态角。可以看出其姿态测量误差都小于0.2度，其中俯仰角的测姿误差小于0.1度，该精度可满足四旋翼飞行器的室内飞行需求。

表1

航向角RMSE(°)	俯仰角RMSE(°)	横滚角RMSE(°)
			0.1646	0.0667	0.0665

在室内运动捕捉系统中，将本发明方法应用于四旋翼飞行器航点飞行的实时测姿，图4为四旋翼飞行器飞行航迹示意图，图5为四旋翼飞行器的实时姿态角示意图。动态实验结果可表面，本发明方法可以较好的应用于四旋翼飞行器室内飞行的实时测姿。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (5)

1.面向四旋翼飞行器的运动捕捉系统实时测姿方法，其特征在于，包括以下步骤：

(1)在四旋翼飞行器上安装5个标记点，分别记为M₁、M₂、M₃、M₄和G，其中标记点M₁、M₂、M₃、M₄依次安装于飞行器的4个机臂，这4个标记点近似共面，且该平面近似平行于飞行器的横截面，线段M₁M₂和线段M₃M₄的中点连线平行于飞行器的机头方向，点G位于四边形M₁M₂M₃M₄内，且点G到M₁、M₂、M₃、M₄的距离互不相等；

(2)由运动捕捉系统获取5个标记点在导航系下的位置坐标，根据几何原理以及5个标记点的相对位置关系，辨识出5个标记点；

(3)根据辨识出的5个标记点的位置坐标，建立机体系；

(4)计算导航系到机体系的坐标系转换矩阵，根据坐标系转换矩阵中各元素与姿态角的对应关系，求解出飞行器的姿态角。

2.根据权利要求1所述面向四旋翼飞行器的运动捕捉系统实时测姿方法，其特征在于：在步骤(2)中，辨识5个标记点的过程如下：

首先将5个标记点的位置坐标相加求平均，得到的位置坐标记为点分别求出点与5个标记点的距离，则其中最短距离对应的标记点必然是G，再根据G与其余4个标记点的距离，辨识出标记点M₁、M₂、M₃、M₄。

3.根据权利要求1或2所述面向四旋翼飞行器的运动捕捉系统实时测姿方法，其特征在于：在步骤(3)中，建立机体系的过程如下：

首先由M₁、M₂、M₃、M₄四点拟合平面作为机体系的X-O-Y平面，拟合平面记为S：a*x+b*y+c*z+1＝0，将点M_i的坐标代入平面S的方程，i＝1,2,3,4，求得其残差为记残差平方和解下列方程组：

$\left\{\begin{array}{c}\frac{\∂\mathrm{\ϵ}}{\∂a}=0\\ \frac{\∂\mathrm{\ϵ}}{\∂b}=0\\ \frac{\∂\mathrm{\ϵ}}{\∂c}=0\end{array}\right.$

由方程组解得a、b、c的值后，即求出拟合平面S；

然后按照下列公式计算机体系的X轴、Y轴、Z轴的正方向单位向量

其中，H为线段M₁M₂的中点，W为四边形M₁M₂M₃M₄的中心，上标“'”表示各点在拟合平面S的垂直投影的投影点。

4.根据权利要求3所述面向四旋翼飞行器的运动捕捉系统实时测姿方法，其特征在于：在步骤(4)中，所述坐标系转换矩阵的计算公式如下：

$C_n^b={\left[\begin{array}{c}{\left(\stackrel{\→}{t_i^b}\right)}^T\\ {\left(\stackrel{\→}{t_j^b}\right)}^T\\ {\left(\stackrel{\→}{t_k^b}\right)}^T\end{array}\right]}^{-1}*\left[\begin{array}{c}{\left(\stackrel{\→}{t_i^n}\right)}^T\\ {\left(\stackrel{\→}{t_j^n}\right)}^T\\ {\left(\stackrel{\→}{t_k^n}\right)}^T\end{array}\right]$

上式中，上标“b”和“n”分别表示各向量在机体系和导航系下的投影，T表示转置。

5.根据权利要求4所述面向四旋翼飞行器的运动捕捉系统实时测姿方法，其特征在于：在步骤(4)中，设ψ为航向角，θ为俯仰角，γ为横滚角，则：

$\begin{array}{c}{C}_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& -\sin \mathrm{\γ}\\ 0& 1& 0\\ \sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]*\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ 0& -\sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]*\left[\begin{array}{ccc}\cos \mathrm{\ψ}& -\sin \mathrm{\ψ}& 0\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]\end{array}$

即，

其中，c_ij表示坐标系转换矩阵中第i行第j列的元素。