[发明专利]一种单目相机和惯性测量单元相对安装角标定方法

权利要求书

1.一种单目相机和惯性测量单元相对安装角标定方法，其特征在于，具体包括步骤：

步骤一：转动测量组件，采集标定数据；所述的测量组件包括单目相机和惯性测量单元；采集的标定数据包括陀螺仪测量的角速度和单目相机采集的图像；

步骤二：从单目相机采集的图像中提取相对姿态信息；

步骤三：利用非线性最小二乘法求解安装矩阵，实现单目相机和惯性测量单元相对安装角的标定；

利用非线性最小二乘法求解安装矩阵包括设计代价函数、非线性最小二乘法优化和对于不同参考坐标系代价函数优化结果的选择；

(1)设计代价函数；

①固定参考形式的代价函数设计；

定义i系为惯性参考系，选择标定时间段的起始瞬时的相机坐标系为参考坐标系，标定过程中所有采样时刻陀螺仪测量的角速度经过如下推导得到c系相对于c₁系的角速度：

其中表示陀螺仪测量的角速度，即b系相对于i系惯性参考系的旋转角速度在b系下的投影；表示c₁系相对于i系惯性参考系的旋转角速度在b系下的投影；表示b系相对于c₁系的旋转角速度在b系下的投影，c₁系为惯性系；表示c系相对于c₁系的旋转角速度在b系下的投影；表示b系相对于c系的旋转角速度在b系下的投影；表示c系相对于c₁系的旋转角速度在b系下的投影，所述b系为与惯性测量单元固连的载体系，右手系，三轴方向分别为右-下-前，陀螺的测量输出即为角速度在该b系三轴投影坐标；每个瞬时的载体系也是一个惯性系；

安装矩阵定义为由b系到c系的方向余弦阵，三个安装误差角分别为方位误差角ψ、俯仰误差角θ和滚转误差角γ，通过先绕y_b轴旋转ψ，再绕x_b轴旋转θ，最后绕z_b轴旋转γ，的表达式如下：

设定一个安装矩阵估计值

表示c₁系到c系的方向余弦阵的估计值；表示c系相对于c₁系的旋转角速度在c系下的投影的估计值；表示安装矩阵估计值；表示c系相对于c₁系的旋转角速度在b系下的投影，等于陀螺仪测量的角速度表示角速度对时间积分，将得到旋转矢量；∧运算符表示由旋转矢量求反对称阵；exp(·)为矩阵指数映射，将反对称阵转化为三维特殊正交群SO(3)的元素；

安装矩阵由下述任一代价函数得到：

其中，k＝1,...,n表示单目相机的每个采样时刻,n表示采样总数；表示旋转矩阵的转置；函数eulr(·)表示由方向余弦阵求欧拉角；矩阵对数映射log(·)表示将三维特殊正交群SO(3)中的方向余弦阵转换到中的旋转矢量反对称阵；∨运算符表示由反对称阵求旋转矢量；∧运算符表示由旋转矢量求反对称阵；函数qtn(·)表示由方向余弦阵求四元数；表示四元数乘法；||·||₂表示求矩阵二范数；I表示单位矩阵，

②增量形式的代价函数设计；

标定过程中单目相机采集的第k-1帧图像和第k帧图像间，陀螺仪测量的角速度经过推导得到c系相对于瞬时的c_k-1系的角速度：

其中表示陀螺仪测量的角速度，即b系相对于惯性系的旋转角速度在b系下的投影；表示瞬时的c_k-1系相对于惯性系的旋转角速度在b系下的投影；表示b系相对于瞬时的c_k-1系的旋转角速度在b系下的投影，由于任意瞬时的c_k-1系为惯性系；表示c系相对于瞬时的c_k-1系的旋转角速度在b系下的投影；表示b系相对于c系的旋转角速度在b系下的投影，表示c系相对于瞬时的c_k-1系的旋转角速度在b系下的投影；

设定一个安装矩阵估计值

表示瞬时的c_k-1系到瞬时的c_k系的方向余弦阵的估计值；表示c系相对于瞬时的c_k-1系的旋转角速度在c系下的投影的估计值；表示安装矩阵估计值；表示c系相对于瞬时的c_k-1系的旋转角速度在b系下的投影；表示角速度对时间积分，将得到旋转矢量；

安装矩阵由下述任一代价函数得到：

③引入李代数的代价函数设计；

在代价函数中令

其中exp(·)为矩阵指数映射，将反对称阵转化为三维特殊正交群SO(3)的元素；θ_bc表示安装旋转矢量；∧运算符表示由旋转矢量求反对称阵，上述代价函数的形式变化如下，

表示t时刻陀螺仪测量得到的角速度；

(2)非线性最小二乘法优化；

优化上述相应的代价函数F_jq(θ_bc)获得安装矩阵的过程如下：

其中，j＝1,2，q＝1,2,3,4；

(3)对于不同参考坐标系代价函数优化结果的选择；

确定一种代价函数优化的结果为最终的安装矩阵，设采用一个固定参考形式代价函数和一个增量形式代价函数求得的方向余弦阵分别为和具体方法如下：

利用矩阵对数映射log(·)以及∨运算符将和分别转换为对应的安装角旋转矢量和

求和模值之差的绝对值：

其中norm(·)为向量求模运算符；abs(·)为求绝对值运算符；

当Δσ小于阈值时，认为采用不同参考坐标系的代价函数估计结果接近，在和中取任意一个即可；

当Δσ大于等于阈值时，则将两种参考坐标系代价函数的估计结果分别代入选取的两个代价函数中，以引入李代数的代价函数为例，即分别求取和和m和n在1～4中任取一值；若判断且则认为固定参考系形式代价函数的估计结果好，最终安装矩阵取若判断且则认为增量形式代价函数的估计结果好，最终安装矩阵取