机器人控制系列教程之UR机器人运动学求解

运动学简介

运动学是处理运动的几何学以及与时间有关的量，包括正向运动学和逆向运动学。
正运动学指从机器人的关节空间描述计算笛卡尔空间描述的机器人末端执行器的位置和姿态，该问题通常是一个几何问题，给定一组关节角度，计算末端坐标系相对于基坐标系的位置和姿态。
逆运动学指从笛卡尔空间描述下的机器人末端执行器位置和姿态反算出机器人关节空间应该达到的关节角度组合，是实现机器人控制的一个基本问题。
对于串联机器人(开链)通常因为正运动学方程是非线性的，逆运动学问题较为困难，很难得到封闭解，甚至无解。而正运动学的像空间就形成了逆运动学的有解空间，称为机器人的工作空间。

UR机器人的正向运动学

UR构型的机械臂是优傲机器人公司在2008年推出的轻巧灵活的协作机器人，它通过直观的用户界面开创了人性化的3D编程设计，让任何人都可以快速设置和操作机器人手臂。迄今为止，优傲机器人已经累计销售超过5万台。2013年，UR在中国成立优傲机器人贸易（上海）有限公司，迄今已有七十余家代理商。

可得D-H参数为：

利用MATLAB机器人工具箱，可进行如下编写，建立UR5机器人模型。

clear,clc,close all;
%% 建立机器人DH参数，初始状态为竖直状态
L1=Link('d',162.50,'a',0,'alpha',0,'modified');
L2=Link('d',0,'a',0,'alpha',pi/2,'offset',pi/2,'modified');
L3=Link('d',0,'a',425,'alpha',0,'modified');
L4=Link('d',126.70+6.6,'a',392.25,'alpha',0,'offset',pi/2,'modified');
L5=Link('d',99.70,'a',0,'alpha',pi/2,'modified');
L6=Link('d',99.60-4.5,'a',0,'alpha',-pi/2,'modified');
 
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','UR5')
Theta=[0 0 0 0 0 0];
Theta=Theta/180*pi; %换算成弧度
forwarda=robot.fkine(Theta) %求正解的齐次变换矩阵
W=[-1000,+1000,-1000,+1000,-1000,+1000];
robot.plot(Theta,'tilesize',150,'workspace',W); %显示三维动画
robot.teach(forwarda,'rpy' ) %显示roll/pitch/yaw angles，GUI可调界面

代码运行完成后，可看到：

UR5机器人逆向运动学

机械臂逆向运动学建模就是已知机械臂初末位姿，对各个自由度的空间角度和位置求解的一种方法，由于逆向运动学的特殊性，最后的结果可能是多解或者是无解。当存在多个解时需要考虑实际情况，比如机械臂是否能到达目标位置，即机械臂的避障问题以及空间位置是否存在等问题，而无解时说明机械臂不能到达所求的位置。据文献所述，Pieper 等提出一种针对六个旋转关节及后面三个关节轴相交机械臂的求逆解的方法，他也证明出具有该结构的机械臂可以进行逆运动学求解，这种求解方法也可用于其它类型的机械臂。不同的求解法都各有优缺点，在实际使用中，要根据具体任务和机械臂的具体结构选择合适的机械臂逆向运动学求解法。下表对机械臂进行逆向运动学求解的几种方法的分析对比：

本文使用代数法进行求解，已知末端相对于世界坐标系的总变换矩阵，求解各关节角度。因为变换矩阵第四行是 0 和 1，其余元素皆与角度有关，根据矩阵相等就是每个元素对应相等，所以可得一系列等式。对公式(3-14)的两边进行可逆矩阵左乘，可分别求出对应关节角度，过程如下：

1. 求解${\theta }_1$。基体坐标系到末端坐标系得转换矩阵表示为：
   
   可见，${\theta }_1$具有两个解，同理，以此类推即可求解出其他各个关节角的代数式。详见[1]。最后得到的所有解可罗列为：

所有关节角度求出后，可知非奇异姿态存在八组结果，因此要进行选解以满足任务需求。在实际运动过程中，每一时刻只能选择一组角度进行运动，因此必须对多解的情况挑选最合适的解。首先解要满足机械臂各关节的运动范围，为避免关节碰撞要运动“最短行程”。为提高移动效率，尽量选择移动关节较小部分，避免移动较大关节”。当机械臂运动路径存在障碍时，“近解”可能发生碰撞就要选择“远解”。因此要计算全部的解进行选择。
MATLAB的机器人工具箱中，对于串联机器人的逆运动学的求解，可在建立完成机器人的正向运动之后调用ikine函数进行逆向运动学求解，该函数基于正向运动学和雅可比转置矩阵开发了一套通用方法，对于任何机械臂位形都可以计算其正向运动学和雅可比转置矩阵，且不存在奇异性，其内部通过Levenberg-Marquadt可变步长求解器。原理如下：

假设机器人的期望位姿和当前位姿之间存在着一个特殊的弹簧，这个弹簧既可以改变机器人末端的姿态也可以改变位置，而当前位姿和期望位姿之间的差异值是一个包含平移和旋转分量的六维分量。而这个弹簧的力旋量也是一个六维向量，包含力和力矩。通过雅可比矩阵的转置矩阵可将该力旋量转换为关节力，依次迭代，从而求解出来关节的角度，具体详细原理解释请见[2]中的8.4小节。代码实现如下：

clear,clc,close all;
%% 建立机器人DH参数，初始状态为竖直状态
L1=Link('d',162.50,'a',0,'alpha',0,'modified','qlim',[-180,180]/180*pi);
L2=Link('d',0,'a',0,'alpha',pi/2,'offset',pi/2,'modified','qlim',[-145,65]/180*pi);
L3=Link('d',0,'a',425,'alpha',0,'modified','qlim',[-65,220]/180*pi);
L4=Link('d',126.70+6.6,'a',392.25,'alpha',0,'offset',pi/2,'modified','qlim',[-180,180]/180*pi);
L5=Link('d',99.70,'a',0,'alpha',pi/2,'modified','qlim',[-135,135]/180*pi);
L6=Link('d',99.60-4.5,'a',0,'alpha',-pi/2,'modified','qlim',[-360,360]/180*pi);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','UR5')

%% 正向运动学
% 已知：
Theta=[-170 0 0 0 0 0];
Theta=Theta/180*pi; %换算成弧度
forwarda=robot.fkine(Theta) %求正解的齐次变换矩阵
W=[-1000,+1000,-1000,+1000,-1000,+1000];
robot.plot(Theta,'tilesize',150,'workspace',W); %显示三维动画
robot.teach(forwarda,'rpy' ) %显示roll/pitch/yaw angles，GUI可调界面
% 求解
Tf=robot.fkine(Theta);

%% 逆向运动学
% 已知：
Tf;
%求解：
Thetai=robot.ikine(Tf,'q0',robot.offset);
Thetai=Thetai/pi*180 %换算成角度

运行结果如下图所示。

参考资料

[1] https://blog.csdn.net/AlbertDS/article/details/123679114

[2]. PeterCorke.机器人学、机器视觉与控制:MATLAB算法基础[M].电子工业出版社,2016.