一种基于遗传算法的视觉惯性组合的SLAM方法与流程

本发明涉及导航领域，特别是涉及一种基于遗传算法的视觉惯性组合的SLAM方法。

背景技术：

在陌生环境下自主移动的机器人成为目前机器人领域研究的热点问题，与传统机器人不同的是，自主式机器人不依赖人为控制，不受环境限制，在一些危险场所的紧急救援中，可以起到关键的作用。同时，机器人SLAM(simultaneous localization and mapping，即时定位与同步构图)不仅可以完成自身准确的定位，还能将环境特征提取出来，用以构建地图，在陌生环境中，机器人只需要经过有限次的行走就可以将整个环境地图构建，从而在实际情况中，有助于我们快速了解事故现场的情况。因此，研究使机器人能够高精度的定位导航以及环境地图的精确绘制具有十分重要的理论价值以及实际应用。

惯性导航是以经典力学里的牛顿第二定律为理论依据发展起来的，它通过惯性仪器陀螺仪和加速度计测量机器人的旋转角速度和加速度，通过积分求解其姿态，速度，位置信息。其稳定性高，隐蔽性好，不易受外部环境的影响，且完全依赖自身的惯性器件，具有良好的自主性。不过由于其总是通过当前时刻的信息量来推算下一时刻的信息，因此会产生一定量的累积误差。在使用惯性导航工作了一段时间后，其精度会逐渐发散。所以惯性导航不适合长时间，长距离的导航。

视觉导航是新兴的导航技术，主要是计算机视觉以及图像处理技术的高速发展。它通过摄像头拍摄图片，根据图像处理提取特征点进行运动建模，并实现三维重建。通过视觉里程计模型准确输出其位置以及航向信息。视觉传感器类似于人的眼睛，在所有传感器中探测范围最广，探测的信息最为丰富。并且由于视觉摄像头可以观测到全局特征路标，可以修正视觉里程计长时间工作的累积误差，其精度不会受时间影响。但是其易受环境条件的影响，如光照可能会影响某些摄像头。并且图像特征点提取的准确性、匹配的快速性，以及数据关联的准确性实时性都会极大的影响到视觉导航，并且视觉导航的数据更新较慢。

技术实现要素：

发明目的：为了解决现有的机器人SLAM技术的不足，本发明提出的一种基于遗传算法的视觉惯性组合导航的SLAM方法。

技术方案：一种基于遗传算法的视觉惯性组合的SLAM方法，包括如下步骤：

1)坐标系的统一与摄像机的标定：惯导下的坐标系包括机体坐标系与导航坐标系，视觉下的坐标系包括图像坐标系、摄像机坐标系和导航坐标系，IMU与双目摄像机均捷联在机器人上，图像坐标系与摄像机坐标系的关系由摄像机内参表示，导航坐标系取作当地地理坐标系，摄像机坐标系与导航坐标系的关系表示为一个旋转矩阵R和一个平移向量通过对双目摄像机的标定确定旋转矩阵R与平移向量这些摄像机参数，进行像素点的三维重构；

2)惯性导航解算：利用捷联在机器人上的IMU进行测量，IMU上的陀螺仪负责测量运动姿态，并建立姿态矩阵，IMU上的加速度计测量机体下的加速度，并通过姿态矩阵变换至导航坐标系中，通过积分求解速度、位置；

3)视觉导航解算：利用装载机器人头部的双目摄像机进行图像的获取，对图像进行降噪处理后利用BRISK算法提取其特征点，根据步骤1)标定好的摄像机参数并通过跟踪特征点坐标变化来估计机器人的三维运动，解算其姿态、速度；

4)信息融合：利用扩展卡尔曼滤波器对视觉信息与惯导信息进行融合，以惯导模型建立滤波器状态方程，并取部分惯导输出位姿信息及视觉导航输出信息作为观测变量建立滤波器观测方程，建立系统的滤波器模型；

