[发明专利]摄像头与惯性传感器组合定位定姿系统的标定方法

权利要求书

1.摄像头与惯性传感器组合定位定姿系统的标定方法，包括以下步骤：

步骤S1：标定摄像头的内参数矩阵；

步骤S2：对于一个尺寸已知的标定物从不同角度拍摄多幅图像，同时记录在拍摄每幅图像时惯性传感器输出的横滚角和俯仰角信息；

步骤S3：定义世界坐标系、摄像机坐标系、惯性传感器坐标系和地磁坐标系；

步骤S4：基于每幅图像中标定物的图像信息和空间信息，计算该时刻世界坐标系到摄像机坐标系的旋转矩阵；

步骤S5：将所拍摄图像两两组合生成多个图像对，对于每个图像对建立一个关于惯性传感器坐标系到摄像机坐标系旋转矩阵R_m-c的方程组：

Rw-c,ij×Rm-c×-sinβjsinαjcosβjcosαjcosβj=Rm-c×-sinβisinαicosβicosαicosβi,]]>

其中，R_w-c，ij表示摄像机坐标系在i、j两时刻之间的旋转矩阵，α_i，α_j分别表示惯性传感器在i、j时刻的横滚角，β_i，β_j分别表示惯性传感器在i、j时刻的俯仰角，则对于多个图像对，利用惯性传感器输出的横滚角和俯仰角，可建立关于惯性传感器坐标系到摄像机坐标系旋转矩阵R_m-c的多个方程组；

步骤S6：将关于惯性传感器坐标系到摄像机坐标系旋转矩阵R_m-c的多个方程组转化为一个含约束的非线性优化问题，求解所述含约束的非线性优化问题则计算出惯性传感器坐标系到摄像机坐标系的旋转矩阵R_m-c；

步骤7：对于每幅图像建立一个关于地磁坐标系到世界坐标系旋转矩阵R_e-w的方程组：

cosβisinαisinβicosαisinβi0cosαi-sinαi-sinβisinαicosβicosαicosβi×Rm-c-1×Rw-c,i×Re-w=cosγisinγi0-sinγicosγi0001,]]>

其中，R_w-c，i表示在i时刻世界坐标系到摄像机坐标系的旋转矩阵，R_m-c表示惯性传感器坐标系到摄像机坐标系的旋转矩阵，α_i表示惯性传感器在i时刻的横滚角，β_i表示惯性传感器在i时刻的俯仰角，γ_i表示惯性传感器在i时刻的航向角；利用上式两边对应项相等的性质以及旋转矩阵的正交性约去有关惯性传感器航向角γ_i的计算项，可得如下线性方程组：

B_iV_e-w＝b_i，

其中B_i，b_i分别为上式经过化简后的系数矩阵与常数项，V_e-w是地磁坐标系到世界坐标系旋转矩阵R_e-w的向量化表示形式，则对于多幅图像，利用惯性传感器输出的横滚角和俯仰角，可建立关于地磁坐标系到世界坐标系旋转矩阵R_e-w的多个方程组，进而建立关于地磁坐标系到世界坐标系旋转矩阵R_e-w的向量化V_e-w的多个线性方程组；

步骤S8：将关于地磁坐标系到世界坐标系旋转矩阵R_e-w的向量化V_e-w的多个线性方程组转化为一个含约束的非线性优化问题，求解所述含约束的非线性优化问题则计算出地磁坐标系到世界坐标系的旋转矩阵。