基于贝塞尔曲线的机器人路径规划方法及装置

摘要

本发明是一种基于贝塞尔曲线的机器人路径规划方法及装置，机器人路径规划参数输入单元接收设定机器人状态参数和约束条件、路径离散化最小时间分辨率；机器人路径生成单元依据机器人状态参数生成一组由四个4维向量组成的贝塞尔曲线控制点，规划出机器人起点位置到目标点位置的连续路径；机器人路径生成单元按路径离散化最小时间分辨率形成时间点序列，再依据连续路径计算离散化路径；路径点参数检测单元依据约束条件检测与时间点相应的各路径点处的速度、加速度、转弯半径是否满足约束条件，如果不满足约束条件，重新生成控制点；如果满足约束条件，规划路径输出单元接收并输出路径点参数检测单元规划的满足约束条件的机器人路径。

权利要求

1.一种基于贝塞尔曲线的机器人路径规划方法，其特征在于，该机器人路径规划方法的步骤如下：

步骤S1：机器人路径规划参数输入单元设定机器人状态参数和约束条件、路径离散化最小时间分辨率；

步骤S2：机器人路径生成单元依据机器人状态参数生成一组由四个4维向量组成的贝塞尔曲线控制点P1、P2、P3和P4，规划出机器人起点位置到目标点位置的连续路径；其中4维向量由空间三维坐标和一维时间坐标组成；规划的路径也由包含空间三维坐标和一维时间坐标的4维坐标描述；

步骤S3：路径点参数计算单元按路径离散化最小时间分辨率形成时间点序列，再依据连续路径计算离散化路径，即计算各时间点处路径点坐标、速度、加速度和转弯半径；

步骤S4：路径点参数检测单元依据约束条件检测与时间点相应的各路径点处的速度、加速度、转弯半径是否满足约束条件，如果不满足约束条件，则跳转至步骤S2，重新生成控制点；如果满足约束条件，则跳转至步骤S5；

步骤S5：规划路径输出单元输出规划的满足约束条件的机器人路径。

2.根据权利要求1的路径规划方法，其特征在于，所述机器人状态参数包括机器人起点位置坐标(xs，ys，zs)、起点位置处的速度 机器人期望到达的目标点位置坐标(xg，yg，zg)、到达目标点位置时的速度 期望的到达时间tf；约束条件包括机器人的最大速度vmax和最小速度vmin、最大加速度amax和最小加速度amin、最小转弯半径Rmin；其中期望的到达时间tf是指机器人从起点位置坐标运动到目标点位置坐标期望花费的时间。

3.根据权利要求1的路径规划方法，其特征在于，采用所述贝塞尔曲线描述路径的三维空间坐标和一维时间坐标。

4.根据权利要求1的路径规划方法，其特征在于，利用机器人起点位置坐标、期望到达的目标点位置坐标和期望的到达时间tf确定贝塞尔曲线的控制点P1和P4。

5.根据权利要求1的路径规划方法，其特征在于，所述贝塞尔曲线的控制点P2和P3利用如下公式选取：

其中，(x1，y1，z1)为控制点P1的X轴、Y轴、Z轴坐标，(x4，y4，z4)为控制点P4的X轴、Y轴、Z轴坐标， 为起点位置处机器人速度v1在X轴、Y轴、Z轴的投影； 为机器人到达目标点位置时的速度v4在X轴、Y轴、Z轴的投影，k1，k2为随机选取的实数，且满足：且 |v1|表示起点位置处的机器人速度值，|v4|表示到达目标点位

置时的机器人速度值，tf为机器人期望的到达时间。