5)全局路标的修正以及构图：利用双目摄像机观测到的图像中提取的特征点作为特征路标，并基于遗传算法与地图已有的特征点库做数据关联，将扩展后的状态量反馈回滤波器中，同时，根据特征点的关联性判断该特征点是否应该加入地图中，并对地图中的特征点进行重定位，完成同步构图。

所述步骤1)中坐标系的统一与摄像机的标定具体步骤包括：

(1.1)建立惯性导航的机体坐标系b，并取当地的地理坐标系作为导航坐标系n，其间的关系可以表示为一个姿态矩阵双目摄像机所得二维图像平面即为图像坐标系平面c，其经过自身内参矩阵的变换可以得到摄像机坐标系w；其关系表示为：取其左上角点为原点，取当地地理坐标系为视觉中的导航坐标系；其间的关系表示为一个旋转矩阵R和一个平移向量

(1.2)摄像机的标定即确定二维图像像素点与三维坐标的对应关系；设图像坐标系下二维图像点坐标为m＝[u_c，v_c]^T，三维空间点的坐标为M＝[X,Y,Z]^T，根据小孔成像原理，它们的关系为sm＝A[R,t]M；s称为比例系数，A是摄像机的内参，其标准形式为

设摄像机坐标系两轴的像素点分别为dx，dy；令f表示摄像机焦距；则上式表示为：

设每张图片的单应性矩阵为相机平面的坐标通过Harris角点检测再由梯度搜索得到，而导航坐标系则由黑白棋盘格表示；

建立的闭合性矩阵方程为：

标定所需的图片由双目摄像机拍摄，标定所需导航坐标系由10×10的黑白棋盘模拟；标定至少需要三张图片；

标定所得结果内参数矩阵A，旋转矩阵R，平移向量使得如下非线性模型最小：

图像个数为n(n≥3)，单个图像标定点数k，取k＝10；

摄像机的透镜在制作过程中会发生一定程度的畸变，其径向畸变系数和横向畸变系数分别为k₁,k₂，则用

进行畸变校正；

其最小二乘解为：

k＝(D^TD)^-1D^Td

其畸变校正前后坐标关系：

将标定及优化过的摄像机参数用于视觉导航中作为惯性导航中的姿态矩阵。

所述步骤2)惯性导航解算的具体步骤包括：

(2.1)利用陀螺仪输出三轴角速度ω＝[ω_x ω_y ω_z]^T作为观测量，设表征姿态旋转的四元数为Q＝[q₀ q₁ q₂ q₃]，求解四元数的微分方程：

给定初始四元数Q₀，并实时更新四元数值，建立四元数矩阵：

根据四元数与姿态矩阵的关系可以得到

θ＝arcsin(C₃₂)

惯性导航解算出含漂移的姿态信息，并随含有随时间增长的累积误差；

(2.2)加速度计测量机体坐标系下的机器人加速度f^b，由陀螺仪实时计算更新的姿态角构成的姿态矩阵为即得到用于导航计算的加速度：

根据惯导系统的基本比力方程，求取其速度：

由速度积分求其位置：

(2.3)由惯性导航得出的信息量含有随时间增长的累积误差，考虑其误差量，在卡尔曼滤波中状态量含有：

所述步骤3)视觉导航解算的具体步骤包括：

(3.1)在视觉里程计中融入一部分惯导信息，捷联在机器人上的加速度计直接输出机体坐标系下的加速度值，通过：

得到其横滚角与纵摇角，用以修正视觉中的姿态信息；

(3.2)在视觉导航中，采用已标定好的双目摄像机采集图像，通过均值滤波器进行图像去噪，利用BRISK算法进行图像特征点的提取；其具体步骤如下：

(3.2.1)建立尺度空间：将一张图片按照降采样的方法，生成N张图片，称为N个尺度空间，在每一个尺度空间内，对图像进行分层，保证在每个尺度空间内的图片个数为3～5个，形成一个从大到小的金字塔的排列；尺度空间的大小关系依次表示为：

2^i-1(σ,kσ,k²σ,...k^n-1σ),k＝2^1/s

