CN111913484B - 一种变电站巡检机器人在未知环境下的路径规划方法 - Google Patents

Abstract

本发明涉及一种变电站巡检机器人在未知环境下的路径规划方法。本发明的主要构思：本方法首先在机器人行动过程中用激光雷达和摄像头采集周围的环境信息，得到其它机器人的当前位置，再根据构建的卡尔曼滤波器进行传感器融合且预测其它机器人的下一时刻位置，在每个时刻根据预测信息和机器人当前位置计算出机器人运动的期望方向角，重复多次，直到机器人到达目标位置。本发明将动态位置环境下的动态障碍物视为瞬间静止的静态障碍物，用卡尔曼滤波预测其下一个位置时刻，再用机器人的期望方向角来确定机器人的前进方向，预测速度快，更全面，预测效果好。

Description

一种变电站巡检机器人在未知环境下的路径规划方法

技术领域

本发明涉及机器人路径规划领域，具体涉及一种在变电站强电磁干扰以及存在其它运动机器人的环境下的路径规划的方法。

背景技术

在变电站强电磁干扰的环境下，依靠机器人身上激光雷达、摄像头以及电磁干扰测量仪等传感器，预测其它机器人的运动轨迹，将预测信息融入路径规划算法中，提前绕开其它机器人以及障碍物所在位置，成功到达目标地点，这是未来机器人在变电站巡检时需要解决的问题之一。随着现代机器人技术发展，变电站的巡检工作未来将由机器人代替人工完成，省时省力，所以机器人在巡检过程中选择较优的路径将会是一个重要的研究方向。

现有技术大部分是在知晓全局环境的情况下，对机器人的路径进行规划，但是现有方法限制条件较多，难以应用在大多数特殊环境下。

发明内容

本发明的目的是为了针对在变电站强电磁干扰环境下机器人路径规划的局限，采用了一种利用神经网络矫正受电磁干扰影响的传感器数据，并且在基于有专家先验策略的多数据融合预测的卡尔曼滤波器中来预测其它机器人位置并进行规避的路径规划方法。

本发明的主要构思：本方法首先在机器人行动过程中用激光雷达和摄像头采集周围的环境信息，得到其它机器人的当前位置，再根据构建的卡尔曼滤波器进行传感器融合且预测其它机器人的下一时刻位置，在每个时刻根据预测信息和机器人当前位置计算出机器人运动的期望方向角，重复多次，直到机器人到达目标位置。

本发明方法的步骤包括：

步骤一、为一个机器人规划路径时，将其余机器人视为障碍物，且所有机器人都处于匀速运动状态。将机器人出发点设为全局直角坐标系的原点O(0，0)，需要到达的目标点设为G(x_g，y_g)。同时在机器人离开原点去往目标点路径上，以机器人的当前位置R(x_r，y_r)设为局部极坐标系的极点，R指向G的方向为极坐标系的极轴。机器人在行进过程中扫描到的障碍物以P(ρ，α)表示，其中ρ为机器人与障碍物之间的距离，α为机器人与障碍物之间的连线与极轴的夹角。

步骤二、通过坐标转换公式将障碍物的局部极坐标转为全局直角坐标系的位置L(x_lf，y_lf)。

步骤三、利用少量真实数据训练用来矫正受电磁干扰的激光雷达传感器数据的BP神经网络。

3.1采用一个三层BP神经网络来矫正受到电磁干扰影响的激光雷达传感器的数据。利用机器人上的电磁干扰测量仪测量当前时刻的电磁干扰强度，记为h。

3.2建立所述三层BP神经网络，一层输入层(三个神经元)，一层隐藏层(三个神经元)，一层输出层(两个神经元)。其输入为受到电磁干扰影响的障碍物的位置

输出为修正好的障碍物当前时刻的位置

3.3确定BP神经网络的输入层到隐藏层的前向传递公式和隐藏层到输出层的前向传递公式。

3.4计算BP神经网络的前向传播误差，其公式为：

其中

