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

权利要求书

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

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

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

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

1)坐标系定义；

2)建立状态方程；

3)序贯滤波；

4)无人机高度切换；

高度切换具体的做法为：

(1)如果是Vicon系统测量数据作为组合导航高度测量vicon_height，持续计算无人机上的气压计与组合导航输出值之间的偏差baro_hgt_offset；

(2)判断如果500ms内，无法接收到vicon系统的数据信息，那么迅速切换到气压高度baro_offset；

(3)切换成气压高度作为组合导航高度测量后，后续所有气压测量值baro_height减去baro_hgt_offset，以减去baro_hgt_offset作为组合导航的高度输入量，并且实时监测是否有Vicon系统的量测数据；

(4)如果检测到有Vicon系统的量测数据，计算组合导航输出高度与Vicon系统测量的高度之间的偏差值vicon_hgt_offset，计算不少于5次的vicon_hgt_offset并做平均作为最终的vicon_hgt_offset,而后切换成vicon的测量数据作为组合导航高度参考信息，并且后续所有的vicon测量的高度值减去vicon_hgt_offset；

根据以上步骤，在气压高度以及vicon测量数据高度数据之间进行切换，保持无人机高度方向稳定。

2.根据权利要求1所述的一种基于室内视觉vicon系统的无人机导航定位方法，其特征在于，步骤1中，位置包括在Vicon系统局部坐标系下的三轴位置，姿态包括无人机的滚转，俯仰以及Vicon系统的X轴与无人机自身坐标系的X轴之间的航向信息。

3.根据权利要求1所述的一种基于室内视觉vicon系统的无人机导航定位方法，其特征在于，步骤1中具体的：启动Vicon系统的SDK应用程序，此时将会输出在Vicon系统视场内的所有无人机的姿态和位置数据，通过解析这些数据，打包后一并通过一对多数传广播发送给所有无人机，无人机上的数据链接收到数据后，并与自身的ID做对比，从数据中解析得到Vicon系统观测的基于Vicon系统坐标系的位置和姿态信息。

4.根据权利要求1所述的一种基于室内视觉vicon系统的无人机导航定位方法，其特征在于，步骤1中Vicon系统上传数据给无人机时，使用840M数据链，波特率设置为115200Bd。

5.根据权利要求1所述的一种基于室内视觉vicon系统的无人机导航定位方法，其特征在于，坐标系定义：选取Vicon系统自身定义的坐标系为导航坐标系即为n系；无人机重心为原点的前右下坐标系为机体坐标系，即为b系。