(3.2.2)特征点检测：对每一个尺度空间内的每层图像的像素点进行检测，若某个像素点在其同层相邻的8个像素点以及上下层18个像素点中是极值点，则称该点为特征点；对关键点的位置进行二维二次函数插值，得到更精确的亚像素坐标位置，再对尺度方向进行一维插值，得到极值点对应的尺度，称为特征尺度；

(3.2.3)特征点描述：以某个特征点为中心，在其周围采样M个点，以δ为方差进行高斯滤波；将采样点两两组合，称为采样点对，表示为：A＝{(p_i,p_j)∈R²×R²|i＜M∧j＜i∧i,j∈M}，计算其局部梯度：

特征点主方向为：

α＝arctan2(g_y,g_x)

将建立好的特征点描述符用于特征点的匹配过程，BRISK算法是二进制描述特征，对N个特征点，有N(N-1)/2个特征点对，即含有N(N-1)/2个匹配距离；设x_k,x_k-1表示摄像机特征点集第k帧和第k-1帧的姿态，则表征旋转的四元数为：

x_k＝q*x_k-1+ω(k)

在已匹配好的特征点中选取至少4组不共线的点，利用RANSAC算法进行迭代计算旋转矩阵R，四元数反对称矩阵的变换关系为：

而ω(k)表征这一过程的高斯白噪声；

根据视觉旋转四元数q得到的视觉姿态为：

此视觉姿态含有累积误差，用惯性姿态做一步修正；

(3.3)视觉导航中，通过已匹配好的特征点进行机器人三维运动的估计；设特征点集合为p_i，p_ik＝[u_k v_k 1]和p_i(k-1)＝[u_i(k-1) v_i(k-1) 1]是时刻k和k-1的特征点运动矢量；根据运动学原理，二者间的关系用一组旋转矩阵和平移矢量表示：

p_ik＝R_mp_i(k-1)+t_m

由于三维反对称阵与三维向量对应，用a^x表示三维向量对应的反对称阵；令：

d＝1+||a||²

则旋转矩阵为：

R_m＝I+2d^-1a^x(I+a^x)

基于此向量建立的目标函数为：

平移向量可以如下求得：

令则原函数为：

使目标函数J最小的必须满足正则方程：

通过求解线性最小二乘问题，求得向量再一步求取旋转矩阵R_m，从而完成运动的估计

所述步骤4)信息融合的具体步骤包括：

(4.1)建立机器人的运动模型

(4.1.1)机器人运动加速度的描述：利用机器人当前的统计模型描述加速度的统计分布，其下一时刻加速度为有限值且在当前加速度的邻域内，用E_a(t)表示有色加速度噪声，则加速度描述为：

(4.1.2)取视觉惯性组合系统的状态变量为

分别表示机器人惯导东向、北向的位置，速度，加速度以及机器人运动的航向；组合导航系统的连续时间状态方程为：

根据运动学关系，并考虑加入加速度高斯白噪声以及航向误差并带入其状态转移矩阵表示为：

(4.1.3)设视觉惯导系统采样周期为T，对连续系统离散化：

X_k＝φ_k,k-1X_k-1+U_k+ω_k

其中：

上式为离散型视觉/惯导组合导航系统的状态方程；

(4.2)建立视觉惯导组合导航系统观测模型

(4.2.1)取惯导输出的东向位置x_e，北向位置x_n，视觉航向角视觉里程计输出距离s作为观测量；

引入各个观测量的观测噪声，建立的连续系统观测方程为：

(4.2.2)连续系统离散化

观测矩阵

其中：

(4.3)根据扩展卡尔曼滤波器建立的离散状态方程以及观测方程，得到视觉惯性组合系统的递推滤波方程为：

a)状态一步预测：

b)观测值修正状态估计：

c)卡尔曼滤波增益的计算：

d)误差协方差一步预测：

e)误差协方差估计：P_k＝[I-K_kH_k]P_k,k-1

其中观测噪声R_k～(0,σ²)，过程噪声

