[word格式]FANUC M-6iB型机器人运动学逆解研究.doc

[word格式] FANUC_M-6iB型机器人运动学逆解研究
FANUC_M-6iB型机器人运动学逆解研究
一
174一
机械设计与制造
MachineryDesign&Manufacture
第l0期
G2006023),江苏大学高级专业人才科研启动基金项目(06JDG033)
第10期李发忠等:FANUC_M一6iB型机器人运动学逆解研究一175一
末端执行器相对于机器人坐标系的总变换矩阵:
0
.;r::;
末端执行器位置方程:
p=c1c嚣(dc5+e)+23(c4s5+1)+a+cs2)一1s4s5
P=Ic?(d5+e)+d23(c5+1)+n+cs2)+cls4s5
p.=cc2+c?(c4譬5+1)一s.(dc5+e)
末端执行器姿态方程:
nx=Cl$玛c4C5C6一S4s6)一C6(ClC?s5+s1$4C5)一51C4s6
IZy~$l(s?c4c6—5趋s456—c?s5c6)+el(s4c5c6+c4s6)
ns23s5C6+c23(c4C5C6一s4s6)
Ox=cl(5?C4c6+ss4c6—?s5C6)+5l(4c6一s4C5s6)
D=sls?c4c,s6+5?54c6—Ic23s5c6)+Cl(s4c5C6--c4c6)
0=s凹s5C6+c?(c4C5C6+s4s6)
kx=c1(c23c5+s?c45)一sl4s5
ky=s1(c23c5+s?C4s5)+cls4s5
=c23c4S5一?C5
其中,s~=sin(),C23=COS(—)
图1FANUCM一6iB型机器人
3运动学逆解
运动学逆解是在机器人运动学正解的基础上,由给定工具坐
标系的位姿,求出在该位姿时各关节的变量值,它是离线编程和
轨迹规划的关键,运动学逆解的有解及无解,解的连续性及优化,
直接关系到离线编程的可行性和正确性.
由正解方程得,FANUC—M-6iB型机器人的位置由关节角
0,02,03,04,05决定,姿态由,,,以,,决定,直接采用常规
方法求运动学逆解难度太大.
作者通过对方程(1)和各关节齐次矩阵进行仔细研究,提出
只需要一次矩阵逆乘的逆解算法,如图2所示,作者建立的关节
4,5和6的坐标原点重合于点0,因此,该点的位置只与前三个
关节的旋转角度相关,只要求出0456在基坐标中的平移值即可求
出前三个关节变量,然后基于已知的三个变量和姿态又可求出后
三个关节变量.
图2FANUC—M一6iB型机器人关节坐标与参数
,2和3的关节变量
由FANUC—M一6iB型机器人运动学正解知,为常数满秩
(1)左右同时右乘:r,,得:
-
i
=
012
,
34566-1
===
003
63
T6=
nyoyarP
0:P
00O1
CLC23C1823slc1(o+cs2)一bsl
5lc23?-_clsl(叶c52)+6c1
-
$23c230CC2
0001
6—_s6一c5
cs~r6c4c~c6IIs6—cs
—64c6--S4C~C?c6ss
0OO
从方程(2),(3)导出:
=cec?+?+a+cs2)
pr=slec?+出趋+0+cs2)
;
=cc2+dc~3一e$2
将(4),(5)两式取平方相加:
(2)
(3)
(4)
(5)
(6)
(ec?+舭s2)i+(7)
(1)当取时ecdss2>0时
令ko:V+对方程(6),(7)整理得:
ec~3+ds?---ko—a---c$2(8)
dc?一es?=—CC2(9)
式(8),(9)取平方和得:
2(ko-a)csz~2Z?ccz_()2=0(10)
令,u--tan(Og2)~1]有几何代换公式:
c2=(1-u),(1+),s~=2u/(1+u)(11)
把方程(11)代入(10)得:
1+后2M+3=O(12)
其中,kl=d2+e2(j}0一a)2-(加),k2=4dk0一a),k3=d2+e2(‰
一
n)一(一C).
令A=kd一舭k,,解一元二次方程(12)得:
?
0
0
0
-ll
7j
一
176一机械设计与制造


j章.,?>…【-
“
求解时采用双变量反正切函数atan2(y,)来确定角度,它
计算arctan(y/x