用于高精度运动平台控制的任意阶S型曲线运动规划方法与流程

用于高精度运动平台控制的任意阶s型曲线运动规划方法
技术领域
1.本发明涉及高精度控制及制造领域，具体涉及一种用于高精度运动平台控制的任意阶s型曲线运动规划方法。


背景技术：

2.在现代化制造领域中，高精度运动平台，如高精度直线平台、高精度旋转平台、scara机器人等得到广泛应用。高精度运动平台的控制策略一般采用闭环控制，闭环控制中常需要用到规划指令信号与反馈信号，尤其在进行对精度和速度有较高要求的加工任务时，如何在保持定位精度的前提下尽可能地缩短定位时间对于提高设备的生产效率和改善加工质量至关重要，解决这类问题的关键技术之一就是高速高精度运动规划。
3.在高速高精度运动规划中，高阶运动规划曲线对执行部件的速度平滑和性能的充分发挥有着极大的提升。然而受限于现有控制器或控制卡的功能，目前在工程领域中应用较多的运动规划曲线多为低阶的运动规划曲线，对于高阶运动规划曲线的推导构造鲜有研究涉及。
4.高精度运动平台的控制多需要结合控制器或者控制卡实现，目前应用较为广泛的有固高运动控制卡、pmac控制器、dspace控制器等。目前，控制器或控制卡中使用最为广泛的是二阶运动规划曲线和三阶运动规划曲线，即梯形速度规划和三阶s型速度规划。通常而言，采用梯形速度规划可以极大地缩短运动规划的时间，但是梯形速度规划曲线存在加速度突变，容易激发较大的残余振动，进而导致运动系统需要较多的振动衰减时间来满足定位精度。所以一般在半导体制造等精密工程领域中，使用较多的为三阶s型速度规划，然而三阶s型速度规划曲线中，仍存在急动度不连续的情况。在一些场合中，高阶非对称曲线的规划方法可以得到相比于低阶运动规划曲线更好的效果。然而，目前工程中常用的控制器或控制卡并不具备高阶s型曲线运动规划的功能，大多情况下需要使用者自己写出高阶s型运动规划曲线的表达式，然后根据采样时间逐步计算规划指令，最后发送给控制器或控制卡实现闭环控制。
5.目前尚没有一个统一的方法来设计用于高精度运动平台控制的任意阶次s型运动规划曲线，这对于工程应用的编程实现极为不便。


技术实现要素：

