[发明专利]一种基于环境约束下的视觉-惯性运动估计方法

权利要求书

1.一种基于环境约束下的视觉-惯性运动估计方法，其特征在于，包括以下步骤：

步骤1，在载体上安装捷联惯导系统和单目相机，完成捷联惯导系统和单目相机的校准，并在场景内放置AprilTag标签；

步骤2，由单目相机获取未失真的图像，然后由AprilTag标签检测器处理单目相机获取的图像，分配给图像与标签相关联的唯一标识符号id，标签检测器通过迭代优化标签上3D角点的重投影误差来估计每个标签和单目相机之间的相对转换；

步骤3，通过分析IMU测量模型和相机观测模型，然后把标签的位姿也包含到滤波器状态中，来建立EKF滤波器的状态和预测模型；

步骤4，使用标签角点的重投影误差作为EKF滤波器的更新项来执行更新步骤，最后得到精准的运动估计。

2.根据权利要求1所述的一种基于环境约束下的视觉-惯性运动估计方法，其特征在于，在步骤1中，在运动载体上安装捷联惯导系统和单目相机，他们共同组成视觉-惯性紧耦合组合导航系统，捷联惯性导航系统可根据自身采集到的数据来描述载体运动，单目相机则通过图像特征匹配来估计自身的状态，AprilTag标签是一种比较成熟的视觉标签，常用于机器人和无人机定位，比二维码先进，相似于二维码标签，具有6DOF位置和姿态信息，它们能够更可靠地从更长的范围内被检测到，所以可将AprilTag标签放置在室内或室外场景中。

3.根据权利要求1所述的一种基于环境约束下的视觉-惯性运动估计方法，其特征在于，在所述步骤2中，单目相机获取未失真的照片，经过AprilTag检测器的处理，为每个检测到的标签相关联的图像分配唯一标识符号id，并估计相机相对于标签3D位置、方向，该估计值通过迭代优化重投影误差获得，其中，重投影误差是被投影的标签上的角点和它们在图像空间的检测值之间的误差。

4.根据权利要求1所述的一种基于环境约束下的视觉-惯性运动估计方法，其特征在于，所述步骤3中相机观测模型为：

根据针孔相机模型成像原理，一个在相机坐标系下的3D点P(X,Y,Z)，则它在像素坐标对应的点为：

其中K被称为相机的内参数矩阵，fx、fy、cx、cy是相机内参数。

将此公式简化成：

其中代表了相机像素坐标下的点，P_C代表了相机坐标下的点。

定义相机坐标系为C，标签的坐标系为t，其中标签坐标系的原点是April tag的几何中心，z轴和标签平面垂直；当检测器第一次检测到标签时，为相机提供最初的位姿信息，在随后的检测过程估计每个标签和相机之间的相对转换，给定特定标签相对于相机框架的相对位置和姿态P_Ct和q_tC，计算从相机看到的第i个标签角点P_Ci固定在标签坐标系T上的位置：

在检测到的角点的像素位置上给噪声建模，利用数量投影到图像平面上可得出对应的像素坐标测量值假设其高斯噪声模型n_p,i～σ(0,R_p)：R_p、n_p,i,就是一个高斯噪声模型，符合(0,R_p)的正态分布，

根据相机的重投影误差定义：将像素坐标(观测到的投影位置)与3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差，即重投影误差e可以表示为：

e＝P_i-fP_Ci (5) 。