CN109976344A - 巡检机器人姿态矫正方法 - Google Patents

Abstract

本发明提出了一种巡检机器人姿态矫正方法，具体步骤为：根据图像坐标系与摄像机坐标系、相机坐标系与世界坐标系变换关系完成摄像机标定，获得摄像机内参；基于ORB的特征提取匹配算法对巡检机器人拍摄的图像进行特征匹配，并对匹配结果进行优化；建立摄像机与机器人的位姿关系，根据获得的匹配点对进行相对位姿解算，得到机器人的当前旋转角，所述机器人的当前旋转角包括里程计旋转角和视觉旋转角；融合里程计旋转角和视觉旋转角得到机器人当前待矫正角度，完成姿态矫正。本发明采用距离阈值和交叉匹配两种优化方法进行匹配点优化，优化后匹配效果更加可靠。

Description

巡检机器人姿态矫正方法

技术领域

本发明属于机器人定位导航技术，具体为一种巡检机器人姿态矫正方法。

背景技术

自主移动机器人定位系统中里程计定位是AMCL算法精确定位的基础，处于定位系统的底层。若驱动轮打滑将导致机器人里程计定位位姿有较大偏差，偏差过大会导致AMCL定位失败，通常定位失败后无法从失败中恢复。

目前机器人在原地旋转过程中，如果遇到地面不平坦的情况，有可能会出现驱动轮打滑的情况。而激光定位只能对机器人位置信息进行修正，对其姿态无法修正。当机器人打滑时会导致里程计推算的角度比实际转过的角度大，导致机器人的姿态估计发生错误，影响地图匹配，严重情况下会导致机器人坐标跑飞。

现有的姿态矫正方法只采用了里程计进行姿态推算，其所得到的误差较大，矫正结果较为粗糙，并且其适应性和鲁棒性较差。

发明内容

本发明的目的在于提出了一种巡检机器人姿态矫正方法，以解决机器人驱动轮打滑情况下里程计姿态推算误差较大的问题。

实现本发明的技术解决方案为：一种巡检机器人姿态矫正方法，具体步骤为：

步骤1、根据图像坐标系与摄像机坐标系、相机坐标系与世界坐标系变换关系完成摄像机标定，获得摄像机内参；

步骤2、基于ORB的特征提取匹配算法对巡检机器人拍摄的图像进行特征匹配，并对匹配结果进行优化；

步骤3、建立摄像机与机器人的位姿关系，根据获得的匹配点对进行相对位姿解算，得到机器人的当前旋转角，所述机器人的当前旋转角包括里程计旋转角和视觉旋转角；

步骤4、融合里程计旋转角和视觉旋转角得到机器人当前待矫正角度，完成姿态矫正。

优选地，步骤2基于ORB的特征提取匹配算法对巡检机器人拍摄的图像进行特征匹配，并对匹配结果进行优化的具体步骤为：

步骤2-1、利用FAST角点检测方法提取图像特征点；

步骤2-2、利用BEIEF算法提取图像特征描述子，得到初步匹配结果；

步骤2-3、选择满足以下条件的点对作为正确的匹配点对：

p∈correctMatch,当dis(p)＜max(2*min_dis，d₀)

其中dis(p)表示p点对汉明距离，min_dis表示匹配的点对的汉明距离最小值，d₀设置的经验值。

步骤2-4、基于OpenCV库函数利用交叉匹配进一步筛选匹配点，得到最终匹配结果。

优选地，步骤3建立摄像机与机器人的位姿关系，根据获得的匹配点对进行相对位姿解算，得到机器人的当前旋转角的具体步骤为：

步骤3-1、将摄像机水平安装在机器人上，对于空间中任意一点Q在视点O_c1和O_c2中的图像坐标点1(x_c1,y_c1,z_c1)，点2(x_c2,y_c2,z_c2)，两个坐标点具体为：

其中，u_d1＝u₁-u₀,v_d1＝v₁-v₀,u_d2＝u₂-u₀,v_d2＝v₂-v₀，f_x，f_y表示横轴焦距和纵轴焦距；

根据两个视点的变换矩阵得到上述两点初步转换后的坐标表达式：

步骤3-2、根据初步转换后的坐标表达式，简化消去z_w′得到：

步骤3-3、根据机器人模型不能单独沿某个轴运动的特性，则满足p_x＝kp_z，k≠0，将步骤3-2中的式子可化简为：

其中，k为比例系数，且

步骤3-4、根据所得到的两个视点中对应所标定点的匹配点，求解h₁～h₃；

步骤3-5、根据参数h₁～h₃求出机器人转过的角度Δθ，具体为：

