CN101441736B - 汽车起重机机器人路径规划方法 - Google Patents

Abstract

本发明涉及汽车起重机机器人的路径规划方法，特别涉及一种改进的基于神经网络能量函数的自主移动机器人路径规划算法。定义路径总能量函数为碰撞罚函数与相应于路径长度部分能量函数的加权和。检测路径的出发点和目标点是否都在障碍物的中轴线上，如果出发点和目标点都在障碍物的轴线上，随机选择一个路径点，改变该路径点的坐标后再进行路径规划。在路径规划的过程中检测路径点的位置是否在障碍物内，根据路径点位于障碍物内外的不同位置用不同的动态运动方程移动路径点的位置，使其朝着使总能量函数的函数值减小的方向运动，最终规划出最短的避障路径。

Description

汽车起重机机器人路径规划方法

技术领域

本发明涉及汽车起重机机器人的路径规划，特别涉及一种改进的基于神经网络能量函数的自主移动机器人路径规划方法。

背景技术

随着科技的发展，汽车起重机机器人将代替人们在危险的环境中工作，汽车起重机机器人，属于一种自主移动机器人，它需要具有识别周围环境，规划运动路径在有障碍物的环境下进行无碰撞移动的能力。自主移动机器人路径规划的传统路径规划方法中的人工势场法，其基本思想是通过寻找路径点的能量函数的极小值点而使路径避开障碍物，但存在局部极小值问题且不适于寻求最短路径。“人工神经网络”是在对人脑组织结构和运行机制的认识理解基础之上模拟其结构和智能行为的一种工程系统。其信息处理功能是由网络单元的输入输出特性、网络拓扑结构、神经元之间的连接强度(连接权)等共同决定的。一种基于神经网络能量函数的路径规划算法(简称NA路径规划算法)，收敛速度快，并且能够规划出最短避障路径。但是，在此算法中存在局部极小值问题。本发明提出一种改进的基于神经网络能量函数的路径规划方法，可以解决此种路径规划的局部极小值问题。

发明内容

本发明的目的在于克服NA路径规划算法存在的不足而提供一种可避免局部极小值问题、规划的无碰路径达到最短无碰路径的改进的基于神经网络能量函数的路径规划方法。

为实现上述目的，本发明所采用的技术方案包括以下几个步骤：

步骤1：定义路径总能量函数：一条路径的碰撞罚函数定义为各路径点的碰撞罚函数之和，而一个点的碰撞罚函数是通过它对各个障碍物的三层前向神经网络表示得到的；输入层的两个结点的输入分别是路径点的横坐标和纵坐标x、y，其中x、y为实数，中间层的每个结点相应于障碍物的一条边的不等式限制条件，输入层和中间层的连接权系数等于不等式中x、y前面的系数，中间层每个结点的阈值等于相应不等式中的常数项；中间层到输出层的连接权为1，输出层结点的阈值取为不等式的个数减去0.5后的负数；

该连续网络的运算关系为

$C=f\left(I_O\right)=1/(1+e^{-I_o/T})---\left(1\right)$

$I_O=\underset{m=1}{\overset{M}{\mathrm{\Σ}}}{O}_{\mathrm{Hm}}+{\mathrm{\θ}}_T---\left(2\right)$

$O_{\mathrm{Hm}}=f_{H_m}\left(I_{\mathrm{Hm}}\right)---\left(3\right)$

I_Hm＝w_xmx_i+w_ymy_i+θ_Hm    (4)

其中各符号的含义为，C：顶层结点输出；I_O：顶层结点输入；θ_T：顶层结点阈值；O_Hm：中间层第m个结点的输出；I_Hm：中间层第m个结点的输入；θ_Hm：中间层第m个结点的阈值；w_xm，w_ym：第m个不等式限制条件的系数；激发函数为常用的S形函数，即

$f_{\mathrm{Hm}}\left(x\right)=\frac{1}{1+e^{-x/T_{\mathrm{Hm}}}}---\left(5\right)$

模拟退火温度变化公式为：

