机械臂运动路径设计分析

机械臂运动路径设计分析
摘要
本文探讨了六自由度机械臂从一点到另一点沿任意轨迹移动路径、一点到另一点沿着给定轨迹移动路径、以及无碰撞路径规划问题，并讨论了设计参数对机械臂灵活性和使用范围的影响，同时给出了建议。

问题一：（1）首先确定初始坐标均为零时机械臂姿态，建立多级坐标系，利用空间解析几何的变换基本原理及相对坐标系的齐次坐标变换的矩阵解析方法，来建立机器人的运动系统的多级变换方程。

通过逆运动学解法和构建规划，来求优化指令
（2）假定机械臂初始姿态为Φ0，曲线离散化，每个离散点作为末端位置，通过得到的相邻两点的姿态，利用（1）中算法计算所有相邻两点间的增量指令，将满足精度要求的指令序列记录下来。

（3）通过将障碍物理想化为球体，将躲避问题就转化成保证机械手臂上的点与障碍球球心距离始终大于r的问题。

进而通过迭代法和指令检验法，剔除不符合要求的指令，从而实现避障的目的
问题二：将问题二中的实例应用到问题一中的相对应的算法中，部分结果见附录
问题三：灵活性与适用范围相互制约，只能根据权重求得较优连杆长度。

关键词：多级坐标变换逆运动学解法优化指令检验
一、问题重述
1.1 问题背景
某型号机器人（图示和简化图略）一共有6个自由度，分别由六个旋转轴（关节）实现，使机器人的末端可以灵活地在三维空间中运动。

