一种巡检机器人的组合定位方法

技术领域

本发明涉及巡检机器人领域，尤其涉及一种巡检机器人的组合定位方法。

背景技术

变电站场景巡检机器人在执行巡检工作时，需要实时获取自身的位姿等导航信息。因变电站场景特征纹理丰富的特点，通常会采用激光雷达作为主要的定位手段。然而，变电站场景会存在极端的如长走廊等特征较少或相似的情景，加上雨雪等恶劣天气，此时激光雷达定位性能会大打折扣甚至无法定位。针对这一问题，采用特性互补的传感器如惯性测量单元(inertial measurement unit,imu)、轮速里程计、全球导航卫星系统(GlobalNavigation Satellite System,GNSS)进行融合的手段来提升系统的定位稳定性。

全球导航卫星系统GNSS主要有中国北斗、美国GPS、俄罗斯GLONASS和欧盟Galileo，其定位原理利用一组卫星的伪距、卫星星历、卫星发射时间以及钟差来实现接收机自身的位置解算。理论上说，一般至少需要4颗卫星的测量信息即可获得三维空间的位置坐标。

随着卫星定位技术的发展，巡检机器人对高精度卫星定位技术的需求愈发强烈。单点GNSS成本低廉、使用简单，但是为米级的定位精度；载波相位差分技术(RTK)定位精度能达到厘米级，然而成本通常较高，并且需要额外部署基站以及人力参与定期维护。分米级卫星定位技术RAC是最近几年出现的并在自动驾驶、服务机器人等众多领域已经广泛应用的一种新型传感器技术，其定位精度分米级。RAC技术成本介于单点GNSS技术和RTK技术之间，除了无需部署基站的优点外，在多路径场景下相比RTK技术更具定位稳定性。

发明内容

发明目的：本发明针对上述不足，引用专利(申请号201610822102.0)的分米级卫星定位RAC技术，提出了一种基于IMU/RAC/轮速里程计的低成本、分米级定位精度的变电站巡检机器人组合定位方法。

技术方案：

一种巡检机器人的组合定位方法，包括步骤：

(1)在巡检机器人上安装惯性传感器及RAC传感器，在巡检机器人底盘上安装轮速计；

(2)巡检机器人以恒定速度运动，并实时获取惯性传感器、RAC传感器及轮速计的数据；

(3)分别将轮速计采集得到的机器人运动速度、RAC传感器得到的机器人位置信息作为误差观测量，并构建基于误差状态量的卡尔曼滤波器，最终实现巡检机器人的组合定位。

所述步骤(1)中，在巡检机器人上设置有RTK移动站，配置一RTK基站通过调频信号与RTK移动站进行通信，通过RTK差分定位对RAC传感器的定位数据进行验证。

所述步骤(2)中，巡检机器人在空旷场景下按照1m/s速度直线移动。

所述步骤(3)中，构建基于误差状态量的卡尔曼滤波器以惯性传感器的误差传播过程作为卡尔曼滤波器的预测值，根据误差传播方程求解关于误差状态量δx的状态转移雅可比矩阵，根据RAC、轮速计的观测方程求解观测雅可比矩阵。

所述步骤(3)具体为：

(31)假设t时刻巡检机器人的真实状态为x

其中，p

带有误差量的状态为x：

其中，p表示巡检机器人位置带有误差量的状态，v表示巡检机器人速度带有误差量的状态，q表示巡检机器人旋转变换带有误差量的状态，a

误差状态为δx：

其中，δp表示巡检机器人位置的误差状态，δv表示巡检机器人速度的误差状态，δθ表示巡检机器人旋转变换的误差状态，δa

(32)那么三者之间的关系为：

p

v

a

ω

(33)则带有误差量的状态更新如下：

