[发明专利]双IMU单目视觉组合测量非惯性系下目标物位姿方法

说明书

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

技术领域

本发明涉及惯性测量技术、视觉测量技术、组合测量技术，尤其涉及双IMU和单目视觉组合测量非惯性系下目标物位姿方法。

背景技术

惯性相对位姿测量方法将惯性传感器如加速度计和陀螺仪直接固连在被测物体和运动参考系上，也可以将多种惯性传感器组合成惯性测量单元(IMU)后再进行安装。传感器输出敏感到的加速度和角速度，根据多个传感器输出得到相对角速度和相对加速度。再利用牛顿定律得到相对位置和角度信息。故原理简单、响应快速，在传感器精度保证下瞬间位姿测量精度高。不过，相对角速度依然因陀螺仪漂移存在偏差。而由不同惯性单元的输出求相对加速度时还需考虑因相对运动引起的向心方向加速度、切向方向加速度及科式加速度，当然这些加速度由公式可求。随着微机电惯性器件(MEMS加速度计、陀螺仪)的流行，惯性测量所需成本降低、安装更加小型便捷，所以在工业生产和科学研究中得到越来越多的应用。

视觉位姿测量方法通常是从拍摄的图像中提取特征点信息，之后计算得到目标物相对于参考坐标系的位姿信息。常见的方法有比例正交投影迭代变换算法(POSIT算法)、N点透视投影算法(PNP算法)等。在视觉测量中，参考坐标系可以是惯性坐标系，也可以是非惯性系，两种情况下测量算法大致相同。但是视觉测量容易受遮挡物干扰、环境光线干扰，目标物和参考系同时运动时靶标更易移出摄像机视场，同时其基于图像处理的算法也让单一的视觉测量无法满足对目标物的快速实时定位。

组合位姿测量技术是使用多种测量方式同时测量被测物体的位姿信息，可以结合各单一方法的优点、弥补各单一方法的不足，常见的组合形式有全球定位系统(GPS)器件与惯性器件组合、惯性器件与磁强计组合、惯性器件与电荷耦合元件(CCD)组合等。组合测量的核心在于对各种方式的测量结果进行评估，和对不同来源的测量数据进行融合。互补滤波器、卡尔曼滤波器等都是有效的数据融合方式，其中卡尔曼滤波器在导航、定位等应用中都得到了广泛使用。而经典的卡尔曼滤波器(KF)经过修改，又可以演化为拓展卡尔曼滤波(EKF)、H∞滤波、联邦卡尔曼滤波等以满足不同的测量需求。

为充分利用惯性位姿测量和视觉位姿测量的特点，实现扬长补短，本方案设计了一套惯性和视觉组合相对位姿测量系统，系统使用了两个IMU和一组单目视觉。提出了相对位姿测量的实现方法，可快速、精确得测量目标物相对运动参考系的位姿，该方法简化后也可应用在物体相对于惯性系的位姿测量中。

发明内容

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

目标物与运动参考系之间相对角速度的测量值存在漂移误差，其粗补偿步骤是，采用一个IMU和特征点靶标固连在目标物上，其上的IMU坐标系作为目标系，另一个IMU和CCD摄像机固连在运动平台上，其上的IMU坐标系作为参考系；