CN106323294A - 变电站巡检机器人定位方法及定位装置 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及变电站智能巡检技术领域，是一种变电站巡检机器人定位方法及定位装置。

背景技术

现有的移动机器人的定位有基于里程计的定位、基于惯性传感器的定位、视觉定位、超声定位、地图匹配定位、基于多种传感信息的定位、路标与灯塔的定位和同时定位与地图构建(SLAM)等定位方式。目前，变电站智能巡检机器人主要包括轨道式巡检机器人和轮式巡检机器人。轨道式巡检机器人系统是由设置在轨道上的定位片对机器人进行定位，定位精度高，但其能够巡检的空间有限，只能在固定的轨道中移动。轮式巡检机器人定位方式主要是依靠磁轨道进行定位和利用自身传感器系统进行定位，依靠磁轨道进行定位需要事先在底下铺设磁轨道，成本较高，行动范围有限；依靠自身传感器进行定位可以使机器人在大范围内进行移动，但传统的方式是使用激光扫描雷达使机器人进行自定位，成本高昂，机器人不能实现智能定位。

发明专利201210186563.5公布了一种基于二维码的移动机器人定位系统和方法，将视觉定位技术、二维码定位技术及两自由度测速技术有机结合在一起，实现了对移动机器人的准确定位功能，解决了传统视觉定位在图像处理上过于复杂和定位精度不够准确的问题。该方法存在的问题是：1、施工难度大，对人工和环境设备要求非常苛刻，形成人工成本和设备成本的高投入；2、容错性差，移动机器人运行非常不稳定：由于采用一个二维码读码器定位的方法不能容错人工和设备的误差而常常造成误判断，致使移动机器人不得不长期处于大幅度位姿调整的状态，移动机器人运行时机体晃动、摇摆不定，严重影响了稳定性。3、该方法由于采用随动轮测速机构来检测移动机器人的速度，没有对随动轮打滑的情况进行考虑，会造成测速机构所测得的速度不够准确。

发明专利201210472984.4公布了一种基于多个二维码读码器的移动机器人定位系统及方法。该方法是在机器人与地面上安装至少两个二维码读码器和分布于地面上的二维码标签，每次至少同时读取两个二维码标签来实现对移动机器人的定位，有效容错性差，以及读取单个二维码标签所带来的不足。但是该方法仍然存在以下缺点与不足：1、由于二维码张贴在地面上，因此对于地面的平整度要求较高，对环境要求较为苛刻，并且该方法只能适用于室内移动机器人的定位，在室外等情况下无法适用；2、该方法在张贴二维码时施工难度较大，对人工和环境设备要求较为严格，容易形成较高的人工成本与设备成本。

发明内容

本发明提供了一种变电站巡检机器人定位装置和方法，克服了上述现有技术之不足，其能有效解决现有技术中存在的移动机器人稳定性差、容错性差以及移动机器人不能适用于室外移动定位的问题。

本发明的技术方案之一是通过以下措施来实现的：变电站巡检机器人定位方法包括以下步骤：

第一步，建立坐标系，建立世界坐标系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\\ \mathrm{\ω}\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\\ \mathrm{\ω}\end{array}\right|=\left|\begin{array}{ccc}cos\mathrm{\φ}& -sin\mathrm{\φ}& 0\\ sin\mathrm{\φ}& \cos \mathrm{\φ}& 0\\ 0& 0& 1\end{array}\right|*\left|\begin{array}{c}{v}_x\\ v_y\\ \mathrm{\ω}\end{array}\right|$

移动机器人的初始姿态为(x₀，y₀，φ₀)，Δt时间后移动机器人的姿态为(x₁，y₁，φ₁)，计算公式如下：

$\left|\begin{array}{c}{x}_1\\ y_1\\ {\mathrm{\φ}}_1\end{array}\right|=\left|\begin{array}{ccc}\mathrm{\Δ}t& 0& 0\\ 0& \mathrm{\Δ}t& 0\\ 0& 0& \mathrm{\Δ}t\end{array}\right|*\left|\begin{array}{c}{V}_x\\ V_y\\ \mathrm{\ω}\end{array}\right|+\left|\begin{array}{c}{x}_0\\ y_0\\ {\mathrm{\φ}}_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₁的旋转矩阵₂ ¹R以及平移矩阵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，则：

