机器人研究方向的自我学习[4] 操作臂逆运动学及其仿真

操作臂逆运动学

之前发的有错误，所以又做了一遍,均以角度为单位

1、坐标系命名

基坐标系{B}，位于操作臂的基座上。
固定坐标系{S}，一般与任务相关，通常根据基坐标系确定。
腕部坐标系{W}，依附于操作臂的末端连杆。
工具坐标系{T}，附与机器人所夹持工具的末端。
目标坐标系{G}，是对机器人移动工具到达的位置描述。

2、逆解

1、解的个数不仅取决云操作臂的关节数量，且连杆的非零参数越多，到达某一特定目标的方式越多。
2、解的目标位置必须在工作空间内。
3、所有包含转动关节和移动关节的串联型6自由度机构均是可解的.

3、习题

a

这是个超越方程，采用Matlab即可求得：

clc 
clear
  L1= sym('L1');
  L2= sym('L2');
  L3= sym('L3');
  m= sym('m');%m表示θ1，n表示θ2
  n= sym('n');
  x = sym('x');
  y = sym('y');

 [m,n]=solve(cos(m)*x+sin(m)*y==cos(n)*L2+L1,-sin(m)*x+cos(m)*y==sin(n)*L2,m,n);
 m
 n

结果如下：

m =
 
 2*atan((2*L1*y - (L1^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) - (L2^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) + (x^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) + (y^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) + (2*L1*L2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2))/(L1^2 + 2*L1*x - L2^2 + x^2 + y^2))
 2*atan((2*L1*y + (L1^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) + (L2^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) - (x^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) - (y^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) - (2*L1*L2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2))/(L1^2 + 2*L1*x - L2^2 + x^2 + y^2))
 
 
n =
 
 -2*atan(((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2)/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2))
  2*atan(((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2)/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2))
 

b

test.m

clear 
clc
T=input("输入矩阵T：");%0-3T
L1=input("输入L1：");
L2=input("输入L2：");
L3=input("输入L3：");

x=T(1,4);
y=T(2,4);

%用a、b、c、分别代替θ1,θ2,θ3
t=atan2(T(2,1),T(1,1));%a+b+c=t

[a,b]=inverse(L1,L2,L3,x,y);
 c=t-a-b;
 a1=a*180/pi;
 b1=b*180/pi;
 c1=c*180/pi;
 
a1
b1
c1

inverse.m

function [m,n]=inverse(L1,L2,L3,x,y)
m=[2*atan((2*L1*y - (L1^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) - (L2^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) + (x^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) + (y^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) + (2*L1*L2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2))/(L1^2 + 2*L1*x - L2^2 + x^2 + y^2)),
   2*atan((2*L1*y + (L1^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) + (L2^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) - (x^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) - (y^2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2) - (2*L1*L2*((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2))/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2))/(L1^2 + 2*L1*x - L2^2 + x^2 + y^2))]
 
n=[-2*atan(((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2)/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)),
   2*atan(((- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2)*(L1^2 + 2*L1*L2 + L2^2 - x^2 - y^2))^(1/2)/(- L1^2 + 2*L1*L2 - L2^2 + x^2 + y^2))]

已知θ1=90,θ2=90,θ3=90时，
T= [ 0 1 0 -3;
-1 0 0 4;
0 0 1 0;
0 0 0 1;]
运行程序

得到两组解：[90 90 90 ]和[163.7398 -90 -163.7398]

运行如下程序进行验证

clc
clear 
   
a=input("输入欧拉角a：");%a-θ1,b-θ2,c-θ3
b=input("输入欧拉角b：");
c=input("输入欧拉角c：");


A=[cosd(a) -sind(a) 0 0;
    sind(a) cosd(a) 0 0;
    0 0 1 0;
    0 0 0 1;]%0-1T
B=[cosd(b) -sind(b) 0 4;
    sind(b) cosd(b) 0 0;
    0 0 1 0;
    0 0 0 1;]%1-2T,L1=4
C=[cosd(c) -sind(c) 0 3;
    sind(c) cosd(c) 0 0;
    0 0 1 0;
    0 0 0 1;]%2-3T,L2=3

T=A*B*C;%0-3T

T

参考如下链接：
https://blog.csdn.net/queensyb/article/details/106100456

运用matlab 自带函数ikine（）求逆运动学解：

%%逆动力学
clc
clear all
close all

L(1)= Link('d', 0, 'a', 0, 'alpha', 0);
L(2)= Link('d', 0, 'a', 4, 'alpha', 0);
L(3)= Link('d', 0, 'a', 3, 'alpha', 0);

robot=SerialLink([L(1),L(2),L(3)],'name','t');   %SerialLink 类函数 %连接连杆
robot.name='三连杆平面机械臂';
robot.comment='三连机械臂';
robot.display();    %Link类函数，显示建立机器人DH参数
p=robot.fkine([pi/2,pi/2,pi/2]);%正运动学
q = robot.ikine(p,'mask',[1 1 1 0 0 0])*180/pi;%逆运动学

% 通过手动输入各个连杆转角，模型会自动运动到相应位置
theta1=[0 0 0];    %机器人伸直且垂直
robot.plot(theta1);  %SerialLink类函数,显示机器人图像
theta2=[pi 0 pi/2 ];
robot.plot(theta2);

p
q

运行结果：