CN105698765A - 双imu单目视觉组合测量非惯性系下目标物位姿方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

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

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

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

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

发明内容

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

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

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

$\left[\begin{array}{c}\∂{\widehat{\mathrm{\φ}}}_l\\ \∂{\widehat{\mathrm{\θ}}}_l\\ \∂{\widehat{\mathrm{\ψ}}}_l\end{array}\right]=\left[\begin{array}{cccc}{d}_{1n}& d_{1(n-1)}& ...& d_{10}\\ d_{2n}& d_{2(n-1)}& ...& d_{20}\\ d_{3n}& d_{3(n-1)}& ...& d_{30}\end{array}\right]\left[\begin{array}{c}{\left(l\mathrm{\τ}\right)}^n\\ {\left(l\mathrm{\τ}\right)}^{n-1}\\ .\\ .\\ .\\ {\left(l\mathrm{\τ}\right)}^0\end{array}\right]---\left(1\right)$

式中，d_ij为第i条n次曲线的第j次项系数，τ为IMU采样周期，l为整数表示第l个IMU采样周期结束， ${\left[\begin{array}{ccc}\∂{\widehat{\mathrm{\φ}}}_l& \∂{\widehat{\mathrm{\θ}}}_l& \∂{\widehat{\mathrm{\ψ}}}_l\end{array}\right]}^T$ 为第l个周期结束时三个相对姿态角的漂移的估计，上标T表示转置；目标物静止时对相对姿态角进行m次测量，采用最小二乘法拟合m次测量结果，得到系数矩阵：

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

单一测量方式位姿信息的输出过程：由两个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^单目-W^IMU)(W^单目-W^IMU)^T]？≥ε₁(3)

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

数据融合具体步骤是，根据第k次图像采集时刻IMU测量的位姿量和单目视觉测量的位姿量中对应位姿元素的差异大小，在该元素之前乘上k时刻的关注因子L_k，由此构造出一个新的位姿元素的线性组合Z_k：

作为最优化目标，式中 $X_k={\left[\begin{array}{cccccc}{\mathrm{\φ}}_k& {\mathrm{\θ}}_k& {\mathrm{\ψ}}_k& {\stackrel{\→}{r}_{NB,\left(x\right)}^N}_k& {\stackrel{\→}{r}_{NB,\left(y\right)}^N}_k& {\stackrel{\→}{r}_{NB,\left(z\right)}^N}_k\end{array}\right]}^T$ 表示选取的状态量，其各元素为第k次图像采集时刻各相对位姿量的值，且X_k与和的各元素形式相同，l_ii为L_k对角线上的元素，1≤i≤6；根据本方案搭建的系统，构造随机非线性离散时间系统，系统方程为：

状态方程中， $X_{k+\frac{t+1}{T}}={\left[\begin{array}{cccccc}{\mathrm{\φ}}_{k+\frac{t+1}{T}}& {\mathrm{\θ}}_{k+\frac{t+1}{T}}& {\mathrm{\ψ}}_{k+\frac{t+1}{T}}& {\stackrel{\→}{r}_{NB,\left(x\right)}^N}_{k+\frac{t+1}{T}}& {\stackrel{\→}{r}_{NB,\left(y\right)}^N}_{k+\frac{t+1}{T}}& {\stackrel{\→}{r}_{NB,\left(z\right)}^N}_{k+\frac{t+1}{T}}\end{array}\right]}^T$ 为状态量，T为视觉采样周期与惯性采样周期之比，k表示第k次视觉采样，t+1表示视觉采样周期内第t+1次惯性采样，所以状态量表示第时刻的相对位姿量， $U_{k+\frac{t}{T}}={\left[\begin{array}{cccccc}{\stackrel{\→}{\mathrm{\ω}}_{NB,\left(x\right)}^B}_{k+\frac{t}{T}}& {\stackrel{\→}{\mathrm{\ω}}_{NB,\left(y\right)}^B}_{k+\frac{t}{T}}& {\stackrel{\→}{\mathrm{\ω}}_{NB,\left(z\right)}^B}_{k+\frac{t}{T}}& 1& 1& 1\end{array}\right]}^T$ 为输入控制量，前三个元素表示时刻B系相对N系的角速度在B系三轴方向上的量值，Γ为6×6的单位矩阵，为时刻未知的系统噪声；观测方程中，Y_k+1为观测量，元素形式与X_k+1相同，Y_k+1取值于k+1时刻单目视觉的测量结果H为6×6的单位矩阵，而V_k+1为k+1时刻未知的观测噪声；用次优H∞滤波估计出基于上述系统方程和公式(4)的位姿信息，步骤为：先线性化状态方程，再设定变化的阈值γ_k，再由次优H∞滤波公式不断迭代出满足位姿线性组合最优化时的相对位姿；滤波器迭代公式中k+1时刻的估计误差方差方阵P_k+1的迹随k时刻阈值γ_k取值的增大而逐渐减小并趋于稳定，据此采用while循环结构并根据P_k+1斜率变化情况求出P_k+1接近稳定值时的γ_k值：

