Paul方法也被称为Paul反变换法,是比较常用的机器人运动学逆解计算方法。该方法要建立机器人的运动学矩阵方程,如式1,等式右边矩阵已知,等式左边矩阵中的关节变量未知。

首先用矩阵左乘式1矩阵方程,然后从等式两边矩阵元素中寻找并建立含有单关节变量的等式,解出该变量,再寻找并建立其他的单变量等式,如果没能解出所有的关节变量,则再在等式两侧左乘矩阵,然后再寻找并建立可求解的单变量等式,直到所有的变量都解出。

下面,我们以一个三自由度机械臂为例,讲解使用Paul方法得到其逆解。

三自由度机械臂DH参数表

很显然,该机器人在结构上满足Pieper 准则(6自由度机械臂存在解析解的充分条件是相邻的三个关节旋转轴相交于一点),所以它存在解析解。
由该机器人的DH参数可得各连杆的齐次位姿矩阵为:

假设机器人末端坐标系相对于基坐标系的位姿矩阵为:

则:

即:


假设机器人末端坐标系相对于基坐标系的位姿矩阵为:

%Forward函数(正解):
function T03 = forward(theta1 ,theta2 ,theta3,a1,a2)
%Eorward函数根据theta值计算末端姿态矩阵,其中thetal,theta2和theta3分别为三个关节角,本程序均以角度进行计算
T01 =[cosd(theta1), - sind(theta1),0,0;
sind(theta1),cosd(theta1),0,0; 0,0,1,al;0,0,0,1];
T12 =[cosd(theta2),- sind(theta2),0,0;0,0,1,0;-sind(theta2),cosd(theta2),0,0;0,0,0,1];
T23=[cosd(theta3)-sind(theta3),0,0;0,0,-1,- a2; sind(theta3),cosd(theta3),0,0;0,0,0,1];
T03=T01*T12*T23;
%求末端姿态矩阵

%Inverse函数(逆解)
function theta = inverse(T03,a1,a2)
%Inverse函数用于求解逆解中各theta值,参数T03为确定的姿态矩阵
T=T03;
const=45;%theta1的角度值,用户可赋值为任意值,只有当ax=ay=0成立时使用
tol=1e-4;
if (T(1,3)= =0&&T(2,3)= =0) %当ax=ay=0成立时
    theta = zeros(1,3);
    if abs(T(3,4)-(al + a2))< tolinver_theta2 1=0;
        inver_thetal_1= const;
        inver_theta3_1= atan2d( - T(1,2),T(1,1))- inver_theta1_1;
        theta(1,:) =[inver_thetal_1 ,inver_theta2_1 ,inver_theta3_1];
    elseif abs(T(3,4)-(al-a2))< tol
        inver_theta2_2=180;
        inver_thetal_2= const;
        inver_theta3_2= atan2d(T(1,2),-T(1,1))+ inver_thetal_2;
        theta(1,:)=[inver_theta1_2 ,inver_theta2_2,inver_theta3_2] ;
    end
else
    s2 = (1-T(3,3)^2)^(0.5);
    inver theta2_1= atan2d(s2,T(3,3));%theta2 的第一组解
    inver theta2_2= atan2d( - s2,T(3,3));%theta2的第二组解
    inver_thetal_1 = atan2d(T(2,3),T(1,3));%theta的第一组解
    inver_theta1_2=atan2d( -T(2,3),-T(1,3)) ;theta1 的第二组解
    inver_theta3_1 = atan2d(T(3,2) * cscd(inver_theta2_1), -T(3,1) * cscd(inver_theta2_1) ) ;
    inver theta3_2 = atan2d(T(3,2) * cscd(inver_theta2_2), -T(3,1) * cscd(inver_theta2_2) ) ;
    theta = zeros(2,3);
    theta(1,:)=[inver_theta1_1,inver_theta2_1 ,inver_theta3_1];
    theta(2,:) = [inver_theta1_2 , inver_theta2_2 , inver_theta3_2];
end
end

%验证程序
clc;
clear all;
close all;
a1_0=12;%取关节长度为12,可任意取值
a2_0=20;%取关节长度为20,可任意取值
T03_0=forward(45,180,-20,a1_0,a2_0);
%调用Eorward函数,求末端姿态矩阵
disp(T03_0);
theta=inverse(T03_0,al_0,a2_0);
%调用 Inverse并输出求得theta解矩阵
disp(theta);
%根据逆解求得的关节角求末端姿态矩阵
[R C]= size(theta);
if R==1 %逆解为无数组解情况时,本例中取inver_theta1_1=45时的特解
    T03_verify=zeros(4,4);
    T03_verify= forward( theta(1,1),theta(1,2),theta(1,3),a1_0,a2_0);
    disp(T03_verify);
elseif R==2%逆解为两组解情况
    T03_verify= zeros(4,4,2);
    for i=1:2
        T03_verify(:,:,i) = forward( theta(i,1), theta(i,2),theta(i,3),a1_0,a2_0);
        disp(T03_verify(:,:,i));
    end
end