所述步骤5)全局路标的修正以及构图具体步骤包括：

(5.1)视觉双目摄像头采集图像的特征点抽象为全局路标，机器人在移动过程中，由惯导IMU和视觉VO输出的运动信息含有累积误差，在有视觉特征路标的情况下，通过观测特征路标的距离和角度，扩展原状态量：

其中：

将扩展后的状态量反馈回扩展卡尔曼滤波器中；

(5.2)机器人SLAM完成即时定位，以及通过双目摄像机观察环境中的特征，并同步构成地图；在观察环境的特征时，机器人SLAM需要判断当前的特征与已有的地图中的特征点库是否具有关联性；若有关联性则不处理，若无关联性则将此特征点作为新的环境特征加入地图中，在同步构图过程中，对原特征点进行重定位；

判断是否具有关联性以两特征点的马氏距离为评价量，基于遗传算法的数据关联的具体步骤为：

(5.2.1)独立检测：去除环境特征点中与当前观测特征点完全无关的，当前观测点与地图特征点距离的误差向量为：

其距离误差协方差为：

两点间的马氏距离表示为：

设置关联性阈值α表示关联性的置信区间，取为90％；

(5.2.2)对经过独立检测后的地图内的特征点与当前观测特征点进行数据关联，选择地图中的特征点作为本征染色体序列F；

个体可以表示为：I_t＝{b_it}，t＝(1,2...p)，b表示特征值序号；观测值序列与本征染色体序列的关联解表示为：H_ir＝{F_i(b_i)}，第i个观测序列对应原染色体序列中的某个关联性特征的集合，其关联性在关联解集合H_ir中定义；

以观测路标点的距离误差及其协方差作为评价准则，并设关联解是一个高斯分布，则个体适应度函数为：

在其关联解集合中，个体I_t的适应度函数为：

对个体进行选择操作：在一个种群中，其个体评价值为{F_t}，t＝(1,2,...N)，则个体是否被选择的概率为：P_t＝F_t/∑F_t，设置阈值R1，若P>R1表明该个体可以直接被选择进入下一代；

交叉操作：

设立阈值R2，对满足R₂＜P_t＜R₁的个体进行两两交叉，设I_a,I_b分别是待交叉的个体，则：

用新的个体进入下一代；

变异操作：

对于P_t＜R₂的个体进行变异操作：

其中η～N(0,1)，α为分布修正参数；

(5.2.3)遗传算法终止条件：

条件1：针对不同大小，不同数量特征点的地图，设置其终止代数为δ，当迭代次数i＞δ时，遗传算法终止；

条件2：当寻找到最优解时，算法终止

经过遗传算法i代后，其最优解差分很小，将其终止；

将关联好的特征点加入地图中，在即时定位的同时完成同步构图。

有益效果：本发明使用修正视觉里程计解算出机器人在移动过程中的姿态、位置，同时为了避免环境的影响，融入了惯导信息进行组合导航，在进行机器人实时精确定位的同时引入遗传算法进行地图的同步构建。首先，统一视觉导航与惯性导航的坐标系，并对双目摄像机进行参数标定，从而根据图像的像素坐标来求解三维空间坐标；其次，惯性导航进行独立解算，根据坐标系的转化，求解在导航坐标系下的姿态、位置；再次，视觉导航解算，利用双目摄像机拍摄图像，提取图像的特征点并做k-d匹配，根据特征点的运动估计机器人三维空间的运动，从而求解其姿态、位置；然后，利用扩展卡尔曼滤波器对视觉导航信息与惯性导航信息融合，建立了系统的滤波模型；最后，考虑到定位的局部性，利用双目摄像机观测全局特征点路标，并基于遗传算法对地图特征进行数据关联，并将扩展后的状态向量反馈回滤波器中。相对于现有技术，本发明可以进行长时间、高精度的定位，并且由于加入了遗传算法改进地图的数据关联，使得在同步构建地图中精度提高很多。

附图说明

图1是本发明方法流程图；

图2是本发明中摄像机坐标系与导航坐标系关系图；

