CN112363528A - 基于机载视觉的无人机抗干扰集群编队控制方法 - Google Patents

Abstract

本发明公开了一种基于机载视觉的无人机抗干扰集群编队控制方法，该方法中，僚机通过机载视觉系统识别长机，获得自身相对于长机的位置和速度信息，从而使得僚机能够在信号干扰/屏蔽的情况下执行编队作业，控制僚机以平滑变化的速度到达集群编队中的期望位置，再保持与长机相同的速度跟随长机飞行，从而完成编队控制。

Description

基于机载视觉的无人机抗干扰集群编队控制方法

技术领域

本发明涉及无人机控制方法，尤其涉及一种基于机载视觉的无人机抗干扰集群编队控制方法。

背景技术

随着第四次工业革命的到来，融合人工智能技术和机器人技术的空中无人集群智能化系统引领了全球的研发浪潮，基于多无人机系统自主协同的智能集群技术代表着未来科技的发展方向，在军事领域已经成为形成新质战斗力的重要途经，这是加速发展的体系化联合作战的重要需求，也是新时代面向未来战争的重要战略需求。

由于任务本身及执行环境的日渐复杂，单个无人机在信息的获取处理和对运动的控制能力等方面都是有限的，因此人们考虑借助多机协作的方式来弥补上述不足。多无人机集群编队控制系统可以通过相互协作在时间和空间上并发地执行复杂任务，因此对无人机单体的设计难度比较低，其只需具备特定功能，通过多机间的功能互补，协同机动，来缩短复杂任务的执行时间，提高任务执行效率，具有经济可靠、灵活性高、响应快速等优点。

无人机的集群编队控制问题主要分四个技术模块——编队生成、编队保持、编队切换和编队避障。编队生成是多无人机系统根据环境的限制、任务需求和无人机数量来设计稳定的编队队形；编队保持是多无人机系统在整体运动中保持编队不变，或者在允许队形少量变化的条件下保持整个编队统一前进；编队避障是多无人机借助环境信息来避免与环境中的障碍物发生碰撞的同时，又保证不会与其他无人机发生碰撞。目前无人机集群编队控制的研究，主要是解决集群无人机协同编队控制中的编队保持问题，编队保持算法主要有领航-跟随法(Leader-Follower，LF)、基于行为法的方法、虚拟结构法。

但由于算法本身的原因，常用的三种编队保持算法都有其各自的局限性：领航-跟随法的方法原理简单、易于实现，但鲁棒性稍差，而且控制误差会逐级向后传递并被放大；基于行为法的方法具有很好的灵活性和鲁棒性，使每个无人机均可兼顾各种行为，利于实现实时的分布式控制，但难以保持编队的准确性；虚拟结构法容易指定无人机群体的行为，能够获取较高的轨迹跟踪效果，但要求队形在虚拟结构下运动，因而限制了应用范围。单一地使用某种传统的编队控制算法，都会由于无法兼顾集群编队过程中长、僚机的飞行状态变化，而降低系统的控制效率和任务执行的成功率。

此外，为了形成编队并保证一定的编队保持效率，需要多无人机之间能获取并交换状态信息(位置速度信息等)，目前现有技术中主要使用INS/GNSS组合导航技术来完成无人机的定位、导航以及授时等工作，通过多路组网通信来完成一对多、多对一的数据交换，在大多数工况下都可以正常使用，定位准确且数据测量传输效率较高。

集群编队控制系统中运动状态的获取和交换极易受到周围环境的影响，此外反无人机技术的发展使得原本就复杂的拒止作战环境更加严峻，无人机集群非但无法完成指定的任务，就连安全返航都很艰难。拒止环境是指GNSS信号受电磁环境限制无法完成定位、导航以及授时等工作的环境，在复杂环境下其信号易遭到干扰甚至关闭，因此仅靠INS无法准确获取系统中各无人机的准确位置信息。另外，在强拒止环境下，无人机之间以及同地面站之间的各种通讯设备都会受到严重干扰，轻则无功而返，重则坠机牺牲，因此无人机集群编队控制系统的抗干扰问题亟待解决。

由于上述原因，本发明人对现有的无人机集群编队控制方法做了深入研究，以期待设计出一种能够解决上述问题的，能够抗干扰的集群编队控制方法。

发明内容

