CN104331080B - 用于移动式机器人的定点跟踪路径规划方法 - Google Patents

Abstract

本发明公开了一种用于移动式机器人的定点跟踪路径规划方法，步骤包括：确定移动式机器人所需经过的关键点，确定初始斜率值范围并初始化路径最小值为0；从初始斜率值范围中搜索选择一个斜率作为当前初始斜率，求解各组相邻关键点之间的二次曲线，分别基于不同的当前初始斜率计算各组相邻关键点之间的二次曲线按顺序连接所组成路径曲线的总长度，选择总长度最小的路径曲线经离散化处理，最终得到移动式机器人的实际轨迹曲线的离散点坐标和速度。本发明利用少量关键位置信息作为输入即可通过二次曲线拟合完成在动态情况下的路径寻优，兼具快速性和准确性，对机器人传感器和处理器要求低。

Description

用于移动式机器人的定点跟踪路径规划方法

技术领域

本发明涉及移动式机器人的定点跟踪路径规划技术，具体涉及一种用于移动式机器人的定点跟踪路径规划方法。

背景技术

自20世纪60年代初问世以来，机器人技术得到迅速发展。机器人技术是综合了计算机、控制论、信息处理、传感技术、通信、导航、人工智能、仿生学等多学科而形成的高新技术。随着机器人技术的不断进步，机器人学科越来越具有强大的生命力，在某种程度上已经代表当今信息技术、自动化技术、系统集成等技术的最新发展，是典型的高新技术综合体。

路径规划是移动式机器人的一个重要问题。移动式机器人的路径规划是给定机器人及其工作环境信息，按照某种优化指标，寻求有界输入使系统在规定的时间内从起始点转移到目标点。它的目标是在一个存在障碍物的环境中，为移动式机器人寻找一条无碰撞路径。其主要研究内容按机器人工作环境不同可分为静态结构化环境、动态已知环境和动态不确定环境，按机器人获取信息方式的不同可分为基于模型的路径规划和基于传感器的路径规划，按环境信息已知程度可分为全局路径规划和局部路径规划。机器人路径规划的研究始于20世纪70年代，目前对这一问题的研究仍十分活跃，许多学者做了大量工作。对全局路径规划，比较常见的方法有拓扑法、可视图法、栅格法和层次分解法，而势场法、模糊逻辑法、神经网络法则适用于局部路径规划。现有方法存在建模和运算复杂、实时性较差的缺点。

发明内容

本发明要解决的技术问题是：针对现有技术的上述技术问题，提供一种利用少量关键位置信息作为输入即可通过二次曲线拟合完成在动态情况下的路径寻优，兼具快速性和准确性，对机器人传感器和处理器要求低的用于移动式机器人的定点跟踪路径规划方法。

为了解决上述技术问题，本发明采用的技术方案为：

一种用于移动式机器人的定点跟踪路径规划方法，步骤包括：

1)确定移动式机器人在二维空间中所需经过的关键点，确定初始斜率值范围并初始化路径最小值为0；

2)从所述初始斜率值范围中搜索选择一个斜率作为当前初始斜率；

3)基于所述当前初始斜率求解各组相邻关键点之间的二次曲线；

4)计算各组相邻关键点之间的二次曲线按顺序连接所组成路径曲线的总长度；如果所述路径最小值为0或者当前初始斜率下路径曲线的总长度小于路径最小值，则将所述路径最小值的值更新设置为当前初始斜率下路径曲线的总长度，记录当前初始斜率下的路径曲线，跳转执行步骤5)；否则，直接跳转执行步骤5)；

5)判断斜率搜索范围中的斜率是否搜索完毕，如果尚未搜索完毕，则跳转执行步骤2)；否则，则输出记录的路径曲线，并将所述记录的路径曲线经离散化处理，最终得到移动式机器人的实际轨迹曲线的离散点。

优选地，所述步骤3)的详细步骤如下：

3.1)定义依次经过移动式机器人在二维空间中所需经过的关键点的总插值曲线，所述总插值曲线为二次曲线，所述总插值曲线由各组相邻关键点之间的二次曲线按顺序连接组成；

3.2)基于所述当前初始斜率确定各个关键点的斜率；