为训练数据的真实值，y_i为训练数据神经网络的输出值。

其中N为训练数据个数。

3.5利用反向传播公式更新权重矩阵：

其中l的取值为2和3，W²为输入层到隐藏层权重矩阵，W³为隐藏层到输出层权重矩阵，W^lf，b^lf为未更新的参数，W^l，b^l为更新后的参数。

3.6采用利用少量真实数据训练BP神经网络。所有数据都有一定的误差允许范围，在误差允许范围内将数据进行随机微小增减，不影响接下来的预测行为，将改动的数据和原数据进行随机打乱，组成新的数据集。其数据改动公式为：

(x，y)＝(x±e，y±e)

其中(x，y)是位置数据，e为数据允许的误差范围。

3.7用准备好的包含电磁干扰强度的数据集进行BP神经网络的训练，直到网络的判断准确度超过95％，停止训练，得到训练好的BP神经网络。

步骤四、利用步骤三中的BP神经网络对电磁干扰下的激光雷达传感器数据进行矫正，矫正后位置数据为L₁(x_ld，y_ld)；摄像头传感器拍摄到有障碍物的图像，然后根据solvePNP算法得到障碍物位置L₂(x，y)。

步骤五、利用基于专家先验策略的多数据融合卡尔曼滤波器预测动态障碍物下一时刻的位置，具体是：

5.1、专家根据其经验知识做出如下的判断：

对于多传感器融合的卡尔曼滤波器，根据传感器的固有特性以及多个传感器之间的相互作用确定在滤波器中所占的权重。根据专家知识可得：激光雷达传感器权重为：

其摄像头传感器权重为

其中K₁(n)和K₂(n)分别为其卡尔曼增益。

5.2、卡尔曼滤波通过系统输入输出观测数据对系统的状态进行最优估计，其动态障碍物位置预测的状态方程和激光雷达传感器与摄像头传感器观测方程为：

X(n+1)＝A×X(n)+B×V₁(n)

Y₁(n)＝C₁×X(n)+I₁×V₂(n)

Y₂(n)＝C₂×X(n)+I₂×V₃(n)

其中X(n)表示系统的状态向量；A表示状态转移矩阵，用于描述由前一时刻到当前时刻下的运动状态转移方式；B表示干扰转移矩阵；V₁(n)、V₂(n)和V₃(n)为N×1阶系统的噪声向量，用于描述系统从一个状态转移到另一个状态的噪声或者误差；Y₁(n)表示N×1阶动态系统的激光雷达传感器的观测向量，表示动态系统在n时刻被测量到的状态；Y₂(n)表示N×1阶动态系统的摄像头传感器的观测向量，表示动态系统在n时刻被测量到的状态；C₁、C₂和I₁、I₂为已知的动态系统测量参数矩阵，且

5.3、假设障碍物在x方向上速度为v_x和在y方向上的速度为v_y，激光雷达传感器和摄像头传感器采样一次的时间设为T，障碍物在n时刻的直角坐标系坐标为L(x_n，y_n)，将A和B初始化：

5.4、假设V_x(n)，V_y(n)是均值为零，方差为σ_v ²的高斯白噪声V₁(n)的两个正交向量，任意时刻V_x(n)，V_y(n)相互独立；也假设W_x(n)，W_y(n)是均值为零，方差为σ_w ²的高斯白噪声V₂(n)和V₃(n)的两个正交向量，任意时刻W_x(n)，W_y(n)相互独立。其协方差分别为Q(n)和R₁(n)，R₂(n)。其自相关矩阵分别为：

E(V₁(n)V₂ ^T(k))＝O；E(V₁(n)V₃ ^T(k))＝O；E(V₂(n)V₃ ^T(k))＝O

将误差矩阵P初始化为0矩阵，由P(n)＝AP(n-1)A^T+Q(n-1)进行误差矩阵预测。

K₁(n)为n时刻激光雷达传感器数据的卡尔曼增益，其公式为：

K₁(n)＝P(n)C₁ ^T(C₁P(n)C₁ ^T+R₁)^-1

