1.一种变电站巡检机器人在未知环境下的路径规划方法,其特征在于包括以下步骤:步骤一、为一个机器人规划路径时,将其余机器人视为障碍物,且所有机器人都处于匀速运动状态;
将机器人出发点设为全局直角坐标系的原点O(0,0),需要到达的目标点设为G(xg,yg);
同时在机器人离开原点去往目标点路径上,以机器人的当前位置R(xr,yr)设为局部极坐标系的极点,R指向G的方向为极坐标系的极轴;机器人在行进过程中扫描到的障碍物以P(ρ,α)表示,其中ρ为机器人与障碍物之间的距离,α为机器人与障碍物之间的连线与极轴的夹角;
步骤二、通过坐标转换公式将障碍物的局部极坐标转为全局直角坐标系的位置L(xlf,ylf);
步骤三、利用少量真实数据训练用来矫正受电磁干扰的激光雷达传感器数据的BP神经网络;
步骤四、利用步骤三中的BP神经网络对电磁干扰下的激光雷达传感器数据进行矫正,矫正后位置数据为L1(xld,yld);摄像头传感器拍摄到有障碍物的图像,然后根据solvePNP算法得到障碍物位置L2(x,y);
步骤五、利用基于专家先验策略的多数据融合卡尔曼滤波器预测动态障碍物下一时刻的位置;
步骤六、机器人在规划路径时将动态障碍物视为瞬时静止的障碍物,其规划方向是:将步骤五经过卡尔曼滤波预测的动态障碍物下一时刻的位置,通过步骤二中的逆向公式计算出极坐标系下的动态障碍物的坐标P(ρt+1,αt+1);同时假设障碍物的模型为圆形,半径为R;
步骤七、将机器人的运动方向角设为 其与极坐标系的极轴方向之间的夹角为期望方向角优化目标为在避开障碍物的情况下,让机器人的运动方向角与当前极坐标系的极轴方向之间的夹角的绝对值最小,即 其中αg为极坐标下目标点G的极角;
步骤八、假设只考虑单障碍物的情形,同时由于步骤六中将动态障碍物视为瞬间静止的障碍物,所以对于确定期望方向角没有影响;对于期望方向角有三种情形:(1):机器人在极坐标系的极轴方向上没有障碍物,机器人将直接沿着极轴方向运动,此时期望方向角(2):机器人在极坐标系的极轴方向上有障碍物,以极轴方向为基准线,记为df,分别计算出障碍物圆的左右两侧切线dl,dr与基准线df间的夹角βl,βr;设障碍物的坐标为T(ρT,αT),βl,βr的计算方法为:当 时,βl=β+αT,βr=β-αT;
当 时,βl=β+αT-2π,βr=β-αT+2π;
当βl<βr,机器人沿着dl方向运动,期望方向角 当βl>βr,机器人沿着dr方向运动,期望方向角(3):当βl=βr时,则规定机器人沿dr方向运动,即期望方向角 然后机器人跟着期望方向角运动单位时间的长度;
不断重复步骤四到步骤八,直到机器人到达目标点。
2.根据权利要求1所述的一种变电站巡检机器人在未知环境下的路径规划方法,其特征在于:步骤三具体是:
3.1采用一个三层BP神经网络来矫正受到电磁干扰影响的激光雷达传感器的数据;利用机器人上的电磁干扰测量仪测量当前时刻的电磁干扰强度,记为h;
3.2建立所述三层BP神经网络,一层输入层,一层隐藏层,一层输出层;其输入为受到电磁干扰影响的障碍物的位置 输出为修正好的障碍物当前时刻的位置
3.3确定BP神经网络的输入层到隐藏层的前向传递公式和隐藏层到输出层的前向传递公式;
3.4计算BP神经网络的前向传播误差:
其中 为训练数据的真实值,yi为训练数据神经网络的输出值;
其中N为训练数据个数;
3.5利用反向传播公式更新权重;
3.6采用利用少量真实数据训练BP神经网络;
所有数据都有一定的误差允许范围,在误差允许范围内将数据进行随机微小增减,不影响接下来的预测行为,将改动的数据和原数据进行随机打乱,组成新的数据集;其数据改动公式为:(x,y)=(x±e,y±e)
其中(x,y)是位置数据,e为数据允许的误差范围;
3.7用准备好的包含电磁干扰强度的数据集进行BP神经网络的训练,直到网络的判断准确度超过95%,停止训练,得到训练好的BP神经网络。
3.根据权利要求1所述的一种变电站巡检机器人在未知环境下的路径规划方法,其特征在于:步骤五具体是:
5.1、专家根据其经验知识做出如下的判断:
根据专家知识可得:激光雷达传感器权重为: 摄像头传感器权重为其中K1(n)和K2(n)分别为卡尔曼增益;
5.2、卡尔曼滤波通过系统输入输出观测数据对系统的状态进行最优估计,其动态障碍物位置预测的状态方程和激光雷达传感器与摄像头传感器观测方程为:X(n+1)=A×X(n)+B×V1(n)
Y1(n)=C1×X(n)+I1×V2(n)
Y2(n)=C2×X(n)+I2×V3(n)
其中X(n)表示系统的状态向量;A表示状态转移矩阵,用于描述由前一时刻到当前时刻下的运动状态转移方式;B表示干扰转移矩阵;V1(n)、V2(n)和V3(n)为N×1阶系统的噪声向量,用于描述系统从一个状态转移到另一个状态的噪声或者误差;Y1(n)表示N×1阶动态系统的激光雷达传感器的观测向量,表示动态系统在n时刻被测量到的状态;Y2(n)表示N×1阶动态系统的摄像头传感器的观测向量,表示动态系统在n时刻被测量到的状态;C1、C2和I1、I2为已知的动态系统测量参数矩阵,且
5.3、假设障碍物在x方向上速度为vx和在y方向上的速度为vy,激光雷达传感器和摄像头传感器采样一次的时间设为T,障碍物在n时刻的直角坐标系坐标为L(xn,yn),将A和B初始化:2
5.4、假设Vx(n),Vy(n)是均值为零,方差为σv的高斯白噪声V1(n)的两个正交向量,任意时刻Vx(n),Vy(n)相互独立;也假设Wx(n),Wy(n)是均值为零,方差为σw2的高斯白噪声V2(n)和V3(n)的两个正交向量,任意时刻Wx(n),Wy(n)相互独立;其协方差分别为Q(n)和R1(n),R2(n);其自相关矩阵分别为:E(V1(n)V2T(k))=O;E(V1(n)V3T(k))=O;E(V2(n)V3T(k))=O将误差矩阵P初始化为0矩阵,由P(n)=AP(n-1)AT+Q(n-1)进行误差矩阵预测;
K1(n)为n时刻激光雷达传感器数据的卡尔曼增益,其公式为:K1(n)=P(n)C1T(C1P(n)C1T+R1)-1K2(n)为n时刻摄像头传感器数据的卡尔曼增益,其公式为:K2(n)=P(n)C2T(C2P(n)C2T+R2)-1
5.5、激光雷达传感器局部滤波器位置预测 和更新估计误差矩阵为:
摄像头传感器局部滤波器位置预测 和更新估计误差矩阵 为:
5.6、下一时刻障碍物的位置预测X(n+1)和更新估计误差矩阵P(n+1)由两个传感器局部滤波器按一定的权值融合得到:在第n个时刻利用步骤五预测得障碍物下一时刻的位置。
4.根据权利要求1所述的一种变电站巡检机器人在未知环境下的路径规划方法,其特征在于:在机器人前进过程中,只考虑朝向目标点G的方向上有没有障碍物,如果有则只关心第一个障碍物;若在朝向目标点的运动方向上有间隔小于d长度的障碍物,则将密集障碍物归为一个团块,视为一个障碍物。