CN114383612A - 一种视觉辅助惯性差分位姿测量系统 - Google Patents

Abstract

本发明涉及一种视觉辅助惯性差分位姿测量系统，包括：测量单元，以及与测量单元连接的运动载具IMU、被测物体IMU和视觉单元；运动载具IMU和被测物体IMU用于获取运动载具和被测物体的运动信息；视觉单元用于获取被测物体相对于运动载具的视觉位姿数据；测量单元，接收运动载具和被测物体的运动信息，进行双IMU差分获得的载体坐标系下被测物体相对载体的惯性位姿数据；接收视觉位姿数据；并以视觉位姿数据为外部观测量对包括惯性位姿数据在内的状态数据进行卡尔曼滤波，输出滤波后的被测物体与运动载具的相对位姿数据。本发明充分利用惯性测姿采样频率高、短时精度高，视觉测姿无累积误差的特性，实现高精度的位姿追踪。

Description

一种视觉辅助惯性差分位姿测量系统

技术领域

本发明涉及位姿测量和卡尔曼滤波技术领域，尤其涉及一种视觉辅助惯性差分位姿测量系统。

背景技术

随着现代装备的智能化升级，包括被测物体在运动载具中位姿信息获取提出了新的需求。例如头戴显示功能对运动载具中乘员姿态位姿信息。这类装备需要获取运动载具中操控人员与运动载具实时准确的相对姿态变化，将乘员头部动作与装备操控结合，实现人机一体的自然交互方式，从而实现装备快速协同、信息互联等功能，提升战场效率。相对位姿测量相比于惯性系下绝对姿态测量存在的主要差别为：其一，在载具相对惯性系存在未知运动的条件下，被测目标的单一惯性信息也不可作为直接参考，需要同载具的惯性信息差分处理，通过转换到载具坐标系下进行相对姿态解算；其二，卫星导航与惯性导航这种常用的组合导航手段不再可用，需要使用卫星导航、视觉导航、雷达导航等多种导航方式分别对运动载具上与被测物体上两个IMU的零偏分别进行修正。

在目前常用的相对位姿测量手段中，惯性与视觉组合是一种可靠的相对姿态测量方式。惯性测姿的短时精度较高，长时积分会产生累积误差，而视觉测姿虽然易受外部近似特征干扰及光线变化影响，但特征准确时精度稳定，在测姿精度上与惯性测量形成互补。从测量频率上，惯性信息可实现高达1kHz采样的实时测量，图像信息通常为60Hz或120Hz，且存在稳定解算延迟，惯性与视觉的融合可实现与纯惯性相同的高频率输出。惯性与视觉的组合可充分利用惯性测量采样周期短、短时精度高的特性，并发挥视觉测量可直接获得无累积误差相对姿态的特点。

现有的视觉观测与惯性组合的形式，缺少足够的观测信息，且无法同时抑制两个惯性器件漂移，导致导航系统积分发散。

发明内容

鉴于上述的分析，本发明旨在提供一种视觉辅助惯性差分位姿测量系统，提升相对位姿测量结果的准确性。

本发明提供的技术方案是：

本发明公开了一种视觉辅助惯性差分位姿测量系统，包括：测量单元，以及与测量单元连接的运动载具IMU、被测物体IMU和视觉单元；

运动载具承载被测物体，所述运动载具IMU和被测物体IMU分别布置于运动载具和被测物体上，用于获取运动载具和被测物体的运动信息；

所述视觉单元布置于运动载具内，用于获取被测物体相对于运动载具的视觉位姿数据；

所述测量单元，接收所述运动载具和被测物体的运动信息，进行双IMU差分获得的载体坐标系下被测物体相对运动载体的惯性位姿数据；接收所述视觉位姿数据；并以视觉位姿数据为外部观测量对包括惯性位姿数据在内的状态数据进行卡尔曼滤波，输出滤波后的被测物体与运动载具的相对位姿数据。