3.3)采用二次样条插值的方法，基于相邻关键点的坐标、相邻关键点中一个关键点的斜率确定每一组相邻关键点之间的二次曲线常数项系数、一次项系数以及二次项系数，从而确定每一组相邻关键点之间的二次曲线。

优选地，所述步骤3.1)中的总插值曲线的表达式如式(1)所示；

y＝a(i,1)*x²+a(i,2)*x+a(i,3)            (1)

式(1)中，a(i,1)为二次项系数，a(i,2)为一次项系数，a(i,3)为常数项系数。

优选地，所述步骤3.2)中具体是指在当前初始斜率的基础上，根据式(2)迭代确定各个关键点的斜率；

$z(i+1)=-z\left(i\right)+2*\frac{y(i+1)-y\left(i\right)}{x(i+1)-x\left(i\right)}---\left(2\right)$

式(2)中，z(i+1)表示第i+1个关键点的斜率，z(i)表示第i个关键点的斜率，y(i+1)表示第i+1个关键点的纵坐标，y(i)表示第i个关键点的纵坐标，x(i+1)表示第i+1个关键点的横坐标，x(i)表示第i个关键点的横坐标。

优选地，所述步骤3.3)中确定的每一组相邻关键点之间的二次曲线常数项系数的值为该二次曲线起始关键点的纵坐标，一次项系数的值为该二次曲线起始关键点的斜率，二次项系数的计算表达式如式(3)所示；

$a(i,1)=0.5*\frac{z(i+1)-z\left(i\right)}{x(i+1)-x\left(i\right)}---\left(3\right)$

式(3)中，a(i,1)表示二次曲线的二次项系数，z(i+1)表示该二次曲线结束关键点的斜率，z(i)表示该二次曲线起始关键点的斜率，x(i+1)表示该二次曲线结束关键点的横坐标，x(i)表示该二次曲线起始关键点的横坐标。

优选地，所述步骤4)中计算各组相邻关键点之间的二次曲线按顺序连接所组成路径曲线的总长度时，具体是指根据式(4)计算路径曲线的总长度；

$lengthSum=\mathrm{\Σ}{\∫}_i\frac{1}{\sqrt{1+yi{\left(x\right)}^{\′}*yi{\left(x\right)}^{\′}}}dx---\left(4\right)$

式(4)中，lengthSum表示各个相邻关键点之间的二次曲线依次相连得到连续曲线的总长度，yi(x)表示第i组相邻关键点之间的二次曲线函数值，yi(x)′表示第i组相邻关键点之间的二次曲线的微分值。

优选地，所述步骤5)将记录的路径曲线经离散化处理具体是指根据式(5)所示的递推函数将路径曲线经离散化处理；

