双IMU单目视觉组合测量非惯性系下目标物位姿方法技术

本发明专利技术涉及惯性测量技术、视觉测量技术、组合测量技术，为准确测量非惯性系下目标物相对于运动参考系的位姿信息，本发明专利技术采用的技术方案是，双IMU单目视觉组合测量非惯性系下目标物位姿方法，根据两个惯性测量单元IMU输出的差异得到非惯性系下的目标物与运动参考系之间的相对角速度和相对加速度，相对角速度经过补偿并求出相对姿态角，相对加速度经过积分求出相对位置；利用单目视觉直接测出在非惯性系中目标物上的多特征点靶标相对摄像机坐标系的旋转矩阵和位置矢量，由旋转矩阵计算相对姿态角；统一两者测量结果到相同的坐标系，再根据数据融合方法最终实现相对位姿测量。本发明专利技术主要应用于位姿信息测量场合。

【技术实现步骤摘要】

本专利技术涉及惯性测量技术、视觉测量技术、组合测量技术，尤其涉及双IMU和单目视觉组合测量非惯性系下目标物位姿方法。
技术介绍
惯性相对位姿测量方法将惯性传感器如加速度计和陀螺仪直接固连在被测物体和运动参考系上，也可以将多种惯性传感器组合成惯性测量单元(IMU)后再进行安装。传感器输出敏感到的加速度和角速度，根据多个传感器输出得到相对角速度和相对加速度。再利用牛顿定律得到相对位置和角度信息。故原理简单、响应快速，在传感器精度保证下瞬间位姿测量精度高。不过，相对角速度依然因陀螺仪漂移存在偏差。而由不同惯性单元的输出求相对加速度时还需考虑因相对运动引起的向心方向加速度、切向方向加速度及科式加速度，当然这些加速度由公式可求。随着微机电惯性器件(MEMS加速度计、陀螺仪)的流行，惯性测量所需成本降低、安装更加小型便捷，所以在工业生产和科学研究中得到越来越多的应用。视觉位姿测量方法通常是从拍摄的图像中提取特征点信息，之后计算得到目标物相对于参考坐标系的位姿信息。常见的方法有比例正交投影迭代变换算法(POSIT算法)、N点透视投影算法(PNP算法)等。在视觉测量中，参考坐标系可以是惯性坐标系，也可以是非惯性系，两种情况下测量算法大致相同。但是视觉测量容易受遮挡物干扰、环境光线干扰，目标物和参考系同时运动时靶标更易移出摄像机视场，同时其基于图像处理的算法也让单一的视觉测量无法满足对目标物的快速实时定位。组合位姿测量技术是使用多种测量方式同时测量被测物体的位姿信息，可以结合各单一方法的优点、弥补各单一方法的不足，常见的组合形式有全球定位系统(GPS)器件与惯性器件组合、惯性器件与磁强计组合、惯性器件与电荷耦合元件(CCD)组合等。组合测量的核心在于对各种方式的测量结果进行评估，和对不同来源的测量数据进行融合。互补滤波器、卡尔曼滤波器等都是有效的数据融合方式，其中卡尔曼滤波器在导航、定位等应用中都得到了广泛使用。而经典的卡尔曼滤波器(KF)经过修改，又可以演化为拓展卡尔曼滤波(EKF)、H∞滤波、联邦卡尔曼滤波等以满足不同的测量需求。为充分利用惯性位姿测量和视觉位姿测量的特点，实现扬长补短，本方案设计了一套惯性和视觉组合相对位姿测量系统，系统使用了两个IMU和一组单目视觉。提出了相对位姿测量的实现方法，可快速、精确得测量目标物相对运动参考系的位姿，该方法简化后也可应用在物体相对于惯性系的位姿测量中。
技术实现思路
为克服现有技术的不足，准确测量非惯性系下目标物相对于运动参考系的位姿信息，本专利技术采用的技术方案是，双IMU单目视觉组合测量非惯性系下目标物位姿方法，根据两个惯性测量单元IMU输出的差异得到非惯性系下的目标物与运动参考系之间的相对角速度和相对加速度，相对角速度经过补偿并求出相对姿态角，相对加速度经过积分求出相对位置；利用单目视觉直接测出在非惯性系中目标物上的多特征点靶标相对摄像机坐标系的旋转矩阵和位置矢量，由旋转矩阵计算相对姿态角；统一两者测量结果到相同的坐标系，再根据数据融合方法最终实现相对位姿测量。目标物与运动参考系之间相对角速度的测量值存在漂移误差，其粗补偿步骤是，采用一个IMU和特征点靶标固连在目标物上，其上的IMU坐标系作为目标系，另一个IMU和CCD摄像机固连在运动平台上，其上的IMU坐标系作为参考系；将目标物静止时两个IMU输出的角速度做差，再根据方向余弦矩阵迭代公式、方向余弦矩阵与欧拉角的关系得到三个相对姿态角即方位角ψ、俯仰角θ和横滚角φ随时间的变化情况；再用三条一元多次函数曲线拟合，每条曲线对应一个相对姿态角：∂φ^l∂θ^l∂ψ^l=d1nd1(n-1)...d10d2nd2(n-1)...d20d3nd3(n-1)...d30(lτ)n(lτ)n-1...(lτ)0---(1)]]>式中，dij为第i条n次曲线的第j次项系数，τ为IMU采样周期，l为整数表示第l个IMU采样周期结束，∂φ^l∂θ^l∂ψ^lT]]>为第l个周期结束时三个相对姿态角的漂移的估计，上标T表示转置；目标物静止时对相对姿态角进行m次测量，采用最小二乘法拟合m次测量结果，得到系数矩阵：式中，为与的内积，g为每次测量时IMU采样的周期数，fp为第i条曲线对应的相对姿态角的第p次测量结果，为fp与的内积；将拟合得到的曲线作为相对姿态角漂移的粗估计，再由各时刻的粗估计值逆推出相对角速度各时刻的粗补偿值。单一测量方式位姿信息的输出过程：由两个IMU得到相对加速度和经过补偿的相对角速度，相对加速度经两次积分得到相对位移；相对角速度经离散欧拉角迭代方法得到相对姿态角；单目摄像机拍摄靶标上4个非共面点，用POSIT算法得到C系到W系间的位姿信息，将视觉测量结果统一为N系到B系的位姿信息，其中，仅用一个IMU和特征点靶标固连在目标物上，其上的IMU坐标系作为目标系B系，另一个IMU和CCD摄像机固连在运动平台上，其上的IMU坐标系作为参考系N系，转台系R系与运动平台系S系的轴与各自的旋转轴对齐；世界坐标系W系设定在靶标上；摄像机坐标系C系设定在摄像机CCD面上。数据融合之前的两次判定步骤是，判定条件1：POSIT算法是否报错，该判定条件旨在避免采集的图像受遮挡而缺失靶标点；判定条件2：tr(P)＝tr[(W单目-WIMU)(W单目-WIMU)T]？≥ε1(3)式中，W单目为单目视觉测量得到的N系到B系的位姿量，WIMU为双IMU惯性测量得到的N系到B系的位姿量，ε1为阈值，方阵P的特征值反映了在其特征向量方向上视觉测量结果和双IMU测量结果之间的差异的大小，故用P的迹作为判定量，旨在剔除受环境干扰而质量太差的图像。数据融合具体步骤是，根据第k次图像采集时刻IMU测量的位姿量和单目视觉测量的位姿量中对应位姿元素的差异大小，在该元素之前乘上k时刻的关注因子Lk，由此构造出一个新的位姿元素的线性组合Zk：作为最优化目标，式中Xk=φkθkψkr→NB,(x)Nkr→NB,(y)Nkr→NB,(z)NkT]]>表示选取的状态量，其各元素为第k次图像采集时刻各相对位姿量的值，且Xk与和的各元素形式相同，lii为Lk对角线上的元素，1≤i≤6；根据本方案搭建的系统，构造随机非线性离散时间系统，系统方程为：状态方程中，Xk+t+1T=&phi本文档来自技高网...