$T=\frac{\mathrm{\β}}{\log (1+t)}---\left(6\right)$

$T_{\mathrm{Hm}}\left(t\right)=\frac{{\mathrm{\β}}_m}{\log (1+t)}---\left(7\right)$

其中β是模拟退火初始温度，β_m是相应于障碍物每一条边的模拟退火初始温度，可以根据障碍物的形状，设定各边的不同的初始温度，这样对于一些不对称图形可避免其罚函数曲面形成一边倒的情况；

整条路径相应于碰撞函数部分的能量为

$E_C=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\underset{k=1}{\overset{K}{\mathrm{\Σ}}}{C}_i^k---\left(8\right)$

其中K是障碍物的个数，N是路径点的个数，C_i ^k是第i个路径点P(x_i，y_i)对第k个障碍物的碰撞函数；

相应于路径长度部分的能量函数是所有线段长度的平方和，即对所有路径点P(x_i，y_i)，i＝1，2，…，N，

$E_l=\underset{i=1}{\overset{N-1}{\mathrm{\Σ}}}[{(x_{i+1}-x_i)}^2+{(y_{i+1}-y_i)}^2]---\left(9\right)$

整条路径的总能量函数为

E＝w_lE_l+w_cE_c               (10)

其中w_l和w_c分别表示对每一部分的权重(w_l+w_c＝1)；

步骤2：根据出发点坐标P(x₁，y₁)及目标点坐标P(x_N，y_N)，初始路径一般取为出发点到目标点的直线上均匀分布的点列，当x₁≠x_N时

x_i＝x₁+i(x_N-x₁)/(N-1)

y_i＝(y_N-y₁)(x_i-x₁)/(x_N-x₁)+y₁(i＝2，3，…，N-1)    (11)

步骤3：检测路径的起点和终点是否都在障碍物的中轴线上，根据不同情况分别进行下面两种操作：

(1)若起点和终点至少有一个不在障碍物的中轴线上，则执行步骤4；

(2)若起点和终点都在障碍物的中轴线上，则随机选择一个路径点，改变该路径点坐标后，执行步骤4；

步骤4：对于路径点P(x_i，y_i)，i＝2，3，…，N-1，用检测器检测是否在障碍物内，根据不同情况分别进行下面两种操作：

(1)若路径点P(x_i，y_i)在障碍物内，则按下列运动方程移动

${\stackrel{\·}{x}}_i=-{\mathrm{\η}}_1({2w}_l({2x}_i-x_{i-1}-x_{i+1})+w_c\underset{k=1}{\overset{K}{\mathrm{\Σ}}}{f}^{\′}\left({\left(I_O\right)}_i^k\right)\left(\underset{m=1}{\overset{M}{\mathrm{\Σ}}}{f}_{H_m}^{\′}\left({\left(I_{H_m}\right)}_i^k\right)w_{\mathrm{xm}}^k\right))$

${\stackrel{\·}{y}}_i=-{\mathrm{\η}}_1({2w}_l({2y}_i-y_{i-1}-y_{i+1})+w_c\underset{k=1}{\overset{K}{\mathrm{\Σ}}}{f}^{\′}\left({\left(I_O\right)}_i^k\right)\left(\underset{m=1}{\overset{M}{\mathrm{\Σ}}}{f}_{H_m}^{\′}\left({\left(I_{H_m}\right)}_i^k\right)w_{\mathrm{ym}}^k\right))---\left(12\right)$

${\stackrel{\·}{x}}_i=-{\mathrm{\η}}_1({2w}_l({2x}_i-x_{i-1}-x_{i+1})+w_c{f}^{\′}\left({\left(I_O\right)}_i\right)+f_H^{\′}\left({\left(I_H\right)}_i\right)(P-x_i))$

${\stackrel{\·}{y}}_i=-{\mathrm{\η}}_1({2w}_l({2w}_i-y_{i-1}-y_{i+1})+w_c{f}^{\′}\left({\left(I_O\right)}_i\right)+f_H^{\′}\left({\left(I_H\right)}_i\right)(Q-y_i))---\left(13\right)$

