[发明专利]用于高精度运动平台控制的任意阶S型曲线运动规划方法

权利要求书

1.一种用于高精度运动平台控制的任意阶S型曲线运动规划方法，其特征在于，包括以下步骤：

步骤1，对高精度运动平台控制所必须的规划指令，即任意阶S型运动规划曲线进行定义；

步骤2，针对所定义的任意阶S型运动规划曲线，构造该运动规划曲线的求导矩阵；

步骤3，根据高精度运动平台的规划距离、最大速度、最大加速度、速度连续、加速度连续要求，建立等效的方程组，通过求解方程组得到任意阶次的S型曲线的表达式；利用该表达式进行高精度运动平台高阶S型曲线的运动规划；

对所述任意阶S型运动规划曲线进行定义，包括：

n阶S型运动规划曲线的位移表达式是2ⁿ-1段且每段均为n次多项式的表达式，S_i(t)表示n阶S型运动规划曲线的第i段曲线的位移表达式：

S_i(t)＝a_i,0+a_i,1t+a_i,2t²+a_i,3t³+...+a_i,ntⁿ,i＝1,2,3,...,2ⁿ-1  (1)

其中，a_i,0,a_i,1,a_i,2,...,a_i,n表示第i段曲线位移表达式的n+1个系数，t表示时间；

所述构造该运动规划曲线的求导矩阵，包括：

n阶S型运动规划曲线第i段位移曲线表达式S_i(t)及其一系列导数的表达式如下：

其中，上标n表示n阶导数；用列向量x表示第i段曲线未知系数向量，如式(5)所示：

x＝[a_i,0,a_i,1,a_i,2,...,a_i,n]^T  (5)

在进行高精度运动平台的控制时，所用规划指令中包含n阶S型运动规划曲线的导数，将每阶导数表达式中的各项系数、求导系数、时间t及其幂级数分开，形成未知系数向量x、时间矩阵D_n、以及求导系数矩阵C_n，其中D_n和C_n如式(6)和式(7)所示：

将式(4)拆分成由x、D_n、C_n经过特定运算得到的形式，如式(8)所示：

D_n(.*)C_n·x(8)

运算(.*)表示将D_n和C_n中相同位置索引的元素相乘；式(8)即为式(4)的矩阵形式，其中D_n(.*)C_n为n阶S型运动规划曲线的求导矩阵，记为G_n：

G_n＝D_n(.*)C_n  (9)

所述根据高精度运动平台的规划距离、最大速度、最大加速度、速度连续、加速度连续要求，建立等效的方程组，包括：

高精度运动平台控制所需的n阶S型运动规划曲线的位移表达式是2ⁿ-1段且每段均为n次多项式的表达式；将n阶S型运动规划曲线的段数记为n_s，总的时间节点个数记为n_p，每一位移分段表达式包含n_c个未知系数需要确定，则对于n阶S型运动规划曲线，需要确定的未知系数个数记为s_n；

其中：

s_n＝n_s×n_c

通过n阶S型运动规划曲线的首尾时间节点处的约束条件、n阶S型运动规划曲线的0阶导至n-1阶导在第2个时间节点至第n_p-1个时间节点处的连续条件、以及n阶S型运动规划曲线的2阶导至n阶导的特殊约束条件来构造s_n个方程；

对于n阶S型运动规划曲线的首尾时间节点处的约束条件，表示如下：

将式(1)代入式(12)可得：

末时间节点处的约束条件可以用式(14)表示：

将式(1)代入式(14)得：

将式(13)和式(15)写成A₁X＝b₁的形式，其中X为未知系数列向量：

A₁为2n×s_n的常数矩阵，b₁为2n×1的常数列向量；利用式(9)的求导矩阵，写出A₁，对于第一个时间节点的约束条件，矩阵A₁的部分元素为：

A₁(j,1:n+1)表示矩阵A₁第j行，第1列至第n+1列的元素，下同；G(j,:)|_t＝0表示时间t＝0时的求导矩阵第j行的元素；对于末时间节点的约束条件：

表示时的求导矩阵第j行的元素；同时，b₁部分元素为：

b₁(n+1,1)＝Q  (18)

其中，时间节点向量

除了式(16)、(17)、(18)所指定的值，A₁和b₁的其他元素均为0；

n阶S型运动规划曲线的0阶导至n-1阶导在第2个时间节点至第n_p-1个时间节点处的连续条件，表示为：

将式(1)代入式(19)得：

将式(19)写成矩阵A₂X＝b₂的形式，A₂为n×(n_p-2)行，s_n列的常数矩阵，b₂为[n×(n_p-2)]×1的常数列向量；利用式(9)的求导矩阵，写出A₂：

表示时间t＝T_j的求导矩阵第k行的元素；除了式(21)所指定的元素外，矩阵A₂其余元素均为0，b₂的元素均为0；

所述n阶S型运动规划曲线的2阶导至n阶导的特殊约束条件，表示为：

n阶S型运动规划曲线的2阶导至n阶导存在恒等于0的曲线段，且2阶导至n阶导恒等于0的曲线段索引与曲线的阶数和导数有关；通过归纳总结可知，n阶S型运动规划曲线的g阶导数中恒等于0的曲线段索引为：

向量V_n,g为由n阶S型运动规划曲线的g阶导数中恒等于0的曲线段索引组成的行向量，L_n,g为向量V_n,g的长度；

将以上约束条件同样写成矩阵形式A₃X＝b₃，常数向量b₃的元素均为0，常数矩阵A₃的元素由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，则让矩阵A₃中对应位置的元素值为1即可；经过归纳总结，可以得到A₃：

其中，B_g为n阶S型运动规划曲线第g阶导中恒等于0的方程组构成的常数矩阵，V_n,g(k)表示向量V_n,g的第k个元素；

通过求解方程组得到任意阶次的S型曲线的表达式，包括：

求得的A₁、A₂、A₃以及b₁、b₂、b₃，可以构造矩阵A和向量b：

通过下式即可求得未知系数向量X，则n阶S型运动规划曲线便构造完毕：

X＝A^-1b  (29)。