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法向量的夹角求横滚角。

在n系下分别用表示Xb轴、Yb轴、Zb轴上方向为正方向的单位向量，在b系下分别用表示Xb轴、Yb轴、Zb轴上方向为正方向的单位向量。根据b系和n系之间的转化关系有：

图1　三点测姿算法原理示意图 Fig.1　Schematic diagram of three markers attitude estimation algorithm

该算法不仅要求3个标记点满足直角三角形的安装约束，而且要求两条直角边(记为L1和L2)分别与真实机体系的X轴、Y轴方向一致；然而标记点的安装常由人工完成，难以保证安装精度。

将Δl2代入式(6)～式(8)，得到Δψ2、Δθ2和Δγ2。从而可以推得Δψ1>Δψ2、Δθ1>Δθ2和Δγ1>Δγ2，证明相对于三点法，五点法确定的机体系与真实机体系之间的偏差更小。利用Δψi、Δθi和Δγi可得到有：

2　MCS五点测姿算法设计与分析

针对MCS中三点测姿算法测姿误差受标记点安装误差影响大的问题，本文设计了一种五点测姿算法。本节将对五点测姿算法流程进行阐述，并且对两种测姿算法的测姿误差进行对比分析。

2.1　五点测姿算法设计

本文中的四旋翼飞行器测姿算法一共采用5个标记点安装在飞行器上，如图2所示。其中，A、B、C、D 4点分别代表安装在机臂上的4个标记点，用来求解飞行器姿态。AB和CD中点的连线平行机头方向，第5个标记点G为辨识点，位于四边形ABCD内，用于辨识前4个标记点，其距离约束关系为AG<BG<DG<CG。

图2　五点测姿算法标记点安装图 Fig.2　Installation diagram of five markers attitude estimation algorithm

定义2.3[4] 称系统(1.1)具有持久性，若存在独立于系统(1.1)的解的正常数mi，Mi(i=1,2,3,4)，使得系统(1.1)的任意解(x(t),y(t),u(t),v(t))T都满足

由于MCS下捕捉标记点得到的位置信息顺序是随机的，因此该算法中首先根据标记点的位置关系得到辨识点G信息，然后利用G与其他4点的距离关系辨识出A、B、C、D 4点。

图3　五点测姿算法流程图 Fig.3　Flowchart of five markers attitude estimation algorithm

图4　五点测姿算法原理示意图 Fig.4　Schematic diagram of five markers attitude estimation algorithm

四旋翼飞行器的飞行动力由螺旋桨提供，通过A、B、C、D 4点位置对旋翼所在位置进行全局平面拟合(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轴对应机体的机头方向。则：

五点法标记点的安装没有角度约束要求，直接固定在四旋翼飞行器4个支架上，易于实现。采用旋转矢量法对载体姿态进行求解 [7-9]，其算法流程如图3所示，原理示意图如图4所示。

看得出，这句话对乔十二郎的安抚作用十分明显。秦铁崖身材过分高大，十分醒目，加上他长期办案，干的是刀头上舔血的营生，这使他养成坚韧不拔、永不服输的战斗作风，龙骧虎步，目光锐利，因此无论置身何处，威慑力都是强大的。乔十二郎虽说报仇心切，意志坚定，到底是小孩子，况且连连犯案，心底那份恐惧挥之不去。第一眼看到秦铁崖时，其内心忍不住悲鸣：“我命休矣！如何是好？家仇未报，我死不瞑目！”

(1)

所有节点都相当于“校验员”，它们无时无刻不在检查区块中的交易信息是否正确，并且在检查交易的时候，不断尝试产生随机数，计算哈希值，使数据具有很强的安全性，黑客无法入侵，无法修改账户余额。随着用户的增加，越来越多的后续节点（用户）加入到了比特币网络中，共同完成共识的过程[3]；而整个过程中网络中每个节点的地位是相同的。比特币不是凭空产生，而是通过消耗了电力、物力并应运算而产生的，因此具有价值，可以兑换成现有的货币。

(2)

由Yb轴和Zb轴的关系可以建立Xb轴：

ti=tj×tk

(3)

社会经济的快速发展，特别是“健康中国”的有效推进，体育作为特殊的身体练习项目之一，既能满足人们的健康需要，也能提升人们的生活品质，体育健身健康逐渐成为人们生活的必需．习近平在党的十九大报告中指出：中国特色社会主义进入新时代，我国社会主要矛盾已经转化为人民日益增长的美好生活需要和不平衡不充分的发展之间的矛盾 [1]．健康与养生已成为人们对美好生活的基本需求之一，满足人民对体育健康和养生的需求已成为一种社会责任．从民族传统武术文化传承的视角，探索和构建民族传统养生与健康高素质人才的培养模式，是一个全新且具挑战性的尝试．

(4)

(5)

然后利用变化矩阵与姿态角的关系即可求出姿态角。

2.2　MCS两种测姿算法对比

实验1中，标记点直接粘贴在转台平面上，所有标记点共面，且其所在平面与转台平面平行。由表1和图6可知，将两种测姿算法结果与转台对比，其俯仰角和横滚角偏差相近，测姿精度相当；比较其航向角偏差，三点法大于五点法；比较其RMSE，五点法优于三点法。因此，当标记点安装误差主要存在于与转台平面平行方向上时，五点法优于三点法，主要体现于航向角的测量精度。

图5中，b0系为真实机体系。三点法中，由B、C、D 3点确定机体系b1求姿态角，L1为Yb1轴，L2为Xb1轴；五点法中，由A、B、C、D 4个标记点确定机体系b2求姿态角。

图5　标记点安装示意图 Fig.5　Installation diagram of markers

三点法中，理想情况为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)。