6.一种基于贝塞尔曲线的机器人路径规划装置，其特征在于，由机器人路径规划参数输入单元、机器人路径生成单元、规划点参数计算单元、路径点参数检测单元、规划路径输出单元组成，其中：机器人路径规划参数输入单元设定机器人状态参数和约束条件、路径离散化最小时间分辨率；机器人路径生成单元与机器人路径规划参数输入单元连接，机器人路径生成单元依据机器人状态参数生成一组由四个4维向量组成的贝塞尔曲线控制点，规划出机器人起点位置到目标点位置的连续路径；路径点参数计算单元与机器人路径生成单元连接，路径点参数计算单元按路径离散化最小时间分辨率形成时间点序列，再依据连续路径计算离散化路径，即计算各时间点处路径点坐标、速度、加速度和转弯半径；路径点参数检测单元与路径点参数计算单元连接，路径点参数检测单元依据约束条件检测与时间点相应的各路径点处的速度、加速度、转弯半径是否满足约束条件，如果不满足约束条件，重新生成控制点，规划出机器人起点位置到目标点位置的连续路径；如果满足约束条件，保存规划的满足约束条件的机器人路径；规划路径输出单元与路径点参数检测单元连接，接收并输出规划的满足约束条件的机器人路径。

7.根据权利要求6所述的基于贝塞尔曲线的机器人路径规划装置，其特征在于，所述4维向量由空间三维坐标和一维时间坐标组成。

8.根据权利要求6所述的基于贝塞尔曲线的机器人路径规划装置，其特征在于，所述规划出机器人起点位置到目标点位置的连续路径也由包含空间三维坐标和一维时间坐标的4维坐标描述。

说明书

基于贝塞尔曲线的机器人路径规划方法及装置

技术领域

[0001] 本发明属于机器人路径规划算法研究，属于复杂系统与智能控制领域。该发明可以用于地面移动机器人、水下航行器控制以及无人机控制等领域。

背景技术

[0002] 机器人技术的研究越来越热门，越来越多的科研工作者展开了机器人路径规划的研究。

[0003] 机器人路径规划的方法有很多，主要有基于行为的方法、遗传算法、神经网络等方法。鲍尔奇(Balch)与阿金(Arkin)提出了将行为的控制方法用于机器人路径规划与控制，引入了一些基本行为，包括避障、避碰和目标导航等。荷兰德(Holland)提出了遗传算法的方法，该算法基本思想是以自然遗传机制和自然选择等生物进化理论为基础，构造的一类随机优化搜索算法，很多学者利用该算法实现路径规划。斯坦福大学的尼尔森(Nilsson)提出可视图法，这种方法能完成最短路径的搜索，但并没有考虑到机器人本身的形状、大小，且随着障碍物的增多，或障碍物的不规则性而导致图中的顶点集过大，计算复杂性随之增加，搜索时间长。在可视图法的基础上进行了改进的沃罗诺伊(Voronoi)图法，基本思想是首先产生与障碍物多边形所有的边等距离的沃罗诺伊(Voronoi)边，并将所有沃罗诺伊(Voronoi)边的交点组成顶点集，再通过可视图类似的搜索最短路径边的方法来规划路径。模糊逻辑算法是对经典的二值逻辑的模糊化，模糊逻辑具有的鲁棒性与“感知一动作”行为结合起来，为移动机器人路径规划问题提出了一种新思路，利用模糊逻辑进行路径规划，解决了传统人工势场法中局部极小的问题，利用该算法对所处环境信息的模糊化过程，避开了传统算法中对移动机器人定位精度要求高的约束。目前对于机器人路径规划的方法很多，但是对于路径上各点处如何处理并满足速度约束、加速度约束、转弯半径约束等约束条件保证机器人规划路径可行则很少有详细的介绍。

[0004] 机器人路径规划算法主要是规划出路径，并解决路径的可行性问题，即满足机器人的物理性能上的各种约束以及避碰、避障约束等。

发明内容

[0005] 为了解决多约束情况下的机器人路径规划问题，本发明的目的是提出一种机器人路径规划算法，该算法能够为机器人规划无障碍条件下连续可行的路径，解决机器人的各种物理约束条件。为此，本发明提出了一种基于贝塞尔曲线的机器人路径规划方法。

[0006] 为达成所述目的，本发明的第一方面是提供一种基于贝塞尔(Bezier)曲线的机器人路径规划方法，该机器人路径规划方法的步骤如下：

[0007] 步骤S1：机器人路径规划参数输入单元设定机器人状态参数和约束条件、路径离散化最小时间分辨率；