为了克服上述问题，本发明人进行了锐意研究，设计出一种基于机载视觉的无人机抗干扰集群编队控制方法，该方法中，僚机通过机载视觉系统识别长机，获得自身相对于长机的位置和速度信息，从而使得僚机能够在信号干扰/屏蔽的情况下执行编队作业，控制僚机以平滑变化的速度到达集群编队中的期望位置，再保持与长机相同的速度跟随长机飞行，从而完成编队控制，完成本发明。

具体来说，本发明的目的在于提供一种基于机载视觉的无人机抗干扰集群编队控制方法，所述集群编队包括一个长机和至少一个僚机，该方法包括如下步骤：

步骤1，僚机通过机载视觉系统识别长机，并获得自身相对于长机的位置和速度信息；

步骤2，控制僚机以平滑变化的速度到达集群编队中的期望位置，再保持与长机相同的速度跟随长机飞行。

其中，在步骤2中，通过下式(一)控制僚机的速度大小：

其中，V_fi表示僚机的速度大小，

V_max表示僚机所能达到的最大速度大小，

V_team表示集群编队的速度大小，即长机的速度大小，

d_i表示僚机与期望位置之间的距离，

R_c表示控制区半径，

R_d表示死区半径。

其中，在步骤2中，通过下式(二)控制僚机的速度方向：

其中，V表示僚机在水平方向上的速度方向矢量，

V_x表示V在水平坐标系中X轴方向的分量，

V_y表示V在水平坐标系中Y轴方向的分量，

x′_i表示期望位置在水平坐标系中的X轴坐标，

y′_i表示期望位置在水平坐标系中的Y轴坐标，

x_i表示僚机在水平坐标系中的X轴坐标，

y_i表示僚机在水平坐标系中的Y轴坐标。

其中，所述控制区半径R_c取值为10～30米，所述死区半径R_d取值为0.1～1米。

其中，在执行步骤2以前，所述僚机调整自身高度，使得僚机与长机处于同一水平高度上。

其中，所述期望位置为集群编队中僚机所在位置，该期望位置由僚机实时解算得到。

其中，所述步骤1包括如下子步骤：

子步骤1，通过僚机上的摄像机追踪并拍摄长机，获得连续的包含长机的图像；

子步骤2，通过图像识别模块处理所述图像，并从每帧图像中都提取出长机的4个特征点的像素坐标；

子步骤3，根据所述长机的4个特征点的像素坐标及摄像机参数获得长机的状态估计。

其中，所述子步骤3包括如下亚子步骤：

亚子步骤1，通过长机特征点的像素坐标获得长机的旋转参数，

亚子步骤2，通过长机的旋转参数获得长机的姿态，

亚子步骤3，通过长机的姿态获得长机的加速度，

亚子步骤4，通过长机的加速度获得长机的实际位置和速度。

本发明所具有的有益效果包括：

(1)根据本发明提供的基于机载视觉的无人机抗干扰集群编队控制方法解决了无人机集群系统在面对复杂拒止环境时出现的卫星定位、机间通信、遥测链路受到干扰甚至损坏的导航控制问题，机载视觉的使用提高了无人机集群系统在完成任务时的抗干扰能力；

(2)根据本发明提供的基于机载视觉的无人机抗干扰集群编队控制方法中采用领航-跟随法和行为法的控制使得集群编队队形合理且能够稳定保持，采用动态死区法的约束使得编队形成及后续飞行过程中无人机的速度线性平滑变化，无明显波动，整体编队控制精度较高，从而使得僚机快速精准地飞到期望位置点。

附图说明

图1示出本发明基于机载视觉的无人机抗干扰集群编队控制方法的整体逻辑图；

图2示出根据本发明实施例1中无人机集群编队直线飞行仿真轨迹图；

图3示出根据本发明实施例1中僚机实际位置与期望位置间距离变化曲线；

图4示出根据本发明实施例1中飞行过程中僚机速度大小变化曲线；

图5示出根据本发明实施例2中无人机集群编队绕圆飞行仿真轨迹图；

图6示出根据本发明实施例2中僚机实际位置与期望位置间距离变化曲线图；

图7示出根据本发明实施例2中飞行过程中僚机速度大小变化曲线图。

具体实施方式

下面通过附图和实施例对本发明进一步详细说明。通过这些说明，本发明的特点和优点将变得更为清楚明确。

