[发明专利]用于SLAM过程的基于差分进化算法进行BA问题求解的方法

权利要求书

1.一种用于SLAM过程的基于差分进化算法进行BA问题求解的方法，其特征在于，包括以下步骤：

步骤1、读取机器人的视觉图像，提取当前帧与上一帧图像的特征点，并获取所有匹配的特征点对(a_i,b_i)；a_i为上一帧图像中的特征点，b_i为当前帧图像中的特征点；其中，0＜i≤m，m为特征点对的数目；

步骤2、计算上一帧图像中所有特征点a_i各自对应的空间坐标P_i；

步骤3、根据步骤2求得的各个特征点a_i的空间坐标P_i以及当前帧的对应特征点b_i计算误差函数；

步骤4、利用差分进化算法求取误差函数的最优解所对应的相机的位姿ξ即完成BA问题的求解；具体为：

步骤4-1、种群初始化，在解空间中随机均匀产生M个个体，每个个体均为6维向量；

步骤4-2、进行变异操作，具体为：在第g次迭代进行第i个个体的进化过程中，在种群中随机选择3个个体：x_p1(g)，x_p2(g)，x_p3(g)，且p1≠p2≠p3≠i，求取变异向量v_i(g)；

步骤4-3、进行交叉操作，具体为：在第g次迭代进行第i个个体的进化过程中，将v_i(g)与当前种群中第i个个体v_i(g)的每一维的数据以交叉概率Cr进行交叉操作，生成新的个体u_i(g)；

步骤4-4、进行选择操作，具体为：在第g次迭代进行第i个个体的进化过程中，将新的个体u_i(g)与当前种群中第i个个体x_i(g)分别代入误差函数中获得各自对应的适应度，选取适应度低的个体作为下一代个体x_i(g+1)；

步骤4-5、重复步骤4-2～步骤4-4，直至第g次迭代中所有个体完成更新，并执行下一步；

步骤4-6、进行终止条件判断，具体为：设第g代每个个体的适应度为f(x_i(g))，判断其中最大适应度与最小适应度的差值e是否小于设定阈值，若小于，则结束整个过程，并输出最小适应度对应的值ξ_min即完成BA问题的求解，否则进入第g+1代循环，执行步骤4-2～步骤4-5。

2.根据权利要求1所述的用于SLAM过程的基于差分进化算法进行BA问题求解的方法，其特征在于，步骤2所述计算上一帧图像中所有特征点a_i各自对应的空间坐标P_i，所用公式为：

式中，K为相机的内参数矩阵，f_x为图像坐标的u轴缩放因子与焦距的乘积，单位为像素，f_y为图像坐标的v轴缩放因子与焦距的乘积，单位为像素；c_x、c_y为图像原点的平移量，[u,v]^T为特征点的像素坐标，Z为该特征点的深度，P为计算得到的特征点的空间坐标。