[0008] 步骤S2：机器人路径生成单元依据机器人状态参数生成一组由四个4维向量组成的贝塞尔曲线控制点P1、P2、P3和P4，规划出机器人起点位置到目标点位置的连续路径；其中4维向量由空间三维坐标和一维时间坐标组成；规划的路径也由包含空间三维坐标和一维时间坐标的4维坐标描述；

[0009] 步骤S3：路径点参数计算单元按路径离散化最小时间分辨率形成时间点序列，再依据连续路径计算离散化路径，即计算各时间点处路径点坐标、速度、加速度和转弯半径；

[0010] 步骤S4：路径点参数检测单元依据约束条件检测与时间点相应的各路径点处的速度、加速度、转弯半径是否满足约束条件，如果不满足约束条件，则跳转至步骤S2，重新生成控制点；如果满足约束条件，则跳转至步骤S5；

[0011] 步骤S5：规划路径输出单元输出规划的满足约束条件的机器人路径。

[0012] 为达成所述目的，本发明的第二方面是提供一种基于贝塞尔(Bezier)曲线的机器人路径规划装置，该装置由机器人路径规划参数输入单元、机器人路径生成单元、规划点参数计算单元、路径点参数检测单元、规划路径输出单元组成，其中：机器人路径规划参数输入单元接收设定机器人状态参数和约束条件、路径离散化最小时间分辨率；机器人路径生成单元与机器人路径规划参数输入单元连接，机器人路径生成单元依据机器人状态参数生成一组由四个4维向量组成的贝塞尔曲线控制点，规划出机器人起点位置到目标点位置的连续路径；路径点参数计算单元与机器人路径生成单元连接，机器人路径生成单元按路径离散化最小时间分辨率形成时间点序列，再依据连续路径计算离散化路径，即计算各时间点处路径点坐标、速度、加速度和转弯半径；路径点参数检测单元与路径点参数计算单元连接，路径点参数检测单元依据约束条件检测与时间点相应的各路径点处的速度、加速度、转弯半径是否满足约束条件，如果不满足约束条件，重新生成机器人路径；如果满足约束条件，保存规划的满足约束条件的机器人路径；规划路径输出单元与路径点参数检测单元连接，接收并输出规划的满足约束条件的机器人路径。

[0013] 本发明有益效果：本发明采用基于贝塞尔曲线的机器人路径规划算法来解决无障碍环境下机器人路径规划问题，规划出来的路径满足机器人本身物理约束以及避碰约束等。在给定机器人起点位置、速度和方向，目标点位置、速度和方向条件下，可以规划出从机器人起点到目标点的路径，规划的路径光滑连续可导，满足机器人的速度约束、加速度约束、转弯半径约束等，并且能按照预定时间到达目标点。该算法能快速实现机器人路径规划，方法简单、可靠，易于实现，计算量小，实时性较好，能较好的满足机器人路径规划时间要求。

附图说明

[0014] 图1是本发明实施例基于贝塞尔曲线的机器人路径规划结构示意图。

[0015] 图2是本发明基于贝塞尔曲线的机器人路径规划算法流程图。

具体实施方式

[0016] 为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明基于贝塞尔曲线的机器人路径规划方法和装置进一步详细说明。

[0017] 如图1示出本发明实施例基于贝塞尔曲线的机器人路径规划结构示意图，该结构是在一台计算机上实现，是由机器人路径规划参数输入单元1、机器人路径生成单元2、规划点参数计算单元3、路径点参数检测单元4、规划路径输出单元5组成，机器人路径规划参数输入单元1接收设定机器人状态参数和约束条件、路径离散化最小时间分辨率；机器人路径生成单元2与机器人路径规划参数输入单元1连接，机器人路径生成单元2依据机器人状态参数生成一组由四个4维向量组成的贝塞尔曲线控制点P1、P2、P3和P4，规划出机器人起点位置到目标点位置的连续路径；其中4维向量由空间三维坐标和一维时间坐标组成；规划的路径也由包含空间三维坐标和一维时间坐标的4维坐标描述；路径点参数计算单元3与机器人路径生成单元2连接，机器人路径生成单元2按路径离散化最小时间分辨率形成时间点序列，再依据连续路径计算离散化路径，即计算各时间点处路径点坐标、速度、加速度和转弯半径；路径点参数检测单元4与路径点参数计算单元3连接，路径点参数检测单元4依据约束条件检测与时间点相应的各路径点处的速度、加速度、转弯半径是否满足约束条件，如果不满足约束条件，重新生成机器人路径；如果满足约束条件，保存规划的满足约束条件的机器人路径；规划路径输出单元5与路径点参数检测单元4连接，接收并输出规划的满足约束条件的机器人路径。