$\left\{\begin{array}{c}x\[n+1\]=T*V\[n\]*\frac{1}{\sqrt{1+K\[n\]*K\[n\]}}+x\[n\]\\ y\[n+1\]=a(i,1)*x{\[n+1\]}^2+a(i,2)*x\[n+1\]+a(i,3)\end{array}\right.---\left(5\right)$

式(5)中，(x[n],y[n])表示递推得到的第n个离散点的坐标，(x[n+1],y[n+1])表示第n+1个离散点的坐标，T表示相邻离散点间隔的步长，V[n]表示移动式机器人经过第n个离散点的速度，x[n]表示第n个离散点的横坐标，a(i,3)表示第n个离散点对应二次曲线的常数项系数，a(i,2)表示第n个离散点对应二次曲线的一次项系数，a(i,1)表示第n个离散点对应二次曲线的二次项系数；K[n]表示移动式机器人经过第n个离散点的速度，K[n]的表达式如式(6)所示；

K[n]＝2*a(i,1)*x[n]+a(i,2)        (6)

式(6)中，a(i,1)表示第n个离散点对应二次曲线的二次项系数，x[n]表示第n个离散点的横坐标，a(i,2)表示第n个离散点对应二次曲线的一次项系数。

本发明用于移动式机器人的定点跟踪路径规划方法具有下述优点：

1、针对现有技术的定点跟踪路径规划方法对环境感知要求较高、所需要的传感器信息较多而且依赖于传感器的检测信息，需要复杂的建模和运算过程，存在实时性较差的缺点，本发明针对常规环境运行，利用少量关键位置信息作为输入，基于所述当前初始斜率求解各组相邻关键点之间的二次曲线，获取总长度最小的各组相邻关键点之间的二次曲线按顺序连接所组成路径曲线，通过二次曲线拟合的方法即可完成在动态情况下的路径寻优，兼具快速性和准确性，降低了对机器人传感器和处理器的要求，具备过程简单，容易实现，鲁棒性好，求解效率高等优点。

2、针对现有技术的定点跟踪路径规划方法一般对环境感知要求较高，所需要的传感器信息较多的问题，而本发明用于移动式机器人的定点跟踪路径规划方法利用少量关键位置信息作为输入即可通过二次曲线拟合完成在动态情况下的路径寻优，输入数据信息量少，对机器人传感器要求低，解决了移动机器人在位姿信息较少情况下，如何在动态情况下仅通过几个关键位置信息快速实现移动式机器人的路径规划的问题，为移动式机器人路径规划提供了一个简捷有效的方法。

附图说明

图1为本发明实施例方法的基本流程示意图。

图2为本发明实施例得到的绘制总曲线长度与初始斜率关系示意图。

图3为本发明实施例得到的实际轨迹曲线上各个离散点的坐标位置示意图。

具体实施方式

如图1所示，本实施例用于移动式机器人的定点跟踪路径规划方法的步骤包括：

1)确定移动式机器人在二维空间中所需经过的关键点，确定初始斜率值范围并初始化路径最小值为0；本实施例中将移动式机器人简化成一个在二维空间中运动的点，仅仅要求移动式机器人能够通过传感器能感知当前位置和障碍物位置，能够确定移动式机器人需要通过的关键点的位置即可，输入数据信息量少，对机器人传感器要求低，解决了移动机器人在位姿信息较少情况下，如何在动态情况下仅通过几个关键位置信息快速实现路径规划的问题，为移动式机器人路径规划提供了一个简捷有效的方法。

2)从初始斜率值范围中搜索选择一个斜率作为当前初始斜率；需要说明的是，初始斜率值范围可以根据需要设置为数组值或者阈值的形式，本实施例中初始斜率值范围具体为数组，例如取值为数组{-0.4，-0.3，-0.2，-0.1，0，0.1，0.2，0.3，0.4，0.5，0.6，0.7，0.8}所示的形式，数组中的每一个元素则代表一个具体的斜率值。因此，步骤2)～步骤4)实质上位针对每一个数组中的元素(当前初始斜率)所进行的遍历过程。

3)基于当前初始斜率求解各组相邻关键点之间的二次曲线。

本实施例中，步骤3)的详细步骤如下：

3.1)定义依次经过移动式机器人在二维空间中所需经过的关键点的总插值曲线，总插值曲线为二次曲线，总插值曲线由各组相邻关键点之间的二次曲线按顺序连接组成；

3.2)基于当前初始斜率确定各个关键点的斜率；

3.3)采用二次样条插值的方法，基于相邻关键点的坐标、相邻关键点中一个关键点的斜率确定每一组相邻关键点之间的二次曲线常数项系数、一次项系数以及二次项系数，从而确定每一组相邻关键点之间的二次曲线。

本实施例采用迭代的方法求出需要机器人通过的相邻两个关键点之间的二次曲线，首先计算出第i个关键点和第i+1个关键点之间的二次曲线，接着再求出第i+1个关键点和第i+2个关键点之间的二次曲线，以此往下，最终得出所有相邻关键点间的二次曲线。

本实施例中，步骤3.1)中的总插值曲线的表达式如式(1)所示；

y＝a(i,1)*x²+a(i,2)*x+a(i,3)        (1)

式(1)中，a(i,1)为二次项系数，a(i,2)为一次项系数，a(i,3)为常数项系数。

本实施例中，步骤3.2)中具体是指在当前初始斜率的基础上，根据式(2)迭代确定各个关键点的斜率；

$z(i+1)=-z\left(i\right)+2*\frac{y(i+1)-y\left(i\right)}{x(i+1)-x\left(i\right)}---\left(2\right)$