v←v+(R(a

a

ω

其中，Δt表示更新前后两个时刻的状态之间的时间间隔，R表示更新前后两个时刻的状态之间的旋转矩阵，a

(34)根据误差传播方程得到滤波器预测的误差状态如下：

δp←δp+δvΔt

δv←δv+(-R[a

δθ←R

δa

δω

即为关于误差状态量δx的状态转移雅可比矩阵；

其中[a

(35)根据轮速计得到机器人速度的观测方程：

δv

其中，δv

其中，I

(36)根据RAC传感器得到的机器人位置观测方程：

通过RAC传感器得到机器人的位置观测值为p

p

则得到关于机器人位置的误差状态的雅可比矩阵：

其中，h表示上述RAC传感器得到的机器人位置观测方程。

有益效果：本发明的巡检机器人组合定位方法采用了低成本、分米级定位精度用来解决激光雷达在一些极端工况下定位性能降低的问题，进一步提升了巡检机器人系统的定位稳定性。

附图说明

图1为本发明的巡检机器人组合定位软硬件架构设计图。

图2为在空旷场景中RAC与RTK的位置轨迹对比图；其中，矩形标记线为RAC的位置轨迹，圆形标记线为RTK的位置轨迹。

图3为基于ESKF(误差状态卡尔曼滤波)的组合定位滤波器原理图。

图4为在复杂场景中RAC、RTK以及二者组合定位结果对比图；其中，矩形标记线为单RAC的定位结果，圆形标记线为单RTK的定位结果，正常标记线为二者组合定位结果。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明。

图1为本发明的巡检机器人组合定位软硬件架构设计图。如图1所示，本发明的变电站巡检机器人的组合定位方法包括如下步骤：

步骤一、在巡检机器人上安装惯性传感器及RAC传感器，在巡检机器人底盘上安装轮速计，在巡检机器人上设置有RTK移动站，配置一RTK基站通过调频信号与RTK移动站进行通信，通过RTK移动站完成RTK差分定位；

其中，惯性传感器选择市面上已经完成转台标定和温度补偿的成熟型惯性传感器；且，可以根据不同精度要求，在机器人上安装不同精度级别的惯性传感器；

步骤二、在巡检机器人巡检时，通过RTK移动站和RAC传感器获取巡检机器人的定位数据，并同步绘制定位轨迹图，如图2所示，其中，矩形标记线为RAC的位置轨迹，圆形标记线为RTK的位置轨迹；

在本发明中，通过RTK差分定位对RAC传感器的定位数据进行验证。

步骤三、运动组合对准：通过滤波方法并结合外部观测自动进行航向收敛，以此达到惯性传感器坐标系与RAC坐标系之间对齐的目的，在本发明中，滤波方法采用卡尔曼(kalman)滤波，外部观测采用的惯性传感器、RAC传感器以及轮速计，具体如下：

(1)巡检机器人以恒定速度移动；一般情况下，是在空旷场景下巡检机器人按照1m/s速度直线移动；

(2)将惯性传感器采集得到的包含零偏和噪声的角速度和加速度数据进行积分结算，并用四元数表达，得到惯性传感器坐标系与RAC坐标系之间的旋转变换变换关系；

但是，积分解算会导致随着时间位姿误差的累积的问题，因此需要采用无误差累积的绝对定位手段进行误差观测；此处分别使用轮速计采集得到的速度、RAC传感器的位置信息作为误差观测量，如图3所示；

(3)假设t时刻巡检机器人的真实状态为x

其中，p

带有误差量的状态为x：

其中，p表示巡检机器人位置带有误差量的状态，v表示巡检机器人速度带有误差量的状态，q表示巡检机器人旋转变换带有误差量的状态，a

则误差状态为δx：

其中，δp表示巡检机器人位置的误差状态，δv表示巡检机器人速度的误差状态，δθ表示巡检机器人旋转变换的误差状态，δa

(4)那么三者之间的关系为：

p

v

a

ω

(5)构建基于误差状态量δx的kalman滤波器，以惯性传感器的误差传播过程作为卡尔曼滤波器的预测值，根据误差传播方程求解关于误差状态量δx的状态转移雅可比矩阵，根据RAC、轮速计的观测方程求解观测雅可比矩阵，此处的输入分别为角速度ω

(51)惯性传感器的噪声和扰动在带有误差量的状态x的积分解算过程中是不考虑的，则带有误差量的状态更新如下：

v←v+(R(a

a

ω

其中，Δt表示更新前后两个时刻的状态之间的时间间隔，R表示更新前后两个时刻的状态之间的旋转矩阵，a

(52)根据误差传播方程，即滤波器预测的误差状态如下：

δp←δp+δvΔt

δv←δv+(-R[a

δθ←R

δa

δω

即为关于误差状态量δx的状态转移雅可比矩阵；

其中[a

(53)根据RAC、轮速计的观测方程求解观测雅可比矩阵：

①根据轮速计得到机器人速度的观测方程：

假设巡检机器人的侧向和天向没有运动，因此

则根据轮速计得到机器人运动速度的观测方程为：

δv

其中，δv

其中，I

②根据RAC传感器得到的机器人位置观测方程：

通过RAC传感器得到机器人的位置观测值为p

p

则得到关于机器人位置的误差状态的雅可比矩阵：

其中，h表示上述RAC传感器得到的机器人位置观测方程。

步骤四、组合定位方法的效果评估：

如图4所示，右上角代表机器人处于多路径场景中，用该融合算法解算出的位姿(正常标记线)分别与单独RAC(矩形标记线)、单独RTK(圆形标记线)做对比，从定位结果来看，在多路径场景下单独RTK或者RAC均发生了一定程度上的定位漂移，而本发明的组合定位方法在保证精度的同时，也提升了整体系统的定位稳定性。

以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护范围。