【个人理解,有错请指出,谢谢】
求解方法为解析法(满足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即可。
姿态反转的情况,也就是剩下的四组解
验证每一组有效解是否正确
将每一组解带入正解算法验证。末端位姿矩阵唯一性,每一组解的姿态矩阵都应一样