$\mathrm{\γ}=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₁坐标中如下表示：

$\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}-\mathrm{\Δ}x\\ -\mathrm{\Δ}y\\ -\mathrm{\Δ}z\end{array}\right|;$

其中

$L*\sin \mathrm{\β}=\sqrt{{(l*\cos \mathrm{\γ}*\sin \mathrm{\α})}^2+h^2}$

$\mathrm{\α}=arcsin\frac{\sqrt{{(L*sin\mathrm{\β})}^2-h^2}}{l*cos\mathrm{\γ}}$

Δz＝h

Δx＝l*sinα

Δy＝l*cosα

3个未知数θ_x，θ_y和β,需要3个方程，则OA,OB在x₁o₁y₁平面中可表示为：

$\stackrel{\→}{OA}=(a,b,0),\stackrel{\→}{OB}=(c,d,0)$

在o₂x₂y₂z₂坐标系中:

$\stackrel{\→}{OA}\left(x_2{y}_2{z}_2\right)={(a,b,0)}^T*M+\left|\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\\ \mathrm{\Δ}z\end{array}\right|$

$\stackrel{\→}{OB}\left(x_2{y}_2{z}_2\right)={(c,d,0)}^T*M+\left|\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\\ \mathrm{\Δ}z\end{array}\right|$

$\stackrel{\→}{OA}\left(x_2{y}_2{z}_2\right)*\stackrel{\→}{OB}\left(x_2{y}_2{z}_2\right)=0---\left(1\right)$

在z₂方向为0

z₂轴与x₁轴之间的夹角为

${\left({\mathrm{cos\θ}}_{z_{1}x}\right)}^2+{\mathrm{cos\β}}^2+{\mathrm{cos\γ}}^2=1$

其中r已知，可用β表示；根据z₂轴在o₁x₁y₁z₁坐标系中的向量得到关于β的方程；