6.本发明的目的是提供一种用于高精度运动平台控制的任意阶s型曲线运动规划方法，用以解决现有技术中存在的问题。
7.为了实现上述任务，本发明采用以下技术方案：
8.一种用于高精度运动平台控制的任意阶s型曲线运动规划方法，包括以下步骤：
9.步骤1，对高精度运动平台控制所必须的规划指令，即任意阶s型运动规划曲线进行定义；
10.步骤2，针对所定义的任意阶s型运动规划曲线，构造该运动规划曲线的求导矩阵；
11.步骤3，根据高精度运动平台的规划距离、最大速度、最大加速度、速度连续、加速度连续等要求，建立等效的方程组，通过求解方程组得到任意阶次的s型曲线的表达式；利用该表达式进行高精度运动平台高阶s型曲线的运动规划。
12.进一步地，所述任意阶s型运动规划曲线进行定义，包括：
13.n阶s型运动规划曲线的位移表达式是2
n-1段且每段均为n次多项式的表达式，si(t)表示n阶s型运动规划曲线的第i段曲线的位移表达式：
14.si(t)＝a
i,0
a
i,1
t a
i,2
t2 a
i,3
t3 ... a
i,n
tn,i＝1,2,3,...,(2
n-1)
ꢀꢀꢀꢀꢀꢀ
(1)
15.其中，a
i,0
,a
i,1
,a
i,2
,...,a
i,n
表示第i段曲线位移表达式的n 1个系数，t表示时间。
16.进一步地，所述构造该运动规划曲线的求导矩阵，包括：
17.n阶s型运动规划曲线第i段位移曲线表达式si(t)及其一系列导数的表达式如下：
[0018][0019]
其中，上标(n)表示n阶导数；用列向量x表示第i段曲线未知系数向量，如式(5)所示：
[0020]
x＝[a
i,0
,a
i,1
,a
i,2
,...,a
i,n
]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0021]
在进行高精度运动平台的控制时，所用规划指令中通常包含n阶s型运动规划曲线的导数，将每阶导数表达式中的各项系数、求导系数、时间t及其幂级数分开，形成未知系数向量x、时间矩阵dn、以及求导系数矩阵cn，其中dn和cn其中如式(6)和式(7)所示：
[0022][0023][0024]
将式(4)拆分成由x、dn、cn经过特定运算得到的形式，如式(8)所示：
[0025]dn
(.*)cn·
x(8)
[0026]
运算(.*)表示将dn和cn中相同位置索引的元素相乘；式(8)即为式(4)的矩阵形式，其中dn(.*)cn为n阶s型运动规划曲线的求导矩阵，记为gn：
[0027]gn
＝dn(.*)cnꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0028]
进一步地，所述根据高精度运动平台的规划距离、最大速度、最大加速度、速度连续、加速度连续等要求，建立等效的方程组，包括：
[0029]
高精度运动平台控制所需的n阶s型运动规划曲线的位移表达式是2
n-1段且每段均为n次多项式的表达式；每一位移分段表达式包含nc个未知数需要确定，则对于n阶s型运动规划曲线，需要确定的未知系数个数记为sn；
[0030]
其中：
[0031][0032]
sn＝ns×
nc[0033]
通过n阶s型运动规划曲线的首尾时间节点处的约束条件、n阶s型运动规划曲线的0阶导至(n-1)阶导在第2个时间节点至第n
p-1个时间节点处的连续条件、以及n阶s型运动规划曲线的2阶导至n阶导的特殊约束条件来构造sn个方程。
[0034]
进一步地，对于n阶s型运动规划曲线的首尾时间节点处的约束条件，表示如下：
[0035][0036]
将式(1)代入式(12)可得：
[0037][0038]
末时间节点处的约束条件可以用式(14)表示：
[0039][0040]
将式(1)代入式(14)得：
[0041][0042]
将式(13)和式(15)写成a1x＝b1的形式，其中x为未知系数列向量：
[0043][0044]
a1为(2n
×
sn)的常数矩阵，b1为(2n
×
1)的常数列向量。利用式(9)的求导矩阵，可以较为方便的写出a1，对于第一个时间节点的约束条件，矩阵a1的部分元素为：
[0045][0046]
a1(j,1:n 1)表示矩阵a1第j行，第1列至第n 1列的元素，下同；g(j,:)|
t＝0
表示时间t＝0时的求导矩阵第j行的元素。对于末时间节点的约束条件：
[0047][0048]
表示时的求导矩阵第j行的元素；同时，b1部分元素为：
[0049]
b1(n 1,1)＝q
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(18)
[0050]
其中，时间节点向量
[0051]
除了式(16)、(17)、(18)所指定的值，a1和b1的其他元素均为0。
[0052]
进一步地，n阶s型运动规划曲线的0阶导至(n-1)阶导在第2个时间节点至第n
p-1个时间节点处的连续条件，表示为：
[0053][0054]
将式(1)代入式(19)得：
[0055][0056]
将式(19)写成矩阵a2x＝b2的形式，a2为n
×
(n
p-2)行，sn列的常数矩阵，b2为[n
×
(n
p-2)]
×
1的常数列向量；利用式(9)的求导矩阵，可以较为方便的写出a2：
[0057][0058]
表示时间t＝tj的求导矩阵第k行的元素；除了式(21)所指定的元素外，矩阵a2其余元素均为0，b2的元素均为0。
[0059]
进一步地，所述阶s型运动规划曲线的2阶导至n阶导的特殊约束条件，表示为：
[0060]
n阶s型运动规划曲线的2阶导至n阶导存在恒等于0的曲线段，且2阶导至n阶导恒等于0的曲线段索引与曲线的阶数和导数有关；通过归纳总结可知，n阶s型运动规划曲线的g阶导数中恒等于0的曲线段索引为：
[0061][0062]
向量v
n,g
为由n阶s型运动规划曲线的g阶导数中恒等于0的曲线段索引组成的行向量，l
n,g
为向量v
n,g
的长度；
[0063]
将以上约束条件同样写成矩阵形式a3x＝b3，常数向量b3的元素均为0，常数矩阵a3的元素由0和1组成，当n阶s型运动规划曲线的第i段曲线段的n阶导恒等于0时，则该曲线段的系数a
i,n
等于0；(n-1)阶导恒等于0时，系数a
i,n-1
和a
i,n
等于0；(n-2)阶导恒等于0时，系数a
i,n-2
、a
i,n-1
、a
i,n
等于0，以此类推；因此，若想让向量x中的某些参数等于0，则让矩阵a3中对应位置的元素值为1即可；经过归纳总结，可以得到a3：
[0064][0065][0066]
其中，bg为n阶s型运动规划曲线第g阶导中恒等于0的方程组构成的常数矩阵，v
n,g
(k)表示向量v
n,g
的第k个元素。
[0067]
进一步地，通过求解方程组得到任意阶次的s型曲线的表达式，包括：
[0068]
求得的a1、a2、a3以及b1、b2、b3，可以构造矩阵a和向量b：
[0069][0070]
通过下式即可求得未知系数向量x，则n阶s型运动规划曲线便构造完毕：
[0071]
x＝a-1bꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(29)
[0072]
进一步地，所述利用该表达式进行高精度运动平台高阶s型曲线的运动规划，包括：
[0073]
在求得系数向量x之后，便可以求得n阶s型运动规划曲线的表达式，根据式(9)可以求得其一系列导数，然后根据采样时间dt计算每个控制周期内发送给控制器的规划指令，从而完成高精度运动平台高阶s型曲线的运动规划。
[0074]
一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，处理器执行计算机程序时实现用于高精度运动平台控制的任意阶s型曲线运动规划方法的步骤。
[0075]
一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器执行时实现用于高精度运动平台控制的任意阶s型曲线运动规划方法的步骤。
[0076]
与现有技术相比，本发明具有以下技术特点：
[0077]
1.本发明所提出的方法可以为高精度运动平台的闭环控制提供任意阶次的s型曲线规划指令，且对任意阶次s型运动规划曲线的构造提供了一种通用的构造方法。
[0078]
2.本发明所提出的构造n阶s型运动规划曲线的方法便于编程实现，可以快速生成任意阶次的s型运动规划曲线。
附图说明
[0079]
图1为高精度运动平台闭环控制图；图中s(t)为n阶s型曲线运动规划指令；sc为位移指令；vc为速度指令；ac为加速度指令；sd为位移反馈信号；vd为速度反馈信号；ad为加速度反馈信号。
[0080]
图2为二阶s型运动规划曲线图，图中t1、t2、t3为时间节点。
[0081]
图3为三阶s型运动规划曲线图，图中t1、t2、t3、t4、t5、t6、t7为时间节点。
[0082]
图4为四阶s型运动规划曲线图，图中t1、t2、t3、t4、t5、t6、t7、t8、t9、t
10
、t
11
、t
12
、t
13
、t
14
、t
15
为时间节点。
[0083]
图5为五阶s型运动规划曲线图。
具体实施方式
[0084]
本发明针对dspace等控制器可以自定义规划曲线的特点，从任意阶次s型运动规划曲线的特性和工程实践性出发，对任意阶次的s型运动规划曲线进行定义，对运动规划曲线中需要满足的连续性要求和特殊约束要求采用理论推导与归纳的方式进行总结，从而获
得构造曲线的所必须的各项系数。采用本发明所提出的方法，仅需少量的代码即可快速生成用于高精度运动平台控制的任意阶次的s型运动规划曲线。
[0085]
参见附图，本发明提出的一种用于高精度运动平台控制的任意阶s型曲线运动规划方法，包括以下步骤：
[0086]
步骤1，对高精度运动平台控制所必须的规划指令，即任意阶s型运动规划曲线进行定义。
[0087]
高精度运动平台的闭环控制框图大致可以用图1进行描述，其中规划指令的设计是闭环控制中重要的一环。目前工程中常用的控制卡或控制器中一般只具备二阶s型曲线运动规划和三阶s型运动规划的功能，而对于更高阶的s型曲线运动规划的功能比较欠缺。通过控制器或控制卡进行高精度运动平台的高阶s型曲线运动规划，通常的做法是先生成任意阶次的s型运动规划曲线，该曲线是关于时间t的表达式，然后根据采样时间dt计算出每一个控制周期里的规划指令值。本发明所要解决的问题便是图1中的规划指令s(t)的生成问题。
[0088]
在高精度运动平台的控制中除了用到位置规划指令，经常也会用到速度规划指令和加速度规划指令等作为指令前馈，这可以大大提高平台的定位精度和运动的响应速度。速度规划指令和加速度规划指令是位置规划指令的一阶导数和二阶导数，因此在进行运动规划时，最重要的是求得位置规划指令的表达式。二阶运动规划曲线如图2所示，其位移表达式是3段的分段表达式，每段表达式是关于时间t的2次多项式；三阶运动规划曲线如图3所示，其位移表达式是7段的分段表达式，每段表达式是关于时间t的3次多项式。因此任意阶s型曲线运动规划的位移表达式可以用分段的n次多项式进行定义。si(t)表示n阶s型运动规划曲线的第i段曲线的位移表达式，如式(1)所示。
[0089]
si(t)＝a
i,0
a
i,1
t a
i,2
t2 a
i,3
t3 ... a
i,n
tn,i＝1,2,3,...,(2
n-1)
ꢀꢀꢀꢀꢀꢀ
(1)
[0090]
其中，a
i,0
,a
i,1
,a
i,2
,...,a
i,n
表示第i段曲线位移表达式的n 1个系数，当该段曲线的系数确定时，则该段曲线也确定。将n阶s型运动规划曲线的段数记为ns；每段曲线的未知系数的个数记为nc；总的时间节点个数记为n
p
。可以知道：
[0091][0092]
时间节点包含运动时间起点与终点，以及第i段和第i 1段曲线交点处所对应的时间，通常用向量t表示时间节点向量，如式(3)所示。
[0093][0094]
步骤2，针对步骤1中定义的任意阶s型运动规划曲线，构造该运动规划曲线的求导矩阵。
[0095]
在进行高精度运动平台的控制时，通常需要用到位移信息s(t)、速度信息v(t)、加速度信息a(t)、加加速度信息j(t)等，即位移曲线s(t)及其一系列导数等。n阶s型运动规划曲线第i段位移曲线表达式si(t)及其一系列导数的表达式如式(4)所示：
[0096][0097]
用列向量x表示第i段曲线未知系数向量，如式(5)所示。
[0098]
x＝[a
i,0
,a
i,1
,a
i,2
,...,a
i,n
]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0099]
在进行高精度运动平台的控制时，所用规划指令中通常包含n阶s型运动规划曲线的导数，将每阶导数表达式中的各项系数、求导系数、时间t及其幂级数分开，形成未知系数向量x、时间矩阵dn、以及求导系数矩阵cn，其中dn和cn其中如式(6)和式(7)所示：
[0100][0101][0102]dn
，cn均为(n 1)
×
(n 1)的矩阵。为方便后续编程实现，将式(4)拆分成由x、dn、cn经过特定运算得到的形式，如式(8)所示：
[0103]dn
(.*)cn·
x
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0104]
运算(.*)表示将dn和cn中相同位置索引的元素相乘。式(8)即为式(4)的矩阵形式，其中dn(.*)cn为n阶s型运动规划曲线的求导矩阵，记为gn：
[0105]gn
＝dn(.*)cnꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0106]
步骤3，最后根据高精度运动平台的规划距离、最大速度、最大加速度、速度连续、加速度连续等要求，建立等效的方程组，通过求解方程组得到任意阶次的s型曲线的表达式。
[0107]
从步骤1可知，高精度运动平台控制所需的n阶s型运动规划曲线的位移表达式是2
n-1段且每段均为n次多项式的表达式。每一位移分段表达式包含nc个未知数需要确定，则对于n阶s型运动规划曲线，需要确定的未知系数个数sn为：
[0108]
sn＝ns×
ncꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0109]
用列向量x表示n阶s型运动规划曲线未知系数向量，如式(11)所示：
[0110]
[0111]
因此如需确定n阶s型运动规划曲线，则需确定向量x，x向量中包含sn个未知数，即需要构造sn个方程。本发明通过n阶s型运动规划曲线的首尾时间节点处的约束条件、n阶s型运动规划曲线的0阶导至(n-1)阶导在第2个时间节点至第n
p-1个时间节点处的连续条件、以及n阶s型运动规划曲线的2阶导至n阶导的特殊约束条件来构造sn个方程。
[0112]
1)n阶s型运动规划曲线的首尾时间节点处的约束条件，即在第一个时间节点处，高精度运动平台的位移、速度、加速度、加加速度等均等于0，即n阶s型运动规划曲线的0阶导至n-1导均等于0；在末时间节点处，高精度运动平台的规划距离等于q，而其速度、加速度、加加速度等均等于0，即n阶s型运动规划曲线的0阶导等于规划的距离q，1阶导至n-1阶导均等于0。第一个时间节点的约束条件可以用式(12)表示：
[0113][0114]
将式(1)代入式(12)可得：
[0115][0116]
末时间节点处的约束条件可以用式(14)表示：
[0117][0118]
将式(1)代入式(14)得：
[0119][0120]
将式(13)和式(15)写成a1x＝b1的形式，其中x为未知系数列向量，a1为(2n
×
sn)的常数矩阵，b1为(2n
×
1)的常数列向量。利用式(9)的求导矩阵，可以较为方便的写出a1，对于
第一个时间节点的约束条件，矩阵a1的部分元素为：
[0121][0122]
a1(j,1:n 1)表示矩阵a1第j行，第1列至第n 1列的元素，下同；g(j,:)|
t＝0
表示时间t＝0时的求导矩阵第j行的元素。对于末时间节点的约束条件：
[0123][0124]
表示时的求导矩阵第j行的元素。同时，b1部分元素为：
[0125]
b1(n 1,1)＝q
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(18)
[0126]
除了式(16)、(17)、(18)所指定的值，a1和b1的其他元素均为0。
[0127]
2)高精度运动平台在高速运动过程中，如若要保持较高的精度，除了其位移曲线需保证在运动时间内连续外，其速度、加速度等曲线也需保证连续性。如图2所示的二阶s型曲线运动规划，其加速度段在时间节点处不连续，容易导致高精度运动平台在运动过程中产生振动，从而丢失定位精度。高精度运动平台要保证较高的定位精度和加减速的平滑性，需保证其规划指令n阶s型运动规划曲线的0阶导至(n-1)阶导在第2个时间节点至第n
p-1个时间节点处的连续条件，可以用式(19)表示：
[0128][0129]
将式(1)代入式(19)得：
[0130][0131]
将式(19)写成矩阵a2x＝b2的形式，a2为n
×
(n
p-2)行，sn列的常数矩阵，b2为[n
×
(n
p-2)]
×
1的常数列向量。利用式(9)的求导矩阵，可以较为方便的写出a2：
[0132][0133]
表示时间t＝tj的求导矩阵第k行的元素。除了式(21)所指定的元素外，
矩阵a2其余元素均为0，b2的元素均为0。
[0134]
3)为了充分发挥高精度运动平台电机的性能，规划指令n阶s型运动规划曲线的2阶导至n阶导需要满足特殊约束条件，即n阶s型运动规划曲线的2阶导至n阶导在某些曲线段恒等于0，而这些曲线段的索引存在一定的规律。通过二阶运动规划曲线、三阶运动规划曲线、四阶运动规划曲线中2阶导至n阶导恒等于0的曲线段索引的规律进行归纳总结，从而推广至n阶运动规划曲线的情况。
[0135]
二阶运动规划曲线如图1所示，可以看出，加速度曲线的第2段曲线恒等于0，即：
[0136][0137]
三阶运动规划曲线如图2所示，可以看出，加速度曲线的第4段曲线恒等于0，加加速度曲线的第2、4、6段曲线即恒等于0，即：
[0138][0139]
四阶运动规划曲线如图3所示，同样可得，加速度曲线的第8段曲线恒等于0，加加速度曲线的第4、8、12段曲线恒等于0，四阶导的第2、4、6、8、10、12、14段曲线恒等于0，即：
[0140][0141]
从式(22)、(23)、(24)可知，2阶导至n阶导恒等于0的曲线段索引与曲线的阶数和导数有关。通过归纳总结可知，n阶s型运动规划曲线的g阶导数中恒等于0的曲线段索引为：
[0142][0143]
向量v
n,g
为由n阶s型运动规划曲线的g阶导数中恒等于0的曲线段索引组成的行向
量，l
n,g
为向量v
n,g
的长度。
[0144]
将以上约束条件同样写成矩阵形式a3x＝b3，常数向量b3的元素均为0，常数矩阵a3的元素由0和1组成。当n阶s型运动规划曲线的第i段曲线段的n阶导恒等于0时，则该曲线段的系数a
i,n
等于0；(n-1)阶导恒等于0时，系数a
i,n-1
和a
i,n
等于0；(n-2)阶导恒等于0时，系数a
i,n-2
、a
i,n-1
、a
i,n
等于0，以此类推。因此，若想让向量x中的某些参数等于0，则让矩阵a3中对应位置的元素值为1即可。经过归纳总结，可以得到a3：
[0145][0146][0147]
其中，v
n,g
(k)表示向量v
n,g
的第k个元素；bg为n阶s型运动规划曲线第g阶导中恒等于0的方程组构成的常数矩阵。
[0148]
综合上述三种情况求得的a1、a2、a3以及b1、b2、b3，可以构造矩阵a和向量b：
[0149][0150]
通过下式即可求得未知系数向量x，则n阶s型运动规划曲线便构造完毕。
[0151]
x＝a-1bꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(29)
[0152]
在求得系数向量x之后，便可以求得n阶s型运动规划曲线的表达式，根据式(9)可以求得其一系列导数，然后根据采样时间dt计算每个控制周期内发送给控制器的规划指令，从而完成高精度运动平台高阶s型曲线的运动规划。
[0153]
实施例：构造用于高精度直线运动平台控制的五阶s型运动规划曲线
[0154]
通过本发明的方法生成任意阶的s型运动规划曲线时，需要给定规划的距离q和时间节点向量t。假设高精度直线平台运动的距离为0.15m，运动规划曲线的阶次为5阶，则根据式(2)可得：
[0155][0156]
令时间节点向量t为：
[0157][0158]
通过式(5)(7)计算时间矩阵d5和求导系数矩阵c5[0159][0160][0161]
然后通过式(16)(17)(21)即可计算出a1和a2。通过式(25)计算v
5,5
、v
5,4
、v
5,3
、v
5,2
以及l
5,5
、l
5,4
、l
5,3
、l
5,2
：
[0162][0163][0164][0165][0166]
通过式(26)即可计算出b2、b3、b4、b5，然后通过式(27)即可得出a3。最后通过式(28)(29)即可计算所有的未知系数。图4为构造出的五阶s型运动规划曲线。
[0167]
以上实施例仅用以说明本技术的技术方案，而非对其限制；尽管参照前述实施例对本技术进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本技术各实施例技术方案的精神和范围，均应包含在本技术的保护范围之内。