一种时变路径规划方法与流程

1.本发明属于机械臂路径规划技术领域，具体涉及一种时变路径规划方法。


背景技术：

2.随着时代的发展，越来越多的设备被用于取代人力。国内外学者针对于机械臂、机器人、小车、遥控飞机的路径规划也展开了大量研究。蛇形机械臂具备很强的灵活性，不仅可以实现基础任务，还可实现避障等额外任务，但目前与蛇形机械臂躲避动态障碍物相关的实时路径规划的研究较少。
3.现有技术中，一般运用贝塞尔曲线规划类似于汽车的单点运动对象的路径，这类路径规划方法为在静态障碍物环境中进行的路径规划，无法实现复杂运动对象，在动态环境中避障的任务。因此，需要提出一种可以规划出时变路径的新型规划方法，使得蛇形机械臂沿着该时变重复路径运动时，可实现避障。


技术实现要素：

4.本发明的目的在于克服上述缺陷，提供一种时变路径规划方法，解决了传统路径规划方法效率低、无法适用于全局避障的技术问题，进一步解决了传统路径规划方法得到的规划路径不光滑的技术问题，本发明能够有效提高路径规划效率，实时调节光滑路径，特别适用于蛇形机械臂等运动对象的全局动态避障。
5.为实现上述发明目的，本发明提供如下技术方案：
6.本发明一种时变路径规划方法，针对不同位置、不同形状的障碍物，提取有限个障碍物特征点表征障碍物信息，求取障碍物中心点o0和障碍物模糊重心点。根据前面的铺垫求得路径曲线的特征点，进而根据特征点生成本时刻对应的路径曲线，比如贝塞尔样条曲线。本时刻求得的路径曲线作为本时刻的运动路径，令运动物体沿着该路径运动。本发明在动态的单障碍物或多障碍物环境中，根据路径规划算法规划出路径，然后根据路径上的节点和动态障碍物的特征点，实时调节路径，实现全局动态避障。
7.一种时变路径规划方法，包括：
8.s1根据任务需求，确定机械臂的路径初始数据；路径初始数据包括路径的起始点x
start
和终止点x
end
；
9.s2获取当前时刻障碍物的特征信息；障碍物的特征信息包括障碍物的形状数据、障碍物的位置数据或障碍物的尺寸数据；
10.s3根据机械臂的路径初始数据和障碍物的特征信息判断是否需要避障；
11.当无需避障时生成路径，结束路径规划；
12.当需要避障时，根据机械臂的路径初始数据和障碍物的特征信息得到无需避障路径在障碍物空间内部分的中心点o0，进入步骤s4；
13.s4获取障碍物的特征点，根据障碍物的特征点确定障碍物的模糊中心c，根据障碍物的模糊中心c和机械臂的路径初始数据确定路径绕过障碍物的方向；
14.s5根据中心点o0和路径绕过障碍物的方向确定路径的特征点；
15.s6根据路径的特征点生成当前时刻的路径；
16.s7在下一时刻，返回步骤s2。
17.进一步的，步骤s1中，初始数据还包括起始点方向d
start
或终止点方向d
end
。
18.进一步的，步骤s3中，根据机械臂的路径初始数据和障碍物的特征信息判断是否需要避障的方法包括：
19.连接起始点x
start
与终止点x
end
生成一条直线，根据障碍物的特征信息判断该直线与障碍物是否相交，如果不相交则判断为无需避障，如果相交则判断为需要避障；
20.当无需避障时，将该直线作为生成的路径，结束路径规划；
21.当需要避障时，将该直线与障碍物相交部分的线段的中点作为中心点o0。
22.进一步的，步骤s3的方法包括：
23.s3.1设定左动点x
left_temp
的初始位置为x
start
；,
24.s3.2基于障碍物的特征信息，判断x
left_temp
是否落入障碍物空间内，如果落入障碍物空间内，则将x
left_temp
作为该直线与障碍物相交部分的线段的第一端点x
left
，进入步骤s3.4,如果未落入障碍物空间内，则进入步骤s3.3；
25.s3.3令x
left_temp
向x
end
运动步长δ；
26.判断x
left_temp
是否抵达或超出x
end
，如果抵达或超出x
end
则输出以x
start
和x
end
为两端点的直线路径，结束路径规划，如果未抵达或超出x
end
则返回步骤s3.2；
27.s3.4设定右动点x
right_temp
的初始位置为x
end
；
28.s3.5基于障碍物的特征信息，判断x
right_temp
是否落入障碍物空间内，如果落入障碍物空间内，则将x
right_temp
作为该直线与障碍物相交部分的线段的第二端点x
right
，进入步骤s3.7,如果未落入障碍物空间内，则进入步骤s3.6；
29.s3.6令x
right_temp
向x
start
运动步长δ并返回步骤s3.5；
30.s3.7将以x
left
和x
right
为两端点的线段的中点作为中心点o0。
31.进一步的，步骤s4中，障碍物的特征点包括障碍物的拐点；
32.障碍物的模糊中心c坐标为各障碍物的特征点坐标的平均值。
33.进一步的，步骤s4中，当二维障碍物为无拐角的图形，障碍物的特征点根据如下方法确定：
34.连接起始点x
start
与终止点x
end
生成第1条直线；
35.过中心点o0作第2～n条直线，n条直线中，相邻两条直线的夹角θ，θ＝180
°
/(n-1)；n≥3；
36.将n条直线与障碍物边界的交点作为障碍物的特征点；
37.当二维障碍物为多边形，障碍物的特征点包括多边形的顶点。
38.如果障碍物为不规则图形，也可以将其等效为包含该障碍物的规则多边形或圆形一类的规则形状，即将不规则图形等效为规则图形。
39.对于三维障碍物，也可以通过上述方法确定障碍物特征点，对于无拐点的障碍物，可以过中心点o0作若干条均匀分布的直线，将这些直线与障碍物边界的交点作为障碍物特征点，对于有拐点的障碍物，可以将拐点作为障碍物特征点，或者将拐点和直线与障碍物边界的交点共同作为障碍物的特征点。可将坐标值突变超过预定阈值的点作为拐点，预定阈
值可根据实际任务需求和精确度设定。
40.进一步的，根据障碍物的模糊中心c确定路径绕过障碍物的方向的方法包括：
41.连接起始点x
start
与终止点x
end
生成一条直线；
42.如果障碍物的模糊中心c在该直线的一侧，则路径从该直线另一侧绕过障碍物，将位于该侧的障碍物的特征点记为障碍物的有效特征点；
43.如果障碍物的模糊中心c落在该直线上，则路径从该直线的任意一侧绕过障碍物。
44.进一步的，设路径的特征点为p
i0
，i＝1,2,
…
n；
45.步骤s5中，根据中心点o0和路径绕过障碍物的方向确定路径的特征点的方法包括：
46.s5.1将x
start
和x
end
分别作为p
10
和
47.s5.2以中心点o0为中心点放大障碍物的边界，将放大后边界及其内部的点集记为集合k；
48.s5.3过x
start
点做沿第一特定方向的直线，延长该直线直至与集合k相交，将该交点作为过x
end
点做沿第二特定方向的直线，延长该直线直至与集合k相交，将该交点作为
49.当设有起始点方向d
start
或终止点方向d
end
约束时，第一特定方向和第二特定方向分别为d
start
和d
end
；
50.当未设置起始点方向d
start
或终止点方向d
end
约束时，第一特定方向和第二特定方向均为过x
start
点和x
end
点的连线方向；
51.s5.4将集合k中，与障碍物的有效特征点相对应的点作为
52.或，建立以中心点o0为原点，x
start
点和x
end
点的连线方向为x轴，+y轴垂直于x轴的坐标系且指向路径绕过障碍物的方向，令n＝6，
[0053][0054]
其中，min(k,x)为集合k中所有点在x轴方向的最小值，max(k,y)为集合k中所有点在y轴方向的最大值，max(k,x)为集合k中所有点在x轴方向的最大值。
[0055]
进一步的，步骤s6中，生成当前时刻的路径曲线为圆弧、正余弦曲线、n次多项式曲线、贝塞尔样条曲线或b样条曲线；
[0056]
步骤s6中，当生成的路径曲线为圆弧、正余弦曲线、n次多项式曲线或b样条曲线时，路径曲线为通过各路径的特征点的平滑曲线；
[0057]
当生成的路径曲线为贝塞尔样条曲线时，根据路径的特征点生成n-1阶贝塞尔曲线，利用n-1阶贝塞尔曲线确定贝塞尔样条曲线。
[0058]
进一步的，当生成的路径曲线为贝塞尔样条曲线时：
[0059]
首先利用n-1阶贝塞尔曲线和比例因子κ得到贝塞尔样条曲线的函数p(κ)；
[0060]
将p(κ)转换为沿二维正交坐标轴方向的变化函数p
x
(κ)和py(κ)；
[0061]
根据p
x
(κ)和py(κ)计算贝塞尔样条曲线的曲率半径r：
[0062]
[0063]
其中，p
′
x
为p
x
(κ)对及比例因子κ的一阶导数；p
′y为py(κ)对及比例因子κ的一阶导数；p
″
x
为p
x
(κ)对及比例因子κ的二阶导数；p
″y为py(κ)对及比例因子κ的二阶导数；
[0064]
控制比例因子κ，使曲率半径r满足蛇形机械臂沿着该路径运动时，关节偏角始终在限制范围之内。
[0065]
本发明与现有技术相比具有如下至少一种有益效果：
[0066]
(1)本发明创造性的提出一种时变路径规划方法，针对不同位置、不同形状的障碍物，提取有限个特征点来表征该动态障碍物的关键信息，降低了障碍物模型的复杂程度，而实现在路径规划过程中，可以实现实时分析动态障碍物信息的目的，障碍物表面特征点为生成路径样条曲线提供了基础；
[0067]
(2)本发明通过构建中心点o0和模糊中心c准确的给出了绕过障碍物的最优方向，后续路径规划只考虑该方向位置处的障碍情况即可，能够有效提高路径规划效率；
[0068]
(3)本发明提出了路径特征点的确定方法，过程准确可靠，具有较强的可操作性，能够保证蛇形机械臂的实时全局避障；
[0069]
(4)本发明在实时路径规划前，先制定好特征点与路径之间的转换关系(步骤5、步骤6、步骤7)。在实时路径规划时，只需带入特征点的实时数据，即可生成对应的样条曲线，此时的样条曲线即为蛇形机械臂的运动路径，大大降低了计算时间。
[0070]
(5)本发明借助贝塞尔曲线的特性，可直接规划出光滑路径，然后根据路径上的节点和动态障碍物的特征点，实时调节光滑路径。
[0071]
(6)本发明借助贝塞尔曲线的全局特性，可通过实时调节障碍物特征点、曲线特征点，实时调节全局路径。
[0072]
(7)本发明提取出动态障碍物的实时信息，利用特征点生成对应的光滑路径曲线，可实现机械臂的实时避障。避免了大量不必要的计算，提高了搜索效率，能够高效求解多目标条件或多障碍物环境下的路径规划问题。
[0073]
(8)本发明在路径规划过程中，可按照任务需求调控规划路径的起始点方向和终止点方向。
[0074]
(9)本发明中的方法具备多个可调节的参数：中心点o0位置、障碍物特征点个数与位置、缩放中心点位置、缩放比例系数，等，可以根据具体的应用环境来选择或调节各个参数，因此，本发明中的方法具备很好的环境适应性。
附图说明
[0075]
图1为本发明时变路径规划方法的流程图；
[0076]
图2为本发明针对矩形障碍物生成的贝塞尔曲线路径；
[0077]
图3为本发明针对圆形障碍物生成的贝塞尔曲线路径；
[0078]
图4为本发明针对非平行四边形障碍物生成的贝塞尔曲线路径；
[0079]
图5为本发明针对不规则形状障碍物生成的贝塞尔曲线路径；
[0080]
图6为本发明求取为生成路径曲线做铺垫的中心点的流程图；
[0081]
图7为本发明κ＝0.3时的贝塞尔曲线生成过程图；
[0082]
图8为本发明多障碍物环境下的避障曲线图；
[0083]
图9为本发明多障碍物环境下的机械臂运动过程图；
[0084]
图10为本发明臂杆末端点与障碍物的最近距离的变化曲线。
具体实施方式
[0085]
下面通过对本发明进行详细说明，本发明的特点和优点将随着这些说明而变得更为清楚、明确。
[0086]
在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。
[0087]
本发明提出一种时变路径规划方法，针对于不同位置、不同形状的障碍物，提取有限个特征点来表征障碍物信息，根据特征点生成此时刻对应的样条曲线。根据路径规划算法规划出路径，然后根据路径上的节点和动态障碍物对应的特征点，实时调节路径，从而使得蛇形机械臂沿着该时变重复路径运动时，可实现避障。
[0088]
本发明采用一种时变路径规划方法，该方法的流程图如图1所示：
[0089]
步骤1：首先构建运动对象的运动学模型并输入初始数据a：起始点的位置x
start
、方向d
start
，终止点的位置x
end
、方向d
end
、蛇形机械臂的模型；
[0090]
步骤2：获取障碍物的实时形状位置信息；
[0091]
步骤3：求得为生成曲线做铺垫的中心点o0；
[0092]
步骤4：从障碍物表面e中，提取出表征障碍物信息的表面特征点ei，获取障碍物的模糊中心c，并判断运动物体从障碍物的哪边走；
[0093]
步骤5：根据制定的比例系数k与放大中心点o0，放大障碍物的表面，得到集合k，并按照一定规律生成路径曲线的特征点p
i0
；
[0094]
步骤6：根据曲线特征点生成贝塞尔曲线，输出本时刻的暂态曲线。
[0095]
通过本发明方法可得到各个时刻对应的路径曲线，在本发明中以图2、图3、图4、图5所示的四个例子，讲述本发明的过程与原理。图中的x轴方向是从起始点x
start
指向终止点x
end
的方向，y轴与x轴垂直。本发明适应于蛇形机械臂、冗余机械臂、连续型机械臂、移动小车、移动机器人、遥控飞机等运动对象。本发明以蛇形机械臂为例，具体讲述过程与原理。
[0096]
步骤1，输入初始数据a，具体如下：
[0097]
根据任务需求，设定机械臂运动路径对应的起始点x
start
与终止点x
end
，如果路径开始与结束时的方向也有约束，则设定起始点方向d
start
与终止点方向d
end
。这四个参数之间的关系如下式所示：
[0098][0099]
例如图4中的d
start
与d
end
都为-30
°
，图2、图3、图5中的d
start
与d
end
都为0
°
，故未做标注。
[0100]
将起始点x
start
、终止点x
end
、起始点方向d
start
、终止点方向d
end
这些初始数据记为初始数据a。
[0101]
该步骤中，还可以考虑根据运动对象搭建对应的d-h坐标系；构建转换矩阵，分析各个运动学参数之间的转换关系。规划完路径后，运动物体(机械臂)沿着该路径运动时，可以得到运动物体(机械臂)系统中的各个参数的变化规律。
[0102]
步骤2，获取障碍物在本时刻的信息，并提取出关键信息，具体如下：
[0103]
(2.1)获取障碍物信息的方式有多种，例如通过图像处理、构建稀疏地图等，在本发明中不具体讲述；
[0104]
(2.2)获取障碍物信息后，需提取出关键信息，在本发明中称之为障碍物的特征信息，将障碍物的这些特征信息存储在一个矩阵中，该矩阵记为b；
[0105]
(2.3)b包括：占据一位存储空间的障碍物的形状数据b(1)、占据六位存储空间的三维或二维空间中的位置数据b(2:7)、尺寸数据b(8:)；b为一个数组，b(2:7)表示该数据在数组中存储为第2位到第7位。
[0106]
其中，只需一个数据存储障碍物的形状数据；通过x、y、z、θ
x
、θy、θz这六个数据存储三维障碍物的位置数据；或者通过x、y、θz这三个数据存储二维障碍物的位置数据，z、θ
x
、θy这三个数据始终为零；不同形状障碍物的尺寸数据占用的数据个数也不同，例如：通过半径r这一个数据存储球型障碍物的尺寸数据、通过初始数据长a、宽b、高c这三个数据存储长方体障碍物的尺寸数据、通过各个顶点的初始位置信息存储不规则的多面体障碍物的尺寸数据。
[0107]
障碍物的形状数据b(1)与尺寸数据b(8:)不随着时间而改变，但动态障碍物的位置数据b(2:7)会随着时间而改变，例如发生平移、旋转等运动。因此，需要实时重复步骤2提取动态障碍物的信息，为实时避障获取初始数据。
[0108]
在步骤1、步骤2中，分别提取了不需要更新的初始数据a与需要实时更新的初始数据b。
[0109]
步骤3，通过初始数据a中的两端点x
start
、x
end
和障碍物特征信息b，求得中心点o0，为后面步骤中的放大做铺垫，具体通过下述方式实现：
[0110]
(3.1)连接起始点x
start
与终止点x
end
生成一条直线；
[0111]
(3.2)如果该直线不与障碍物相交，即无中心点o0，则不需要考虑避障问题，可令运动对象从起始点x
start
沿着该直线向终止点x
end
运动；
[0112]
(3.3)如果该直线有一部分线段在障碍物里面，这条线段的两端分别用障碍物左端点x
left
和障碍物右端点x
right
表示，线段的中点即为中心点，图2-图5中标记为o0，求解中心点o0的流程图如图6所示，具体步骤如下；
[0113]
(3.4)在输入障碍物的特征信息b以及起始点x
start
、终止点x
end
之后，为了找到障碍物左端点x
left
，令左动点x
left_temp
按照制定的步长δ逐步从x
start
向x
end
运动，计算公式如下所示：
[0114]
x
left_temp
＝x
start
+n1·
δ
[0115]
式中的n1为步数，n1＝1，2，3，
……
[0116]
(3.5)每走一步就判断该步中的x
left_temp
是否在障碍物空间内，如果不在障碍物空间内，则分析是否抵达或超出x
end
点。如果是，则无需考虑避障问题；如果未抵达，则令x
left_temp
继续向x
end
运动，再次循环(3.4)和(3.5)；
[0117]
(3.6)如果x
left_temp
在障碍物空间内，则将当前的x
left_temp
视为障碍物左端点x
left
；
[0118]
(3.7)在确定好障碍物左端点x
left
之后，找寻障碍物右端点x
right
。令右动点x
right_temp
按照制定的步长δ逐步从x
end
向x
start
运动，计算公式如下所示：
[0119]
x
right_temp
＝x
end-n2·
δ
[0120]
式中的n2为步数，n2＝1，2，3，
……
[0121]
(3.8)每走一步就判断该步中的x
right_temp
是否在障碍物空间内，如果不在，则令x
right_temp
继续向x
start
运动，再次循环(3.7)和(3.8)；
[0122]
(3.9)如果x
right_temp
在障碍物空间内，则将当前的x
right_temp
视为障碍物右端点x
right
；
[0123]
(3.10)确定好x
left
与x
right
之后，计算出中心点o0，计算公式如下所示：
[0124]
o0＝(x
right
+x
left
)/2
[0125]
o0实际是x
right
与x
left
中间的点，在后续步骤中作为o0比例放大中心点，也可以看做是拐弯部分路径规划中心点。
[0126]
步骤4，从障碍物表面e中，提取出表征障碍物信息的表面特征点ei，获取障碍物的模糊中心c，并判断运动物体从障碍物的哪边走，具体如下：
[0127]
(4.1)根据任务自行设定提取特征点的个数及特征点的位置，特征点可根据起始点x
start
与终止点x
end
的位置确定，或者将拐点作为特征点，同时需要考虑障碍物类型及拐点数目。
[0128]
(4.2)将障碍物表面的拐点(比如转折点和棱角)，设为障碍物特征点ei，例如图2中的ei即为矩形障碍物特征点，这些点表征障碍物形状及大小，表征障碍物的关键信息，根据这几个障碍物特征点确定模糊重心c，可减少计算量；
[0129]
具体的，部分特殊形状的障碍物无拐点，可按照制定的规则设定障碍物特征点，例如在图3所示的圆形障碍物中，过o0点做相邻夹角为60
°
的四条直线，直线与圆形障碍物的交点ei视为障碍物特征点。通过该方法针对圆形障碍物设立特征点，可保证生成的路径曲线始终都在障碍物外边；
[0130]
具体的，三角形障碍物可提取三个ei，四边形障碍物提取四个ei，n边形提取n个ei。在得到这些障碍物特征点后，取其平均即为障碍物的模糊重心c。计算公式如下：
[0131][0132]
式中的w为障碍物特征点的个数，ei为障碍物特征点的位置数据。
[0133]
(4.5)通过判断模糊重心c在起始点x
start
与终止点x
end
连线的哪边，从而决定运动物体从障碍物的哪边走，即确定规划的路径从哪边(是位于障碍物上面还是下面)绕过障碍物。如果c在x
start
与x
end
连线的下面，说明障碍物大部分的空间在连线下面，那么运动物体从障碍物上面走，即从障碍物上面规划路径。如果c在x
start
与x
end
连线的上面，说明障碍物大部分的空间在连线上面，那么运动物体从障碍物下面走，即从障碍物下面规划路径。图2、图3、图4、图5中的模糊重心c在连线下面，所以在障碍物的上部分区域规划路径。
[0134]
步骤5，提取路径曲线的特征点p
i0
，具体如下：
[0135]
(5.1)将障碍物表面所有点的集合记为集合e，以o0点为中心点放大障碍物的边界，放大比例为k，将放大后的边界上的所有点的集合记为集合k。为了避免规划的路径不与障碍物相交，放大比例k必须大于1。k越大则规划出的路径越远离障碍物，但k过大会造成路径长度变长。综合考虑避障和路径长度尽可能短这两要素，本发明优选将放大比例k设为2，集合e与集合k满足以下关系：
[0136]
k＝k
·
e-o0[0137]
(5.2)将需要进行路径规划部分(比如图2、图3、图4、图5中，在x
start
与x
end
连线的上部分区域规划路径)的ei放大，放大的后的点视为路径曲线特征点(例如图2、图3、图4)，或者将坐标最大值点视为曲线特征点(例如图5)。本步骤中，针对一些障碍物(如图5中的障碍物)，放大的后的点视为路径曲线特征点，但规划出的路径无法满足避障的条件。可采用两种解决方式：(1)增大放大比例k；(2)将坐标最大值点视为曲线特征点，即在放大后的集合k中，选择极值端点。根据运动物体从障碍物的哪边走，在放大后的集合k中，选择路径曲线的中间的特征点p
i0
，如果路径曲线为贝塞尔时，这些点之间的连线为贝塞尔多边形。在图像中可以具体表示，例如图2中的线段满足以下关系：
[0138][0139]
图3中的线段满足以下关系：
[0140][0141]
图4中的线段满足以下关系：
[0142][0143]
图5中的点满足以下关系：
[0144][0145]
式中的min(k,x)是集合k中所有点在x轴方向的最小值。
[0146]
(5.3)将起始点x
start
与终止点x
end
设为曲线特征点中的第一个点p
10
和最后一个点(一共n个曲线特征点)；
[0147]
(5.4)过x
start
点(起始点)做方向为d
start
(起始方向)的直线，延长该直线直至与集合k相交，将该交点设置为曲线特征点中的第二个点
[0148]
(5.5)过x
end
点(终止点)做方向为d
end
(终止方向)的直线，延长该直线直至与集合k相交，将该交点设置为曲线特征点中的倒数第二个点
[0149]
(5.6)通过(5.1)和(5.2)求得曲线特征点中间的几个点：通过(5.3)设定了曲线特征点中的第一个点p
10
和最后一个点通过(5.4)求得曲线特征点中的第二个点通过(5.5)求得曲线特征点中的倒数第二个点总之，根据步骤4获取的障碍物特征点ei和路径规划的方向，求得了所有的曲线特征点p
i0
。
[0150]
步骤6，根据曲线特征点p
i0
生成贝塞尔曲线，具体如下：
[0151]
表征路径的曲线可以是圆弧、正余弦曲线、n次多项式曲线、贝塞尔样条曲线、b样条曲线等，贝塞尔样条曲线与b样条曲线是通过拟合这些特征点，从而形成曲线，其余曲线是通过插值这些特征点，从而形成曲线。
[0152]
本发明以贝塞尔曲线为例，讲述如何利用曲线特征点规划出时变的路径。
[0153]
根据障碍物可得到n个特征点：通过这些特征点及比例因子κ，可生成n-1阶贝塞尔曲线。从p
10
到由各特征点形成的折线构成的多边形，在本发明中称为特征多边形。图7以κ＝0.3为例，经过第1次迭代后的点满足以下关系：
[0154][0155]
经过第2次迭代后的点满足以下关系：
[0156][0157]
经过第3次迭代，可得到贝塞尔曲线上的点，其满足以下关系：
[0158][0159]
图中的点及线段满足以下关系：
[0160][0161][0162]
式中p
ij
为第j次迭代后得到的第i个点，p(κ)为表征贝塞尔曲线的函数，κ∈[0,1]。当κ从0变为1，则生成了如图7所示的n＝4个顶点定义的一条三次贝塞尔曲线。
[0163]
以此类推，可得到由n+1个顶点定义的n次贝塞尔曲线p(κ)满足下式：
[0164][0165]
式中为概率论中表示组合方式的数量。
[0166]
贝塞尔曲线对应的p(κ)可整理为如下所示的矩阵形式：
[0167]
p(κ)＝pk1(1-κ)k2(κ)h
t
[0168]
式中的矩阵p、k1、k2、h分别如下：
[0169][0170]
k1(1-κ)＝[(1-κ)n,(1-κ)
n-1
,
…
,(1-κ)0]
·in
[0171]
k2(κ)＝[κ0,κ1,
…
,κn]
·in
[0172]ht
＝[λ1,λ2,
…
,λn]
[0173]
式中的p为特征点的几何矩阵；k1、k2为参数κ相关的对角矩阵；h为权重矩阵；为权重系数。
[0174]
将k1、k2、p(κ)这些变量对κ求导，得：
[0175][0176][0177][0178][0179]
只需将具体的κ值带入，上述求导公式，即可提前求解出与κ相关的矩阵。按照期望来调节权重矩阵即可得到最终的改进曲线p(κ)及其一二阶导数，即得到本时刻最终的贝塞尔曲线。
[0180]
在二维空间中的曲线p(κ)沿着x轴与y轴方向会发生变化，对应的变化函数，可写为p
x
(κ)与py(κ)。该曲线对应的曲率半径r的计算公式如下：
[0181][0182]
式中，p
′
x
为p
x
(κ)对参数κ的一阶导数；p
′y为py(κ)对参数κ的一阶导数；p
″
x
为p
x
(κ)对参数κ的二阶导数；p
″y为py(κ)对参数κ的二阶导数。
[0183]
可通过控制公式中的参数，来保证贝塞尔曲线的曲率半径r始终在规定的范围内，从而保证类似于蛇形机械臂的运动对象沿着该路径运动时，关节偏角始终在限制范围之内。
[0184]
执行完步骤6之后生成了本时刻对应的贝塞尔曲线，在步骤7中将本时刻求得的贝塞尔曲线作为暂态运动路径，输出作为运动物体的运动路径。然后，在下一时刻重复执行步骤2、步骤3、步骤4、步骤5、步骤6、步骤7。
[0185]
以上过程是针对于一个动态障碍物，生成障碍物特征点、路径特征点，并最终路径曲线的过程。利用本发明中的方法，生成贝塞尔曲线的过程如下：生成每个障碍物的特征点，从中筛选出主要的障碍物特征点，然后生成曲线特征点，根据这些特征点，生成光滑过渡的贝塞尔曲线，运动对象沿着该路径运动时，可实现动态避障。针对于多个障碍物生成贝塞尔曲线的方法、原理，与针对于单个障碍物生成贝塞尔曲线的方法、原理类似，在此不再详细叙述。
[0186]
在多障碍物环境中，某一时刻生成的避障曲线图如图8所示，图中包括“l型障碍物”、“长方形障碍物”、“三角形障碍物”、“圆形障碍物”、“五角星障碍物”，当这五个障碍物沿着运动路径运动时，利用本发明中的方法实时生成贝塞尔曲线，从而保障运动物体在运动过程中还可实现避障。
[0187]
上述为本发明对应的一种基于贝塞尔曲线的时变路径规划方法的过程，下面通过理论推导及仿真，证明本发明的有效性与实用性。
[0188]
首先，通过理论推导证明本发明的有效性与实用性。利用本发明规划出的路径，可保障运动物体始终不会碰到障碍物，并可以通过调节放大系数(放大比例k)，来调节运动物体与障碍物之间的距离。下面以圆形障碍物为例，叙述证明过程。
[0189]
若以o0为原点，则图3中的e1、e2、e3、e4四个点的坐标分别为：
[0190][0191]
式中的r为圆形障碍物的尺寸信息：圆的半径；y0为点o0(中心点)与点c(模糊重心)之间的距离。
[0192]
通过本发明中的方法可得到六个特征点，进而推导出五阶贝塞尔曲线的表达式：
[0193][0194]
带入这6个点，可分析出当κ＝0.5时，贝塞尔曲线与障碍物之间的距离最近。令κ＝0.5，整理可得：
[0195][0196]
保证曲线在障碍物外边，即：
[0197][0198]
整理可得：
[0199][0200]
带入ξ，并整理，可得：
[0201][0202]
令y0＝k
·
r，其中的k∈(0,1)。带入化简得：
[0203][0204]
进一步整理可得：
[0205][0206]
令上式中的不等号左边为f(k)，即：
[0207][0208]
对f(k)求导，分析该函数在区间(0,1)之间的增减性，可得在区间之间为增函数，在区间之间为减函数，两个极小值点f(k＝0)＝1.32与f(k＝1)＝0都
满足大于等于0的条件。通过此处的理论推导，可以得到贝塞尔曲线始终都不触碰障碍物的结论。
[0209]
接下来，通过仿真来证明本发明的有效性与实用性。借助仿真软件，输入“l型障碍物”、“长方形障碍物”、“三角形障碍物”、“圆形障碍物”、“五角星障碍物”这五个障碍物的所有特征点生成一条运动路径，通过本发明中的方法可实时生成障碍物特征点、曲线特征点、贝塞尔曲线，蛇形机械臂沿着实时路径运动时，可保证始终不触碰障碍物。某一时刻生成的路径如图8所示，图中的*号是每个障碍物的特征点，曲线为每个障碍物的运动路径，围绕障碍物的形状为障碍物放大后的边界。多障碍物环境下的机械臂运动过程图如图9所示，图中为机械臂在不同时刻的运动状态。在运动的过程中，本发明中举例的运动物体是一种机械臂，该机械臂一共8个臂杆，第i个臂杆末端点(也可称为第i个关节的中心点)与所有障碍物的最近距离为di，臂杆末端点与障碍物距离的变化曲线如图10所示，图10中，运动物体(机械臂)一共8个臂杆，第i个臂杆末端点与所有障碍物的最近距离为di，图中的d8是关键点(每个臂杆末端点，或称之为关节中心点)与障碍物的最近距离，从图中可看出距离始终大于零。
[0210]
如果运动空间存在一个障碍物或多个障碍物，利用该发明可保证障碍物与机械臂(或者说是实时运动路径)始终实时保持一定的距离。
[0211]
本发明特别适用于蛇形机械臂、冗余机械臂、连续型机械臂、移动小车、移动机器人、遥控飞机等运动对象在动态变化的单障碍物环境以及动态变化的多障碍物环境中的路径规划。
[0212]
以上结合具体实施方式和范例性实例对本发明进行了详细说明，不过这些说明并不能理解为对本发明的限制。本领域技术人员理解，在不偏离本发明精神和范围的情况下，可以对本发明技术方案及其实施方式进行多种等价替换、修饰或改进，这些均落入本发明的范围内。本发明的保护范围以所附权利要求为准。
[0213]
本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。