式(2)中，z(i+1)表示第i+1个关键点的斜率，z(i)表示第i个关键点的斜率，y(i+1)表示第i+1个关键点的纵坐标，y(i)表示第i个关键点的纵坐标，x(i+1)表示第i+1个关键点的横坐标，x(i)表示第i个关键点的横坐标。

本实施例中，步骤3.3)中确定的每一组相邻关键点之间的二次曲线常数项系数a(i,3)的值为该二次曲线起始关键点的纵坐标y(i)，一次项系数a(i,2)的值为该二次曲线起始关键点的斜率z(i)，二次项系数a(i,1)的计算表达式如式(3)所示；

$a(i,1)=0.5*\frac{z(i+1)-z\left(i\right)}{x(i+1)-x\left(i\right)}---\left(3\right)$

式(3)中，a(i,1)表示二次曲线的二次项系数，z(i+1)表示该二次曲线结束关键点的斜率，z(i)表示该二次曲线起始关键点的斜率，x(i+1)表示该二次曲线结束关键点的横坐标，x(i)表示该二次曲线起始关键点的横坐标。基于表达式如式(1)所示总插值曲线的二次样条插值的过程就是求出连接各给定点的多段二次曲线的过程。由于确定一条二次曲线需要三个独立的条件，在已知相邻两个给定点的条件下，还需要第三个条件，本方法给出的第三个条件为二次曲线在一个给定点处的斜率。假设给定三个期望机器人通过的关键点坐标(x(1),y(1))、(x(2),y(2))、(x(3),y(3))＝(10,14),(36,28),(60,59)，其中x(1)<x(2)<x(3)，假设在第i个和i+1个(i＝1,2)关键点之间的二次曲线如式(2)所示。假设二次曲线在第i个点处的斜率为z(i)＝z，则二次曲线在第i+1个点处的斜率可以根据式(2)迭代确定。例如取初始条件(当前初始斜率)为z(1)＝1，则求得z(2)＝0.0769，z(3)＝2.5064，进而求出两段二次曲线的参数为：a(1,1)＝-0.0178，a(1,2)＝1，a(1,3)＝14，a(2,1)＝0.0506，a(2,2)＝0.0769，a(2,3)＝28；因此路径曲线中第一段二次曲线为y1(x)＝-0.0178*x²+1*x+14，第二段二次曲线为y2(x)＝0.0506*x²+0.0769*x+28。

4)计算各组相邻关键点之间的二次曲线按顺序连接所组成路径曲线的总长度；如果路径最小值为0或者当前初始斜率下路径曲线的总长度小于路径最小值，则将路径最小值的值更新设置为当前初始斜率下路径曲线的总长度，记录当前初始斜率下的路径曲线，跳转执行步骤5)；否则，直接跳转执行步骤5)。

本实施例中，步骤4)中计算各组相邻关键点之间的二次曲线按顺序连接所组成路径曲线的总长度时，具体是指根据式(4)计算路径曲线的总长度；

$lengthSum=\mathrm{\&Sigma;}{\&Integral;}_i\frac{1}{\sqrt{1+yi{\left(x\right)}^{\&prime;}*yi{\left(x\right)}^{\&prime;}}}dx---\left(4\right)$

式(4)中，lengthSum表示各个相邻关键点之间的二次曲线依次相连得到连续曲线的总长度，yi(x)表示第i组相邻关键点之间的二次曲线函数值，yi(x)′表示第i组相邻关键点之间的二次曲线的微分值。

计算各组相邻关键点之间的二次曲线按顺序连接所组成路径曲线的总长度需要求解式(4)所示的目标函数。各组相邻关键点之间的二次曲线按顺序连接所组成路径曲线由每一段二次曲线的长度累积得到，参见前文可知，通过二次样条插值，移动式机器人的路径被规划成了分段的二次曲线，曲线方程已知且较简单，可以通过积分的方法求出路径的总长度，在同一个点处，给定不同的初始条件(第一个关键点的斜率)，则规划出不同的二次曲线，找出路径总长度最短的一条即可。取初始条件为二次曲线在第一个给定点处的斜率z(1)＝z；假设在此条件下第i段二次曲线的方程为：y＝yi(x)＝a(i,1)*x²+a(i,2)*x+a(i,3)，则第i段二次曲线的微分如(4-1)所示，那么第i段二次曲线的长度length_i如(4-2)所示，

