一种基于室内视觉vicon系统的无人机导航定位方法技术方案

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

A navigation and positioning method of UAV Based on indoor vision Vicon system

【技术实现步骤摘要】
一种基于室内视觉vicon系统的无人机导航定位方法
本专利技术属于无人机应用
，特别涉及一种基于室内视觉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维状态方程，即：X＝[q1q2q3q4vxvyvzpxpypzgyro_biasxgyro_biasygyro_biaszacc_biasxacc_biasyacc_biasz](1)其中：[q1q2q3q4]为姿态的四元素表示方法；[vxvyvz]为n系下的三轴速度；[pxpypz]为n系下的三轴位置；[gyro_biasxgyro_biasygyro_biasz]为陀螺仪三轴零偏；[acc_biasxacc_biasyacc_biasz]为加速度计三轴零偏。进一步的，序贯滤波：Step1：状态一步预测：使用无人机自身的惯导设备进行一步状态预测，状态预测包括所有状态的一步预测：姿态四元素的更新：其中表示tm时刻的姿态变换四元素，是从tm-1时刻到tm时刻姿态四元素变化，Δθ是陀螺在时间段[tm-1,tm]内输出的角增量并且Δθ＝|Δθ|，mems低精度陀螺仪采用角增量输出采样的方式，只需将其乘以采样间隔时间Ts即可变换为角增量；速度更新对于低中速v<100m/s行驶的运载体忽略地球自转以及地球曲率的影响，速度更新简化方程为其中为tm时刻的惯导速度，为与四元素对应的姿态阵，Δvm是加速度计在时间段[tm-1,tm]时间内输出的比力增量，实际中采用比例乘以采样间隔进行近似，式中的g为重力加速度矢量；位置更新陀螺bias更新向量为m时刻的陀螺bias更新值，为m-1时刻的陀螺bias值；加计bias更新根据上述状态更新方程，求取状态转移矩阵记为F；Step2：误差协方差矩阵更新Pk/k-1＝Fk/k-1Pk-1/k-1Fk/k-1+Gk-1Qk-1Gk-1+Qs上式中为状态jacobian矩阵和控制jacobian矩阵；Step3：计算kalman滤波增益Step4：状态更新Vicon系统获取得到的无人机的姿态和位置信息作为量测信息，并且对滤波状态进行更新；Step5：误差协方差矩阵更新以及状态反馈Pk＝(I-KkHk)Pk/k-1完成一次滤波状态和协方差矩阵更新后，滤波器得到陀螺和加计的bias估计值，在进行下一轮的状态更新之前见对采样得到角增量和速度增量进行状态修正，以此来对IMU数据进行修正。进一步的，高度切换具体的做法为：(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测量数据高度数据之间进行切换，保持无人机高度方向稳定。与现有技术相比，本专利技术有以下技术效果：本专利技术Vicon系统的摄像头捕获到视场内带反射球的无人机运动载体，将视觉信息经过专用路由器发送到高性能计算机，经过计算机处理输出视场内无人机的位置和姿态信息，无人机接受vicon系统发送的姿态和位置信息结合自身的惯导设备进行组合导航滤波可以得到精确平滑的姿态和位置数据，便于上层任务(控制任务，编队协同任务，人工智能算法任务等先进算法)进行本文档来自技高网...

【技术保护点】
1.一种基于室内视觉vicon系统的无人机导航定位方法，其特征在于，包括以下步骤：/n步骤1，启动Vicon系统的SDK，并通过无线数据链路将Vicon系统解析得到的以无人机为运动载体的位置和姿态数据；/n步骤2，无人机接收到Vicon系统通过无线数据链路上传的姿态和位置数据，并以此作为组合导航算法的测量量结合无人机上的惯导设备，进行组合导航，实时输出无人机的位置，速度和姿态信息。/n

【技术特征摘要】
1.一种基于室内视觉vicon系统的无人机导航定位方法，其特征在于，包括以下步骤：
步骤1，启动Vicon系统的SDK，并通过无线数据链路将Vicon系统解析得到的以无人机为运动载体的位置和姿态数据；
步骤2，无人机接收到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数据链，波特率设置为115200。


5.根据权利要求1所述的一种基于室内视觉vicon系统的无人机导航定位方法，其特征在于，步骤2中，以Vicon系统发送的无人机的姿态和位置数据作为量测，通过序贯kalman滤波的方式进行组合导航算法，具体的包括以下步骤：
1)坐标系定义；
2)建立状态方程；
3)序贯滤波；
4)无人机高度切换。


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


7.根据权利要求6所述的一种基于室内视觉vicon系统的无人机导航定位方法，其特征在于，建立状态方程：
选取的n系下的姿态，速度，位置，加速度计零偏，陀螺仪零偏作为组合导航算法的状态，共16维状态方程，即：
X＝[q1q2q3q4vxvyvzpxpypzgyro_biasxgyro_biasygyro_biaszacc_biasxacc_biasyacc_biasz](1)
其中：
[q1q2q3q4]为姿态的四元素表示方法；
[vxvyvz]为n系下的三轴速度；
[pxpypz]为n系下的三轴位置；
[gyro_biasxgyro_biasygyro_biasz]为陀螺仪三轴零偏；
[acc_biasxacc_biasyacc_biasz]为加速度计三轴零偏。


8.根据权利要求5所述的一种基于室内视觉vicon系统的无人机导航定位方法，其特征在于，序贯滤波：
Step1：状态一步预测：使用无人机自身的惯导设备进行一步状态预测，状态预测包括所有状态的一步预测：

【专利技术属性】
技术研发人员：李定涌，闫永驰，陈朋印，
申请(专利权)人：西安因诺航空科技有限公司，
类型：发明
国别省市：陕西;61