东南大学远程教育
机器人技术
第 十 三 讲
主讲教师：王兴松
机器人规划
Robot planning
7.1 机器人规划的作用与任务
7.2 积木世界的机器人规划
7.3 STRIPS 规划系统
7.4 具有学习能力的规划系统
7.5 基于专家系统的机器人规划
7.6 太空构件装配顺序分层规划系统
7.7 机器人轨迹规划
Robot planning
7.1 机器人规划的作用和任务
1.规划的作用与问题分解途径
（ 1）规划的概念及作用
概念：在执行一个问题求解程序中的任何一步
之前，计算该程序几步的过程，是一个行动过程的
描述。
蕴涵排序。
步骤含糊。
具有分层结构。
Robot planning
7.1 机器人规划的作用和任务
1.规划的作用与问题分解途径
（ 1）规划的概念及作用
作用：
缺乏规划可能导致不是最佳的问题求解。
规划可用来监控求解过程。如发射火箭。
Robot planning
7.1 机器人规划的作用和任务
1.规划的作用与问题分解途径
（ 2）问题分解的途径：
途径一：只考虑状态中可能变化了的那些部分。
途径二：把单一的问题分割成为几个子问题。
Robot planning
7.1 机器人规划的作用和任务
1.规划的作用与问题分解途径
（ 3）域的预测和规划的修正：
产生一个有希望成功的规划。
规划失败，从现有状态从新开始规划。
非期望结果并不使余下部分失败。
使规划失败的影响限制在局部内。
记录规划执行步骤及理由，便于回溯。
Robot planning
7.1 机器人规划的作用和任务
2.机器人规划系统的任务与方法
（ 1）机器人规划系统的任务：
◆ 根据最有效的启发信息，选择应用于下一步的
最好规则。
◆ 应用所选取的规则来计算生成的新状态。
◆ 对所求得的解答进行检验。
◆ 检验空端，并舍弃。
◆ 检验殆正确的解答，并应用具体技术使之完全
正确。
Robot planning
7.1 机器人规划的作用和任务
2.机器人规划系统的任务与方法
（ 2）机器人规划系统的方法：
ⅰ 选择和应用规则：
方法一，
对每个动作都叙述起所引起的状态表示
的每一个变化。
另用某些语句来描述所有其它维持不变的
事物。
优点：只需一个机理 —— 消解。
缺点：若问题状态复杂，则需很多的公理条数。
Robot planning
7.1 机器人规划的作用和任务
2.机器人规划系统的任务与方法
（ 2）机器人规划系统的方法：
方法一例：
谓词集合描述状态：如图
动作影响到的状态：
用框架公理规则描述不受操作符影响的状态：
) ) ]),,((,())),,((,([
)],,(),([
00
00
SBAU N ST A C KDOBC L E A RSBAU N ST A C KDOAH O L D I N G
SBAONSAC L E A R
?
??
))),,((,(),( 00 SBAU N S T A C KDOBO N T A B L ESBO N T A B L E ?
))),,((,,()],(),,([ 00 SBAU N S T A C KDONMONAMNESNMON ??
Robot planning
7.1 机器人规划的作用和任务
2.机器人规划系统的任务与方法
（ 2）机器人规划系统的方法：
ⅰ 选择和应用规则：关于框架公理
使用初始表，删除表和添加表。
优点：
减少提供给操作符的信息。
操作后不必推演所有信息。
便于回溯。如图：
Robot planning
7.1 机器人规划的作用和任务
2.机器人规划系统的任务与方法
（ 2）机器人规划系统的方法：
ⅱ 检验解答与空端：
求得能将初始状态变换为目标状态的操作符序
列，即得解。
空端，从它无法到达目标的端点。
正向推理及反向推理。
Robot planning
7.1 机器人规划的作用和任务
2.机器人规划系统的任务与方法
（ 2）机器人规划系统的方法：
ⅲ 修正殆正确解的各种方法：
◆ 检查求得的状态，并把它与期望目标加以比较。
◆ 注意有关出错的知识，然后加以直接修正。
◆ 不修正，不完全确定地让差别保留到最后的可
能时刻。
Robot planning
7.2 积木世界的机器人规划
1.积木世界的机器人问题
（ 1）积木世界：几个有标记的一样大小的积木。
（ 2）机器人
动 作,unstack(a,b) stack(a,b)
pickup(a) putdown(a)
状态描述,ON(a,b) ONTABLE（ a）
CLEAR(a) HOLDING(a)
HANDEMPTY
Robot planning
7.2 积木世界的机器人规划
1.积木世界的机器人问题
（ 2）机器人 例：
初始状态,CLEAR（ B）
CLEAR（ C）
ON（ C,A）
ONTABLE（ A）
ONTABLE（ B）
HANDEMPTY
目标状态,ON（ B,C） ∧ ON（ A,B）
Robot planning
7.2 积木世界的机器人规划
2.用 F规则求解规划序列
（ 1）先决条件
（ 2）删除表
（ 3）添加表
例 move(x,y,z)
先决条件 CLEAR（ x）,CLEAR（ z）,ON（ x,y）
删除表 ON(x,y),CLEAR(z)
添加表 ON(x,z),CLEAR(y)
Robot planning
7.2 积木世界的机器人规划
2.用 F规则求解规划序列
表示图 4的所有状态空间
Robot planning
7.3 STRIPS规划系统
1.STRIPS系统的组成
（ 1）世界模型。
为一阶谓词演算公式。
（ 2）操作符（ F规则）。
包括先决条件，删除表，添加表。
（ 3）操作方法。
应用状态空间表示和中间 — 结局分析。
Robot planning
7.3 STRIPS规划系统
2.STRIPS系统规划过程
例 7.1,要求机器人到邻室去取回一个箱子。
Robot planning
7.3 STRIPS规划系统
提供两个操作符：
gothru(d,r1,r2)
pushthru(b,d,r1,r2)
这个问题的差别表：
Robot planning
7.3 STRIPS规划系统
初始状态 M0,INROOM（ ROBOT,R1）
INROOM（ VOX1,R2）
CONNECTS（ D1,R1,R2）
目标状态 G0,INROOM（ ROBOT,R1） ∧
INROOM（ BOX1,R1）
采用中间 — 结局分析方法来逐步求解
经 gothru(C1,R1,R2)
中间状态 M1,INROOM（ ROBOT,R2）
CONNECTS（ D1,R1,R2）
经 pushthru
中间状态 M2,M2=G0
Robot planning
7.3 STRIPS规划系统
例 7.1的搜索图及与或图
Robot planning
7.3 STRIPS规划系统
3.含有多重解答的规划
例 7.2,要求机器人将不同区域的三个箱子移到同
一区域内。
Robot planning
7.3 STRIPS规划系统
3.含有多重解答的规划
操作符：
push(k,m,n) goto(m,n)
初始模型 M0,ATR(a),AT(BOX1,b),
AT(BOX2,c),AT(BOX3,d)
目标模型 G0,(BOX1,x） ∧ （ BOX2,x） ∧
（ BOX3,x）
Robot planning
7.3 STRIPS规划系统
采用消解反演和中间 — 结局分析。
解 1,{goto(a,c),push(BOX2,c,b),goto(b,d),
push(BOX3,d,b)}
Robot planning
7.3 STRIPS规划系统
3.含有多重解答的规划
解 2,{goto(a,b),push(BOX1,b,c),goto(c,d),
push(BOX3,d,c)}
解 3,{goto(a,b),push(BOX1,b,d),goto(d,c),
push(BOX2,c,d)}
Robot planning
7.4 具有学习能力的规划系统
PULP— Ⅰ 系统
普渡大学,1976~1979,S.Tangwongsan &傅京孙
一种管理式（监督式）学习系统。
优点：
输入的目标语句表示为英语语句。
应用辅助物体改善了系统对物体的操作能力。
Robot planning
7.4 具有学习能力的规划系统
1.PULP— Ⅰ 系统的结构与操作方式
字典：英语词汇的集合。
模型：部分包括模型世界内
物体现有状态的事实。
过程：集中了预先准备好的
知识。
方块：集中了 LISP程序。
SENEF程序：用语义网络来
表示知识。
操作如图。
Robot planning
7.4 具有学习能力的规划系统
2.PULP— Ⅰ 的世界模型和规划结果。
一个具体任务下的世界模型。
Robot planning
7.4 具有学习能力的规划系统
2.PULP— Ⅰ 的世界模型和规划结果。
对 STRIPS,ABSTRIPS和
PULP— Ⅰ 三个系统的规划速
度进行比较。
Robot planning
7.5 基于专家系统的机器人规划
1.系统结构和规划机理
采用基于规则的专家系统。
（ 1）系统结构和规划机理 。 五部分
◆知识库。
◆控制策略。
◆推理机。
◆知识获取。
◆解释与说明。
一定的工具：支援软件和硬件。
Robot planning
7.5 基于专家系统的机器人规划
1.系统结构和规划机理
（ 2）任务级机器人规划三要素
◆建立模型。
对物体和机器人的几何，运动，物理描述。
◆任务说明。
说明各物体所期望的空间关系。
◆程序综合。
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
Robot Planning Expert Systems
(1)系统简化框图。
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(2)世界模型
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(3)规划与执行结果
共使用 15条规则。
规则 14：
表示机器人用小车把重型零件（ object）从
某工段 area Rb的位置 (Xb,Yb)推移 (push)到其目
的位置 (Xg,Yg)工段的位置所应具备的条件和必
须遵循的操作。
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(3)规划与执行结果
例，物体在第 1工段，运送至第 4工段第 8工作台。
应具备的条件,4条
◆已知零件位置。 at(Object,Rb,(Xb,Yb))
◆ 已知目标工段。 at4(Goal,Rg,(Xg,Yg))
◆ 零件两位置关系。
diff(Rb,R4),diff(R4,Rb),diff(Rg,R1),
diff(R1,Rb),diff(Rb,Rg),diff(R4,R1),
(connects(D2,R1,Rb)； connects(D1,Rb,R1)),
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(3)规划与执行结果
例，接上
(connects(D2,Rg,R1)； connects(D2,R1,Rg)),
(connects(D3,Rb,R4)； connects(D3,R4,Rb)),
(connects(D3,R4,Rg)； connects(D3,Rg,R4)),
(neighbors(Rb,R4)； neighbors(Rg,R4))
◆ 已知推零件过程中所经过的两个门道的位置。
at6(D1,Rb,(X2,Y2)),at6(D1,R1,(X3,Y3)),
at6(D2,R1,(X4,Y4)),at6(D2,Rg,(X5,Y5)),
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(3)规划与执行结果
动作序列
push((Rb,(Xb,Yb)),(Rb,(X2,Y2))),
pushthru(D1,(Rb,(X2,Y2)),(R1,(X3,Y3))),
push((R1,(X3,Y3)),(R1,(X4,Y4))),
pushthru(D2,(R1,(X4,Y4)),(Rg,(X5,Y5))),
push((Rg,(X5,Y5)),(Rg,(Xg,Yg))),
transfer(Object,cart1,Goal)
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(3)规划与执行结果
一个具体任务：
处在工段 3的机器人把工段 2的重型零件 part10搬
移至工段 5的工作台 5上。
用户输入数据：
[area3,a,b],
[area6,_,_],
[area2,0,2],
[area5,4.5,1],
[part10,table5].
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(3)规划与执行结果
一个具体任务：
作业任务的图解。
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(3)规划与执行结果
本系统最大可能解为 8个。
例：原处于工段 1的机器人，
使用工段 6的小车把工段 1内的一
个重型零件送至工段 6的某处。
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(4)比较
表 7.2比较 4个系统的复杂性
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(4)比较
比较 4个系统的规划速度。
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(4)比较
表 7.3仔细地比较了 PULP— 1和 ROPES的规划速度
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(5)结论与讨论
◆更好的规划性能和更快的规划速度。
◆可输出某任务的所有解。
◆将概率，可信度，模糊理论运用于不确定任务。
◆ C-PROLOG语言简单，有效。
◆操作符数目增大时，规划时间增加得很少。
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 1）拆开顺序规划器的可行性准则
a,
几何可行性的定义：
任何支梁如果在下一个执行装配操作点，机器人
臂和末端装置可达的，则称为支梁几何可行性。
凸形外壳的定义：
如果一组顶点处在所建立的三维容积空间的端点
上，那么这组顶点就是顶点集合的凸形外壳。
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 1）拆开顺序规划器的可行性准则
几何可行性的一般定义：
3D（ N,S）,CH（ N）
N,结点数 S,支梁数
3D：结点和支梁组成的三维空间
CH：结点的凸形外壳
ⅰ 检验所有处于凸形外壳上的支梁，并放入活动
表 (active list)A-。
ⅱ,具有,
ⅲ 结束。
Robot planning
SSi ?? ),( 1NNi ))()(( 1 ik SANCHNNCHNif ?
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 1）拆开顺序规划器的可行性准则
b,
结构可行性的定义：
如果移动一个支梁导致最大稳定和刚性结构，则
该支梁具有满足结构可行性条件的特性。
三维空间结构的两个层次：
◆支梁和结点相互连接，形成完整的上层结构。
◆形成互连刚性子结构的支梁和结点集合，建立完
整结构。
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 1）拆开顺序规划器的可行性准则
C，连接关系，稳定性和可行性准则
结构容积单元（ SVE）,Structural Volume Element
是支梁和结点的集合，它们以规定的方法互连，
形成结构上刚性和稳定的状态。
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 1）拆开顺序规划器的可行性准则
非结构容积单元（ NSVE）：
是不形成结构单元的支梁和结点的任务集合。
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 1）拆开顺序规划器的可行性准则
SVE和 NSEV结构提供了空间构件装配的分层表示。
SVE的 3种互连关系：
◆点连接 0（ 1）阶
◆线连接 0（ 2）阶
◆面连接
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 2）规划规则的表示与搜索
Ⅰ 状态和规划表示
分层表示。
底层部分包括支梁和结点。
高层部分包括 SVE和 NSEV。
由上述两部分组成的关系图表示三维装配约束。
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 2）规划规则的表示与搜索
Ⅱ 分层搜索策略
搜索空间限制在可能导致可行解的范围内。
分层方法由两层拆装组成。
第一层应用 SVE约束决定 SVE单元的顺序。
SVE被定序后，就开始第二层拆装规划。
由保证子目标最优解，从而保证全局最优。
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 2）规划规则的表示与搜索
第一层 SVE级算法：
◆开始。
◆ SVE 确定 3D空间的所有 SVE单元。
◆ SVEvalid 确定所有有效的 SVE单元。
◆ SVEminface 确定含有最少面数的 SVE单元。
◆如果 SVEminface=1 SVEnext SVEminface,否则
SVEease 确定从 SVEminface拆装的最容易 SVE单元。
◆ SVEnext SVEease
◆ 3D REMOVE(SVEease)
◆ DISASSEMBLEsve(3D’)
?
?
?
?
?
?
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 2）规划规则的表示与搜索
第一层 SVE级 确定顺序之准则：
◆移动应当影响最少的面连接数。
◆移动必须是最容易的。
中间不稳定态尽可能少。
◆移动必须是有效的。
前后两个 SVE必须相连。
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 2）规划规则的表示与搜索
第二层 支梁级,
选择具有最小总代价的有效支梁。
确定几何可行的支梁。
运用启发代价函数。
例：图 7.26
四面体 {6,5,4,3,2,1}
五面体 {7,3,2,1,6,4,8,5}
Robot planning
7.6 太空构件装配顺序分层规划系统
2.三维结构装配顺序规划示例
例 7.3 3D(10,24),7个 SVE单元,3个五面体。
步骤：
13，（ 7,19,18）
（ 24,12,17），（ 11,23,16）
（ 22,10,15），（ 20,8,14,9）
Robot planning
7.6 太空构件装配顺序分层规划系统
2.三维结构装配顺序规划示例
例 7.4 3D（ 31,102）
Robot planning
7.7 机器人轨迹规划
轨 迹,机械手的位移，速度和加速度。
轨迹规划,由任务要求，计算出预算的运动轨迹。
轨迹规划器：简化了编程手续。
1.轨迹规划应考虑的问题
◆机器人规划方式的分类
Robot planning
7.7 机器人轨迹规划
1.轨迹规划应考虑的问题
◆机械手常用的两种轨迹规划方法：
方法一，给出插值点上一组显式约束。
方法二，给出运动路径的解析式。
◆轨迹规划可在关节空间或直角空间中。
◆规划器的任务：解变换方程，运动学反解和插值
运算。
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 1）三次多项式插值
四个约束条件：
由上确定了一个三次多项式：
)1.7()( )0( 0
??
?
?
?
fft ??
??
)2.7(
0)(
0)0(
??
???
?
?
?
?
ft?
?
)3.7()( 332210 tatataat ?????
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 1）三次多项式插值
关节速度和加速度：
关于四个系数的线性方程：
)4.7(
52)(
32)(
32
2
321
??
???
??
???
??
?
taat
tataat
?
?
)5.7(
320
0
2
321
1
3
3
2
210
00
?
?
?
?
?
?
?
???
?
????
?
ff
ffff
tataa
a
tatataa
a
?
?
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 1）三次多项式插值
解得四个系数的表达式：
)6.7(
)(
2
)(
3
0
033
022
1
00
?
?
?
?
?
?
?
?
?
???
??
?
?
??
??
?
f
f
f
f
t
a
t
a
a
a
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 2）过路径点的三次多项式插值
将速度约束条件 (7.2)变为：
重新求得三项式的系数：
)7.7(
)(
)0( 0
??
???
?
?
??
??
fft ??
??
)9.7(
)(
1
)(
2
12
)(
3
00
33
00
22
01
00
?
?
?
?
?
?
?
?
?
?????
????
?
?
??
??
?
f
f
f
f
f
ff
f
f
tt
a
ttt
a
a
a
????
????
?
?
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 2）过路径点的三次多项式插值
确定路径点上关节速度的三种方法：
◆根据工具坐标在直角坐标空间中的瞬时线速度、
和角速度来确定。
◆采用适当的启发式方法，有控制系统自动地选
择。
◆要保证每个路径点上的加速度连续。
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 3）高阶多项式插值
五次多项式：
6个约束条件：
)10.7()( 5544332210 tatatatataat ???????
)11.7(
201262
2
5432
3
5
2
432
2
0
4
5
3
4
2
321
1
0
5
5
4
4
3
3
2
210
00
?
?
?
?
?
?
?
?
?
?
?
????
?
?????
?
??????
?
??
??
?
?
fff
f
ffff
f
ffffff
tatataa
a
tatatataa
a
tatatatataa
a
?
?
?
?
?
?
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 3）高阶多项式插值
其解为：
)12.7(
2
)()66(1212
2
)23()1614(3030
2
)3()128(2020
2
3
2
00
0
3
3
2
00
0
4
3
2
00
0
3
0
2
0
1
00
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?????
?
?????
?
?????
?
?
?
?
??????
??????
??????
??
?
f
f
f
fff
f
f
f
fff
f
f
f
fff
t
tt
a
t
tt
a
t
tt
a
a
a
a
??????
??????
??????
?
?
?
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 4）用抛物线过渡的线性插值
将线性函数与两段抛物线函数平滑地衔接在一
起形成一段轨迹。
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 4）用抛物线过渡的线性插值
,tb处的速度
,tb处的关节角度
：过渡域内加速度
令 t=2th,由式 13,14得
t：所要求的运动持续时间
)14.7(
2
1
)13.7(
2
0 bb
bh
bh
tb
t
tt
?
?
??
?
?
?
???
??
?
)15.7(0)( 02 ???? ???? ???? fbb ttt
b?
???
tb??
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 4）用抛物线过渡的线性插值
任意给定, 和,选择相应的 和,得到
路径曲线。
需满足条件：
)16.7(
2
)(4
2
0
22
??
????
??
??
?
???? f
b
tt
t
)17.7()(4 2 0tf ??? ???
???
btf? 0? t
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 5）过路径点的用抛物线过渡的线性插值
j,k,l：连续的三个路径点。
,k点过渡域的持续时间。
,j,k之间线性域的持续
时间。
：连接 j和 k的路径段的全
部持续时间。
,j,k点之间线性域的速度。
,j点过渡域的加速度。
kt
jkt
djkt
jk
??
j
???
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 5）过路径点的用抛物线过渡的线性插值
任意给定, 和,可得 。
对于内部路径点,
k? ktdjkt
)18.7(
2
1
2
1
)s g n (
?
?
?
?
?
?
?
?
?
?
?
???
?
?
??
?
?
??????
?
kjd jkjk
k
jkkl
k
k
jk
kl
k
d jk
jk
jk
tttt
t
t
?
??
????
??
?
??
k?
)1,;2,1,( ??? nkjkj
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 5）过路径点的用抛物线过渡的线性插值
对于第一个路径段：
从而求得,进而求得 和,
)19.7(
2
1 11
112
12 t
tt d
???
?
? ???
)20.7(
2
1
2
1
)(2
)s g n (
211212
112
12
12
1
122
12121
1121
?
?
?
?
?
?
?
?
?
?
?
?
?
???
?
?
?
?
???
??
?
??
??????
tttt
tt
ttt
d
d
dd
??
?
?
??
????
1t 12?? 12t
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 5）过路径点的用抛物线过渡的线性插值
对于最后一个路径段：
根据上式便可求得：
)21.7(
2
1
)1(
1
nn
nnnd
n t
tt
n ??
?
? ?
?
? ???
)22.7(
2
1
2
1
)(2
)s g n (
1)1()1(
)1(
1
)1(
1
12
)1()1(
1
?
?
?
?
?
?
?
?
?
?
?
?
?
???
?
?
?
?
???
??
???
?
?
?
?
??
?
??
???
?
???
nnnndnn
nnnd
nn
nn
n
nn
nndnndn
nnnn
tttt
tt
ttt
??
?
?
??
????
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 1）物体对象的描述
在给出物体的几何图形及固接坐标后，其相对
于参考系的位姿则可用与它固接的坐标系来表示。
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 2）作业的描述
作业的运动可用手部位
姿结点序列来规定。
每个结点是由工具坐标
系相对于作业坐标系的齐次
变换来描述。
相应的关节变量可用运
动学反解计算。
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
如图 7.37所示，从 到 的运动可表示为，从
到
：工具坐标系 {T}相对于末端连杆系 {6}的变
换。
：分别为两个结点 和 相对于坐标系 {B}
的齐次坐标。
)23.7(160)(60 ?? EiBBi TPTT
1?iPiP
)24.7(1610)1(60 ??? ? EiBBi TPTT
iP 1?iP
ET6
1?iBiB PP 和
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
从 到 的运动可由, 驱动变换, D(λ )来表
示：
D(λ ),归一化时间 λ 的函数。
t,自运动开始算起的时间。
T,走过该轨迹的总时间。
)26.7()()( 16060 ?? EiBB TDPTT ??
]10[,,?? ?? Tt
iP 1?iP
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
在, t=0,则 λ =0,D(0)是 4× 4的单位矩阵，
则式 26与式 23相同。
在, t=T,则 λ =1，有
得
1)1( ?? iBiB PDP
iP
1?iP
)27.7()1( 11 ??? iBiB PPD
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
1000
1000 iziziziz
iyiyiyiy
ixixixix
iiii
i
B
paon
paon
paon
paon
P
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
????
????
????
????
?
1000
1000 1111
1111
1111
1111
1
zizizizi
yiyiyiyi
xixixixi
iiii
i
B
paon
paon
paon
paon
P
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
?
?
?
?
?
?
?
?
?
?
?
?
?????
?????
?????
?
????
????
????
1000
)(
)(
)(
)1(
1111
1111
1111
iiiiiiiii
iiiiiiiii
iiiiiiiii
ppaaaoana
ppoaooono
ppnanonnn
D
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
到 的运动分解为一个平移运动和两个旋转
运动。
L(λ ),的坐标原点沿直线运动到 的原点。
Ra(λ ),的接近矢量 转向 的接近矢量 。绕 a
Ro(λ ),的方向矢量 转向 的方向矢量 。绕 o
1?iP
)28.7()()()()( 0 ???? RRLD a?
iP
iP
iP
iP
1?iP
1?iP
1?iP
1?ia
1?io
ia
io
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
表示由绕 k转动 θ 角得到,k是 的 y轴绕
其 z轴转过 ψ 角得到，即
)29.7(
1000
100
010
001
)(
?
?
?
?
?
?
?
?
?
?
?
?
?
z
y
x
L
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
1
0
1
0
1000
0100
00
00
1
0
??
??
?
?
cs
sc
c
s
k
)(?aR iP
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
由通用旋转变换公式，得
)31.7(
1000
0100
00)()(
00)()(
)(0
?
?
?
?
?
?
?
?
?
?
?
? ?
?
????
????
?
cs
sc
R
)30.7(
1000
0)()()(
0)()()()(
0)()()()(
)(
2
2
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
??
??
?
????????
????????????
????????????
?
csssc
sscvcvcs
ccvcscvs
R a
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
将式（ 7,29）和 (7.31)带入 (7.28)中，得
)32.7(1000)( ?
?
?
??
?? dpdadodnD ?
?
?
?
?
?
?
?
?
?
?
????
????
???
?
)]()[()]()[(
)]()()[()]()[(
)]()[()]()[(
2
2
??????????
?????????????
???????????
sscscs
cvccvcss
vcscvss
do
dadodn
z
y
x
dp
c
ss
sc
da ??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
???
???
)(
)(
)(
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
解得：
)33.7(
)(
)(
)(
1
1
1
??
?
?
?
???
???
???
?
?
?
iii
iii
iii
ppaz
ppoy
ppnx
)35.7(]
])()[(
t a n [
)34.7(]t a n [
1
212
1
2
1
1
1
????
????
???
?
???
?
???
?
?
?
?
??
?
?
ii
iiii
ii
ii
aa
aoan
a
an
ao
a
)36.7(]t a n [ ?????? ???? csa
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 4）两段路径之间的过渡
手部达到结点前的时刻 τ 开始改变速度，保持
加速度不变，直到到达结点之后 τ 为止。
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 4）两段路径之间的过渡
加速度值：
其中
)37.7(][2 1)( 2 ???? ????????? tBTCtx
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
??
??
??
??
BA
BA
BA
BA
BC
BC
BC
BC
y
x
B
y
x
C
y
x
tx
?
?
?
?
?
?
)(
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 4）两段路径之间的过渡
由 7.37得在区间 -τ <t<τ 中的速度和位移
△ C和 △ B：分别为 B到 C和 B到 A的直角坐标距离和角度。
T,机械手从 B到 C所需时间。
)39.7(]2)[()(
)38.7(][
1
)(
BBB
T
Ctx
B
B
T
Ctx
????????
?
?????
?
??
?
?
?
?
?
Robot planning
7.7 机器人轨迹规划
4.规划轨迹的实时生成
（ 1）关节空间轨迹的生成
对于三次样条，随时间变化不断按式 7.3和 7.4。
到达路径段终点时，调用新路径段三次样条系数。
对于带抛物线过渡的直线样条插值。处于线性
域：
)41.7(
0 ?
?
?
?
?
?
?
?
?
??
??
??
?
?
??
???
jk
jkj t
Robot planning
7.7 机器人轨迹规划
4.规划轨迹的实时生成
（ 1）关节空间轨迹的生成
当处于过渡域时：
令 则
)42.7(
2
1
)(
2
?
?
?
?
??
?
?
?
?
??
????
????
????
???
k
i nbkjk
i nbki nbjkj
t
ttt
??
???
????
)21( jkjin b tttt ???
Robot planning
7.7 机器人轨迹规划
4.规划轨迹的实时生成
（ 2）笛卡儿空间轨迹的生成
带有抛物线过渡的线性规划轨迹，在线性域：
t,从第 j个路径点算起的时间。
：由类似 (7.18)的方程求出。
)44.7(
0 ?
?
?
?
?
?
?
?
?
??
??
??
?
x
xx
txxx
jk
jkj
jkx
?
Robot planning
7.7 机器人轨迹规划
4.规划轨迹的实时生成
（ 2）笛卡儿空间轨迹的生成
带有抛物线过渡的线性规划轨迹，在过渡域：
)45.7(2
1
)(
)
2
1
(
2
?
?
?
?
?
?
?
?
?
?
??
????
???
????
???
???
k
i n b
kjk
i n b
k
i n b
jk
j
jkji n b
xx
txxx
txttxxx
tttt
机器人技术
第 十 三 讲
主讲教师：王兴松
机器人规划
Robot planning
7.1 机器人规划的作用与任务
7.2 积木世界的机器人规划
7.3 STRIPS 规划系统
7.4 具有学习能力的规划系统
7.5 基于专家系统的机器人规划
7.6 太空构件装配顺序分层规划系统
7.7 机器人轨迹规划
Robot planning
7.1 机器人规划的作用和任务
1.规划的作用与问题分解途径
（ 1）规划的概念及作用
概念：在执行一个问题求解程序中的任何一步
之前，计算该程序几步的过程，是一个行动过程的
描述。
蕴涵排序。
步骤含糊。
具有分层结构。
Robot planning
7.1 机器人规划的作用和任务
1.规划的作用与问题分解途径
（ 1）规划的概念及作用
作用：
缺乏规划可能导致不是最佳的问题求解。
规划可用来监控求解过程。如发射火箭。
Robot planning
7.1 机器人规划的作用和任务
1.规划的作用与问题分解途径
（ 2）问题分解的途径：
途径一：只考虑状态中可能变化了的那些部分。
途径二：把单一的问题分割成为几个子问题。
Robot planning
7.1 机器人规划的作用和任务
1.规划的作用与问题分解途径
（ 3）域的预测和规划的修正：
产生一个有希望成功的规划。
规划失败，从现有状态从新开始规划。
非期望结果并不使余下部分失败。
使规划失败的影响限制在局部内。
记录规划执行步骤及理由，便于回溯。
Robot planning
7.1 机器人规划的作用和任务
2.机器人规划系统的任务与方法
（ 1）机器人规划系统的任务：
◆ 根据最有效的启发信息，选择应用于下一步的
最好规则。
◆ 应用所选取的规则来计算生成的新状态。
◆ 对所求得的解答进行检验。
◆ 检验空端，并舍弃。
◆ 检验殆正确的解答，并应用具体技术使之完全
正确。
Robot planning
7.1 机器人规划的作用和任务
2.机器人规划系统的任务与方法
（ 2）机器人规划系统的方法：
ⅰ 选择和应用规则：
方法一，
对每个动作都叙述起所引起的状态表示
的每一个变化。
另用某些语句来描述所有其它维持不变的
事物。
优点：只需一个机理 —— 消解。
缺点：若问题状态复杂，则需很多的公理条数。
Robot planning
7.1 机器人规划的作用和任务
2.机器人规划系统的任务与方法
（ 2）机器人规划系统的方法：
方法一例：
谓词集合描述状态：如图
动作影响到的状态：
用框架公理规则描述不受操作符影响的状态：
) ) ]),,((,())),,((,([
)],,(),([
00
00
SBAU N ST A C KDOBC L E A RSBAU N ST A C KDOAH O L D I N G
SBAONSAC L E A R
?
??
))),,((,(),( 00 SBAU N S T A C KDOBO N T A B L ESBO N T A B L E ?
))),,((,,()],(),,([ 00 SBAU N S T A C KDONMONAMNESNMON ??
Robot planning
7.1 机器人规划的作用和任务
2.机器人规划系统的任务与方法
（ 2）机器人规划系统的方法：
ⅰ 选择和应用规则：关于框架公理
使用初始表，删除表和添加表。
优点：
减少提供给操作符的信息。
操作后不必推演所有信息。
便于回溯。如图：
Robot planning
7.1 机器人规划的作用和任务
2.机器人规划系统的任务与方法
（ 2）机器人规划系统的方法：
ⅱ 检验解答与空端：
求得能将初始状态变换为目标状态的操作符序
列，即得解。
空端，从它无法到达目标的端点。
正向推理及反向推理。
Robot planning
7.1 机器人规划的作用和任务
2.机器人规划系统的任务与方法
（ 2）机器人规划系统的方法：
ⅲ 修正殆正确解的各种方法：
◆ 检查求得的状态，并把它与期望目标加以比较。
◆ 注意有关出错的知识，然后加以直接修正。
◆ 不修正，不完全确定地让差别保留到最后的可
能时刻。
Robot planning
7.2 积木世界的机器人规划
1.积木世界的机器人问题
（ 1）积木世界：几个有标记的一样大小的积木。
（ 2）机器人
动 作,unstack(a,b) stack(a,b)
pickup(a) putdown(a)
状态描述,ON(a,b) ONTABLE（ a）
CLEAR(a) HOLDING(a)
HANDEMPTY
Robot planning
7.2 积木世界的机器人规划
1.积木世界的机器人问题
（ 2）机器人 例：
初始状态,CLEAR（ B）
CLEAR（ C）
ON（ C,A）
ONTABLE（ A）
ONTABLE（ B）
HANDEMPTY
目标状态,ON（ B,C） ∧ ON（ A,B）
Robot planning
7.2 积木世界的机器人规划
2.用 F规则求解规划序列
（ 1）先决条件
（ 2）删除表
（ 3）添加表
例 move(x,y,z)
先决条件 CLEAR（ x）,CLEAR（ z）,ON（ x,y）
删除表 ON(x,y),CLEAR(z)
添加表 ON(x,z),CLEAR(y)
Robot planning
7.2 积木世界的机器人规划
2.用 F规则求解规划序列
表示图 4的所有状态空间
Robot planning
7.3 STRIPS规划系统
1.STRIPS系统的组成
（ 1）世界模型。
为一阶谓词演算公式。
（ 2）操作符（ F规则）。
包括先决条件，删除表，添加表。
（ 3）操作方法。
应用状态空间表示和中间 — 结局分析。
Robot planning
7.3 STRIPS规划系统
2.STRIPS系统规划过程
例 7.1,要求机器人到邻室去取回一个箱子。
Robot planning
7.3 STRIPS规划系统
提供两个操作符：
gothru(d,r1,r2)
pushthru(b,d,r1,r2)
这个问题的差别表：
Robot planning
7.3 STRIPS规划系统
初始状态 M0,INROOM（ ROBOT,R1）
INROOM（ VOX1,R2）
CONNECTS（ D1,R1,R2）
目标状态 G0,INROOM（ ROBOT,R1） ∧
INROOM（ BOX1,R1）
采用中间 — 结局分析方法来逐步求解
经 gothru(C1,R1,R2)
中间状态 M1,INROOM（ ROBOT,R2）
CONNECTS（ D1,R1,R2）
经 pushthru
中间状态 M2,M2=G0
Robot planning
7.3 STRIPS规划系统
例 7.1的搜索图及与或图
Robot planning
7.3 STRIPS规划系统
3.含有多重解答的规划
例 7.2,要求机器人将不同区域的三个箱子移到同
一区域内。
Robot planning
7.3 STRIPS规划系统
3.含有多重解答的规划
操作符：
push(k,m,n) goto(m,n)
初始模型 M0,ATR(a),AT(BOX1,b),
AT(BOX2,c),AT(BOX3,d)
目标模型 G0,(BOX1,x） ∧ （ BOX2,x） ∧
（ BOX3,x）
Robot planning
7.3 STRIPS规划系统
采用消解反演和中间 — 结局分析。
解 1,{goto(a,c),push(BOX2,c,b),goto(b,d),
push(BOX3,d,b)}
Robot planning
7.3 STRIPS规划系统
3.含有多重解答的规划
解 2,{goto(a,b),push(BOX1,b,c),goto(c,d),
push(BOX3,d,c)}
解 3,{goto(a,b),push(BOX1,b,d),goto(d,c),
push(BOX2,c,d)}
Robot planning
7.4 具有学习能力的规划系统
PULP— Ⅰ 系统
普渡大学,1976~1979,S.Tangwongsan &傅京孙
一种管理式（监督式）学习系统。
优点：
输入的目标语句表示为英语语句。
应用辅助物体改善了系统对物体的操作能力。
Robot planning
7.4 具有学习能力的规划系统
1.PULP— Ⅰ 系统的结构与操作方式
字典：英语词汇的集合。
模型：部分包括模型世界内
物体现有状态的事实。
过程：集中了预先准备好的
知识。
方块：集中了 LISP程序。
SENEF程序：用语义网络来
表示知识。
操作如图。
Robot planning
7.4 具有学习能力的规划系统
2.PULP— Ⅰ 的世界模型和规划结果。
一个具体任务下的世界模型。
Robot planning
7.4 具有学习能力的规划系统
2.PULP— Ⅰ 的世界模型和规划结果。
对 STRIPS,ABSTRIPS和
PULP— Ⅰ 三个系统的规划速
度进行比较。
Robot planning
7.5 基于专家系统的机器人规划
1.系统结构和规划机理
采用基于规则的专家系统。
（ 1）系统结构和规划机理 。 五部分
◆知识库。
◆控制策略。
◆推理机。
◆知识获取。
◆解释与说明。
一定的工具：支援软件和硬件。
Robot planning
7.5 基于专家系统的机器人规划
1.系统结构和规划机理
（ 2）任务级机器人规划三要素
◆建立模型。
对物体和机器人的几何，运动，物理描述。
◆任务说明。
说明各物体所期望的空间关系。
◆程序综合。
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
Robot Planning Expert Systems
(1)系统简化框图。
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(2)世界模型
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(3)规划与执行结果
共使用 15条规则。
规则 14：
表示机器人用小车把重型零件（ object）从
某工段 area Rb的位置 (Xb,Yb)推移 (push)到其目
的位置 (Xg,Yg)工段的位置所应具备的条件和必
须遵循的操作。
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(3)规划与执行结果
例，物体在第 1工段，运送至第 4工段第 8工作台。
应具备的条件,4条
◆已知零件位置。 at(Object,Rb,(Xb,Yb))
◆ 已知目标工段。 at4(Goal,Rg,(Xg,Yg))
◆ 零件两位置关系。
diff(Rb,R4),diff(R4,Rb),diff(Rg,R1),
diff(R1,Rb),diff(Rb,Rg),diff(R4,R1),
(connects(D2,R1,Rb)； connects(D1,Rb,R1)),
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(3)规划与执行结果
例，接上
(connects(D2,Rg,R1)； connects(D2,R1,Rg)),
(connects(D3,Rb,R4)； connects(D3,R4,Rb)),
(connects(D3,R4,Rg)； connects(D3,Rg,R4)),
(neighbors(Rb,R4)； neighbors(Rg,R4))
◆ 已知推零件过程中所经过的两个门道的位置。
at6(D1,Rb,(X2,Y2)),at6(D1,R1,(X3,Y3)),
at6(D2,R1,(X4,Y4)),at6(D2,Rg,(X5,Y5)),
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(3)规划与执行结果
动作序列
push((Rb,(Xb,Yb)),(Rb,(X2,Y2))),
pushthru(D1,(Rb,(X2,Y2)),(R1,(X3,Y3))),
push((R1,(X3,Y3)),(R1,(X4,Y4))),
pushthru(D2,(R1,(X4,Y4)),(Rg,(X5,Y5))),
push((Rg,(X5,Y5)),(Rg,(Xg,Yg))),
transfer(Object,cart1,Goal)
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(3)规划与执行结果
一个具体任务：
处在工段 3的机器人把工段 2的重型零件 part10搬
移至工段 5的工作台 5上。
用户输入数据：
[area3,a,b],
[area6,_,_],
[area2,0,2],
[area5,4.5,1],
[part10,table5].
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(3)规划与执行结果
一个具体任务：
作业任务的图解。
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(3)规划与执行结果
本系统最大可能解为 8个。
例：原处于工段 1的机器人，
使用工段 6的小车把工段 1内的一
个重型零件送至工段 6的某处。
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(4)比较
表 7.2比较 4个系统的复杂性
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(4)比较
比较 4个系统的规划速度。
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(4)比较
表 7.3仔细地比较了 PULP— 1和 ROPES的规划速度
Robot planning
7.5 基于专家系统的机器人规划
2.ROPES机器人规划系统。
(5)结论与讨论
◆更好的规划性能和更快的规划速度。
◆可输出某任务的所有解。
◆将概率，可信度，模糊理论运用于不确定任务。
◆ C-PROLOG语言简单，有效。
◆操作符数目增大时，规划时间增加得很少。
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 1）拆开顺序规划器的可行性准则
a,
几何可行性的定义：
任何支梁如果在下一个执行装配操作点，机器人
臂和末端装置可达的，则称为支梁几何可行性。
凸形外壳的定义：
如果一组顶点处在所建立的三维容积空间的端点
上，那么这组顶点就是顶点集合的凸形外壳。
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 1）拆开顺序规划器的可行性准则
几何可行性的一般定义：
3D（ N,S）,CH（ N）
N,结点数 S,支梁数
3D：结点和支梁组成的三维空间
CH：结点的凸形外壳
ⅰ 检验所有处于凸形外壳上的支梁，并放入活动
表 (active list)A-。
ⅱ,具有,
ⅲ 结束。
Robot planning
SSi ?? ),( 1NNi ))()(( 1 ik SANCHNNCHNif ?
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 1）拆开顺序规划器的可行性准则
b,
结构可行性的定义：
如果移动一个支梁导致最大稳定和刚性结构，则
该支梁具有满足结构可行性条件的特性。
三维空间结构的两个层次：
◆支梁和结点相互连接，形成完整的上层结构。
◆形成互连刚性子结构的支梁和结点集合，建立完
整结构。
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 1）拆开顺序规划器的可行性准则
C，连接关系，稳定性和可行性准则
结构容积单元（ SVE）,Structural Volume Element
是支梁和结点的集合，它们以规定的方法互连，
形成结构上刚性和稳定的状态。
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 1）拆开顺序规划器的可行性准则
非结构容积单元（ NSVE）：
是不形成结构单元的支梁和结点的任务集合。
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 1）拆开顺序规划器的可行性准则
SVE和 NSEV结构提供了空间构件装配的分层表示。
SVE的 3种互连关系：
◆点连接 0（ 1）阶
◆线连接 0（ 2）阶
◆面连接
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 2）规划规则的表示与搜索
Ⅰ 状态和规划表示
分层表示。
底层部分包括支梁和结点。
高层部分包括 SVE和 NSEV。
由上述两部分组成的关系图表示三维装配约束。
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 2）规划规则的表示与搜索
Ⅱ 分层搜索策略
搜索空间限制在可能导致可行解的范围内。
分层方法由两层拆装组成。
第一层应用 SVE约束决定 SVE单元的顺序。
SVE被定序后，就开始第二层拆装规划。
由保证子目标最优解，从而保证全局最优。
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 2）规划规则的表示与搜索
第一层 SVE级算法：
◆开始。
◆ SVE 确定 3D空间的所有 SVE单元。
◆ SVEvalid 确定所有有效的 SVE单元。
◆ SVEminface 确定含有最少面数的 SVE单元。
◆如果 SVEminface=1 SVEnext SVEminface,否则
SVEease 确定从 SVEminface拆装的最容易 SVE单元。
◆ SVEnext SVEease
◆ 3D REMOVE(SVEease)
◆ DISASSEMBLEsve(3D’)
?
?
?
?
?
?
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 2）规划规则的表示与搜索
第一层 SVE级 确定顺序之准则：
◆移动应当影响最少的面连接数。
◆移动必须是最容易的。
中间不稳定态尽可能少。
◆移动必须是有效的。
前后两个 SVE必须相连。
Robot planning
7.6 太空构件装配顺序分层规划系统
1.太空构件装配及其分层顺序规划
（ 2）规划规则的表示与搜索
第二层 支梁级,
选择具有最小总代价的有效支梁。
确定几何可行的支梁。
运用启发代价函数。
例：图 7.26
四面体 {6,5,4,3,2,1}
五面体 {7,3,2,1,6,4,8,5}
Robot planning
7.6 太空构件装配顺序分层规划系统
2.三维结构装配顺序规划示例
例 7.3 3D(10,24),7个 SVE单元,3个五面体。
步骤：
13，（ 7,19,18）
（ 24,12,17），（ 11,23,16）
（ 22,10,15），（ 20,8,14,9）
Robot planning
7.6 太空构件装配顺序分层规划系统
2.三维结构装配顺序规划示例
例 7.4 3D（ 31,102）
Robot planning
7.7 机器人轨迹规划
轨 迹,机械手的位移，速度和加速度。
轨迹规划,由任务要求，计算出预算的运动轨迹。
轨迹规划器：简化了编程手续。
1.轨迹规划应考虑的问题
◆机器人规划方式的分类
Robot planning
7.7 机器人轨迹规划
1.轨迹规划应考虑的问题
◆机械手常用的两种轨迹规划方法：
方法一，给出插值点上一组显式约束。
方法二，给出运动路径的解析式。
◆轨迹规划可在关节空间或直角空间中。
◆规划器的任务：解变换方程，运动学反解和插值
运算。
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 1）三次多项式插值
四个约束条件：
由上确定了一个三次多项式：
)1.7()( )0( 0
??
?
?
?
fft ??
??
)2.7(
0)(
0)0(
??
???
?
?
?
?
ft?
?
)3.7()( 332210 tatataat ?????
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 1）三次多项式插值
关节速度和加速度：
关于四个系数的线性方程：
)4.7(
52)(
32)(
32
2
321
??
???
??
???
??
?
taat
tataat
?
?
)5.7(
320
0
2
321
1
3
3
2
210
00
?
?
?
?
?
?
?
???
?
????
?
ff
ffff
tataa
a
tatataa
a
?
?
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 1）三次多项式插值
解得四个系数的表达式：
)6.7(
)(
2
)(
3
0
033
022
1
00
?
?
?
?
?
?
?
?
?
???
??
?
?
??
??
?
f
f
f
f
t
a
t
a
a
a
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 2）过路径点的三次多项式插值
将速度约束条件 (7.2)变为：
重新求得三项式的系数：
)7.7(
)(
)0( 0
??
???
?
?
??
??
fft ??
??
)9.7(
)(
1
)(
2
12
)(
3
00
33
00
22
01
00
?
?
?
?
?
?
?
?
?
?????
????
?
?
??
??
?
f
f
f
f
f
ff
f
f
tt
a
ttt
a
a
a
????
????
?
?
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 2）过路径点的三次多项式插值
确定路径点上关节速度的三种方法：
◆根据工具坐标在直角坐标空间中的瞬时线速度、
和角速度来确定。
◆采用适当的启发式方法，有控制系统自动地选
择。
◆要保证每个路径点上的加速度连续。
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 3）高阶多项式插值
五次多项式：
6个约束条件：
)10.7()( 5544332210 tatatatataat ???????
)11.7(
201262
2
5432
3
5
2
432
2
0
4
5
3
4
2
321
1
0
5
5
4
4
3
3
2
210
00
?
?
?
?
?
?
?
?
?
?
?
????
?
?????
?
??????
?
??
??
?
?
fff
f
ffff
f
ffffff
tatataa
a
tatatataa
a
tatatatataa
a
?
?
?
?
?
?
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 3）高阶多项式插值
其解为：
)12.7(
2
)()66(1212
2
)23()1614(3030
2
)3()128(2020
2
3
2
00
0
3
3
2
00
0
4
3
2
00
0
3
0
2
0
1
00
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?????
?
?????
?
?????
?
?
?
?
??????
??????
??????
??
?
f
f
f
fff
f
f
f
fff
f
f
f
fff
t
tt
a
t
tt
a
t
tt
a
a
a
a
??????
??????
??????
?
?
?
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 4）用抛物线过渡的线性插值
将线性函数与两段抛物线函数平滑地衔接在一
起形成一段轨迹。
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 4）用抛物线过渡的线性插值
,tb处的速度
,tb处的关节角度
：过渡域内加速度
令 t=2th,由式 13,14得
t：所要求的运动持续时间
)14.7(
2
1
)13.7(
2
0 bb
bh
bh
tb
t
tt
?
?
??
?
?
?
???
??
?
)15.7(0)( 02 ???? ???? ???? fbb ttt
b?
???
tb??
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 4）用抛物线过渡的线性插值
任意给定, 和,选择相应的 和,得到
路径曲线。
需满足条件：
)16.7(
2
)(4
2
0
22
??
????
??
??
?
???? f
b
tt
t
)17.7()(4 2 0tf ??? ???
???
btf? 0? t
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 5）过路径点的用抛物线过渡的线性插值
j,k,l：连续的三个路径点。
,k点过渡域的持续时间。
,j,k之间线性域的持续
时间。
：连接 j和 k的路径段的全
部持续时间。
,j,k点之间线性域的速度。
,j点过渡域的加速度。
kt
jkt
djkt
jk
??
j
???
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 5）过路径点的用抛物线过渡的线性插值
任意给定, 和,可得 。
对于内部路径点,
k? ktdjkt
)18.7(
2
1
2
1
)s g n (
?
?
?
?
?
?
?
?
?
?
?
???
?
?
??
?
?
??????
?
kjd jkjk
k
jkkl
k
k
jk
kl
k
d jk
jk
jk
tttt
t
t
?
??
????
??
?
??
k?
)1,;2,1,( ??? nkjkj
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 5）过路径点的用抛物线过渡的线性插值
对于第一个路径段：
从而求得,进而求得 和,
)19.7(
2
1 11
112
12 t
tt d
???
?
? ???
)20.7(
2
1
2
1
)(2
)s g n (
211212
112
12
12
1
122
12121
1121
?
?
?
?
?
?
?
?
?
?
?
?
?
???
?
?
?
?
???
??
?
??
??????
tttt
tt
ttt
d
d
dd
??
?
?
??
????
1t 12?? 12t
Robot planning
7.7 机器人轨迹规划
2.关节轨迹的插值计算
（ 5）过路径点的用抛物线过渡的线性插值
对于最后一个路径段：
根据上式便可求得：
)21.7(
2
1
)1(
1
nn
nnnd
n t
tt
n ??
?
? ?
?
? ???
)22.7(
2
1
2
1
)(2
)s g n (
1)1()1(
)1(
1
)1(
1
12
)1()1(
1
?
?
?
?
?
?
?
?
?
?
?
?
?
???
?
?
?
?
???
??
???
?
?
?
?
??
?
??
???
?
???
nnnndnn
nnnd
nn
nn
n
nn
nndnndn
nnnn
tttt
tt
ttt
??
?
?
??
????
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 1）物体对象的描述
在给出物体的几何图形及固接坐标后，其相对
于参考系的位姿则可用与它固接的坐标系来表示。
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 2）作业的描述
作业的运动可用手部位
姿结点序列来规定。
每个结点是由工具坐标
系相对于作业坐标系的齐次
变换来描述。
相应的关节变量可用运
动学反解计算。
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
如图 7.37所示，从 到 的运动可表示为，从
到
：工具坐标系 {T}相对于末端连杆系 {6}的变
换。
：分别为两个结点 和 相对于坐标系 {B}
的齐次坐标。
)23.7(160)(60 ?? EiBBi TPTT
1?iPiP
)24.7(1610)1(60 ??? ? EiBBi TPTT
iP 1?iP
ET6
1?iBiB PP 和
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
从 到 的运动可由, 驱动变换, D(λ )来表
示：
D(λ ),归一化时间 λ 的函数。
t,自运动开始算起的时间。
T,走过该轨迹的总时间。
)26.7()()( 16060 ?? EiBB TDPTT ??
]10[,,?? ?? Tt
iP 1?iP
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
在, t=0,则 λ =0,D(0)是 4× 4的单位矩阵，
则式 26与式 23相同。
在, t=T,则 λ =1，有
得
1)1( ?? iBiB PDP
iP
1?iP
)27.7()1( 11 ??? iBiB PPD
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
1000
1000 iziziziz
iyiyiyiy
ixixixix
iiii
i
B
paon
paon
paon
paon
P
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
????
????
????
????
?
1000
1000 1111
1111
1111
1111
1
zizizizi
yiyiyiyi
xixixixi
iiii
i
B
paon
paon
paon
paon
P
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
?
?
?
?
?
?
?
?
?
?
?
?
?????
?????
?????
?
????
????
????
1000
)(
)(
)(
)1(
1111
1111
1111
iiiiiiiii
iiiiiiiii
iiiiiiiii
ppaaaoana
ppoaooono
ppnanonnn
D
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
到 的运动分解为一个平移运动和两个旋转
运动。
L(λ ),的坐标原点沿直线运动到 的原点。
Ra(λ ),的接近矢量 转向 的接近矢量 。绕 a
Ro(λ ),的方向矢量 转向 的方向矢量 。绕 o
1?iP
)28.7()()()()( 0 ???? RRLD a?
iP
iP
iP
iP
1?iP
1?iP
1?iP
1?ia
1?io
ia
io
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
表示由绕 k转动 θ 角得到,k是 的 y轴绕
其 z轴转过 ψ 角得到，即
)29.7(
1000
100
010
001
)(
?
?
?
?
?
?
?
?
?
?
?
?
?
z
y
x
L
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
1
0
1
0
1000
0100
00
00
1
0
??
??
?
?
cs
sc
c
s
k
)(?aR iP
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
由通用旋转变换公式，得
)31.7(
1000
0100
00)()(
00)()(
)(0
?
?
?
?
?
?
?
?
?
?
?
? ?
?
????
????
?
cs
sc
R
)30.7(
1000
0)()()(
0)()()()(
0)()()()(
)(
2
2
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
??
??
?
????????
????????????
????????????
?
csssc
sscvcvcs
ccvcscvs
R a
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
将式（ 7,29）和 (7.31)带入 (7.28)中，得
)32.7(1000)( ?
?
?
??
?? dpdadodnD ?
?
?
?
?
?
?
?
?
?
?
????
????
???
?
)]()[()]()[(
)]()()[()]()[(
)]()[()]()[(
2
2
??????????
?????????????
???????????
sscscs
cvccvcss
vcscvss
do
dadodn
z
y
x
dp
c
ss
sc
da ??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
???
???
)(
)(
)(
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 3）两个结点之间的, 直线, 运动
解得：
)33.7(
)(
)(
)(
1
1
1
??
?
?
?
???
???
???
?
?
?
iii
iii
iii
ppaz
ppoy
ppnx
)35.7(]
])()[(
t a n [
)34.7(]t a n [
1
212
1
2
1
1
1
????
????
???
?
???
?
???
?
?
?
?
??
?
?
ii
iiii
ii
ii
aa
aoan
a
an
ao
a
)36.7(]t a n [ ?????? ???? csa
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 4）两段路径之间的过渡
手部达到结点前的时刻 τ 开始改变速度，保持
加速度不变，直到到达结点之后 τ 为止。
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 4）两段路径之间的过渡
加速度值：
其中
)37.7(][2 1)( 2 ???? ????????? tBTCtx
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
??
??
??
??
BA
BA
BA
BA
BC
BC
BC
BC
y
x
B
y
x
C
y
x
tx
?
?
?
?
?
?
)(
Robot planning
7.7 机器人轨迹规划
3.笛卡儿路径轨迹规划
（ 4）两段路径之间的过渡
由 7.37得在区间 -τ <t<τ 中的速度和位移
△ C和 △ B：分别为 B到 C和 B到 A的直角坐标距离和角度。
T,机械手从 B到 C所需时间。
)39.7(]2)[()(
)38.7(][
1
)(
BBB
T
Ctx
B
B
T
Ctx
????????
?
?????
?
??
?
?
?
?
?
Robot planning
7.7 机器人轨迹规划
4.规划轨迹的实时生成
（ 1）关节空间轨迹的生成
对于三次样条，随时间变化不断按式 7.3和 7.4。
到达路径段终点时，调用新路径段三次样条系数。
对于带抛物线过渡的直线样条插值。处于线性
域：
)41.7(
0 ?
?
?
?
?
?
?
?
?
??
??
??
?
?
??
???
jk
jkj t
Robot planning
7.7 机器人轨迹规划
4.规划轨迹的实时生成
（ 1）关节空间轨迹的生成
当处于过渡域时：
令 则
)42.7(
2
1
)(
2
?
?
?
?
??
?
?
?
?
??
????
????
????
???
k
i nbkjk
i nbki nbjkj
t
ttt
??
???
????
)21( jkjin b tttt ???
Robot planning
7.7 机器人轨迹规划
4.规划轨迹的实时生成
（ 2）笛卡儿空间轨迹的生成
带有抛物线过渡的线性规划轨迹，在线性域：
t,从第 j个路径点算起的时间。
：由类似 (7.18)的方程求出。
)44.7(
0 ?
?
?
?
?
?
?
?
?
??
??
??
?
x
xx
txxx
jk
jkj
jkx
?
Robot planning
7.7 机器人轨迹规划
4.规划轨迹的实时生成
（ 2）笛卡儿空间轨迹的生成
带有抛物线过渡的线性规划轨迹，在过渡域：
)45.7(2
1
)(
)
2
1
(
2
?
?
?
?
?
?
?
?
?
?
??
????
???
????
???
???
k
i n b
kjk
i n b
k
i n b
jk
j
jkji n b
xx
txxx
txttxxx
tttt