进一步地，所述视觉单元包括第一视觉模块和/或第二视觉模块；

所述第一视觉模块的第一相机模组安装于运动载具上，与第一相机配合的第一标志点布置于被测物体内；第一相机模组测量被测物体上布置的第一标志点，获得被测物体相对运动载具的第一视觉位姿数据；

所述第二视觉模块的第二相机模组安装于被测物体上，与第二相机模组配合的第二标志点布置于运动载具上；第二相机模组测量运动载具上布置的第二标志点，通过坐标系转换获得被测物体相对运动载具的第二视觉位姿数据。

进一步地，所述运动载具IMU与第一相机模组固连，所述被测物体IMU与第二相机模组固连。

进一步地，所述第一标志点在被测物体上进行布局，保证在被测物体在其运动范围内，使所述第一标志点处于第一相机模组的视觉范围内；所述第一标志点的数量大于等于4；

所述第二标志点在运动载具内的布局，保证在固定角度内，使所述第二标志点处于第二相机模组的视觉范围内；所述第二标志点的数量大于等于4。

进一步地，所述第一标志点和第二标志点使用红外发光点的方式或红外发光二维码的方式；所述第一相机模组和第二相机模组采用单目、双目或结构光的方式。

进一步地，所述第一标志点和第二标志点采用同频闪烁的方式与对应的相机模组同步。

进一步地，所述卡尔曼滤波中的状态向量：

X＝[p_h-p,v_h-p,q_h-p,b_ha,b_pa,b_hω,b_pω]^T；

其中，p_h-p、v_h-p、q_h-p定义为双IMU差分获得的载体坐标系下被测物体相对载体的位置、速度、旋转四元数；b_hω、b_ha为被测物体IMU测量角速度与加速度的零偏；b_pω、b_pa为运动载具IMU测量角速度与加速度的零偏。

进一步地，所述运动载具IMU测量角速度与加速度的零偏为经过修正过的零偏数据。

进一步地，所述旋转四元数q_h-p采用角速度差分法进行求解；

在角速度差分法中，通过对当前时刻被测物体与运动载具的相对角速度进行积分得到被测物体与运动载具的相对位姿。

进一步地，所述角速度差分法，包括：

获取被测物体坐标系到运动载体坐标系的相对旋转矩阵；

根据被测物体与运动载体的IMU获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量；

使用当前时刻相对角增量计算相对旋转四元数得到被测物体相对于运动载体的位姿数据。

本发明的可以至少实现以下有益效果之一：

本发明提出的视觉辅助惯性差分位姿测量系统，通过视觉辅助获取被测物体与运动载具的相对位姿关系，对进行惯性积分差分的位姿测量结果进行修正，得到准确的位姿测量结果。这种方法充分利用惯性测姿采样频率高、短时精度高，视觉测姿无累积误差的特性，实现高精度的位姿追踪。该方法相较于传统相对位姿测量方案，更为有效调用各种导航方式特性，且布置简便，适用于乘用车辆、战斗机、武装直升机中头戴显示系统等工程应用。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1为本发明实施例中的觉辅助惯性差分位姿测量系统组成框图。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。

本实施例所公开的一种视觉辅助惯性差分位姿测量系统，如图1所示，包括：测量单元，与测量单元连接的运动载具IMU、被测物体IMU和视觉单元；

所述运动载具IMU和被测物体IMU分别布置于运动载具和运动载具内的被测物体上，用于获取运动载具和被测物体的运动信息；

所述视觉单元布置于运动载具内，用于获取被测物体相对于运动载具的视觉位姿数据；

所述测量单元，接收运动载具和被测物体的运动信息，进行双IMU差分获得的载体坐标系下被测物体相对载体的惯性位姿数据；接收所述视觉位姿数据；并以视觉位姿数据为外部观测量对包括惯性位姿数据在内的状态数据进行卡尔曼滤波，输出滤波后的被测物体与运动载具的相对位姿数据。