K₂(n)为n时刻摄像头传感器数据的卡尔曼增益，其公式为：

K₂(n)＝P(n)C₂ ^T(C₂P(n)C₂ ^T+R₂)^-1

5.5、激光雷达传感器局部滤波器位置预测

和更新估计误差矩阵

为：

摄像头传感器局部滤波器位置预测

和更新估计误差矩阵

为：

5.6、下一时刻障碍物的位置预测X(n+1)和更新估计误差矩阵P(n+1)由两个传感器局部滤波器按一定的权值融合得到：

在第n个时刻利用步骤五预测的障碍物下一时刻的位置。

步骤六、机器人在规划路径时将动态障碍物视为瞬时静止的障碍物，其规划方向将步骤五经过卡尔曼滤波预测的动态障碍物下一时刻的位置，通过步骤二中的逆向公式计算出极坐标系下的动态障碍物的坐标P(ρ_t+1，α_t+1)。同时假设就障碍物的模型为圆形，半径为R。

步骤七、将机器人的运动方向角设为

其与极坐标系的极轴方向之间的夹角为期望方向角

优化目标为在避开障碍物的情况下，让机器人的运动方向角与当前极坐标系的极轴方向之间的夹角的绝对值最小，即

其中α_g为极坐标下目标点G的极角。

步骤八、假设只考虑单障碍物的情形，同时由于步骤六中将动态障碍物视为瞬间静止的障碍物，所以对于确定期望方向角没有影响。对于期望方向角有三种情形：

(1)：机器人在极坐标系的极轴方向上没有障碍物，机器人将直接沿着极轴方向运动，此时期望方向角

(2)：机器人在极坐标系的极轴方向上有障碍物，以极轴方向为基准线，记为d_f，分别计算出障碍物圆的左右两侧切线d_l，d_r与基准线d_f间的夹角β_l，β_r。设障碍物的坐标为T(ρ_T，α_T)，β_l，β_r的计算方法为：

当

时，β_l＝β+α_T，β_r＝β-α_T；

当

时，β_l＝β+α_T-2π，β_r＝β-α_T+2π；

当β_l＜β_r，机器人沿着d_l方向运动，期望方向角

当β_l＞β_r，机器人沿着d_r方向运动，期望方向角

(3)：当β_l＝β_r时(情形(2)的特例)，则规定机器人沿d_r方向运动，即期望方向角

然后机器人跟着期望方向角运动单位时间的长度。

不断重复步骤四到步骤八，直到机器人到达目标点。

本发明相对于现有技术所具有的有益效果：本发明将动态位置环境下的动态障碍物视为瞬间静止的静态障碍物，用卡尔曼滤波预测其下一个位置时刻，再用机器人的期望方向角来确定机器人的前进方向，预测速度快，更全面，预测效果好。

附图说明

图1是本发明的流程图；

图2是机器人，障碍物和目标点的坐标示意图；

图3是机器人的期望方向角确定示意图。

具体实施方式

以下结合附图对本发明作进一步说明。

如图1所示，一种变电站巡检机器人在未知环境下的路径规划方法，该方法具体包括以下步骤：

步骤一、为一个机器人规划路径时，将其余机器人视为障碍物，且所有机器人都处于匀速运动状态。将机器人出发点设为全局直角坐标系的原点O(0，0)，需要到达的目标点设为G(x_g，y_g)。同时在机器人离开原点去往目标点路径上，以机器人的当前位置R(x_r，y_r)设为局部极坐标系的极点，R指向G的方向为极坐标系的极轴。机器人在行进过程中扫描到的障碍物以P(ρ，α)表示，其中ρ为机器人与障碍物之间的距离，α为机器人与障碍物之间的连线与极轴的夹角，见图2。

步骤二、通过坐标转换公式将障碍物的局部极坐标转为全局直角坐标系的位置L(x_lf，y_lf)。

步骤三、利用少量真实数据训练用来矫正受电磁干扰的激光雷达传感器数据的BP神经网络。