$\frac{dyi\left(x\right)}{dx}=yi{\left(x\right)}^{\&prime;}=2a(i,1)*x+a(i,2)---(4-1)$

式(4-1)中，a(i,1)为二次项系数，a(i,2)为一次项系数。

$\begin{array}{c}{\mathrm{length}}_i={\&Integral;}_i\frac{1}{\sqrt{1+yi{\left(x\right)}^{\&prime;}*yi{\left(x\right)}^{\&prime;}}}dx\\ ={\&Integral;}_i\frac{1}{\sqrt{1+{\left(2a\right(i,1)*x+a(i,2\left)\right)}^2}}dx\\ =\frac{\ln \left(2a\right(i,1)*x+a(i,2)=\sqrt{1+{\left(2a\right(i,1)*x+a(i,2\left)\right)}^2})}{2a(i,1)}{|}_{x\left(i\right)}^{x(i+1)}\end{array}---(4-2)$

式(4-2)中，a(i,1)为二次项系数，a(i,2)为一次项系数，yi(x)′表示第i段二次曲线yi(x)的微分。式(4-2)所示的求积分的过程运用了基本积分公式(4-3)。

$\&Integral;\frac{1}{\sqrt{x*x+a*a}}dx=\ln (x+\sqrt{x*x+a*a})+C---(4-3)$

基本积分公式(4-3)中，C为常数。

在得到第i段二次曲线的长度length_i的基础上，各组相邻关键点之间的二次曲线按顺序连接所组成路径曲线的总长度可表达为式(4-4)所示。

$lengthSum\left(z\right)=\underset{i}{\&Sigma;}{\mathrm{length}}_i---(4-4)$

式(4-4)中，lengthSum(z)表示各组相邻关键点之间的二次曲线按顺序连接所组成路径曲线的总长度，lengthSum(z)中的z表示该总长度lengthSum(z)与当前初始斜率z相关，在不同当前初始斜率z取值下，lengthSum(z)分别会有不同的取值。取不同的当前初始斜率z，绘制lengthSum(z)_z曲线，找到lengthSum(z)最小时对应的z即可找到最优路径曲线。根据实际需求，最优路径曲线必须经过给定的关键点，最优路径曲线的导数必须连续，最优路径曲线的方程尽量简单；本实施例采用的样条插值规划方法能够满足上述要求，而之所以选择二次样条差值是因为，三次及高次样条插值所得的曲线方程较复杂，不适合运用于本方法的跟踪算法，而二次样条插值基本能够满足实际需求，所以本实施例采用二次样条插值进行路径规划。例如，分别取当前初始斜率z＝-0.4，-0.3，-0.2，-0.1，0，0.1，0.2，0.3，0.4，0.5，0.6，0.7，0.8；运用上述算法求得相应的路径曲线的总长度分别为36.1305，33.9604，32.1955，30.9070，30.0308，29.4864，29.2049，29.1293，29.2091，29.3930，29.6224，29.8299，29.9447，此时可以绘制得到如图2所示的规划曲线的长度(路径曲线的总长度)与初始斜率关系图，可以求出当前初始斜率z＝0.3时，路径曲线长度最小值为29.1293，此路径曲线即为最优路径曲线，在此初始条件(当前初始斜率z)下，最优路径曲线第一段二次曲线为：y1(x)＝0.0092*x²+0.3*x+14；最优路径曲线的第二段二次曲线为：y2(x)＝0.0214*x²+0.7769*x+28。

5)判断斜率搜索范围中的斜率是否搜索完毕，如果尚未搜索完毕，则跳转执行步骤2)。否则，则输出记录的路径曲线，并将记录的路径曲线经离散化处理，最终得到移动式机器人的实际轨迹曲线的离散点。

本实施例中，步骤5)将记录的路径曲线经离散化处理具体是指根据式(5)所示的递推函数将路径曲线经离散化处理；