${(0,0,1)}^T*M+\left|\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\\ \mathrm{\Δ}z\end{array}\right|={(cos\mathrm{\β},cos(90-\mathrm{\γ}),{\mathrm{cos\θ}}_{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的旋转矩阵为如下：

$R_1^O=\left|\begin{array}{ccc}\stackrel{\→}{x_1}*\stackrel{\→}{X_O}& \stackrel{\→}{y_1}*\stackrel{\→}{X_O}& \stackrel{\→}{z_1}*\stackrel{\→}{X_O}\\ \stackrel{\→}{x_1}*\stackrel{\→}{Y_O}& \stackrel{\→}{y_1}*\stackrel{\→}{Y_O}& \stackrel{\→}{z_1}*\stackrel{\→}{Y_O}\\ \stackrel{\→}{x_1}*\stackrel{\→}{Z_O}& \stackrel{\→}{y_1}*\stackrel{\→}{Z_O}& \stackrel{\→}{z_1}*\stackrel{\→}{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=R_1^{O}R_2^{1}R_3^{2}R_4^3{O_4}_5+P_{O1}+P_{12}+P_{23}+P_{34}={(x,y,z)}^T$

移动机器人坐标系相对于世界坐标系沿着Z轴方向旋转，就是移动机器人坐标系相对于世界坐标系的旋转矩阵，通过旋转矩阵，可以计算出移动机器人坐标系相对于世界坐标系的夹角θ，移动机器人的位姿用M₂矩阵表示为：

M₂＝(x,y,θ)^T；

第四步，二维码定位数据对里程计定位结果修正，使用二维码的定位结果来修正里程计的数据，则：M₁＝M₂＝(x,y,θ)^T。

本发明的技术方案之二是通过以下措施来实现的：一种变电站巡检机器人定位装置，包括驱动机器人运动的轮式平台、设置在轮式平台内的机器人运动控制器、编码器、通信模块、摄像机和云台，所述轮式平台沿圆周等间距安装有三个全向轮，三个全向轮上各安装有一个编码器，所述云台底部设有支撑轴，支撑轴固定安装在轮式平台上，所述摄像机安装在云台的顶部，所述机器人运动控制器与通信模块双向通信连接，编码器与机器人运动控制器双向电连接，摄像机和云台均与通信模块双向通信连接。

上述通信模块包括无线路由器和串口联网模块，所述摄像机、云台、无线路由器和机器人运动控制器均与串口联网模块通信连接。

上述还包括远程PC机，所述远程PC机与无线路由器无线通信连接。

上述摄像机包括红外摄像机和高清摄像机，红外摄像机和高清摄像机均与串口联网模块通信连接。

上述编码器A、编码器B和编码器C均为光电编码器。

本发明通过使用三个全向轮移动机器人平台比其他普通轮移动机器人平台运动灵活，可以向任意方向运动。通过使用云台，对二维码的张贴位置没有特殊的要求，实施会更加简单高效。通过采用编码器与二维码方式相结合定位，即使在二维码有障碍物遮挡的情况下仍然可以知道移动机器人的位置。变电站巡检机器人使用二维码的定位方法，在室内与室外环境下都可以使用。

附图说明

附图1为本发明实施例1全方位移动机器人定位方法流程图。

附图2为本发明全方位移动机器人定位坐标系示意图。

附图3为本发明全方位移动机器人运动学简化模型示意图。

附图4为本发明高清摄像机与二维码位置关系示意图。

附图5为本发明高清摄像机相对于世界坐标系的定位坐标示意图。

附图6为本发明二维码读取过程示意图。

附图7为本发明全方位移动机器人运动路线及二维码张贴位置示意图。

附图8为本发明全方位移动机器人依据二维码位置校正自身位置示意图。

附图9为本发明全方位移动机器人定位系统框图。

附图10为本发明全方位移动机器人平台效果示意图。

附图中的编码分别为：1为轮式平台，2为机器人运动控制器，3为云台，4为支撑轴，5为全向轮，6为无线路由器，7为编码器A，8为编码器B，9为编码器C，10为串口联网模块，,11为远程PC机,12为红外摄像机，13为高清摄像机。

具体实施方式

本发明不受下述实施例的限制，可根据本发明的技术方案与实际情况来确定具体的实施方式。

在本发明中，为了便于描述，各部件的相对位置关系的描述均是根据说明书附图1的布图方式来进行描述的，如：前、后、上、下、左、右等的位置关系是依据说明书附图1的布图方向来确定的。

下面结合实施例及附图对本发明作进一步描述：

实施例1：如附图1、2、3、4、5、6、7、8所示，变电站巡检机器人定位方法包括以下步骤：

第一步，建立坐标系，建立世界坐标系OXYZ，二维码坐标系o₁x₁y₁z₁，摄像机坐标系o₂x₂y₂z₂，云台a部分坐标系o₃x₃y₃z₃，云台b部分坐标系o₃x₄y₄z₄和机器人坐标系o₅x₅y₅z₅；

这里的二维码坐标系o₁x₁y₁z₁相对于世界坐标系OXYZ有平移，相对于Z轴旋转。摄像机坐标系o₂x₂y₂z₂相对于二维码坐标系o₁x₁y₁z₁为平移和旋转。云台A部分坐标系o₃x₃y₃z₃相对于摄像机坐标系o₂x₂y₂z₂只是相对于x₂轴的平移。云台A部分坐标系o₃x₃y₃z₃相对于云台B部分坐标系o₃x₄y₄z₄只是相对于z₃轴的旋转，所以他们共用一个坐标系原点o₃。云台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\\ \mathrm{\ω}\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\\ \mathrm{\ω}\end{array}\right|=\left|\begin{array}{ccc}cos\mathrm{\φ}& -sin\mathrm{\φ}& 0\\ sin\mathrm{\φ}& \cos \mathrm{\φ}& 0\\ 0& 0& 1\end{array}\right|*\left|\begin{array}{c}{v}_x\\ v_y\\ \mathrm{\ω}\end{array}\right|$

移动机器人的初始姿态为(x₀，y₀，φ₀)，Δt时间后移动机器人的姿态为(x₁，y₁，φ₁)，计算公式如下：

$\left|\begin{array}{c}{x}_1\\ y_1\\ {\mathrm{\φ}}_1\end{array}\right|=\left|\begin{array}{ccc}\mathrm{\Δ}t& 0& 0\\ 0& \mathrm{\Δ}t& 0\\ 0& 0& \mathrm{\Δ}t\end{array}\right|*\left|\begin{array}{c}{V}_x\\ V_y\\ \mathrm{\ω}\end{array}\right|+\left|\begin{array}{c}{x}_0\\ y_0\\ {\mathrm{\φ}}_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，边A1O1的长度为标定时假设当物体距离相机距离为1m时，实际中物体的长度和图像中物体长度的比为m，相机与二维码之间的距离计算公式如下：

$l=\frac{{\mathrm{length}}_{AO}}{{\mathrm{length}}_{A_1{O}_1}}*m$

已知二维码张贴高度h，移动机器人高清摄像机距离二维码的长度为l，则：

$\mathrm{\γ}=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₁坐标中如下表示：

$\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}-\mathrm{\Δ}x\\ -\mathrm{\Δ}y\\ -\mathrm{\Δ}z\end{array}\right|;$

其中

$L*\sin \mathrm{\β}=\sqrt{{(l*\cos \mathrm{\γ}*\sin \mathrm{\α})}^2+h^2}$

$\mathrm{\α}=arcsin\frac{\sqrt{{(L*sin\mathrm{\β})}^2-h^2}}{l*cos\mathrm{\γ}}$

Δz＝h

Δx＝l*sinα

Δy＝l*cosα

3个未知数θ_x，θ_y和β,需要3个方程，则OA,OB在x₁o₁y₁平面中可表示为：

$\stackrel{\→}{OA}=(a,b,0),\stackrel{\→}{OB}=(c,d,0)$

在o₂x₂y₂z₂坐标系中:

$\stackrel{\→}{OA}\left(x_2{y}_2{z}_2\right)={(a,b,0)}^T*M+\left|\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\\ \mathrm{\Δ}z\end{array}\right|$

$\stackrel{\→}{OB}\left(x_2{y}_2{z}_2\right)={(c,d,0)}^T*M+\left|\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\\ \mathrm{\Δ}z\end{array}\right|$

$\stackrel{\→}{OA}\left(x_2{y}_2{z}_2\right)*\stackrel{\→}{OB}\left(x_2{y}_2{z}_2\right)=0---\left(1\right)$

在z₂方向为0

z₂轴与x₁轴之间的夹角为

${\left({\mathrm{cos\θ}}_{z_{1}x}\right)}^2+{\mathrm{cos\β}}^2+{\mathrm{cos\γ}}^2=1$

其中r已知，可用β表示；根据z₂轴在o₁x₁y₁z₁坐标系中的向量得到关于β的方程；

${(0,0,1)}^T*M+\left|\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\\ \mathrm{\Δ}z\end{array}\right|={(cos\mathrm{\β},cos(90-\mathrm{\γ}),{\mathrm{cos\θ}}_{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的旋转矩阵为如下：

$R_1^O=\left|\begin{array}{ccc}\stackrel{\→}{x_1}*\stackrel{\→}{X_O}& \stackrel{\→}{y_1}*\stackrel{\→}{X_O}& \stackrel{\→}{z_1}*\stackrel{\→}{X_O}\\ \stackrel{\→}{x_1}*\stackrel{\→}{Y_O}& \stackrel{\→}{y_1}*\stackrel{\→}{Y_O}& \stackrel{\→}{z_1}*\stackrel{\→}{Y_O}\\ \stackrel{\→}{x_1}*\stackrel{\→}{Z_O}& \stackrel{\→}{y_1}*\stackrel{\→}{Z_O}& \stackrel{\→}{z_1}*\stackrel{\→}{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=R_1^{O}R_2^{1}R_3^{2}R_4^3{O_4}_5+P_{O1}+P_{12}+P_{23}+P_{34}={(x,y,z)}^T$

移动机器人坐标系相对于世界坐标系沿着Z轴方向旋转，就是移动机器人坐标系相对于世界坐标系的旋转矩阵，通过旋转矩阵，可以计算出移动机器人坐标系相对于世界坐标系的夹角，移动机器人的位姿用M₂矩阵表示为：

M₂＝(x,y,θ)^T；

第四步，二维码定位数据对里程计定位结果修正，使用二维码的定位结果来修正里程计的数据，则：M₁＝M₂＝(x,y,θ)^T。

这里考虑到机器人在移动的过程中存在打滑的情况，所以里程计的定位结果存在误差。二维码的位置在世界坐标系中是已知的，所以使用二维码的定位结果来修正里程计的数据；当移动机器人移动到没有二维码的地方时采用里程计的定位结果M₁，当移动机器人运动到有二维码的地方时，使用二维码的定位结果M₂来修正里程计的定位结果M₁。因移动机器人在移动的过程中存在打滑的情况，里程计的定位结果存在误差，通过二维码定位数据对里程计定位结果进行修正移动机器人不需要进行大幅度位姿调整，有效提高了移动机器人的容错性和稳定性。

实施例2：如附图9、10所示，一种变电站巡检机器人定位装置，包括驱动机器人运动的轮式平台1、设置在轮式平台1内的机器人运动控制器2、编码器、通信模块、摄像机和云台3，所述轮式平台1沿圆周等间距安装有三个全向轮5，三个全向轮5上各安装有一个编码器，所述云台3底部设有支撑轴4，支撑轴4固定安装在轮式平台1上，所述摄像机安装在云台3的顶部，所述机器人运动控制器2与通信模块双向通信连接，编码器与机器人运动控制器2双向电连接，摄像机和云台3均与通信模块双向通信连接。

在实际工作时，轮式平台1起到驱动变电站巡检机器人运动和支撑云台的作用，在每个全向轮5上分别安装有一个电机，机器人运动控制器2控制电机的转向和转速，电机驱动车轮转动，从而实现机器人的运动；这里采用三个全向轮5带动旋转机器人的运动，能够快速实现机器人的360°转向，比起四轮驱动更方便灵活。这里的电机、机器人运动控制器2、通信模块均由固定在轮式平台内的大容量电池提供电源；编码器分别为编码器A7、编码器B8和编码器C9，机器人运动控制器2接收编码器A7、编码器B8和编码器C9发送的全向轮5的位置信息。这里的云台3为两自由度云台，包括云台a部分和云台b部分，云台a部分的两侧臂安装在云台b部分上，云台a部分和云台b部分均能够在机器人运动控制器的控制指令下旋转，云台a部分能够带动摄像机转动，随时调整摄像头的角度，便于寻找设备二维码位置。

如附图9、10所示，通信模块包括无线路由器6和串口联网模块10，所述摄像机、云台3、无线路由器6和机器人运动控制器2均与串口联网模块10通信连接。串口联网模块10的作用是将串口转化为RJ-45的网络接口，摄像机、串口联网模块10与无线路由器6之间均通过网口连接，云台3与串口联网模块10之间使用RS485通信接口连接，机器人运动控制器2与串口联网模块10之间使用RS232通信接口连接。

如附图9、10所示，还包括远程PC机11，所述远程PC机11与无线路由器6无线通信连接。远程PC机11与无线路由器6之间采用无线网络通信连接，用户通过远程PC机11控制巡检机器人运动，实现对设备的远程监控。

如附图9、10所示，所述摄像机包括红外摄像机12和高清摄像机13，红外摄像机12和高清摄像机13均与串口联网模块10通信连接。高清摄像机13上设有网络接口和USB接口，主要用于采集变电站内的二维码图像信息，这里的二维码图像均为矩形；红外摄像机13主要利用红外采集二维码的图像，便于更清楚的采集二维码信息。

如附图9、10所示，编码器均为光电编码器。这里的光电编码器A7、光电编码器B8和光电编码器C9分别安装在驱动移动机器人全向轮5运动的三个电机上，用于检测电机的转速，便于计算，从而实现对移动机器人进行精确定位。

实施例3：如附图1、2、3、4、5、6、7、8、9、10所示，基于二维码信息对变电站巡检机器人精确定位方法，包括以下步骤：

(1)使用二维码生成器生成带有编号的变电站设备二维码信息；

(2)将生成的二维码图片装订好，贴在二维码信息对应的变电站相应设备上并获得每个二维码在整个地图中的坐标；

(3)由计算机自动生成机器人的运动轨迹图，将轨迹图通过远程PC机以无线的形式发送给移动机器人；

(4)将搭载云台及高清摄像机的移动机器人放在变电站中按轨迹图进行移动，在机器人运动过程中，根据编码器得到机器人的位置信息，编码器具有累计误差，机器人运动越久，定位误差越大；当误差增大时，机器人自身检测到的自身位置和实际机器人所在的位置就会不同；

(5)在没有二维码区域移动机器人利用编码器进行定位，当捕捉到二维码时，机器人利用二维码信息进行识别，定位自身所在位置；二维码所代表的就是地图中坐标信息，设定移动机器人设置有两种路线，第一种路线依序经过的设备二维码位置为是0001至0002至0003至0004至0005至0006至0007至0008至0009,第二种路线是0001至0004至0002至0003至0005至0006至0008至0007至0009，在两种路线中，机器人在运动过程中自身定位误差会不同，但移动机器人根据二维码的设备位置信息调整后，可以纠正其定位误差；因为二维码自身所包含的信息中有自身的编号，机器人通过无线通信将编号信息发送给远程PC机11，远程PC机11遍寻编号后，得知该编号二维码在地图中的位置信息；

(6)远程PC机11查得二维码所在位置信息后，将信息发送给移动机器人，移动机器人根据自己相对于二维码的位置，从而得到移动机器人在全局中的准确位置，将自身的位置信息进行纠正，实现对移动机器人的全局精确定位。例如：0012号二维码所在位置是(X₁,Y₁),然后根据机器人相对二维码的距离和方位，确定机器人自身位置(X₂,Y₂)。

以上技术特征构成了本发明的实施例，其具有较强的适应性和实施效果，可根据实际需要增减非必要的技术特征，来满足不同情况的需求。

Claims (9)

1.一种变电站巡检机器人定位方法，其特征在于包括以下步骤：

第一步，建立坐标系，建立世界坐标系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\\ \mathrm{\ω}\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\\ \mathrm{\ω}\end{array}\right|=\left|\begin{array}{ccc}cos\mathrm{\φ}& -sin\mathrm{\φ}& 0\\ sin\mathrm{\φ}& \cos \mathrm{\φ}& 0\\ 0& 0& 1\end{array}\right|*\left|\begin{array}{c}{v}_x\\ v_y\\ \mathrm{\ω}\end{array}\right|$

移动机器人的初始姿态为(x₀，y₀，φ₀)，Δt时间后移动机器人的姿态为(x₁，y₁，φ₁)，计算公式如下：

$\left|\begin{array}{c}{x}_1\\ y_1\\ {\mathrm{\φ}}_1\end{array}\right|=\left|\begin{array}{ccc}\mathrm{\Δ}t& 0& 0\\ 0& \mathrm{\Δ}t& 0\\ 0& 0& \mathrm{\Δ}t\end{array}\right|*\left|\begin{array}{c}{V}_x\\ V_y\\ \mathrm{\ω}\end{array}\right|+\left|\begin{array}{c}{x}_0\\ y_0\\ {\mathrm{\φ}}_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，则：

$\mathrm{\γ}=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}-\mathrm{\Δ}x\\ -\mathrm{\Δ}y\\ -\mathrm{\Δ}z\end{array}\right|;$

其中

$L*sin\mathrm{\β}=\sqrt{{(l*cos\mathrm{\γ}*sin\mathrm{\α})}^2+h^2}$

$\mathrm{\α}=arcsin\frac{\sqrt{{(L*sin\mathrm{\β})}^2-h^2}}{l*cos\mathrm{\γ}}$

Δz＝h

Δx＝l*sinα

Δy＝l*cosα

3个未知数θ_x，θ_y和β,需要3个方程，则OA,OB在x₁o₁y₁平面中可表示为：

$\stackrel{\→}{OA}=(a,b,0),\stackrel{\→}{OB}=(c,d,0)$

在o₂x₂y₂z₂坐标系中:

$\stackrel{\→}{OA}\left(x_2{y}_2{z}_2\right)={(a,b,0)}^T*M+\left|\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\\ \mathrm{\Δ}z\end{array}\right|$

$\stackrel{\→}{OB}\left(x_2{y}_2{z}_2\right)={(c,d,0)}^T*M+\left|\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\\ \mathrm{\Δ}z\end{array}\right|$

$\stackrel{\→}{OA}\left(x_2{y}_2{z}_2\right)*\stackrel{\→}{OB}\left(x_2{y}_2{z}_2\right)=0---\left(1\right)$

在z₂方向为0

$cos\mathrm{\θ}=\frac{\stackrel{\→}{O_1{A}_1}*\stackrel{\→}{O_1{B}_1}}{\left|O_1{A}_1\right|*\left|O_1{B}_1\right|}---\left(2\right)$

z₂轴与x₁轴之间的夹角为

${\left({\mathrm{cos\θ}}_{z_{1}x}\right)}^2+{\mathrm{cos\β}}^2+{\mathrm{cos\γ}}^2=1$

其中r已知，可用β表示；根据z₂轴在o₁x₁y₁z₁坐标系中的向量得到关于β的方程；

${(0,0,1)}^T*M+\left|\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\\ \mathrm{\Δ}z\end{array}\right|={(cos\mathrm{\β},cos(90-\mathrm{\γ}),{\mathrm{cos\θ}}_{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的旋转矩阵为如下：

$R_1^O=\left|\begin{array}{ccc}\stackrel{\→}{x_1}*\stackrel{\→}{X_O}& \stackrel{\→}{y_1}*\stackrel{\→}{X_O}& \stackrel{\→}{z_1}*\stackrel{\→}{X_O}\\ \stackrel{\→}{x_1}*\stackrel{\→}{Y_O}& \stackrel{\→}{y_1}*\stackrel{\→}{Y_O}& \stackrel{\→}{z_1}*\stackrel{\→}{Y_O}\\ \stackrel{\→}{x_1}*\stackrel{\→}{Z_O}& \stackrel{\→}{y_1}*\stackrel{\→}{Z_O}& \stackrel{\→}{z_1}*\stackrel{\→}{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=R_1^{O}R_2^{1}R_3^{2}R_4^3{O_4}_5+P_{O1}+P_{12}+P_{23}+P_{34}={(x,y,z)}^T$

移动机器人坐标系相对于世界坐标系沿着Z轴方向旋转，就是移动机器人坐标系相对于世界坐标系的旋转矩阵，通过旋转矩阵，可以计算出移动机器人坐标系相对于世界坐标系的夹角θ，移动机器人的位姿用M₂矩阵表示为：

M₂＝(x,y,θ)^T

第四步，二维码定位数据对里程计定位结果修正，使用二维码的定位结果来修正里程

计的数据，则：M₁＝M₂＝(x,y,θ)^T。

2.一种变电站巡检机器人定位装置，其特征在于包括驱动机器人运动的轮式平台、设置在轮式平台内的机器人运动控制器、编码器、通信模块、摄像机和云台，所述轮式平台沿圆周等间距安装有三个全向轮，三个全向轮上各安装有一个编码器，所述云台底部设有支撑轴，支撑轴固定安装在轮式平台上，所述摄像机安装在云台的顶部，所述机器人运动控制器与通信模块双向通信连接，编码器与机器人运动控制器双向电连接，摄像机和云台均与通信模块双向通信连接。

3.根据权利要求2所述的变电站巡检机器人定位装置，其特征在于通信模块包括无线路由器和串口联网模块，所述摄像机、云台、无线路由器和机器人运动控制器均与串口联网模块通信连接。

4.根据权利要求2或3所述的变电站巡检机器人定位装置，其特征在于还包括远程PC机，所述远程PC机与无线路由器无线通信连接。

5.根据权利要求3所述的变电站巡检机器人定位装置，其特征在于所述摄像机包括红外摄像机和高清摄像机，红外摄像机和高清摄像机均与串口联网模块通信连接。

6.根据权利要求4所述的变电站巡检机器人定位装置，其特征在于所述摄像机包括红外摄像机和高清摄像机，红外摄像机和高清摄像机均与串口联网模块通信连接。

7.根据权利要求2或3所述的变电站巡检机器人定位装置，其特征在于编码器均为光电编码器。

8.根据权利要求4所述的变电站巡检机器人定位装置，其特征在于编码器均为光电编码器。

9.根据权利要求5所述的变电站巡检机器人定位装置，其特征在于编码器均为光电编码器。