3.1采用一个三层BP神经网络来矫正受到电磁干扰影响的激光雷达传感器的数据。利用机器人上的电磁干扰测量仪测量当前时刻的电磁干扰强度，记为h。

3.2建立所述三层BP神经网络，一层输入层(三个神经元)，一层隐藏层(三个神经元)，一层输出层(两个神经元)。其输入为受到电磁干扰影响的障碍物的位置

输出为修正好的障碍物当前时刻的位置

3.3确定BP神经网络的输入层到隐藏层的前向传递公式和隐藏层到输出层的前向传递公式。

3.4计算BP神经网络的前向传播误差，其公式为：

其中

为训练数据的真实值，y_i为训练数据神经网络的输出值。

其中N为训练数据个数。

3.5利用反向传播公式更新权重矩阵：

其中l的取值为2和3，W^lf，b^lf为未更新的参数，W^l，b^l为更新后的参数。

3.6采用利用少量真实数据训练BP神经网络。所有数据都有一定的误差允许范围，在误差允许范围内将数据进行随机微小增减，不影响接下来的预测行为，将改动的数据和原数据进行随机打乱，组成新的数据集。其数据改动公式为：

(x，y)＝(x±e，y±e)

其中(x，y)是位置数据，e为数据允许的误差范围。

3.67用准备好的包含电磁干扰强度的数据集进行BP神经网络的训练，直到网络的判断准确度超过95％，停止训练，得到训练好的BP神经网络。

步骤四、利用步骤三中的BP神经网络对电磁干扰下的激光雷达传感器数据进行矫正，矫正后位置数据为L₁(x_ld，y_la)；摄像头传感器拍摄到有障碍物的图像，然后根据solvePNP算法得到障碍物位置L₂(x，y)。

步骤五、利用基于专家先验策略的多数据融合卡尔曼滤波器预测动态障碍物下一时刻的位置，具体是：

5.1、专家根据其经验知识做出如下的判断：

对于多传感器融合的卡尔曼滤波器，根据传感器的固有特性以及多个传感器之间的相互作用确定在滤波器中所占的权重。根据专家知识可得：激光雷达传感器权重为：

其摄像头传感器权重为

其中K₁(n)和K₂(n)分别为其卡尔曼增益。

5.2、卡尔曼滤波通过系统输入输出观测数据对系统的状态进行最优估计，其动态障碍物位置预测的状态方程和激光雷达传感器与摄像头传感器观测方程为：

X(n+1)＝A×X(n)+B×V₁(n)

Y₁(n)＝C₁×X(n)+I₁×V₂(n)

Y₂(n)＝C₂×X(n)+I₂×V₃(n)

其中X(n)表示系统的状态向量；A表示状态转移矩阵，用于描述由前一时刻到当前时刻下的运动状态转移方式；s表示干扰转移矩阵；V₁(n)、V₂(n)和V₃(n)为N×1阶系统的噪声向量，用于描述系统从一个状态转移到另一个状态的噪声或者误差；Y₁(n)表示N×1阶动态系统的激光雷达传感器的观测向量，表示动态系统在n时刻被测量到的状态；Y₂(n)表示N×1阶动态系统的摄像头传感器的观测向量，表示动态系统在n时刻被测量到的状态；C₁、C₂和I₁、I₂为已知的动态系统测量参数矩阵，且

5.3、假设障碍物在x方向上速度为v_x和在y方向上的速度为v_y，激光雷达传感器和摄像头传感器采样一次的时间设为T，障碍物在n时刻的直角坐标系坐标为L(x_n，y_n)，将A和B初始化：

5.4、假设V_x(n)，V_y(n)是均值为零，方差为σ_v ²的高斯白噪声V₁(n)的两个正交向量，任意时刻V_x(n)，V_y(n)相互独立；也假设W_x(n)，W_y(n)是均值为零，方差为σ_w ²的高斯白噪声V₂(n)和V₃(n)的两个正交向量，任意时刻W_x(n)，W_y(n)相互独立。其协方差分别为Q(n)和R₁(n)，R₂(n)。其自相关矩阵分别为：