图3是本发明中惯性导航解算信息的流程图；

图4是本发明中加入全局路标修正后的视觉导航流程图；

图5是本发明中两幅不同角度的图片经BRISK算法检测并k-d匹配特征点示意图；

图6a是本发明中惯导定位误差图，图6b是本发明中组合定位误差图；

图7a是本发明中基本数据关联的同步构图，图7b是本发明中基于遗传算法数据关联的同步构图。

具体实施方式

下面将结合附图，对本发明的实施案例进行详细的描述；

如图1-5所示，本发明所述的一种基于遗传算法的视觉惯性组合的SLAM方法，该方法包含以下步骤：

步骤1：坐标系的统一，将惯导下的机体坐标系和导航坐标系与视觉下的摄像机坐标系和导航坐标系统一起来。两类坐标系间的关系主要就是旋转矩阵R和平移向量

步骤2：双目视觉摄像机的标定，获得其内参矩阵以及外部旋转参数R以及平移参数t。从而将图像的二维特征点坐标和机器人三维运动坐标对应起来。并且通过摄像机的标定可以让它的镜头畸变得到校正。

步骤3：惯性导航解算，利用陀螺仪测定机器人运动角速度，通过求解四元数微分方程，得到机器人的运动姿态，并建立姿态矩阵。利用加速度计测定机器人在机体系下的加速度，通过姿态矩阵求得导航系下的加速度，并通过惯导的比力方程求解其速度，再积分求其位置。

步骤4：利用步骤2标定好的双目摄像机进行图像的采集，将图像去噪处理后利用BRISK算法对图像进行特征提取，并做特征点匹配。

步骤5：设两幅图像间的姿态旋转四元数为q，选取步骤4中至少4组已匹配好的特征点进行RANSAC求解其一致性，得到其旋转矩阵，将其扩展后得到旋转四元数。通过旋转四元数获得视觉姿态角的信息。

步骤6：利用步骤4中匹配好的特征点做三维运动估计，通过三维向量与反对称阵的关系将非线性目标函数转化为线性最小二乘求解的问题。从而得到视觉里程计的位置输出。

步骤7：将步骤3、5、6得到的视觉/惯导信息进行融合，利用扩展卡尔曼滤波器，并选取加速度、速度、位置、航向作为状态量，选取惯导位置、航向、视觉里程计位置为观测量。建立了离散型状态模型和观测模型。

步骤8：将步骤4中的特征点建立特征点路标，并作为机器人的观测量。将观测新息加入步骤7所述的状态量中，并将扩展后的状态量反馈回步骤7所述的扩展卡尔曼滤波器，建立新的模型。

步骤9：根据步骤8中的特征点路标构建环境地图，并基于遗传算法将当前观测的特征点与环境地图已有的特征点进行数据关联。将新息加入环境地图中，完成地图的同步构建。

其中，步骤1：坐标系的统一与摄像机的标定，惯导下的坐标系包括机体坐标系与导航坐标系，视觉下的坐标系包括图像坐标系、摄像机坐标系和导航坐标系，IMU(Inertial measurement unit，惯性测量单元)与双目摄像机均捷联在机器人上，图像坐标系与摄像机坐标系的关系由摄像机内参表示，导航坐标系取作当地地理坐标系，摄像机坐标系与导航坐标系的关系表示为一个旋转矩阵R和一个平移向量通过对双目摄像机的标定确定旋转矩阵R与平移向量这些摄像机参数，进行像素点的三维重构；具体如下：

1)建立惯性导航的机体坐标系b，并取当地的地理坐标系作为导航坐标系n，其间的关系可以表示为一个姿态矩阵双目摄像机所得二维图像平面即为图像坐标系平面c，其经过自身内参矩阵的变换可以得到摄像机坐标系w。其关系可以表示为：取其左上角点为原点，取当地地理坐标系为视觉中的导航坐标系。其间的关系可以表示为一个旋转矩阵R和一个平移向量

