[发明专利]一种基于室内视觉vicon系统的无人机导航定位方法

说明书

一种基于室内视觉vicon系统的无人机导航定位方法，包括以下步骤：步骤1，启动Vicon系统的SDK，并通过无线数据链路将Vicon系统解析得到的以无人机为运动载体的位置和姿态数据；步骤2，无人机接收到Vicon系统通过无线数据链路上传的姿态和位置数据，并以此作为组合导航算法的测量量结合无人机上的惯导设备，进行组合导航，实时输出无人机的位置，速度和姿态信息。本发明提出的基于Vicon+INS组合导航算法，可以实现无人机在室内的高精度的姿态和位置精度；在室内情况下对无人机运动速度的估计；降低对数据链路的要求；使研究人员更专注于上层算法的研究。

技术领域

本发明属于无人机应用技术领域，特别涉及一种基于室内视觉vicon系统的无人机导航定位方法。

背景技术

为了便于研究无人机集群算法，编队协同算法，人工智能算法，智能控制算法的研究，搭建了室内定位系统实验室，以无人机作为被控对象进行研究。

由于无人机在室内进行飞行，无法接收GPS信号，因此需要使用外部的定位系统。基于视觉的英国Vicon系统是一种很好的方案。Vicon系统定位精度高，数据更新频率快，能实时反应以无人机作为运动载体的位置和姿态信息，Vicon系统的延时小于2.5ms(最大刷新率400Hz)，但是受限于目前民用无人机数据链硬件的原因，作为运动载体的无人机无法接受数据链刷这么快且数据链大的数据。

发明内容

本发明的目的在于提供一种基于室内视觉vicon系统的无人机导航定位方法，以解决上述问题。

为实现上述目的，本发明采用以下技术方案：

一种基于室内视觉vicon系统的无人机导航定位方法，包括以下步骤：

步骤1，启动Vicon系统的SDK，并通过无线数据链路将Vicon系统解析得到的以无人机为运动载体的位置和姿态数据；

步骤2，无人机接收到Vicon系统通过无线数据链路上传的姿态和位置数据，并以此作为组合导航算法的测量量结合无人机上的惯导设备，进行组合导航，实时输出无人机的位置，速度和姿态信息。

进一步的，步骤1中，位置包括在Vicon系统局部坐标系下的三轴位置，姿态包括无人机的滚转，俯仰以及Vicon系统的X轴与无人机自身坐标系的X轴之间的航向信息。

进一步的，步骤1中具体的：启动Vicon系统的SDK应用程序，此时将会输出在Vicon系统视场内的所有无人机的姿态和位置数据，通过解析这些数据，打包后一并通过一对多数传广播发送给所有无人机，无人机上的数据链接收到数据后，并与自身的ID做对比，从数据中解析得到Vicon系统观测的基于Vicon系统坐标系的位置和姿态信息。

进一步的，步骤1中Vicon系统上传数据给无人机时，使用840M数据链，波特率设置为115200。

进一步的，步骤2中，以Vicon系统发送的无人机的姿态和位置数据作为量测，通过序贯kalman滤波的方式进行组合导航算法，具体的包括以下步骤：

1)坐标系定义；

2)建立状态方程；

3)序贯滤波；

4)无人机高度切换。

进一步的，坐标系定义：选取Vicon系统自身定义的坐标系为导航坐标系即为n系；无人机重心为原点的前右下坐标系为机体坐标系，即为b系。

进一步的，建立状态方程：

选取的n系下的姿态，速度，位置，加速度计零偏，陀螺仪零偏作为组合导航算法的状态，共16维状态方程，即：