其中(12)用于P(x_i，y_i)位于多边形的障碍物内的情况，(13)用于P(x_i，y_i)位于圆心在(P，Q)的圆形障碍物内的情况；

(2)若P(x_i，y_i)在障碍物之外，则按下列运动方程移动

${\stackrel{\·}{x}}_i=-{\mathrm{\η}}_2({2x}_i-x_{i-1}-x_{i+1})$

${\stackrel{\·}{y}}_i=-{\mathrm{\η}}_2({2y}_i-y_{i-1}-y_{i+1})---\left(14\right)$

步骤5：重复执行步骤4，直到路径收敛；

其中上述步骤1-5基于以下假设：

(1)障碍物是二维环境中的多边形围成的平面图形或者是圆形的平面图形；

(2)障碍物为静止的；

(3)计算时障碍物的尺寸按机器人的半径作适当的扩展。

本发明给出了一种改进的基于神经网络能量函数的路径规划方法，能够解决NA路径规划算法的局部极小值问题，完成最短避障路径规划，为移动机器人的最优路径规划提供了一个简捷有效的方法。

附图说明

图1为一个点到一个障碍物的罚函数的神经网络。

图2为NA路径规划算法的计算实例。

图3是在出发点和目标点都在障碍物的中轴线上的条件下，用NA路径规划算法进行的仿真实验结果。

图4是在出发点和目标点都在障碍物的中轴线上的条件下，用本发明的改进的基于神经网络能量函数的路径规划方法进行路径规划的仿真实验过程及路径规划结果图。

具体实施方式

本发明的改进的基于神经网络能量函数的路径规划方法的路径规划具体实施方法分为如下几个步骤进行：

步骤1：定义路径总能量函数。

一条路径的碰撞罚函数定义为各路径点的碰撞罚函数之和，而一个点的碰撞罚函数是通过它对各个障碍物的三层前向神经网络表示得到的。图1表示了一个点到一个障碍物的罚函数的神经网络。输入层两个结点的输入值分别是路径点的横坐标和纵坐标x、y，x、y均为实数，中间层的每个结点是相应于障碍物的一条边的不等式限制条件，输入层和中间层的连接权系数等于不等式中x、y前面的系数，中间层每个结点的阈值等于相应不等式中的常数项。中间层到输出层的连接权为1，输出层结点的阈值取为不等式的个数减去0.5后的负数。

该连续网络的运算关系为

$C=f\left(I_O\right)=1/(1+e^{-I_o/T})---\left(1\right)$

$I_O=\underset{m=1}{\overset{M}{\mathrm{\Σ}}}{O}_{\mathrm{Hm}}+{\mathrm{\θ}}_T---\left(2\right)$

$O_{\mathrm{Hm}}=f_{H_m}\left(I_{\mathrm{Hm}}\right)---\left(3\right)$

I_Hm＝w_xmx_i+w_ymy_i+θ_Hm    (4)

其中各符号的含义为，C：顶层结点输出；I_O：顶层结点输入；θ_T：顶层结点阈值，θ_T一般为大于0小于1的实数；O_Hm：中间层第m个结点的输出；I_Hm：中间层第m个结点的输入；θ_Hm：中间层第m个结点的阈值，θ_Hm一般为大于0小于1的实数w_xm，w_ym：第m个不等式限制条件的系数。激发函数为常用的S形函数，即

$f_{\mathrm{Hm}}\left(x\right)=\frac{1}{1+e^{-x/T_{\mathrm{Hm}}}}---\left(5\right)$

模拟退火温度变化公式为：

$T=\frac{\mathrm{\β}}{\log (1+t)}---\left(6\right)$

$T_{\mathrm{Hm}}\left(t\right)=\frac{{\mathrm{\β}}_m}{\log (1+t)}---\left(7\right)$

其中β是模拟退火初始温度，β_m是相应于障碍物每一条边的模拟退火初始温度，β和β_m一般取为大于0小于1的实数，可以根据障碍物的形状，设定各边的不同的初始温度，这样对于一些不对称图形可避免其罚函数曲面形成一边倒的情况。