[0018] 下面利用该结构实现本发明的基于贝塞尔曲线的机器人路径规划方法，请参考图2示出的具体步骤如下：

[0019] 步骤S1：机器人路径规划参数输入单元1设定机器人状态参数和约束条件、路径离散化最小时间分辨率；

[0020] 机器人状态参数主要包括机器人起点位置坐标(xs，ys，zs)、起点位置处的速度x y z(vs，vs，vs)，机器人期望到达的目标点位置坐标(xg，yg，zg)、到达目标点位置时的速度x y z
(vg，vg，vg)，期望的到达时间tf；约束条件主要包括机器人的最大速度vmax和最小速度vmin、最大加速度amax和最小加速度amin、最小转弯半径Rmin；其中期望的到达时间tf是指机器人从起点位置坐标运动到目标点位置坐标期望花费的时间；设定机器人路径离散化最小时间分辨率tmin。

[0021] 步骤S2：机器人路径生成单元2依据机器人状态参数生成一组由四个4维向量组成的贝塞尔曲线控制点：P1、P2、P3和P4，规划出机器人起点位置到目标点位置的连续路径；其中4维向量由空间三维坐标和一维时间坐标组成；规划的路径也由包含空间三维坐标和一维时间坐标的4维坐标描述；采用所述贝塞尔曲线描述路径的三维空间坐标和一维时间坐标。

[0022] 本发明的路径规划算法中，规划的机器人路径包括三维空间位置坐标和到达该空间位置的时间坐标的4维坐标组成。要规划的机器人路径上各路径点采用笛卡儿空间坐标系的X轴、Y轴、Z轴，和时间轴T构成的四维空间描述。要规划的机器人路径在X轴、Y轴、Z轴和时间轴T上的投影用三次贝塞尔曲线表达，其表达式为：

[0023] x(τ)＝axτ3+bxτ2+cxτ+dx

[0024] y(τ)＝ayτ3+byτ2+cyτ+dy

[0025] (1)

[0026] z(τ)＝azτ3+bzτ2+czτ+dz

[0027] t(τ)＝atτ3+btτ2+ctτ+dt

[0028] 其中，x(τ)，y(τ)，z(τ)，t(τ)表示要规划的机器人路径上各路径点的位置坐标x、y、z和时间t；τ为贝塞尔曲线的参数，其取值为τ∈[0，1]。τ＝0时，x(τ)，y(τ)，z(τ)，t(τ)为机器人的起点位置(xs，ys，zs)，起点时间0；τ＝1时，x(τ)，y(τ)，z(τ)，t(τ)为机器人的期望到达的目标点位置(xg，yg，zg)，和期望的达到时间tf；贝塞尔曲线的各项系数包括：ax，bx，cx，dx为x(τ)的系数、ay，by，cy，dy为y(τ)的系数、az，bz，cz，dz为z(τ)的系数以及at，bt，ct，dt为t(τ)的系数。

[0029] 上述贝塞尔曲线的各项系数可以根据四个控制点唯一确定，每个控制点为(x，y，z，t)描述的四维空间点。假设三次贝塞尔曲线的四个控制点坐标分别为：控制点P1(x1，y1，z1，t1)，控制点P2(x2，y2，z2，t2)，控制点P3(x3，y3，z3，t3)，控制点P4(x4，y4，z4，t4)，由这四个控制点，可以求得对应的贝塞尔曲线的各项系数为：