在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。

根据本发明提供的基于机载视觉的无人机抗干扰集群编队控制方法，所述集群编队包括一个长机和至少一个僚机，一般来说僚机数量都不止一个，根据所要执行任务的具体内容安排僚机数量，并且根据僚机数量设定合理的编队形式，如三角形、多边形、圆形等等；当集群编队中无人机数量为3个时，首选为三角形队列，当集群编队中无人机数量为4个或更多时，既可以设置为矩形等多边形队列，也可以设置为圆形队列，还可以继续设置三角形组合队列，该三角形组合队列中可以包含多个三角形，即第一个三角形中的两个僚机可以分别作为其他三角形队列的长机，依此类推，可以根据无人机数量设置对应的三角形数量。

具体来说，该方法包括如下步骤：

步骤1，僚机通过机载视觉系统识别长机，并获得自身相对于长机的位置和速度信息；即获得长机坐标系中僚机自身的位置信息和速度信息；

步骤2，控制僚机以平滑变化的速度到达集群编队中的期望位置，再保持与长机相同的速度跟随长机飞行。

优选地，在执行步骤2以前，所述僚机调整自身高度，使得僚机与长机处于同一水平高度上，再在该同一水平高度上执行步骤2。

其中，在步骤2中，利用领航-跟随法设计僚机在队形中的期望位置，结合基于行为法的飞行控制使僚机飞到期望点，利用动态死区法对各个无人机的速度进行约束使无人机的速度线性平滑变化，准确进入期望点，并稳定。

在僚机起飞后，首先基于视觉传感器对周围环境和无人机做识别和定位，寻找并确定长机位置，进而基于获取的长机位置信息和僚机中预存的编队信息，计算集群控制的编队参考点，即僚机在编队中的位置，或者称之为编队中僚机相对于长机的位置，确定之后再基于行为法建立僚机的运动模型，最后基于动态死区法来约束飞行过程中的速度变化，从而保证编队形成的稳定性，并保持该编队状态来执行任务。

通过长机的位置速度信息和领航-跟随法的编队队形设计，我们可以得到僚机的期望位置，因此需要控制其飞行到达该处，这种行为称作是驶向目标行为。

在实际控制无人机驶向目标时要求僚机能准确的进入编队中期望的位置点，并保持在该处，在僚机到达对应位置点时，我们希望它与长机之间保持相对静止。飞行过程中，在形成编队前僚机的速度会大于长机的速度，即产生追赶长机的行为，由于惯性的原因，如果不对末端速度加以约束便会导致僚机严重冲过期望点，因此为了使其可以准确地到达编队中的位置点并稳定保持，通过动态死区的思想对过程中的速度做分段约束。

具体来说，在步骤2中，通过下式(一)控制僚机的速度大小：

其中，V_fi表示僚机的速度大小；

V_max表示僚机所能达到的最大速度大小；

V_team表示集群编队的速度大小，即长机的速度大小；

d_i表示僚机与期望位置之间的距离，在步骤1中得到僚机与长机之间的相对位置关系，结合编队信息中期望位置与长机之间的位置关系实时求得所述d_i；

在所述僚机中存储有编队信息，即各个僚机相对于长机的位置关系；在实时获得长机位置以后，即可实时解算出所述期望位置。

R_c表示控制区半径，其取值为10～30米，优选为20米，该值是在集群编队起飞前设定好的，具体根据集群编队的数量、任务种类及编队无人机之间的距离设置；

R_d表示死区半径，表示编队行进过程中允许僚机偏离期望位置的误差量，一般取值为0.1～1米，优选限定为0.1米，即将僚机控制在期望位置附近0.1米处。所述控制区和死区都是以期望位置为圆形的圆形区域。

无人机在捕获到长机位置信息后，以最大速度朝向长机方向飞行，在进入到控制区后，其速度又将从外到内逐渐减小，准备进入死区，最终以与长机相同的速度前进并保持队形，过程中速度线性变化。

在步骤2中，通过下式(二)控制僚机的速度方向：

其中，V表示僚机在水平方向上的速度方向矢量，

V_x表示V在水平坐标系中X轴方向的分量，

V_y表示V在水平坐标系中Y轴方向的分量，

x′_i表示期望位置在水平坐标系中的X轴坐标，

