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

权利要求书

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

2.如权利要求1所述的双IMU单目视觉组合测量非惯性系下目标物位姿方法，其特征是，目标物与运动参考系之间相对角速度的测量值存在漂移误差，其粗补偿步骤具体是，；

将目标物静止时两个IMU输出的角速度做差，再根据方向余弦矩阵迭代公式、方向余弦矩阵与欧拉角的关系得到三个相对姿态角即方位角ψ、俯仰角θ和横滚角φ随时间的变化情况；再用三条一元多次函数曲线拟合，每条曲线对应一个相对姿态角：

式中，d_ij为第i条n次曲线的第j次项系数，τ为IMU采样周期，l为整数表示第l个IMU采样周期结束，为第l个周期结束时三个相对姿态角的漂移的估计，上标T表示转置；目标物静止时对相对姿态角进行m次测量，采用最小二乘法拟合m次测量结果，得到系数矩阵：

式中，为与的内积，g为每次测量时IMU采样的周期数，f_p为第i条曲线对应的相对姿态角的第p次测量结果，为f_p与的内积；将拟合得到的曲线作为相对姿态角漂移的粗估计，再由各时刻的粗估计值逆推出相对角速度各时刻的粗补偿值。

3.如权利要求1所述的双IMU单目视觉组合测量非惯性系下目标物位姿方法，其特征是，单一测量方式位姿信息的输出过程：由两个IMU得到相对加速度和经过补偿的相对角速度，相对加速度经两次积分得到相对位移；相对角速度经离散欧拉角迭代方法得到相对姿态角；单目摄像机拍摄靶标上4个非共面点，用POSIT算法得到C系到W系间的位姿信息，将视觉测量结果统一为N系到B系的位姿信息，其中，仅用一个IMU和特征点靶标固连在目标物上，其上的IMU坐标系作为目标系B系，另一个IMU和CCD摄像机固连在运动平台上，其上的IMU坐标系作为参考系N系，转台系R系与运动平台系S系的轴与各自的旋转轴对齐；世界坐标系W系设定在靶标上；摄像机坐标系C系设定在摄像机CCD面上。

4.如权利要求3所述的双IMU单目视觉组合测量非惯性系下目标物位姿方法，其特征是，数据融合之前的两次判定步骤是，判定条件1：POSIT算法是否报错，该判定条件旨在避免采集的图像受遮挡而缺失靶标点；判定条件2：

式中，W单目为单目视觉测量得到的N系到B系的位姿量，W^IMU为双IMU惯性测量得到的N系到B系的位姿量，ε₁为阈值，方阵P的特征值反映了在其特征向量方向上视觉测量结果和双IMU测量结果之间的差异的大小，故用P的迹作为判定量，旨在剔除受环境干扰而质量太差的图像。