[发明专利]一种巡检机器人的组合定位方法

权利要求书

1.一种巡检机器人的组合定位方法，其特征在于：包括步骤：

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

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

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

2.根据权利要求1所述的巡检机器人的组合定位方法，其特征在于：所述步骤(1)中，在巡检机器人上设置有RTK移动站，配置一RTK基站通过调频信号与RTK移动站进行通信，通过RTK差分定位对RAC传感器的定位数据进行验证。

3.根据权利要求1所述的巡检机器人的组合定位方法，其特征在于：所述步骤(2)中，巡检机器人在空旷场景下按照1m/s速度直线移动。

4.根据权利要求1所述的巡检机器人的组合定位方法，其特征在于：所述步骤(3)中，构建基于误差状态量的卡尔曼滤波器以惯性传感器的误差传播过程作为卡尔曼滤波器的预测值，根据误差传播方程求解关于误差状态量δx的状态转移雅可比矩阵，根据RAC、轮速计的观测方程求解观测雅可比矩阵。

5.根据权利要求4所述的巡检机器人的组合定位方法，其特征在于：所述步骤(3)具体为：

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

其中，p_t表示t时刻巡检机器人位置的真实状态，v_t表示t时刻巡检机器人速度的真实状态，q_t表示t时刻巡检机器人旋转变换的真实状态，a_bt表示t时刻加速度计零偏，ω_bt表示t时刻陀螺仪零偏；

带有误差量的状态为x：

其中，p表示巡检机器人位置带有误差量的状态，v表示巡检机器人速度带有误差量的状态，q表示巡检机器人旋转变换带有误差量的状态，a_b表示加速度计零偏，ω_b表示陀螺仪零偏；

误差状态为δx：

其中，δp表示巡检机器人位置的误差状态，δv表示巡检机器人速度的误差状态，δθ表示巡检机器人旋转变换的误差状态，δa_b表示加速度计零偏的误差状态，δω_b表示陀螺仪零偏的误差状态；

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

p_t＝p+δp

v_t＝v+δv

δq＝e^δθ/2

a_bt＝a_b+δa_b

ω_bt＝ω_b+δω_b

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

v←v+(R(a_m-a_b)+g)Δt

a_b←a_b

ω_m←ω_m

其中，Δt表示更新前后两个时刻的状态之间的时间间隔，R表示更新前后两个时刻的状态之间的旋转矩阵，a_m表示当前时刻的加速度测量值，ω_m表示当前时刻的角速度测量值，g表示重力加速度；

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

δp←δp+δvΔt

δv←δv+(-R[a_m-a_b]×δθ-Rδa_b)Δt+v_i

δθ←R^T{(ω_m-ω_b)Δt}δθ-δω_bΔt+θ_i

δa_b←δa_b+a_i

δω_b←δω_b+ω_i

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

其中[a_m-a_b]_×表示反对称矩阵；v_i,θ_i,a_i,ω_i通过惯性器件官方文档或实验手段获取得到；

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

δv^b＝R_bwδv^w+[v^b]_×δθ

其中，δv^b表示在机器人坐标系下机器人运动速度的误差状态，R_bw表示机器人坐标系至世界坐标系之间的旋转变换矩阵，δv^w表示在世界坐标系下机器人运动速度的误差状态，[v^b]_×表示在机器人坐标系下机器人运动速度的反对称矩阵，则得到关于机器人运动速度的误差状态的观测雅可比矩阵为：

其中，I₃表示3阶的单位矩阵；

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

通过RAC传感器得到机器人的位置观测值为p_rac，通过步骤(52)得到滤波器关于机器人位置的预测值为p，则观测方程为：

p_rac＝p

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

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