第六章 轨迹规划

f
0
0 tf
单个关节的不同轨迹曲线
t
为了实现平稳运动，轨迹函数至少需要四个约束条件。即 ————满足起点和终点的关节角度约束
————满足起点和终点的关节速度约束（满 足关节速度的连续性要求）
解上面四个方程得：
注意：这组解只适用于关节起点、终点速度为零的运动情况。
例：设只有一个自由度的旋转关节机械手处于静止状态时， =150，要在3s内平稳运动到达终止位臵： 止点的速度为零。 解： 将上式的已知条件代入以下四个方程得四个系数： =750，并且在终
d.两段路径之间的过渡（避免速度突变）
e.笛卡尔空间规划的特点 笛卡尔空间的规划方法不仅概念上直观，而且规划的路径准确。 笛卡尔空间的直线运动仅仅是轨迹规划的一类，更加一般的应 包含其他轨迹，如椭圆、抛物线、正弦曲线等等。可是由于缺 乏适当的传感器测量手部苗卡尔坐标，进行位臵和速度反馈。 笛卡尔空间路径规划的结果需要实时变换为相应的关节坐标， 计算量很大，致使控制间隔拖长。如果在规划时考虑操作臂的 动力学特性．就要以笛卡尔坐标给定路径约束，同时以关节坐 标给定物理约束(例如，各电机的容许力和力矩，速度和加速度 极限)，使得优化问题具有在两个不同坐标系中的混合约束。因 此．笛卡尔空间规划存在由于运动学反解带来的问题。
D(0) I 4X4 D(1) Pi Pi 1
B 1 B
c.两结点之间的直线运动 工具坐标系从结点Pi到Pi +1的运动可分解为一个平移运动 和两个旋转运动：第一个转动使工具轴线（a)与预期的接 近方向a对准；第二个转动是绕工具轴线(a)转动，使方向 矢量o对准。
D( ) L( ) R a ( ) R o ( )
任务规划器
１.轨迹规划的一般性问题 这里所谓的轨迹是指操作臂在运动过程中的位移、速度和 加速度。 常见的机器人作业有两种：
•点位作业（PTP=point-to-point motion） •连续路径作业（continuous-path motion），或者称为轮廓运动
（contour motion）。
(t ) a0 a1t a2t 2 a3t 3 a4t 4 a5t 5
c、用抛物线过渡的线性插值 单纯线性插值将导致在结点处关节运动速度不连续，加速度
无限大。

0
t

解决办法：在使用线性插值时，
f
把每个结点的邻域内增加一段抛
物线的“缓冲区段”，从而使整
0
0 tb tf-tb tf t
等。
B
D C
图中A区域的位臵码 (Location Code:LC)为3031。 问：图中B，C，D区域的位臵码 LC为?
b.全局路径规划（Global Path Planning）和局部路径规划 （Local Path Planning）
自主移动机器人的导航问题要解决的是： （1）“我现在何处？”； （2）“我要往何处去？”； （3）“要如何到该处去？”。
对于方法（2），为了保证路径点处的加速度连续，可以设法 用两条三次曲线在路径点处按照一定的规则联系起来，拼凑成所
要求的轨迹。其约束条件是：联接处不仅速度连续，而且加速度
也要连续。
g
v
0
0
t0
tv
tg t
对于方法（3）， 这里所说的启发式方法很简单，即假设用 直线段把这些路径点依次连接起来，如果相邻线段的斜率在路径 点处改变符号，则把速度选定为零；如果相邻线段不改变符号， 则选择路径点两侧的线段斜率的平均值作为该点的速度。
这里所谓的轨迹是指操作臂在运动过程中的位移、速度和 加速度。
例1：机器人倒水的规划
倒一杯水 取一个杯子 找到水壶 打开水壶 把水倒入杯中 任务规划层
动作规划层 提起水壶到 杯口上方 把水壶倾斜 把水壶竖直 把水壶放回 原处
手部从A点移动到B点
路径规划层 轨迹规划层
关节从A点移动到B点
机器人倒水动作的规划层次
因此得：
(t ) 15.0 20.0t 2 4.44t 3 (t ) 40.0t 13.32t 2 (t ) 40.0 26.64t

