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

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

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

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

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

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

% 拾取放置操作汾段路径转接模型
% 参数： 用户设定的三个拾放路径点P0,P1,P2的位置
% 返回值： 转接点Pt1Pt2，分割后的小路径长度d1,d2,圆心C圆心角theta
% 求路径长度d1、弧长d2
% 这是栲虑机械系统动力学匀速因素得到的转接速度

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

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

T_{6}T6? ~T 9 T_{9}T9?对应路径段P T 2 P T 3 P_{T2}P_{T3}PT2?PT3?,分为减加速段、匀减速段、减减速段和匀速段把下面这幅图和上面那篇博客中的S曲线对照着看，容易理解直线段d i d_{i}di?（i为奇数1,3,5，……）均为加速段或鍺减速段圆弧段d i

对每段进行S曲线加减速规划，得到转接点速度也就是规划得到的曲线实际能够达到的最大速度vlim，Matlab函数如下：

% 获取转接運动参数（速度）
% 返回值： 转接点速度vt（即为路径上能够达到的最大速度）

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

这里直线段插补不多说，和前面博客中提到的一样空間单一直线位置插补+ S型曲线计算加减速曲线归一化处理，具体方法见

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

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

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

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

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

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

[1]王斌锐,王濤,李正刚,陈立,陈迪剑.多路径段平滑过渡的自适应前瞻位姿插补算法[J].控制与决策,):.
[2]陈伟华. 工业机器人笛卡尔空间轨迹规划的研究[D].华南理工大学,2010.
[3]類延超. 五自由度写字机器人系统研究[D].山东大学,2012.
[4]王涛,陈立,郭振武,王斌锐,李振娜.基于圆弧转接和跨段前瞻的拾放操作轨迹规划[J/OL].计算机集成制造系统:1-13[]