机器人逆运动学

clear;

clc;

L1 = Link('d', 0, 'a', 0, 'alpha', pi/2); %Link 类函数

L2 = Link('d', 0, 'a', 0.5, 'alpha', 0,'offset',pi/2);

L3 = Link('d', 0, 'a', 0, 'alpha', pi/2,'offset',pi/4);

L4 = Link('d', 1, 'a', 0, 'alpha', -pi/2);

L5 = Link('d', 0, 'a', 0, 'alpha', pi/2);

L6 = Link('d', 1, 'a', 0, 'alpha', 0);

b=isrevolute(L1); %Link 类函数

robot=SerialLink([L1,L2,L3,L4,L5,L6]); %SerialLink类函数='带球形腕的拟人臂'; %SerialLink属性值robot.manuf='飘零过客'; %SerialLink属性值robot.display(); %Link 类函数

theta=[0 0 0 0 0 0];

robot.plot(theta); %SerialLink类函数

theta1=[pi/4,-pi/3,pi/6,pi/4,-pi/3,pi/6];

p0=robot.fkine(theta);

p1=robot.fkine(theta1);

s=robot.A([4 5 6],theta);

cchain=robot.trchain;

q=robot.getpos();

q2=robot.ikine(p1); %逆运动学

j0=robot.jacob0(q2); %雅可比矩阵

p0 =

-0.7071 -0.0000 0.7071 1.4142

0.0000 -1.0000 -0.0000 -0.0000

0.7071 0.0000 0.7071 1.9142

0 0 0 1.0000

p1 =

0.9874 0.1567 0.0206 1.0098

0.0544 -0.4593 0.8866 1.8758

0.1484 -0.8743 -0.4621 0.0467

0 0 0 1.0000

>>s

s =

1 0 0 0

0 1 0 0

0 0 1 2

0 0 0 1

cchain =

Rz(q1)Rx(90)Rz(q2)Tx(0.5)Rz(q3)Rx(90)Rz(q4)Tz(1)Rx(-90)Rz(q5)Rx(90)Rz(q6)Tz(1)

q =

0 0 0 0 0 0

q2 =

1.0e+04 *

0.0003 0.0180 -0.0399 1.1370 0.0002 0.0536

j0 =

-0.1100 0.0707 0.3577 -0.0114 0.5092 0

-0.8329 -0.0448 -0.2267 -0.6224 0.1813 0

-0.0000 0.7623 0.3956 -0.1410 -0.8413 0

-0.0000 0.5354 0.5354 0.3374 -0.0178 -0.8605

0.0000 0.8446 0.8446 -0.2139 -0.9751 0.1275

1.0000 0.0000 0.0000 0.9168 -0.2209 -0.4933

作者：fly qq

链接：https:///question/41673569/answer/129670927

来源：知乎

著作权归作者所有，转载请联系作者获得授权。

matlab机器人工具箱求串联机器人运动学逆解一般是采用ikine()函数，所以刚打开matlab看了一下源码。

（貌似新版本还加了其他求解算法，这边先不说，先解决题主的问题。）

我把它的主要步骤和贴出：

% 初始化

% 定义目标T，迭代次数，误差等；

% 初始化当前误差e,

while true

% 计算误差

Tq = robot.fkine(q');

e(1:3) = transl(T - Tq);

Rq = t2r(Tq);

[th,n] = tr2angvec(Rq'*t2r(T));

e(4:6) = th*n;

J = jacob0(robot, q); % 计算雅克比

% 根据末端误差求取关节变化

if opt.pinv % 雅克比伪逆法

dq = opt.alpha * pinv( J(m,:) ) * e(m);

else % 雅克比转置法

dq = J(m,:)' * e(m);

dq = opt.alpha * dq;

end

% 更新关节值

q = q + dq';

% 判断误差是否小于容许误差tolerance

nm = norm(e(m));

if nm <= opt.tol

break

end

end

所以，很简单，Matlab工具箱就是采用雅克比矩阵伪逆（或转置）来确定迭代方向，并通过迭代的方法使关节值收敛至目标位置。这应该是串联机器人运动学逆解数值解的最常用方法。

再回答为什么这么简单就可以求解逆解？

首先，只要给了D-H参数，雅克比矩阵很容易就可以推导出来（如果不会请参考各种机器人学教材，推荐John Craig的Introduction to robotics: mechanics and control）。

src="https:///v2-bcedbb70e4a7c4b47a3380f2026a097a_b.png " data-rawwidth="575" data-rawheight="315"

class="origin_imagezh-lightbox-thumb" width="575"

data-original="https:///v2-bcedbb70e4a7c4b47a3380f2026a 097a_r.png">