E(V₁(n)V₂ ^T(k))＝o；E(V₁(n)V₃ ^T(k))＝o；E(V₂(n)V₃ ^T(k))＝O

将误差矩阵P初始化为0矩阵，由P(n)＝AP(n-1)A^T+Q(n-1)进行误差矩阵预测。

K₁(n)为n时刻激光雷达传感器数据的卡尔曼增益，其公式为：

K₁(n)＝P(n)C₁ ^T(C₁P(n)C₁ ^T+R₁)^-1

K₂(n)为n时刻摄像头传感器数据的卡尔曼增益，其公式为：

K₂(n)＝P(n)C₂ ^T(C₂P(n)C₂ ^T+R₂)^-1

5.5、激光雷达传感器局部滤波器位置预测

和更新估计误差矩阵

为：

摄像头传感器局部滤波器位置预测

和更新估计误差矩阵

为：

5.6、下一时刻障碍物的位置预测X(n+1)和更新估计误差矩阵P(n+1)由两个传感器局部滤波器按一定的权值融合得到：

在第n个时刻利用步骤五预测的障碍物下一时刻的位置。

步骤六、机器人在规划路径时将动态障碍物视为瞬时静止的障碍物，其规划方向将步骤五经过卡尔曼滤波预测的动态障碍物下一时刻的位置，通过步骤二中的逆向公式计算出极坐标系下的动态障碍物的坐标P(ρ_t+1，α_t+1)。同时假设就障碍物的模型为圆形，半径为R。

步骤七、将机器人的运动方向角设为

其与极坐标系的极轴方向之间的夹角为期望方向角

优化目标为在避开障碍物的情况下，让机器人的运动方向角与当前极坐标系的极轴方向之间的夹角的绝对值最小，即

其中α_g为极坐标下目标点G的极角。

步骤八、在机器人前进过程中，只考虑朝向目标点G的方向上有没有障碍物，如果有则只关心第一个障碍物。若在朝向目标点的运动方向上有间隔小于d长度的障碍物，则将密集障碍物归为一个团块，视为一个障碍物，障碍物直径为2r+d。设两个障碍物的极坐标系的坐标点为P₁(ρ₁，α₁)，P₂(ρ₂，α₂)，则d的长度为：

步骤九、假设只考虑单障碍物的情形，同时由于步骤六中将动态障碍物视为瞬间静止的障碍物，所以对于确定期望方向角没有影响。对于期望方向角有三种情形，如图3所示：

(1)：机器人在极坐标系的极轴方向上没有障碍物，机器人将直接沿着极轴方向运动，此时期望方向角

(2)：机器人在极坐标系的极轴方向上有障碍物，以极轴方向为基准线，记为d_f，分别计算出障碍物圆的左右两侧切线d_l，d_r与基准线d_f间的夹角β_l，β_r。设障碍物的坐标为T(ρ_T，α_T)，β_l，β_r的计算方法为：

当

时，β_l＝β+α_T，β_r＝β-α_T；

当

时，β_l＝β+α_T-2π，β_r＝β-α_T+2π；

当β_l＜β_r，机器人沿着d_l方向运动，期望方向角

当β_l＞β_r，机器人沿着d_r方向运动，期望方向角

(3)：当β_l＝β_r时(情形(2)的特例)，则规定机器人沿d_r方向运动，即期望方向角

然后机器人跟着期望方向角运动单位时间的长度。

不断重复步骤四到步骤九，直到机器人到达目标点。

Claims (4)

1.一种变电站巡检机器人在未知环境下的路径规划方法，其特征在于包括以下步骤：

步骤一、为一个机器人规划路径时，将其余机器人视为障碍物，且所有机器人都处于匀速运动状态；