$while\left|\frac{tr\left(P_{k+1}^{+}\right)-tr\left(P_{k+1}^{-}\right)}{{\mathrm{\&gamma;}}_k^{+}-{\mathrm{\&gamma;}}_k^{-}}\right|<{\mathrm{\&epsiv;}}_2$

${\mathrm{\&gamma;}}_k^{-}={\mathrm{\&gamma;}}_k^{+}$

$P_{k+1}^{-}=P_{k+1}^{+}$

${\mathrm{\&gamma;}}_k^{+}={\mathrm{\&alpha;\&gamma;}}_k^{-}---\left(5\right)$

根据计算的值

end

${\mathrm{\&gamma;}}_k={\mathrm{\&gamma;}}_k^{+},P_{k+1}=P_{k+1}^{+}$

式中，ε₂为阈值，取一初值后，为与大于1的放大倍数α的乘积，为根据对应求出的估计误差方差方阵。

双IMU模块和单目视觉模块均测量有效时刻，得到融合时刻非惯性系N系到目标系B系的相对姿态角和相对位置矢量；而在其他时刻，由双IMU惯性测量方式测量N系到B系的相对位姿信息；最后用融合得到的姿态角分段更新姿态角漂移的估计曲线，以实现修正相对角速度的粗补偿值；修正的步骤，k时刻修正量为X_k,i为k时刻系统状态量的第i个元素，i取值为1、2、3表示数据融合之后的三个相对姿态角，通过漂移曲线降次化处理，用三个修正后的漂移量估计出一条二次曲线，并作为下一段周期内的漂移曲线，最终得到分段更新的姿态角漂移评定曲线。

本发明的特点及有益效果是：

本发明设计了基于双IMU和单目视觉组合测量非惯性系下目标物位姿的方法，该方法结合了惯性位姿测量短时测量准确高、响应快和视觉测量准确度稳定性好的优，弥补了惯性测量时漂问题，解决了视觉位姿测量方法易受环境干扰而丢失目标或产生较大测量误差的问题，也改善其测量速度较低的特点。

本方案还设计了基于上述测量方法的系统。系统结构简单，传感器安装简易。配合上位机进行算法处理，可以简单、快速、准确地实现目标物相对于非惯性的定位。

在实际工业生产和科学研究中，测量被测物相对于某一非惯性系的位姿具有重要和实际的意义。应用本发明的方法很方便地测出包括头盔、机器人等刚性被测件相对于运动参考系的三个姿态角和位置矢量。本方法的特点是定位快速并准确，对于视觉测量可能存在的被遮挡现象有较好地容错性，事实证明该方法的可靠性高。

附图说明：

图1基于惯性和视觉组合相对位姿测量的系统图。

图2各坐标系之间的关系。

图3相对角速度补偿效果，图中，a：横滚角的静态测量补偿效果，b俯仰角的静态测量补偿效果，c方位角的静态补偿效果。

图4数据流向的结构框图。

图5一条分段更新的姿态角漂移评定曲线。

具体实施方式

