一种基于环境约束下的视觉-惯性运动估计方法与流程

本发明属于组合导航定位领域，特别涉及一种基于环境约束下的视觉惯性运动估计方法。

背景技术：

机器人、无人机等运动物体在执行任务时，最重要的是估计它们相对于其工作空间的状态，得到它们的位姿和速度信息以便用于控制、运动规划、导航以及与环境的交互或验证。通常使用运动捕捉系统来获得运动物体的状态估计，但是这些系统通常很昂贵，工作空间大小有限并且不适合户外应用。而成本低廉的惯性传感器(imu)和视觉传感器(相机)在定位时因具有良好的互补性，成为运动状态估计的完美组合。imu可以根据自身的运动信息估计载体的运动，而视觉则以特征点匹配等方式得到自身的运动估计。

目前，许多不同的运动估计算法已经被开发出来。其中，m.faessler[m.faessler,e.mueggler,k.schwabe,andd.scaramuzza,“amonocularposeestimationsystembasedoninfraredleds,”inieeeinternationalconferenceonroboticsandautomation,2014]等，所提出的估计方法在遮挡或运动模糊期间无法提供任何估计，而且只能根据位置和方向估计来计算线速度。j.sola[j.sola,t.vidal-calleja,j.civera,andj.m.m.montiel,“impactoflandmarkparametrizationonmonocularekf-slamwithpointsandlines,”internationaljournalofcomputervision,vol.97,no.3,2012]等人提出了单目slam方法。但是，该测量没有融合惯性测量数据，因此不能提供绝对尺度。m.bryson[m.brysonands.sukkarieh,“buildingarobustimplementationofbearing-onlyinertialslamforauav,”journaloffieldrobotics,vol.24,no.1-2,pp.113–143,2007.]等人出了一种基于人工标记的slam方法。但是，此处基准点仅表示为点特征，因此仅估计了基准点的3d位置。

技术实现要素：

本发明旨在解决以上现有技术的问题。提出了一种基于环境约束下的视觉惯性运动估计方法，将纸质的人工地标(具有6dof的标签)和单目视觉惯性slam相结合，使用扩展卡尔曼滤波器(ekf)将惯性测量值和对人工视觉标记上角点的观测紧密地融合在一起，可精确的估算出运动物体相对于工作空间的位姿和速度的信息，并且该方法可在室内或室外场景中使用，以方便将其应用于控制、运动规划、导航等任务中。本发明的技术方案如下：

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

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

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

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

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

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

进一步的，在所述步骤2中，单目相机获取未失真的照片，经过apriltag检测器的处理，为每个检测到的标签相关联的图像分配唯一标识符号id，并估计相机相对于标签3d位置、方向，该估计值通过迭代优化重投影误差获得，其中，重投影误差是被投影的标签上的角点和它们在图像空间的检测值之间的误差。

进一步的，所述步骤3中相机观测模型为：

根据针孔相机模型成像原理，一个在相机坐标系下的3d点p(x,y,z)，则它在像素坐标对应的点为：

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

将此公式简化成：

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

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

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

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

e＝pi-fpci(5)

进一步的，所述步骤3中的imu模型为：

定义imu坐标系为i,全局坐标系为g，imu的状态向量表示为：

其中，ⁱqg(t)、^gvi(t)和^gpi(t)分别代表了imu坐标系i相对于全局坐标系g的旋转、速度和位移，bg(t)和ba(t)分别代表了陀螺仪和加速度计的噪声；

系统状态随时间演变的模型是：

其中，ⁱω和^ga是角速度和线性加速度，ω是一个矩阵，它里面包含用于更新姿态四元数所需要的三轴陀螺信息，nwg和nwa代表了imu的高斯白噪声；

ω3、ω2、ω1分别表示3轴陀螺的输出；

陀螺仪和加速计的测量值可以表示为：

是四元数向对应的旋转矩阵；am(t)表示上述的加速度测量值。

进一步的，所述建立ekf滤波器的状态和预测模型具体包括：

为了传递运动载体的状态，该滤波器使用惯导测量值，把标签的位姿也包含到滤波器状态中，并根据可用的标签角测量值执行更新，根据前面建立的传感器模型，可以建立滤波器的状态方程：

x＝[pvqbgbaptqtpicqic](9)

其中,p、v、q分别表示运动载体的位置，速度和姿态，pt和qt分别表示标签的位姿信息，pic和qic分别表示相机到imu的转换的位姿信息，将这些状态求导，联立imu状态方程可得：

加入了额外的连续高斯白噪声wp、wpf、wpc和wqv,以激活整个滤波状态，以激活整个滤波状态，并对状态离散化造成的建模误差进行处理。

进一步的，在所述步骤(4)中,，当检测器观察到标签时，使用标签角点的重投影误差作为ekf滤波器的更新项来执行更新步骤，最后得到精准的运动估计，具体包括：

对于每一个标签角i,基于公式(4),定义更新项为yi，既标签角点角点的重投影误差为：

使用当前时刻的对标签的观测值来更正预测阶段的估计值，不断跟新滤波器状态，得到运动物体精准的运动估计。

本发明的优点及有益效果如下：

本发明可精确的估算出运动物体相对于工作空间的位姿和速度的信息，以方便将其应用于控制、运动规划、导航等任务中。创新点具体是步骤3与步骤4，在此过程中可以实现以下有益效果：