将机器人出发点设为全局直角坐标系的原点O(0，0)，需要到达的目标点设为G(x_g，y_g)；同时在机器人离开原点去往目标点路径上，以机器人的当前位置R(x_r，y_r)设为局部极坐标系的极点，R指向G的方向为极坐标系的极轴；机器人在行进过程中扫描到的障碍物以P(ρ，α)表示，其中ρ为机器人与障碍物之间的距离，α为机器人与障碍物之间的连线与极轴的夹角；

步骤二、通过坐标转换公式将障碍物的局部极坐标转为全局直角坐标系的位置L(x_lf，y_lf)；

步骤三、利用少量真实数据训练用来矫正受电磁干扰的激光雷达传感器数据的BP神经网络；

步骤四、利用步骤三中的BP神经网络对电磁干扰下的激光雷达传感器数据进行矫正，矫正后位置数据为L₁(x_ld，y_ld)；摄像头传感器拍摄到有障碍物的图像，然后根据solvePNP算法得到障碍物位置L₂(x，y)；

步骤五、利用基于专家先验策略的多数据融合卡尔曼滤波器预测动态障碍物下一时刻的位置；

步骤六、机器人在规划路径时将动态障碍物视为瞬时静止的障碍物，其规划方向是：将步骤五经过卡尔曼滤波预测的动态障碍物下一时刻的位置，通过步骤二中的逆向公式计算出极坐标系下的动态障碍物的坐标P(ρ_t+1，α_t+1)；同时假设障碍物的模型为圆形，半径为R；

步骤七、将机器人的运动方向角设为

其与极坐标系的极轴方向之间的夹角为期望方向角

优化目标为在避开障碍物的情况下，让机器人的运动方向角与当前极坐标系的极轴方向之间的夹角的绝对值最小，即

其中α_g为极坐标下目标点G的极角；

步骤八、假设只考虑单障碍物的情形，同时由于步骤六中将动态障碍物视为瞬间静止的障碍物，所以对于确定期望方向角没有影响；对于期望方向角有三种情形：

(1)：机器人在极坐标系的极轴方向上没有障碍物，机器人将直接沿着极轴方向运动，此时期望方向角

(2)：机器人在极坐标系的极轴方向上有障碍物，以极轴方向为基准线，记为d_f，分别计算出障碍物圆的左右两侧切线d_l，d_r与基准线d_f间的夹角β_l，β_r；设障碍物的坐标为T(ρ_T，α_T)，β_l，β_r的计算方法为：

当

时，β_l＝β+α_T，β_r＝β-α_T；

当

时，β_l＝β+α_T-2π，β_r＝β-α_T+2π；

当β_l＜β_r，机器人沿着d_l方向运动，期望方向角

当β_l＞β_r，机器人沿着d_r方向运动，期望方向角

(3)：当β_l＝β_r时，则规定机器人沿d_r方向运动，即期望方向角

然后机器人跟着期望方向角运动单位时间的长度；

不断重复步骤四到步骤八，直到机器人到达目标点。

2.根据权利要求1所述的一种变电站巡检机器人在未知环境下的路径规划方法，其特征在于：步骤三具体是：

3.1采用一个三层BP神经网络来矫正受到电磁干扰影响的激光雷达传感器的数据；利用机器人上的电磁干扰测量仪测量当前时刻的电磁干扰强度，记为h；

3.2建立所述三层BP神经网络，一层输入层，一层隐藏层，一层输出层；其输入为受到电磁干扰影响的障碍物的位置

输出为修正好的障碍物当前时刻的位置

3.3确定BP神经网络的输入层到隐藏层的前向传递公式和隐藏层到输出层的前向传递公式；

3.4计算BP神经网络的前向传播误差：

其中

为训练数据的真实值，y_i为训练数据神经网络的输出值；

其中N为训练数据个数；

3.5利用反向传播公式更新权重；

3.6采用利用少量真实数据训练BP神经网络；

所有数据都有一定的误差允许范围，在误差允许范围内将数据进行随机微小增减，不影响接下来的预测行为，将改动的数据和原数据进行随机打乱，组成新的数据集；其数据改动公式为：