具体的，所述视觉单元包括第一视觉模块和/或第二视觉模块；

所述第一视觉模块的第一相机模组安装于运动载具上，与第一相机配合的第一标志点布置于被测物体内；第一相机模组测量被测物体上布置的第一标志点，获得被测物体相对运动载具的第一视觉位姿数据；

所述第一标志点在被测物体上的布局，保证在被测物体在较大范围运动的情况下，仍使所述第一标志点处于第一相机模组的视觉范围内，所述第一标志点的数量大于等于4；

所述第一视觉模块的优势在于，被测物体带动标志点靶标运动时，靶标始终不会超出相机视野，视觉测量系统能够全时段工作。

所述第二视觉模块的第二相机模组安装于被测物体上，与第二相机模组配合的第二标志点布置于运动载具上；第二相机模组测量运动载具上布置的第二标志点，通过坐标系转换获得被测物体相对运动载具的第二视觉位姿数据。

所述第二标志点在运动载具内的布局，保证在固定角度内，使所述第二标志点处于第二相机模组的视觉范围内，所述第二标志点的数量大于等于4。

所述第二视觉模块的优势在于，标志点靶标可以在相机视野内占有更大面积，因此视觉相对测量精度也更高，同时这种布置方式还带来了另外一种优势，即多套测量系统的多个相机可共享同一套标志点靶标，保证各套系统工作互不干扰。但缺点在于，在被测物体运动过程中，一旦标志点靶标超出相机视野，视觉测量将无法工作。

优选的，所述第一标志点和第二标志点使用红外发光点的方式或红外发光二维码的方式；

所述第一相机模组和第二相机模组采用单目、双目或结构光方式。只要能准确捕获红外标志点的图像信息即可。

更优选的，所述第一标志点和第二标志点采用同频闪烁的方式与对应的相机模组同步。通过图像处理提取红外点位置，避免外部自然光红外波段的光线干扰。

本实施例中的优选方案中采用同时包括第一视觉模块和第二视觉模块的视觉单元，综合了以上两者，既使用了布置于运动载具上的相机模组也使用了布置于被测物体上的相机模组，保证了测量精度的同时保证了较大的测量范围。

由于第一视觉模块和第二视觉模块测量的视觉位姿数据是用来对双IMU差分获得的载体坐标系下被测物体相对载体的惯性位姿数据进行修正的，因此为了减少标定工作量，在进行布置时，所述运动载具IMU与第一相机模组固连，所述被测物体IMU与第二相机模组固连。

具体的，所述测量单元中的卡尔曼滤波的状态向量

X＝[p_h-p,v_h-p,q_h-p,b_ha,b_pa,b_hω,b_pω]^T；

其中，p_h-p、v_h-p、q_h-p定义为双IMU差分获得的载体坐标系下被测物体相对运动载体的位置、速度、旋转四元数；b_hω、b_ha为被测物体IMU测量角速度与加速度的零偏；b_pω、b_pa为运动载具IMU测量角速度与加速度的零偏。

其中，载体坐标系下被测物体相对载体的位置p_h-p、速度v_h-p可采用现有的技术方法获得，在此就不赘述。

优选的，所述旋转四元数q_h-p采用角速度差分法进行求解；

在角速度差分法中，通过对当前时刻被测物体与运动载具的相对角速度进行积分得到被测物体与运动载具的相对位姿。

进一步地，所述角速度差分法，包括：

1)获取被测物体坐标系到运动载具坐标系的相对旋转矩阵R_rel；

被测物体坐标系到运动载具坐标系的相对旋转矩阵R_rel可通过对被测物体和运动载具的标定获得。

2)根据被测物体与运动载具的IMU获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量；

根据被测物体与运动载具的IMU获取角速度信息ω_h(t)、ω_p(t)与t_k-1时刻相对旋转矩阵

计算当前时刻t的相对角增量为：