本发明所采用的技术方案是：非惯性系下使用惯性测量单元IMU来测量目标物与运动参考系之间的相对位姿需要测出两者之间的相对角速度和相对加速度，这可以根据两个IMU输出的差异得到。进一步讲，由于在非惯性系中和在惯性系中测量目标物位姿的差异性，非惯性系下的相对角速度等于两个IMU中的对应陀螺仪输出之差，而相对加速度等于两个IMU中的对应加速度计输出之差再减去由相对运动引起的3个额外加速度，即向心方向加速度、切向方向加速度和科式加速度。然而，这样得到的相对角速度存在随时间的漂移误差，需进一步补偿。由相对加速度和补偿过的相对角速度可计算出目标物相对运动参考系的相对姿态角和相对位移，相对位移进一步推出相对位置关系。

单目视觉可以直接根据采集的图像计算出在非惯性系中的多特征点靶标相对摄像机坐标系的旋转矩阵和位置矢量。而旋转矩阵可以转化为相对姿态角。由此，本方案根据数据融合方法发明了采用双IMU和单目相机实现相对位姿测量的方法，即测量目标系相对运动参考系的相对姿态角和相对位置矢量。

首先，说明本专利申请书中各外文字符的命名规则。规则为：外文字符左侧的上标表示变量所在的坐标系，左侧的下标表示变量描述的两个坐标系，以及是否为变量(当变量对应某矢量时)在左上标的坐标系下的某一轴上的投影；右侧的下标表示时间，以及是否为变量(当变量对应某矩阵时)的元素。举例：表示时刻B系相对于N系的角速度在B系中的值，k表示第k次视觉采集图像，T为视觉采样周期与惯性采样周期之比，t表示视觉采样周期内第t次惯性采样，而表示时刻B系相对于N系的角速度在B系x轴上的投影；外文字符可以换成加速度速度空间位置矢量方位角ψ、俯仰角θ和横滚角φ等。举例：表示时刻的状态量，而表示时刻状态量的第i个元素；外文字符X可以换成观测量Y、双IMU惯性测量得到位姿量W^IMU、单目视觉测量得到的位姿量W^单目、控制量U和姿态角修正量△。

本发明的系统使用了两个IMU，一组单目视觉系统即1个摄像机和1个含4个非共面特征点的靶标，它们的安装位置已标注在图1。每个IMU封装了多个惯性传感器，包括3个彼此垂直方向上的加速度计、3个彼此垂直方向上的陀螺仪等。本方案的系统图如图1所示，各坐标系之间的关系如图2所示。为了精简系统，仅用一个IMU和特征点靶标固连在目标物上，其上的IMU坐标系作为目标系B系，另一个IMU和摄像机固连在运动平台上，其上的IMU坐标系作为参考系N系。转台系R系与运动平台系S系的轴与各自的旋转轴对齐；世界坐标系W系设定在靶标上；摄像机坐标系C系设定在其光电耦合元件CCD的感应平面上。在运动过程中，B系与W系之间及N系与C系之间的位姿关系(旋转矩阵和位置矢量)保持不变。其求解过程：先使用非共面多矢量标定方法标定出初始状态R系到B系、R系到W系、S系到B系、S系到N系的旋转矩阵和位置矢量再使用单目视觉拍摄系统初始状态的图像并根据正交投影迭代算法(POSIT)标定出初始状态C系到W的位姿关系；由上述结果可推出系统中B系与W系之间及N系与C系之间的恒定的位姿关系。

前述相对角速度若直接等于两个IMU中的对应陀螺仪输出之差，就忽略了相对角速度的时间漂移误差。该误差影响了使用IMU测量位姿的准确性，所以必须评估相对角速度随时间的漂移情况。方案提出的具体步骤为：将静止时两个IMU输出的角速度做差，再根据方向余弦矩阵迭代公式、方向余弦矩阵与欧拉角的关系得到三个相对姿态角即方位角ψ、俯仰角θ和横滚角φ随时间的变化情况。再用三条一元多次函数曲线拟合，每条曲线对应一个相对姿态角：