[0030] ax＝-x1+3x2-3x3+x4 ay＝-y1+3y2-3y3+y4

[0031] bx＝3x1-6x2+3x3， by＝3y1-6y2+3y3，

[0032] cx＝-3x1+3x2 cy＝-3y1+3y2

[0033] dx＝x1 dy＝y1

[0034] az＝-z1+3z2-3z3+z4 at＝-t1+3t2-3t3+t4

[0035] bz＝3z1-6z2+3z3， bt＝3t1-6t2+3t3， (2)

[0036] cz＝-3z1+3z2 ct＝-3t1+3t2

[0037] dz＝z1 dt＝t1

[0038] 利用机器人起点位置坐标、期望到达的目标点位置坐标和期望的到达时间tf确定贝塞尔曲线的控制点P1和P4。在τ＝0时，令贝塞尔曲线的控制点P1(x1，y1，z1，t1)，为机器人的起点位置(xs，ys，zs)且t1＝0；在τ＝1时，令贝塞尔曲线的控制点P4(x4，y4，z4，t4)，为机器人期望到达的目标点位置(xg，yg，zg)，且t4＝tf，其中tf为机器人期望的到达时间。只需再确定两个中间控制点P2、P3即可规划出机器人的路径。

[0039] 在去除时间轴一维的机器人路径上，各点处的切向方向与该点处速度方向一致，因此，控制点P1与控制点P2的连线与控制点P1处的切向方向一致，即与机器人起点位置处的速度方向一致；控制点P3与控制点P4的连线与控制点P4处的切向方向一致，即与机器人的目标点位置处的速度方向一致。

[0040] 故两个中间控制点P2、P3的X轴、Y轴、Z轴坐标可以表示为如下公式：

[0041] P2＝(x1，y1，z1)+k1×v1/|v1|

[0042] ，0＜k1，k2＜|P1P4| (3)

[0043] P3＝(x4，y4，z4)-k2×v4/|v4|

[0044] 其中，(x1，y1，z1)为控制点P1的X轴、Y轴、Z轴坐标，即机器人起点位置(xs，ys，x y zzs)；v1表示起点位置处的机器人速度(vs，vs，vs)，|v1|表示起点位置处的机器人速度值，v1/|v1|为起点位置处的机器人速度方向；(x4，y4，z4)为控制点P4的X轴、Y轴、Z轴坐标，即x y
机器人期望到达的目标点位置(xg，yg，zg)；v4为机器人到达目标点位置时的速度(vg，vg，z
vg)，|v4|表示到达目标点位置时的机器人速度值，v4/|v4|为机器人到达目标点位置时的速度方向：k1为控制点P1和控制点P2间的距离 k2为控制点P3和控制点P4之间的距离

[0045] 公式(3)可用于确定两个中间控制点P2、P3在三维空间上的X轴、Y轴、Z轴坐标，但是为方便后续速度、加速度、转弯半径、期望的到达时间等约束条件判断，故需要确定各个路径点处的时刻。由于贝塞尔曲线控制点P1和贝塞尔曲线控制点P4对应的时刻分别为0和tf。设两个中间控制点P2、P3对应的时刻分别为t2，t3，显然t2＜t3。

[0046] 包含时间轴的中间控制点P2、P3的选取公式为：

[0047]

[0048]

[0049] 其中k1，k2为随机选取的实数，且满足：

[0050]

[0051] 式中，(x1，y1，z1)为控制点P1的X轴、Y轴、Z轴坐标，(x4，y4，z4)为控制点P4的X轴、Y轴、Z轴坐标，vsx，vsy，vsz为起点位置处机器人速度v1在X轴、Y轴、Z轴的投影；vgx，vgy，vgz为机器人到达目标点位置时的速度v4在X轴、Y轴、Z轴的投影。

[0052] 随机选择一组满足不等式 且k1/|v1|＜(tf-k2/|v4|)的实数k1，k2，就可利用公式(4)获得一组中间控制点P2、P3。由已知的控制点P1、P4和获得的P2、P3通过公式(2)可计算出公式(1)中贝塞尔曲线方程的各项的系数，将τ由0到1连续取值并带入公式(1)即可获得规划的连续路径，tf为机器人期望的到达时间。

