【机器人】线性方程最小二乘解法对机械臂DH参数进行标定
1.前言
看了一篇文章，觉得里面的DH参数标定法讲的很清晰，便使用Matlab复现了一下，效果还可以，特写此文记录。
2.原理
2.1.误差最佳值的最小二乘计算
有研究结果表明：几乎95％的机器人位置误差都是由机器人内部运动学的不准确描述引起的，所以对DH模型参数进行标定是很有必要的。
DH参数物理含义：选自《机器人学、机器视觉与控制-Matlab基础》
当DH参数确定后，我们可以通过确定每根轴的角度值。在每根轴上建立相应的齐次方程而构建机械臂人的运动模型，就可以确定机械臂相对于机械臂的基座的末端位置和姿态。
通过将各个齐次矩阵联乘即可得到末端位姿和基坐标的转换矩阵
，本文用
表示：
后面的计算只会用到F的第4列
其中有末端位姿P：
为了简化求解运算，我们将用测量所得的连杆扭角值来代替理论的连杆扭角值α，同时认为通过机器人码盘读取的每根轴的转角值θ不存在误差，末端位姿可以简化为：
注意，我们在初次计算P的时候，DH参数采用的是厂家给的理想值！是需要进行标定的！
因为DH参数存在误差，所以真实的机械臂末端位姿为：
理想位姿和实际位姿的误差为：
*******当DH误差都比较小时，可以简化成相应的线性方程：（此处不是很懂）**********
因此对于一个末端位姿xyz，我们有三个方程成立，此三个方程成为一组位姿的方程，分别为x y z三个方向上的偏差：
对于我们六关节机器人，则有6个d和a参数，故一组方程为一个3x6的方程组，同时也是一组雅可比：
将其写为矩阵形式：
其中：
A为雅可比矩阵，每3行为一组，共有n组，其中n为我们所取的测试位姿个数，也就是说如果我们采样了n个位姿进行标定，则会产生3n*12维的A矩阵
B为n组位姿的误差矩阵，维数为3n*1：
δX也就是我们要求的标定参数：DH参数的误差，为一个12*1维的矩阵
***************只要我们样本n取得足够大，就可以利用最小二乘法求得δX的最佳值*****************
其中，δX为12*1维，inv(AT*A)为12*12维，AT为12*3n维，B为3n*1维，显然维数匹配
2.2.求到误差了干啥？
每次求到了误差δX，我们便将δX叠加到厂家给的DH参数上，获得新的DH参数，再重新求解误差，反复迭代，直到位姿的xyz误差足够小为止，此时的
δX+厂家DH参数=实际DH参数
3.Matlab代码实现
代码已上传到GitHub：我的GitHub
算法要点1
每次迭代时，都是将上一步求得的误差系数δX叠加到理论DH参数上，也就是厂家给的DH参数
d1 = d1_idea + delta_x(1)
d2 = d2_idea + delta_x(2)
d3 = d3_idea + delta_x(3)
d4 = d4_idea + delta_x(4)
d5 = d5_idea + delta_x(5)
d6 = d6_idea + delta_x(6)
a1 = a1_idea + delta_x(7)
a2 = a2_idea + delta_x(8)
a3 = a3_idea + delta_x(9)
a4 = a4_idea + delta_x(10)
a5 = a5_idea + delta_x(11)
a6 = a6_idea + delta_x(12)算法要点2
误差计算时是以理论xyz位姿减去实际xyz位姿，本程序以puma560机械臂为例子
dP = p560.fkine(qn).t - F1;
B1 = dP;
其中
F = p560_real.fkine([q1, q2, q3, q4, q5, q6]).t %F矩阵只要正运动学中xyz部分，不要角度部分算法要点3
因为AT*A不是一个满秩的矩阵，所以不能使用inv函数求逆，要使用广义逆函数：pinv求逆
delta_x = pinv(Ao'*Ao)*Ao'*Bo; %误差系数算法要点4
使用Matlab符号函数，虽然速度很慢，但是最终还是得到了满意的结果
syms d1 d2 d3 d4 d5 d6; %机械臂连杆偏移
syms a1 a2 a3 a4 a5 a6; %机械臂连杆长度
syms q1 q2 q3 q4 q5 q6; %转动角度
p560_real(1) = Link([0, d1, a1, 1.5708, 0]);
p560_real(2) = Link([0, d2, a2, 0, 0]);
p560_real(3) = Link([0, d3, a3, -1.5708, 0]);
p560_real(4) = Link([0, d4, a4, 1.5708, 0]);
p560_real(5) = Link([0, d5, a5, -1.5708, 0]);
p560_real(6) = Link([0, d6, a6, 0, 0]);
p560_real = SerialLink(p560_real, 'name', 'p560_real'); %建立真实的puma560模型，参数比理论测量大0.01
disp('建立完成')
F = p560_real.fkine([q1, q2, q3, q4, q5, q6]).t %F矩阵只要正运动学中xyz部分，不要角度部分
A = [
diff(F, a1), diff(F, a2), diff(F, a3), diff(F, a4), diff(F, a5), diff(F, a6), diff(F, d1), diff(F, d2), diff(F, d3), diff(F, d4), diff(F, d5), diff(F, d6)
] %建立误差线性矩阵其中，diff(F, a1)是在对F求a1的偏导
完成对符号参数赋值后，再将符号函数转为数值函数：
A1 = double(subs(A));注意要用double()转换一下，不然仍为sym数据类型，就会出现不计算sin函数的情况，之前我就遇到了计算出来的结果中含有sin(332/22)这种样子的数据，并没有计算sin值，这就是因为没有转为数值函数，仍为sym符号数据类型
4.程序效果
人为设置偏差：delta_x = [0.012; 0.021; 0.013; 0.01; 0.01; -0.01; 0.01; 0.01; 0.01; 0.012; -0.01; -0.01];
也就是说机械臂的理想DH参数要加上这些人为设置的偏差才是实际的DH，我们需要通过该程序拟合出最佳的误差系数
迭代10次的效果：
可以发现，很接近了
且误差已经很小了，效果不错哦
参考文献
[1]任永杰,邾继贵,杨学友, 等.利用激光跟踪仪对机器人进行标定的方法[J].机械工程学报,2007,43(9):195-200. DOI:10.3321/j.issn:0577-6686.2007.09.038.