$\left[\begin{array}{c}\&part;{\widehat{\mathrm{\&phi;}}}_l\\ \&part;{\widehat{\mathrm{\&theta;}}}_l\\ \&part;{\widehat{\mathrm{\&psi;}}}_l\end{array}\right]=\left[\begin{array}{cccc}{d}_{1n}& d_{1(n-1)}& ...& d_{10}\\ d_{2n}& d_{2(n-1)}& ...& d_{20}\\ d_{3n}& d_{3(n-1)}& ...& d_{30}\end{array}\right]\left[\begin{array}{c}{\left(l\mathrm{\&tau;}\right)}^n\\ {\left(l\mathrm{\&tau;}\right)}^{n-1}\\ .\\ .\\ .\\ {\left(l\mathrm{\&tau;}\right)}^0\end{array}\right]---\left(1\right)$

式中，d_ij为第i条n次曲线的第j次项系数，τ为IMU采样周期，l为整数表示第l个IMU采样周期结束， ${\left[\begin{array}{ccc}\&part;{\widehat{\mathrm{\&phi;}}}_l& \&part;{\widehat{\mathrm{\&theta;}}}_l& \&part;{\widehat{\mathrm{\&psi;}}}_l\end{array}\right]}^T$ 为第l个周期结束时三个相对姿态角的漂移的估计，上标T表示转置。系统静止时对相对姿态角进行m次测量，采用最小二乘法拟合m次测量结果，得到系数矩阵：

式中，为与的内积，g为每次测量时IMU采样的周期数，f_p为第i条曲线对应的相对姿态角的第p次测量结果，为f_p与的内积。将拟合得到的曲线作为相对姿态角漂移的粗估计，再由各时刻的粗估计值逆推出相对角速度各时刻的粗补偿值。图3为相对角速度的补偿效果。但是，此粗补偿值只能在位姿测量初始阶段作为相对角速度的补偿值，在后续还需进行修正。

这样，经过补偿的相对角速度经离散欧拉角迭代方法得到N系到B系的相对姿态角，而相对加速度经两次积分得到N系到B系的相对位移，加上前一时刻的相对位置矢量更新出新的相对位置矢量。同时，单目摄像机实时拍摄非共面特征点靶标的图像，经POSIT算法可得到C系到W系间的位姿信息。再根据图2各坐标系之间的关系，将单目视觉测量结果也统一为N系到B系的位姿信息，实现双IMU和单目视觉各自测量结果的坐标系统一，以便进行两者测量结果的数据融合。

本发明中，IMU位姿测量结果和单目视觉位姿测量结果的数据融合发生在单目相机采集到图像之后，但并非每次采集到图像之后都发生数据融合，需进行两次不融合判定。方案提出每次判定条件。图4为数据流向的结构框图。判定条件1：POSIT算法是否报错？该判定条件旨在避免采集的图像受遮挡而缺失靶标点。判定条件2：

tr(P)＝tr[(W^单目-W^IMU)(W^单目-W^IMU)^T]？≥ε₁(3)

式中，W^单目为单目视觉测量得到的N系到B系的位姿量，为一个6×1矩阵，各元素依次为单目视觉测量得到的横滚角、俯仰角、方位角和N系到B系的位置矢量在参考系三轴方向上的投影。同理，W^IMU为双IMU惯性测量得到的N系到B系的位姿量，也为一个6×1矩阵，各元素依次为IMU惯性测量得到的横滚角、俯仰角、方位角和N系到B系的位置矢量在参考系三轴方向上的投影。ε₁为阈值。该判定条件旨在剔除受环境干扰而质量太差的图像。因为误差方阵P的特征值反映了在特征向量方向上误差的大小，故用P的迹作为判定量。当两个判定条件均不成立，则进行数据融合。

发生数据融合时，双IMU惯性测量和单目视觉测量的位姿结果都已经统一到相同的坐标系。数据融合的思路是：先确定最优化目标，再构建系统方程，根据最优化目标和系统方程选择合适的滤波方式，最后优化滤波器使其参数达到自适应。具体过程为：根据第k次图像采集时刻IMU测量的位姿量和单目视觉测量的位姿量中对应位姿元素的差异大小，在该元素之前乘上k时刻的关注因子L_k，由此构造出一个新的位姿元素的线性组合Z_k：