【技术保护点】
一种双IMU单目视觉组合测量非惯性系下目标物位姿方法,其特征是，根据两个惯性测量单元IMU输出的差异得到非惯性系下的目标物与运动参考系之间的相对角速度和相对加速度，相对角速度经过补偿并求出相对姿态角，相对加速度经过积分求出相对位置；利用单目视觉直接测出在非惯性系中目标物上的多特征点靶标相对摄像机坐标系的旋转矩阵和位置矢量，由旋转矩阵计算相对姿态角；统一两者测量结果到相同的坐标系，再根据数据融合方法最终实现相对位姿测量。

【技术特征摘要】
1.一种双IMU单目视觉组合测量非惯性系下目标物位姿方法,其特征是，根据两个惯性测量
单元IMU输出的差异得到非惯性系下的目标物与运动参考系之间的相对角速度和相对加
速度，相对角速度经过补偿并求出相对姿态角，相对加速度经过积分求出相对位置；利用
单目视觉直接测出在非惯性系中目标物上的多特征点靶标相对摄像机坐标系的旋转矩阵
和位置矢量，由旋转矩阵计算相对姿态角；统一两者测量结果到相同的坐标系，再根据数
据融合方法最终实现相对位姿测量。
2.如权利要求1所述的双IMU单目视觉组合测量非惯性系下目标物位姿方法，其特征是，
目标物与运动参考系之间相对角速度的测量值存在漂移误差，其粗补偿步骤是，采用一个
IMU和特征点靶标固连在目标物上，其上的IMU坐标系作为目标系，另一个IMU和CCD
摄像机固连在运动平台上，其上的IMU坐标系作为参考系；
将目标物静止时两个IMU输出的角速度做差，再根据方向余弦矩阵迭代公式、方向
余弦矩阵与欧拉角的关系得到三个相对姿态角即方位角ψ、俯仰角θ和横滚角φ随时间的
变化情况；再用三条一元多次函数曲线拟合，每条曲线对应一个相对姿态角：
∂φ^l∂θ^l∂ψ^l=d1nd1(n-1)...d10d2nd2(n-1)...d20d3nd3(n-1)...d30(lτ)n(lτ)n-1···(lτ)0---(1)]]>式中，dij为第i条n次曲线的第j次项系数，τ为IMU采样周期，l为整数表示第l个IMU
采样周期结束，为第l个周期结束时三个相对姿态角的漂移的估计，上
标T表示转置；目标物静止时对相对姿态角进行m次测量，采用最小二乘法拟合m次测
量结果，得到系数矩阵：
式中，为与的内积，g为每次测量时IMU采
样的周期数，fp为第i条曲线对应的相对姿态角的第p次测量结果，为
fp与的内积；将拟合得到的曲线作为相对姿态角漂移的粗估计，再由各时刻的粗估计
值逆推出相对角速度各时刻的粗补偿值。
3.如权利要求1所述的双IMU单目视觉组合测量非惯性系下目标物位姿方法，其特征是，
单一测量方式位姿信息的输出过程：由两个IMU得到相对加速度和经过补偿的相对角速
度，相对加速度经两次积分得到相对位移；相对角速度经离散欧拉角迭代方法得到相对姿

\t态角；单目摄像机拍摄靶标上4个非共面点，用POSIT算法得到C系到W系间的位姿信
息，将视觉测量结果统一为N系到B系的位姿信息，其中，仅用一个IMU和特征点靶标
固连在目标物上，其上的IMU坐标系作为目标系B系，另一个IMU和CCD摄像机固连
在运动平台上，其上的IMU坐标系作为参考系N系，转台系R系与运动平台系S系的轴
与各自的旋转轴对齐；世界坐标系W系设定在靶标上；摄像机坐标系C系设定在摄像机
CCD面上。
4.如权利要求1所述的双IMU单目视觉组合测量非惯性系下目标物位姿方法，其特征是，
数据融合之前的两次判定步骤是，判定条件1：POSIT算法是否报错，该判定条件旨在避
免采集的图像受遮挡而缺失靶标点；判定条件2：
tr(P)＝tr[(W单目-WIM...

【专利技术属性】
技术研发人员：孙长库，徐怀远，郭肖亭，王鹏，
申请(专利权)人：天津大学，
类型：发明
国别省市：天津;12