2)摄像机的标定即是确定二维图像像素点与三维坐标的对应关系。设在图像坐标系下二维图像点坐标为m＝[u_c，v_c]^T，三维空间点的坐标为M＝[X,Y,Z]^T，根据小孔成像原理，它们的关系为sm＝A[R,t]M。s称为比例系数，A是摄像机的内参，其标准形式为

设摄像机坐标系两轴的像素点分别为dx，dy。令f表示摄像机焦距。则上式可以表示为：

设每张图片的单应性矩阵为相机平面的坐标通过Harris角点检测再由梯度搜索得到，而导航坐标系则由黑白棋盘格表示。

因此建立的闭合性矩阵方程为：

本次标定所需的图片由双目摄像机拍摄，标定所需导航坐标系由10×10的黑白棋盘模拟。

本次标定至少需要三张图片。

本次标定所得结果内参数矩阵A，旋转矩阵R，平移向量t使得如下非线性模型最小：

图像个数为n(n≥3)，单个图像标定点数k，取k＝10。

摄像机的透镜在制作过程中会发生一定程度的畸变，其径向畸变系数和横向畸变系数分别为k₁,k₂，则用

进行畸变校正。

其最小二乘解为：

k＝(D^TD)^-1D^Td

其畸变校正前后坐标关系：

将标定及优化过的摄像机参数用于视觉导航中，其作用相当于惯性导航中的姿态矩阵。

步骤2：惯性导航解算，利用捷联在机器人上的IMU进行测量，IMU上的陀螺仪负责测量运动姿态，并建立姿态矩阵，IMU上的加速度计测量机体下的加速度，并通过姿态矩阵变换至导航坐标系中，通过积分求解速度、位置；具体如下：

1)利用陀螺仪输出三轴角速度ω＝[ω_x ω_y ω_z]^T作为观测量，设表征姿态旋转的四元数为Q＝[q₀ q₁ q₂ q₃]，求解四元数的微分方程：

给定初始四元数Q₀，并实时更新四元数值，建立四元数矩阵：

根据四元数与姿态矩阵的关系可以得到

θ＝arcsin(C₃₂)

惯导可以独立解算出含漂移的姿态信息，并随含有随时间增长的累积误差。

2)加速度计测量机体坐标系下的机器人加速度f^b，由陀螺仪实时计算更新的姿态角构成的姿态矩阵为即可得到用于导航计算的加速度：

根据惯导系统的基本比力方程，求取其速度：

由速度积分求其位置：

3)由惯导得出的信息量含有随时间增长的累积误差，考虑其误差量，在卡尔曼滤波中状态量应含有：

步骤3：视觉导航解算，利用装载机器人头部的双目摄像机进行图像的获取，对图像进行降噪处理后利用BRISK算法(Binary Robust Invariant Scalable Keypoints)提取其特征点，根据步骤1标定好的摄像机参数并通过跟踪特征点坐标变化来估计机器人的三维运动，解算其姿态、速度；具体如下：

1)在视觉里程计中融入一部分惯导信息，由于捷联在机器人上的加速度计可以直接输出机体坐标系下的加速度值，因此，可以通过：

得到其横滚角与纵摇角，用以修正视觉中的姿态信息。这种姿态角没有通过积分的运算，不含有累积误差。

2)在视觉导航中，采用已标定好的双目摄像机采集图像，通过均值滤波器进行图像去噪，利用BRISK算法进行图像特征点的提取。这是一种二进制的特征描述算子，在对有较大模糊的图像配准时效果很好。其具体步骤如下：

a.建立尺度空间：将一张图片按照降采样的方法，生成N张图片，称为N个尺度空间，在每一个尺度空间内，对图像进行分层，保证在每个尺度空间内的图片个数为3～5个，形成一个从大到小的金字塔的排列。因此尺度空间的大小关系可以依次表示为：

2^i-1(σ,kσ,k²σ,...k^n-1σ),k＝2^1/s

