基于Robotics toolbox的定制/非标机构的运动学正逆解

建立坐标系

这一步很重要，如果发现DH参数无法确定，可能是坐标系的建立有问题，返回来重新建立。

我发现网上基本都是六自由度全转动机器人，很少有定制机构的机器人建模，特别是移动+转动的，这也给我DH参数的确定带来了一定的困扰。

我这里是两个平移副+三个转动副：5自由度机器人；注意标准型DH参数的坐标系i的原点位于下一个关节处。如图所示我这种坐标系的建立方法就是错误的，移动副的坐标系原点应该位于下一个转动副的关节轴上。所以坐标系原点的位置很重要，不是随便确定的。
建立坐标轴的步骤如下：
1）确定Zi轴
作为第i+1个关节的驱动轴，其方向和关节轴线方向保持一致。例如Z0是第1个关节的驱动轴,Z1是第2个关节的驱动轴。可能看上去有点混乱，但是符合规则的，这保证了：当关节i被驱动时，连杆i以及与其相连的坐标系 OiXiYiZi，将会经历一个对应的运动。
另外 Zi轴的方向符合右手定则，注意和关节的转动方向对应！

2）确定基础坐标系
由前面的讨论可知，满足DH建模约束条件的坐标变换可以用
四个参数得到！
基础坐标系的选择近乎是任意的，我们可以选择将基础坐标系的原点 Oo放置在Z0 轴上任何一点。然后，可以通过任意方便的方式来选择X0轴和Y0轴。但是建立0坐标系时，要尽可能使得最后的DH参数简单。

3）确定 Xi方向
a.轴 Zi-1和轴Zi不共面
那么轴Zi-1和轴Zi的公垂线定义了Xi轴，并且它于轴Zi的交点即为原点Oi。
b.轴Zi-1平行于轴Zi
这种情况下，轴Zi-1和轴Zi之间存在无穷多个共同法线。将穿过原点Oi-1的法线选作Xi轴， Oi是该法线和Zi轴的交点。
c.轴Zi-1和轴Zi相交
选择Xi垂直于由Zi-1和Zi组成的平面。原点Oi一般设为Zi-1和Zi的交点。不过，轴线Zi上的任意一点都可以被选作原点。

4)确定坐标系N
以上的三个步骤适用于坐标系0,…N-1。最终的坐标系通常被称为末端执行器或者工具坐标系。最常见的是，将原点Oi以对称方式布置在夹具的中间, Zi轴方向沿着末端移动方向（最后一个关节伸缩的方向）。Xi轴方向沿着夹具的上下开合方向。

因为有很多情况可以随意选择坐标系 Xi轴的方向，这就导致了不同的DH参数。为了规范DH建模方法，总结“使得DH参数简单”的一些法则。
1、确定Xi轴方向时，如果有很多种选择，尽可能选择与Xi-1相同的方向
2、确定坐标系0时，使得关节变量1的取值为零。如果是转动关节，关节变量就是θ；如果是平动关节，关节变量是d
3、符合人的习惯

DH参数确定

个人感觉这一步是最关键的，尤其是对于非标机构来说，这一步做好了后面基本就是调用的问题。

一张经典的标准DH参数图：

如果关节i是一个转动关节，那么连杆i-1和连杆i之间沿着关节轴线i的距离di就是一个定值，在设计好机器人中该值不会再改变，而θi会发生改变，因此θi称为关节变量，意思就是机器人在运动过程中它会发生变化。

如果关节i是一个移动关节，那么连杆i-1 和连杆i之间的夹角θi就是一个定值，变化的是两个连杆沿着关节轴线的距离di，此时di被称为关节变量。

严格参照下图进行坐标系及其参数的定义并反复检查（特别对于非标机构）：如果某个参数按照定义无法确定，则坐标系原点或方向的确定有问题，返回重新调整坐标系，尤其是确定 Xi方向

坐标系{i} 的Zi轴沿着关节轴线i，坐标原点在长度线ai和关节轴线的交点处，沿着长度线ai 由关节轴线i指向关节轴线i-1 是Xi的正方向。Yi按照右手螺旋定则确定。