作为最优化目标，式中 $X_k={\left[\begin{array}{cccccc}{\mathrm{\&phi;}}_k& {\mathrm{\&theta;}}_k& {\mathrm{\&psi;}}_k& {\stackrel{\&RightArrow;}{r}_{NB,\left(x\right)}^N}_k& {\stackrel{\&RightArrow;}{r}_{NB,\left(y\right)}^N}_k& {\stackrel{\&RightArrow;}{r}_{NB,\left(z\right)}^N}_k\end{array}\right]}^T$ 表示选取的状态量，其各元素为第k次图像采集时刻各相对位姿量的值(具体含义可参见上文的外文字符命名规则)，且X_k与和的各元素形式相同。l_ii为L_k对角线上的元素，1≤i≤6。根据本方案搭建的系统，构造随机非线性离散时间系统，系统方程为：

状态方程中， $X_{k+\frac{t+1}{T}}={\left[\begin{array}{cccccc}{\mathrm{\&phi;}}_{k+\frac{t+1}{T}}& {\mathrm{\&theta;}}_{k+\frac{t+1}{T}}& {\mathrm{\&psi;}}_{k+\frac{t+1}{T}}& {\stackrel{\&RightArrow;}{r}_{NB,\left(x\right)}^N}_{k+\frac{t+1}{T}}& {\stackrel{\&RightArrow;}{r}_{NB,\left(y\right)}^N}_{k+\frac{t+1}{T}}& {\stackrel{\&RightArrow;}{r}_{NB,\left(z\right)}^N}_{k+\frac{t+1}{T}}\end{array}\right]}^T$ 为状态量(元素的具体含义可参见上文的外文字符命名规则)，T为视觉采样周期与惯性采样周期之比，k表示第k次视觉采样，t+1表示视觉采样周期内第t+1次惯性采样，所以状态量表示第时刻的相对位姿量。 $U_{k+\frac{t}{T}}={\left[\begin{array}{cccccc}{\stackrel{\&RightArrow;}{\mathrm{\&omega;}}_{NB,\left(x\right)}^B}_{k+\frac{t}{T}}& {\stackrel{\&RightArrow;}{\mathrm{\&omega;}}_{NB,\left(y\right)}^B}_{k+\frac{t}{T}}& {\stackrel{\&RightArrow;}{\mathrm{\&omega;}}_{NB,\left(z\right)}^B}_{k+\frac{t}{T}}& 1& 1& 1\end{array}\right]}^T$ 为输入控制量，前三个元素表示时刻B系相对N系的角速度在B系三轴方向上的量值。Γ为6×6的单位矩阵，为时刻未知的系统噪声。观测方程中，Y_k+1为观测量，元素形式与X_k+1相同，Y_k+1取值于k+1时刻单目视觉的测量结果H为6×6的单位矩阵，而V_k+1为k+1时刻未知的观测噪声。融合时采用次优H∞滤波器，该滤波器具有较好的鲁棒性，可弱化系统噪声和观测噪声的不确定而带来的影响。用次优H∞滤波估计出基于上述系统方程和公式(4)的位姿信息，步骤为：先线性化状态方程，再设定变化的阈值γ_k，再由次优H∞滤波公式不断迭代出满足位姿线性组合最优化时的相对位姿。在该系统中经分析，滤波器迭代公式中k+1时刻的估计误差方差方阵P_k+1的迹随k时刻阈值γ_k取值的增大而逐渐减小并趋于稳定，据此采用while循环结构并根据P_k+1斜率变化情况求出P_k+1接近稳定值时的γ_k值：

$while\left|\frac{tr\left(P_{k+1}^{+}\right)-tr\left(P_{k+1}^{-}\right)}{{\mathrm{\&gamma;}}_k^{+}-{\mathrm{\&gamma;}}_k^{-}}\right|<{\mathrm{\&epsiv;}}_2$

${\mathrm{\&gamma;}}_k^{-}={\mathrm{\&gamma;}}_k^{+}$