整条路径相应于碰撞函数部分的能量为

$E_C=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\underset{k=1}{\overset{K}{\mathrm{\Σ}}}{C}_i^k---\left(8\right)$

其中K是障碍物的个数，N是路径点的个数，C_i ^k表示第i个路径点P(x_i，y_i)对第k个障碍物的碰撞函数。

相应于路径长度部分的能量函数定义为所有线段长度的平方和，即对所有路径点P(x_i，y_i)，i＝1，2，…，N，

$E_l=\underset{i=1}{\overset{N-1}{\mathrm{\Σ}}}[{(x_{i+1}-x_i)}^2+{(y_{i+1}-y_i)}^2]---\left(9\right)$

整条路径的总能量函数为

E＝w_lE_l+w_cE_c           (10)

其中w_l和w_c分别表示对每一部分的权重，w_l和w_c均为正实数(w_l+w_c＝1)。

步骤2：根据出发点坐标P(x₁，y₁)及目标点坐标P(x_N，y_N)，初始路径一般取为出发点到目标点的直线上均匀分布的点列，当x₁≠x_N时

x_i＝x₁+i(x_N-x₁)/(N-1)

y_i＝(y_N-y₁)(x_i-x₁)/(x_N-x₁)+y₁(i＝2，3，…，N-1)    (11)

步骤3：检测路径的起点和终点是否都在障碍物的中轴线上，根据不同情况分别进行下面两种操作：

(1)若起点和终点至少有一个不在障碍物的中轴线上，则执行步骤4；

(2)若起点和终点都在障碍物的中轴线上，则随机选择一个路径点，将其横坐标或纵坐标加上或减去一个大于0且小于1的正数，执行步骤4。

步骤4：对于路径点P(x_i，y_i)，i＝2，3，…，N-1，用检测器检测是否在障碍物内，根据不同情况分别进行下面两种操作：

(1)若路径点P(x_i，y_i)在障碍物内，则按下列运动方程移动

${\stackrel{\·}{x}}_i=-{\mathrm{\η}}_1({2w}_l({2x}_i-x_{i-1}-x_{i+1})+w_c\underset{k=1}{\overset{K}{\mathrm{\Σ}}}{f}^{\′}\left({\left(I_O\right)}_i^k\right)\left(\underset{m=1}{\overset{M}{\mathrm{\Σ}}}{f}_{H_m}^{\′}\left({\left(I_{H_m}\right)}_i^k\right)w_{\mathrm{xm}}^k\right))$

${\stackrel{\·}{y}}_i=-{\mathrm{\η}}_1({2w}_l({2y}_i-y_{i-1}-y_{i+1})+w_c\underset{k=1}{\overset{K}{\mathrm{\Σ}}}{f}^{\′}\left({\left(I_O\right)}_i^k\right)\left(\underset{m=1}{\overset{M}{\mathrm{\Σ}}}{f}_{H_m}^{\′}\left({\left(I_{H_m}\right)}_i^k\right)w_{\mathrm{ym}}^k\right))---\left(12\right)$

${\stackrel{\·}{x}}_i=-{\mathrm{\η}}_1({2w}_l({2x}_i-x_{i-1}-x_{i+1})+w_c{f}^{\′}\left({\left(I_O\right)}_i\right)+f_H^{\′}\left({\left(I_H\right)}_i\right)(P-x_i))$

${\stackrel{\·}{y}}_i=-{\mathrm{\η}}_1({2w}_l({2w}_i-y_{i-1}-y_{i+1})+w_c{f}^{\′}\left({\left(I_O\right)}_i\right)+f_H^{\′}\left({\left(I_H\right)}_i\right)(Q-y_i))---\left(13\right)$

其中(12)用于P(x_i，y_i)位于多边形的障碍物内的情况，(13)用于P(x_i，y_i)位于圆心在(P，Q)的圆形障碍物内的情况，η₁是大于0且小于1的正实数。

(2)若P(x_i，y_i)在障碍物之外，则按下列运动方程移动

