路径轨迹规划

路径轨迹规划（1）加减速控制简述加减速控制算法的目标是建立加减速过程中速度相对于时间的函数关系式f=V(t)。

按照加减速控制算法与插补算法的先后位置关系，加减速控制方式可分为前加减速控制和后加减速控制。

前加减速控制即插补计算前进行加减速运算，其优点在于对合成速度进行控制，不影响位置精度，但是需要预测减速点；后加减速控制即插补计算后进行加减速运算，它是对各插补轴分别进行加减速控制，由于各轴没有协调关系，因此合成位置可能不准确。

后加减速控制只适用线性插补，在应用上有很大的局限性。

（2）几种速度控制模型1）直线加减速速度控制模型 直线加减速是当机床启动、停止或者运动速速改变时，速度将按照一定斜率的直线上升或下降。

数学表达式为：at t +=0)(νν直线加减速控制算法的主要优点是算法简单，机器人响应快，效率高，适合进行实时运算，但是机器人运动存在柔性冲击，速度的过渡不够平滑。

2）指数加减速速度控制模型 指数加减速是启动或停止时的速度发生突变，并且速度变化随时间按指数规律上升或下降。

速度数学表达式为： 加速时：)1()(τtc e v t v --=减速时：τtc ev t v -=)(加速度数学表达式为：加速时：ττt c e v t a -=)()(减速时：ττt c e v t a --=)()(指数型加减速曲线的优点是数学表达式相对简单，可以实时计算，加减速结束时加速度变小冲击变小；缺点是启动过程仍存在较大冲击。

2）S 曲线加减速速度控制模型通过对启动阶段即高速阶段的加速度衰减，来保证电机性能的充分发挥和减小启动冲击。

正常情况下S 曲线加减速的运行过程分为7段：加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速段、减减速段，如下图所示：图中符号分别表示： s v ：起始速度e v ：终点速度k t ：)7...1,0(=k 各个阶段的过渡点时刻k τ：)7...2,1(=k 局部时间坐标，表示以各个阶段的起始点作为零点的时间表示，)7...2,1(1=-=-k t t k k τ k T ：)7...2,1(=k 各个阶段的持续运行时间L ：整个运行长度加加速度J 、加速度a 、速度v 、位移s 等计算公式的通用形式如下所示：1t 2t 3t 4t 6t 5t 7t ttttsLv aJsv ev maxv maxA maxD -1T 1τ2τ3τ4τ5τ6τ7τ2T 3T 4T 5T 6T 7T 1J 3J -5J -7J⎪⎪⎪⎪⎩⎪⎪⎪⎪⎨⎧<≤<≤<≤-<≤<≤-<≤<≤=76655443322110000)(t t t Jt t t t t t J t t t t t t J t t t t t J t J⎪⎪⎪⎪⎩⎪⎪⎪⎪⎨⎧<≤+-<≤-<≤-<≤<≤-<≤<≤=76756555454332312111100)(t t t J JT t t t JT t t t J t t t t t t J JT t t t JT t t J t a ττττ⎪⎪⎪⎪⎩⎪⎪⎪⎪⎨⎧-==<≤+--==<≤--==<≤-==<≤+==<≤-++==<≤++==<≤+=250607776277506650506665650525040555425040304443032102033322331022101022212101210111215.05.05.05.05.05.05.005.0)(JT v v t t t t t J JT v T JT v v t t t t t JT v JT v v t t t t t J v v v t t t t t v JT v v t t t t t J JT v T JT v v t t t t t JT v JT v v t t t t J v t v s s 时时时时时时时ττττττττ⎪⎪⎪⎪⎪⎪⎪⎩⎪⎪⎪⎪⎪⎪⎪⎨⎧-+==<≤+-+-+==<≤-+-+==<≤-++==<≤+++==<≤-++++==<≤+++==<≤+=355060607776372757060626560505066652656050535504040555435504044030304443403033110202033323323130202221201010222122120101311011131131612121216161316121212161061)(JT T v s s t t t t t J JT v s T JT T v s s t t t t t JT v s JT T v s s t t t t t J v s T v s s t t t t t v s JT T v s s t t t t t J JT v s T JT T v s s t t t t t JT v s JT T v s t t t t J v t s s s 时时时时时时时τττττττττττττττ （3）插补模式轨迹规划是指在笛卡儿坐标系中的规划。

由于关节空间的轨迹规划是对单个轴的规划，由于机器人机构的特殊性，关节空间规划不能保证特定的轨迹，如果对于那些路径、姿态有严格要求的作业，例如弧焊作业，就必须在笛卡儿坐标系内进行规划。

由于末端执行器的位姿都是时间的函数，所以对轨迹的空间形状有一定的设计要求，这需要相应的机器人轨迹插补算法和逆运动学计算来确定一个机器人的各关节角，以实现要求的空间轨迹。

直线插补和圆弧插补是机器人轨迹规划系统中不可缺少的基本插补算法，也是机器人轨迹规划中最常用的规划方法，本机器人系统实现了直线插补和圆弧插补。

1）直线插补如图所示始点坐标和姿态为),,(1111z y x P 、),,(1111γβαθ，终点坐标和姿态为),,(2222z y x P 、),,(2222γβαθ，开始时的加速段或结束时的减速段（加速段与减速段具有对称性）的加速度为a ，直线段运动的速度为v 。

2）圆弧插补如图，圆弧三点坐标()1111,,z y x P 、()2222,,z y x P 、()3333,,z y x P ，姿态为),,(1111γβαθ、),,(2222γβαθ、),,(3333γβαθ，始末加速段加速度为a ，中间段速度YXZO tv直线插补及梯形速度控制av),,(2222γβαθ),,(2222z y x P ),,(1111z y x P ),,(1111γβαθ求直线距离有解？ Y N运动总时间 总点数 位姿增量 加减速控制参各插补点位姿 求加减速时间直线插补梯形加减速控制参数求解加速后的位移 归一化处理 加减速控制参数设置安全加速直线插补流程图为v 。

①判断三点共线利用向量21P P 和向量32P P 叉乘来判断。

②三平面法求圆心和半径1P 、2P 和3P 点确定的平面M0323232313131333=---------z z y y x x z z y y x x z z y y x x 过21P P 中点且与之垂直的平面T()()()()()()0212121122112211221=-⎥⎦⎤⎢⎣⎡+-+-⎥⎦⎤⎢⎣⎡+-+-⎥⎦⎤⎢⎣⎡+-z z z z z y y y y y x x x x x 过32P P 中点且与之垂直的平面S()()()()()()0212121233223322332=-⎥⎦⎤⎢⎣⎡+-+-⎥⎦⎤⎢⎣⎡+-+-⎥⎦⎤⎢⎣⎡+-z z z z z y y y y y x x x x x 联立三个平面方程，用消去法可求得圆心，在求解过程中要讨论六种情况（消去过程中分母不能为零的六种情况）。

求半径：()()()201201201z z y y x x r -+-+-=③求变换矩阵xzyO()1111,,z y x P ()2222,,z y x P ()3333,,z y x P ()0000,,z y x P R O UV共线？初始化数据三平面法求圆心和半径求坐标变换矩各点新坐标 直线插补YN平面圆弧插补 原坐标插补点数圆弧插补圆弧插补流程以圆心0P 为原点R O 建立坐标系，以10P P 方向为U 轴，其单位方向矢量为：PP P P u 010=W 轴为与向量21P P 和32P P 相垂直的方向，单位方向矢量为：32213221P P P P P P P P w ⨯⨯=v 轴按右手法则来定，其单位方向矢量为：u w v ⨯=因此，变换矩阵如下：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=1000oz z z z oy y y yox x x x R p w v u p w v u p w v u T 逆矩阵如下：⎥⎦⎤⎢⎣⎡-=-1001P R R T T T R其中，⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=z zzy yyx x xw v u w v u w v u R ，⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=oz oy ox p p p P ④将各点转换为新坐标0321000======w w w w v u ，半径1u r =⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-111111111z y x T w v u R ，⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-112221222z y x T w v u R ，⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-113331333z y x T w v u R ⑤平面圆弧插补在平面UV O R -平面内进行圆弧插补，0θ为圆弧的弧度：⎪⎩⎪⎨⎧+=),(2tan 2),(2tan 33330u v A u v A ππθ 000333<=>v v vp 点为圆弧上任一点，弧度为θ，则有：λθθ=则插补点坐标为：()()⎪⎩⎪⎨⎧===0sin cos w r v r u θθ ⑥插补点原坐标系坐标p 点在原坐标系中的坐标()z y x ,,为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡11w v u T z y x R ⑦姿态的求解姿态各轴的增量：⎪⎩⎪⎨⎧-=∆-=∆-=∆131313γγγβββααα 可得插补点姿态如下：⎪⎩⎪⎨⎧∆+=∆+=∆+=γλγγβλββαλαα111 （4）连续直线路径、轨迹在直线插补规划中，启动加速停止减速，若连续直线运动，则再启动运动到下一点，这样使电机不停地启动和停止，引起较大的振动和磨损。

为避免此问题，RO UV()3333,,w v u P ()1111,,w v u P RO UV()3333,,w v u P ()1111,,w v u P ()w v u p ,,()w v u p ,,θθrr圆弧插补新坐标可用圆弧过渡的方法将相邻直线连接，完成平滑匀速过渡。