CN101231714A - 机器人三维路径规划方法 - Google Patents

Abstract

本发明涉及自主移动机器人三维路径规划，特别涉及一种基于神经网络能量函数的自主移动机器人三维路径规划算法，定义路径总能量函数为碰撞罚函数与相应于路径长度部分能量函数的加权和，在路径规划的过程中检测路径点的位置是否在障碍物内，根据路径点位于障碍物内外的不同位置用不同的动态运动方程移动路径点的位置，使其朝着使总能量函数的函数值减小的方向运动，最终规划出最短的避障路径。

Description

机器人三维路径规划方法

技术领域

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

背景技术

自主移动机器人路径规划是指机器人在有障碍物的工作环境中，寻找一条从给定起点到终点的运动路径，使机器人能无碰撞地绕过所有的障碍物。机器人路径规划问题可以分为两种，一种是基于环境先验完全信息的全局路径规划，另一种是基于传感器信息的局部路径规划，后者环境是未知或者部分未知的。全局路径规划已提出的典型方法有可视图法、图搜索法、人工势场法等。可视图法的优点是可以求得最短路径，但缺乏灵活性，并且存在组合爆炸问题。图搜索法比较灵活，机器人的起始点和目标点的改变不会造成连通图的重新构造，但不是任何时候都可以获得最短路径。可视图法和图搜索法适用于多边形障碍物的避障路径规划问题，但不适用解决圆形障碍物的避障路径规划问题。人工势场法的基本思想是通过寻找路径点的能量函数的极小值点而使路径避开障碍物，但存在局部极小值问题且不适于寻求最短路径。文献[1]给出的基于神经网络结构能量函数的路径规划算法(称为NA算法)，引入神经网络结构和模拟退火等方法，计算简单，能避免某些局部极值情况，对人工势场法给予了较大的改进。但NA算法仅适用于二维空间，本发明提出一种适用于三维空间的基于神经网络能量函数的路径规划算法。

发明内容

本发明有4点假设：(1)障碍物是三维环境中的多面体。(2)障碍物为静止的。(3)计算时障碍物的尺寸按机器人的半径作适当的扩展。

本发明的目的在于将NA路径规划算法推广到三维空间，使机器人能够在三维空间环境中规划出避障路径，并且使规划的无碰路径达到最短无碰路径。

为实现上述目的，本发明所采用的技术方案是：

定义路径总能量函数为碰撞罚函数与相应于路径长度部分能量函数的加权和，它是各个路径点的函数，通过移动路径点的位置，使其朝着使总能量函数的函数值减小方向运动，最终规划出避障的最短路径。

路径的碰撞罚函数定义为各路径点的碰撞罚函数之和，而一个点的碰撞罚函数是通过它对各个障碍物的神经网络表示得到的，其中模拟退火温度的初始温度，可根据障碍物的形状，设定相应于障碍物各边的不同的初始温度。

设置一个检测路径点位置的检测器：一个神经网络分类器，在路径规划的过程中检测路径点的位置P(x，y，z)，判断该点是否在障碍物内，即是否与障碍物相碰。

输入出发点P(x₁，y₁，z₁)及目标点P(x_N，y_N，z_N)的坐标，对于t＝0，初始路径一般取为出发点到目标点的直线上均匀分布的点列。通过检测器检测路径点P(x_i，y_i，z_i)在障碍物内或者在障碍物外，按照不同的动态运动方程，计算出相应路径点的移动位置。

附图说明

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

图2是用本发明的基于神经网络能量函数的三维路径规划算法进行路径规划的仿真实验结果图。

图3是用本发明的基于神经网络能量函数的三维路径规划算法进行路径规划的仿真实验过程。

具体实施方式

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

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

一条路径的碰撞罚函数定义为各路径点的碰撞罚函数之和，而一个点的碰撞罚函数是通过它对各个障碍物的三层前向神经网络表示得到的。图1表示了一个点到一个障碍物的罚函数的神经网络。输入层三个结点的输入值分别是路径点的横坐标、纵坐标和竖坐标x、y、z，中间层的每个结点相应于障碍物的一个面的不等式限制条件，输入层和中间层的连接权系数等于不等式中x、y、z前面的系数，中间层每个结点的阈值等于相应不等式中的常数项。中间层到输出层的连接权为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_{\mathrm{Hm}}=w_{\mathrm{xm}}{x}_i+w_{\mathrm{ym}}{y}_i+w_{\mathrm{zm}}{z}_i+{\mathrm{\θ}}_{\mathrm{Hm}}---\left(4\right)$

其中各符号的含义为，C：顶层结点输出；I_O：顶层结点输入；θ_T：顶层结点阈值；O_Hm：中间层第m个结点的输出；I_Hm：中间层第m个结点的输入；θ_Hm：中间层第m个结点的阈值；w_xm，w_ym，w_zm：第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_O=\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，z_i)对第k个障碍物的碰撞函数。

相应于路径长度部分的能量函数定义为所有线段长度的平方和，即