$P_{k+1}^{-}=P_{k+1}^{+}$

${\mathrm{\&gamma;}}_k^{+}={\mathrm{\&alpha;\&gamma;}}_k^{-}---\left(5\right)$

根据计算的值

end

${\mathrm{\&gamma;}}_k={\mathrm{\&gamma;}}_k^{+},P_{k+1}=P_{k+1}^{+}$

式中，ε₂为阈值，取一初值后，为与大于1的放大倍数α的乘积，为根据对应求出的估计误差方差方阵。这样获得的变化的阈值让滤波器的参数实现了自适应。至此可在双IMU模块和单目视觉模块均测量有效时刻，即在当单目摄像机采集到满特征点图像且图像质量良好时，得到非惯性系N系到目标系B系的相对姿态角和相对位置矢量。而在其他时刻，由双IMU惯性测量方式测量N系到B系的相对位姿信息。

最后用融合得到的姿态角更新姿态角漂移的估计曲线，以实现修正相对角速度的粗补偿值。如图5所示，k时刻的修正量X_k,i为k时刻系统状态量第i个元素，i取值为1、2、3表示数据融合之后的三个相对姿态角。因数据融合时间间隔短，故通过漂移曲线降次化处理，用三个修正后的漂移量估计出一条二次曲线，并作为下一段周期内的漂移曲线，最终得到分段更新的姿态角漂移评定曲线(图5中的分段实线)。

根据相机的视场角度和焦距，将含有特征点的靶标和单目摄像机安装在转台的合适位置，使得转台运动时，摄像机拍摄的靶标基本清晰可见。IMU安装在转台上的位置不能遮挡靶标。测量的环境也应当避免发生迅速变化，在位姿测量期间环境温度宜保持稳定，压力和湿度也宜保持稳定。在传感器按图1安装完成之后，先需对各个坐标系之间的旋转和位置关系进行双矢量标定，标定结果会直接影响方案的实施效果。再进行位姿测量，考虑到惯性测量与视觉测量需要时间同步，所以视觉测量周期设定为惯性测量周期的整数倍，并在初始阶段让惯性测量单元与单目相机同时开始采集数据。

在安装良好、标定可靠的情况下，通过本方案提出的位姿测量方法，可对多组运动状态下的转台运动进行实时定位。

本发明所有测量的内容都是在非惯性系下进行的，不存在惯性系。比如由两个IMU中的陀螺仪输出得到相对角速度，两个IMU中的加速度计输出得到相对加速度，根据这两个相对量得到非惯性下目标物相对运动参考系的相对姿态角和相对位置矢量。

当然，涉及到使用IMU进行测量，IMU包含了多种惯性传感器，即加速度计和陀螺仪，故使用了IMU进行测量的测量方式称为惯性测量方式。

综上所述，本发明提供了一种使用惯性测量方式和视觉测量方式进行组合，来测量非惯性系下目标物位姿的方法。在数据融合时刻，将IMU测量和单目视觉测量的结果进行融合，得到相对位姿量；其余时刻，用IMU测量的结果作为相对位姿量。

在本说明书阐述内容基础上，进行细节改变得到的类似技术方案亦在本说明书涵盖范围之内。

Claims (6)

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

2.如权利要求1所述的双IMU单目视觉组合测量非惯性系下目标物位姿方法，其特征是，目标物与运动参考系之间相对角速度的测量值存在漂移误差，其粗补偿步骤是，采用一个IMU和特征点靶标固连在目标物上，其上的IMU坐标系作为目标系，另一个IMU和CCD摄像机固连在运动平台上，其上的IMU坐标系作为参考系；

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

$\left[\begin{array}{c}\&part;{\widehat{\mathrm{\&phi;}}}_l\\ \&part;{\widehat{\mathrm{\&theta;}}}_l\\ \&part;{\widehat{\mathrm{\&psi;}}}_l\end{array}\right]=\left[\begin{array}{cccc}{d}_{1n}& d_{1(n-1)}& ...& d_{10}\\ d_{2n}& d_{2(n-1)}& ...& d_{20}\\ d_{3n}& d_{3(n-1)}& ...& d_{30}\end{array}\right]\left[\begin{array}{c}{\left(l\mathrm{\&tau;}\right)}^n\\ {\left(l\mathrm{\&tau;}\right)}^{n-1}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\left(l\mathrm{\&tau;}\right)}^0\end{array}\right]---\left(1\right)$

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