(x，y)＝(x±e，y±e)

其中(x，y)是位置数据，e为数据允许的误差范围；

3.7用准备好的包含电磁干扰强度的数据集进行BP神经网络的训练，直到网络的判断准确度超过95％，停止训练，得到训练好的BP神经网络。

3.根据权利要求1所述的一种变电站巡检机器人在未知环境下的路径规划方法，其特征在于：步骤五具体是：

5.1、专家根据其经验知识做出如下的判断：

根据专家知识可得：激光雷达传感器权重为：

摄像头传感器权重为

其中K₁(n)和K₂(n)分别为卡尔曼增益；

5.2、卡尔曼滤波通过系统输入输出观测数据对系统的状态进行最优估计，其动态障碍物位置预测的状态方程和激光雷达传感器与摄像头传感器观测方程为：

X(n+1)＝A×X(n)+B×V₁(n)

Y₁(n)＝C₁×X(n)+I₁×V₂(n)

Y₂(n)＝C₂×X(n)+I₂×V₃(n)

其中X(n)表示系统的状态向量；A表示状态转移矩阵，用于描述由前一时刻到当前时刻下的运动状态转移方式；B表示干扰转移矩阵；V₁(n)、V₂(n)和V₃(n)为N×1阶系统的噪声向量，用于描述系统从一个状态转移到另一个状态的噪声或者误差；Y₁(n)表示N×1阶动态系统的激光雷达传感器的观测向量，表示动态系统在n时刻被测量到的状态；Y₂(n)表示N×1阶动态系统的摄像头传感器的观测向量，表示动态系统在n时刻被测量到的状态；C₁、C₂和I₁、I₂为已知的动态系统测量参数矩阵，且

5.3、假设障碍物在x方向上速度为v_x和在y方向上的速度为v_y，激光雷达传感器和摄像头传感器采样一次的时间设为T，障碍物在n时刻的直角坐标系坐标为L(x_n，y_n)，将A和B初始化：

5.4、假设V_x(n)，V_y(n)是均值为零，方差为σ_v ²的高斯白噪声V₁(n)的两个正交向量，任意时刻V_x(n)，V_y(n)相互独立；也假设W_x(n)，W_y(n)是均值为零，方差为σ_w ²的高斯白噪声V₂(n)和V₃(n)的两个正交向量，任意时刻W_x(n)，W_y(n)相互独立；其协方差分别为Q(n)和R₁(n)，R₂(n)；其自相关矩阵分别为：

E(V₁(n)V₂ ^T(k))＝O；E(V₁(n)V₃ ^T(k))＝O；E(V₂(n)V₃ ^T(k))＝O

将误差矩阵P初始化为0矩阵，由P(n)＝AP(n-1)A^T+Q(n-1)进行误差矩阵预测；

K₁(n)为n时刻激光雷达传感器数据的卡尔曼增益，其公式为：

K₁(n)＝P(n)C₁ ^T(C₁P(n)C₁ ^T+R₁)^-1

K₂(n)为n时刻摄像头传感器数据的卡尔曼增益，其公式为：

K₂(n)＝P(n)C₂ ^T(C₂P(n)C₂ ^T+R₂)^-1

5.5、激光雷达传感器局部滤波器位置预测

和更新估计误差矩阵

为：

摄像头传感器局部滤波器位置预测

和更新估计误差矩阵

为：

5.6、下一时刻障碍物的位置预测X(n+1)和更新估计误差矩阵P(n+1)由两个传感器局部滤波器按一定的权值融合得到：

在第n个时刻利用步骤五预测得障碍物下一时刻的位置。

4.根据权利要求1所述的一种变电站巡检机器人在未知环境下的路径规划方法，其特征在于：在机器人前进过程中，只考虑朝向目标点G的方向上有没有障碍物，如果有则只关心第一个障碍物；若在朝向目标点的运动方向上有间隔小于d长度的障碍物，则将密集障碍物归为一个团块，视为一个障碍物。

Images