y′_i表示期望位置在水平坐标系中的Y轴坐标，

x_i表示僚机在水平坐标系中的X轴坐标，

y_i表示僚机在水平坐标系中的Y轴坐标。

所述水平坐标系为二维坐标系，且所述长机位于该二维坐标系中，可以选取该二维平面中的任意方向作为X轴，与之垂直的方向即为Y轴。

在获得僚机的包含大小和方向矢量的速度后，控制该僚机以该速度飞行。

在一个优选的实施方式中，所述步骤1包括如下子步骤：

子步骤1，通过僚机上的摄像机追踪并拍摄长机，获得连续的包含长机的图像；

子步骤2，通过图像识别模块处理所述图像，并从每帧图像中都提取出长机的4个特征点的像素坐标；

子步骤3，根据所述长机的4个特征点的像素坐标及摄像机参数获得长机的状态估计。所述长机的状态估计包括长机的位置、长机的姿态和长机的速度。

在利用视觉传感器实现相对导航时，集群控制系统中无人机的位置信息完全由视觉传感器提供，通过视觉信息的分析结果控制僚机的飞行轨迹。所述长机上的特征点可以是预先涂装在长机上的特殊颜色区域，即在长机的电机位置涂装特殊颜色，以便于僚机能够更为快速准确地从图像中识别出电机的位置。

在一个优选的实施方式中，所述图像识别模块采用深度学习的方式进行编队无人机的检测和跟踪练习，优选地，采用对抗学习法，利用对抗生成网络(GAN)，使用无标注的数据进行对抗学习，不断提升网络的泛化能力，满足不确定场景下机器视觉的可靠识别和精确匹配要求，另外，采用该方法后，视觉系统可以在实际使用过程中不断获取各种场景下的无标注图像数据进行迭代优化训练，以提高识别性能。通过对抗生成网络的训练可以使集群编队中的僚机准确地借助机载视觉传感器将长机以及环境识别出来，并且获得包含长机的图像，进而从每帧图像中都提取出长机的4个特征点的像素坐标；所述四个特征点为长机上4个电机所在位置。

在一个优选的实施方式中，所述子步骤3包括如下亚子步骤：

亚子步骤1，通过长机特征点的像素坐标获得旋转矩阵，

亚子步骤2，通过旋转矩阵获得长机的姿态，

亚子步骤3，通过长机的姿态获得长机的加速度，

亚子步骤4，通过长机的加速度获得长机的实际位置和速度。

优选地，在所述亚子步骤1中，通过下式(三)获得目标无人机的旋转参数：

其中，R表示旋转矩阵，即用于从正交坐标系O_aX_aY_aZ_a到相机坐标系O_cX_cY_cZ_c做转换的3×3旋转矩阵，旋转矩阵中的9个参数也称之为旋转参数；

R′表示任意旋转矩阵，其第三列[r₇ r₈ r₉]^T等于旋转轴Za，且R′满足旋转矩阵的正交约束；

旋转轴

表示点P_i0指向点P_j0的向量，||P_i0P_j0||表示点P_i0指向点P_j0的向量的模；

通过步骤2中获得4个特征点的像素坐标后可以求解出其中两个点P_i0和点P_j0的深度，从而确定式(三)中的旋转轴Za，即[r₇ r₈ r₉]^T；

rot(Z，α)表示目标无人机绕Z轴的旋转角度为α；

c＝cosα，s＝sinα；

r₁到r₉分别表示任意3×3旋转矩阵R′中的每个元素，第三列[r₇ r₈ r₉]^T等于旋转轴Za。

在所述亚子步骤2中，通过下式(四)获得目标无人机的姿态：

其中，θ₁表示目标无人机的俯仰角，其取值范围为

θ₂表示目标无人机的俯仰角，当θ₁大于90°或小于-90度时无人机俯仰角通过θ₂表示，

ψ₁表示俯仰角为θ₁时对应求解得到的目标无人机的偏航角，

ψ₂表示俯仰角为θ₂时对应求解得到的目标无人机的偏航角，

φ₁表示俯仰角为θ₁时对应求解得到的目标无人机的滚转角，

φ₂表示俯仰角为θ₂时对应求解得到的目标无人机的滚转角，

R₃₁、R₃₂、R₃₃表示式(三)中求解得到的旋转矩阵R中第三行的三个元素，