$E_i=\underset{i=1}{\overset{N-1}{\mathrm{\Σ}}}[{(x_{i+1}-x_i)}^2+{(y_{i+1}-y_i)}^2+{(z_{i+1}-z_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₁，z₁)及目标点坐标P(x_N，y_N，z_N)，初始路径一般取为出发点到目标点的直线上均匀分布的点列：

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

y_i＝y₁+i(y_N-y₁)/(N-1)

z_i＝z₁+i(z_N-z₁)/(N-1)

(i＝2，3，…，N-1)     (11)

步骤3：对于路径点P(x_i，y_i，z_i)，i＝2，3，…，N-1，用检测器检测是否在障碍物内，

根据不同情况分别进行下面两种操作：

(1)若路径点P(x_i，y_i，z_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))$

${\stackrel{\·}{z}}_i={-\mathrm{\η}}_1({2w}_l({2z}_i-z_{i-1}-z_{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{zm}}^k\right))---\left(12\right)$

(2)若P(x_i，y_i，z_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})$

${\stackrel{\·}{z}}_i={-\mathrm{\η}}_2({2z}_i-z_{i-1}{z}_{i+1})---\left(13\right)$

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

仿真实验

图2是用本发明的基于神经网络能量函数的三维路径规划算法进行路径规划的仿真实验结果图。

图3是用本发明的基于神经网络能量函数的三维路径规划算法进行路径规划的仿真实验过程。

本发明给出了一种基于神经网络能量函数的三维路径规划算法，使机器人能够在三维空间环境中规划出最短避障路径，为自主移动机器人在三维空间的最优路径规划提供了一个简捷有效的方法。

Claims (3)

1.一种机器人三维路径规划方法，其特征在于：解决自主移动机器人在有障碍物的三维空间的路径规划问题，规划出的避障路径达到最短避障路径。

2.根据权利要求1所述的机器人三维路径规划方法，其特征在于：定义路径总能量函数为碰撞罚函数与相应于路径长度部分能量函数的加权和，它是各个路径点的函数，通过移动路径点的位置，使其朝着使总能量函数的函数值减小方向运动，最终规划出避障的最短路径，

路径的碰撞罚函数定义为各路径点的碰撞罚函数之和，而一个点的碰撞罚函数是通过它对各个障碍物的神经网络表示得到的，其中模拟退火温度的初始温度，可根据障碍物的形状，设定相应于障碍物各面的不同的初始温度，

设置一个检测路径点位置的检测器：一个神经网络分类器，在路径规划的过程中检测路径点的位置，判断其是否在障碍物内，即是否与障碍物相碰，

初始路径一般取为出发点到目标点的直线上均匀分布的点列，通过检测路径点在障碍物内或者在障碍物外，按照不同的动态运动方程，计算出相应路径点的移动位置。

3.根据权利要求1所述的机器人三维路径规划方法，其特征在于：基于神经网络能量函数的三维路径规划算法的路径规划具体实施方法分为如下几个步骤进行：

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

一条路径的碰撞罚函数定义为各路径点的碰撞罚函数之和，而一个点的碰撞罚函数是通过它对各个障碍物的三层前向神经网络表示得到的，输入层的三个结点的输入分别是路径点的横坐标、纵坐标和竖坐标x、y、z，中间层的每个结点相应于障碍物的一个面的不等式限制条件，输入层和中间层的连接权系数等于不等式中x、y、z前面的系数，中间层每个结点的阈值等于相应不等式中的常数项。中间层到输出层的连接权为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_{\mathrm{Hm}}=w_{\mathrm{xm}}{x}_i+w_{\mathrm{ym}}{y}_i+w_{\mathrm{zm}}{z}_i+{\mathrm{\θ}}_{\mathrm{Hm}}---\left(4\right)$

其中各符号的含义为，C：顶层结点输出；I_O：顶层结点输入；θ_T：顶层结点阈值；O_Hm：中间层第m个结点的输出；I_Hm：中间层第m个结点的输入；θ_Hm：中间层第m个结点的阈值；w_xm，w_ym，w_zm：第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_O=\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，z_i)对第k个障碍物的碰撞函数，

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

$E_i=\underset{i=1}{\overset{N-1}{\mathrm{\Σ}}}[{(x_{i+1}-x_i)}^2+{(y_{i+1}-y_i)}^2+{(z_{i+1}-z_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₁，z₁)及目标点坐标P(x_N，y_N，z_N)，初始路径一般取为出发点到目标点的直线上均匀分布的点列：

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

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

z_i＝z₁+i(z_N-z₁)/(N-1)

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

(1)若路径点P(x_i，y_i，z_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))$

${\stackrel{\·}{z}}_i={-\mathrm{\η}}_1({2w}_l({2z}_i-z_{i-1}-z_{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{zm}}^k\right))$

$(i=\mathrm{2,3},\·\·\·,N-1)---\left(12\right)$

(2)若P(x_i，y_i，z_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})$

${\stackrel{\·}{z}}_i={-\mathrm{\η}}_2({2z}_i-z_{i-1}{z}_{i+1})$

$(i=\mathrm{2,3},\·\·\·,N-1)---\left(13\right)$

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

Images