[发明专利]一种基于多传感器融合的电缆沟智能巡检机器人定位方法

权利要求书

1.一种基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，包括以下步骤：

步骤1、基于扩展卡尔曼滤波将编码器、陀螺仪、深度相机的RGB图得到的机器人位姿信息进行融合；

步骤2、基于RBPF粒子滤波算法进行定位和建图。

2.如权利要求1所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述步骤1扩展卡尔曼滤波的状态方程与观测方程如下式：

其中X(k)∈Rⁿ是所述机器人k时刻系统状态即所述机器人k时刻的位姿向量，A(k-1)∈R^n,n为经过线性化处理得到的状态转移矩阵，O(k-1)∈R^n,h表示k时刻系统输入到系统状态的映射关系，B(k-1)∈R^n,h是过程噪声分布矩阵，W(k-1)∈R^h是过程向量，u(k-1)∈R^h是所述机器人k-1时刻得到的里程计控制输入信息；Y_i(k-1)∈R^m是第i个传感器的测量信息向量，C_i(k-1)是相应传感器测量矩阵，V_i(k-1)∈R^m为测量噪声。

3.如权利要求2所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述步骤1用协方差矩阵表示系统状态估计的不确定性。

4.如权利要求3所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述步骤1协方差之间的关系满足：

Σ(k)＝A(k)Σ(k-1)A(k)^T+O(k)Q(k)Q(k)^T

Σ(k)表示k时刻系统状态的不确定性对应的协方差矩阵，Q(k)表示k时刻系统输入控制信息的不确定性对应的协方差。

5.如权利要求4所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述编码器的初始协方差矩阵为[100000；010000；000000；000000；000000；000001]。

6.如权利要求4所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述深度相机的RGB图的初始协方差矩阵为[100000；010000；000000；000000；000000；000001]。

7.如权利要求1所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述机器人路过障碍物信息如式：z_1:k＝(z(1),z(2),…,z(k))。

8.如权利要求1所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述RBPF粒子滤波算法包括重要性重采样算法。

9.如权利要求8所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述重要性重采样算法通过不断迭代来估计每一时刻所述机器人的位姿。

10.如权利要求8所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述重要性重采样算法包括以下步骤：采样-计算权重-重采样-地图估计。