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

摘要

本发明为摄像头与惯性传感器组合定位定姿系统的标定方法，包括：标定摄像头的内参数矩阵；对于一个尺寸已知的标定物从不同角度拍摄多幅图像，记录在拍摄每幅图像时惯性传感器输出的横滚角和俯仰角；定义世界坐标系、摄像机坐标系、惯性传感器坐标系和地磁坐标系；基于每幅图像中标定物的图像信息和空间信息，计算该时刻世界坐标系到摄像机坐标系的旋转矩阵；将摄取图像两两组合，对于每个组合均建立一个关于惯性传感器坐标系到摄像机坐标系旋转矩阵的方程组并进行求解，计算出惯性传感器坐标系到摄像机坐标系的旋转矩阵；对于每幅图像建立一个关于地磁坐标系到世界坐标系旋转矩阵的方程组并进行求解，计算出地磁坐标系到世界坐标系的旋转矩阵。

主权项

摄像头与惯性传感器组合定位定姿系统的标定方法，包括以下步骤：步骤S1：标定摄像头的内参数矩阵；步骤S2：对于一个尺寸已知的标定物从不同角度拍摄多幅图像，同时记录在拍摄每幅图像时惯性传感器输出的横滚角和俯仰角信息；步骤S3：定义世界坐标系、摄像机坐标系、惯性传感器坐标系和地磁坐标系；步骤S4：基于每幅图像中标定物的图像信息和空间信息，计算该时刻世界坐标系到摄像机坐标系的旋转矩阵；步骤S5：将所拍摄图像两两组合生成多个图像对，对于每个图像对建立一个关于惯性传感器坐标系到摄像机坐标系旋转矩阵Rm‑c的方程组： R w - c , ij × R m - c × - sin β j sin α j cos β j cos α j cos β j = R m - c × - sin β i sin α i cos β i cos α i cos β i , 其中，Rw‑c，ij表示摄像机坐标系在i、j两时刻之间的旋转矩阵，αi，αj分别表示惯性传感器在i、j时刻的横滚角，βi，βj分别表示惯性传感器在i、j时刻的俯仰角，则对于多个图像对，利用惯性传感器输出的横滚角和俯仰角，可建立关于惯性传感器坐标系到摄像机坐标系旋转矩阵Rm‑c的多个方程组；步骤S6：将关于惯性传感器坐标系到摄像机坐标系旋转矩阵Rm‑c的多个方程组转化为一个含约束的非线性优化问题，求解所述含约束的非线性优化问题则计算出惯性传感器坐标系到摄像机坐标系的旋转矩阵Rm‑c；步骤7：对于每幅图像建立一个关于地磁坐标系到世界坐标系旋转矩阵Re‑w的方程组： cos β i sin α i sin β i cos α i sin β i 0 cos α i - sin α i - sin β i sin α i cos β i cos α i cos β i × R m - c - 1 × R w - c , i × R e - w = cos γ i sin γ i 0 - sin γ i cos γ i 0 0 0 1 , 其中，Rw‑c，i表示在i时刻世界坐标系到摄像机坐标系的旋转矩阵，Rm‑c表示惯性传感器坐标系到摄像机坐标系的旋转矩阵，αi表示惯性传感器在i时刻的横滚角，βi表示惯性传感器在i时刻的俯仰角，γi表示惯性传感器在i时刻的航向角；利用上式两边对应项相等的性质以及旋转矩阵的正交性约去有关惯性传感器航向角γi的计算项，可得如下线性方程组：BiVe‑w＝bi，其中Bi，bi分别为上式经过化简后的系数矩阵与常数项，Ve‑w是地磁坐标系到世界坐标系旋转矩阵Re‑w的向量化表示形式，则对于多幅图像，利用惯性传感器输出的横滚角和俯仰角，可建立关于地磁坐标系到世界坐标系旋转矩阵Re‑w的多个方程组，进而建立关于地磁坐标系到世界坐标系旋转矩阵Re‑w的向量化Ve‑w的多个线性方程组；步骤S8：将关于地磁坐标系到世界坐标系旋转矩阵Re‑w的向量化Ve‑w的多个线性方程组转化为一个含约束的非线性优化问题，求解所述含约束的非线性优化问题则计算出地磁坐标系到世界坐标系的旋转矩阵。