因此，三点测姿算法在旋翼飞行器上应用时会存在以下不足：当标记点的安装存在误差时，L1与机体纵轴位置存在偏差，且p0与机体横截面不重合，从而导致从测姿原理上产生较大测姿误差。

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

(9)

记fk()(k=1,2,3)分别为根据变化矩阵求航向角、俯仰角和横滚角的函数，三点法和五点法测姿误差可分别表示为fk()-fk()和fk()-fk()。可以得到：

而苏轼建设性的提出“均户口、按地征赋役”则是他比较重要的治税思想，他还提出通过赋税手段调节商品流通，建议免征五谷力胜钱以通商贾，免榷零售小商盐课以广流通，免征漕船携带商品之税以繁荣京师市场等建议，苏轼的治税思想对当时的税收制度有着一定的积极影响。

(10)

由此看出，本文提出的五点测姿算法可以降低安装标记点的几何空间约束，并减小安装误差对测姿误差的影响，因此五点法的测姿误差比三点法小。

师：中学生之间的恋爱是真正的爱情吗？对异性的好感是爱情吗？我们来看看爱情的一种定义：以婚姻为目的建立感情的过程。马克思主义认为爱情是指建立在一定的物质基础上，男女双方基于共同的世界观、人生观和共同的生活理想，彼此爱慕，渴望对方成为自己终身伴侣的一种强烈、纯真、专一的感情。科学家总结说，要想获得理想的爱情，必须具备以下几个条件：

3　算法验证

为验证提出的五点测姿法的测姿精度，将标记点安装在三轴小型机械转台的转台平面上，通过转台平面(相当于机体系X-O-Y平面)的变化模拟四旋翼飞行器姿态角变化。实验分为实验1和实验2，实验中同时改变转台转动的3个姿态角，将三点法与五点法的测姿结果与转台转动角度进行对比，并分析其均方根误差(RMSE)。实验1中，主要针对标记点的安装在与转台平面平行方向上存在误差时，对比两种算法的测姿精度。实验2相对于实验1，不仅考虑了标记点在与转台平面平行方向上的安装误差，还考虑了与转台平面的垂直方向上的安装误差。

五点测姿算法和三点测姿算法的测姿精度取决于其建立的机体系的准确度，因此从两种测姿算法确定的机体系与真实机体系的偏差来分析测姿误差大小。

表1　实验1测姿均方根误差表

Table 1　Attitude estimation RMSE in the first experiment

航向角/ ° 俯仰角/ ° 横滚角/ ° 五点法0.16460.06670.0665三点法0.33270.13520.1342

图6　实验1测姿误差曲线 Fig.6　Attitude estimation error in the first experiment

实验2中，给一个标记点(相对位置与图5中点C相同)在与转台平面的垂直方向上引入5mm的安装误差。由表2和图7可知，通过比较其姿态角偏差和RMSE得出结论，当标记点安装在转台系(机体系)三轴方向上都存在误差时，五点法的3个姿态角测量精度都明显优于三点法。

表2　实验2测姿均方根误差表

Table 2　Attitude estimation RMSE in the second experiment

航向角/ ° 俯仰角/ ° 横滚角/ ° 五点法0.16500.06680.0679三点法0.87731.52741.8769

图7　实验2测姿误差曲线 Fig.7　Attitude estimation error in the second experiment

实验1和实验2的结果表明，针对实际应用时，标记点在机体系三轴方向上都存在安装误差，较三点测姿算法，五点测姿算法能够通过补偿标记点安装误差来减小测姿误差，其测姿RMSE值小于0.2°,保证了测姿精度，测姿效果更好。

此外，为进一步验证五点测姿法在多旋翼飞行器中的应用效果，进行了基于该方法的四旋翼飞行器航迹飞行实验。图8和图9为其飞行航迹图，图10为利用五点测姿算法对其实时动态测姿图。结果表明，应用五点测姿算法，能够较好地完成旋翼飞行器实时动态测姿任务。

图8　四旋翼飞行器实际飞行 Fig.8　Actual flight of quadrotor

图9　室内MCS中四旋翼飞行器航点飞行轨迹 Fig.9　Flight trajectory of quadrotor in MCS

图10　五点测姿算法实时动态测姿曲线 Fig.10　Real-time attitude estimation of five markers attitude estimation algorithm

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.