一种基于弹性绳运动性质的三维空间路径规划方法

摘要

本发明提出一种基于弹性绳运动性质的三维空间路径规划方法，包括初始化寻回环境和参数、计算弹性绳上的点受弹性绳作用引起的位移、计算弹性绳上的点受弹性绳和障碍物作用引起的位移、计算移动后的位置、判断弹性绳上所有点均是否均发生了一次运动和判断弹性绳是否达到平衡位置等几个步骤。本发明提出的一种基于弹性绳运动性质的三维空间路径规划方法，相对于其它路径规划方法运算量小，寻路速度快；环境表示简单，只要知道障碍物表面指向障碍物外的法线方向即可，且不需要将空间分割成单元格的形式，所找到的路径平滑性，准确性更好。

主权项

1.一种基于弹性绳运动性质的三维空间路径规划方法，其特征在于：具体包括以下几个步骤：步骤一：设弹性绳上点为{A_i}，发生一次移动后为{A′_i}，(i＝1，2，...n)，n为弹性绳上点的个数，n≥3；空间中起点和终点的坐标分别为A₁、A_n，起点和终点的位移始终为零；障碍物的球心和半径分别为{O_j}，{R_j}，(j＝1，2，...m)，障碍物的个数m≥0，k为阻力系数；判断弹性绳和障碍物是否相邻的阀值参数为ξ，判断是否平衡的迭代停止阀值为ζ；步骤二：令A₁′＝A₁，A_n′＝A_n；弹性绳上当前运动点A_i的下标i＝2，弹性绳从第二个点开始运动；步骤三：根据公式计算A_i点受到弹性绳的作用所引起的位移其中k为阻力系数，A_i-1′表示A_i-1一次移动后的点，A_i在A_i+1的引力作用下的位移为A_i在A_i-1的引力作用下的位移为令表示障碍物的下标j＝1，计算A_i点与障碍物O₁的作用；步骤四：判断A_i点是否在障碍物内O_j，若成立，则A_i点在障碍物内，计算A_i点弹性绳与障碍物O_j共同作用下的合位移为：$\overrightarrow{A_i{A}_i^{\prime }}=\overrightarrow{S_{A_i}}+\overrightarrow{S_{O_j{A}_i}^{\prime }}+\overrightarrow{S_{O_j{A}_i}^{\prime \prime }}+\overrightarrow{S_{O_j{A}_i}^{\prime \prime \prime }}$$=\overrightarrow{S_{A_i}}+\overrightarrow{S_{O_j{A}_i}^{\prime \prime \prime }}$$=\overrightarrow{S_{A_i}}+\frac{\overrightarrow{O_j{A}_i}}{\left|\overrightarrow{O_j{A}_i}\right|}*L^{\prime }$其中然后转入步骤八，表示障碍物球心到A_i的距离，为寻路起点到终点的距离；若不成立，则A_i点不在障碍物内，进入步骤五；其中，$\overrightarrow{S_{O_j{A}_i}^{\prime \prime \prime }}=\frac{\overrightarrow{O_j{A}_i}}{\left|\overrightarrow{O_j{A}_i}\right|}*L^{\prime }*g\left(\right|\overrightarrow{O_j{A}_i}|-R_j),g\left(x\right)=\left\{\begin{array}{cc}0& x\ge 0\\ 1& x<0\end{array}\right.;$$\overrightarrow{S_{O_j{A}_i}^{\prime \prime }}=-\frac{\overrightarrow{O_j{A}_i}}{{\left|\overrightarrow{O_j{A}_i}\right|}^2}*(\overrightarrow{S_{A_i}}·\overrightarrow{O_j{A}_i})*f(\overrightarrow{S_{A_i}}·\overrightarrow{O_j{A}_i})*f(R_j-|\overrightarrow{O_j{A}_i}\left|\right)*f\left(\right|\overrightarrow{O_j{A}_i}|-R_j-\xi ),f\left(x\right)=\left\{\begin{array}{cc}0& x>0\\ 1& x\le 0\end{array}\right.;$$\overrightarrow{S_{O_j{A}_i}^{\prime }}=\frac{\overrightarrow{S_{A_i}}}{\left|\overrightarrow{S_{A_i}}\right|}*(L-|\overrightarrow{S_{A_i}}|-\xi )*g(R_j+\xi -|\overrightarrow{O_j{A}_i}\left|\right)*g\left(\right|\overrightarrow{O_j{A}_i}+\overrightarrow{S_{A_i}}|-R_j),g\left(x\right)=\left\{\begin{array}{cc}0& x\ge 0\\ 1& x<0\end{array}\right.;$步骤五：判断A_i点是否在障碍物表面，若成立，则A_i点在障碍物表面，此时若有成立，则计算A_i点在弹性绳与障碍物O_j共同作用下的合位移其中ξ为用于判断弹性绳和障碍物是否相邻的阀值参数；$\overrightarrow{A_i{A}_i^{\prime }}=\overrightarrow{S_{A_i}}+\overrightarrow{S_{O_j{A}_i}^{\prime }}+\overrightarrow{S_{O_j{A}_i}^{\prime \prime }}+\overrightarrow{S_{O_j{A}_i}^{\prime \prime \prime }}$计算得到A_i点在弹性绳与障碍物O_j共同作用下的合位移$=\overrightarrow{S_{A_i}}-\frac{\overrightarrow{O_j{A}_i}}{{\left|\overrightarrow{O_j{A}_i}\right|}^2}*(\overrightarrow{S_{A_i}}·\overrightarrow{O_j{A}_i})$然后进入步骤八；若不成立，则直接进入步骤七；若不成立，则A_i点不在障碍物表面，进入步骤六；步骤六：判断A_i点受到弹性绳的作用所引起的位移下是否会运动到障碍物O_j内，若成立，则A_i点会运动到障碍物O_j内，则计算A_i点在弹性绳与障碍物O_j共同作用下的合位移$\overrightarrow{A_i{A}_i^{\prime }}=\overrightarrow{S_{A_i}}+\overrightarrow{S_{O_j{A}_i}^{\prime }}+\overrightarrow{S_{O_j{A}_i}^{\prime \prime }}+\overrightarrow{S_{O_j{A}_i}^{\prime \prime \prime }}$$=\overrightarrow{S_{A_i}}+\overrightarrow{S_{O_j{A}_i}^{\prime }}$$=\overrightarrow{S_{A_i}}+\frac{\overrightarrow{S_{A_i}}}{\left|\overrightarrow{S_{A_i}}\right|}*(L-\overrightarrow{S_{A_i}}-\xi )$$=\frac{\overrightarrow{S_{A_i}}}{\left|\overrightarrow{S_{A_i}}\right|}*(L-\xi )$其中$L=-\frac{\overrightarrow{S_{A_i}}·\overrightarrow{O_j{A}_i}}{\left|\overrightarrow{S_{A_i}}\right|}-\sqrt{{\left(\frac{\overrightarrow{S_{A_i}}·\overrightarrow{O_j{A}_i}}{\left|\overrightarrow{S_{A_i}}\right|}\right)}^2-{\left|\overrightarrow{O_j{A}_i}\right|}^2+R_j^2},$计算A_i点在弹性绳与障碍物O_j共同作用下的合位移进入步骤八；若不成立，则说明A_i点不会运动到障碍物O_j内，直接进入步骤七；步骤七：A_i点仅受弹性绳的作用而不受障碍物的作用，A_i点在弹性绳与障碍物O_j共同作用下的合位移然后令表示障碍物的下标j进行加1，再判断j是否大于m，若大于m，进入步骤八，否则返回步骤四；步骤八：计算A_i点移动后的位置令当前运动点A_i的下标i进行加1，并判断i是否大于等于n，若i大于等于n，进入步骤九，否则返回步骤三；步骤九：判断弹性绳是否达到平衡位置，若成立，则说弹性绳达到平衡位置，停止寻路，此时弹性绳所处的位置即为所要寻找的路径，其中ζ为判断平衡的迭代停止阀值，否则没有达到平衡位置，令A_i＝A_i′(i＝1，2，...，n)，并返回步骤二。