$\left\{\begin{array}{c}x\&lsqb;n+1\&rsqb;=T*V\&lsqb;n\&rsqb;*\frac{1}{\sqrt{1+K\&lsqb;n\&rsqb;*K\&lsqb;n\&rsqb;}}+x\&lsqb;n\&rsqb;\\ y\&lsqb;n+1\&rsqb;=a(i,1)*x{\&lsqb;n+1\&rsqb;}^2+a(i,2)*x\&lsqb;n+1\&rsqb;+a(i,3)\end{array}\right.---\left(5\right)$

式(5)中，(x[n],y[n])表示递推得到的第n个离散点的坐标，(x[n+1],y[n+1])表示第n+1个离散点的坐标，T表示相邻离散点间隔的步长，V[n]表示移动式机器人经过第n个离散点的速度，x[n]表示第n个离散点的横坐标，a(i,3)表示第n个离散点对应二次曲线的常数项系数，a(i,2)表示第n个离散点对应二次曲线的一次项系数，a(i,1)表示第n个离散点对应二次曲线的二次项系数；K[n]表示移动式机器人经过第n个离散点的斜率，K[n]的表达式如式(6)所示；

K[n]＝2*a(i,1)*x[n]+a(i,2)        (6)

式(6)中，a(i,1)表示第n个离散点对应二次曲线的二次项系数，x[n]表示第n个离散点的横坐标，a(i,2)表示第n个离散点对应二次曲线的一次项系数。

需要说的是，机器人的路径和轨迹是两个不同的概念，路径是一条几何曲线，而轨迹则是以时间为参数的曲线。一旦运动的轨迹确定，机器人运动的时间，以及在各个时间点的状态也就确定了。也就是说路径只包含了机器人运动的位置、方向等信息，而轨迹还包含了速度、加速度和运动时间等信息，轨迹包含的运动信息更加全面。因此，轨迹规划一般较复杂，要在规划出路径曲线后找出路径上的点和时间的关系。假设规划所得的路径曲线为y(x)，令y对时间t求导则有式(5-1)，令x对时间t求导则有式(5-2)。

$\frac{dy}{dt}=\frac{dy}{dx}*\frac{dx}{dt}---(5-1)$

$\frac{dx}{dt}=V_x---(5-2)$

由速度分解关系式(5-3)，其中V和K如式(5-4)所示。

$V_x=V*\frac{1}{\sqrt{1+K*K}}---(5-3)$

$V=\sqrt{V_x*V_x+V_y*V_y},K=\frac{dy}{dx}---(5-4)$

式(5-3)和式(5-4)中，V_x表示速度在x轴方向的分量，V_y表示速度在y轴方向的分量。

结合上述(5-1)～(5-4)，可以得出式(5-5)。

$\left\{\begin{array}{c}\frac{dy}{dt}=K*V*\frac{1}{\sqrt{1+K*K}}\\ \frac{dx}{dt}=V*\frac{1}{\sqrt{1+K*K}}\end{array}\right.---(5-5)$

对规划的轨迹曲线离散化处理时，取时间步长为T，由于时间步长T较小，所以可以近似认为在T时间内机器人的速度不变，可得递推公式如式(5-6)所示。

$\frac{x\&lsqb;n+1\&rsqb;-x\&lsqb;n\&rsqb;}{T}=V\&lsqb;n\&rsqb;*\frac{1}{\sqrt{1+K\&lsqb;n\&rsqb;*K\&lsqb;n\&rsqb;}}---(5-6)$

式(5-6)中，x[n+1]表示第n+1个离散点的横坐标，x[n]表示第n个离散点的横坐标，T表示表示相邻离散点间隔的步长，V[n]表示表示第n个离散点的速度，K[n]表示移动式机器人经过第n个离散点的斜率，即为式(5-7)。

$K\&lsqb;n\&rsqb;=\frac{dy}{dx}{|}_{x=x\&lsqb;n\&rsqb;}---(5-7)$

式(5-7)中，x[n]表示第n个离散点的横坐标。

由于y[n+1]＝y(x)|_x＝x[n+1]必然成立，则有式(5-8)。

$\left\{\begin{array}{c}x\&lsqb;n+1\&rsqb;=T*V\&lsqb;n\&rsqb;*\frac{1}{\sqrt{1+K\&lsqb;n\&rsqb;*K\&lsqb;n\&rsqb;}}+x\&lsqb;n\&rsqb;\\ y\&lsqb;n+1\&rsqb;=y\left(x\right){|}_{x=x\&lsqb;n+1\&rsqb;}\end{array}\right.---(5-8)$

由规划得到的二次曲线满足y＝a(i,1)*x²+a(i,2)*x+a(i,3)，因此可以推到得到式(5)。

经过处理后，离散化后的路径曲线上各点的位置坐标、速度等信息都与时间对应了，即获得了各时刻各机器人的状态信息。这种近似处理会产生一定误差，但是由于T取得较小，机器人运动状态在小时间内变化很小，故这种近似处理是比较准确的。例如假设机器人以恒定的速度运动(V＝2m/s)，取离散时间步长为T＝1s，假设期望机器人从关键点(x(1),y(1))＝(10,14)沿着路径曲线运动到关键点(x(2),y(2))＝(36,28)，再沿着路径曲线移动到(x(3),y(3))＝(60,59)，可以得到式(5-9)的结果。

x[1]＝x(1)＝10

y[1]＝y(1)＝14

$K\&lsqb;1\&rsqb;=\frac{dy}{dx}{|}_{x=x\&lsqb;1\&rsqb;}=2*a(1,1)*x\&lsqb;1\&rsqb;+a(1,2)=2*0.0092*10+0.3=0.48---(5-9)$

$x\&lsqb;2\&rsqb;=T*V*\frac{1}{\sqrt{1+K\&lsqb;1\&rsqb;*K\&lsqb;1\&rsqb;}}+x\&lsqb;1\&rsqb;=11.803$

y[2]＝y(x)|_x＝x[2]＝14.061

根据式(5-9)可知，第1个离散点的坐标为(10，14)，第2个离散点的坐标为(11.803，14.061)，移动式机器人经过第1个离散点的速度为0.48。同理可求出路径曲线上一系列的离散点的坐标和速度，这些点最终构成移动式机器人轨迹曲线的离散点。本实施例中，按上述方法求得的移动式机器人的轨迹曲线上的各个离散点的坐标位置如图3所示。

以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (7)

1.一种用于移动式机器人的定点跟踪路径规划方法，其特征在于步骤包括：

1)确定移动式机器人在二维空间中所需经过的关键点，确定初始斜率值范围并初始化路径最小值为0；

2)从所述初始斜率值范围中搜索选择一个斜率作为当前初始斜率；

3)基于所述当前初始斜率求解各组相邻关键点之间的二次曲线；