${\stackrel{\·}{x}}_i=-{\mathrm{\η}}_2({2x}_i-x_{i-1}-x_{i+1})$

${\stackrel{\·}{y}}_i=-{\mathrm{\η}}_2({2y}_i-y_{i-1}-y_{i+1})---\left(14\right)$

η₂是大于0且小于1的正实数。

步骤5：重复执行步骤4，直到路径收敛。

仿真实验

图2为NA路径规划算法的一个计算实例。

图3是在目标点和起始点坐标为x_s＝0.9，y_s＝0.2，x_t＝0.9，y_t＝1.6条件下，用NA路径规划算法进行的仿真实验结果，路径规划落入局部极小值，没有规划出避障路径。

图4是在目标点和起始点坐标为x_s＝0.9，y_s＝0.2，x_t＝0.9，y_t＝1.6条件下，用本发明的改进的基于神经网络能量函数的路径规划算法进行路径规划得仿真实验过程及路径规划结果图。即图4是在目标点和起始点坐标与图2的在目标点和起始点坐标完全一样的情况下用改进算法进行的仿真实验结果，它是一条折线形的最短避障路径。

Claims (1)

1.一种汽车起重机机器人路径规划方法，用于克服NA路径规划算法存在的不足而提供一种可避免局部极小值问题、规划的无碰路径达到最短无碰路径的改进的基于神经网络能量函数的路径规划方法，其特征在于包括以下几个步骤：

步骤1：定义路径总能量函数：一条路径的碰撞罚函数定义为各路径点的碰撞罚函数之和，而一个点的碰撞罚函数是通过它对各个障碍物的三层前向神经网络表示得到的；输入层的两个结点的输入分别是路径点的横坐标和纵坐标x、y，其中x、y为实数，中间层的每个结点相应于障碍物的一条边的不等式限制条件，输入层和中间层的连接权系数等于不等式中x、y前面的系数，中间层每个结点的阈值等于相应不等式中的常数项；中间层到输出层的连接权为1，输出层结点的阈值取为不等式的个数减去0.5后的负数；

该连续网络的运算关系为

$C=f\left(I_O\right)=1/(1+e^{-I_o/T})---\left(1\right)$

$I_O=\underset{m=1}{\overset{M}{\mathrm{\Σ}}}{O}_{\mathrm{Hm}}+{\mathrm{\θ}}_T---\left(2\right)$

$O_{\mathrm{Hm}}=f_{H_m}\left(I_{\mathrm{Hm}}\right)---\left(3\right)$

I_Hm＝w_xmx_i+w_ymy_i+θ_Hm    (4)

其中各符号的含义为，C：顶层结点输出；I_O：顶层结点输入；θ_T：顶层结点阈值；O_Hm：中间层第m个结点的输出；I_Hm：中间层第m个结点的输入；θ_Hm：中间层第m个结点的阈值w_xm，w_ym：第m个不等式限制条件的系数；激发函数为常用的S形函数，即

$f_{\mathrm{Hm}}\left(x\right)=\frac{1}{1+e^{-x/T_{\mathrm{Hm}}}}---\left(5\right)$

模拟退火温度变化公式为：

$T=\frac{\mathrm{\β}}{\log (1+t)}---\left(6\right)$

$T_{\mathrm{Hm}}\left(t\right)=\frac{{\mathrm{\β}}_m}{\log (1+t)}---\left(7\right)$

其中β是模拟退火初始温度，β_m是相应于障碍物每一条边的模拟退火初始温度，可以根据障碍物的形状，设定各边的不同的初始温度，这样对于一些不对称图形可避免其罚函数曲面形成一边倒的情况；

整条路径相应于碰撞函数部分的能量为

$E_C=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\underset{k=1}{\overset{K}{\mathrm{\Σ}}}{C}_i^k---\left(8\right)$

其中K是障碍物的个数，N是路径点的个数，C_i ^k是第i个路径点P(x_i，y_i)对第k个障碍物的碰撞函数；

相应于路径长度部分的能量函数是所有线段长度的平方和，即对所有路径点P(x_i，y_i)，i＝1，2，…，N，