机器人关于六个自由度的每一个组合),,,,,(654321θθθθθθ=Θ，表示机械臂的一个姿态，显然每个姿态确定顶端指尖的空间位置X ：X f →Θ)(。

假定机器人控制系统只能够接收改变各个关节的姿态的关于连杆角度的增量指令（机器指令）),,,,,(654321θθθθθθ∆∆∆∆∆∆=∆Θ=P ，使得指尖（指尖具有夹工具、焊接、拧螺丝等多种功能，不过在这里不要求考虑这方面的控制细节）移动到空间点X′，其中各个增量i θ∆只能取到-2, -1.9, -1.8, ┅，1.8, 1.9, 2这41个离散值（即精度为0.1°，绝对值不超过2°）。

通过一系列的指令序列n P P P P ,,,,321 可以将指尖依次到达位置X 0，X 1，┅，X n ，则称X 0，X 1，┅，X n 为从指尖初始位置X 0到达目标位置X n 的一条路径（运动轨迹）。

根据具体的目标和约束条件计算出合理、便捷、有效的指令序列是机器人控制中的一个重要问题。

假设机器人的初始位置在y-z 平面上，约定直角坐标系的原点设在图示的A 点，z 轴取为AB 方向，
x 轴垂直纸面而y 轴则在基座所固定的水平台面上 1.2 目标任务
问题一：设计一个通用的算法，用来计算执行下面指定动作所要求的指令序列，并要求对算法的适用范围、计算效率以及近似算法所造成的误差和增量i θ∆离散取值所造成的误差大小进行讨论（不考虑其他原因造成的误差）：
（1）已知初始姿态Φ0和一个可达目标点的空间位置（O x , O y , O z ），计算指尖到达目标点的指令序列。

（2）要求指尖沿着预先指定的一条空间曲线x = x (s ), y = y (s ), z = z (s ), a ≦ s ≦b 移动，计算满足要求的指令序列。

（3）在第①个问题中，假设在初始位置与目标位置之间的区域中有若干个已知大小、形状、方向和位置的障碍物，要求机械臂在运动中始终不能与障碍物相碰，否则会损坏机器。

这个问题称机械臂避碰问题，要求机械臂末端在误差范围内到达目标点并且整个机械臂不碰到障碍物（机械臂连杆的粗细自己设定）。

问题二：应用你的算法就下面具体的数据给出计算结果，并将计算结果以三组六维的
指令序列（每行6个数据）形式存放在Excel 文件里，文件名定为answer1.xls ，answer2.xls 和answer3.xls 。

假设在机械臂的旁边有一个待加工的中空圆台形工件，上部开口。

工件高180mm ，下底外半径168mm ，上底外半径96mm ，壁厚8mm 。

竖立地固定在x －y 平面的操作台上，底部的中心在 (210, 0, 0)。

①．要求机械臂（指尖）从初始位置移动到工具箱所在位置的 (20,－200, 120) 处，以夹取要用的工具。

②．如果圆台形工件外表面与平面x =2z 的交线是一条裂纹需要焊接，请你给出机械臂指尖绕这条曲线一周的指令序列。

③．有一项任务是在工件内壁点焊四个小零件，它们在内表面上的位置到x-y 平面的投影为（320,-104）、（120,106）、（190,-125）和（255,88）。

要求机械臂从圆台的上部开口处伸进去到达这些点进行加工，为简捷起见，不妨不计焊条等的长度，只考虑指尖的轨迹
问题三：制造厂家希望通过修改各条连杆的相对长度以及各关节最大旋转角度等设计参数提高机械臂的灵活性和适用范围。

请根据你们的计算模型给他们提供合理的建议。

二、问题分析
机械臂运动路径设计问题主要涉及到相对坐标系坐标变换、机器人正运动学分析、逆运动学求解、优化以及机器人避碰问题。

1．运动方程的建立
从机构学观点来看，机器人属于空间机构范畴。

杆件每次转动因此，采用空间解析几何的变换基本原理及坐标变换的矩阵解析方法，来建立机器人的运动系统的多级变换方程。

由于旋转轴涉及到平行连杆和垂直连杆两类，因此对于各类旋转变换，所使用的变换矩阵也不相同。

同时，此题中涉及的机器人有6个自由度，则从工件的坐标位置到固定坐标系的变换要经过多级坐标变换。

采用多级坐标变换的方法。

由上述三点，便可建立机器人运动系统的坐标变换关系式。

2．尖端轨迹曲线模型的建立
对于已给定一条空间曲线x=(s),y=(y),z=(z),可将其看成一个点的集合。

因此机械臂实现一个空间轨迹的过程是实现轨迹离散的过程。

如果这些离散点间距很大，机械臂轨迹与要求的轨迹就有较大误差。

只有这些离散点彼此很接近，才有可能使机械臂的轨迹以满足要求精确度逼近要求的轨迹。

连续轨迹的控制实际上就是在多次执行离散点间的点位控制，离散点点越密集越能逼近要求的运动曲线
3．避碰问题
假设空间障碍物为半径为r 的球体，则这些球体空间便形成了机械手臂的约束。

而躲避问题就转化成保证机械手臂上的点与障碍球球心距离始终大于r 的问题。

根据运动规律，不难知道手臂相对于初始位置的姿态决定于之前执行的所有指令的和。

已知连杆上的点L 在其所在的相对坐标系中的坐标()'
,,x y z 及转动的指令，根据齐次坐
标变换矩阵就可得到L 在固定坐标系中的坐标(),,x y z ，然后可计算距离。

所以 可以用问题1-1算法产生点到点的指令，可以利用迭代法从初始位置开始提前检验每个指令，不满足要求的无法执行。

4.自由度分析：
由题中指出的：指尖———E 点，具有夹工具、焊接、拧螺丝等多种功能，不过在这里不要求考虑这方面的控制细节。

则将情况理想化，不考虑第6个自由度对运动的影响。

三、模型的假设
1.各关节连杆在输入指令后同时开始转动，速度为低速，各杆件之间无摩擦， 臂各旋转轴最大运动速度相同
2.在轨迹规划中不考虑机械臂关节转角的限制。

3.不考虑机械臂结构和装配产生的误差。

4.连杆为刚体，不会发生形变
5.设机器人的初始位置是在一个平面上的（y-z 平面）。

6.关节视为质点，它们所占的面积可忽略不计。

四、符号说明
i U ：坐标系，o U 为固定坐标系，其余为固定在杆上的相对坐标系
1i i
T -：i U 到1i U -的坐标系变换矩阵
'
x
:尖端移动终点在固定坐标系中的x 方向坐标
'y ：尖端移动终点在固定坐标系中的y 方向坐标
'z ：尖端移动终点在固定坐标系中的z 方向坐标
0x ：尖端转动起点在固定坐标系中初始x 方向坐标
0y ：尖端转动起点在固定坐标系中初始y 方向坐标
0z ：尖端转动起点在固定坐标系中初始z 方向坐标
i θ------第i 个自由度的相对于初始姿态转角(i=1,2,3,4,5,6)
i θ∆-----第i 个自由度转角的增量(i=1,2,3,4,5,6)
si
------sin i θ
ci ------cos i θ
五、模型建立与求解
5.1运动学模型建立与求解
5.1.1机器人运动方程的建立
为了产生点到点的运动序列, 首先我们先作出初始角度均为0时的姿态，以
每个节点为原点建立坐标系0U 到6U 如图5.1.1。

图-5.1.1
从图-5.1.2可以看出F 点相当于将A U 绕Z 轴旋转得到01T ，同理其他节点分别相当于将前一坐标系绕X,X,Z,X,Z 轴转动。

可得到各坐标系相对于前一个的坐标系的坐标变换公式。

从而得到机械臂各个关节的变换矩阵
旋转矩阵：11
121321222331
32
33A
B
r r r R r r r r r r ⎡⎤
⎢⎥=⎢⎥⎢⎥⎣⎦
位置矢量：
x A
y z p p p p ⎡⎤⎢⎥=⎢⎥
⎢⎥⎣⎦
1
1A A
i B
i
R P T -⎡⎤=⎢
⎥⎣⎦
图-5.1.2
：机器人连杆参数（见表格1）
11110
1
cos()sin()00sin()
cos()000011400001T θθθθ-⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦22122210
00sin()cos()00cos()sin()000
01T θθθθ⎡⎤
⎢⎥⎢
⎥=⎢⎥
-⎢
⎥
⎣⎦
33233310000cos()sin()000sin()0cos()12550001T θθθθ⎡⎤⎢⎥-⎢⎥=⎢⎥⎢⎥⎣⎦444434
cos()sin()
00
sin()0cos()
000012550001T θθθθ-⎡⎤⎢⎥⎢⎥=⎢⎥
⎢⎥⎣⎦
55455510000sin()cos()00cos()sin()00001T θθθθ⎡⎤⎢⎥⎢⎥=⎢⎥-⎢⎥⎣⎦666645cos()sin()0
00sin()0cos()0000100001T θθθθ-⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦
：
06112233445566()()()()()()000
1x x x x y
y y
y z z z z n o a p n o a
p T A A A A A A n o a p θθθθθθ⎡⎤⎢⎥⎢⎥==⎢⎥⎢
⎥⎣⎦
其中
12345612346145614612356x n C C C C C C C S S S S C C S C S C S S S =-++-12345612346145614612356
y n S C C C C S C S S C S C C C C S S S S S =----2345623462356
z n S C C C S S S C S S =-+12345612346145614612356x o C C C C S C C S C S S C S S C C C S S S =---++12345612346145614612356
y o S C C C S S C S S C S C S C C C S S S S =--+-+2345623462356z o S C C S S S C C S S =---
123451451235x a C C C S S S S C S C =++ 123451451235
y a S C C S C S S S S C =-+
2345235
z a S C S C C =-
12345145123512312656565255255x p C C C S S S S C S C C S C C =++++2345235232
6565255255z p S C S C C C S =--+12345145123512312
656565255255y p S C C S C S S S S C S S S C =-+++
2．确定固定坐标系中E 点位置
由题设中，已知固定坐标系原点，根据给定的连杆长度和i θ角度，易计算得出，在固定坐标系中E 位置：
E x =-65(-cos(s1)sin(s4)+(-sin(s1)cos(s2)cos(s3)+sin(s1)sin(s2)sin(s3))cos(s 4))sin(s5)+65(sin(s1)cos(s2)sin(s3)+sin(s1)sin(s2)cos(s3))cos(s5)+255s in(s1)cos(s2)sin(s3)+255sin(s1)sin(s2)cos(s3)+255sin(s1)sin(s2)
E y=-65(-sin(s1)sin(s4)+(cos(s1)cos(s2)cos(s3)-cos(s1)sin(s2)sin(s3))cos(s4
))sin(s5)+65(-cos(s1)cos(s2)sin(s3)-cos(s1)sin(s2)cos(s3))cos(s5)-255cos(s1)cos(s2)*sin(s3)-255cos(s1)sin(s2)cos(s3)-255cos(s1)sin(s2)
E z=140-65(sin(s2)cos(s3)+cos(s2)sin(s3))cos(s4)sin(s5)+65(-sin(s2)sin(s3)+cos(s2)cos(s3))cos(s5)-255sin(s2)sin(s3)+255cos(s2)cos(s3)+255cos (s2)
这样便得到了运动学方程。

5.1.2 利用逆运动学方法求解 （1）求1θ
可用逆变换111()A θ-左乘运动方程式两边得：
101162233445566()()()()()()
A T A A A A A θθθθθθ-=
根据对性元素相等可解的 解得:
116565arctan
arctan
6565y y y y x x
x x
p a p a p a p a θθπ--==+--或
（2）求3θ
由：
111
332211445566()()()()()0
00
1x x x x y
y y y z z z z n o a p n
o a p A A A A A A n o a p θθθθθθ---⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦
（）
解得:
3311arcsin arctan ;arcsin arctan 6
6
a a θθπ=+=-+
（3）求2θ
23112345()x y z c c a s a s a c s ++= （1）
231123345()(255)65x y z c c p s p s p c c s ++-= （2）
解得：
232323sin()arcsin ;arcsin w B B w B w θθθπ+=⇒=-=--或
其中
1111365()tan 65;(255)x y x y z z c p s p c a s a w p a B c +-+⎧
=⎪
-⎨
⎪=⎩
2233323arcsin ;arcsin B w B w θθθθθπθ=-=--=---或
（4）求5θ
23112355231123()arccos[()]x y z x y z s c a s a c a c s c a s a c a θ+-=⇒=+-
或52311232arccos[()]x y z s c a s a c a θπ=-+- （5）求4θ
由114x y s a c a S S -=，可以解得：
1111445
5
arcsin
;x y
x y
s a c a s a c a s s s θ--=
⇒=或1145
arcsin
x y
s a c a s θπ-=-
（6）求6θ
2311232311235665
()()sin sin x y z
x y z s c o s o c o s c o s o c o s s θθ+-+-=⇒=
23112365
()arcsin
sin x y z
s c o s o c o θθ+-=；或23112365
()arcsin
sin x y z
s c o s o c o θπθ+-=-
5.2问题1—1的模型
在已有六自由度手臂运动方程和逆运动学解法的基础上，若已知机械臂末端转动终点坐标和转动起点坐标已知，就可以得到各关节的坐标，连杆的相对于初始状态的角度变化123456(,,,,,)θθθθθθ∆Φ=及机械臂的姿态。

5.2.1机械臂最佳姿态的确定
对于一般六自由度机械臂，带入末端坐标（x,y,z ）会得到16组
123456(,,,,,)θθθθθθ∆Φ=，考虑用机械臂指尖实际到达的空间位置（'x ,'
y ,'z
）到理想位
置（x,y,z ）的偏差距离与移动距离的比值最少来衡量机械臂是否是最佳姿态，所以
我们通过定义一个参量－偏移系数来寻找最佳指令， 其数学模型可表示为：
Min M ⎧⎪=⎨⎪⎩ '
x :尖端移动终点在固定坐标系中的x 方向坐标
'y ：尖端移动终点在固定坐标系中的y 方向坐标
'
z
：尖端移动终点在固定坐标系中的z 方向坐标
0x ：尖端转动起点在固定坐标系中初始x 方向坐标
0y ：尖端转动起点在固定坐标系中初始y 方向坐标
0z ：尖端转动起点在固定坐标系中初始z 方向坐标
当尖端移动终点坐标（x ，y ，z ）已知情况下，利用MATLAB Robotics Tool 可得到T 矩阵，利用逆运动学解法，可得到123456(,,,,,)θθθθθθ，因指令中各个增量i θ∆只能取到-2, -1.9, -1.8, ┅，1.8, 1.9, 2这41个离散值（即精度为0.1°，绝对值不超过2°），所以最终转动的123456,,,,,θθθθθθ均只保留小数点后一位。

所以对得到的
12
3
4
5
6
(,,,,,)θθθθθθ取位处理，然后回代入正方向运动学方程，解得的（'x ,'
y ,'z ）才为
实际位置。

通过对M 的比较得到最佳12356*4(,,,,,)θθθθθθ。

5.2.2 指令生成
在已知i θ（i=1,2,3,4,5，6）后，只需要设计一种方式使机械臂从1θ转到2θ。

这里根据点位机械臂运动特点选用加速——匀速——减速三段式控制方式。

如图5－2中的方式二。

规定一个加速时间T(T<L/2),当t=T,机器人以t v 匀速运动，最后在目标点前T 时刻开始减速。

这种实时控制计算量小，易于控制。

在我们的程序中不防假设前10个指令加速，后10个指令减速，其他匀速运动。

程序见附录2。

（假设变速过程极快）
5.3问题1－2的模型——机械臂连续轨迹的控制问题曲线
对于已给定一条空间曲线x=(s),y=(y),z=(z),可将其看成一个点的集合。

因此机械臂实现一个空间轨迹的过程是实现轨迹离散的过程。

如果这些离散点间距很大，机械臂轨迹与要求的轨迹就有较大误差。

只有这些离散点彼此很接近，才有可能使机械臂的轨迹以满足要求精确度逼近要求的轨迹。

连续轨迹的控制实际上就是在多次执行离散点间的点位控制，离散点点越密集越能逼近要求的运动曲线。

对于问题1－2预先指定的曲线我们先要把它离散化，把得到点i a 的坐标()
,,i
i i x y z 带入逆运动方程中，然后根据一定的路径约束和实际情况的约束找出通过点i a 时机械
臂的姿态，然后1i a -的姿态[]161,
,i θθ-作为初始位置问题1－1的算法中得到1i a -移动到
i a 的最佳指令1236*45(,,,,,)i θθθθθθ
由于问题1—1算法中假定各关节在一次转动中产生等量指令，且各关节同时转动所以机械臂尖端在1i a -，i a 之间运动路径基本相同，再加之取点较多，所以此处对于1i a -，i a 之间，尖端的路径曲线不予以考虑
因此曲线运动的最佳指令为
c 12356*
4(,,,,,)θθθθθθ= 1
n
i =∑1236*45(,,,,,)i θθθθθθ
5.4问题1－3的模型——机械手臂避障问题（指令检验法）
假设空间障碍物为半径为r 的球体，则这些球体空间便形成了机械手臂的约束。

而躲避问题就转化成保证机械手臂上的点与障碍球球心距离始终大于r 的问题。

根据运动规律，不难知道手臂相对于初始位置的姿态决定于之前执行的所有指令的和。

已知连杆上的点L 在其所在的相对坐标系中的坐标()'
,,x y z 及转动的指令，根据齐次坐标变换矩阵就可得到L 在固定坐标系中的坐标(),,x y z ，然后可计算距离。

所以 可以用问题1-1算法产生点到点的指令，可以利用迭代法从初始位置开始提前检验每个指令，不满足要求的无法执行。

根据这些要求建立执行检验模型如下：
{}
000100200
3004005006222
02220222
min 180180125125138138270270133.5120270270()()()0()()()100()()()x x y y z z
x x y x z x x x y x z x obj st obj l o l o l o l o l o l o r obj l o l o l o r θθθθθθ⎧⎪
⎪-<≤⎪-<≤⎪⎪-<≤⎪
⎪-<≤⎪-<≤⎨-<≤=-+-+-⎧-+-+->⎪=⎨
-+-+-≤⎪⎩⎪
⎪⎪
⎪⎪⎪⎪⎪⎪⎪⎩
…………（6－15）
此模型的目标函数是一个符合的函数，目标函数0obj 表征机械臂上的点与目标点的距离，若与内壁的距离小于r ，1obj 就会突然变大，从而不满足目标函数。

六、模型的验证
6.1 问题2-1的求解
根据问题1－1的模型算法，计算得出以连杆DE 从空间起始点0Φ指向工具箱位
置（20，-200，120）的姿态，通过机械臂逆运动学计算得出它到这点的角度为（-84.30,61.00,-43.30,0,-17.60）。

把得到的角度代入指令生成算法中得到：（0，0，-1.6，0，-1,6，0）；（-0.1，-0.1，-1.6，0，-1,6，0）；（-0.2，0.1，-1.6，0，-1,6，0）等，详见answer1.xls。

MATLAB Robotic仿真如下（程序见附件2）。

表6－1 关节角度误差
6.2.2对模型二的仿真
对图中曲线采用1-2模型解得指令见answer2，（程序见附件3）
七、模型的评价
1、基于齐次坐标变换法的点位控制模型评价：该模型可以适用于所有已知目标位置和姿态的点位控制问题。

算法的计算效率非常的高，并且实时控制性好。

算法误差产生于多次三角变换求解所带来的舍入误差。

2、连续轨迹控制模型评价：该模型适用于所有参数方程的空间曲线生成机器指
令。

适用于机器人在低速运动的情况。

但离散点数量较少时，会产生误差。

3、指令检验法模型评价：该模型精度高，易于理解。

但设计算法上较为复杂。

参考文献
参考文献按正文中的引用次序列出，其中：
[1]《机械运动学教程》
[2] 《机器人手臂下棋运动的逆运动学分析与仿真》赵艳云
表 1
：
附件：
问题1-1
nx=T06(1,1);ny=T06(2,1);nz=T06(3,1);
ox=T06(1,2);oy=T06(2,2);oz=T06(3,2);
ax=T06(1,3);ay=T06(2,3);az=T06(3,3);
px=T06(1,4);py=T06(2,4);pz=T06(3,4);
n=[nx ny nz];
o=[ox oy oz];
a=[ax ay az];
p=[px py pz];
theta=zeros(6,100);
for i=2:100
%theta1
theta11=atan((py-65*ay)/(px-65*ax));
theta12=pi+atan((py-65*ay)/(px-65*ax));
if theta11-theta(i-1,1)>=theta12-theta(i-1,1)
theta(i,1) =theta12;
else
theta(i,1) =theta11;
end
%theta3
theta31=asin(1)+atan(1/6);
theta32=pi-asin(1)+atan(1/6);
if theta31-theta(i-1,3)>=theta32-theta(i-1,3)
theta(i,3) = theta32;
else
theta(i,3) = theta31;
end
%theta2
w=atan((c1(i,j)*px+s1(i,j)*py-65*(c1(i,j)*ax+s1(i,j)*ay))/(pz-65*az));
B=(255*c3(i,j))/sqrt((c1(i,j)*px+s1(i,j)*py-65*c1(i,j)*ax-65*s1(i,j)*ay)^ 2+(pz-p*az)^2);
theta21=asin(B-w-theta(i,3));
theta22=pi-asin(B)-w-theta(i,3);
if theta21-theta(i-1,2)>=theta22-theta(i-1,2)
theta(i,2) = theta22;
else
theta(i,2) = theta21;
end
%theta5
theta51=acos(s231(i,j)*(c1(i,j)*ax+s1(i,j)*ay)-c231(i,j)*az);
theta52=2*pi-acos(s232(i,j)*(c1(i,j)*ax+s1(i,j)*ay)-c232(i,j)*az);
if theta51-theta(i-1,5)>=b5-theta(i-1,5)
theta(i,5) = theta52;
else
theta(i,5) = theta51;
end
%theta4
theta41=asin((s511(i,j)*ax-c1(i,j)*ay)/s511(i,j));
theta42=pi-asin((s1(i,j)*ax-c1(i,j)*ay)/s511(i,j));
if atheta41-theta(i-1,4)>=theta42-theta(i-1,4)
theta(i,4) = theta42;
else
theta(i,4) = theta41;
end
%theta6
theta61=asin((sin(c1(i,j)*ox+s1(i,j)*oy)-c231(i,j)*oz)/s511(i,j));
theta62=pi-asin((s231(i,j)*(c1(i,j)*ox+s1(i,j)*oy)-c231(i,j)*oz)/s511(i,j ));
if theta61-theta(i-1,6)>=theta62-theta(i-1,6)
theta(i,6) = theta62;
else
theta(i,6) = theta61;
end
end
问题2-1
T06=transl(20,-200,120); nx=T06(1,1);ny=T06(2,1);nz=T06(3,1); ox=T06(1,2);oy=T06(2,2);oz=T06(3,2);
ax=T06(1,3);ay=T06(2,3);az=T06(3,3);
px=T06(1,4);py=T06(2,4);pz=T06(3,4);
n=[nx ny nz];
o=[ox oy oz];
a=[ax ay az];
p=[px py pz];
theta=zeros(7,6);
for i=2:100 %theta1
theta11=atan((py-65*ay)/(px-65*ax));
theta12=pi+atan((py-65*ay)/(px-65*ax));
if theta11-theta(i-1,1)>=theta12-theta(i-1,1)
theta(i,1) =theta12;
else
theta(i,1) =theta11;
end
%theta3
theta31=asin(1)+atan(1/6);
theta32=pi-asin(1)+atan(1/6);
if theta31-theta(i-1,3)>=theta32-theta(i-1,3)
theta(i,3) = theta32;
else
theta(i,3) = theta31;
end
c1(i)=cos(theta(i,1));
s1(i)=sin(theta(i,1));
c3(i)=cos(theta(i,3));
s3(i)=sin(theta(i,3));
c23(i)=cos(theta(i,1)+theta(i,3));
s23(i)=sin(theta(i,1)+theta(i,3));
%theta2
w(i)=atan((c1(i)*px+s1(i)*py-65*(c1(i)*ax+s1(i)*ay))/(pz-65*az));
B(i)=(255*c3(i))/sqrt((c1(i)*px+s1(i)*py-65*c1(i)*ax-65*s1(i)*ay)^2+(pz-1 *az)^2);
theta21=asin(B(i)-w(i)-theta(i,3));
theta22=pi-asin(B(i))-w(i)-theta(i,3);
if theta21-theta(i-1,2)>=theta22-theta(i-1,2)
theta(i,2) = theta22;
else
theta(i,2) = theta21;
end
%theta5
theta51=acos(s23(i)*(c1(i)*ax+s1(i)*ay)-c23(i)*az);
theta52=2*pi-acos(s23(i)*(c1(i)*ax+s1(i)*ay)-c23(i)*az);
if theta51-theta(i-1,5)>=theta52-theta(i-1,5)
theta(i,5) = theta52;
else
theta(i,5) = theta51;
end
c5(i)=cos(theta(i,5));
s5(i)=sin(theta(i,5));
%theta4
theta41=asin((s5(i)*ax-c1(i)*ay)/s5(i));
theta42=pi-asin((s1(i)*ax-c1(i)*ay)/s5(i));
if theta41-theta(i-1,4)>=theta42-theta(i-1,4)
theta(i,4) = theta42;
else
theta(i,4) = theta41;
end
%theta6
theta61=asin((sin(c1(i)*ox+s1(i)*oy)-c23(i)*oz)/s5(i));
theta62=pi-asin((s23(i)*(c1(i)*ox+s1(i)*oy)-c23(i)*oz)/s5(i)); if theta61-theta(i-1,6)>=theta62-theta(i-1,6)
theta(i,6) = theta62;
else
theta(i,6) = theta61;
end
end
问题2-1
L1 = link([ pi/2 0 0 140 0], 'standard');
L2 = link([ 0 225 0 0 0], 'standard');
L3 = link([-pi/2 0 0 0 0], 'standard');
L4 = link([pi/2 0 0 225 0], 'standard');
L5 = link([-pi/2 0 0 0 0], 'standard');
L6 = link([0 0 0 65 0], 'standard');
r=robot({L1 L2 L3 L4 L5 L6});
='robot';
drivebot(r);
q=[0 0 -pi/2 0 -pi/2 0];
q1=[-1.4713 1.0647 -0.7557 0 -0.3072 0]; qi=q1+q;
t=0:0.056:15;
q=jtraj(q,qi,t);
hold on
T=fkine(r,q);
sizeT=size(T);
for i=1:sizeT(3)
x=T(1,4,i);
y=T(2,4,i);
z=T(3,4,i);
hold on
plot3(x,y,z)
end
plot(r,q)。