顺便说一下改进DH参数和标准DH参数的区别：其实就是看连杆i的原点位于下一个关节还是本关节。换句话说：标准型i-1轴的坐标系原点位于远端，改进型i-1坐标系原点位于近端；

必须严格区分，涉及到后面确定参数的问题，本机构中若使用改进DH参数，轴3的关节角始终为0，若使用标准DH参数，轴3的关节角为θ。

此外，还有一种确定DH参数的方法，就是通过原点逐渐到远端的矩阵变化，不过实操中发现并没有那么简单。以下是两种方法的参考代码（非本文中的机构）：

方法一：
L1 = Link('d', 0, 'a', 0, 'alpha', pi/2);	%Link 类函数
L2 = Link('d', 0, 'a', 0.5, 'alpha', 0,'offset',pi/2);
L3 = Link('d', 0, 'a', 0, 'alpha', pi/2,'offset',pi/4);
L4 = Link('d', 1, 'a', 0, 'alpha', -pi/2);
L5 = Link('d', 0, 'a', 0, 'alpha', pi/2);
L6 = Link('d', 1, 'a', 0, 'alpha', 0);
robot=SerialLink([L1,L2,L3,L4,L5,L6]);      %SerialLink 类函数
	
方法二：
s = 'Rz(q1).Rx(q2).Ty(L1).Rx(q3).Tz(L2)';
dh = DHFactor(s)

矩阵转换介绍

表示依次进行ZYX顺序的旋转
绕基坐标系（固定坐标系）旋转左乘，绕自身坐标系（当前坐标系）右乘

• 旋转矩阵：R

close all;clc;clear;
%% 旋转
%% 以下为旋转矩阵表示法
% 绕x旋转90度
Rx = rotx(90);
% 绕y旋转90度
Ry = roty(90);
% 绕z旋转90度
Rz = rotz(90);
% 绘制旋转后的坐标系
trplot(Rx);
% 绘制世界坐标到Rx的动画过程
tranimate(Rx);
% 两个旋转的叠加, 即先绕世界坐标系的x轴转90度，
% 再沿当前坐标系的y轴转动90度
figure;
Rxy = Rx*Ry;
tranimate(Rxy);

• 旋转+平移

%% 旋转与平移
% 纯平移
T_transl = transl(1,0,0);    %旋转不变，沿x轴平移1的齐次变换矩阵
% 纯旋转
T_rot = trotx(90);             %不移动，绕x轴转动90度
% 组合
T =  transl(1,0,0) * trotx(90);  % 先沿x方向移动1，再绕当前的x轴转动90度
%提取旋转部分
rot = t2r(T);
%提取平移部分
tans = transl(T);
% 绘图
figure;
trplot(T);
% 绘制动画
figure;
tranimate(T);

• 欧拉角-R

%% zyz欧拉角————奇异点y=0,有多解
R_zyz = eul2r(15,30,60);  %欧拉角转旋转矩阵，这里输入是角度制
gamma = tr2eul(R_zyz); %旋转矩阵转欧拉角，这里输出是弧度制
% 等同于下式：
R_zyz = rotz(15)*roty(30)*rotz(60);
% 角度制与弧度制的相互转化
gamma2=rad2deg(gamma);
gamma3=deg2rad(gamma2);

注意欧拉角中的Y方向旋转的角度不能是0或负值，或kpi,会奇异

• 翻滚摆转俯仰RPY-R

%% rpy角——————attention    奇异点p=+-90,无多解
R_rpy = rpy2r(15,30,60);  %翻滚摆转俯仰转为旋转矩阵，这里输入是角度制
rpy = tr2rpy(R_rpy); %旋转矩阵转翻滚摆转俯仰，这里输出是弧度制
rpy2= rad2deg(rpy);

% 等同于下式：
R_rpy2 = rotz(60)*roty(30)*rotx(15);

运动学

以下是参考代码（非本文中的机构）：