Δθ＝arctan(-h₂-h₁h₃,h₁-h₂h₃)。

进一步地，步骤3-4中根据所得到的两个视点中对应所标定点的匹配点，求解h₁～h₃的具体方法为：

当匹配点对数n为3，根据步骤3-3直接求解h₁～h₃；

当匹配点对数n大于3时，首先构造超定方程组：

其中，

利用最小二乘法构造误差平方和函数，解出超定方程组，即使得误差平方和函数取得最小值的h₁～h₃即为超定方程组的解，其中，误差平方和函数为：

本发明与现有技术相比，其显著优点为：(1)本发明采用距离阈值和交叉匹配两种优化方法进行匹配点优化，优化后匹配效果更加可靠；(2)本发明根据最小二乘法原则进行位姿解算，提高了解算效率以及解算所得的姿态角的精确度；(3)本发明融合里程计和单目视觉能够获得更为准确的机器人姿态，适应性高，鲁棒性好。

下面结合附图对本发明做进一步详细的描述。

附图说明

图1为本发明摄像机坐标系与世界坐标系关系示意图。

图2为本发明O_c1O_c2视点坐标系示意图。

图3为本发明摄像机安装图。

图4为本发明的整体流程图。

具体实施方式

一种巡检机器人姿态矫正方法，具体步骤为：

步骤1、根据图像坐标系与摄像机坐标系、相机坐标系与世界坐标系变换关系完成摄像机标定，获得摄像机内参；具体步骤为：

步骤1-1、根据图像坐标系与摄像机坐标系变换关系，得到相机的内参矩阵，图像坐标系与摄像机坐标系的关系表示为：

其中，(u,v)表示图像中任一点Q的像素坐标，(x_c,y_c,z_c)表示该点对应的摄像机坐标系坐标；f_x，f_y表示横轴焦距和纵轴焦距；u₀，v₀表示光轴中心线与图像平面的交点，K为相机的内参矩阵；

步骤1-2、如图1所示，根据摄像机坐标系和世界坐标系变换关系，得到相机的外参矩阵，摄像机坐标系和世界坐标系变换关系为：

其中，(x_w,y_w,z_w)为任一点Q的世界坐标系中坐标，R为旋转矩阵，P为三维平移向量，M表示外参矩阵；

步骤1-3、加入机器人方向角θ，计算世界坐标系相对于摄像机坐标系的旋转矩阵^cR_w、机器人坐标系相对于摄像机坐标系的平移矩阵^cP_r以及世界坐标系相对于摄像机坐标系的位置^cp_w，世界坐标系相对于摄像机坐标系的旋转矩阵^cR_w具体为：

其中，θ为机器人方向角，^cR_r为基础变换矩阵；

机器人坐标系相对于摄像机坐标系的平移矩阵^cP_r具体为：

其中l为机器人中心到摄像机中心的水平距离，h为摄像机中心距离地面的垂直距离；

世界坐标系相对于摄像机坐标系的位置^cp_w为：

式中，(x,y)为机器人的位置；

步骤1-4、利用张氏标定法进行摄像机标定，确定内参矩阵，即代表图像坐标系与摄像机坐标系的具体关系。

步骤2、基于ORB的特征提取匹配算法对巡检机器人拍摄的图像进行特征匹配，并对匹配结果进行优化；具体步骤为：

步骤2-1、利用FAST角点检测方法提取图像特征点；

步骤2-2、利用BEIEF算法提取图像特征描述子，得到初步匹配结果；

步骤2-3、选择满足以下条件的点对作为正确的匹配点对：

p∈correctMatch,当dis(p)＜max(2*min_dis，d₀) (6)

其中dis(p)表示p点对汉明距离，min_dis表示匹配的点对的汉明距离最小值，d₀设置的经验值；

步骤2-4、基于OpenCV库函数利用交叉匹配进一步筛选匹配点，得到最终匹配结果，交叉匹配是指用匹配的结果点对再次进行匹配，如果匹配结果不改变，则认为这是一个正确的匹配。例如第一次特征点A使用暴力匹配的方法，匹配到的特征点是特征点B；反过来，使用特征点B进行匹配，如果匹配到的仍然是特征点A，则就认为这是一个正确的匹配，否则就是一个错误的匹配。

步骤3、建立摄像机与机器人的位姿关系，根据获得的匹配点对进行相对位姿解算，得到机器人的当前旋转角，所述机器人的当前旋转角包括里程计旋转角和视觉旋转角；

