1.本发明属于机器人路径规划领域，具体涉及一种曲率连续的工业机器人路径光顺方法及系统。
背景技术：
2.工业机器人的运动路径通常包括圆弧段路径、直线段路径等不同类型的路径。为了提高机器人的运动平稳性和速度，解决机器人在拐角处的频繁加减速问题，不同类型的路径段之间需要进行光顺处理。直线段路径的特征是路径上曲率处处为零，因此从一个直线段路径运动到另一个直线段路径时，在拐角处光顺前后的曲率没有发生变化，相应的光顺方法有很多。然而，对于圆弧段路径，曲率可能为任意大于零的数值，因此从一个圆弧段路径运动到另一个圆弧段路径或者另一个直线段路径时，在拐角处光顺处理时需要考虑过渡曲线两端曲率不同的特点。因此，如何根据过渡段两侧不同类型的路径段进行光顺，成为了机器人路径光顺需要解决的问题。
3.现有技术中，有人采用五次b样条曲线过渡微小直线段，进行过渡转角处局部光顺，实现了拐角处c3连续，但是该方法是针对线下命令，没有涉及曲率变化的曲线；还有人提出c3连续的短线段过渡光顺算法，采用ph曲线保证了直线段和ph曲线交界处的平滑转接，该算法同样是针对g01指令对路径进行光顺。
4.现有技术中，还有采用nubrs曲线、五次和七次ph曲线、bezier曲线等不同样条曲线光顺微小直线段路径。例如，采用假设圆弧过渡法光顺圆弧段路径和圆弧段路径、圆弧段路径和直线段路径之间的转接，虽然可以针对不同直径和不同加工误差要求的圆弧段路径实现光顺，但是其设定的假设圆弧曲率和过渡前后的曲率不相等，导致在运动衔接点处出现运动的不平稳。
5.由此可见，现有技术光顺后的姿态路径实现了一定的曲率连续性和运动的平稳性，但是针对复杂的圆弧路径并没有考虑曲率连续的光顺，还欠缺对多种不同类型的路径光顺方法。
技术实现要素：
6.为克服现有技术中存在的缺陷，本发明提供一种曲率连续的工业机器人路径光顺方法及系统，采用含形状调节参数的五次贝塞尔曲线，实现了圆弧段路径和其他类型路径转接时的曲率连续，提高了工业机器人通过不同路径时的运动平稳性。
7.本发明方法所采用的技术方案为：一种曲率连续的工业机器人路径光顺方法，包括以下步骤：
8.读取给定的工业机器人的路径，计算路径上所有位置点处的曲率；
9.根据曲率是否出现突变，把路径划分为曲率突变区和曲率连续区；
10.对曲率突变区，计算该区域两个端点处的曲率、单位切向量和单位法向量；
11.定义含形状调节参数的五次贝塞尔曲线，用于光顺曲率突变区；
12.根据所述端点处的边界条件，以及根据曲率变化能和长度能量复合最小原则，定义五次贝塞尔曲线中形状调节参数的目标函数；
13.把形状调节参数求解问题转化为最小化目标函数问题，通过目标函数分别对形状调节参数求偏导数，采用拟牛顿法对目标函数进行求解，得到形状调节参数的数值；
14.将形状调节参数的数值代入五次贝塞尔曲线，得到曲率突变区的光顺路径。
15.本发明系统所采用的技术方案为：一种曲率连续的工业机器人路径光顺系统，包括以下模块：
16.曲率计算模块，用于读取给定的工业机器人的路径，计算路径上所有位置点处的曲率；
17.突变区划分模块，用于根据曲率是否出现突变，把路径划分为曲率突变区和曲率连续区；
18.突变区处理模块，用于计算曲率突变区的两个端点处的曲率、单位切向量和单位法向量；
19.贝塞尔曲线定义模块，用于定义含形状调节参数的五次贝塞尔曲线光顺曲率突变区；并根据所述端点处的边界条件，以及根据曲率变化能和长度能量复合最小原则，定义五次贝塞尔曲线中形状调节参数的目标函数；
20.形状调节参数求取模块，用于把形状调节参数求解问题转化为最小化目标函数问题，通过目标函数分别对形状调节参数求偏导数，采用拟牛顿法对目标函数进行求解，得到形状调节参数的数值；将形状调节参数的数值代入五次贝塞尔曲线，得到曲率突变区的光顺路径。
21.与现有技术相比，本发明取得的技术效果包括：
22.本发明采用含形状调节参数的五次贝塞尔曲线，实现了圆弧段路径和其他类型路径转接时的曲率连续，实现了机器人路径曲率连续的平滑过渡，提高了工业机器人通过不同路径时的运动平稳性。
附图说明
23.图1是本发明实施例中工业机器人路径光顺方法的流程图。
24.图2是本发明实施例1中未光顺的路径示意图。
25.图3是本发明实施例1中光顺后的路径示意图。
26.图4是本发明实施例1中给定路径和光顺曲线的曲率变化情况。
27.图5是本发明实施例2中未光顺的路径示意图。
28.图6是本发明实施例2中光顺后的路径示意图。
29.图7是本发明实施例2中给定路径和光顺曲线的曲率变化情况。
具体实施方式
30.下面结合实施例和附图对本发明的技术方案做进一步详细的描述，但本发明的实施方式并不限于此。
31.实施例1
32.本实施例提供一种工业机器人圆弧路径和圆弧路径过渡时的曲率连续的光顺方
法，如图1所示，对路径光顺方法所包括的主要步骤，详细描述如下：
33.步骤1、读取给定的工业机器人的路径，计算路径上所有位置点处的曲率；
34.所述工业机器人的路径c(t)包括两个圆弧段路径，分别为c1(t)和c2(t)，圆心分别记作c1和c2，半径分别记作r1和r2，每一个圆弧段路径包括300个位置点pi(xi,yi,zi)，其中i＝1,2,3
…
,n，xi、yi、zi分别是第i个位置点在x轴、y轴和z轴上的坐标，n是路径中包括的位置点个数，本实施例中n为300；未光顺的姿态路径如图2所示。
35.计算两个圆弧路径段上所有位置点处的曲率k(t)：
[0036][0037]
其中，c'(t)是路径c(t)的一阶导数，c”(t)是路径c(t)的二阶导数，t为归一化参数变量，t∈[0,1]。
[0038]
步骤2、根据曲率是否出现突变，把路径划分为曲率突变区和曲率连续区；
[0039]
本发明把两个圆弧段路径的接触点pj处定义为曲率突变点，本实施例把归一化参数变量t∈[0.9,1]包含的圆弧段路径划分为曲率突变区。
[0040]
步骤3、对曲率突变区，计算该区域两个端点处的曲率、单位切向量和单位法向量；
[0041]
曲率突变区的两个端点的位置点记作p0和p1，由步骤1中的曲率计算公式，得到曲率突变区两个端点的曲率k0和k1；由两个圆弧段路径的接触点pj，以及曲率突变区的两个端点p0和p1，分别计算两个圆弧段路径c1(t)和c2(t)在两个端点处的切向量，对切向量进行单位化处理得到单位切向量t0和t1；将两个单位切向量分别按逆时针旋转90
°
得到两个法向量，对两个法向量分别进行单位化处理得到单位法向量n0和n1。
[0042]
也就是说，本实施例中的单位切向量t0和t1，由曲率突变区的两个端点和曲率突变点构成的向量单位化得到；单位法向量记作n0和n1，由单位切向量按逆时针旋转90
°
得到
[0043]
步骤4、定义含形状调节参数的五次贝塞尔曲线，用于光顺曲率突变区；
[0044]
定义含形状调节参数的五次贝塞尔曲线b(t)为：
[0045]
b(t)＝b0b0(t) b1b1(t) b2b2(t) b3b3(t) b4b4(t) b5b5(t)
[0046]
其中，b0、b1、b2、b3、b4和b5为控制点，各控制点的表达式为：
[0047][0048]
其中，α0、α1、β0、β1为大于零的形状调节参数，用于调节样条曲线起点和终点切线方向及控制点位置；
[0049]
b0(t)、b1(t)、b2(t)、b3(t)、b4(t)和b5(t)为控制点伯恩斯坦多项式，表达式为：
[0050][0051]
步骤5、根据所述端点处的边界条件，以及根据曲率变化能和长度能量复合最小原则，定义五次贝塞尔曲线中形状调节参数的目标函数；
[0052]
目标函数定义为：
[0053][0054]
其中，b”'(t)是b(t)的三阶导数，b'(t)是b(t)的一阶导数。
[0055]
步骤6、把形状调节参数求解问题转化为最小化目标函数问题，通过目标函数分别对形状调节参数求偏导数，采用拟牛顿法对目标函数进行求解，得到形状调节参数的数值；
[0056]
最小化目标函数问题为：minf(α0,α1,β0,β1)；
[0057]
通过目标函数分别对形状调节参数求偏导数，包括：和
[0058]
采用拟牛顿法对目标函数进行求解，包括以下步骤：
[0059]
给定初始值和停止条件ε；其中上标0表示第0次迭代时α0,α1,β0,β1的初始值；
[0060]
设定海森矩阵hk的初始值h0为单位矩阵，并计算目标函数的梯度gk：
[0061][0062]
确定搜索方向dk＝h
kgk
；
[0063]
更新形状调节参数其中上标k表示第k次迭代时α0,α1,β0,β1的值；上标k 1表示第k 1次迭代时α0,α1,β0,β1的值；λk表示步长因子，由求解取得最小值时的λ所确定；
[0064]
判断是否成立，如果成立则输出形状调节参数，否则更新海森矩阵和目标函数的梯度：
[0065][0066][0067]
并进行下一次形状调节参数的计算。
[0068]
步骤7、将形状调节参数的数值代入五次贝塞尔曲线，得到曲率突变区的光顺路径。
[0069]
图2展示了本实施例两个圆弧段路径光顺前的路径，图3展示了本实施例两个圆弧段路径光顺后的曲率变化情况。可以看出，采用本实施例方法进行路径光顺后，两个过渡路径段的曲率分别和两个圆弧路径段的曲率相等，如图4所示，保证了光顺后路径曲率的连续变化，因此机器人的运动速度可以不降为零，从而提高运动效率，并且保证运动的平稳性。
[0070]
实施例2
[0071]
本实施例提供一种工业机器人圆弧路径和直线路径过渡时的曲率连续的路径光顺方法，所包括的主要步骤和实施例1基本相同。图5展示了本实施例一个圆弧段路径和一个直线路径段光顺前的路径，图6展示了本实施例一个圆弧段路径和一个直线路径段光顺后的路径，图7展示了本实施例一个圆弧路径段和一个直线路径段光顺后的曲率变化情况，光顺后路径曲率连续变化，机器人的运动速度可以不降为零，提高了机器人的运动效率，也保证了机器人运动的平稳性。
[0072]
实施例3
[0073]
本实施例与实施例1、实施例2基于相同的发明构思，提供的是一种曲率连续的工业机器人路径光顺系统，包括以下模块：
[0074]
曲率计算模块，用于读取给定的工业机器人的路径，计算路径上所有位置点处的曲率；
[0075]
突变区划分模块，用于根据曲率是否出现突变，把路径划分为曲率突变区和曲率连续区；
[0076]
突变区处理模块，用于计算曲率突变区的两个端点处的曲率、单位切向量和单位法向量；
[0077]
贝塞尔曲线定义模块，用于定义含形状调节参数的五次贝塞尔曲线光顺曲率突变区；并根据所述端点处的边界条件，以及根据曲率变化能和长度能量复合最小原则，定义五次贝塞尔曲线中形状调节参数的目标函数；
[0078]
形状调节参数求取模块，用于把形状调节参数求解问题转化为最小化目标函数问题，通过目标函数分别对形状调节参数求偏导数，采用拟牛顿法对目标函数进行求解，得到形状调节参数的数值；将形状调节参数的数值代入五次贝塞尔曲线，得到曲率突变区的光顺路径。
[0079]
其中，定义的含形状调节参数的五次贝塞尔曲线b(t)为：
[0080]
b(t)＝b0b0(t) b1b1(t) b2b2(t) b3b3(t) b4b4(t) b5b5(t)
[0081]
其中，b0、b1、b2、b3、b4和b5为控制点，各控制点的表达式为：
[0082][0083]
b0(t)、b1(t)、b2(t)、b3(t)、b4(t)和b5(t)为控制点伯恩斯坦多项式为：
[0084][0085]
其中，p0和p1为曲率突变区两个端点的位置点；α0、α1、β0和β1为大于零的形状调节参数，用于调节样条曲线起点和终点切线方向及控制点位置；k0和k1为曲率突变区两个端点的曲率。
[0086]
形状调节参数求取模块，根据所述端点处的边界条件，以及根据曲率变化能和长度能量复合最小原则，定义的五次贝塞尔曲线中形状调节参数的目标函数为：
[0087][0088]
其中，b”'(t)是b(t)的三阶导数，b'(t)是b(t)的一阶导数；
[0089]
形状调节参数求取模块把形状调节参数求解问题转化为最小化目标函数问题，通过目标函数分别对形状调节参数求偏导数，采用拟牛顿法对目标函数进行求解，得到形状调节参数的数值，包括：
[0090]
最小化目标函数问题为：minf(α0,α1,β0,β1)
[0091]
通过目标函数分别对形状调节参数求偏导数，包括：和
[0092]
采用拟牛顿法对目标函数进行求解，包括以下步骤：
[0093]
给定初始值和停止条件ε；
[0094]
设定海森矩阵初始值h0为单位矩阵，计算目标函数的梯度
[0095]
确定搜索方向dk＝h
kgk
，其中hk为海森矩阵；
[0096]
更新形状调节参数其中上标k表示第k次迭代时α0,α1,β0,β1的值，上标k 1表示第k 1次迭代时α0,α1,β0,β1的值；λk表示步长因子，由求解取得最小值时的λ所确定；
[0097]
判断是否成立，如果成立则输出形状调节参数，否则更新海森矩阵和目标函数的梯度：
[0098][0099][0100]
并进行下一次形状调节参数计算。
[0101]
本实施例的各模块实现过程请参见实施例1和实施例2，不赘述。
[0102]
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可对上述实施例进行变化、修改、替换和变形。