clear;
clc;
L1 = Link('d', 0, 'a', 0, 'alpha', pi/2);	%Link 类函数
L2 = Link('d', 0, 'a', 0.5, 'alpha', 0,'offset',pi/2);
L3 = Link('d', 0, 'a', 0, 'alpha', pi/2,'offset',pi/4);
L4 = Link('d', 1, 'a', 0, 'alpha', -pi/2);
L5 = Link('d', 0, 'a', 0, 'alpha', pi/2);
L6 = Link('d', 1, 'a', 0, 'alpha', 0);
%% L6.jointtype='P’意味着第四根link是由移动关节连接的
%% L1.qlim=[-pi,pi];  表示关节极限
%%L1=Link([0 d1 a1 alpha1], 'modified');  表示基于改进的DH建立模型的方法

b=isrevolute(L1);           %检查关节是否为转动关节
robot=SerialLink([L1,L2,L3,L4,L5,L6]);      %SerialLink 类函数
robot.name='robot';         %SerialLink 属性值
robot.display();            %显示机器人
theta1=[pi/4,-pi/3,pi/6,pi/4,-pi/3,pi/6];   %设置角度
robot.plot(theta1);         %绘制机器人，关节变量为[pi/4,-pi/3,pi/6,pi/4,-pi/3,pi/6]时的机器人状态：

%%如果joint是一个数组，则会绘制机器人运动的动画，其中joint的列数必须和关节数目相同：
%joint(: , 1) = linspace(pi/6,pi/2,100);
%joint(: , 2) = linspace(0,pi/4,100);
%joint(: , 3) = linspace(pi/3,pi/2,100);
%joint(: , 4) = linspace(0,160,100);
%Scara.plot(joint ,'jointdiam',1,'fps',100,'trail','r-');


p1=robot.fkine(theta1);     %运动学正解，输入为关节角度，输出机器人末端的齐次变换矩阵T
s=robot.A([4 5 6],theta1);  %关节传动矩阵
cchain=robot.trchain;       %转换为初等变换序列
q=robot.getpos();           %获取关节坐标

q2=robot.ikine(p1);         %逆运动学，数值解法，输出为关节坐标（角度）。输入机器人末端的齐次变换矩阵T

%‘mask’,M: mask vector (6x1) that correspond to translation in X, Y and Z, and rotation about X, Y and Z %respectively.
%这里Scara机器人只有四个自由度，在x，y两个方向的旋转不存在自由度，因此为[1 1 1 0 0 1]；mask的XYZ定义相对于末端来说
%’q0’,Q为求解提供一个关节初值，因为数值法逆解可能不会收敛到想要的构型，提供一个初值，可以增大得到我们想要构型的概率。
%qi = p560.ikine(T, [0 0 3 0 0 0])  给定关节坐标初始值
%’lambda’,L ： initial value of lambda (default 0.1) 数值解法的步长，如果求解失败，可以减小步长再尝试，当然减小步长意味着求解时间更长；
%’tol’,T容许误差，精度要求不高的话可以适当调大一点，默认为1e^-10
%’ilimit’,L迭代的最大次数，求解失败时可以适当增大重新尝试。
%’transpose’,A迭代时使用雅克比的转置代替伪逆

%只能用于6 自由度或更高的结构。您使用的掩蔽选项可用于驱动不足的机器人。
%对于具有 4 或 5 DOF 的机器人，这种方法很难使用，因为方向由世界坐标中的 T 指定，并且可实现的方向是工具位置的函数。
%改用 SerialLink.ikcon() 可能是可行的替代方案。注意轴的限制解除，否则求解失败，初始值不同，结果不同，正解以后结果都正确；
https://robotics.stackexchange.com/questions/11203/showing-error-while-using-inverse-kinematics-ikine-for-4-dof-robotic-arm


%qi = p560.ikine6s(T, 'ru')   %逆运动学，解析解法，输出为关节坐标（角度）。输入机器人末端的齐次变换矩阵T
%qplot(t, qi); %%观察所有关节的角度变化

总结

1.一定要注意标准DH和改进DH的区别，并正确确定坐标系原点和方向；
2.必须严格按照表格检查各个DH参数，切记切记；
3.非6自由度的运动学逆解经常失败，掩码无效。改用ikcon函数可正确求解，但注意适当解除运动限制并正确确定初始值；