R₂₁表示式(三)中求解得到的旋转矩阵R中第二行的第一个元素，

R₁₁表示式(三)中求解得到的旋转矩阵R中第一行的第一个元素；

a sin表示反正弦函数，a tan 2表示反正切函数。

长机姿态包括长机机体坐标系与惯性坐标的三个夹角，即滚转角、俯仰角和偏航角，通过上述式(四)即可以获得。

在所述亚子步骤3中，通过下式(五)获得目标无人机的加速度：

a＝[a_x,a_y,a_z]^T (五)

其中，a表示目标无人机的加速度，

a_x表示惯性坐标系中X轴方向的加速度分量，

a_y表示惯性坐标系中Y轴方向的加速度分量，

a_z表示竖直方向的加速度分量，a_z＝0

其中，g表示重力加速度；

θ表示式(四)中求解所得目标无人机的俯仰角，

表示式(四)中求解所得目标无人机的滚转角，

ψ表示式(四)中求解所得目标无人机的偏航角。

本申请中优选地设定长机在水平平面内保持稳定飞行。

在所述亚子步骤4中，通过下式(六)获得目标无人机的实际位置和速度，

其中，K_k代表卡尔曼增益，γ_k表示用于模拟间歇测量的二进制随机变量，如果目标无人机在第k帧图像中被检测出，则γ_k＝1，如果目标无人机在第k帧图像中未被检测出，则γ_k＝0；

w_k表示第k帧图像对应的过程噪声，w_k-1表示第k-1帧图像对应的过程噪声

表示基于k-1帧图像估计出的第k帧图像对应的状态量，

表示估计得到的最优的第k-1帧图像对应的状态量，

表示估计得到的最优的第k帧图像对应的状态量，

输出值

和

都可以表示为X，

Z_k表示第k帧图像对应的量测量，可表示为Z；

A表示过程矩阵，H表示观测矩阵；

P表示目标无人机的位置，v表示目标无人机的速度，a表示目标无人机的加速度，h表示图像的采样周期，优选为25Hz，I₃表示三维单位矩阵。

所述僚机上还设置有卫星接收装置和惯导系统，能够通过卫星接收装置接收卫星信号，从而实时获得较为准确的自身位置及速度信息，或者通过惯性导航实时获得自身位置及速度信息。在获得自身位置及速度信息的基础上，结合自身在长机坐标系中相对于长机的位置、速度、姿态信息，即可获得长机在地球坐标系中的位置和速度信息。

当卫星信号被屏蔽时，利用长机坐标系中的状态估计仍然能够通过式(一)和式(二)获得僚机的速度信息，从而控制僚机飞行。

实施例

选取三架5公斤级的四旋翼无人机组成集群编队，编队队形采用等边三角形的形式，一架长机和两架僚机分别位于三角形的三个顶点，等边三角形的边长为20米。

僚机上搭载ZED MINI双目深度相机，其稳定准确识别距离在50m以上，三架无人机的尺寸都不超过0.5m，其速度进行限幅在10m/s，集群编队的最大速度为V_max＝10m/s，集群编队整体的飞行速度为5m/s。

设置动态死区控制中的控制区半径为R_c＝20m，死区半径为R_d＝0.1m，僚机的速度通过式(一)和(二)进行控制：

其中，V_fi表示僚机的速度大小，V表示僚机在水平方向上的速度方向矢量；V_x和V_y表示僚机在水平方向上的速度分量，即表征速度的方向。

基于MATLAB/Simulink环境进行多无人机集群编队直线飞行和绕圆飞行的仿真，

实施例1，集群编队直线飞行仿真

长机和僚机都处于同一水平面上，建立水平方向的二维导航坐标系，设长机初始位置坐标为(20,20)m，僚机1的初始位置坐标为(-10,0)m，僚机2的初始位置坐标为(10,0)m，零时刻三架飞机均已起飞到对应坐标点处悬停，而后长机以V_team＝5m/s的速度沿着坐标系对角线做匀速直线运动，另外两架僚机中实时解算出其自身的期望位置，即三架无人机组成三角形的另外两个顶点位置，通过式(一)和式(二)对两架僚机进行控制。

