-
机器人控制算法一之四轴机械臂正、逆运动学详解
1.余弦定理
推广：a^2=b^2+c^2-2*b*c*cos(A) b^2=a^2+c^2-2*a*c*cos(B) c^2=a^2+b^2-2*a*b*cos(C) cos(A)=(b^2+c^2-a^2)/(2*b*c) cos(B)=(a^2+c^2-b^2)/(2*a*c) cos(C)=(a^2+b^2-c^2)/(2*a*b)- 1
- 2
- 3
- 4
- 5
- 6
- 7
2.DH建模
2.1 DH参数
2.2 标准DH参数和修正DH参数有何异同？
参考：https://blog.csdn.net/maple_2014/article/details/105612912
1.标准DH参数坐标系建立在传动轴上，而修正DH参数坐标系建立在驱动轴上
2.坐标系建立位置发生了变化，连杆之间的坐标系变换关系自然也发生变化
对于传统的串联机器人而言，两者的表现能力是一样的，没有优劣之分，我们可以选择其中一种方法进行建模。然而，由于修正DH参数坐标系建立在驱动轴上，对于树状结构的机器人，其表现能力更强，可以简化问题。
对于标准DH参数，根据DH参数表，并对变换公式矩阵连乘得到的是末端工具坐标系到机器人基坐标系的齐次变换矩阵；对于修正DH参数，根据DH参数表，并对式(10) 连乘得到的是最后一个驱动关节上的坐标系到机器人基坐标系的齐次变换矩阵，变换到末端工具坐标系还需增加一个变换(通常为平移变换)2.3 matlab仿真
clc close all clear all %*****************************建立机械臂的仿真模型********************************* %theta:Z轴旋转角度 %d：沿Z轴平移距离 %alpha：沿x轴旋转角度 %a：沿x轴平移距离 %参数的设定 h0=800; l1=2500; l2=2000; l3=2500; %转角 t0=0; t1= pi/4; t2=-pi/4; t3=-pi/2; %使用Link类函数，基于DH法建模（先绕z轴旋转，再沿x轴移动，再沿x轴旋转，再沿z轴平移） L(1)=Link('revolute','d',h0,'a',0,'alpha',pi/2,'offset',pi*3/2); L(2)=Link('revolute','d',0,'a',l1,'alpha',0,'offset',t1); L(3)=Link('revolute','d',0,'a',l2,'alpha',0,'offset',t2); L(4)=Link('revolute','d',0,'a',l3,'alpha',0,'offset',t3); % % theta d a alpha % L1=Link([ 0 h0 h0 pi/2 ], 'modified'); % [四个DH参数], options % L2=Link([ 0 0 l1 0], 'modified'); % L3=Link([ 0 0 l2 0], 'modified'); % L4=Link([ 0 0 l3 0], 'modified'); % % %使用Seriallink类函数把我们上面使用Link函数建立的连杆连成一个整体，生成一个串联机械臂模型 robot=SerialLink (L,'name','4-breaker'); %使用.plot绘制出某组关节变量的机械臂三维模型 %Three_Link.plot([0,0,0]) robot.display robot.teach % % 绘制直线 % T1=transl(2000,1800,1000);%根据给定起始点，得到起始点位姿 % T2=transl(2000,6500,1000);%根据给定终止点，得到终止点位姿 % T=ctraj(T1,T2,50) % Tj=transl(T); % plot3(Tj(:,1),Tj(:,2),Tj(:,3));%输出末端轨迹 % zlim([-10000,10000]); % xlabel('x'),ylabel('y'),zlabel('z'); % grid on; % q=robot.ikine(T,'q0',[0 0 0 0], 'mask',[1 1 1 0 0 0]); % hold on % robot.plot(q);%动画演示 % %*****************************正运动学********************************** % % 初始值及目标值 % init_ang=[0 0 0 0]; % targ_ang=[0, -pi/6, -pi/5, pi/6]; % step=200; % % [q,qd,qdd]=jtraj(init_ang,targ_ang,step); %关节空间规划轨迹，得到机器人末端运动的[位置，速度，加速度] % T0=robot.fkine(init_ang); % 正运动学解算 % Tf=robot.fkine(targ_ang); % figure; % subplot(2,4,3); i=1:4; plot(q(:,i)); title("位置"); grid on; % subplot(2,4,4); i=1:4; plot(qd(:,i)); title("速度"); grid on; % subplot(2,4,7); i=1:4; plot(qdd(:,i)); title("加速度"); grid on; % % Tc=ctraj(T0,Tf,step); % 笛卡尔空间规划轨迹，得到机器人末端运动的变换矩阵 % Tjtraj=transl(Tc); % subplot(2,4,8); plot2(Tjtraj, 'r'); % title('p1到p2直线轨迹'); grid on; % subplot(2,4,[1,2,5,6]); % plot3(Tjtraj(:,1),Tjtraj(:,2),Tjtraj(:,3),"b"); grid on; % hold on; % view(3); % 解决robot.teach()和plot的索引超出报错 % qq=robot.ikine(Tc, 'q0',[0 0 0 0], 'mask',[1 1 1 0 0 0]); % 逆解算 % robot.plot(qq); % %- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
机器人库模型参数：
Link函数为初始化机器人模型并赋予其一个初始姿态，offset指的是绕Z轴旋转角度theta，上图是我改变了坐标系0的位姿对比结果，我也分不清楚用的是哪种建模方法，一直理不太清楚，但感觉是改进型，因为坐标系在驱动轴上
3.运动学建模
可参考这篇博文，写的很奈斯：古月居-吴健新：机械臂运动学逆解
3.1 正运动学
// 根据舵机角度正向解出目标坐标 x = (L1 * sin(j1) + L2 * sin(j1 + j2) + L3 * sin(j1 + j2 + j3))*cos(j0); y = (L1 * sin(j1) + L2 * sin(j1 + j2) + L3 * sin(j1 + j2 + j3))*sin(j0); z = L1 * cos(j1) + L2 * cos(j1 + j2) + L3 * cos(j1 + j2 + j3);- 1
- 2
- 3
- 4
3.2 逆运动学
逆运动学主要有两种解法，解析法和代数法，解析法常用的是几何法，普遍通过高中三角函数公式就可以求得，代数法比较麻烦，这篇文章讲的很清晰:六轴UR机械臂标准DH正逆运动学公式推导+代码验证C++，几何法的话较为常用，比如开始介绍的余弦定理，下图是我基于几何法的一些想法：
刚开始求逆时很懵，看的台湾大学一个教程求逆解需要知道末端点位姿和一个角度，然后就以为求逆都需要末端位姿才可以求解，这两天又研究了一下，发现仅通过末端点坐标也可以求，只不过有很多个解，需要添加约束以过滤出最优解，比如二轴可能有2个解，三轴可能有4个解，四周可能有6个解，也比知道这个数字准不准确，大概就这个意思吧…
Kkoutian吴使用了这样一种解法：对于四轴机械臂，j0可由末端点的x、y得到，然后首先确定j1的大小，即从-90到90度循环给j1赋值，然后通过余弦定理等运算推理出j2,j3，这样的话就得到了多个解，最后需要添加约束找出最优解，具体可看：运动学逆解其C++实现代码如下：
#include "pch.h" #include#include #include #define RAD2ANG (3.1415926535898/180.0) #define ANG2RAD(N) ( (N) * (180.0/3.1415926535898) ) void inverseKinematics(double x, double y, double z); int main() { inverseKinematics(0, 30, 0);//逆解目标为（0，30，0）这个坐标 } void inverseKinematics(double x, double y, double z) { double a, b, c; //临时变量 double L1 = 10, L2 = 11, L3 = 14;//3节手臂的长度 double m, n, t, q, p;//临时变量 double j1, j2, j3, j0;//4个舵机的旋转角度 double x1, y1, z1;//逆解后正解算出来的值，看是否与逆解值相等 char i = 0; j0 = atan2(y, x); a = x / cos(j0); if (x == 0) a = y; //如果x为0，需要交换x，y b = z; for (j1 = -90; j1 < 90; j1++) { j1 *= RAD2ANG; j3 = acos((pow(a, 2) + pow(b, 2) + pow(L1, 2) - pow(L2, 2) - pow(L3, 2) - 2 * a*L1*sin(j1) - 2 * b*L1*cos(j1)) / (2 * L2*L3)); //if (abs(ANG2RAD(j3)) >= 135) { j1 = ANG2RAD(j1); continue; } m = L2 * sin(j1) + L3 * sin(j1)*cos(j3) + L3 * cos(j1)*sin(j3); n = L2 * cos(j1) + L3 * cos(j1)*cos(j3) - L3 * sin(j1)*sin(j3); t = a - L1 * sin(j1); p = pow(pow(n, 2) + pow(m, 2), 0.5); q = asin(m / p); j2 = asin(t / p) - q; //if (abs(ANG2RAD(j2)) >= 135) { j1 = ANG2RAD(j1); continue; } /***************计算正解然后与目标解对比，看解是否正确**************/ x1 = (L1 * sin(j1) + L2 * sin(j1 + j2) + L3 * sin(j1 + j2 + j3))*cos(j0); y1 = (L1 * sin(j1) + L2 * sin(j1 + j2) + L3 * sin(j1 + j2 + j3))*sin(j0); z1 = L1 * cos(j1) + L2 * cos(j1 + j2) + L3 * cos(j1 + j2 + j3); j1 = ANG2RAD(j1); j2 = ANG2RAD(j2); j3 = ANG2RAD(j3); if (x1<(x + 1) && x1 >(x - 1) && y1<(y + 1) && y1 >(y - 1) && z1<(z + 1) && z1 >(z - 1)) { printf("j0:%f,j1:%f,j2:%f,j3:%f,x:%f,y:%f,z:%f\r\n", ANG2RAD(j0), j1, j2, j3, x1, y1, z1); i = 1; } } for (j1 = -90; j1 < 90; j1++)//这个循环是为了求解另一组解，j2 = asin(t / p) - q;j2 = -(asin(t / p) - q);多了个负号 { j1 *= RAD2ANG; j3 = acos((pow(a, 2) + pow(b, 2) + pow(L1, 2) - pow(L2, 2) - pow(L3, 2) - 2 * a*L1*sin(j1) - 2 * b*L1*cos(j1)) / (2 * L2*L3)); //if (abs(ANG2RAD(j3)) >= 135) { j1 = ANG2RAD(j1); continue; } m = L2 * sin(j1) + L3 * sin(j1)*cos(j3) + L3 * cos(j1)*sin(j3); n = L2 * cos(j1) + L3 * cos(j1)*cos(j3) - L3 * sin(j1)*sin(j3); t = a - L1 * sin(j1); p = pow(pow(n, 2) + pow(m, 2), 0.5); q = asin(m / p); j2 = -(asin(t / p) - q); //if (abs(ANG2RAD(j2)) >= 135) { j1 = ANG2RAD(j1); continue; } x1 = (L1 * sin(j1) + L2 * sin(j1 + j2) + L3 * sin(j1 + j2 + j3))*cos(j0); y1 = (L1 * sin(j1) + L2 * sin(j1 + j2) + L3 * sin(j1 + j2 + j3))*sin(j0); z1 = L1 * cos(j1) + L2 * cos(j1 + j2) + L3 * cos(j1 + j2 + j3); j1 = ANG2RAD(j1); j2 = ANG2RAD(j2); j3 = ANG2RAD(j3); if (x1<(x + 1) && x1 >(x - 1) && y1<(y + 1) && y1 >(y - 1) && z1<(z + 1) && z1 >(z - 1)) { printf("j0:%f,j1:%f,j2:%f,j3:%f,x:%f,y:%f,z:%f\r\n", ANG2RAD(j0), j1, j2, j3, x1, y1, z1); i = 1; } } if (i == 0)printf("无解"); } - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
-
相关阅读:
力扣hot100——第1天：1两数之和、2两数相加、3无重复字符的最长子串
Citus 分布式 PostgreSQL 集群 - SQL Reference(创建和修改分布式表 DDL)
oracle11g体系结构
【SCAU数据挖掘】数据挖掘期末总复习题库选择题及解析
图像的离散傅里叶变换-python实战
腾讯程序员的手码K8S+Jenkins笔记
hutool工具导出excel代码示例
CSS中的定位
【Java 设计模式】UML 之类图
java实现朴素rpc
- 原文地址：https://blog.csdn.net/yohnyang/article/details/126117662