技术特征：
1.一种时变路径规划方法，其特征在于，包括：s1根据任务需求，确定机械臂的路径初始数据；路径初始数据包括路径的起始点x
start
和终止点x
end
；s2获取当前时刻障碍物的特征信息；障碍物的特征信息包括障碍物的形状数据、障碍物的位置数据或障碍物的尺寸数据；s3根据机械臂的路径初始数据和障碍物的特征信息判断是否需要避障；当无需避障时生成路径，结束路径规划；当需要避障时，根据机械臂的路径初始数据和障碍物的特征信息得到无需避障路径在障碍物空间内部分的中心点o0，进入步骤s4；s4获取障碍物的特征点，根据障碍物的特征点确定障碍物的模糊中心c，根据障碍物的模糊中心c和机械臂的路径初始数据确定路径绕过障碍物的方向；s5根据中心点o0和路径绕过障碍物的方向确定路径的特征点；s6根据路径的特征点生成当前时刻的路径；s7在下一时刻，返回步骤s2。2.根据权利要求1所述的一种时变路径规划方法，其特征在于，步骤s1中，初始数据还包括起始点方向d
start
或终止点方向d
end
。3.根据权利要求1所述的一种时变路径规划方法，其特征在于，步骤s3中，根据机械臂的路径初始数据和障碍物的特征信息判断是否需要避障的方法包括：连接起始点x
start
与终止点x
end
生成一条直线，根据障碍物的特征信息判断该直线与障碍物是否相交，如果不相交则判断为无需避障，如果相交则判断为需要避障；当无需避障时，将该直线作为生成的路径，结束路径规划；当需要避障时，将该直线与障碍物相交部分的线段的中点作为中心点o0。4.根据权利要求3所述的一种时变路径规划方法，其特征在于，步骤s3的方法包括：s3.1设定左动点x
left_temp
的初始位置为x
start
；,s3.2基于障碍物的特征信息，判断x
left_temp
是否落入障碍物空间内，如果落入障碍物空间内，则将x
left_temp
作为该直线与障碍物相交部分的线段的第一端点x
left
，进入步骤s3.4,如果未落入障碍物空间内，则进入步骤s3.3；s3.3令x
left_temp
向x
end
运动步长δ；判断x
left_temp
是否抵达或超出x
end
，如果抵达或超出x
end
则输出以x
start
和x
end
为两端点的直线路径，结束路径规划，如果未抵达或超出x
end
则返回步骤s3.2；s3.4设定右动点x
right_temp
的初始位置为x
end
；s3.5基于障碍物的特征信息，判断x
right_temp
是否落入障碍物空间内，如果落入障碍物空间内，则将x
right_temp
作为该直线与障碍物相交部分的线段的第二端点x
right
，进入步骤s3.7,如果未落入障碍物空间内，则进入步骤s3.6；s3.6令x
right_temp
向x
start
运动步长δ并返回步骤s3.5；s3.7将以x
left
和x
right
为两端点的线段的中点作为中心点o0。5.根据权利要求1所述的一种时变路径规划方法，其特征在于，步骤s4中，障碍物的特征点包括障碍物的拐点；障碍物的模糊中心c坐标为各障碍物的特征点坐标的平均值。
6.根据权利要求5所述的一种时变路径规划方法，其特征在于，步骤s4中，当二维障碍物为无拐角的图形，障碍物的特征点根据如下方法确定：连接起始点x
start
与终止点x
end
生成第1条直线；过中心点o0作第2～n条直线，n条直线中，相邻两条直线的夹角θ，θ＝180
°
/(n-1)；n≥3；将n条直线与障碍物边界的交点作为障碍物的特征点；当二维障碍物为多边形，障碍物的特征点包括多边形的顶点。7.根据权利要求1所述的一种时变路径规划方法，其特征在于，根据障碍物的模糊中心c确定路径绕过障碍物的方向的方法包括：连接起始点x
start
与终止点x
end
生成一条直线；如果障碍物的模糊中心c在该直线的一侧，则路径从该直线另一侧绕过障碍物，将位于该侧的障碍物的特征点记为障碍物的有效特征点；如果障碍物的模糊中心c落在该直线上，则路径从该直线的任意一侧绕过障碍物。8.根据权利要求7所述的一种时变路径规划方法，其特征在于，设路径的特征点为p
i0
，i＝1,2,
…
n；步骤s5中，根据中心点o0和路径绕过障碍物的方向确定路径的特征点的方法包括：s5.1将x
start
和x
end
分别作为p
10
和s5.2以中心点o0为中心点放大障碍物的边界，将放大后边界及其内部的点集记为集合k；s5.3过x
start
点做沿第一特定方向的直线，延长该直线直至与集合k相交，将该交点作为过x
end
点做沿第二特定方向的直线，延长该直线直至与集合k相交，将该交点作为当设有起始点方向d
start
或终止点方向d
end
约束时，第一特定方向和第二特定方向分别为d
start
和d
end
；当未设置起始点方向d
start
或终止点方向d
end
约束时，第一特定方向和第二特定方向均为过x
start
点和x
end
点的连线方向；s5.4将集合k中，与障碍物的有效特征点相对应的点作为或，建立以中心点o0为原点，x
start
点和x
end
点的连线方向为x轴，+y轴垂直于x轴的坐标系且指向路径绕过障碍物的方向，令n＝6，p
30
＝(min(k,x),max(k,y))，其中，min(k,x)为集合k中所有点在x轴方向的最小值，max(k,y)为集合k中所有点在y轴方向的最大值，max(k,x)为集合k中所有点在x轴方向的最大值。9.根据权利要求1所述的一种时变路径规划方法，其特征在于，步骤s6中，生成当前时刻的路径曲线为圆弧、正余弦曲线、n次多项式曲线、贝塞尔样条曲线或b样条曲线；步骤s6中，当生成的路径曲线为圆弧、正余弦曲线、n次多项式曲线或b样条曲线时，路径曲线为通过各路径的特征点的平滑曲线；当生成的路径曲线为贝塞尔样条曲线时，根据路径的特征点生成n-1阶贝塞尔曲线，利用n-1阶贝塞尔曲线确定贝塞尔样条曲线。10.根据权利要求9所述的一种时变路径规划方法，其特征在于，当生成的路径曲线为贝塞尔样条曲线时：
首先利用n-1阶贝塞尔曲线和比例因子κ得到贝塞尔样条曲线的函数p(κ)；将p(κ)转换为沿二维正交坐标轴方向的变化函数p
x
(κ)和p
y
(κ)；根据p
x
(κ)和p
y
(κ)计算贝塞尔样条曲线的曲率半径r：其中，p
′
x
为p
x
(κ)对及比例因子κ的一阶导数；p
′
y
为p
y
(κ)对及比例因子κ的一阶导数；p
″
x
为p
x
(κ)对及比例因子κ的二阶导数；p
″
y
为p
y
(κ)对及比例因子κ的二阶导数；控制比例因子κ，使曲率半径r满足蛇形机械臂沿着该路径运动时，关节偏角始终在限制范围之内。

技术总结
本发明公开了一种时变路径规划方法，包括：S1确定机械臂的路径初始数据，包括路径的起始点x


技术研发人员：贾龙飞 尹业成 李科 杨玉凯 宋洪舟 于志远 陈志鸿 陈婷
受保护的技术使用者：北京精密机电控制设备研究所
技术研发日：2023.04.21
技术公布日：2023/7/25