仿真结果如图2、图3和图4所示，从图中不难看出，三架无人机可以形成预期的编队形状，领航-跟随法满足编队队形设计的要求，通过动态死区法成功控制僚机在形成队形过程中的速度线性平滑变化，并准确地进入编队控制算法计算得到的期望位置，多无人机编队控制在直线飞行仿真中基本不存在飞行误差。

实施例2，集群编队绕圆飞行仿真

建立水平方向的二维导航坐标系，设长机初始位置坐标为(0,17.32)m，僚机1的初始位置坐标为(-10,0)m，僚机2的初始位置坐标为(10,0)m，即零时刻三架旋翼无人机均已按照等边三角形进行编队，且起飞到对应坐标点处悬停，而后长机将从原点出发以V_team＝5m/s的速度绕原点做匀速圆周运动，航迹半径为400m，通过式(一)和式(二)对两架僚机进行控制。

仿真结果如图5、图6和图7所示，分析可得，三架旋翼无人机一直保持初始编队队形绕圆飞行，速度方向变化光滑无明显波动，且过程中集群编队保持稳定，控制误差较小，满足设计预期。

以上结合了优选的实施方式对本发明进行了说明，不过这些实施方式仅是范例性的，仅起到说明性的作用。在此基础上，可以对本发明进行多种替换和改进，这些均落入本发明的保护范围内。

Claims (8)

1.一种基于机载视觉的无人机抗干扰集群编队控制方法，其特征在于，所述集群编队包括一个长机和至少一个僚机，该方法包括如下步骤：

步骤1，僚机通过机载视觉系统识别长机，并获得自身相对于长机的位置和速度信息；

步骤2，控制僚机以平滑变化的速度到达集群编队中的期望位置，再保持与长机相同的速度跟随长机飞行。

2.根据权利要求1所述的基于机载视觉的无人机抗干扰集群编队控制方法，其特征在于，

在步骤2中，通过下式(一)控制僚机的速度大小：

其中，V_fi表示僚机的速度大小，

V_max表示僚机所能达到的最大速度大小，

V_team表示集群编队的速度大小，即长机的速度大小，

d_i表示僚机与期望位置之间的距离，

R_c表示控制区半径，

R_d表示死区半径。

3.根据权利要求1所述的基于机载视觉的无人机抗干扰集群编队控制方法，其特征在于，

在步骤2中，通过下式(二)控制僚机的速度方向：

其中，V表示僚机在水平方向上的速度方向矢量，

V_x表示V在水平坐标系中X轴方向的分量，

V_y表示V在水平坐标系中Y轴方向的分量，

x′_i表示期望位置在水平坐标系中的X轴坐标，

y′_i表示期望位置在水平坐标系中的Y轴坐标，

x_i表示僚机在水平坐标系中的X轴坐标，

y_i表示僚机在水平坐标系中的Y轴坐标。

4.根据权利要求2所述的基于机载视觉的无人机抗干扰集群编队控制方法，其特征在于，

所述控制区半径R_c取值为10～30米，所述死区半径R_d取值为0.1～1米。

5.根据权利要求2所述的基于机载视觉的无人机抗干扰集群编队控制方法，其特征在于，

在执行步骤2以前，所述僚机调整自身高度，使得僚机与长机处于同一水平高度上。

6.根据权利要求2所述的基于机载视觉的无人机抗干扰集群编队控制方法，其特征在于，

所述期望位置为集群编队中僚机所在位置，该期望位置由僚机实时解算得到。

7.根据权利要求1所述的基于机载视觉的无人机抗干扰集群编队控制方法，其特征在于，

所述步骤1包括如下子步骤：

子步骤1，通过僚机上的摄像机追踪并拍摄长机，获得连续的包含长机的图像；

子步骤2，通过图像识别模块处理所述图像，并从每帧图像中都提取出长机的4个特征点的像素坐标；

子步骤3，根据所述长机的4个特征点的像素坐标获得长机的状态估计。

8.根据权利要求7所述的基于机载视觉的无人机抗干扰集群编队控制方法，其特征在于，

所述子步骤3包括如下亚子步骤：

亚子步骤1，通过长机特征点的像素坐标获得旋转矩阵，

亚子步骤2，通过旋转矩阵获得长机的姿态，

亚子步骤3，通过长机的姿态获得长机的加速度，

亚子步骤4，通过长机的加速度获得长机的实际位置和速度。

Images