b.特征点检测：对每一个尺度空间内的每层图像的像素点进行检测，若某个像素点在其同层相邻的8个像素点以及上下层18个像素点中是极值点，则称该点为特征点。对关键点的位置进行二维二次函数插值，得到更精确的亚像素坐标位置，再对尺度方向进行一维插值，得到极值点对应的尺度，称为特征尺度。

c.特征点描述：为避免降采样引起的混叠效应，以某个特征点为中心，在其周围采样M个点，以δ为方差进行高斯滤波。将采样点两两组合，称为采样点对，表示为：A＝{(p_i,p_j)∈R²×R²|i＜M^j＜i^i,j∈M}，计算其局部梯度：

特征点主方向为：

α＝arctan2(g_y,g_x)

将建立好的特征点描述符用于特征点的匹配过程，BRISK算法是二进制描述特征，对N个特征点，有N(N-1)/2个特征点对，即含有N(N-1)/2个匹配距离。设x_k,x_k-1表示摄像机特征点集第k帧和第k-1帧的姿态，则表征旋转的四元数为：

x_k＝q*x_k-1+ω(k)

在已匹配好的特征点中选取至少4组不共线的点，利用RANSAC算法进行迭代计算旋转矩阵R，四元数反对称矩阵的变换关系为：

而ω(k)表征这一过程的高斯白噪声。

根据视觉旋转四元数q得到的视觉姿态为：

此视觉姿态含有累积误差，可以用如步骤3所述第1)步的惯性姿态做一步修正。

3)视觉导航中，通过已匹配好的特征点进行机器人三维运动的估计。设特征点集合为p_i，p_ik＝[u_k v_k 1]和p_i(k-1)＝[u_i(k-1) v_i(k-1) 1]是时刻k和k-1的特征点运动矢量。根据运动学原理，二者间的关系可以用一组旋转矩阵和平移矢量表示：

p_ik＝R_mp_i(k-1)+t_m

由于三维反对称阵与三维向量对应，用a^x表示三维向量对应的反对称阵。令：

d＝1+||a||²

则旋转矩阵为：

R_m＝I+2d^-1a^x(I+a^x)

基于此向量建立的目标函数为：

平移向量可以如下求得：

令则原函数为：

使目标函数J最小的必须满足正则方程：

通过求解线性最小二乘问题，求得向量再一步求取旋转矩阵R_m，从而完成运动的估计

步骤4：信息融合，利用扩展卡尔曼滤波器对视觉信息与惯导信息进行融合，以惯导模型建立滤波器状态方程，并取部分惯导输出位姿信息及视觉导航输出信息作为观测变量建立滤波器观测方程，建立系统的滤波器模型；具体如下：

1)建立机器人的运动模型，其具体步骤包括：

a.机器人运动加速度的描述：利用机器人当前的统计模型描述加速度的统计分布，其下一时刻加速度为有限值且在当前加速度的邻域内，用E_a(t)表示有色加速度噪声，则加速度描述为：

b.取视觉/惯性组合系统的状态变量为

分别表示机器人东向、北向的位置，速度，加速度以及机器人运动的航向。

组合导航系统的连续时间状态方程为：

根据运动学关系，并考虑加入加速度高斯白噪声以及航向误差并带入其状态转移矩阵可表示为：

c.设视觉/惯导系统采样周期为T，对连续系统离散化：

X_k＝φ_k,k-1X_k-1+U_k+ω_k

其中：

上式即为离散型视觉/惯导组合导航系统的状态方程。

2)视觉/惯导组合导航系统观测模型建立如下：

a.取惯导输出的东向位置x_e，北向位置x_n，视觉航向角视觉里程计输出距离s作为观测量。

引入各个观测量的观测噪声，建立的连续系统观测方程为：

b.连续系统离散化

观测矩阵

其中：

3)根据EKF建立的离散状态方程以及观测方程，可以得到系统的递推滤波方程为：

a)状态一步预测：

b)观测值修正状态估计：

c)卡尔曼滤波增益的计算：

d)误差协方差一步预测：

e)误差协方差估计：P_k＝[I-K_kH_k]P_k,k-1