3)使用当前时刻相对角增量计算相对旋转四元数得到被测物体相对于运动载具的位姿数据。

用当前时刻相对角增量

计算相对旋转四元数

为

相对旋转四元数

即为旋转四元数q_h-p。

更优选的方案中，为了提高测量的精度，状态向量中的运动载具IMU的角速度与加速度的零偏作为b_pω、b_pa可以采用经过修正过的零偏数据。

具体的修正可采用卫星导航系统或里程计以及高度计等测量系统。

优选的，通过卫星导航对运动载具IMU的角速度与加速度的零偏进行修正的方案中，

系统还包括卫星导航单元；该卫星导航单元为卫星导航载波相位差分系统；

其天线布置于运动载具外接收卫星信号；处理器布置于运动载具内，进行地理坐标系中的运动载具运动数据测量。

更优选的，根据所述运动载具的运动数据对运动载具IMU测量角速度与加速度的零偏进行修正过程包括两种方式：

方式一、构建运动载具在地理坐标系运动的状态方程，建立卡尔曼滤波器，利用卫星导航单元测量的数据生成运动载具在地理坐标系中的观测量，对载具在地理坐标系运动状态方程的零偏进行更新，得到运动载具IMU的零偏；

方式二、构建运动载具和卫星导航单元深组合状态方程，建立卡尔曼滤波器，将运动载具IMU介入到卫星导航的卫星捕获环路，通过滤波器得到运动载具IMU的零偏。可采用现有的深组合状态方程进行滤波处理，在本实施例中并不需要做出具体的限定。

在测量单元中，忽略杆臂效应，卡尔曼滤波的状态方程可以用下式表示：

式中，

为相对旋转矩阵；a_h、b_ha和n_ha为被测物体IMU加速度、加速度零偏和加速度噪声；a_p、b_pa、n_pa为运动载具IMU加速度、加速度零偏和加速度噪声；ω_h、b_hω、n_hω为被测物体IMU角速度、角速度零偏和角速度噪声，ω_p、b_pω、n_pω为运动载具IMU角速度、角速度零偏和角速度噪声；

为被测物体IMU加速度零偏噪声和角速度零偏噪声；

为运动载具IMU加速度零偏噪声和角速度零偏噪声。

为不含误差的状态向量；

定义为不含误差的载体坐标系下被测物体相对载体的位置、速度、旋转四元数；

为不含误差的被测物体IMU测量角速度与加速度的零偏；b_pω、b_pa为不含误差的运动载具IMU测量角速度与加速度的零偏；

则用于表示真实状态X与不含噪声状态

的状态误差向量ΔX为：

其中各项的具体展开如下：

根据建立当前时刻误差状态方程Δx与下一时刻状态误差

的递推关系

Δt为当前时刻到下一时刻的时间差。

得到，系统状态误差递推关系即状态误差方程为：

F_X为状态转移矩阵；F_N为噪声转移矩阵；

U＝[a_h,ω_h,q_p,ω_p]^T；

a_h、ω_h、a_p、和ω_p分别为被测物体IMU加速度与角速度输出、运动载具IMU的加速度与角速度输出；

N为状态噪声向量，

其中，

和

分别为被测物体IMU加速度噪声、运动载具IMU加速度噪声、被测物体IMU角速度噪声、运动载具IMU角速度噪声，被测物体IMU加速度零偏噪声、运动载具IMU加速度零偏噪声、被测物体IMU角速度零偏噪声和运动载具IMU角速度零偏噪声的方差。

进一步地，

所述状态转移矩阵

其中，

为不含误差的被测物体到运动载具相对旋转矩阵；

I为单位矩阵；

噪声转移矩阵

则，系统的状态协方差矩阵为

基于上述过程，所述更新卡尔曼滤波的误差状态协方差矩阵的过程包括：

1)获取被测物体、载体的IMU数据；

2)根据被测物体与载体运动模型更新状态向量

