基于北斗导航系统的智能巡检机器人定位方法与流程

1.本发明属于变电站智能巡检技术领域，具体涉及一种基于北斗导航系 统的智能巡检机器人定位方法。


背景技术：

2.近年来，随着我国具有完全自主知识产权的北斗导航系统的广泛应用 和人工智能技术的迅猛发展，智能巡检机器人凭借其强大而可靠的定位导 航和智能识别技术，逐渐取代人力成为变电站巡检的主要方式，在此过程 中，越来越多的变电站采用了基于北斗导航系统的智能巡检机器人定位方 法。
3.基于北斗导航系统的智能巡检机器人定位方法的具体实施方式甚多， 但一般说来，均包括以下六个基本步骤：
4.步骤一，确定参考基准站，通常是在变电站内选取一个具有精密坐标 值的固定点作为参考基准站；
5.步骤二，通过安装在参考基准站的北斗接收单元计算出参考基准站的 测量坐标值，再由参考基准站精密坐标值和测量坐标值，计算出差分校正 量；
6.步骤三，根据北斗卫星信号计算出巡检机器人的定位位置，并由参考 基准站传送过来的差分校正量校正巡检机器人的定位位置，从而得到巡检 机器人实时定位位置；
7.步骤四，建立工作坐标系，并确定工作坐标系原点，通常以巡检机器 人充电站坐标位置作为其工作坐标系原点；
8.步骤五，计算出巡检机器人的实时定位位置以及参考基准站定位位置 到工作坐标系的转换矩阵；
9.步骤六，将巡检路径坐标信息通过转换矩阵转换成巡检机器人工作坐 标系，计算出最优巡检路径并进行巡检导航。
10.上述六个基本步骤的实施效果依赖于相应的算法。目前，智能巡检机 器人的主流定位方案为激光雷达扫描(lidar-scan)，亦即通过当前帧点云 数据与地图数据配准来解算机器人实时位姿(位置与姿态)，相应的算法 可统称为激光配准算法，主要包括迭代最近邻点(iterative closest point， icp)算法及其变种算法，如点对线迭代最近邻点算法(point-to-lineicp)， 点对面迭代最近邻点算法(point-to-plane icp)，正态分布变换算法(normaldistribution transformation，ndt)，以及基于特征匹配算法等。除此之外， 还有很多基于深度学习来改善匹配速度与稳定性的算法，如蒙特卡洛定位 算法(monte carlo localization，mcl)凭借稳定性与重定位成为现最为流 行的定位方案。
11.上述算法可以在周围环境特征丰富且智能巡检机器人行使平稳的情 况下给出最优位姿估计，但是，上述算法也存在一些缺陷，例如，智能巡 检机器人行进过程中因转弯角度大、行驶速度快、移动物体与透明物体干 扰而导致的误匹配等情况，又如，在开阔环境下，周围环境特征较少，也 会直接导致智能巡检机器人的估计位姿错误。


技术实现要素：