其中观测噪声R_k～(0,σ²)，过程噪声

步骤5：全局修正与同步构图，全局路标的修正以及构图：利用双目摄像机观测到的图像中提取的特征点作为特征路标，并基于遗传算法与地图已有的特征点库做数据关联，将扩展后的状态量反馈回滤波器中，同时，根据特征点的关联性判断该特征点是否应该加入地图中，并对地图中的特征点进行重定位，完成同步构图。具体如下：

1)视觉双目摄像头采集图像的特征点可以抽象为全局路标，机器人在移动过程中，由惯导IMU和视觉VO(Visual Odometry,视觉里程计)输出的运动信息含有累积误差，在有视觉特征路标的情况下，通过观测特征路标的距离和角度，可以扩展原状态量：

其中：

将扩展后的状态向量反馈回如步骤4所述的EKF滤波器中。

2)机器人SLAM除了完成即时定位的功能，还通过双目摄像机观察环境中的特征，同步的构成地图。在观察环境的特征时，需要判断当前的特征与已有的地图(特征点库)是否具有关联性。若有关联性则不处理，若无关联性则将此特征点作为新的环境特征加入地图中，并且由于视觉观测本身的误差，原地图中的特征点的位置也含有误差，在同步构图过程中，也需要对原特征点进行重定位。地图中的特征点不能太多，一个特征点需要维护至少3×3的协方差矩阵。因此数据关联在机器人SLAM同步构图中非常重要。

数据关联以两特征点的马氏距离为评价量，基于遗传算法的数据关联的具体步骤为：

a.独立检测：去除环境特征点中与当前观测特征点完全无关的，当前观测点与地图特征点距离的误差向量为：

其距离误差协方差为：

两点间的马氏距离表示为：

设置关联性阈值α表示关联性的置信区间，一般取为90％。

b.对经过独立检测后的地图内的特征点与当前观测特征点进行数据关联，选择地图中的特征点作为本征染色体序列F。

个体可以表示为：I_t＝{b_it}，t＝(1,2...p)，b表示特征值序号。因此观测值序列与本征染色体序列的关联解可表示为：H_ir＝{F_i(b_i)}，第i个观测序列对应原染色体序列中的某个关联性特征的集合，因此其关联性应该在关联解集合H_ir中定义。以观测路标点的距离误差及其协方差作为评价准则，并设关联解是一个高斯分布，则个体适应度函数为：

在其关联解集合中，个体I_t的适应度函数为：

对个体进行选择操作：在一个种群中，其个体评价值为{F_t}，t＝(1,2,...N)，则个体是否被选择的概率为：P_t＝F_t/∑F_t，设置阈值R1，若P>R1表明该个体可以直接被选择进入下一代。

交叉操作：

设立阈值R2，对满足R₂＜P_t＜R₁的个体进行两两交叉，设I_a,I_b分别是待交叉的个体，则：

用新的个体进入下一代。

变异操作：

对于P_t＜R₂的个体进行变异操作：

其中η～N(0,1)，α为分布修正参数。

c.遗传算法终止条件：

条件1：针对不同大小，不同数量特征点的地图，设置其终止代数为δ，当迭代次数i＞δ时，遗传算法终止。

条件2：当寻找到最优解时，算法终止

表明，经过遗传算法i代后，其最优解差分很小，将其终止。

将关联好的特征点加入如步骤5所述的地图中，在即时定位的同时完成同步构图。

图6a与图6b是机器人在运动过程中单独使用惯性导航，以及使用视觉/惯导组合导航的定位误差图。在给定相同的仿真初始条件的情况下，惯性导航定位误差较组合导航更大。

图7a和图7b是机器人采用基本的数据关联方法以及优化过的基于遗传算法的数据关联方法同步构图的结果。可以看出，在遗传算法加入后，地图特征点的估计位置与仿真给定的理论位置非常接近，而基本数据关联的特征点估计却发生了一定程度的偏离，显示出本发明改进算法的可靠性和优越性。