CN106885573A - 面向四旋翼飞行器的运动捕捉***实时测姿方法 - Google Patents
面向四旋翼飞行器的运动捕捉***实时测姿方法 Download PDFInfo
- Publication number
- CN106885573A CN106885573A CN201710080845.XA CN201710080845A CN106885573A CN 106885573 A CN106885573 A CN 106885573A CN 201710080845 A CN201710080845 A CN 201710080845A CN 106885573 A CN106885573 A CN 106885573A
- Authority
- CN
- China
- Prior art keywords
- sin
- cos
- psi
- gamma
- theta
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了面向四旋翼飞行器的运动捕捉***实时测姿方法,利用运动捕捉***获取安装在四旋翼飞行器上五个标记点的位置信息,并结合四旋翼飞行器运动特性建立机体坐标系,由坐标系间的转化关系求解姿态。相对于传统的MEMS‑IMU测姿方法,本发明公开的方法测姿精度更高,并且能够实现动态实时测姿,为机载低成本MEMS惯性导航***提供校正信息,提高其姿态估计精度,具有精度高、实时性强、易于实现等特点。
Description
技术领域
本发明属于多旋翼飞行器技术领域,特别涉及了一种面向四旋翼飞行器的测姿方法。
背景技术
旋翼飞行器具有体积小、重量轻且能耗低的特点,能够较好地在室内无GPS(Global Positioning System)环境中完成搜索、环境监测和救援等任务,是目前无人机领域的研究热点之一。导航***为旋翼飞行器提供其飞行控制***所必须的导航信息,是其执行各种复杂飞行任务的必要保障。
受旋翼飞行器体积、载重和成本的条件限制,其通常选用低成本、小型化的低精度机载导航传感器,如MEMS(Micro-Electro-Mechanical System)惯性导航***。而在室内环境中,无法使用卫星导航***为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.1646 | 0.0667 | 0.0665 |
在室内运动捕捉***中,将本发明方法应用于四旋翼飞行器航点飞行的实时测姿,图4为四旋翼飞行器飞行航迹示意图,图5为四旋翼飞行器的实时姿态角示意图。动态实验结果可表面,本发明方法可以较好的应用于四旋翼飞行器室内飞行的实时测姿。
以上实施例仅为说明本发明的技术思想,不能以此限定本发明的保护范围,凡是按照本发明提出的技术思想,在技术方案基础上所做的任何改动,均落入本发明保护范围之内。
Claims (5)
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为线段M1M2的中点,W为四边形M1M2M3M4的中心,上标“'”表示各点在拟合平面S的垂直投影的投影点。
4.根据权利要求3所述面向四旋翼飞行器的运动捕捉***实时测姿方法,其特征在于:在步骤(4)中,所述坐标系转换矩阵的计算公式如下:
上式中,上标“b”和“n”分别表示各向量在机体系和导航系下的投影,T表示转置。
5.根据权利要求4所述面向四旋翼飞行器的运动捕捉***实时测姿方法,其特征在于:在步骤(4)中,设ψ为航向角,θ为俯仰角,γ为横滚角,则:
即,
其中,cij表示坐标系转换矩阵中第i行第j列的元素。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710080845.XA CN106885573A (zh) | 2017-02-15 | 2017-02-15 | 面向四旋翼飞行器的运动捕捉***实时测姿方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710080845.XA CN106885573A (zh) | 2017-02-15 | 2017-02-15 | 面向四旋翼飞行器的运动捕捉***实时测姿方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN106885573A true CN106885573A (zh) | 2017-06-23 |
Family
ID=59180671
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710080845.XA Pending CN106885573A (zh) | 2017-02-15 | 2017-02-15 | 面向四旋翼飞行器的运动捕捉***实时测姿方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106885573A (zh) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107966112A (zh) * | 2017-12-03 | 2018-04-27 | 中国直升机设计研究所 | 一种大尺寸旋翼运动参数测量方法 |
CN108680158A (zh) * | 2018-03-30 | 2018-10-19 | 南京航空航天大学 | 一种基于极化域波达因子的飞行器群内近距定位方法 |
CN112057883A (zh) * | 2020-09-08 | 2020-12-11 | 北京北特圣迪科技发展有限公司 | 一种四吊点柔索表演悬挂平台的姿态控制方法 |
CN112649824A (zh) * | 2020-12-23 | 2021-04-13 | 中国自然资源航空物探遥感中心 | 飞行姿态确定方法及装置 |
CN113819889A (zh) * | 2021-09-09 | 2021-12-21 | 中国电子科技集团公司第五十四研究所 | 一种基于飞行器旋翼光源检测的相对测距测姿方法 |
CN116461714A (zh) * | 2023-05-17 | 2023-07-21 | 北京科技大学 | 一种基于动捕***的仿蝴蝶扑翼飞行器飞行参数确定方法 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102829785A (zh) * | 2012-08-30 | 2012-12-19 | 中国人民解放军国防科学技术大学 | 基于序列图像和基准图匹配的飞行器全参数导航方法 |
CN103697883A (zh) * | 2014-01-07 | 2014-04-02 | 中国人民解放军国防科学技术大学 | 一种基于天际线成像的飞行器水平姿态确定方法 |
CN103837151A (zh) * | 2014-03-05 | 2014-06-04 | 南京航空航天大学 | 一种四旋翼飞行器的气动模型辅助导航方法 |
CN104897172A (zh) * | 2015-06-18 | 2015-09-09 | 南京航空航天大学 | 基于运动捕捉***的旋转mems惯导磁航向角误差补偿方法 |
CN104950902A (zh) * | 2015-06-10 | 2015-09-30 | 杨珊珊 | 多旋翼飞行器的控制方法及多旋翼飞行器 |
CN105488770A (zh) * | 2015-12-11 | 2016-04-13 | 中国测绘科学研究院 | 一种面向对象的机载激光雷达点云滤波方法 |
CN105759834A (zh) * | 2016-03-09 | 2016-07-13 | 中国科学院上海微***与信息技术研究所 | 一种主动捕获低空小型无人飞行器的***及方法 |
-
2017
- 2017-02-15 CN CN201710080845.XA patent/CN106885573A/zh active Pending
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102829785A (zh) * | 2012-08-30 | 2012-12-19 | 中国人民解放军国防科学技术大学 | 基于序列图像和基准图匹配的飞行器全参数导航方法 |
CN103697883A (zh) * | 2014-01-07 | 2014-04-02 | 中国人民解放军国防科学技术大学 | 一种基于天际线成像的飞行器水平姿态确定方法 |
CN103837151A (zh) * | 2014-03-05 | 2014-06-04 | 南京航空航天大学 | 一种四旋翼飞行器的气动模型辅助导航方法 |
CN104950902A (zh) * | 2015-06-10 | 2015-09-30 | 杨珊珊 | 多旋翼飞行器的控制方法及多旋翼飞行器 |
CN104897172A (zh) * | 2015-06-18 | 2015-09-09 | 南京航空航天大学 | 基于运动捕捉***的旋转mems惯导磁航向角误差补偿方法 |
CN105488770A (zh) * | 2015-12-11 | 2016-04-13 | 中国测绘科学研究院 | 一种面向对象的机载激光雷达点云滤波方法 |
CN105759834A (zh) * | 2016-03-09 | 2016-07-13 | 中国科学院上海微***与信息技术研究所 | 一种主动捕获低空小型无人飞行器的***及方法 |
Non-Patent Citations (6)
Title |
---|
LUPASHIN S,ET AL.: ""The Flying Machine Arena as of 2010"", 《PROC OF IEEE INTERNATIONAL CONFERENCE ON ROBITCS AND AUTOMATION,SHANGHAI,CHINA,2011》 * |
刘建业等: ""微小型四旋翼飞行器多信息非线性融合导航方法及实现"", 《南京航空航天大学学报》 * |
张佳: ""四旋翼飞行器轨迹及运动目标跟踪控制研究"", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 * |
曾庆化等: ""基于MCS 的多旋翼飞行器测试平台地面站设计"", 《计算机技术与发展》 * |
李莉,胡本田主编: "《统计学概论》", 30 April 2001, 中国商业出版社 * |
赵继: ""基于运动捕捉***的四旋翼飞行器导航控制技术研究"", 《万方学位论文数据库》 * |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107966112A (zh) * | 2017-12-03 | 2018-04-27 | 中国直升机设计研究所 | 一种大尺寸旋翼运动参数测量方法 |
CN108680158A (zh) * | 2018-03-30 | 2018-10-19 | 南京航空航天大学 | 一种基于极化域波达因子的飞行器群内近距定位方法 |
CN108680158B (zh) * | 2018-03-30 | 2022-07-26 | 南京航空航天大学 | 一种基于极化域波达因子的飞行器群内近距定位方法 |
CN112057883A (zh) * | 2020-09-08 | 2020-12-11 | 北京北特圣迪科技发展有限公司 | 一种四吊点柔索表演悬挂平台的姿态控制方法 |
CN112649824A (zh) * | 2020-12-23 | 2021-04-13 | 中国自然资源航空物探遥感中心 | 飞行姿态确定方法及装置 |
CN113819889A (zh) * | 2021-09-09 | 2021-12-21 | 中国电子科技集团公司第五十四研究所 | 一种基于飞行器旋翼光源检测的相对测距测姿方法 |
CN113819889B (zh) * | 2021-09-09 | 2024-01-26 | 中国电子科技集团公司第五十四研究所 | 一种基于飞行器旋翼光源检测的相对测距测姿方法 |
CN116461714A (zh) * | 2023-05-17 | 2023-07-21 | 北京科技大学 | 一种基于动捕***的仿蝴蝶扑翼飞行器飞行参数确定方法 |
CN116461714B (zh) * | 2023-05-17 | 2024-01-16 | 北京科技大学 | 一种基于动捕***的仿蝴蝶扑翼飞行器飞行参数确定方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106885573A (zh) | 面向四旋翼飞行器的运动捕捉***实时测姿方法 | |
CN105242285B (zh) | 一种基于卫星通信的无人机导航数据被欺骗识别方法 | |
CN106054929B (zh) | 一种基于光流的无人机自动降落引导方法 | |
CN103697889B (zh) | 一种基于多模型分布式滤波的无人机自主导航与定位方法 | |
CN110926468B (zh) | 基于传递对准的动中通天线多平台航姿确定方法 | |
CN108152529A (zh) | 一种基于飞行参数计算风速及风向的方法 | |
Langelaan et al. | Wind field estimation for autonomous dynamic soaring | |
CN105698762A (zh) | 一种单机航迹上基于不同时刻观测点的目标快速定位方法 | |
CN106155081B (zh) | 一种旋翼无人机大范围目标监测和精确定位方法 | |
CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
CN106289246A (zh) | 一种基于位置和姿态测量***的柔性杆臂测量方法 | |
CN103913181A (zh) | 一种基于参数辨识的机载分布式pos传递对准方法 | |
CN106373159A (zh) | 一种简化的无人机多目标定位方法 | |
CN103744052B (zh) | 一种应用于空中目标定位的双星测时差测向方法及装置 | |
CN106595668A (zh) | 一种用于光电吊舱的无源定位算法 | |
CN104115081A (zh) | 利用恒定倾斜角转弯的风计算*** | |
CN107831776A (zh) | 基于九轴惯性传感器的无人机自主返航方法 | |
CN108536163B (zh) | 一种单面结构环境下的动力学模型/激光雷达组合导航方法 | |
Wilson et al. | A vision based relative navigation framework for formation flight | |
CN106017460B (zh) | 一种地形辅助惯导紧组合的水下潜器导航定位方法 | |
CN109084760A (zh) | 一种楼宇间导航*** | |
CN110243377A (zh) | 一种基于分层式结构的集群飞行器协同导航方法 | |
CN112556724A (zh) | 动态环境下的微型飞行器低成本导航***初始粗对准方法 | |
CN104019818A (zh) | 一种基于预测轨迹的行星导航轨道器布局优化方法 | |
CN103453907A (zh) | 基于分层大气模型的行星进入段导航滤波方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20170623 |
|
RJ01 | Rejection of invention patent application after publication |