[发明专利]机器人三维路径规划方法

权利要求书

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

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

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

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

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

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

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

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

该连续网络的运算关系为

C=f(IO)=1/(1+e-Io/T)---(1)]]>

IO=Σm=1MOHm+θT---(2)]]>

OHm=fHm(IHm)---(3)]]>

IHm=wxmxi+wymyi+wzmzi+θHm---(4)]]>

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

fHm(x)=11+e-x/THm---(5)]]>

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

T=βlog(1+t)---(6)]]>

THm(t)=βmlog(1+t)---(7)]]>

其中β是模拟退火初始温度。β_m是相应于障碍物每一个面的模拟退火初始温度，可以根据障碍物的形状，设定各面的不同的初始温度，这样对于一些不对称图形可避免路径规划的局部极小值情况，

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

EO=Σi=1NΣk=1KCik---(8)]]>

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

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

Ei=Σi=1N-1[(xi+1-xi)2+(yi+1-yi)2+(zi+1-zi)2]---(9)]]>

整条路径的总能量函数定义为

                  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)在障碍物内，则按下列运动方程移动

x·i=-η1(2wl(2xi-xi-1-xi+1)+wcΣk=1Kf′((IO)ik)(Σm=1MfHm′((IHm)ik)wxmk))]]>

y·i=-η1(2wl(2yi-yi-1-yi+1)+wcΣk=1Kf′((IO)ik)(Σm=1MfHm′((IHm)ik)wymk))]]>

z·i=-η1(2wl(2zi-zi-1-zi+1)+wcΣk=1Kf′((IO)ik)(Σm=1MfHm′((IHm)ik)wzmk))]]>

(i=2,3,···,N-1)---(12)]]>

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

x·i=-η2(2xi-xi-1-xi+1)]]>

y·i=-η2(2yi-yi-1-yi+1)]]>

z·i=-η2(2zi-zi-1zi+1)]]>

(i=2,3,···,N-1)---(13)]]>

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