变电站巡检机器人定位方法及定位装置

摘要

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

主权项

一种变电站巡检机器人定位方法，其特征在于包括以下步骤：第一步，建立坐标系，建立世界坐标系OXYZ，二维码坐标系o₁x₁y₁z₁，摄像机坐标系o₂x₂y₂z₂，云台a部分坐标系o₃x₃y₃z₃，云台b部分坐标系o₃x₄y₄z₄和机器人坐标系o₅x₅y₅z₅；第二步，计算里程计的定位结果M₁，对移动机器人进行运动学建模，使用三轮全向轮移动机器人作为移动平台，XOY为世界坐标系，xoy为机器人坐标系，移动机器人的三个轮子分别标记为i,j,k，三个轮子各自的线速度分别为V_i,V_j,V_k，在机器人坐标系中，移动机器人在X轴和Y轴两个坐标上的速度分别为v_x，v_y，方向与坐标轴方向相同，机器人自身旋转的角速度为ω，R为轮子到机器人中心点O的距离；在世界坐标系中机器人沿两坐标轴的速度分别为V_X,V_Y，通过分析可得在机器人坐标系中根据机器人三个全向轮的线速度可以计算出机器人的速度为：$\left|\begin{array}{c}{v}_x\\ v_y\\ \omega \end{array}\right|=\left|\begin{array}{ccc}\frac{2}{3}& -\frac{1}{3}& -\frac{1}{3}\\ 0& -\frac{\sqrt{3}}{3}& \frac{\sqrt{3}}{3}\\ \frac{1}{3R}& \frac{1}{3R}& \frac{1}{3R}\end{array}\right|*\left|\begin{array}{c}{V}_i\\ V_j\\ V_k\end{array}\right|$机器人坐标系与世界坐标系的夹角为φ，由机器人坐标系中的速度可以计算得到在世界坐标系下的速度为：$\left|\begin{array}{c}{V}_x\\ V_y\\ \omega \end{array}\right|=\left|\begin{array}{ccc}cos\phi & -sin\phi & 0\\ sin\phi & \cos \phi & 0\\ 0& 0& 1\end{array}\right|*\left|\begin{array}{c}{v}_x\\ v_y\\ \omega \end{array}\right|$移动机器人的初始姿态为(x₀，y₀，φ₀)，Δt时间后移动机器人的姿态为(x₁，y₁，φ₁)，计算公式如下：$\left|\begin{array}{c}{x}_1\\ y_1\\ {\phi }_1\end{array}\right|=\left|\begin{array}{ccc}\Delta t& 0& 0\\ 0& \Delta t& 0\\ 0& 0& \Delta t\end{array}\right|*\left|\begin{array}{c}{V}_x\\ V_y\\ \omega \end{array}\right|+\left|\begin{array}{c}{x}_0\\ y_0\\ {\phi }_0\end{array}\right|$因为移动机器人在地面运动，移动机器人的位姿用M₁表示为：M₁＝(x₁,y₁,φ₁)^T第三步，通过二维码图像计算移动机器人的定位结果M₂，包括以下步骤：(1)通过图像处理，读取二维码的信息，包括以下步骤：(a)相机拍照，(b)灰度化，(c)二值化，(d)形态学滤波，(e)边缘检测，(f)霍夫直线变换，(g)透视变换，(h)二维码信息读取；(2)通过二维码计算摄像机坐标系o₂x₂y₂z₂相对于二维码坐标系o₁x₁y₁z₁的旋转矩阵以及平移矩阵P₁₂，计算方法如下：二维码贴在x₁o₁y₁平面中，z₁轴垂直于墙面向外，移动机器人相机镜头的朝向为z₂轴，镜体方向为x₂轴，图像信息位于x₂o₂y₂坐标中；设边AO长度为length_AO，边A₁O₁的长度为标定时假设当物体距离相机距离为1m时，实际中物体的长度和图像中物体长度的比为m，相机与二维码之间的距离计算公式如下：$l=\frac{{\mathrm{length}}_{AO}}{{\mathrm{length}}_{A_1{O}_1}}*m$已知二维码张贴高度h，移动机器人高清摄像机距离二维码的长度为l，则：$\gamma =arcsin\frac{h}{l}$在世界坐标系中∠AOB＝90°在相机坐标系中∠A₁O₁B₁＝θ，o₂X₂y₂z₂相对于o₁x₁y₁z₁绕z轴旋转‑90°,然后在z₁轴方向不再转动，在移动机器人运动和相机云台运动过程中，相对于z₁轴不再旋转，只相对于x₁轴和y₁轴转动，绕x₁转动用θ_x表示，绕y₁轴用θ_y表示。o₂x₂y₂z₂中的坐标在o₁x₁y₁z₁坐标中如下表示：$\begin{array}{c}\\ \end{array}$$\left|\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right|=M^{-1}*\left|\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right|+\left|\begin{array}{c}-\Delta x\\ -\Delta y\\ -\Delta z\end{array}\right|;$其中$L*sin\beta =\sqrt{{(l*cos\gamma *sin\alpha )}^2+h^2}$$\alpha =arcsin\frac{\sqrt{{(L*sin\beta )}^2-h^2}}{l*cos\gamma }$ Δz＝h Δx＝l*sinα Δy＝l*cosα 3个未知数θ_x，θ_y和β,需要3个方程，则OA,OB在x₁o₁y₁平面中可表示为：$\overrightarrow{OA}=(a,b,0),\overrightarrow{OB}=(c,d,0)$ 在o₂x₂y₂z₂坐标系中:$\overrightarrow{OA}\left(x_2{y}_2{z}_2\right)={(a,b,0)}^T*M+\left|\begin{array}{c}\Delta x\\ \Delta y\\ \Delta z\end{array}\right|$$\overrightarrow{OB}\left(x_2{y}_2{z}_2\right)={(c,d,0)}^T*M+\left|\begin{array}{c}\Delta x\\ \Delta y\\ \Delta z\end{array}\right|$$\overrightarrow{OA}\left(x_2{y}_2{z}_2\right)*\overrightarrow{OB}\left(x_2{y}_2{z}_2\right)=0---\left(1\right)$在z₂方向为0$cos\theta =\frac{\overrightarrow{O_1{A}_1}*\overrightarrow{O_1{B}_1}}{\left|O_1{A}_1\right|*\left|O_1{B}_1\right|}---\left(2\right)$z₂轴与x₁轴之间的夹角为${\left({\mathrm{cos\theta }}_{z_{1}x}\right)}^2+{\mathrm{cos\beta }}^2+{\mathrm{cos\gamma }}^2=1$其中r已知，可用β表示；根据z₂轴在o₁x₁y₁z₁坐标系中的向量得到关于β的方程；${(0,0,1)}^T*M+\left|\begin{array}{c}\Delta x\\ \Delta y\\ \Delta z\end{array}\right|={(cos\beta ,cos(90-\gamma ),{\mathrm{cos\theta }}_{z_{1}x})}^2---\left(3\right)$联立方程(1)(2)(3)可求得θ_x，θ_y，和β，得到o₂x₂y₂到o₁x₁y₁z₁的转换矩阵以及平移矩阵P₁₂；(3)各个坐标系之间旋转矩阵与平移矩阵的计算：在世界坐标系中，设定二维码的位置为(x₁,y₁,z₁)，二维码坐标系o₁x₁y₁z₁相对于世界坐标系OXYZ的平移矩阵为P_O1的计算如下：P₀₁＝(X₁,Y₁,Z₁)^T二维码坐标系的三个坐标轴与世界坐标系的三个轴之间的夹角组成的3×3的矩阵，各个夹角所对应的余弦值所组成的矩阵为二维码坐标系o₁x₁y₁z₁相对于世界坐标系OXYZ的旋转矩阵为如下：${}_1{}^{O}R=\left|\begin{array}{ccc}\overrightarrow{x_1}*\overrightarrow{X_O}& \overrightarrow{y_1}*\overrightarrow{X_O}& \overrightarrow{z_1}*\overrightarrow{X_O}\\ \overrightarrow{x_1}*\overrightarrow{Y_O}& \overrightarrow{y_1}*\overrightarrow{Y_O}& \overrightarrow{z_1}*\overrightarrow{Y_O}\\ \overrightarrow{x_1}*\overrightarrow{Z_O}& \overrightarrow{y_1}*\overrightarrow{Z_O}& \overrightarrow{z_1}*\overrightarrow{Z_O}\end{array}\right|$移动机器人各个坐标系相对于相邻的坐标系的旋转角度以及平移距离在移动机器人系统均已知，其他坐标系的旋转矩阵以及平移矩阵也按照上述方法来计算；二维码坐标系o₁x₁y₁z₁相对于世界坐标系OXYZ有平移，相对于Z轴旋转，旋转矩阵为平移矩阵为P_O1；摄像机坐标系o₂x₂y₂z₂相对于二维码坐标系o₁x₁y₁z₁为平移和旋转，旋转矩阵为平移矩阵为P₁₂；云台a部分坐标系o₃x₃y₃z₃相对于摄像机坐标系o₂x₂y₂z₂只是相对于x₂轴的平移，旋转矩阵为平移矩阵为P₂₃；云台a部分坐标系o₃x₃y₃z₃相对于云台b部分坐标系o₃x₄y₄z₄只是相对于z₃轴的旋转，所以他们共用一个坐标系原点o₃，旋转矩阵为平移矩阵为P₃₄；移动机器人坐标系o₅x₅y₅z₅相对于云台b部分坐标系o₃x₄y₄z₄只是相对于轴方向的平移和旋转，并且在云台b部分的坐标系o₃x₄y₄z₄下，移动机器人的坐标为⁴O₅；(4)通过旋转矩阵与平移矩阵计算移动机器人的位姿：通过计算表示出云台b部分坐标系相对于世界坐标系的旋转矩阵为平移矩阵为P_O1+P₁₂+P₂₃+P₃₄，移动机器人坐标系原点O₅就是移动机器人的位置；根据坐标系变换规则，可以计算出移动机器人位置：${{}^{O}O}_5={}_1{}^{O}R{}_2{}^{1}R{}_3{}^{2}R{}_4{}^{3}R{{}^{4}O}_5+P_{O1}+P_{12}+P_{23}+P_{34}={(x,y,z)}^T$移动机器人坐标系相对于世界坐标系沿着Z轴方向旋转，就是移动机器人坐标系相对于世界坐标系的旋转矩阵，通过旋转矩阵，可以计算出移动机器人坐标系相对于世界坐标系的夹角θ，移动机器人的位姿用M₂矩阵表示为：M₂＝(x,y,θ)^T第四步，二维码定位数据对里程计定位结果修正，使用二维码的定位结果来修正里程计的数据，则：M₁＝M₂＝(x,y,θ)^T。