无姿态规划，小伙伴可以自己开发

本教程只是提供框架，工程慎用。

S速度规划代码OpenRobotSL/TrajectoryPlanningS速度规划代码

https://github.com/OpenRobotSL/TrajectoryPlanning​github.com基于S型曲线的连续多段曲线插补平滑过渡的规划算法（Matlab）_xuuyann-CSDN博客​blog.csdn.net

写在前面

前面的博客已经写了关于空间直线与空间圆弧的常用插补算法，而这些都是单一路径，实际中并不实用。对于连续多段路径，传统方法是将多段路径细分，然后对每段路径采用首尾速度为0的加减速算法（S型曲线或梯形曲线），这就带来了频繁启停的问题，容易对机械臂造成冲击，同时运行时间较长。

下面我把前面博客中提到的非对称S型加减速算法与空间中多段路径相结合，实现多段路径平滑过渡，减少运行时间。简单画一个轮廓状的“S”字符：

本文方法：

速度与加速度曲线：

采用分段路径首尾速度为0的加减速控制方法：

速度与加速度曲线：

对比结果还是很明显滴~本文的方法运行时间更短了，没有频繁启停，轮廓曲线更圆滑。如果在加上动力学方面的约束，就更好了。这个以后再讨论。

预处理

“预处理”，顾名思义就是拿到了一个轮廓轨迹，先想想这个轨迹如何分段，分段后如何衔接。

一般工业机器人任务空间的运动轨迹都是由圆弧段和直线段组成的，对于多段连续直线路径而言，任意两段相连直线段的连接处需要平滑处理，这里选择建立圆弧模型进行路径拐角过渡。下面给出圆弧模型的建立过程：

Matlab函数如下：

% 拾取放置操作分段路径转接模型

基于非对称S曲线的轨迹规划

核心方法：机器人学回炉重造（6）：关节空间规划方法——梯形加减速（与抛物线拟合的线性函数）、S型曲线规划

一些文献中给S型曲线加了很多修饰词，比如非对称、带约束啥的，其实就是我上面那篇博客的S曲线，首尾速度可以不一样，带有位移、速度以及加速度约束。

% 获取转接运动参数（速度）

插补

这里先只讲位置插补

预处理过后，轨迹就分段成为了一大堆直线段和一大堆圆弧段，这里要做的就是对这些空间直线和空间圆弧进行插补，于是又回到老话题了……

这里直线段插补不多说，和前面博客中提到的一样，空间单一直线位置插补+ S型加减速曲线归一化处理，具体方法见基于带约束S型加减速曲线的空间直线插补与空间圆弧插补算法（Matlab）

圆弧段插补也大同小异，方法有所不同，但基本是采用局部坐标系的空间圆弧插补方法和齐次坐标变换原理。新建立的局部坐标系UVW如下图所示，下面图中点的下标和我上面写的有些不一样，忽略所有$P_{bi}$并将$P_{ei}$看成$P_{Ti}$就行了，我就没有重新画图了。以第一段轨迹为例，新坐标系建立过程如下：

然后就是老生常谈的话题，把局部新坐标系中的圆弧通过左乘该坐标系相对基座标系的齐次变换矩阵从而变换回原来的XYZ值。

Matlab函数如下，我在函数中留了姿态插补四元数参数的位置，留着以后加姿态插补用：

% 过渡圆弧的插补算法，搭配ContinueSpaceLine使用

字符轮廓提取

CAD中画50mm*50mm的栅格图，在栅格图内绘制字体轮廓，然后提取关键点坐标。

以栅格左下角的点作为坐标原点，以水平方向为X轴，以垂直方向为Y轴建立平面直角坐标系，字符“S”的关键点坐标如图所示

提取坐标原点O的坐标值，然后用其他关键点的坐标值减去原点的，就能得到这些关键点在当前栅格坐标系下的坐标值，如下表

| 关键点 | A | B | C | D | E | F | G | H | I | J | K | L | | :----: | :--: | :--: | :--: | :--: | :--: | :--: | :--: | :--: | :--: | :--: | :--: | :--: | | X | 5 | 45 | 45 | 12 | 12 | 46 | 46 | 5 | 5 | 38 | 38 | 5 | | Y | 0 | 0 | 22 | 28 | 39 | 39 | 45 | 46 | 22 | 16 | 7 | 7 |

ok完事儿~

拼起来

是的，最后总的程序就是把上面分段规划得到的一堆直线插补点和一堆圆弧插补点，按照时间顺序拼起来。总程序我就不放了，仁者见仁智者见智。