如图2所示，在摄像机任意两个视点O_c1，O_c2，坐标系分别为C₁，C₂，同时在O_c1处建立参考坐标系w′，参考坐标系w′各轴与C₂各轴平行，则坐标系C₁到C₂可以看作是先纯旋转再纯平移的过程。如图2所示，参考坐标系w′相对于摄像机坐标系只有绕Y_c1轴的旋转，及沿X_c2和Z_c2的平移。根据摄像机外参模型得到两个视点的变换矩阵如下式(7)(8)：

其中Δθ为摄像机从视点O_c1到视点O_c2转过的角度，p_x为沿x轴移动的距离，p_z为沿z轴移动的距离。

得到机器人的当前旋转角的具体步骤为：

步骤3-1、如图4所示，将摄像机水平安装在机器人上，对于空间中任意一点Q在视点O_c1和O_c2中的图像坐标点1(x_c1,y_c1,z_c1)，点2(x_c2,y_c2,z_c2)，两个坐标点具体为：

其中，u_d1＝u₁-u₀,v_d1＝v₁-v₀,u_d2＝u₂-u₀,v_d2＝v₂-v₀，其中(u₁，v₁)与(u₂，v₂)分别代表任一点在视点1、2中的像素点坐标，f_x，f_y表示横轴焦距和纵轴焦距；

根据上述式(7)(8)可知并将其分别代入(9)(10)得到上述两点初步转换后的坐标表达式，如式(11)和(12)所示：

步骤3-2、根据初步转换后的坐标表达式，简化消去z_w′得到：

步骤3-3、根据机器人模型不能单独沿某个轴运动的特性，则满足p_x＝kp_z，k≠0，将步骤3-2中的式子可化简为：

其中，k为比例系数，且

步骤3-4、根据所得到的两个视点中对应所标定点的匹配点，求解h₁～h₃；

具体方法为：

当匹配点对数n为3，根据式(15)直接求解h₁～h₃；

当匹配点对数n大于3时，首先构造超定方程组：

其中，

利用最小二乘法构造误差平方和函数，解出超定方程组，即使得误差平方和函数取得最小值的h₁～h₃即为超定方程组的解，其中，误差平方和函数为：

获得使得误差平方和函数取得最小值的h₁～h₃的具体方法为：

将超定方程组以矩阵形式表示为Ax＝b，其中：

根据定理是Ax＝b的最小二乘解充要条件为：是A^TAx＝A^Tb的解；求解A^TAx＝A^Tb的参数h₁～h₃。

步骤3-5、根据参数h₁～h₃求出机器人转过的角度Δθ，具体为：

Δθ＝arctan(-h₂-h₁h₃,h₁-h₂h₃) (18)

步骤4、融合里程计旋转角和视觉旋转角得到机器人当前待矫正角度，完成姿态矫正，具体步骤为：

步骤4-1、遥控机器人多次旋转同一角度，记录里程计旋转角Δθ_o和视觉旋转角Δθ_v，计算二者相应方差δ_o ²，δ_v ²，确定融合权值α：

步骤4-2、根据融合权值得到k时刻的融合角度：

θ_k＝θ_k-1+(1-α)Δθ_o+αΔθ_v (20)

本发明在摄像机基本模型的基础上，基于ORB特征进行了特征提取与匹配优化，并根据机器人与摄像机的位置关系，推导出相对位姿的计算方法。利用加权融合算法，融合里程计姿态与视觉姿态，减小了姿态估计的误差。

Claims (7)

1.一种巡检机器人姿态矫正方法，其特征在于，具体步骤为：

步骤1、根据图像坐标系与摄像机坐标系、相机坐标系与世界坐标系变换关系完成摄像机标定，获得摄像机内参；

步骤2、基于ORB的特征提取匹配算法对巡检机器人拍摄的图像进行特征匹配，并对匹配结果进行优化；

步骤3、建立摄像机与机器人的位姿关系，根据获得的匹配点对进行相对位姿解算，得到机器人的当前旋转角，所述机器人的当前旋转角包括里程计旋转角和视觉旋转角；

步骤4、融合里程计旋转角和视觉旋转角得到机器人当前待矫正角度，完成姿态矫正。

2.根据权利要求1所述的巡检机器人姿态矫正方法，其特征在于，步骤1获得摄像机内参的具体步骤为：

步骤1-1、根据图像坐标系与摄像机坐标系变换关系，得到相机的内参矩阵，图像坐标系与摄像机坐标系的关系表示为：

其中，(u,v)表示图像中任一点Q的像素坐标，(x_c,y_c,z_c)表示该点对应的摄像机坐标系坐标；f_x，f_y表示横轴焦距和纵轴焦距；u₀，v₀表示光轴中心线与图像平面的交点，K为相机的内参矩阵；

步骤1-2、根据摄像机坐标系和世界坐标系变换关系，得到相机的外参矩阵，摄像机坐标系和世界坐标系变换关系为：