$E_l=\underset{i=1}{\overset{N-1}{\mathrm{\Σ}}}[{(x_{i+1}-x_i)}^2+{(y_{i+1}-y_i)}^2]---\left(9\right)$

整条路径的总能量函数为

E＝w_lE_l+w_cE_c    (10)

其中w_l和w_c分别表示对每一部分的权重(w_l+w_c＝1)；

步骤2：根据出发点坐标P(x₁，y₁)及目标点坐标P(x_N，y_N)，初始路径一般取为出发点到目标点的直线上均匀分布的点列，当x₁≠x_N时

x_i＝x₁+i(x_N-x₁)/(N-1)

y_i＝(y_N-y₁)(x_i-x₁)/(x_N-x₁)+y₁    (i＝2，3，…，N-1)    (11)

步骤3：检测路径的起点和终点是否都在障碍物的中轴线上，根据不同情况分别进行下面两种操作：

(1)若起点和终点至少有一个不在障碍物的中轴线上，则执行步骤4；

(2)若起点和终点都在障碍物的中轴线上，则随机选择一个路径点，改变该路径点坐标后，执行步骤4；

步骤4：对于路径点P(x_i，y_i)，i＝2，3，…，N-1，用检测器检测是否在障碍物内，根据不同情况分别进行下面两种操作：

(1)若路径点P(x_i，y_i)在障碍物内，则按下列运动方程移动

${\stackrel{\·}{x}}_i=-{\mathrm{\η}}_1(2w_l(2x_i-x_{i-1}-x_{i+1})+w_c\underset{k=1}{\overset{K}{\mathrm{\Σ}}}{f}^{\′}\left({\left(I_O\right)}_i^k\right)\left(\underset{m=1}{\overset{M}{\mathrm{\Σ}}}{f}_{H_m}^{\′}\left({\left(I_{H_m}\right)}_i^k\right)w_{\mathrm{xm}}^k\right))$

${\stackrel{\·}{y}}_i=-{\mathrm{\η}}_1(2w_l(2y_i-y_{i-1}-y_{i+1})+w_c\underset{k=1}{\overset{K}{\mathrm{\Σ}}}{f}^{\′}\left({\left(I_O\right)}_i^k\right)\left(\underset{m=1}{\overset{M}{\mathrm{\Σ}}}{f}_{H_m}^{\′}\left({\left(I_{H_m}\right)}_i^k\right)w_{\mathrm{ym}}^k\right))---\left(12\right)$

${\stackrel{\·}{x}}_i=-{\mathrm{\η}}_1(2w_l(2x_i-x_{i-1}-x_{i+1})+w_c{f}^{\′}\left({\left(I_O\right)}_i\right)+f_H^{\′}\left({\left(I_H\right)}_i\right)(P-x_i))$

${\stackrel{\·}{y}}_i=-{\mathrm{\η}}_1(2w_l(2y_i-y_{i-1}-y_{i+1})+w_c{f}^{\′}\left({\left(I_O\right)}_i\right)+f_H^{\′}\left({\left(I_H\right)}_i\right)(Q-y_i))---\left(13\right)$

其中(12)用于P(x_i，y_i)位于多边形的障碍物内的情况，(13)用于P(x_i，y_i)位于圆心在(P，Q)的圆形障碍物内的情况；

(2)若P(x_i，y_i)在障碍物之外，则按下列运动方程移动

${\stackrel{\·}{x}}_i=-{\mathrm{\η}}_2(2x_i-x_{i-1}-x_{i+1})$

${\stackrel{\·}{y}}_i=-{\mathrm{\η}}_2(2y_i-y_{i-1}-y_{i+1})---\left(14\right)$

步骤5：重复执行步骤4，直到路径收敛；

其中上述步骤1-5基于以下假设：

(1)障碍物是二维环境中的多边形围成的平面图形或者是圆形的平面图形；

(2)障碍物为静止的；

(3)计算时障碍物的尺寸按机器人的半径作适当的扩展。

Images