CN105137998B - 一种基于激光扫描仪的自主导航方法 - Google Patents
一种基于激光扫描仪的自主导航方法 Download PDFInfo
- Publication number
- CN105137998B CN105137998B CN201510668566.6A CN201510668566A CN105137998B CN 105137998 B CN105137998 B CN 105137998B CN 201510668566 A CN201510668566 A CN 201510668566A CN 105137998 B CN105137998 B CN 105137998B
- Authority
- CN
- China
- Prior art keywords
- module
- inspection
- moment
- crusing robot
- coordinate
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 38
- 238000007689 inspection Methods 0.000 claims abstract description 71
- 238000001914 filtration Methods 0.000 claims description 12
- 230000033001 locomotion Effects 0.000 claims description 11
- 238000005259 measurement Methods 0.000 claims description 9
- 239000011159 matrix material Substances 0.000 claims description 4
- 238000012163 sequencing technique Methods 0.000 claims description 4
- 230000009897 systematic effect Effects 0.000 claims description 4
- 239000004575 stone Substances 0.000 abstract description 5
- 230000010006 flight Effects 0.000 abstract description 3
- 238000010276 construction Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 239000002184 metal Substances 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于激光扫描仪的自主导航方法，包括以下步骤：建立特征地图和巡检线路：通过人工的方式牵引巡检机器人完成对巡检路线的一次巡视，依次巡视形成以下模块：位置测量模块→位置估计模块→几何特征抽象模块→全局特征地图构建模块；巡检：巡检机器人自动按照以下步骤进行巡检，直至结束：位置测量模块→几何特征抽象模块→特征匹配模块→位置估计模块→机器人位置调整模块。本发明具有不用在变电站进行施工，对变电站现有的道路也没有苛刻的条件，碎石路和坡度不大的草地上也可以正常运行，还可以临时、灵活的调整巡检路线的优点。
Description
技术领域
本发明涉及一种自助主导航方法，特别是一种基于激光扫描仪的自主导航方法。
背景技术
目前的电力巡检导航方式有以下几种，分别存在着相应的优缺点。
电磁导航：
电磁导航系统中的金属线由于是被埋设在地下的，具有隐蔽性好、不易被污染的优点。此外，该系统导航对外界信号干扰较小，成本较低，并且它的原理简单可靠。但是，电磁导航系统也有很大的缺点存在：当它的路径一旦被确定，更改起来就会特别困难。因此该导航方式不灵活，也不易于维护。并且在变电站的环境中，容易受到变电站强磁场的干扰。
磁带导航：
磁带导航与电磁导航有着比较相近的原理。在磁带导航中，磁带是被铺设在路面上的。而在电磁导航中，是把导线预先埋设在地下的。相比较而言，磁带导航的方式有不错的灵活性，能够更容易的改变导航路线，同时铺设也会比较容易。但是，这种导航方式也存在着如下缺点：因为将磁带铺设在地面上，所以它有被损坏或者被弄脏的风险；另外在草地和碎石路上，导航磁带是无法铺设的。
激光导航：
激光导航的原理是：在工作场地的四周，固定好最少三块使用高反光性材料制做而成的导引标志。将激光扫描器安装在巡检车上，然后对脉冲激光发射器发射出的并且被四周的导引标志反射回来的激光进行扫描，从而就可以计算出巡检车的位置坐标。但是激光导航存在的问题是，在变电站的环境中，需要铺设反光标识物，并且反光标识物在室外的环境中也容易弄脏、损坏、变形等，致使定位误差变大或者失效。
以上导航方式的缺点一是都需要在变电站进行改造施工，可能影响现有变电站的道路规划结构。二是巡检路径施工完成后，要变更巡检路线十分困难。
发明内容
在下文中给出关于本发明的简要概述，以便提供关于本发明的某些方面的基本理解。应当理解，这个概述并不是关于本发明的穷举性概述。它并不是意图确定本发明的关键或重要部分，也不是意图限定本发明的范围。其目的仅仅是以简化的形式给出某些概念，以此作为稍后论述的更详细描述的前序。
本发明的目的是在于提供一种不用在变电站进行施工，对变电站现有的道路也没有苛刻的条件，碎石路和坡度不大的草地上也可以正常运行，还可以临时、灵活的基于激光扫描仪的自主导航方法。
一种基于激光扫描仪的自主导航方法，包括以下步骤：
A、建立特征地图和巡检线路：通过人工的方式牵引巡检机器人完成对巡检路线的一次巡视，依次巡视形成以下模块：位置测量模块→位置估计模块→几何特征抽象模块→全局特征地图构建模块；
B、巡检：巡检机器人自动按照以下步骤进行巡检，直至结束：位置测量模块→几何特征抽象模块→特征匹配模块→位置估计模块→机器人位置调整模块；
步骤A中所述位置测量模块的建立：首先根据里程计得出巡检机器人在T到T+1时刻沿行进方向运动的距离；然后根据陀螺仪测得行进方向和水平X轴的夹角；根据T时刻的全局坐标(XT,YT)计算得到T+1时刻的全局坐标(XT+1,YT+1)；
步骤A中的所述位置估计模块的建立：运用卡尔曼滤波建立巡检机器人的运动模型和观测模型；
X(K)＝A*X(K-1)+B*U(K)+W(K) (1)
Z(K)＝H*X(K)+V(K) (2)
X(K)为K时刻的系统状态，U(K)为K时刻对系统的控制量，A和B为系统参数；Z(K)为K时刻的测量值，H为测量系数；W(K)和V(K)分别表示系统和测量过程中的噪声，噪声满足高斯白噪声模型，设W(K)和V(K)的协方差分别为Q和R；
基于卡尔曼滤波的5个经典核心公式：
X(k|k-1)＝A*X(k-1|k-1)+B*U(k) (3)
其中X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)为上一时刻的最优估计值，U(k)为现在状态的控制量，在模型中，控制量U(k)为0；
经过公式(3)之后，系统结果已经更新了，接下来计算X(k|k-1)的协方差，用P来表示协方差，则为：
P(k|k-1)＝A*P(k-1|k-1)*AT+Q (4)
其中P(k|k-1)为X(k|k-1)对应的协方差，P(k-1|k-1)为X(k-1|k-1)对应的协方差，AT为A的转置矩阵，Q为系统噪声。
X(k|k)＝X(k|k-1)+Kg(k)*(Z(k)–H*X(k|k-1)) (5)
Kg(k)＝P(k|k-1)*HT/(H*P(k|k-1)*HT+R) (6)
P(k|k)＝(1-Kg(k)*H)*P(k|k-1) (7)；
步骤A中所述的几何特征抽象模块的建立：对于激光扫描仪采集的数据点，将其抽象出几何特征，以便在巡检过程中进行位置匹配；抽象出的几何特征为直线段和圆弧，使用的方法为最小二乘法拟合；对于拟合出的直线段，取线段的中心点(x1,y1)为质心，直线段则标识为(x1,y1,L,θl)，L为线段的长度，θl为线段与x轴的夹角；对于拟合出的圆弧，我们取其圆心(xr,yr)为质心，圆弧则标识为(xr,yr,R)，R为圆弧半径；
步骤A中所述的全局特征地图构建模块的建立：对应每一个拟合出的直线段和圆弧，加上位置测量模块中里程计给出的信息，确定其全局坐标系中的坐标(x,y)，其中每个特征都有全局坐标系中的坐标，和其先后的顺序，以及每一个时刻机器人的位置信息集P((x1,y1),(x2,y2)……(xn,yn))；
巡检步骤B中所述的位置测量模块：根据里程计得出巡检机器人在T到T+1时刻沿行进方向运动的距离；根据陀螺仪测得行进方向和水平X轴的夹角；根据T时刻的全局坐标(XT,YT)计算得到T+1时刻的全局坐标(XT+1,YT+1)；
巡检步骤B中的所述几何特征抽象模块：对于激光扫描仪采集的数据点，使用最小二乘法进行拟合，得到直线段和圆弧；此时：对于拟合出的直线段，得到其(L,θl)，其中L为线段的长度，θl为线段与x轴的夹角；对于拟合出的圆弧，得到其R，R为圆弧半径；
巡检步骤B中的特征匹配模块：对于上一模块中抽象出来的直线段或者圆弧，参考位置测量模块中里程计给出的位置信息以及匹配的先后顺序，将抽象出来的几何特征在全局特征地图中进行匹配，以确认该几何特征对应的全局地图中的特征；选取匹配成功的两个特征，获取其全局位置坐标信息，并且计算质心与巡检机器人的距离d，得到(x1,y1,d1)和(x2,y2,d2)；
巡检步骤B中的位置估计模块：根据上一模块特征匹配得到的两个匹配成功特征的信息(x1,y1,d1)和(x2,y2,d2)，运用平面三角定位的方法，计算当时时刻巡检机器人的坐标(xr,yr)，在取得巡检机器人的坐标之后，运用特征地图和巡检线路的建立中相同的卡尔曼滤波方法计算得到巡检机器人的位置最佳估计值(x′r,y′r)；
巡检步骤B中所述机器人位姿调整模块：根据上一模块得到的位置最佳估计值(x′r,y′r)，在位置信息集P中查找对应的坐标(xs,ys)，调整机器人PID控制部分，实现机器人从(x′r,y′r)--→(xs,ys)的移动。
本发明与现有技术相比具有以下优点：
不用在变电站进行施工，对变电站现有的道路也没有苛刻的条件，碎石路和坡度不大的草地上也可以正常运行，还可以临时、灵活的调整巡检路线。
附图说明
图1为本发明特征地图和巡检线路建立过程的流程图；
图2为本发明巡检过程的流程图；
图3为平面三角定位的算法图；
具体实施方式
为了加深对本发明的理解，下面将结合实施例对本发明作进一步详述，该实施例仅用于解释本发明，并不构成对本发明保护范围的限定。
如图1-3示出了本发明一种基于激光扫描仪的自主导航方法的两种实施方式：
实施例1：
一种基于激光扫描仪的自主导航方法，包括以下步骤：
A、建立特征地图和巡检线路：通过人工的方式牵引巡检机器人完成对巡检路线的一次巡视，依次巡视形成以下模块：位置测量模块→位置估计模块→几何特征抽象模块→全局特征地图构建模块；
B、巡检：
巡检机器人自动按照以下步骤进行巡检，直至结束：位置测量模块→几何特征抽象模块→特征匹配模块→位置估计模块→机器人位置调整模块；
步骤A中所述位置测量模块的建立：首先根据里程计得出巡检机器人在T到T+1时刻沿行进方向运动的距离；然后根据陀螺仪测得行进方向和水平X轴的夹角；根据T时刻的全局坐标(XT,YT)计算得到T+1时刻的全局坐标(YT+1,YT+1)；
步骤A中的所述位置估计模块的建立：运用卡尔曼滤波建立巡检机器人的运动模型和观测模型；
X(K)＝A*X(K-1)+B*U(K)+W(K) (1)
Z(K)＝H*X(K)+V(K) (2)
X(K)为K时刻的系统状态，U(K)为K时刻对系统的控制量，A和B为系统参数；Z(K)为K时刻的测量值，H为测量系数；W(K)和V(K)分别表示系统和测量过程中的噪声，噪声满足高斯白噪声模型，设W(K)和V(K)的协方差分别为Q和R；
基于卡尔曼滤波的5个经典核心公式：
X(k|k-1)＝A*X(k-1|k-1)+B*U(k) (3)
其中X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)为上一时刻的最优估计值，U(k)为现在状态的控制量，在模型中，控制量U(k)为0；
经过公式(3)之后，系统结果已经更新了，接下来计算X(k|k-1)的协方差，用P来表示协方差，则为：
P(k|k-1)＝A*P(k-1|k-1)*AT+Q (4)
其中P(k|k-1)为X(k|k-1)对应的协方差，P(k-1|k-1)为X(k-1|k-1)对应的协方差，AT为A的转置矩阵，Q为系统噪声。
X(k|k)＝X(k|k-1)+kg(k)*(Z(k)–H*X(k|k-1)) (5)
kg(k)＝P(k|k-1)*HT/(H*P(k|k-1)*HT+R) (6)
P(k|k)＝(1-kg(k)*H)*P(k|k-1) (7)；
步骤A中所述的几何特征抽象模块的建立：对于激光扫描仪采集的数据点，将其抽象出几何特征，以便在巡检过程中进行位置匹配；抽象出的几何特征为直线段和圆弧，使用的方法为最小二乘法拟合；对于拟合出的直线段，取线段的中心点((x1,y1)为质心，直线段则标识为((x1,y1,L,θl)，L为线段的长度，θl为线段与x轴的夹角；对于拟合出的圆弧，我们取其圆心(xr,yr)为质心，圆弧则标识为(xr,yr,R)，R为圆弧半径；
步骤A中所述的全局特征地图构建模块的建立：对应每一个拟合出的直线段和圆弧，加上位置测量模块中里程计给出的信息，确定其全局坐标系中的坐标(x,y)，其中每个特征都有全局坐标系中的坐标，和其先后的顺序，以及每一个时刻机器人的位置信息集P((x1,y1),(x2,y2)……(xn,yn))；
巡检步骤B中所述的位置测量模块：根据里程计得出巡检机器人在T到T+1时刻沿行进方向运动的距离；根据陀螺仪测得行进方向和水平X轴的夹角；根据T时刻的全局坐标(XT,YT)计算得到T+1时刻的全局坐标(XT+1,YT+1)；
巡检步骤B中的所述几何特征抽象模块：对于激光扫描仪采集的数据点，使用最小二乘法进行拟合，得到直线段和圆弧；此时：对于拟合出的直线段，得到其(L,θl)，其中L为线段的长度，θl为线段与x轴的夹角；对于拟合出的圆弧，得到其R，R为圆弧半径；
巡检步骤B中的特征匹配模块：对于上一模块中抽象出来的直线段或者圆弧，参考位置测量模块中里程计给出的位置信息以及匹配的先后顺序，将抽象出来的几何特征在全局特征地图中进行匹配，以确认该几何特征对应的全局地图中的特征；选取匹配成功的两个特征，获取其全局位置坐标信息，并且计算质心与巡检机器人的距离d，得到(x1,y1,d1)和(x2,y2,d2)；
巡检步骤B中的位置估计模块：根据上一模块特征匹配得到的两个匹配成功特征的信息(x1,y1,d1)和(x2,y2,d2))，运用平面三角定位的方法，计算当时时刻巡检机器人的坐标(xr,yr)，在取得巡检机器人的坐标之后，运用特征地图和巡检线路的建立中相同的卡尔曼滤波方法计算得到巡检机器人的位置最佳估计值(x′r,y′r)；
巡检步骤B中所述机器人位姿调整模块：根据上一模块得到的位置最佳估计值(x′r,y′r)，在位置信息集P中查找对应的坐标(xs,ys)，调整机器人PID控制部分，实现机器人从(x′r,y′r)--→(xs,ys)的移动；
实施例2：
一种基于激光扫描仪的自主导航方法，包括以下步骤：
A、建立特征地图和巡检线路：通过人工的方式牵引巡检机器人完成对巡检路线的一次巡视，依次巡视形成以下模块：位置测量模块→位置估计模块→几何特征抽象模块→全局特征地图构建模块；
B、巡检：巡检机器人自动按照以下步骤进行巡检，直至结束：位置测量模块→几何特征抽象模块→特征匹配模块→位置估计模块→机器人位置调整模块；
步骤A中所述位置测量模块的建立：首先根据里程计得出巡检机器人在T到T+1时刻沿行进方向运动的距离；然后根据陀螺仪测得行进方向和水平X轴的夹角；根据T时刻的全局坐标(XT,YT)计算得到T+1时刻的全局坐标(XT+1,YT+1)；
步骤A中的所述位置估计模块的建立：运用卡尔曼滤波建立巡检机器人的运动模型和观测模型；
X(K)＝A*X(K-1)+B*U(K)+W(K) (1)
Z(K)＝H*X(K)+V(K) (2)
X(K)为K时刻的系统状态，U(K)为K时刻对系统的控制量，A和B为系统参数；Z(K)为K时刻的测量值，H为测量系数；W(K)和V(K)分别表示系统和测量过程中的噪声，噪声满足高斯白噪声模型，设W(K)和V(K)的协方差分别为Q和R；
基于卡尔曼滤波的5个经典核心公式：
X(k|k-1)＝A*X(k-1|k-1)+B*U(k) (3)
其中X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)为上一时刻的最优估计值，U(k)为现在状态的控制量，在模型中，控制量U(k)为0；
经过公式(3)之后，系统结果已经更新了，接下来计算X(k|k-1)的协方差，用P来表示协方差，则为：
P(k|k-1)＝A*P(k-1|k-1)*AT+Q (4)
其中P(k|k-1)为X(k|k-1)对应的协方差，P(k-1|k-1)为X(k-1|k-1)对应的协方差，AT为A的转置矩阵，Q为系统噪声。
X(k|k)＝X(k|k-1)+Kg(k)*(Z(k)–H*X(k|k-1)) (5)
Kg(k)＝P(k|k-1)*HT/(H*P(k|k-1)*HT+R) (6)
P(k|k)＝(1-Kg(k)*H)*P(k|k-1) (7)。
根据实际的观测模型，简化上述公式。令公式(3)中的A＝1，U(k)＝0，则公式简化为：
X(k|k-1)＝X(k-1|k-1) (8)
公式(4)简化为：
P(k|k-1)＝P(k-1|k-1)+Q (9)
再令H＝1，公式(5)(6)(7)可简化为：
X(k|k)＝X(k|k-1)+Kg(k)*(Z(k)–X(k|k-1)) (10)
Kg(k)＝P(k|k-1)/(P(k|k-1)+R) (11)
P(k|k)＝(1-Kg(k))*P(k|k-1) (12)
通过公式(8)(9)(10)(11)(12)得到每一时刻的最优估计值X(k|k)；
步骤A中所述的几何特征抽象模块的建立：对于激光扫描仪采集的数据点，将其抽象出几何特征，以便在巡检过程中进行位置匹配；抽象出的几何特征为直线段和圆弧，使用的方法为最小二乘法拟合；对于拟合出的直线段，取线段的中心点(x1,y1)为质心，直线段则标识为(x1,y1,L,θl)，L为线段的长度，θl为线段与x轴的夹角；对于拟合出的圆弧，我们取其圆心(xr,yr)为质心，圆弧则标识为(xr,yr,R)，R为圆弧半径；
步骤A中所述的全局特征地图构建模块的建立：对应每一个拟合出的直线段和圆弧，加上位置测量模块中里程计给出的信息，确定其全局坐标系中的坐标(x,y)，其中每个特征都有全局坐标系中的坐标，和其先后的顺序，以及每一个时刻机器人的位置信息集P((x1,y1),(x2,y2)……(xn,yn))；
巡检步骤B中所述的位置测量模块：根据里程计得出巡检机器人在T到T+1时刻沿行进方向运动的距离；根据陀螺仪测得行进方向和水平X轴的夹角；根据T时刻的全局坐标(XT,YT)计算得到T+1时刻的全局坐标(XT+1,YT+1)；
巡检步骤B中的所述几何特征抽象模块：对于激光扫描仪采集的数据点，使用最小二乘法进行拟合，得到直线段和圆弧；此时：对于拟合出的直线段，得到其(L,θl)，其中L为线段的长度，θl为线段与x轴的夹角；对于拟合出的圆弧，得到其R，R为圆弧半径；
巡检步骤B中的特征匹配模块：对于上一模块中抽象出来的直线段或者圆弧，参考位置测量模块中里程计给出的位置信息以及匹配的先后顺序，将抽象出来的几何特征在全局特征地图中进行匹配，以确认该几何特征对应的全局地图中的特征；选取匹配成功的两个特征，获取其全局位置坐标信息，并且计算质心与巡检机器人的距离d，得到(x1,y1,d1)和(x2,y2,d2)；
巡检步骤B中的位置估计模块：根据上一模块特征匹配得到的两个匹配成功特征的信息(x1,y1,d1)和(x2,y2,d2)，运用平面三角定位的方法，计算当时时刻巡检机器人的坐标(xr,yr)，在取得巡检机器人的坐标之后，运用特征地图和巡检线路的建立中相同的卡尔曼滤波方法计算得到巡检机器人的位置最佳估计值(x′r,y′r)；
巡检步骤B中所述机器人位姿调整模块：根据上一模块得到的位置最佳估计值(x′r,y′r)，在位置信息集P中查找对应的坐标(xs,ys)，调整机器人PID控制部分，实现机器人从(x′r,y′r)--→(xs,ys)的移动；
所述平面三角定位的算法为：
以(x1,y1)为圆心，d1为半径做圆C1；(x2,y2)为圆心，d2为半径做圆C2。两圆相交的两点之一即为当时时刻巡检机器人的坐标(xr,yr)。
圆C1 C2的方程分别为：
(x-x1)2+(y-y1)2＝d1 2
(x-x2)2+(y-y2)2＝d2 2
在圆C1上顺序取点(x,y)，代入计算(x-x2)2+(y-y2)2，如果其值与d2 2之差的绝对值小于0.001，则认为此点为巡检机器人的坐标(xr,yr)。取得两点的坐标后，根据巡检机器人与两个匹配成功特征之间的位置相对关系，确认一点为巡检机器人的坐标。
本发明不用在变电站进行施工，对变电站现有的道路也没有苛刻的条件，碎石路和坡度不大的草地上也可以正常运行，还可以临时、灵活的调整巡检路线。
最后应说明的是：虽然以上已经详细说明了本发明及其优点，但是应当理解在不超出由所附的权利要求所限定的本发明的精神和范围的情况下可以进行各种改变、替代和变换。
而且，本发明的范围不仅限于说明书所描述的过程、设备、手段、方法和步骤的具体实施例。
本领域内的普通技术人员从本发明的公开内容将容易理解，根据本发明可以使用执行与在此所述的相应实施例基本相同的功能或者获得与其基本相同的结果的、现有和将来要被开发的过程、设备、手段、方法或者步骤。
因此，所附的权利要求旨在在它们的范围内包括这样的过程、设备、手段、方法或者步骤。
Claims (3)
1.一种基于激光扫描仪的自主导航方法，其特征在于：包括以下步骤：
A、建立特征地图和巡检线路：通过人工的方式牵引巡检机器人完成对巡检路线的一次巡视，依次巡视形成以下模块：位置测量模块→位置估计模块→几何特征抽象模块→全局特征地图构建模块；
B、巡检：
巡检机器人自动按照以下步骤进行巡检，直至结束：位置测量模块→几何特征抽象模块→特征匹配模块→位置估计模块→机器人位置调整模块；
所述建立特征地图和巡检线路中所述位置测量模块的建立：首先根据里程计得出巡检机器人在T到T+1时刻沿行进方向运动的距离；然后根据陀螺仪测得行进方向和水平X轴的夹角；根据T时刻的全局坐标(XT,YT)计算得到T+1时刻的全局坐标(XT+1,YT+1)；
所述建立特征地图和巡检线路中的所述位置估计模块的建立：
运用卡尔曼滤波建立巡检机器人的运动模型和观测模型；
X(K)＝A*X(K-1)+B*U(K)+W(K) (1)
Z(K)＝H*X(K)+V(K) (2)
X(K)为K时刻的系统状态，U(K)为K时刻对系统的控制量，A和B为系统参数；Z(K)为K时刻的测量值，H为测量系数；W(K)和V(K) 分别表示系统和测量过程中的噪声，噪声满足高斯白噪声模型，设W(K)和V(K)的协方差分别为Q和R；
基于卡尔曼滤波的5个经典核心公式：
X(k|k-1)＝A*X(k-1|k-1)+B*U(k) (3)
其中X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)为上一时刻的最优估计值，U(k)为现在状态的控制量，在模型中，控制量U(k)为0；
经过公式(3)之后，系统结果已经更新了，接下来计算X(k|k-1)的协方差，用P来表示协方差，则为：
P(k|k-1)＝A*P(k-1|k-1)*AT+Q (4)
其中P(k|k-1)为X(k|k-1)对应的协方差，P(k-1|k-1)为X(k-1|k-1)对应的协方差，AT为A的转置矩阵，Q系统噪声协方差；
X(k|k)＝X(k|k-1)+Kg(k)*(Z(k)–H*X(k|k-1)) (5)
Kg(k)＝P(k|k-1)*HT/(H*P(k|k-1)*HT+R) (6)
P(k|k)＝(1-Kg( k)*H)*P(k|k-1) (7)；
所述建立特征地图和巡检线路中所述的几何特征抽象模块的建立：对于激光扫描仪采集的数据点，将其抽象出几何特征，以便在巡检过程中进行位置匹配；抽象出的几何特征为直线段和圆弧，使用的方法为最小二乘法拟合；对于拟合出的直线段，取线段的中心点(x1，y1)为质心，直线段则标识为(x1，y1,L,θl)，L为线段的长度，θl为线段与x轴的夹角；对于拟合出的圆弧，我们取其圆心(xr,yr)为质心，圆弧则标识为(xr,yr,R)，R为圆弧半径；
所述建立特征地图和巡检线路中所述的全局特征地图构建模块的建立：对应每一个拟合出的直线段和圆弧，加上位置测量模块中里程计给出的信息，确定其全局坐标系中的坐标(x,y)，其中每个特征都有全局坐标系中的坐标，和其先后的顺序，以及每一个时刻机器人的位置信息集P((x1,y1),(x2,y2)……(xn,yn))；
所述巡检中所述的位置测量模块：根据里程计得出巡检机器人在T到T+1时刻沿行进方向运动的距离；根据陀螺仪测得行进方向和水平X轴的夹角；根据T时刻的全局坐标(XT,YT)计算得到T+1时刻的全局坐标(XT+1,YT+1)；
所述巡检中的所述几何特征抽象模块：对于激光扫描仪采集的数据点，使用最小二乘法进行拟合，得到直线段和圆弧；此时：对于拟合出的直线段，得到其(L,θl)，其中L为线段的长度，θl为线段与x轴的夹角；对于拟合出的圆弧，得到其R，R为圆弧半径；
所述巡检中的特征匹配模块：对于上一模块中抽象出来的直线段或者圆弧，参考位置测量模块中里程计给出的位置信息以及匹配的先后顺序，将抽象出来的几何特征在全局特征地图中进行匹配，以确认该几何特征对应的全局地图中的特征；选取匹配成功的两个特征，获取其全局位置坐标信息，并且计算质心与巡检机器人的距离d，得到(x1,y1,d1)和(x2,y2,d2)；
所述巡检中的位置估计模块：根据上一模块特征匹配得到的两个匹配成功特征的信息(x1,y1,d1)和(x2,y2,d2)，运用平面三角定位的方法，计算当时时刻巡检机器人的坐标(xr,yr)，在取得巡检机器人的坐标之后，运用特征地图和巡检线路的建立中相同的卡尔曼滤波方法计算得到巡检机器人的位置最佳估计值(x'r,y'r)。
2.根据权利要求1所述的基于激光扫描仪的自主导航方法，其特征在于：所述巡检中所述机器人位置调整模块：根据上一模块得到的位置最佳估计值(x'r,y'r)，在位置信息集P中查找对应的坐标(xs,ys)，调整机器人PID控制部分，实现机器人从(x'r,y'r)--→(xs,ys)的移动。
3.根据权利要求1所述的基于激光扫描仪的自主导航方法，其特征在于：所述平面三角定位的算法为：
以(x1,y1)为圆心，d1为半径做圆C1；(x2,y2)为圆心，d2为半径做圆C2；
两圆相交的两点之一即为当时时刻巡检机器人的坐标(xr,yr)；
圆C1C2的方程分别为：
(x-x1)2+(y-y1)2＝d1 2
(x-x2)2+(y-y2)2＝d2 2
在圆C1上顺序取点(x,y)，代入计算(x-x2)2+(y-y2)2，如果其值与d2 2之差的绝对值小于0.001，则认为此点为巡检机器人的坐标(xr,yr)；取得两点的坐标后，根据巡检机器人与两个匹配成功特征之间的位置相对关系，确认一点为巡检机器人的坐标。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510668566.6A CN105137998B (zh) | 2015-10-13 | 2015-10-13 | 一种基于激光扫描仪的自主导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510668566.6A CN105137998B (zh) | 2015-10-13 | 2015-10-13 | 一种基于激光扫描仪的自主导航方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105137998A CN105137998A (zh) | 2015-12-09 |
| CN105137998B true CN105137998B (zh) | 2017-12-08 |
Family
ID=54723373
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510668566.6A Active CN105137998B (zh) | 2015-10-13 | 2015-10-13 | 一种基于激光扫描仪的自主导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105137998B (zh) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108801268B (zh) * | 2018-06-27 | 2021-03-05 | 广州视源电子科技股份有限公司 | 目标对象的定位方法、装置及机器人 |
| CN109343517A (zh) * | 2018-08-20 | 2019-02-15 | 杭州合时机器人科技有限公司 | 巡检机器人定点巡查方法 |
| CN109141402B (zh) * | 2018-09-26 | 2021-02-02 | 亿嘉和科技股份有限公司 | 一种基于激光栅格的定位方法以及机器人自主充电方法 |
| CN109375628A (zh) * | 2018-11-28 | 2019-02-22 | 南京工程学院 | 一种采用激光定向和射频定位的变电站巡检机器人导航方法 |
| CN110160528B (zh) * | 2019-05-30 | 2021-06-11 | 华中科技大学 | 一种基于角度特征识别的移动装置位姿定位方法 |
| CN113075686B (zh) * | 2021-03-19 | 2024-01-12 | 长沙理工大学 | 一种基于多传感器融合的电缆沟智能巡检机器人建图方法 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH10260724A (ja) * | 1997-03-19 | 1998-09-29 | Yaskawa Electric Corp | 通路環境の地図生成方法 |
| CN102402225A (zh) * | 2011-11-23 | 2012-04-04 | 中国科学院自动化研究所 | 一种实现移动机器人同时定位与地图构建的方法 |
| CN103674015A (zh) * | 2013-12-13 | 2014-03-26 | 国家电网公司 | 一种无轨化定位导航方法及装置 |
| CN104914865A (zh) * | 2015-05-29 | 2015-09-16 | 国网山东省电力公司电力科学研究院 | 变电站巡检机器人定位导航系统及方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4788722B2 (ja) * | 2008-02-26 | 2011-10-05 | トヨタ自動車株式会社 | 自律移動ロボット、自己位置推定方法、環境地図の生成方法、環境地図の生成装置、及び環境地図のデータ構造 |
-
2015
- 2015-10-13 CN CN201510668566.6A patent/CN105137998B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH10260724A (ja) * | 1997-03-19 | 1998-09-29 | Yaskawa Electric Corp | 通路環境の地図生成方法 |
| CN102402225A (zh) * | 2011-11-23 | 2012-04-04 | 中国科学院自动化研究所 | 一种实现移动机器人同时定位与地图构建的方法 |
| CN103674015A (zh) * | 2013-12-13 | 2014-03-26 | 国家电网公司 | 一种无轨化定位导航方法及装置 |
| CN104914865A (zh) * | 2015-05-29 | 2015-09-16 | 国网山东省电力公司电力科学研究院 | 变电站巡检机器人定位导航系统及方法 |
Non-Patent Citations (3)
| Title |
|---|
| 全区域覆盖移动机器人同时定位于地图创建(SLAM)技术的研究;王磊;《万方学位论文数据库》;20060425;全文 * |
| 基于地图匹配的变电站巡检机器人激光导航系统设计;李红梅 等;《制造业自动化》;20140131;第36卷(第1期);全文 * |
| 基于激光定位的变电站智能巡检机器人导航系统设计;肖鹏 等;《计算机测量与控制》;20121231;第20卷(第6期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105137998A (zh) | 2015-12-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105137998B (zh) | 一种基于激光扫描仪的自主导航方法 | |
| CN108303099B (zh) | 基于三维视觉slam的无人机室内自主导航方法 | |
| CN108571971B (zh) | 一种agv视觉定位系统及方法 | |
| KR102367438B1 (ko) | 마커를 결합한 동시 위치결정과 지도작성 내비게이션 방법, 장치 및 시스템 | |
| CN106607907B (zh) | 一种移动视觉机器人及其测控方法 | |
| CN104914865B (zh) | 变电站巡检机器人定位导航系统及方法 | |
| Madhavan et al. | Distributed cooperative outdoor multirobot localization and mapping | |
| KR101202695B1 (ko) | 자율 이동 장치 | |
| CN104714555B (zh) | 一种基于边缘的三维自主探索方法 | |
| US8392036B2 (en) | Point and go navigation system and method | |
| CN107491070A (zh) | 一种移动机器人路径规划方法及装置 | |
| CN107478214A (zh) | 一种基于多传感器融合的室内定位方法及系统 | |
| CN109541535A (zh) | 一种基于uwb和视觉slam的agv室内定位及导航的方法 | |
| CN104679004B (zh) | 柔性路径与固定路径相结合的自动导引车及其导引方法 | |
| CN109059942A (zh) | 一种井下高精度导航地图构建系统及构建方法 | |
| CN109828607A (zh) | 一种面向不规则障碍物的无人机路径规划方法及系统 | |
| CN111044073B (zh) | 基于双目激光高精度agv位置感知方法 | |
| CN107167139A (zh) | 一种变电站巡检机器人视觉定位导航方法及系统 | |
| Rekleitis | Cooperative localization and multi-robot exploration | |
| CN109900273B (zh) | 一种室外移动机器人导引方法及导引系统 | |
| CN110398252A (zh) | 路径搜索装置、驾驶控制装置以及自动行驶装置 | |
| WO2005038402A1 (ja) | ナビゲーション装置 | |
| CN105444766B (zh) | 基于深度学习的室内导航方法 | |
| CN110018688B (zh) | 基于视觉的自动引导车定位方法 | |
| CN107037814B (zh) | 无轨观览车的空间定位系统及其定位方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20191010 Address after: 214000 room 3111, No. 1699, Huishan Avenue, Huishan Economic Development Zone, Wuxi City, Jiangsu Province Patentee after: Wuxi Kanglai Medical Technology Co., Ltd. Address before: 431400 Hubei Province, Wuhan city Yangluo street Xinzhou District No. 84, building 2, Pan Long Lu 1 unit 1 floor No. 2 Patentee before: Xu Jin Ping |
|
| TR01 | Transfer of patent right |