其中，(x_w,y_w,z_w)为任一点Q的世界坐标系中坐标，R为旋转矩阵，P为三维平移向量，M表示外参矩阵；

步骤1-3、加入机器人方向角θ，计算世界坐标系相对于摄像机坐标系的旋转矩阵^cR_w、机器人坐标系相对于摄像机坐标系的平移矩阵^cP_r以及世界坐标系相对于摄像机坐标系的位置^cp_w，世界坐标系相对于摄像机坐标系的旋转矩阵^cR_w具体为：

其中，θ为机器人方向角，^cR_r为基础变换矩阵；

机器人坐标系相对于摄像机坐标系的平移矩阵^cP_r具体为：

其中，l为机器人中心到摄像机中心的水平距离，h为摄像机中心距离地面的垂直距离；

世界坐标系相对于摄像机坐标系的位置^cp_w为：

式中，(x,y)为机器人的位置；

步骤1-4、利用张氏标定法进行摄像机标定，确定内参矩阵。

3.根据权利要求1所述的巡检机器人姿态矫正方法，其特征在于，步骤2基于ORB的特征提取匹配算法对巡检机器人拍摄的图像进行特征匹配，并对匹配结果进行优化的具体步骤为：

步骤2-1、利用FAST角点检测方法提取图像特征点；

步骤2-2、利用BEIEF算法提取图像特征描述子，得到初步匹配结果；

步骤2-3、选择满足以下条件的点对作为正确的匹配点对：

p∈correctMatch,当dis(p)＜max(2*min_dis，d₀)

其中dis(p)表示p点对汉明距离，min_dis表示匹配的点对的汉明距离最小值，d₀设置的经验值；

步骤2-4、基于OpenCV库函数利用交叉匹配进一步筛选匹配点，得到最终匹配结果。

4.根据权利要求1所述的巡检机器人姿态矫正方法，其特征在于，步骤3建立摄像机与机器人的位姿关系，根据获得的匹配点对进行相对位姿解算，得到机器人的当前旋转角的具体步骤为：

步骤3-1、将摄像机水平安装在机器人上，对于空间中任意一点Q在视点O_c1和O_c2中的图像坐标点1(x_c1,y_c1,z_c1)，点2(x_c2,y_c2,z_c2)，两个坐标点具体为：

其中，u_d1＝u₁-u₀,v_d1＝v₁-v₀,u_d2＝u₂-u₀,v_d2＝v₂-v₀，f_x，f_y表示横轴焦距和纵轴焦距；

根据两个视点的变换矩阵得到上述两点初步转换后的坐标表达式：

步骤3-2、根据初步转换后的坐标表达式，简化消去z_w′得到：

步骤3-3、根据机器人模型不能单独沿某个轴运动的特性，则满足p_x＝kp_z，k≠0，将步骤3-2中的式子可化简为：

其中，k为比例系数，且

步骤3-4、根据所得到的两个视点中对应所标定点的匹配点，求解h₁～h₃；

步骤3-5、根据参数h₁～h₃求出机器人转过的角度Δθ，具体为：

Δθ＝arctan(-h₂-h₁h₃,h₁-h₂h₃)。

5.根据权利要求4所述的巡检机器人姿态矫正方法，其特征在于，步骤3-4中根据所得到的两个视点中对应所标定点的匹配点，求解h₁～h₃的具体方法为：

当匹配点对数n为3，根据步骤3-3直接求解h₁～h₃；

当匹配点对数n大于3时，首先构造超定方程组：

其中，

利用最小二乘法构造误差平方和函数，解出超定方程组，即使得误差平方和函数取得最小值的h₁～h₃即为超定方程组的解，其中，误差平方和函数为：

6.根据权利要求5所述的巡检机器人姿态矫正方法，其特征在于，获得使得误差平方和函数取得最小值的h₁～h₃的具体方法为：

将超定方程组以矩阵形式表示为Ax＝b，其中：

根据定理是Ax＝b的最小二乘解充要条件为：是A^TAx＝A^Tb的解；求解A^TAx＝A^Tb的参数h₁～h₃。

7.根据权利要求1所述的巡检机器人姿态矫正方法，其特征在于，步骤4融合里程计旋转角和视觉旋转角得到机器人当前待矫正角度的具体步骤为：

步骤4-1、遥控机器人多次旋转同一角度，记录里程计旋转角Δθ_o和视觉旋转角Δθ_v，计算二者相应方差δ_o ²，δ_v ²，确定融合权值α：

步骤4-2、根据融合权值得到k时刻的融合角度：

θ_k＝θ_k-1+(1-α)Δθ_o+αΔθ_v。