(1)精度高、可靠性强：该系统将惯性测量值和人工地标(apriltag)的观测紧密地融合在一起，通过使用提供丰富信息的人工地标，在最大程度地减少估计、构图、回环检测的工作的同时，可精确的估算出运动物体。此外，由于apriltag具有6自由度，所以单次测量完全限制了6自由度的相对位姿，因此单地标就足以估计位姿。

(2)相机观测模型稳健：所采用的视觉测量模型的优势在于，可以直接在检测到的角点的像素位置上建模噪声，因为标签的3d角点的重新误差噪声在很大程度上与摄像机的姿态无关，且恒定。虽然检测器提供了相对于当前摄像机帧的标签姿态估计，并且可以直接在滤波器中使用，但由于该噪声的大小很大程度上取决于标签在相机框架中的位置和方向，因此很难为该相对姿态拟合准确的噪声模型。

(3)成本低廉、适用性强：apriltag标签可以使用纸质打印，成本低，方便部署。视觉和惯导融合增加了系统运动估计的鲁棒性，可应用于室内室外的多场景中。

附图说明

图1是本发明提供优选实施例一种基于环境约束下的视觉-惯性运动估计方法流程图。

图2是相机针孔模型示意图。

图3是相机观测apriltag时相对位姿估计示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是：

一种基于环境约束下的视觉惯性运动估计方法，包括以下步骤：

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

步骤(2)，由相机获取未失真的图像，然后由apriltag标签检测器处理图像，分配给图像与标签相关联的唯一标识符号(id)。此外，它利用标签上3d角点的重投影误差来估计每个标签和相机之间的相对转换

步骤(3)，通过分析相机观测模型、imu测量模型来建立ekf滤波器的状态和预测模型。

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

所述步骤(1)中，安装在运动载体上安装捷联惯导系统和单目相机，他们共同组成视觉-惯性紧耦合组合导航系统，惯性导航系统可根据自身采集到的数据来传播载体运动，单目相机则通过图像特征匹配来估计自身的状态，在本方法中，重点研究标签上角点的检测与匹配。apriltag标签相似于二维码标签，具有6dof(位置和姿态)，可以用普通打印机创建，但它比二维码标签更容易被检测和快速解码。本此方法中，我们可以将apriltag标签放置在室内或室外场景中。

所述步骤(2)中,相机获取未失真的照片，经过apriltag检测器的处理，可以为每个检测到的标签相关联的图像分配唯一标识符号(id)，并估计相机相对于标签3d位置、方向，该估计值通过迭代优化重投影误差获得，其中，重投影误差是被投影的标签上的角点和它们在图像空间的检测值之间的误差。

所述步骤(3)中,通过分析相机观测模型、imu测量模型来建立ekf滤波器的状态和预测模型。

相机观测模型：

根据针孔相机模型成像原理，一个在相机坐标系下的3d点p(x,y,z)，则它在像素坐标对应的点为：

可以将此公式简化成：

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

虽然检测器可以提供相机相对于当前标签的姿态估计，并在滤波器中不断优化，但是该噪声的大小严重依赖当前标签在相机框架中的位置和方向，此很难将拟合准确的噪声模型。因此，该方法使用观测标签的观测模型，可以直接在检测到的角点的像素位置上给噪声建模。利用上述数量投影到图像平面上可得出对应的像素坐标测量值，其中我们假设其高斯噪声模型np,i～σ(0,rp)

所选视觉测量模型优势在于，标签上角点的投影的噪声在很大程度上与摄像机的姿态无关，因此可以假定对于所有标签和测量而言，噪声是恒定且相同的。

imu模型：

定义imu坐标系为i,全局坐标系为g。imu的状态向量可以表示为：

其中，ⁱqg(t)、^gvi(t)和^gpi(t)分别代表了imu坐标系i相对于全局坐标系g的旋转、速度和位移，bg(t)和ba(t)分别代表了陀螺仪和加速度计的噪声。

系统状态随时间演变的模型是：

其中，ⁱω和^ga是角速度和线性加速度，nwg和nwa代表了imu的高斯白噪声,

陀螺仪和加速计的测量值可以表示为：

是四元数向对应的旋转矩阵。

为了传递运动载体的状态，该滤波器使用惯导测量值，把标签的位姿包含到滤波器状态中，来并根据可用的标签角测量值执行更新。根据前面建立的传感器模型，我们可以建立滤波器的状态方程

x＝[pvqbgbaptqtpicqic](28)

其中,p、v、q分别表示运动载体的位置，速度和姿态，pt和qt分别表示标签的位姿信息，pic和qic分别表示相机到imu的转换的位姿信息。将这些状态求导，联立imu状态方程可得:

我们加入了额外的连续高斯白噪声过程wp、wpt、wqt、wpc和wqv,以激活整个滤波状态，并对随后的状态离散化造成的建模误差进行处理。

所述步骤(4)中,，当检测器观察到标签时，使用标签角点的重投影误差作为ekf滤波器的更新项来执行更新步骤，最后得到精准的运动估计。

对于每一个标签角i,基于公式(4),定义更新项为yi：

然后利用更新扩展卡尔曼更新公式不断跟新滤波器状态，得到运动物体精准的运动估计。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。