[发明专利]变电站巡检机器人定位方法及定位装置

摘要

本发明涉及变电站智能巡检技术领域，是一种变电站巡检机器人定位方法及定位装置，前者包括第一步建立坐标系，第二步计算里程计的定位结果，第三步计算移动机器人的位姿，第四步修正定位结果；后者包括驱动机器人运动的轮式平台、机器人运动控制器、编码器、通信模块、摄像机和云台，轮式平台沿圆周等间距安装有三个全向轮，三个全向轮上各安装有一个编码器，云台底部设有支撑轴，支撑轴固定安装在轮式平台上，摄像机安装在云台的顶部。本发明采用编码器与二维码方式相结合定位，即使在二维码有障碍物遮挡的情况下仍然可以知道移动机器人的位置。变电站巡检机器人使用二维码的定位方法，在室内与室外环境下都可以使用。

主权项

一种变电站巡检机器人定位方法，其特征在于包括以下步骤：第一步，建立坐标系，建立世界坐标系OXYZ，二维码坐标系o1x1y1z1，摄像机坐标系o2x2y2z2，云台a部分坐标系o3x3y3z3，云台b部分坐标系o3x4y4z4和机器人坐标系o5x5y5z5；第二步，计算里程计的定位结果M1，对移动机器人进行运动学建模，使用三轮全向轮移动机器人作为移动平台，XOY为世界坐标系，xoy为机器人坐标系，移动机器人的三个轮子分别标记为i,j,k，三个轮子各自的线速度分别为Vi,Vj,Vk，在机器人坐标系中，移动机器人在X轴和Y轴两个坐标上的速度分别为vx，vy，方向与坐标轴方向相同，机器人自身旋转的角速度为ω，R为轮子到机器人中心点O的距离；在世界坐标系中机器人沿两坐标轴的速度分别为VX,VY，通过分析可得在机器人坐标系中根据机器人三个全向轮的线速度可以计算出机器人的速度为：vxvyω=23-13-130-333313R13R13R*ViVjVk]]>机器人坐标系与世界坐标系的夹角为φ，由机器人坐标系中的速度可以计算得到在世界坐标系下的速度为：VxVyω=cosφ-sinφ0sinφcosφ0001*vxvyω]]>移动机器人的初始姿态为(x0，y0，φ0)，Δt时间后移动机器人的姿态为(x1，y1，φ1)，计算公式如下：x1y1φ1=Δt000Δt000Δt*VxVyω+x0y0φ0]]>因为移动机器人在地面运动，移动机器人的位姿用M1表示为：M1＝(x1,y1,φ1)T第三步，通过二维码图像计算移动机器人的定位结果M2，包括以下步骤：(1)通过图像处理，读取二维码的信息，包括以下步骤：(a)相机拍照，(b)灰度化，(c)二值化，(d)形态学滤波，(e)边缘检测，(f)霍夫直线变换，(g)透视变换，(h)二维码信息读取；(2)通过二维码计算摄像机坐标系o2x2y2z2相对于二维码坐标系o1x1y1z1的旋转矩阵以及平移矩阵P12，计算方法如下：二维码贴在x1o1y1平面中，z1轴垂直于墙面向外，移动机器人相机镜头的朝向为z2轴，镜体方向为x2轴，图像信息位于x2o2y2坐标中；设边AO长度为lengthAO，边A1O1的长度为标定时假设当物体距离相机距离为1m时，实际中物体的长度和图像中物体长度的比为m，相机与二维码之间的距离计算公式如下：l=lengthAOlengthA1O1*m]]>已知二维码张贴高度h，移动机器人高清摄像机距离二维码的长度为l，则：γ=arcsinhl]]>在世界坐标系中∠AOB＝90°在相机坐标系中∠A1O1B1＝θ，o2X2y2z2相对于o1x1y1z1绕z轴旋转‑90°,然后在z1轴方向不再转动，在移动机器人运动和相机云台运动过程中，相对于z1轴不再旋转，只相对于x1轴和y1轴转动，绕x1转动用θx表示，绕y1轴用θy表示。o2x2y2z2中的坐标在o1x1y1z1坐标中如下表示：]]>x1y1z1=M-1*x2y2z2+-Δx-Δy-Δz;]]>其中L*sinβ=(l*cosγ*sinα)2+h2]]>α=arcsin(L*sinβ)2-h2l*cosγ]]> Δz＝h Δx＝l*sinα Δy＝l*cosα 3个未知数θx，θy和β,需要3个方程，则OA,OB在x1o1y1平面中可表示为：OA→=(a,b,0),OB→=(c,d,0)]]> 在o2x2y2z2坐标系中:OA→(x2y2z2)=(a,b,0)T*M+ΔxΔyΔz]]>OB→(x2y2z2)=(c,d,0)T*M+ΔxΔyΔz]]>OA→(x2y2z2)*OB→(x2y2z2)=0---(1)]]>在z2方向为0cosθ=O1A1→*O1B1→|O1A1|*|O1B1|---(2)]]>z2轴与x1轴之间的夹角为(cosθz1x)2+cosβ2+cosγ2=1]]>其中r已知，可用β表示；根据z2轴在o1x1y1z1坐标系中的向量得到关于β的方程；(0,0,1)T*M+ΔxΔyΔz=(cosβ,cos(90-γ),cosθz1x)2---(3)]]>联立方程(1)(2)(3)可求得θx，θy，和β，得到o2x2y2到o1x1y1z1的转换矩阵以及平移矩阵P12；(3)各个坐标系之间旋转矩阵与平移矩阵的计算：在世界坐标系中，设定二维码的位置为(x1,y1,z1)，二维码坐标系o1x1y1z1相对于世界坐标系OXYZ的平移矩阵为PO1的计算如下：P01＝(X1,Y1,Z1)T二维码坐标系的三个坐标轴与世界坐标系的三个轴之间的夹角组成的3×3的矩阵，各个夹角所对应的余弦值所组成的矩阵为二维码坐标系o1x1y1z1相对于世界坐标系OXYZ的旋转矩阵为如下：R1O=x1→*XO→y1→*XO→z1→*XO→x1→*YO→y1→*YO→z1→*YO→x1→*ZO→y1→*ZO→z1→*ZO→]]>移动机器人各个坐标系相对于相邻的坐标系的旋转角度以及平移距离在移动机器人系统均已知，其他坐标系的旋转矩阵以及平移矩阵也按照上述方法来计算；二维码坐标系o1x1y1z1相对于世界坐标系OXYZ有平移，相对于Z轴旋转，旋转矩阵为平移矩阵为PO1；摄像机坐标系o2x2y2z2相对于二维码坐标系o1x1y1z1为平移和旋转，旋转矩阵为平移矩阵为P12；云台a部分坐标系o3x3y3z3相对于摄像机坐标系o2x2y2z2只是相对于x2轴的平移，旋转矩阵为平移矩阵为P23；云台a部分坐标系o3x3y3z3相对于云台b部分坐标系o3x4y4z4只是相对于z3轴的旋转，所以他们共用一个坐标系原点o3，旋转矩阵为平移矩阵为P34；移动机器人坐标系o5x5y5z5相对于云台b部分坐标系o3x4y4z4只是相对于轴方向的平移和旋转，并且在云台b部分的坐标系o3x4y4z4下，移动机器人的坐标为4O5；(4)通过旋转矩阵与平移矩阵计算移动机器人的位姿：通过计算表示出云台b部分坐标系相对于世界坐标系的旋转矩阵为平移矩阵为PO1+P12+P23+P34，移动机器人坐标系原点O5就是移动机器人的位置；根据坐标系变换规则，可以计算出移动机器人位置：OO5=R1OR21R32R43O45+PO1+P12+P23+P34=(x,y,z)T]]>移动机器人坐标系相对于世界坐标系沿着Z轴方向旋转，就是移动机器人坐标系相对于世界坐标系的旋转矩阵，通过旋转矩阵，可以计算出移动机器人坐标系相对于世界坐标系的夹角θ，移动机器人的位姿用M2矩阵表示为：M2＝(x,y,θ)T第四步，二维码定位数据对里程计定位结果修正，使用二维码的定位结果来修正里程计的数据，则：M1＝M2＝(x,y,θ)T。