4)计算各组相邻关键点之间的二次曲线按顺序连接所组成路径曲线的总长度；如果所述路径最小值为0或者当前初始斜率下路径曲线的总长度小于路径最小值，则将所述路径最小值的值更新设置为当前初始斜率下路径曲线的总长度，记录当前初始斜率下的路径曲线，跳转执行步骤5)；否则，直接跳转执行步骤5)；

5)判断斜率搜索范围中的斜率是否搜索完毕，如果尚未搜索完毕，则跳转执行步骤2)；否则，则输出记录的路径曲线，并将所述记录的路径曲线经离散化处理，最终得到移动式机器人的实际轨迹曲线的离散点坐标和速度。

2.根据权利要求1所述的用于移动式机器人的定点跟踪路径规划方法，其特征在于，所述步骤3)的详细步骤如下：

3.1)定义依次经过移动式机器人在二维空间中所需经过的关键点的总插值曲线，所述总插值曲线为二次曲线，所述总插值曲线由各组相邻关键点之间的二次曲线按顺序连接组成；

3.2)基于所述当前初始斜率确定各个关键点的斜率；

3.3)采用二次样条插值的方法，基于相邻关键点的坐标、相邻关键点中一个关键点的斜率确定每一组相邻关键点之间的二次曲线常数项系数、一次项系数以及二次项系数，从而确定每一组相邻关键点之间的二次曲线。

3.根据权利要求2所述的用于移动式机器人的定点跟踪路径规划方法，其特征在于：所述步骤3.1)中的总插值曲线的表达式如式(1)所示；

