[发明专利]基于MINIMAL-SNAP的被动运动轨迹生成方法

权利要求书

1.一种基于MINIMAL-SNAP的被动运动轨迹生成方法，其特征在于，包括步骤：

步骤1，进行初始条件设置，分配一个稀疏轨迹点序列，包括设置初始位置，初始速度，初始加速度；

步骤2，设置轨迹分段最大允许数目，进行稀疏轨迹点序列分段，并配置分配时间函数，配置约束加速度，配置约束速度；

步骤3，确定目标约束矩阵；确定过程约束矩阵，包括配置导数约束，配置连续性约束；回调求解段内轨迹参数矩阵，保存末端端点状态值，发送计算结果；

步骤4，遍历循环处理两点之间的所有段，然后再处理轨迹序列的所有点，将每次生成的序列发送往轨迹执行机构进行执行，生成轨迹。

2.根据权利要求1所述的基于MINIMAL-SNAP的被动运动轨迹生成方法，其特征在于，步骤1中，预先分配好稀疏轨迹点序列，采用基于可扩展标记性语言的数据序列表示方法存储分配的稀疏轨迹点信息，以时间或空间顺序存储相应的稀疏轨迹点信息。

3.根据权利要求1所述的基于MINIMAL-SNAP的被动运动轨迹生成方法，其特征在于，步骤2中，每两个离散的点为一个组合，再将两点轨迹分段，段内轨迹序列的空间信息和运动学信息多项式表达为：

X(t)＝c₅t⁵+c₅₄t⁴+c₃t³+c₂t²+c₁t+c₀

对该多项式求导可求速度，加速度。

4.根据权利要求3所述的基于MINIMAL-SNAP的被动运动轨迹生成方法，其特征在于，步骤2中，每完成两个点，时间参数重置并重新开始计时，在每个T_n时刻结束计时都会被重置为0，则每段时间依次为T_n-T_n-i。

5.根据权利要求4所述的基于MINIMAL-SNAP的被动运动轨迹生成方法，其特征在于，步骤2中，采用梯形加减速负责时间矩阵的分配，时间分配函数为：

其中，分配距离dist，加减速距离d。

6.根据权利要求5所述的基于MINIMAL-SNAP的被动运动轨迹生成方法，其特征在于，步骤3中，获取每个段的持续时间，将每一段设定为目标约束矩阵M的一列，并使M的导数表达式满足X(t)的表示形式，则目标约束矩阵满足下面的公式：

7.根据权利要求6所述的基于MINIMAL-SNAP的被动运动轨迹生成方法，其特征在于，步骤3中，为保证轨迹点连续运动平滑，对轨迹矩阵进行导数约束和连续性约束；在上一段的末尾位置和下一段起始位置都能经过P_i，则在此处施加导数约束，导数约束为：

保证生成的轨迹矩阵在加速度，加加速度，加加加速度连续，则至少保证各个导数在P_i，连续性约束为：

8.根据权利要求7所述的基于MINIMAL-SNAP的被动运动轨迹生成方法，其特征在于，步骤3中，在经过轨迹点分段处理和导数约束和连续性约束之后，每次得到的矩阵M是一个带状非奇异矩阵，b是一个指定的始末状态约束，构建一个线性方程组Mc＝b，求解其中参数矩阵c；

轮询执行回调函数，每次回调执行完毕之后根据回调来取得参数矩阵，获得的参数矩阵结合时间分配函数来合成相应的轨迹点的位置、速度、加速度、加加速度信息；然后向执行器发送计算结果，每次都保存一份上次末端状态值，每次发布保持两点连续向执行器发布。