3)更新状态转移矩阵F_X、更新协方差矩阵F_NNF_N ^T；

4)更新误差状态协方差矩阵

适用于本方案的位姿测量卡尔曼滤波的滤波器的观测方程为：

其中，误差向量

H_p为位置量测矩阵；误差向量

H_q为位置量测矩阵；z_p、z_q为卡尔曼滤波的外部观测量的位置向量和速度向量；

为卡尔曼滤波估计的位置向量和速度向量。

所述外部观测量即为所述视觉单元获取被测物体相对于运动载具的视觉位姿数据；在所述第二视觉模块有数据输出时优选使用第二视觉位姿数据，在所述第二视觉模块无数据输出时使用第一视觉位姿数据。保证了测量精度的同时保证了较大的测量范围。

在观测过程中，

1)列写更新部分位置测量模型z_p；

其中，位置测量模型

式中，p_c-t代表传感器观测的位移，由视觉测量经过内参变化后得到；p_h-p、

为被测物体相对载体的平移向量与旋转矩阵；p_c-h为传感器相对被测物体的位置观测量，可通过标定得到；n_p为量测噪声。

则误差向量

展开有：

展开后忽略二阶项，得到：

根据观测方程Δz_p＝H_pΔx，位置量测矩阵H_p写作如下：

式中，p_c-h为位置观测量，[p_c-h]_×为对应叉乘运算矩阵。

2)列写更新部分角度测量模型z_q；

其中，角度测量模型

则误差向量

展开有

根据观测方程Δz_q＝H_qΔx，旋转量测矩阵H_q写作如下

本实施例的更新状态协方差矩阵与状态向量的过程包括：

1)计算观测残差

2)计算更新矩阵S＝HPH^T+R；

3)计算卡尔曼增益K＝PH^TS^-1；

4)计算状态修正量

5)计算状态协方差矩阵的递推结果P←(I_d-KH)P(I_d-KH)^T+KRK^T。

6)将状态更新量

与原本的状态向量

叠加后得到更新后的状态向量。

在本实施例的一个优选方案中，还包括了对视觉单元的视觉位姿数据进行筛选的方法，具体如下：

测量单元中双IMU差分通过积分过程以较高采样频率获取变换矩阵T^imu，同时视觉单元以较低速率获取变换矩阵T^cam。记连续几帧视觉单元测姿更新得到的变换矩阵

与

(依次递推)，选取差分后IMU历史测姿信息中与视觉单元获取时刻最接近的几帧惯性测姿变换矩阵

与

(依次递推)。则计算得到视觉单元得到的相邻两帧相对位姿变换矩阵

相邻两帧最近时刻的惯性测量相对姿态变换矩阵

为求取相邻帧时间内视觉测量与惯性测量的相对偏差，定义

在李代数

上的对数映射的二范数D_k

计算k时刻前一定时间内，n组有效相对变换D_k均方根偏差基准RMSD_k。

判定，若新一帧视觉测量与惯性测量偏差D_k∈[-2RMSD_k,2RMSD_k]，则认为其是有外部位姿测量，对滤波器状态进行更新，同时该D_k计入均方根偏差基准的更新。否则认为是无效的外部位姿测量，丢弃不用，该D_k也不计入均方根偏差基准。

将视觉单元的视觉位姿数据按照上述更新过程接入卡尔曼滤波，修正惯性测姿误差，提高测量精度。

综上所述，本实施例的视觉辅助惯性差分位姿测量系统，通过视觉辅助获取被测物体与运动载具的相对位姿关系，对进行惯性积分差分的位姿测量结果进行修正，得到准确的位姿测量结果。这种方法充分利用惯性测姿采样频率高、短时精度高，视觉测姿无累积误差的特性，实现高精度的位姿追踪。该方法相较于传统相对位姿测量方案，更为有效调用各种导航方式特性，且布置简便。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种视觉辅助惯性差分位姿测量系统，其特征在于，包括：测量单元，以及与测量单元连接的运动载具IMU、被测物体IMU和视觉单元；

