CN111207741B - 一种基于室内视觉vicon系统的无人机导航定位方法 - Google Patents

Abstract

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

Description

一种基于室内视觉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＝[q₁ q₂ q₃ q₄ v_x v_y v_z p_x p_y p_zgyro_bias_x gyro_bias_y gyro_bias_z acc_bias_x acc_bias_y acc_bias_z] (1)

其中：

[q₁ q₂ q₃ q₄]为姿态的四元素表示方法；

[v_x v_y v_z]为n系下的三轴速度；

[p_x p_y p_z]为n系下的三轴位置；

[gyro_bias_x gyro_bias_y gyro_bias_z]为陀螺仪三轴零偏；

[acc_bias_x acc_bias_y acc_bias_z]为加速度计三轴零偏。

进一步的，序贯滤波：

Step1：状态一步预测：使用无人机自身的惯导设备进行一步状态预测，状态预测包括所有状态的一步预测：

姿态四元素的更新：

其中

表示t_m时刻的姿态变换四元素，

是从t_m-1时刻到t_m时刻姿态四元素变化，Δθ是陀螺在时间段[t_m-1,t_m]内输出的角增量并且Δθ＝|Δθ|，mems低精度陀螺仪采用角增量输出采样的方式，只需将其乘以采样间隔时间T_s即可变换为角增量；

速度更新

对于低中速v＜100m/s行驶的运载体忽略地球自转以及地球曲率的影响，速度更新简化方程为

其中

为t_m时刻的惯导速度，

为与四元素

对应的姿态阵，Δv_m是加速度计在时间段[t_m-1,t_m]时间内输出的比力增量，实际中采用比例乘以采样间隔进行近似，式中的g为重力加速度矢量；

位置更新

陀螺bias更新

向量

为m时刻的陀螺bias更新值，

为m-1时刻的陀螺bias值；

加计bias更新

根据上述状态更新方程，求取状态转移矩阵记为F；

Step2：误差协方差矩阵更新

P_k/k-1＝F_k/k-1P_k-1/k-1F_k/k-1+G_k-1Q_k-1G_k-1+Q_s

上式中

为状态jacobian矩阵和控制jacobian矩阵；

Step3：计算kalman滤波增益

Step4：状态更新

Vicon系统获取得到的无人机的姿态和位置信息作为量测信息，并且对滤波状态进行更新；

Step5：误差协方差矩阵更新以及状态反馈

P_k＝(I-K_kH_k)P_k/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系统发送的姿态和位置信息结合自身的惯导设备进行组合导航滤波可以得到精确平滑的姿态和位置数据，便于上层任务(控制任务，编队协同任务，人工智能算法任务等先进算法)进行调用。无人机自身的遥测信息可以通过数据链下传到计算机，以便研究人员实时掌握无人机的运动状态。

本发明提出的基于Vicon+INS组合导航算法，可以实现无人机在室内的高精度的姿态和位置精度；在室内情况下对无人机运动速度的估计；降低对数据链路的要求；使研究人员更专注于上层算法的研究。

附图说明

图1是vicon系统的数据流向示意图；

图2是序贯滤波示意图；

图3是高度切换策略示意图；

具体实施方式

以下结合附图对本发明进一步说明：

请参阅图1至图3，下面结合附图和具体实施方式对发明和专利进行详细说明。

本专利的结构示意图如图所示，本专利的具体实现过程包含两个步骤：

第一步，启动Vicon系统的SDK，并通过无线数据链路将Vicon系统解析得到的以无人机为运动载体的位置和姿态数据，位置包括在Vicon系统局部坐标系下的三轴位置，姿态包括无人机的滚转，俯仰以及Vicon系统的X轴与无人机自身坐标系(前右下)的X轴之间的航向信息；

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

第一部分：

VICON系统数据解析以及数据传输部分：

启动Vicon系统的SDK应用程序，此时将会输出在Vicon系统视场内的所有无人机的姿态和位置数据，通过解析这些数据，并且设计协议，打包后一并通过一对多数传广播发送给所有无人机，无人机上的数据链接收到数据后，并与自身的ID做对比，从数据中解析得到Vicon系统观测的基于Vicon系统坐标系的位置和姿态信息。Vicon系统上传数据给无人机时，使用840M数据链，波特率设置为115200。Vicon系统的姿态数据传输见图1所示，

第二部分：

无人机自身位置解算部分：

因为无人机自身携带惯导设备(陀螺和加计)，以Vicon系统发送的无人机的姿态和位置数据作为量测，通过序贯kalman滤波的方式进行组合导航算法。进而输出更高频率，高精度的无人机自身位置和姿态数据。

①坐标系定义

选取Vicon系统自身定义的坐标系为导航坐标系(固定参考坐标系)即为n系；无人机重心为原点的前右下坐标系为机体坐标系(动坐标系)，即为b系；

②状态方程

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

X＝[q₁ q₂ q₃ q₄ v_x v_y v_z p_x p_y p_zgyro_bias_x gyro_bias_y gyro_bias_z acc_bias_x acc_bias_y acc_bias_z] (1)

其中：

[q₁ q₂ q₃ q₄]为姿态的四元素表示方法；

[v_x v_y v_z]为n系下的三轴速度；

[p_x p_y p_z]为n系下的三轴位置；

[gyro_bias_x gyro_bias_y gyro_bias_z]为陀螺仪三轴零偏；

[acc_bias_x acc_bias_y acc_bias_z]为加速度计三轴零偏。

③序贯滤波