[0053] 步骤S3：路径点参数计算单元3按路径离散化最小时间分辨率形成时间点序列，再依据步骤S2中获得的连续路径计算离散化路径，即计算各时间点处路径点坐标、速度，加速度，转弯半径；

[0054] 将时间点序列 带入公式(1)3 2
中t(τ)＝atτ+btτ+ctτ+dt求出对应的τ，再将τ带入公式(1)求出与时间点对应的路径点坐标。 为下取整函数， 且为整数，tmin为路径离散化最小时间分辨率。

[0055] 依据贝塞尔曲线表达式(1)，通过求导可以得到公式(5)，利用公式(5)可计算路径点处的速度v(τ)，与其在X轴、Y轴、Z轴的投影vx(τ)、vy(τ)、vz(τ)，如下示出：

[0056]

[0057]

[0058]

[0059]

[0060] 将与时间点序列对应的τ带入公式(5)可获得与时间点对应的速度。

[0061] 利用公式(1)和公式(5)，可获得公式(6)计算路径点处的加速度a(τ)，与其在X轴、Y轴、Z轴的投影ax(τ)、ay(τ)、az(τ)：

[0062]

[0063]

[0064]

[0065]

[0066] 将与时间点序列对应的τ带入公式(6)可获得与时间点对应的加速度。

[0067] 规划的路径上任意一点处的曲率κ可以根据公式(7)求取。

[0068]

[0069] 其中，x′，y′，z′分别表示在路径上某点处其位置坐标x(τ)，y(τ)，z(τ)对参数τ的一阶导数，x″，y″，z″分别表示在路径上某点处其位置坐标x(τ)，y(τ)，z(τ)对参数τ的二阶导数，x′，y′，z′和x″，y″，z″的计算公式如下。

[0070] x′＝dx(τ)/dτ＝3axτ2+2bxτ+cx

[0071] y′＝dy(τ)/dτ＝3ayτ2+2byτ+cy

[0072] z′＝dz(τ)/dτ＝3azτ2+2bzτ+cz

[0073] x″＝d2x(τ)/dτ2＝6axτ+2bx

[0074] y″＝d2y(τ)/dτ2＝6ayτ+2by

[0075] z″＝d2z(τ)/dτ2＝6azτ+2by (8)

[0076] 将与时间点序列对应的τ代入公式(8)，再将公式(8)代入公式(7)即可获得与时间点对应的路径点处曲率。曲率k的倒数即为该路径点处的转弯半径R，即R＝1/k。

[0077] 步骤S4：路径点参数检测单元4依据约束条件检测步骤S3中获得的与时间点相应的各路径点处的速度、加速度、转弯半径是否满足约束条件，如果不满足约束条件，则跳转至步骤S2，重新生成控制点；如果满足约束条件，则跳转至步骤S5。

[0078] 将对应于时间点序列 的各路径点速度、加速度和转弯半径代入不等式进行判断，如有任一路径点的速度v(τ)、加速度a(τ)和转弯半径R不满足不等式(9)，即为不满足约束条件。

[0079] vmin＜v(τ)＜vmax，

[0080] amin＜a(τ)＜amax，

[0081] Rmin＜R， (9)

[0082] 其中vmin，vmax为最小、最大速度，amin、amax为最小、最大加速度，Rmin为最小转弯半径约束，R为转弯半径。

[0083] 步骤S5：规划路径输出单元5输出规划的满足约束条件的机器人路径。

[0084] 输出规划的机器人路径，包括步骤S2中贝塞尔曲线控制点P1、P2、P3、P4，贝塞尔曲线的各项系数ax，bx，cx，dx，ay，by，cy，dy，az，bz，cz，dz，at，bt，ct，dt，步骤S3中的时间点序列 对应时间点序列的路径点X轴、Y轴和Z轴坐标、速度、加速度和转弯半径；

[0085] 以上所述，仅为本发明中的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭露的技术范围内，可理解想到的变换或替换，都应涵盖在本发明的权利要求书的保护范围之内。