【个人简记】基于MDH，6R机器人运动学逆解_运动学逆解 matlab代码

【个人理解，有错请指出，谢谢】

求解方法为解析法（满足pieper原则）

以下是我做找逆解公式的时候，matlab的测试代码

​
clear;%清楚空间（base）变量；clear all清除全局（global）变量
clc;%清除命令行
close all%关闭所有的figure窗口 
format long g;% short为四个字节,long为八个字节：保留四位小数, g ：取消科学计数法
pi = 3.14159265;
%%
%代入具体数值运算
    d1 = 445.2; d4 = -748; a2 = 50; a3 = 750; 
    a1 = 0; a4 = 0; a5 = 0; a6 = 0; d2 = 0; d3 = 0; d5 = 0; d6 = 0;
%theta值
    offset = [pi/2 pi/2 0 0 0 0];
    theta_in = [0.1,  0.2,  0.3,  0.4,  0.5,  0.6];
    theta_in = theta_in + offset
    theta1 = theta_in(1); theta2 = theta_in(2); theta3 = theta_in(3); theta4 = theta_in(4); theta5 = theta_in(5); theta6 = theta_in(6);  
    s1 = sin(theta1); s2 = sin(theta2); s3 = sin(theta3); s4 = sin(theta4); s5 = sin(theta5); s6 = sin(theta6); 
    c1 = cos(theta1); c2 = cos(theta2); c3 = cos(theta3); c4 = cos(theta4); c5 = cos(theta5); c6 = cos(theta6);  
%%
%进行确切的alpha计算，不然会导致参数过大    
    s_a1 = 0; s_a2 = 1; s_a3 = 0; s_a4 = -1; s_a5 = 1; s_a6 = -1;
    c_a1 = 1; c_a2 = 0; c_a3 = 1; c_a4 = 0; c_a5 = 0; c_a6 = 0;
%%
% 符号运算
% 测试使用需要注释这里
%     syms ('s1', 's2', 's3', 's4', 's5', 's6', 'c1', 'c2', 'c3', 'c4', 'c5', 'c6', 's23', 'c23')
%     syms ('a2', 'a3', 'd1', 'd4')
%%
%MDH变换
   T01 = [ c1      -s1        0      a1
           s1*c_a1  c1*c_a1  -s_a1  -d1*s_a1
           s1*s_a1  c1*s_a1   c_a1   d1*c_a1
           0 0 0 1];
   
   T12 = [ c2      -s2        0      a2
           s2*c_a2  c2*c_a2  -s_a2  -d2*s_a2
           s2*s_a2  c2*s_a2   c_a2   d2*c_a2
           0 0 0 1];
     
   T23 = [ c3      -s3        0      a3
           s3*c_a3  c3*c_a3  -s_a3  -d3*s_a3
           s3*s_a3  c3*s_a3   c_a3   d3*c_a3
           0 0 0 1];
       
   T34 = [ c4      -s4        0      a4
           s4*c_a4  c4*c_a4  -s_a4  -d4*s_a4
           s4*s_a4  c4*s_a4   c_a4   d4*c_a4
           0 0 0 1];
       
   T45 = [ c5      -s5        0      a5
           s5*c_a5  c5*c_a5  -s_a5  -d5*s_a5
           s5*s_a5  c5*s_a5   c_a5   d5*c_a5
           0 0 0 1];
       
   T56 = [ c6      -s6        0      a6
           s6*c_a6  c6*c_a6  -s_a6  -d6*s_a6
           s6*s_a6  c6*s_a6   c_a6   d6*c_a6
           0 0 0 1];
EndPosture = T01 * T12 * T23 * T34 * T45 * T56;
nx = EndPosture(1,1);  ox = EndPosture(1,2);  ax = EndPosture(1,3);  rx = EndPosture(1,4);
ny = EndPosture(2,1);  oy = EndPosture(2,2);  ay = EndPosture(2,3);  ry = EndPosture(2,4);
nz = EndPosture(3,1);  oz = EndPosture(3,2);  az = EndPosture(3,3);  rz = EndPosture(3,4);
%%
%逆解算法测试
 
%theta1
 
%theta2
 
%theta3
 
%theta4
 
%theta5
 
%theta6
 
%%
%逆运动学测试
format short g
syms ('nx', 'ny', 'nz', 'ox', 'oy', 'oz', 'ax', 'ay', 'az', 'rx', 'ry', 'rz');%假设已知上述值
MT01 = (T01)^(-1); %求逆
MT12 = (T12)^(-1);
MT23 = (T23)^(-1);
MT34 = (T34)^(-1);
MT45 = (T45)^(-1);
MT56 = (T56)^(-1);
T06 = [nx, ox, ax, rx
       ny, oy, ay, ry
       nz, oz, az, rz
       0,  0,  0,  1]; %已知末端位姿
% T06 = T01 * T12 * T23 * T34 * T45 * T56  
 
%解1(2,4); 解2(1,4)(3,4); 解23; 解3
T16 = T12 * T23 * T34 * T45 * T56;
ert1 = MT01 * T06;
%不能用同一行
%解4(1,3)(3,3);解5(2,3); 
T36 = T34 * T45 * T56;
ert3 = MT23 * MT12 * MT01 * T06;
 
%测试使用矩阵
left  = T36
right = ert3
 
A = simplify(left(1,3))
B = simplify(right(1,3))
C = simplify(left(2,3))
D = simplify(right(2,3))
E = simplify(left(3,2))
F = simplify(right(3,2))

逆解万能公式，遇到这样的情况就说明有两个解

补充一张MDH机器人建模图

 可以发现建模满足pieper原则，后三轴交于一点（或者后三轴互相平行）

根据运动学正推可以发现，位置矩阵（3x1）只和θ1、θ2、θ3有关，姿态矩阵（3x3）影响权重较大的是θ4、θ5、θ6。

8组解的由来：

θ1有两种解，如下图。因此利用“atan2（）”求解出θ1的第一种解，那么第二种解就是和第一种解相差pi

 θ2有两种解，如下图

我的逆推流程中θ2遇到了万能公式的情况。

θ1、θ2、θ3这三个角求解就有四组解。

θ4、θ5、θ6的求解公式没有遇见万能公式的情况，那么就是对应的4组解，剩下的456角仅有一一对应的情况。

那么对应的最后一种情况就θ4和θ6变化pi，θ5取负即可完成最后一种情况，示意图如下。

原理是姿态可由一个角取负，剩下两组角变化pi即可。

姿态反转的情况，也就是剩下的四组解

验证每一组有效解是否正确

将每一组解带入正解算法验证。末端位姿矩阵唯一性，每一组解的姿态矩阵都应一样

 matalb算法的逆解思路