tr(P)＝tr[(W^单目-W^IMU)(W^单目-W^IMU)^T]？≥ε₁(3)

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

5.如权利要求4所述的双IMU单目视觉组合测量非惯性系下目标物位姿方法，其特征是，数据融合具体步骤是，根据第k次图像采集时刻IMU测量的位姿量和单目视觉测量的位姿量中对应位姿元素的差异大小，在该元素之前乘上k时刻的关注因子L_k，由此构造出一个新的位姿元素的线性组合Z_k：

作为最优化目标，式中表示选取的状态量，其各元素为第k次图像采集时刻各相对位姿量的值，且X_k与和的各元素形式相同，l_ii为L_k对角线上的元素，1≤i≤6；根据本方案搭建的系统，构造随机非线性离散时间系统，系统方程为：

状态方程中，为状态量，T为视觉采样周期与惯性采样周期之比，k表示第k次视觉采样，t+1表示视觉采样周期内第t+1次惯性采样，所以状态量表示第时刻的相对位姿量，为输入控制量，前三个元素表示时刻B系相对N系的角速度在B系三轴方向上的量值，Γ为6×6的单位矩阵，为时刻未知的系统噪声；观测方程中，Y_k+1为观测量，元素形式与X_k+1相同，Y_k+1取值于k+1时刻单目视觉的测量结果H为6×6的单位矩阵，而V_k+1为k+1时刻未知的观测噪声；用次优H∞滤波估计出基于上述系统方程和公式(4)的位姿信息，步骤为：先线性化状态方程，再设定变化的阈值γ_k，再由次优H∞滤波公式不断迭代出满足位姿线性组合最优化时的相对位姿；滤波器迭代公式中k+1时刻的估计误差方差方阵P_k+1的迹随k时刻阈值γ_k取值的增大而逐渐减小并趋于稳定，据此采用while循环结构并根据P_k+1斜率变化情况求出P_k+1接近稳定值时的γ_k值：

$while\left|\frac{tr\left(P_{k+1}^{+}\right)-tr\left(P_{k+1}^{-}\right)}{{\mathrm{\&gamma;}}_k^{+}-{\mathrm{\&gamma;}}_k^{-}}\right|<{\mathrm{\&epsiv;}}_2$

${\mathrm{\&gamma;}}_k^{-}={\mathrm{\&gamma;}}_k^{+}$

$P_{k+1}^{-}=P_{k+1}^{+}$

${\mathrm{\&gamma;}}_k^{+}={\mathrm{\&alpha;\&gamma;}}_k^{-}---\left(5\right)$

根据计算的值

end

${\mathrm{\&gamma;}}_k={\mathrm{\&gamma;}}_k^{+},P_{k+1}=P_{k+1}^{+}$

式中，ε₂为阈值，取一初值后，为与大于1的放大倍数α的乘积，为根据对应求出的估计误差方差方阵。

6.如权利要求5所述的双IMU单目视觉组合测量非惯性系下目标物位姿方法，其特征是，双IMU模块和单目视觉模块均测量有效时刻，得到融合时刻非惯性系N系到目标系B系的相对姿态角和相对位置矢量；而在其他时刻，由双IMU惯性测量方式测量N系到B系的相对位姿信息；最后用融合得到的姿态角分段更新姿态角漂移的估计曲线，以实现修正相对角速度的粗补偿值；修正的步骤，k时刻修正量为X_k，i为k时刻系统状态量的第i个元素，i取值为1、2、3表示数据融合之后的三个相对姿态角，通过漂移曲线降次化处理，用三个修正后的漂移量估计出一条二次曲线，并作为下一段周期内的漂移曲线，最终得到分段更新的姿态角漂移评定曲线。