12.本发明的目的旨在克服上述现有技术的缺陷，提高智能巡检机器人在 空旷环境、复杂环境下的定位精度。该发明目的是通过下述技术方案实现 的：
13.一种基于北斗导航系统的智能巡检机器人定位方法，其使用的定位系 统包括集成安装在智能巡检机器人的机体内或机体上的中央处理器、数 据转换模块、数据库、时钟模块、网关模块、信息显示模块、有线互联 网模块、北斗模块、编码器、惯性测量单元，还包括设置在智能巡检机 器人云台上的激光摄像仪；
14.北斗模块、编码器、惯性测量单元通过数据转换模块与中央处理器 通信连接，有线互联网模块、信息显示模块通过网关模块与中央处理器 通信连接，数据库、时钟模块、激光摄像仪直接与中央处理器通信连接；
15.所述定位方法包括确定参考基准站、计算差分校正量、校正巡检机器 人的定位位置、建立工作坐标系并确定工作坐标系原点、计算巡检机器人 的实时定位位置以及参考基准站定位位置到工作坐标系的转换矩阵、计算 最优巡检路径并进行巡检导航六个基本步骤，在此过程中，由中央处理 器对北斗模块、编码器和惯性测量单元传输的数据进行扩展卡尔曼滤波 融合，同时结合蒙特卡罗定位算法，将融合位姿作为粒子参照进行采样。
16.在上述技术方案的基础上，本发明可附加下述技术手段，以便更好 地实现本发明的目的：
17.所述编码器光电传感器，且安装在智能巡检机器人的轮子上。
18.进一步地，所述中央处理器对北斗模块、编码器和惯性测量单元传 输的数据进行扩展卡尔曼滤波融合时，使用编码器传输的数据作为观测 量，使用惯性测量单元传输的数据作为测量量，在编码器传输的数据形 成的机器人坐标系中进行第一次扩展卡尔曼滤波融合。
19.进一步地，采用北斗定位数据作为观测量，以第一次扩展卡尔曼滤 波融合数据为测量量在全局坐标系中进行第二次扩展卡尔曼滤波融合， 由此获得智能巡检机器人在全局坐标系中的融合位姿。
20.进一步地，以北斗定位数据为观测量的扩展卡尔曼滤波融合位姿数 据可通过以下步骤，融入到蒙特卡罗定位算法中：
21.步骤一，根据所有样本粒子与融合位姿的欧式距离来调整所有样本 的重要性参数；
22.步骤二，初始采样以及重采样过程中，基于融合后位姿的高斯分布 抽取样本；
23.步骤三，将融合位姿作为样本子集替代样本集中重要性偏低的粒 子；
24.步骤四，根据粒子匹配度来调整融合策略，即位姿匹配度低时，则 表明附近的参照物较少，属于空旷区域，在这种情况下，可以使用扩展 卡尔曼滤波融合智能巡检机器人位姿的增量信息来进行航迹推算，直到 粒子滤波估计位姿重新获取较高匹配度后，再使用蒙特卡罗定位算法。
25.本发明的主要有益效果如下：
26.本发明利用北斗模块、编码器以及惯性测量单元等传感器，基于扩展 卡尔曼滤波融合算法，并结合蒙特卡罗定位算法，可以实时给出智能巡检 机器人无累计误差、高可靠性与稳定性的位姿解，不仅使智能巡检机器人 在空旷环境下能够完成精确的自主导航，而
且确保智能巡检机器人在一些 遮挡环境下的定位精度要求，从而满足复杂环境下的导航需求。
附图说明
27.图1为本发明的一个实施例中的定位系统的结构示意图；
28.图2为本发明的一个实施例的整体算法框架示意图；
29.图3为本发明的一个实施例中的北斗定位差分流程图。
30.图中：cpu——中央处理器；date——数据转换模块； odom——编码器；imu——惯性测量单元；bds——北斗模块； laca——激光摄像仪；time——时针模块；dbse——数据库； gate——网关模块；info——信息显示模块；inte——有线互联网模块
具体实施方式
31.以下结合附图介绍本发明的一个实施例。
32.如图1所示，一种基于北斗导航系统的智能巡检机器人定位方法，该 方法所使用的定位系统包括集成安装在智能巡检机器人的机体内或机体 上的中央处理器、数据转换模块、数据库、时钟模块、网关模块、信息 显示模块、有线互联网模块、北斗模块、编码器、惯性测量单元，还包 括设置在智能巡检机器人云台上的激光摄像仪；
33.所述数据转换模块、数据库、时钟模块、有线互联网模块、北斗模 块、编码器、惯性测量单元、激光摄像仪分别与中央处理器通信连接； 网关模块、信息显示模块、中央处理器两两之间通信连接；
34.所述定位方法包括确定参考基准站、计算差分校正量、校正巡检机器 人的定位位置、建立工作坐标系并确定工作坐标系原点、计算巡检机器人 的实时定位位置以及参考基准站定位位置到工作坐标系的转换矩阵、计算 最优巡检路径并进行巡检导航六个基本步骤，在此过程中，由中央处理 器对北斗模块、编码器和惯性测量单元传输的数据进行扩展卡尔曼滤波 (extend kalman filte，ekf)融合，同时结合蒙特卡罗(monte carlo， mcl)定位算法，将融合位姿作为粒子参照进行采样。
35.如图2并结合图1所示，所述中央处理器对北斗模块、编码器和惯 性测量单元传输的数据进行扩展卡尔曼滤波融合时，使用编码器传输的 数据作为观测量，使用惯性测量单元传输的数据作为测量量，在编码器 传输的数据形成的机器人坐标系中进行第一次扩展卡尔曼滤波融合(其 目的是保证融合数据的连续性)，在此基础上，采用北斗定位数据作为 观测量，以第一次扩展卡尔曼滤波融合数据为测量量在全局坐标系中进 行第二次扩展卡尔曼滤波融合，由此获得智能巡检机器人在全局坐标系 中的融合位姿。
36.如图3所示，为了保证北斗数据精度要求，本实施例采用差分定位 的方式来提高北斗定位精度。由北斗位置服务平台，即差分数据中心来 提供差分数据，其数据通过http来传输，可以通过无线数据传送终端 等设备接入服务，其协议遵循ntrip协议，差分数据通讯格式为 rtcm3.0。解算的位置信息以报文的形式通过串口获取，其服从nmea 报文标准格式。由于北斗模块不仅提供位置信息，还提供了航向信息， 故需要分别读取定位报文与航向报文。其解算的定位数据为：
37.(x
bds y
bds ρ
bds
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
38.式(1)中，(x
bds y
bds
)为位置，ρ
bds
为航向，由于在本地导航策略 中，无需考虑高程信息，且定位的海拔信息误差较大，故本实施例过滤 掉了海拔信息，其中定位数据服从正态分布。
39.惯性测量单元(imu)能够提供智能巡检机器人精确的瞬态信息， 分为三轴加速度信息和三轴角速度信息，并且，惯性测量单元属于自基 准测量单元，除了自身测量信息，无其他观测来提供冗余约束，因此其 通过积分来获取位置与旋转量信息会因为时间尺度的原因，其累计误差 越来越大，导致数据不可用，因此一般采用其瞬态量，或者增量来进行 融合。因为融合算法基于机器人坐标系下，因此需要排除重力加速度的 测量信息，除此之外，还需对其进行标定，假设数据服从零均值正态分 布。
40.为了使编码器获得和传输的数据更加精准，在本实施例中，编码器 光电传感器，且安装在智能巡检机器人的轮子上，其作用是根据智能巡 检机器人动力学模型来推算智能巡检机器人位置、航迹。编码器获得和 传输的数据，其静态误差远小于动态误差，其原因在于，位置数据信息 受智能巡检机器人的运行速度的影响较大，为了反映速度的影响，其数 据方差由速度相关的因子来进行调控，基本策略为方差与速度正相关：
[0041][0042]
式(2)中，σ
init
为编码器的初始方差，v
odom
为机器人速度，h(g) 为关于速度的线性方程。
[0043]
以上，结合附图介绍了本发明的一个实施例所使用的设备及其 工作原理和基本方法，以下，进一步介绍本实施例实施过程中的具 体算法、结合方式及效果验证。
[0044]
一、扩展卡尔曼滤波(ekf)算法
[0045]
在估计理论中，扩展卡尔曼滤波是基于隐马尔可夫模型，对置信度 和噪声进行高斯假设，并对运动模型和观测模型进行线性化后，推导出 的递归滤波器估计算法，针对一些轻度非线性非高斯系统来说扩展卡尔 曼能够给出较好的结果。其非线性运动和观测模型如下：
[0046]
xk＝f(x
k-1
，vk)+wkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0047]
yk＝g(xk)+nkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0048]
xk为状态量，假设xk的置信度函数为高斯分布：
[0049][0050]
yk为观测量，vk为控制量，f(g)与g(g)都是非线性函数，
[0051]
假设：wk和nk都是白噪声，
[0052]
f(g)，g(g)可微。
[0053]
由于f(g)和g(g)的非线性特性，无法计算得到闭式解，因此需要针 对运动和观测模型进行线性化，将f(g)与g(g)进行一阶泰勒展开：
[0054][0055]
式(4)中：式(4)中：
[0056]
观测方程泰勒展开：
[0057][0058]
式(5)中：
[0059]
扩展卡尔曼滤波算法大体可以分为预测与更新两步。
[0060]
(一)预测
[0061][0062][0063]
(二)更新
[0064][0065][0066][0067]
上述算式中：为更新量；
[0068]kk
被称为卡尔曼增益矩阵。
[0069]
预测是根据运动方程来更新均值与方差，更新则是根据观测信息来 修正均值与方差，由此来估计智能巡检机器人准确的位姿数据。
[0070]
扩展卡尔曼滤波经典递归更新方程，通过更新方程，本实施例可以 由计算出
[0071]
二、蒙特卡罗(mcl)定位算法
[0072]
蒙特卡罗定位算法又名粒子滤波算法，是一种非线性递归贝叶斯滤 波算法，其通过大数定理统计的角度来求解位姿后验概率。蒙特卡罗定 位算法基于给定的先验地图信息与感知状态量，根据状态量采样一组具 有权重的随即粒子来表示位姿估计的后验概率，并根据这些粒子与权值 来计算修正当前状态量。其优势是没有模型与高斯假设的限制，其表达 式为：
[0073]
bel(x)≈{x(i),w(i)}
i＝1,...,n
ꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0074]
式(1)中，bel(x)为随机状态量x的粒子样本，状态量包括位置量 与旋转量，每个样本粒子的权重可以通过非负参数w(i)来决定，其也称 为粒子的重要性因子，它代表了智能巡检机器人的估计位姿在此样本处 的概率。基本的蒙特卡罗递归更新通过以下步骤实现：
[0075]
(一)采样(sampling)
[0076]
基于上一时刻的贝叶斯后验估计和噪声的联合概率密度中来采样 粒子：
[0077][0078]
式(2)中：m为粒子序号。
[0079]
(二)预测(predict)
[0080]
将每个先验粒子和噪声样本代入非线性运动模型：
[0081][0082]
上述预测粒子共同近似刻画了反映智能巡检机器人位姿的概率密 度
[0083]
(三)对后验概率进行矫正
[0084]
对后验概率进行矫正，主要分为两步：
[0085]
第一步：根据每个粒子的期望后验和预测后验的收敛程度，对每个 粒子赋予权值w
k,m
：
[0086][0087]
式(4)中η为归一化系数。
[0088]
第二步：根据赋予的权重，对每个粒子进行重要性重采样：
[0089][0090]
三、扩展卡尔曼滤波融合及其与蒙特卡罗定位的结合方式
[0091]
在本实施例中，以北斗定位数据为观测量的扩展卡尔曼滤波融合位 姿数据可通过以下步骤，融入到蒙特卡罗定位算法中：
[0092]
步骤一，根据所有样本粒子与融合位姿的欧式距离来调整所有样本 的重要性参数；
[0093]
步骤二，初始采样以及重采样过程中，基于融合后位姿的高斯分布 抽取样本；
[0094]
步骤三，将融合位姿作为样本子集替代样本集中重要性偏低的粒 子；
[0095]
步骤四，根据粒子匹配度来调整融合策略，即位姿匹配度低时，则 表明附近的参照物较少，属于空旷区域，在这种情况下，可以使用扩展 卡尔曼滤波融合智能巡检机器人位姿的增量信息来进行航迹推算，直到 粒子滤波估计位姿重新获取较高匹配度后，再使用蒙特卡罗定位算法。
[0096]
四、效果验证
[0097]
为了验证本实施例的技术效果，本发明研发团队分别在结构化环境 (周围环境特征丰富的区域)、空旷区域和遮挡区域进行了10次实验， 所选用的智能巡检机器人为科大智能股份有限公司研发的、搭载北斗模 块的科大智能巡检机器人，在每个区域所获得的10组实验数据都取均 值，得到不同环境下的导航定位精度测试结果如表1所示。
[0098]
表1不同环境下的导航定位精度测试结果
[0099][0100]
由表1数据可知，本发明利用北斗模块、编码器以及惯性测量单 元等传感器，基于扩展卡尔曼滤波融合算法，并结合蒙特卡罗定位算法， 可以实时给出无累计误差、高可靠性与稳定性的位姿解，不仅使智能巡检 机器人在空旷环境下能够完成精确的自主导航，而且确保智能巡检机器人 在一些遮挡环境下的定位精度要求，从而满足复杂环境下的导航需求。