专利名称：基于贝塞尔曲线的机器人路径规划方法及装置的制作方法
技术领域：
本发明属于机器人路径规划算法研究，属于复杂系统与智能控制领域。该发明可以用于地面移动机器人、水下航行器控制以及无人机控制等领域。
背景技术：
机器人技术的研究越来越热门，越来越多的科研工作者展开了机器人路径规划的研究。
机器人路径规划的方法有很多，主要有基于行为的方法、遗传算法、神经网络等方法。鲍尔奇(Balch)与阿金(Arkin)提出了将行为的控制方法用于机器人路径规划与控制， 引入了一些基本行为，包括避障、避碰和目标导航等。荷兰德(Holland)提出了遗传算法的方法，该算法基本思想是以自然遗传机制和自然选择等生物进化理论为基础，构造的一类随机优化搜索算法，很多学者利用该算法实现路径规划。斯坦福大学的尼尔森(misson) 提出可视图法，这种方法能完成最短路径的搜索，但并没有考虑到机器人本身的形状、大小，且随着障碍物的增多，或障碍物的不规则性而导致图中的顶点集过大，计算复杂性随之增加，搜索时间长。在可视图法的基础上进行了改进的沃罗诺伊(Voronoi)图法，基本思想是首先产生与障碍物多边形所有的边等距离的沃罗诺伊(Voronoi)边，并将所有沃罗诺伊 (Voronoi)边的交点组成顶点集，再通过可视图类似的搜索最短路径边的方法来规划路径。 模糊逻辑算法是对经典的二值逻辑的模糊化，模糊逻辑具有的鲁棒性与“感知一动作”行为结合起来，为移动机器人路径规划问题提出了一种新思路，利用模糊逻辑进行路径规划，解决了传统人工势场法中局部极小的问题，利用该算法对所处环境信息的模糊化过程，避开了传统算法中对移动机器人定位精度要求高的约束。目前对于机器人路径规划的方法很多，但是对于路径上各点处如何处理并满足速度约束、加速度约束、转弯半径约束等约束条件保证机器人规划路径可行则很少有详细的介绍。
机器人路径规划算法主要是规划出路径，并解决路径的可行性问题，即满足机器人的物理性能上的各种约束以及避碰、避障约束等。
发明内容
为了解决多约束情况下的机器人路径规划问题，本发明的目的是提出一种机器人路径规划算法，该算法能够为机器人规划无障碍条件下连续可行的路径，解决机器人的各种物理约束条件。为此，本发明提出了一种基于贝塞尔曲线的机器人路径规划方法。
为达成所述目的，本发明的第一方面是提供一种基于贝塞尔(Bezier)曲线的机器人路径规划方法，该机器人路径规划方法的步骤如下 步骤Sl 机器人路径规划参数输入单元设定机器人状态参数和约束条件、路径离散化最小时间分辨率； 步骤S2 机器人路径生成单元依据机器人状态参数生成一组由四个4维向量组成的贝塞尔曲线控制点Pi、P2> P3和P4,规划出机器人起点位置到目标点位置的连续路径；其中4维向量由空间三维坐标和一维时间坐标组成；规划的路径也由包含空间三维坐标和一维时间坐标的4维坐标描述； 步骤S3 路径点参数计算单元按路径离散化最小时间分辨率形成时间点序列，再依据连续路径计算离散化路径，即计算各时间点处路径点坐标、速度、加速度和转弯半径； 步骤S4 路径点参数检测单元依据约束条件检测与时间点相应的各路径点处的速度、加速度、转弯半径是否满足约束条件，如果不满足约束条件，则跳转至步骤S2，重新生成控制点；如果满足约束条件，则跳转至步骤S5 ； 步骤S5 规划路径输出单元输出规划的满足约束条件的机器人路径。
为达成所述目的，本发明的第二方面是提供一种基于贝塞尔(Bezier)曲线的机器人路径规划装置，该装置由机器人路径规划参数输入单元、机器人路径生成单元、规划点参数计算单元、路径点参数检测单元、规划路径输出单元组成，其中机器人路径规划参数输入单元接收设定机器人状态参数和约束条件、路径离散化最小时间分辨率；机器人路径生成单元与机器人路径规划参数输入单元连接，机器人路径生成单元依据机器人状态参数生成一组由四个4维向量组成的贝塞尔曲线控制点，规划出机器人起点位置到目标点位置的连续路径；路径点参数计算单元与机器人路径生成单元连接，机器人路径生成单元按路径离散化最小时间分辨率形成时间点序列，再依据连续路径计算离散化路径，即计算各时间点处路径点坐标、速度、加速度和转弯半径；路径点参数检测单元与路径点参数计算单元连接，路径点参数检测单元依据约束条件检测与时间点相应的各路径点处的速度、加速度、 转弯半径是否满足约束条件，如果不满足约束条件，重新生成机器人路径；如果满足约束条件，保存规划的满足约束条件的机器人路径；规划路径输出单元与路径点参数检测单元连接，接收并输出规划的满足约束条件的机器人路径。
本发明有益效果本发明采用基于贝塞尔曲线的机器人路径规划算法来解决无障碍环境下机器人路径规划问题，规划出来的路径满足机器人本身物理约束以及避碰约束等。在给定机器人起点位置、速度和方向，目标点位置、速度和方向条件下，可以规划出从机器人起点到目标点的路径，规划的路径光滑连续可导，满足机器人的速度约束、加速度约束、转弯半径约束等，并且能按照预定时间到达目标点。该算法能快速实现机器人路径规划，方法简单、可靠，易于实现，计算量小，实时性较好，能较好的满足机器人路径规划时间要求。
图1是本发明实施例基于贝塞尔曲线的机器人路径规划结构示意图。
图2是本发明基于贝塞尔曲线的机器人路径规划算法流程图。
具体实施例方式为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明基于贝塞尔曲线的机器人路径规划方法和装置进一步详细说明。
如图1示出本发明实施例基于贝塞尔曲线的机器人路径规划结构示意图，该结构是在一台计算机上实现，是由机器人路径规划参数输入单元1、机器人路径生成单元2、规划点参数计算单元3、路径点参数检测单元4、规划路径输出单元5组成，机器人路径规划参数输入单元1接收设定机器人状态参数和约束条件、路径离散化最小时间分辨率；机器人路径生成单元2与机器人路径规划参数输入单元1连接，机器人路径生成单元2依据机器人状态参数生成一组由四个4维向量组成的贝塞尔曲线控制点Pp P2> P3和P4,规划出机器人起点位置到目标点位置的连续路径；其中4维向量由空间三维坐标和一维时间坐标组成；规划的路径也由包含空间三维坐标和一维时间坐标的4维坐标描述；路径点参数计算单元3与机器人路径生成单元2连接，机器人路径生成单元2按路径离散化最小时间分辨率形成时间点序列，再依据连续路径计算离散化路径，即计算各时间点处路径点坐标、速度、加速度和转弯半径；路径点参数检测单元4与路径点参数计算单元3连接，路径点参数检测单元4依据约束条件检测与时间点相应的各路径点处的速度、加速度、转弯半径是否满足约束条件，如果不满足约束条件，重新生成机器人路径；如果满足约束条件，保存规划的满足约束条件的机器人路径；规划路径输出单元5与路径点参数检测单元4连接，接收并输出规划的满足约束条件的机器人路径。
下面利用该结构实现本发明的基于贝塞尔曲线的机器人路径规划方法，请参考图 2示出的具体步骤如下 步骤Sl 机器人路径规划参数输入单元1设定机器人状态参数和约束条件、路径离散化最小时间分辨率； 机器人状态参数主要包括机器人起点位置坐标(xs，ys, zs)、起点位置处的速度 (vsx, V/, vsz)，机器人期望到达的目标点位置坐标(xg，yg, Zg)、到达目标点位置时的速度 (ν/, ν/, vgz)，期望的到达时间tf ；约束条件主要包括机器人的最大速度Vmax和最小速度 Vmin、最大加速度^iax和最小加速度^lin、最小转弯半径Rmin ；其中期望的到达时间tf是指机器人从起点位置坐标运动到目标点位置坐标期望花费的时间；设定机器人路径离散化最小时间分辨率tmin。
步骤S2 机器人路径生成单元2依据机器人状态参数生成一组由四个4维向量组成的贝塞尔曲线控制点=PpPyP3和P4，规划出机器人起点位置到目标点位置的连续路径； 其中4维向量由空间三维坐标和一维时间坐标组成；规划的路径也由包含空间三维坐标和一维时间坐标的4维坐标描述；采用所述贝塞尔曲线描述路径的三维空间坐标和一维时间坐标。
本发明的路径规划算法中，规划的机器人路径包括三维空间位置坐标和到达该空间位置的时间坐标的4维坐标组成。要规划的机器人路径上各路径点采用笛卡儿空间坐标系的X轴、Y轴、Z轴，和时间轴T构成的四维空间描述。要规划的机器人路径在X轴、Y轴、 Z轴和时间轴T上的投影用三次贝塞尔曲线表达，其表达式为 χ(τ) = ax τ 3+bx τ 2+cx τ +dx y( τ ) = ay τ 3+by τ 2+Cy τ +dy (1) z( τ ) = az τ 3+bz τ 2+cz τ +dz t ( τ ) = at τ 3+bt τ 2+ct τ +dt 其中，χ(τ)，γ(τ)，ζ(τ), t(x)表示要规划的机器人路径上各路径点的位置坐标x、y、z和时间为贝塞尔曲线的参数，其取值为τ e
。τ =0时，χ(τ)， y(T)，z(T)，t(T)为机器人的起点位置(\，1，0，起点时间0; τ = l·时，χ(τ)，γ(τ)，ζ ( τ )，t ( τ )为机器人的期望到达的目标点位置(xg，yg, zg)，和期望的达到时间tf ；贝塞尔曲线的各项系数包括ax，bx，cx，dx为χ( τ )的系数、ay，by，cy，dy为y( τ )的系数、az，bz, cz，dz为ζ ( τ )的系数以及at, bt，ct，dt为t ( τ )的系数。
上述贝塞尔曲线的各项系数可以根据四个控制点唯一确定，每个控制点为(X，y， z,t)描述的四维空间点。假设三次贝塞尔曲线的四个控制点坐标分别为控制点P1O^y1, Z1,、)，控制点 P2 (x2，Y2, z2, t2)，控制点 P3(x3，Y3, z3, t3)，控制点 P4(x4，Y4, z4, t4)，由这四个控制点，可以求得对应的贝塞尔曲线的各项系数为 aXay = -yi+3y2-3y3+y4 bx=3X^6X2 + 3X3 by = Sy1-By^y3, Cx=-3x^3x2cy = -Sy^Sy2 dx=X1dy = Yl azZ1+3Z2 3ζ— -11+3 一3 3+ 4 K=3ZJ-6Z2+3Z3,bt = 3tr6t2+3t3, (2) Cz=-Sz^Sz2 dz=Z1dt = ti 利用机器人起点位置坐标、期望到达的目标点位置坐标和期望的到达时间tf确定
贝塞尔曲线的控制点PjP P4。在τ =0时，令贝塞尔曲线的控制点P1 (Xpy1^pt1)，为机器人的起点位置(xs，ys，zs)且、=0 ；在τ =1时，令贝塞尔曲线的控制点P4 (x4，y4, Z4, t4)，为机器人期望到达的目标点位置(xg，yg, %)，且、=tf，其中tf为机器人期望的到达时间。只需再确定两个中间控制点IVP3即可规划出机器人的路径。
在去除时间轴一维的机器人路径上，各点处的切向方向与该点处速度方向一致， 因此，控制点P1与控制点P2的连线与控制点P1处的切向方向一致，即与机器人起点位置处的速度方向一致；控制点P3与控制点P4的连线与控制点P4处的切向方向一致，即与机器人的目标点位置处的速度方向一致。
故两个中间控制点P2、P3的X轴、Y轴、Z轴坐标可以表示为如下公式
P2 = (X1, Y1, Z1) +Ii1 X V1/ I V1 ,0 < k1 k2 < IP1P4(3) P3 = (x4, y4, z4) ~k2 X V4/ | V4 其中，(Xl, Yl, Z1)为控制AP1的X轴、Y轴、Z轴坐标，即机器人起点位置(xs，ys， zs) ;Vl表示起点位置处的机器人速度V1表示起点位置处的机器人速度值， V1/1V1为起点位置处的机器人速度方向；(x4，y4，z4)为控制点P4WX轴、Y轴、Z轴坐标，即机器人期望到达的目标点位置(xg，yg, zg) ;v4为机器人到达目标点位置时的速度(vgx，ν/, V；), Iv4I表示到达目标点位置时的机器人速度值，V4/| V4I为机器人到达目标点位置时的速度方向^为控制点P1和控制点P2间的距离ι为控制点P3和控制点P4之间的距离
I^l
O 公式(3)可用于确定两个中间控制点己、&在三维空间上的X轴、Y轴、Z轴坐标， 但是为方便后续速度、加速度、转弯半径、期望的到达时间等约束条件判断，故需要确定各个路径点处的时刻。由于贝塞尔曲线控制AP1和贝塞尔曲线控制点P4对应的时刻分别为O和tf。设两个中间控制点P2、P3对应的时刻分别为ち，t3,显然ちく t3。
包含时间轴的中间控制点P2、P3的选取公式为
权利要求
1.一种基于贝塞尔曲线的机器人路径规划方法，其特征在于，该机器人路径规划方法的步骤如下步骤Sl 机器人路径规划参数输入单元设定机器人状态参数和约束条件、路径离散化最小时间分辨率；步骤S2 机器人路径生成单元依据机器人状态参数生成一组由四个4维向量组成的贝塞尔曲线控制点ΡρΡ2、Ρ3和P4,规划出机器人起点位置到目标点位置的连续路径；其中4维向量由空间三维坐标和一维时间坐标组成；规划的路径也由包含空间三维坐标和一维时间坐标的4维坐标描述；步骤S3 路径点参数计算单元按路径离散化最小时间分辨率形成时间点序列，再依据连续路径计算离散化路径，即计算各时间点处路径点坐标、速度、加速度和转弯半径；步骤S4 路径点参数检测单元依据约束条件检测与时间点相应的各路径点处的速度、 加速度、转弯半径是否满足约束条件，如果不满足约束条件，则跳转至步骤S2，重新生成控制点；如果满足约束条件，则跳转至步骤S5 ；步骤S5 规划路径输出单元输出规划的满足约束条件的机器人路径。
2.根据权利要求1的路径规划方法，其特征在于，所述机器人状态参数包括机器人起点位置坐标(xs，ys, %)、起点位置处的速度(vsx，ν/, vsz)，机器人期望到达的目标点位置坐标(xg，yg，zg)、到达目标点位置时的速度(Vgx，Vgy，Vgz)，期望的到达时间tf ；约束条件包括机器人的最大速度Vmax和最小速度Vmin、最大加速度amax和最小加速度^lin、最小转弯半径Rmin ； 其中期望的到达时间tf是指机器人从起点位置坐标运动到目标点位置坐标期望花费的时间。
3.根据权利要求1的路径规划方法，其特征在于，采用所述贝塞尔曲线描述路径的三维空间坐标和一维时间坐标。
4.根据权利要求1的路径规划方法，其特征在于，利用机器人起点位置坐标、期望到达的目标点位置坐标和期望的到达时间tf确定贝塞尔曲线的控制点P1和P4。
5.根据权利要求1的路径规划方法，其特征在于，所述贝塞尔曲线的控制点P2和P3利用如下公式选取
其中，(Xl，yi，Zl)为控制点P1的X轴、Y轴、Z轴坐标，(x4, y4, z4)为控制点P4W X轴、 Y轴、Z轴坐标，ν:, ν/, vsz为起点位置处机器人速度V1在X轴、Y轴、Z轴的投影；ν/，ν/, ν/为机器人到达目标点位置时的速度V4在X轴、Y轴、Z轴的投影，ki; 1 为随机选取的实数，且满足
V1表示起点位置处的机器人速度值，V4表示到达目标点位置时的机器人速度值，tf为机器人期望的到达时间。
6.一种基于贝塞尔曲线的机器人路径规划装置，其特征在于，由机器人路径规划参数输入单元、机器人路径生成单元、规划点参数计算单元、路径点参数检测单元、规划路径输出单元组成，其中机器人路径规划参数输入单元接收设定机器人状态参数和约束条件、路径离散化最小时间分辨率；机器人路径生成单元与机器人路径规划参数输入单元连接，机器人路径生成单元依据机器人状态参数生成一组由四个4维向量组成的贝塞尔曲线控制点，规划出机器人起点位置到目标点位置的连续路径；路径点参数计算单元与机器人路径生成单元连接，机器人路径生成单元按路径离散化最小时间分辨率形成时间点序列，再依据连续路径计算离散化路径，即计算各时间点处路径点坐标、速度、加速度和转弯半径；路径点参数检测单元与路径点参数计算单元连接，路径点参数检测单元依据约束条件检测与时间点相应的各路径点处的速度、加速度、转弯半径是否满足约束条件，如果不满足约束条件，重新生成机器人路径；如果满足约束条件，保存规划的满足约束条件的机器人路径；规划路径输出单元与路径点参数检测单元连接，接收并输出规划的满足约束条件的机器人路径。
7.根据权利要求6所述的基于贝塞尔曲线的机器人路径规划装置，其特征在于，所述4 维向量由空间三维坐标和一维时间坐标组成。
8.根据权利要求6所述的基于贝塞尔曲线的机器人路径规划装置，其特征在于，所述规划的路径也由包含空间三维坐标和一维时间坐标的4维坐标描述。
全文摘要
本发明是一种基于贝塞尔曲线的机器人路径规划方法及装置，机器人路径规划参数输入单元接收设定机器人状态参数和约束条件、路径离散化最小时间分辨率；机器人路径生成单元依据机器人状态参数生成一组由四个4维向量组成的贝塞尔曲线控制点，规划出机器人起点位置到目标点位置的连续路径；机器人路径生成单元按路径离散化最小时间分辨率形成时间点序列，再依据连续路径计算离散化路径；路径点参数检测单元依据约束条件检测与时间点相应的各路径点处的速度、加速度、转弯半径是否满足约束条件，如果不满足约束条件，重新生成控制点；如果满足约束条件，规划路径输出单元接收并输出路径点参数检测单元规划的满足约束条件的机器人路径。
文档编号G05D1/00GK102207736SQ20101013909
公开日2011年10月5日 申请日期2010年3月31日 优先权日2010年3月31日
发明者王硕, 谭民, 胡峰 申请人:中国科学院自动化研究所