本发明属于视觉导航与组合导航技术领域,具体涉及一种惯导/相机安装误差标定方法。
背景技术:
视觉导航以传感器获取的客观事物的视觉图像为基础,通过计算机来模拟人的视觉功能,对图像进行识别和理解,进而获取载体的导航信息。
视觉导航的传感器即光学相机。将惯导与相机捷联安装在一起,形成惯性/视觉组合导航系统,可以将惯性导航的绝对导航信息与视觉导航的相对导航信息进行融合,特别适用于无人机自主着陆、智能机器人导航、工业测量等领域。而惯性/视觉组合导航是建立在惯导/相机安装误差的精密标定基础上的。
惯导与相机捷联安装在一起,惯导坐标系与相机坐标系之间存在一个姿态转转与平移的关系,这个关系称之为相对姿态与相对位置,也称之为姿态安装误差与位置安装误差,统称为安装误差。
技术实现要素:
根据上述问题,本发明提供一种惯导/相机安装误差标定方法,完成惯性/视觉组合导航系统中,惯导与相机姿态安装误差与位置安装误差的精密标定问题。
为了实现这一目的,本发明采取的技术方案是:
一种惯导/相机安装误差标定方法,该方法包括如下步骤:
步骤一、系统安装及其连接方式;步骤二、转台旋转并记录数据;步骤三、构建12个未知量的方程组并计算;步骤四、输出结果;
所述系统安装及其连接方式,将惯导与相机安装在转台上,将标定板放置在转台附近,且处于相机的视场范围内。
一种惯导/相机安装误差标定方法,步骤二、旋转转台并记录数据:
系统安装完成后,旋转转台4次,旋转时要求保证:
1、惯导与相机之间的相对位置与相对姿态不变;
2、标定板位置保持不变,从而保证转台与标定板之间的相对位置与相对姿态不变;
3、转台的XYZ三个轴同时旋转一定的角度(三个轴旋角的大小与方向不要求一致);
4、旋转时要求保证标定板不超出相机的视场范围;
记录4个状态下惯导与转台之间的标定矩阵相机与标定板之间的标定矩阵其中通过光瞄与转台旋转参数获得,通过高精度光学测量方法获得。
一种惯导/相机安装误差标定方法,步骤三、构建12个未知量的方程组并计算:
假设某空间点P由坐标系a中的坐标Pa转换为坐标系b中的坐标Pb,其过程可表示为,
公式(1),其中Pa=[xa,ya,za]T为空间点P在坐标系a中的坐标;是一个平移向量,其中tx为由坐标系a向坐标系b平移过程中x方向的平移量,ty为由坐标系a向坐标系b平移过程中y方向的平移量,tz为由坐标系a向坐标系b平移过程中x方向的平移量;是一个旋转矩阵,描述了从坐标系a向坐标系b的旋转过程,即坐标系a按的旋转顺序,分别旋转后,其三轴将与坐标系b平行,
在和T中的6个参数(α,β,γ,tx,ty,tz)描述了坐标系a与坐标系b之间的相对位置与相对姿态关系,安装误差标定就是要把这6个参数求解出来;
令
公式(9)可以整理成以下形式
其中
与分别称为空间点P在坐标系a向坐标系b下的齐次坐标,中的元素由标定参数计算而来,称之为标定矩阵;
将惯导与相机安装在转台上,将标定板放置在转台附近,且处于相机的视场范围内,转台、惯导、相机、标定板四者之间的关系可写成归一化矩阵连乘的形式:
式中:
——4×4矩阵,惯导与转台之间的标定矩阵,通过光瞄与转台旋转参数方法求得,已知量;
——4×4矩阵,惯导与相机之间的标定矩阵,未知量,待求;
——4×4矩阵,相机与标定板之间的标定矩阵,通过光学测量方法求得,已知量;
——4×4矩阵,标定板与转台之间的标定矩阵,未知量;
在上面的连乘公式中,与这两个未知的4×4矩阵中可以由与共12个未知量表示;
将式(14)写成以下形式:
可以整理出3个以为变量的方程,分别表示为:
因此,得到该状态下12个未知量的3个方程;
转台旋转4次,每次都获得该状态下的3个方程,一共可以获得4个状态下12个未知量的12个方程,表示如下:
采用通用的矩阵分解法、最小二乘法求解以上线性方程组,得到线性方程组的解,获得与共12个未知量的值。
一种惯导/相机安装误差标定方法,步骤四、输出结果,将步骤三中的作为惯导与相机之间的安装误差输出。
本发明的有益效果为:
本发明提出的标定方法无需激光扫描仪与差分卫星导航系统等繁重的设备,而是利用转台与标定板,在室内即可完成惯导与相机之间安装误差的标定,提高了工作效率,降低了标定成本。
具体实施方式
本发明具体实施例如下:
一种惯导/相机安装误差标定方法,包括下述步骤:
(1)系统安装及其连接方式;(2)转台旋转并记录数据;(3)构建12个未知量的方程组并计算;(4)输出结果,完成惯导/相机安装误差标定。
步骤一:系统安装及其连接方式
将惯导与相机安装在转台上,将标定板放置在转台附近,且处于相机的视场范围内。
步骤二:旋转转台并记录数据
系统安装完成后,旋转转台4次,旋转时要求保证:
(1)惯导与相机之间的相对位置与相对姿态不变;
(2)标定板位置保持不变,从而保证转台与标定板之间的相对位置与相对姿态不变;
(3)转台的XYZ三个轴同时旋转一定的角度(三个轴旋角的大小与方向不要求一致);
(4)旋转时要求保证标定板不超出相机的视场范围。
记录4个状态下惯导与转台之间的标定矩阵相机与标定板之间的标定矩阵其中通过光瞄与转台旋转参数获得,通过高精度光学测量方法获得。
步骤三:构建12个未知量的方程组并计算
假设某空间点P由坐标系a中的坐标Pa转换为坐标系b中的坐标Pb,其过程可表示为
公式(1),其中Pa=[xa,ya,za]T为空间点P在坐标系a中的坐标;是一个平移向量,其中tx为由坐标系a向坐标系b平移过程中x方向的平移量,ty为由坐标系a向坐标系b平移过程中y方向的平移量,tz为由坐标系a向坐标系b平移过程中x方向的平移量;是一个旋转矩阵,描述了从坐标系a向坐标系b的旋转过程,即坐标系a按的旋转顺序,分别旋转后,其三轴将与坐标系b平行,
在和T中的6个参数(α,β,γ,tx,ty,tz)描述了坐标系a与坐标系b之间的相对位置与相对姿态关系,安装误差标定就是要把这6个参数求解出来。
令
公式(9)可以整理成以下形式
其中
与分别称为空间点P在坐标系a向坐标系b下的齐次坐标,中的元素由标定参数计算而来,称之为标定矩阵。
在本发明中,需要用到转台、惯导、相机、标定板四个装置,假设这4个装置分别定义了四个坐标系,分别称之为坐标系base、坐标系imu、坐标系cam、坐标系cal。
将惯导与相机安装在转台上,将标定板放置在转台附近,且处于相机的视场范围内,转台、惯导、相机、标定板四者之间的关系可写成归一化矩阵连乘的形式:
式中:
——4×4矩阵,惯导与转台之间的标定矩阵,通过光瞄与转台旋转参数方法求得,已知量;
——4×4矩阵,惯导与相机之间的标定矩阵,未知量,待求;
——4×4矩阵,相机与标定板之间的标定矩阵,通过光学测量方法求得,已知量;
——4×4矩阵,标定板与转台之间的标定矩阵,未知量。
在上面的连乘公式中,与这两个未知的4×4矩阵中可以由与共12个未知量表示。
将式(14)写成以下形式:
可以整理出3个以为变量的方程,分别表示为:
因此,得到该状态下12个未知量的3个方程。
转台旋转4次,每次都获得该状态下的3个方程,一共可以获得4个状态下12个未知量的12个方程,表示如下:
采用通用的矩阵分解法、最小二乘法求解以上线性方程组,得到线性方程组的解,获得与共12个未知量的值。
步骤四:输出结果
将步骤三中的作为惯导与相机之间的安装误差输出。