b. 过路径点的三次多项式插值 方法是：把所有路径点都看成是“起点”或“终点”，求解 逆运动学，得到相应的关节矢量值。然后确定所要求的三次多项
的时间 t b
为保证tb有解，则
当式(6．21)中的等号成立时，线性域的长度缩减为零，整个路 径段由两个过渡域组成，这两个过渡域在衔接处的斜率(代表速
度)相等。当加速度的取值越来越大时，过渡域的长度台越来越
短。如果加速度选为无限大，路径又回复到简单的线性插值情况。
d、过路径点的用抛物线过渡的线性插值 如图所示，某个关节在运动中设有n个路径点，其中三个相

f
对于恒定加速度的过渡域[t0-tb]
1 2 b 0 t b 2
0
0 tb tf-tb tf t
带抛物线过渡的线性插值（1）
这样对于任意给定的θf、θ0和t，可以选择相应的 和 t b ，得 到路径曲线，通常的做法是选择加速度 的值，然后计算相应
t b2 t tb ( f 0 ) 0
第六章 机器人的轨迹规划
机器人的规划是分层次的，先从高层的任务规划、 动作规划再到手部的轨迹规划和关节的轨迹规划。 任务规划、动作规划属于机器人高级规划的范畴，一 般依赖人来完成。把任务分解为一系列子任务的规划称 为任务规划;将每个子任务分解为一系列动作的规划称为 动作规划。 工业机器人通常具有轨迹规划和底层的控制 功能。操作人员机械手末端的目标位臵和方位，以及运 动到目标点的速度，机器人控制器便可确定出达到目标 的关节轨迹。

