matlab练习程序（二维机械臂正逆运动学）

正运动学是已知连杆长度和各连杆旋转角度，求最终(x,y)位置的问题。

逆运动学是已知连杆长度和最终(x,y)位置，求各连杆旋转角度的问题。

二维两关节机械臂可以用下图示意：

 

根据上图可以得到如下公式：

x = L1*cos(theta1) + L2*cos(theta1+theta2)

y = L1*sin(theta1) +  L2*sin(theta1+theta2)

如果需要扩展到n连杆，则可以用如下公式：

正运动学直接套公式即可。

逆运动学则可以根据正向模型设计出最小化误差函数，通过非线性优化方式求解。

matlab代码如下：

clear;close all;clc;

num = 5;            %节点个数
L = 1;              %每个杆臂长度
for i=1:3
    pos = rand(1,2)*(2*num*L) - num*L;          %目标位置
    par = zeros(num,1);                         %每个节点旋转弧度，待优化参数
    
    options.Algorithm = 'levenberg-marquardt';  
    f = @(par) func(par,pos,num,L);
    lb = [];
    ub = [];
    [par,res]= lsqnonlin(f,par,lb,ub,options);  %执行优化
    
    theta = cumsum(par);                        %节点弧度累加和
    node = zeros(num,2);
    for j=1:num
        if j==1
            node(1,:) = [L*cos(theta(1)) L*sin(theta(1))];
        else
            node(j,:) = node(j-1,:) + [L*cos(theta(j)) L*sin(theta(j))];
        end
    end
    
    P=[0 0;node];
    figure;
    plot(pos(1),pos(2),'g*');
    hold on;
    plot(P(:,1),P(:,2),'r-o');
    axis([-num*L num*L -num*L num*L]);
    
end

function re = func(par,pos,num,L)
theta = cumsum(par);
node = zeros(num,2);
for j=1:num
    if j==1
        node(1,:) = [L*cos(theta(1)) L*sin(theta(1))];
    else
        node(j,:) = node(j-1,:) + [L*cos(theta(j)) L*sin(theta(j))];
    end
end
re = pos - node(end,:);                          %损失量
end

结果如下：

这里没有考虑真实的物理限制，因此可能会出现杆臂交叉的情况。