机械臂轨迹规划

位姿1分析（由最初始状态到折叠状态，图中粉色线表示）

1、运动学正解，

求齐次变换矩阵（Matlab 编程）

syms a1a2a3b1b2b3

%各关节变量变化量

a1=—28*pi/180; a2=28*pi/180; a3=0*pi/180;

%各z轴间夹角b1=0;

b2=0;

b3=-pi/2;

%求齐次变换矩阵

由公式

1

i

i

T

-

= [ cos(a) -sin(a) 0 c ;

sin(a)*cos(b) cos(a)*cos(b) -sin(b) -d*sin(b);

sin(a)*sin(b) cos(a)*sin(b) cos(b) d*cos(b);

0 0 0 1 ] 0

3

T=0

1

T*1

2

T*2

3

T

=2*3

3

由此可求出各其次变换矩阵

T=[ 0.8829 0.4695 0 0

1

-0.4695 0.8829 0 0

0 0 1.0000 0

0 0 0 1.0000]

1

T= [ 0.8829 -0.4695 0 245.0000

2

0.4695 0.8829 0 0

0 0 1.0000 0

0 0 0 1.0000]

2

T=[ 1.0000 0 0 0

3

0 0.0000 1.0000 204.0000

0 -1.0000 0.0000 0.0000

0 0 0 1.0000]

T=[ 1.0000 0 0 216.3222 3

0 0.0000 1.0000 88.9795

0 -1.0000 0.0000 0.0000

0 0 0 1.0000]

T= [ 1.0000 0 0 216.3222; 2

0 1.0000 0 -115.0205;

0 0 1.0000 0;

0 0 0 1.0000]

1

T=[ 0.8829 -0.0000 -0.4695 149.2278;

3

0.4695 0.0000 0.8829 180.1213;

0 -1.0000 0.0000 0.0000;

0 0 0 1.0000]

2、求雅克比矩阵

由公式：

z1 =[T10(1,3);T10(2,3);T10(3,3)];

z2 =[T20(1,3);T20(2,3);T20(3,3)];

z3 =[T30(1,3);T30(2,3);T30(3,3)];

p1=[T31(1,4);T31(2,4);T31(3,4)];

p2=[T32(1,4);T32(2,4);T32(3,4)];

r1=[T10(1,1) T10(1,2) T10(1,3);

T10(2,1) T10(2,2) T10(2,3);

T10(3,1) T10(3,2) T10(3,3)];

p11=cross(z1,r1*p1);

p22=cross(z2,r2*p2);

j=[p11(1,1) p22(1,1)

p11(2,1) p22(2,1)

p11(3,1) p22(3,1)

z1(1,1) z2(1,1)

z1(2,1) z2(2,1)

z1(3,1) z2(3,1) ]

J=[ -88.9795 -204.0000

216.3222 0

0 0

0 0

0 0

1.0000 1.0000]

3、运动学反解

由程序：

x=216.3222;

y=88.9795;

f1=x-T30(1,4);

f2=y-T30(2,4);

[a1,a2]=solve(f1,f2,a1,a2);

a11=vpa(a1/pi*180,6);

a22=vpa(a2/pi*180,6);

可得出：a11= -28.0001

72.7177

a22= 27.9999

152.000

4、操作速度分析

syms q1q2v1v2

v=[v1;v2;0;0;0;0];

q=[q1;q2];

je=j*q;

f3=v(1,1)-je(1,1);

f4=v(2,1)-je(2,1);

[q1,q2]=solve(f3,f4,q1,q2)

q11=vpa(q1,3)；

q22=vpa(q2,3)；

可得结果为：q11=0.462e-2*v2

q22=-0.490e-2*v1-.202e-2*v2 5、各关节轨迹规划

Matlab程序：

% v(t)=diff(f(t),t)=a1+2*a2*t+3*a3*t^2 轨迹上的关节速度

% a(t)=diff(f(t),t,2)=2*a2+6*a3*t 轨迹上的关节加速度

% 由在起始点和终止点的关节速度要求，规定

% v(0)=0 v(tf)=0,分别得出a1=a0=0

%运动时间tf设为3s,关节角1终值为bf,初值为0

syms a2a3tf t;

bf=-28*pi/180;

a1=0;

a0=0;

tf=3;

f(tf)=a0+a1*tf+a2*tf^2+a3*tf^3-bf ;

v(tf)=a1+2*a2*tf+3*a3*tf^2 ;

[a2,a3]=solve(f(tf),v(tf),a2,a3)

%绘制关节角随时间的变化的三次曲线

t=0:0.01:tf;

f3=a0+a1*t+a2*t.^2+a3*t.^3

plot(t,f3)

grid

xlabel('时间t/s')

ylabel('关节变量值/°')

title('位姿1时关节角1轨迹规划曲线')

subplot(1,3,2)

v=a1+2*a2*t+3*a3*t.^2

plot(t,v)

grid

xlabel('时间t/s')

ylabel('关节角1的关节速度°/s ')

title('位姿1时关节角1速度轨迹曲线')

subplot(1,3,3)

a=2*a2+6*a3*t

plot(t,a)

grid

xlabel('时间t/s')

ylabel('关节角1的加速度°/s^2 ')

title('位姿1时关节角1加速度轨迹曲线')关节角1三次多项式差值曲线：关节角初值为0°，终值为-28°。