运动载具承载被测物体，所述运动载具IMU和被测物体IMU分别布置于运动载具和被测物体上，用于获取运动载具和被测物体的运动信息；

所述视觉单元布置于运动载具内，用于获取被测物体相对于运动载具的视觉位姿数据；

所述测量单元，接收所述运动载具和被测物体的运动信息，进行双IMU差分获得的载体坐标系下被测物体相对运动载体的惯性位姿数据；接收所述视觉位姿数据；并以视觉位姿数据为外部观测量对包括惯性位姿数据在内的状态数据进行卡尔曼滤波，输出滤波后的被测物体与运动载具的相对位姿数据。

2.根据权利要求1所述的视觉辅助惯性差分位姿测量系统，其特征在于，所述视觉单元包括第一视觉模块和/或第二视觉模块；

所述第一视觉模块的第一相机模组安装于运动载具上，与第一相机配合的第一标志点布置于被测物体内；第一相机模组测量被测物体上布置的第一标志点，获得被测物体相对运动载具的第一视觉位姿数据；

所述第二视觉模块的第二相机模组安装于被测物体上，与第二相机模组配合的第二标志点布置于运动载具上；第二相机模组测量运动载具上布置的第二标志点，通过坐标系转换获得被测物体相对运动载具的第二视觉位姿数据。

3.根据权利要求2所述的视觉辅助惯性差分位姿测量系统，其特征在于，

所述运动载具IMU与第一相机模组固连，所述被测物体IMU与第二相机模组固连。

4.根据权利要求2所述的视觉辅助惯性差分位姿测量系统，其特征在于，

所述第一标志点在被测物体上进行布局，保证在被测物体在其运动范围内，使所述第一标志点处于第一相机模组的视觉范围内；所述第一标志点的数量大于等于4；

所述第二标志点在运动载具内的布局，保证在固定角度内，使所述第二标志点处于第二相机模组的视觉范围内；所述第二标志点的数量大于等于4。

5.根据权利要求4所述的视觉辅助惯性差分位姿测量系统，其特征在于，

所述第一标志点和第二标志点使用红外发光点的方式或红外发光二维码的方式；所述第一相机模组和第二相机模组采用单目、双目或结构光的方式。

6.根据权利要求5所述的视觉辅助惯性差分位姿测量系统，其特征在于，

所述第一标志点和第二标志点采用同频闪烁的方式与对应的相机模组同步。

7.根据权利要求1-6任一项所述的视觉辅助惯性差分位姿测量系统，其特征在于，

所述卡尔曼滤波中的状态向量：

X＝[p_h-p,v_h-p,q_h-p,b_ha,b_pa,b_hω,b_pω]^T；

其中，p_h-p、v_h-p、q_h-p定义为双IMU差分获得的载体坐标系下被测物体相对载体的位置、速度、旋转四元数；b_hω、b_ha为被测物体IMU测量角速度与加速度的零偏；b_pω、b_pa为运动载具IMU测量角速度与加速度的零偏。

8.根据权利要求7所述的视觉辅助惯性差分位姿测量系统，其特征在于，所述运动载具IMU测量角速度与加速度的零偏为经过修正过的零偏数据。

9.根据权利要求7所述的视觉辅助惯性差分位姿测量系统，其特征在于，所述旋转四元数q_h-p采用角速度差分法进行求解；

在角速度差分法中，通过对当前时刻被测物体与运动载具的相对角速度进行积分得到被测物体与运动载具的相对位姿。

10.根据权利要求9所述的视觉辅助惯性差分位姿测量系统，其特征在于，

所述角速度差分法，包括：

获取被测物体坐标系到运动载体坐标系的相对旋转矩阵；

根据被测物体与运动载体的IMU获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量；

使用当前时刻相对角增量计算相对旋转四元数得到被测物体相对于运动载体的位姿数据。

Images