y＝a(i,1)*x²+a(i,2)*x+a(i,3)   (1)

式(1)中，a(i,1)为二次项系数，a(i,2)为一次项系数，a(i,3)为常数项系数。

4.根据权利要求3所述的用于移动式机器人的定点跟踪路径规划方法，其特征在于：所述步骤3.2)中具体是指在当前初始斜率的基础上，根据式(2)迭代确定各个关键点的斜率；

$z(i+1)=-z\left(i\right)+2*\frac{y(i+1)-y\left(i\right)}{x(i+1)-x\left(i\right)}---\left(2\right)$

式(2)中，z(i+1)表示第i+1个关键点的斜率，z(i)表示第i个关键点的斜率，y(i+1)表示第i+1个关键点的纵坐标，y(i)表示第i个关键点的纵坐标，x(i+1)表示第i+1个关键点的横坐标，x(i)表示第i个关键点的横坐标。

5.根据权利要求4所述的用于移动式机器人的定点跟踪路径规划方法，其特征在于：所述步骤3.3)中确定的每一组相邻关键点之间的二次曲线常数项系数的值为该二次曲线起始关键点的纵坐标，一次项系数的值为该二次曲线起始关键点的斜率，二次项系数的计算表达式如式(3)所示；

$a(i,1)=0.5*\frac{z(i+1)-z\left(i\right)}{x(i+1)-x\left(i\right)}---\left(3\right)$

式(3)中，a(i,1)表示二次曲线的二次项系数，z(i+1)表示该二次曲线结束关键点的斜率，z(i)表示该二次曲线起始关键点的斜率，x(i+1)表示该二次曲线结束关键点的横坐标，x(i)表示该二次曲线起始关键点的横坐标。

6.根据权利要求5所述的用于移动式机器人的定点跟踪路径规划方法，其特征在于：所述步骤4)中计算各组相邻关键点之间的二次曲线按顺序连接所组成路径曲线的总长度时，具体是指根据式(4)计算路径曲线的总长度；

$lengthSum=\mathrm{\&Sigma;}{\&Integral;}_i\frac{1}{\sqrt{1+yi{\left(x\right)}^{\&prime;}*yi{\left(x\right)}^{\&prime;}}}dx---\left(4\right)$

式(4)中，lengthSum表示各个相邻关键点之间的二次曲线依次相连得到连续曲线的总长度，yi(x)表示第i组相邻关键点之间的二次曲线函数值，yi(x)′表示第i组相邻关键点之间的二次曲线的微分值。

7.根据权利要求1～6中任意一项所述的用于移动式机器人的定点跟踪路径规划方法，其特征在于：所述步骤5)将记录的路径曲线经离散化处理具体是指根据式(5)所示的递推函数将路径曲线经离散化处理；

$\left\{\begin{array}{c}x\&lsqb;n+1\&rsqb;=T*V\&lsqb;n\&rsqb;*\frac{1}{\sqrt{1+K\&lsqb;n\&rsqb;*K\&lsqb;n\&rsqb;}}+x\&lsqb;n\&rsqb;\\ y\&lsqb;n+1\&rsqb;=a(i,1)*x{\&lsqb;n+1\&rsqb;}^2+a(i,2)*x\&lsqb;n+1\&rsqb;+a(i,3)\end{array}\right.---\left(5\right)$

式(5)中，(x[n],y[n])表示递推得到的第n个离散点的坐标，(x[n+1],y[n+1])表示第n+1个离散点的坐标，T表示相邻离散点间隔的步长，V[n]表示移动式机器人经过第n个离散点的速度，x[n]表示第n个离散点的横坐标，a(i,3)表示第n个离散点对应二次曲线的常数项系数，a(i,2)表示第n个离散点对应二次曲线的一次项系数，a(i,1)表示第n个离散点对应二次曲线的二次项系数；K[n]表示移动式机器人经过第n个离散点的斜率，K[n]的表达式如式(6)所示；

K[n]＝2*a(i,1)*x[n]+a(i,2)   (6)

式(6)中，a(i,1)表示第n个离散点对应二次曲线的二次项系数，x[n]表示第n个离散点的横坐标，a(i,2)表示第n个离散点对应二次曲线的一次项系数。