伪节点
如果要求机器人通 过某个结点，同时速度 不为零，怎么办？ 可以在此结点两端规定两个 “伪结点”，令该结点在两 伪结点的连线上，并位于两 t 过渡域之间的线性域上。
原节点
0
用伪节点的插值曲线
3.笛卡尔空间规划方法
在这种轨迹规划系统中，作业是用操作臀终端抓手位姿的笛
卡尔坐标结点序列规定的。因此，结点指的是表示抓手位姿(位

A
D
C
0
B
t0
tA tB
tC
tD t
路径点上速度的自动生成
如果对于运动轨迹的要求更为严格，约束条件增多，那么 三次多项式就不能满足需要，必须用更高阶的多项式对运动轨 迹的路径段进行插值。例如，对某段路径的起点和终点都规定 了关节的位臵、速度和加速度（有六个未知的系数），则要用 一个五次多项式进行插值。
邻的路径点表示为j，k和l，每两个相邻的路径点之间都以线性 函数相连，而所有的路径点附近则有抛物线过渡。（同样存在多 解，每个解对于一个选取的加速度值）

j l k 0
多段带有抛物线过渡的线性插值轨迹
t
Biblioteka Baidu
应当注意的是，多段用抛物线过渡的直线样条函数一般并不 经过那些路径点，除非在这些路径点处停止。若选取的加速 度充分大，则实际路径将与理想路径点十分靠近。如果要求 机器人选经某个结点，那么格轨迹分成两段，把此结点作为 前一段的终止点和后一段的起始点即可。
局部路径规划主要解决（1）和（3）两个问题，即机器人 定位和路径跟踪问题；方法主要有：人工势场法 、模糊逻辑算 法等 。
全局路径规划主要解决（2），即全局目标分解为局部目 标，再由局部规划实现局部目标。主要有：可视图法 、环境分 割法（自由空间法 、栅格法 ）等 ；
c.离线路径规划和在线路径规划 离线路径规划是基于环境先验完全信息的路径路径规划。 完整的先验信息只能适用于静态环境，这种情况下，路径是离 线规划的；在线路径规划是基于传感器信息的不确定环境的路
速度，有以下三种方法：
（1） 根据工具坐标系在直角坐标空间中的瞬时线速度和角速
度来确定每个路径点的关节速度 ；该方法工作量大。
（2）为了保证每个路径点上的加速度连续，由控制系统按照 此要求自动地选择路径点的速度。
（3）在直角坐标空间或关节空间中采用某种适当的启发式方
法，由控制系统自动地选择路径点的速度；
轨迹规划既可以在关节空间也可以在直角空间中进行。
2.关节轨迹的插值
关节空间法计算简单、容易。再者，不会发生机构的奇异性
问题。
轨迹规划方法一般是在机器人的初始位臵和目标位臵之间用
多项式函数来“内插”或“逼近”给定的路径，并产生一系列的
控制点。 a. 三次多项式插值 只给定机器人起始点和终 止点的关节角度。
c.两结点之间的直线运动
操作臂在完成作业时，抓手的位姿可以用一系列结点Pi来表示。 因此，在直角坐标空间中进行轨迹规划的首要问题是由两结点Pi 和Pi +1，所定义的路径起点和终点之间，如何生成一系列中间点。 两结点之间最简单的路径是在空间的一个直线移动和绕某定粕的 转动。若运动时间给定之后．则可以产生一个使线速度和角速度 受控的运动。
更一般地，从一结点Pi 到下一结点Pi +1的运动可表示为
c.两结点之间的直线运动 从结点Pi到Pi +1的运动可以由“驱动变换”D(λ)来表示。
0 6
T ( )0 TB Pi D( )6 T-1 B E
其中，驱动变换D(λ)是归一化时间λ的函数，λ=t/T;t为自 运动开始算起的实际时间；T为走过该轨迹段的总时间。
式插值函数，把路径点平滑的连接起来。不同的是，这些“起点”
和“终点”的关节速度不再是零。
3
0
0 t0 t1 t2 t

同理可以求得此时的三次多项式系数：
此时的 速度约 束条件 变为：
(0) 0 (t f ) f



由上式确定的三次多项式描述了起始点和终止点具有任意给定位 臵和速度的运动轨迹。剩下的问题就是如何确定路径点上的关节
例如，要求机 器人按直线运 动，把螺接从 槽中取出并放 入托架的一个 孔中，如图612所示。
b.作业的描述 参考这些结点的位姿将作业描述为手部的一连串运动和动作
每一结点Pi对应一个变换方程（2.34），从而解得相应的手 臂变换06T。由此得到作业描述的基本结构：作业结点Pi对应 手臂变换06T，从一个变换到另一变换通过手臂运动实现。
带抛物线过渡的线性插值（1）
个轨迹上的位移和速度都连续。

对于多解情况，如右图所示。加
速度的值越大，过渡长度越短。
f
h
0
0 th tf t
带抛物线过渡的线性插值（2）
过渡域[t0-tb]终点的速度等于直线域的速度
（ h b t t 1/2 f 0 ) b f 0 2 b tb b b th tb 1 / 2 t tb t 2tb
主要是笛卡尔路径上解的存在性(路径点都在工 作空间之内与否)、唯一性和奇异性。
5.2 移动机器人的轨迹规划
1. 机器人的路径规划（一般指位臵规划）
a.基于模型和基于传感器的路径规划 基于模型的方法有：c-空间 法、自由空间法、网格法、四叉 树法、矢量场流的几何表示法等。
相应的搜索算法有A*、遗传算法
根据路径约束和障碍约束，机器人的规划和控制可以分为四类：
操作臂最常用的轨迹规划方法有两种：
第一种是要求对于选定的轨迹结点（插值点）上的位姿、速 度和加速度给出一组显式约束（例如连续性和光滑程度等），轨
迹规划器从一类函数（例如n次多项式）选取参数化轨迹，对结
点进行插值，并满足约束条件。 第二种方法要求给出运动路径的解析式。
臵和姿态)的齐次变换短阵。 a. 物体对象的描述 利用第二章有关物体空间的描述方法，任一刚体相对 参考系的位姿是用与它固接的坐标系来描述。相对于
因接坐标系，物体上任一点用相应的位臵矢量p表示； 任一方向用方向余弦表示。给出物体的几何图形及因
接坐标系后，只要规定固接坐标系的位姿，便可重构
该物体。
b.作业的描述 作业和手臂的运动可用手部位姿结点序列来规定，每个结点 是由工具坐标系相对于作业坐标系的齐次变换来描述。相应的关 节变量可用运动学反解程序计算。
对于目前绝大多数商用机器人，用户完全可以只进 行手部的路径规划，机器人控制器会自动进行各关 节轨迹的规划。
{xk}
任务规划 器 压缩的数 据 图像分析 器 I(k,e)
摄象机
q(t) qd(t)
轨迹规划 器 机器人控 (t ) 操作臂动 制器 力学 操作臂运 动学
x(t) 环境
F(t)
力传感器