状态一步预测

使用无人机自身的惯导设备(陀螺仪和加计)进行一步状态预测，状态预测包括所有状态的一步预测：

姿态四元素的更新：

其中

表示t_m时刻的姿态变换四元素，

是从t_m-1时刻到t_m时刻姿态四元素变化，Δθ是陀螺在时间段[t_m-1，t_m]内输出的角增量并且Δθ＝|Δθ|，mems低精度陀螺仪一般采用角增量输出采样的方式，只需要简单地将其乘以采样间隔时间T_s即可变换为角增量。

速度更新

对于低中速行驶的运载体，例如地速v＜100m/s，可以忽略地球自转以及地球曲率的影响，速度更新简化方程为

其中

为t_m时刻的惯导速度，

为与四元素

对应的姿态阵，Δv_m是加速度计在时间段[t_m-1，t_m]时间内输出的比力增量，实际中可以采用比例乘以采样间隔进行近似，式中的g为重力加速度矢量。

位置更新

(4)陀螺bias更新

向量

为m时刻的陀螺bias更新值，

为m-1时刻的陀螺bias值。

(5)加计bias更新

根据(1)至(5)状态更新方程，求取状态转移矩阵记为F。

误差协方差矩阵更新

P_k/k-1＝F_k/k-1P_k-1/k-1F_k/k-1+G_k-1Q_k-1G_k-1+Q_s

上式中

为状态jacobian矩阵和控制jacobian矩阵。

计算kalman滤波增益

状态更新

Vicon系统获取得到的无人机的姿态和位置信息作为量测信息，并且对滤波状态进行更新。

误差协方差矩阵更新以及状态反馈

P_k＝(I-K_kH_k)P_k/k-1

完成一次滤波状态和协方差矩阵更新后，滤波器可以得到陀螺和加计的bias估计值，在进行下一轮的状态更新之前可以见对采样得到角增量和速度增量进行状态修正(或者称之为状态反馈)，以此来对IMU数据进行修正。序贯滤波的方式可以见下图2所示。

④高度切换

由于无人机在实时的使用过程中会出现飞到Vicon系统镜头盲区的情况，此时Vicon系统镜头捕获不到无人机，那么对无人机的位置和姿态输出回出现数据输出为零的情况，这种情况如果不进行处理，那么无人机会出现无法精确定位自身位置和姿态的情况，这时如果能组合导航算法高度测量参考切换到无人机上的气压高度，结合无人机上的陀螺仪和加计，位置自身的高度，roll和pitch，避免无人机炸机。具体的做法为：

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；

根据以上1～4步骤可以在气压高度以及vicon测量数据高度数据之间进行切换，保持无人机高度方向稳定，示意图见图3。

Claims (7)

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系。

6.根据权利要求5所述的一种基于室内视觉vicon系统的无人机导航定位方法，其特征在于，建立状态方程：

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

X＝[q₁ q₂ q₃ q₄ v_x v_y v_z p_x p_y p_zgyro_bias_x gyro_bias_y gyro_bias_z acc_bias_xacc_bias_y acc_bias_z] (1)

其中：

[q₁ q₂ q₃ q₄]为姿态的四元素表示方法；

[v_x v_y v_z]为n系下的三轴速度；

[p_x p_y p_z]为n系下的三轴位置；

[gyro_bias_x gyro_bias_y gyro_bias_z]为陀螺仪三轴零偏；

[acc_bias_x acc_bias_y acc_bias_z]为加速度计三轴零偏。

7.根据权利要求1所述的一种基于室内视觉vicon系统的无人机导航定位方法，其特征在于，序贯滤波：

Step1：状态一步预测：使用无人机自身的惯导设备进行一步状态预测，状态预测包括所有状态的一步预测：

姿态四元素的更新：

其中

表示t_m时刻的姿态变换四元素，

是从t_m-1时刻到t_m时刻姿态四元素变化，Δθ是陀螺在时间段[t_m-1,t_m]内输出的角增量并且Δθ＝|Δθ|，mems低精度陀螺仪采用角增量输出采样的方式，只需将其乘以采样间隔时间T_s即可变换为角增量；

速度更新

对于低中速v＜100m/s行驶的运载体忽略地球自转以及地球曲率的影响，速度更新简化方程为

其中

为t_m时刻的惯导速度，

为与四元素

对应的姿态阵，Δv_m是加速度计在时间段[t_m-1,t_m]时间内输出的比力增量，实际中采用比例乘以采样间隔进行近似，式中的g为重力加速度矢量；

位置更新

陀螺bias更新

向量

为m时刻的陀螺bias更新值，

为m-1时刻的陀螺bias值；

加速度计bias更新

根据上述状态更新方程，求取状态转移矩阵记为F；

Step2：误差协方差矩阵更新

P_k/k-1＝F_k/k-1P_k-1/k-1F_k/k-1+G_k-1Q_k-1G_k-1+Q_s

上式中

为状态jacobian矩阵和控制jacobian矩阵；

Step3：计算kalman滤波增益

Step4：状态更新

Vicon系统获取得到的无人机的姿态和位置信息作为量测信息，并且对滤波状态进行更新；

Step5：误差协方差矩阵更新以及状态反馈

P_k＝(I-K_kH_k)P_k/k-1

完成一次滤波状态和协方差矩阵更新后，滤波器得到陀螺和加速度计的bias估计值，在进行下一轮的状态更新之前见对采样得到角增量和速度增量进行状态修正，以此来对IMU数据进行修正。

Images