针对一款六轴机器臂的DH建模求解

有幸拿到一款喷涂机器人的部分数据资料，着手写它的运动学算法。
第一步：DH建模


根据图纸资料，建立dh参数表格如下：

第二步，写出各关节矩阵

1.每个关节的乘积模板矩阵，该矩阵由每个关节的z轴旋转矩阵*z轴偏移量矩阵 乘以 x轴旋转矩阵 乘以 x轴偏移量矩阵得来。 相乘的顺序不能打乱：

2.各关节对应的矩阵如下：

第三步，
写出正运动学代码：

#BRTIRSE1506A
#authored by Tom Gong  [email protected]
import numpy as np
import math
from numpy import *


d1=459.5
a2=680.0   
a3=0   
d4=680
d5=0
d6=284.5

def kinematics(thetas):
    rtheta=np.array(thetas)
    theta1=float(rtheta[0][0])
    theta2=float(rtheta[0][1])
    theta3=float(rtheta[0][2])
    theta4=float(rtheta[0][3])
    theta5=float(rtheta[0][4])
    theta6=float(rtheta[0][5])
   

    global d1
    global a2   
    global a3  
    global d4
    global d5
    global d6

    T1=matrix([[cos(theta1),0,-sin(theta1),0],
               [sin(theta1),0,cos(theta1),0],
               [0,-1,0,d1],
               [0,0,0,1]])
    T2=matrix([[cos(theta2),-sin(theta2),0,a2*cos(theta2)],
               [sin(theta2),cos(theta2),0,a2*sin(theta2)],
               [0,0,1,0],
               [0,0,0,1]])
    T3=matrix([[cos(theta3),0,sin(theta3),0],
               [sin(theta3),0,-cos(theta3),0],
               [0,1,0,0],
               [0,0,0,1]])
    T4=matrix([[cos(theta4),0,-sin(theta4),0],
               [sin(theta4),0,cos(theta4),0],
               [0,-1,0,d4],
               [0,0,0,1]])
    T5=matrix([[cos(theta5),0,sin(theta5),0],
               [sin(theta5),0,-cos(theta5),0],
               [0,1,0,0],
               [0,0,0,1]])
    T6=matrix([[cos(theta6),-sin(theta6),0,0],
               [sin(theta6),cos(theta6),0,0],
               [0,0,1,d6],
               [0,0,0,1]])

    t1_t6=T1*T2*T3*T4*T5*T6

   
   
    return t1_t6

thetas=matrix([0,0,0,0,0,0])
print (kinematics(thetas))

以上代码实测通过。

第四步：逆运动学代码。
运动学逆代码开发步骤：
1.根据末端位姿和末端关节d6长度倒推出六轴根部的位置p（x,y,z）.
2.根据这个位置值的x和y坐标，可以得到第一关节角度值，角度值加pi（3.14159265）作为第一关节的第二个解。
3.建立第二臂和第四臂围成的三角形模型，p（x,y,z）是这个三角形的远端点，求得这个点到一轴顶点的位置的距离，作为三角形的第三边长，至此三角形三个边长已知，那么根据三角形余弦公式第三轴的角度可以求得，二轴角度同理也可以求得，这里要注意的是，求得的是三角形的内角，需要做个简单的转换，转换为二轴和三轴的实际转动角度。，
4.建立二角和三角的搭配关系，他们有两组搭配关系，乘以一轴的两组解，至此已经得到四种不同组的解组合了。
5.得到前三个角以后，无论如何用矩阵、逆矩阵相乘的解析法，也没有得到进一步求解4、5、6轴的机会，于是只好另辟新路。先求得末关节臂与第4关节臂的夹角。
6.6关节与4关节夹角的解法：模仿上述第四步的办法，可以求得这个夹角。这个夹角的补角，即是第五关节的转动角度的绝对值了。由于这个关节可以有两个解，一正一负。于是至此，结合前面的四种解的组合，乘以二，可以分出八组解了。其余的两个未知角度，也就是第四和第六角，需要配合已知角度来选择它们自己的两个值。
7.求得上述四个角度以后，因为已知条件有增加了一个，矩阵相乘解析法可以重新拿出来启用，可以求得关节角4和角6，但最大的问题就在这里了。因为上面全是推导完成的，各个角度之间的有机粘合自动切换双解的机制被破坏，当我们拿到第四角时，会发现，它总是不能把合适的两个解对应到合适的前四个已知角的组合当中去。第四角有三种以上的解析法的解法，每一种方法都拿来测试，发现它们都存在两个解的非自动选择问题，似乎很难固定这两个解了，这里所谓的不能固定是这样一种情况： 在编程的时候，为了提高写好代码的准确性，每写好一个角度，都要拿输入的六个角度值，用正运动学算法算出位姿，然后再用写好的这部分逆运动学代码来根据位姿算出各组可能的角度值，也就是用原始输入的一组值在各个组的解中去比对，正常情况下，八组里面总有一组和输入的角度是吻合的，比如上述这种情况，总应该有一组解（虽然还有两个未知数），这一组中已知的角度中，和正运动学运算中的输入值一组的1、2、3、5角度是对应的，不能出现错角才能证明代码设计正确。
8.后来总算找到一个解决办法来确定四角两个值的选择问题，那就是，从种四角解法中选择反余弦函数来解（三个解法当中的一个，这个解法得到正负两个解），然后用空间向量分析，得到第六轴在第五角是正值时在第四臂垂直平面上的投影，分析计算这个 投影出现在这个平面上第几象限，进行统计，发现一个规律，那就是当这个投影在一二象限时，对应一个固定值，而三四象限对应另一个（也就是x轴上下两侧的分法），由此，第四角得解。
9，到了第六角，一样很有个性的，用上述投影的一二、三四象限区分两个解的方式失效了，又经过多方试验，发现他们的区分也是经过这个平面象限的，但是是一四和二三象限的分法，也就是y轴的左右两侧分法。
10，解得全部六个角以后，再去修补第四角的奇异解问题，第四角的余弦值的分母，是一个二三角和的正弦值与五角正弦值的乘积，所以，要考虑到分母为零的情况。 通过一个条件语句，当分母为零时，采用上面第八条提到的其余两个象限法中的一种。 发现它的双值没有办法得到区分确定，与第四轴垂直平面投影的象限的方法失灵，它就是要自由的跳来跳去，各种试验一时不能取得进展，我甚至想到用象限的角平分线来作为区分二值的方式，然而手工计算量庞大到让人生厌，连日的开发工作已经疲惫不堪，况且觉得这样做理论依据不足，除非万不得已不作此尝试。后来经过研究发现，当二三角之和的正弦值为零时，第四轴处于与第一轴平行的状态，或许借助一轴垂直的平面来解决是个路子，因此获得提示，开辟新的方法来计算此状态下的第四角度值。
11.这个方法也是象限分析法，分析末端轴在基座标系下的投影，确定可以使用第一角加上math.atan2(-x,y)再加一个固定值来进行第四角的计算。
12.当五轴为0度时，属于奇异解中的多解问题，可以参考我以前的博客：
https://blog.csdn.net/Tom13a/article/details/93121158
13.在sin(角2+角3)=0的情况下，修补第四角，可能是一次奇异解的突破。没有借助解析法，完全使用的空间几何方法来求解的。