铣削加工机器人（一）：机器人运动学
机器人学
1.1 空间形位与齐次变换
机器人在空间的位姿通常有两种表达方式，一种是在笛卡尔空间表示，通过在世界坐标系和机器人末端上建立坐标系，使用笛卡尔坐标描述机器人的位姿；另一种是通过关节空间表示，使用机器人六个关节的角度来描述机器人的形位。
KUKA KR60-3机器人的姿态通常使用笛卡尔坐标进行控制，机器人基坐标系为XB-YB-ZB，在机器人刀尖上建立工具坐标系XT-YT-ZT，则末端工具在基坐标系中的位姿就可以用XT-YT-ZT相对于XB-YB-ZB的位姿描述。工具坐标系在基坐标系的位姿关系可以用一个 的齐次变换矩阵T表示:
1.2机器人运动学建模
1.2.1 基于旋量法的运动学建模
KUKA KR60-3结构简图如图所示，XB-YB-ZB为基坐标系，XT-YT-ZT为末端工具坐标系，L1~L6为机器人的结构参数， 分别为六个关节的旋量坐标，r1~r6 分别为关节轴线上任意一点，其中r4*、r5、r*6为同一点，即后三关节轴线交点。
在旋量理论中，刚体在笛卡尔空间中的运动速度由角速度和线速度组成，这个向量被称为运动旋量，把运动旋量写成螺旋轴S与绕该转轴速度的合成形式，即：
对于纯转动关节，机器人上一点的形位的变换可以由指数坐标表示，也可以将其写为4×4的齐次变换矩阵形式：
由机器人的结构参数可以得到机器人的旋量参数，如表所示。
| 1 | [0,0,-1] | [0,0,0] | [0,0,0] |
| 2 | [0,1,0] | [350,0,815] | [-815,0,350] |
| 3 | [0,1,0] | [1200,0,815] | [-815,0,1200] |
| 4 | [-1,0,0] | [2020,0,960] | [0,-960,0] |
| 5 | [0,1,0] | [2020,0,960] | [-960,0,2020] |
| 6 | [-1,0,0] | [2020,0,960] | [0,-960,0] |
在KUKA KR60-3工业机器人中，每个连杆通过关节的螺旋运动施加运动给后面的连杆。末端坐标系经由6个关节串联的运动链，做多次螺旋运动，从初始位姿M变换到当前位姿T。在确定基坐标系和机器人初始位姿后，在完成指定的关节运动后，可以通过正向运动学表达式表示机械臂的末端坐标系：
1.2.2 正运动学代码实现
#Normalizes a vectordef Normalize(V): return V / np.linalg.norm(V)
#Converts an so(3) representation to a 3-vectordef so3ToVec(so3mat): return np.array([1], so3mat[0][2], so3mat[1][0]])
#Converts a spatial velocity vector into a 4x4 matrix in se3def VecTose3(V): return np.r_, V[1], V[2]]), [V[3], V[4], V[5]]], np.zeros((1, 4))]
#Converts a 3-vector of exponential coordinates for rotation into axis-angle formdef AxisAng3(expc3): return (Normalize(expc3), np.linalg.norm(expc3))
#Computes the matrix exponential of a matrix in so(3)def MatrixExp3(so3mat): omgtheta = so3ToVec(so3mat) if NearZero(np.linalg.norm(omgtheta)): return np.eye(3) else: theta = AxisAng3(omgtheta)[1] omgmat = so3mat / theta return np.eye(3) + np.sin(theta) * omgmat \ + (1 - np.cos(theta)) * np.dot(omgmat, omgmat)
#Computes the matrix exponential of an se3 representation of exponential coordinatesdef MatrixExp6(se3mat): se3mat = np.array(se3mat) omgtheta = so3ToVec(se3mat[0: 3, 0: 3]) if NearZero(np.linalg.norm(omgtheta)): return np.r_], [[0, 0, 0, 1]]] else: theta = AxisAng3(omgtheta)[1] omgmat = se3mat[0: 3, 0: 3] / theta return np.r_), np.dot(np.eye(3) * theta \ + (1 - np.cos(theta)) * omgmat \ + (theta - np.sin(theta)) \ * np.dot(omgmat,omgmat), se3mat[0: 3, 3]) / theta], [[0, 0, 0, 1]]]#空间坐标系下位姿计算def FKinBody(M, Blist, thetalist): T = np.array(M) for i in range(len(thetalist)): T = np.dot(T, MatrixExp6(VecTose3(np.array(Blist)[:, i] \ * thetalist))) return T#末端坐标系下位姿计算def FKinSpace(M, Slist, thetalist): T = np.array(M) for i in range(len(thetalist) - 1, -1, -1): T = np.dot(MatrixExp6(VecTose3(np.array(Slist)[:, i] \ * thetalist)), T) return T#旋量坐标Slist=np.array([[ 0, 0, 0, -1, 0, -1], [ 0, 1, 1, 0, 1, 0], [ -1, 0, 0, 0, 0, 0], [ 0, -815, -815, 0, -960, 0], [ 0, 0, 0, -960, 0, -960], [ 0, 350, 1200, 0, 2020, 0]])#法兰相对于基座标系姿态M=np.array([[ 0, 0, 1, 2190], [ 0, 1, 0, 0], [-1, 0, 0, 960], [ 0, 0, 0, 1]])#关节角thetalist=np.array([0,-90/180*pi,90/180*pi,0,0,0])#末端位姿计算T = FKinSpace(M,Slist,thetalist)1.3 基于旋量法的逆运动学求解
1.3.1 PK子问题
对于六自由度串联机器人而言，逆运动学可以描述成：已知机器人末端工具坐标系位姿，求解满足当前位姿的一组或多组关节角。由KR60-3机器人后三轴相交与一点的特性，利用Paden-Kahan子问题推导机器人关节逆解解析式。Paden-Kahan子问题是基于旋量理论的指数积公式求解各关节旋转角度的几何方法。基于旋量理论的运动学逆解可以分为三个Paden-Kahan子问题，分别是绕单轴旋转、绕两个轴旋转和旋转到给定距离，目前KR60-3的逆运动学求解中只用到了两类子问题，所以本文只介绍子问题1和子问题2，如图所示。
PK子问题1为一个点绕定轴旋转。描述为空间中一点p绕给定的轴线旋转到q点位置，求解旋转角度，可用旋量法表示为
通过向量之间的几何关系，旋转角可以由以下公式计算：
，
PK子问题2为一个点绕两个相交轴连续旋转。描述为空间中一点p，绕两相交轴和 进行旋转，先将p点绕旋转轴旋转 运动到s点，然后再绕旋转轴旋转运动到q点，和 分别为轴线和 的方向向量。设，，，用公式表示为：
若两个旋转轴重合，问题就变成了PK子问题1，若两个旋转轴不重合，可通过几何关系求解出中间点z，将子问题2分解为两个子问题1进行求解。
1.3.2 KUKA KR60-3逆运动学求解
由螺旋理论可知，位于螺旋轴上的点不会受到螺旋运动的影响，即。由KR60-3机器人结构简图可知，4、5、6关节轴线相交于同一点，在正运动学公式中右乘，就可以消除正运动学公式中4、5、6关节的影响，将6个关节的逆运动学求解分为两部分进行。
上式表示腕部中心点在前三关节中螺旋运动，指经过前三个关节的运动到达。由此可使用Paden-Kahan子问题1，对前3个关节逐一进行求解。基于几何法，使用PK子问题1即可求解出前三个关节的关节角，得到：
其中，上述公式中、、和的表达式如下：
在得到前三个关节的关节角后，将上一步求取的前三个关节带入公式(2-10)中，然后在原公式中取在轴6上但不在4、5轴上一点进行解耦，即可消除第六轴的影响，将后三轴转换为一个子问题2和一个子问题1进行求解，在公式两边同乘以在轴6上但不在4、5轴上一点。
上式可以看成一个子问题2，绕后4、5关节连续旋转，经过中间点，移动到位置，4、5关节轴线交点为。使用子问题2求解出中间点即可将问题转换为两个子问题1，对关节5、6进行求解。在求出关节角、后，代入公式，并取关节6轴线外一点，计算得到绕关节6旋转后的，根据旋量理论将公式进行解耦，即可求出关节角，关节角、、表达式如下：
1.3.3 基于旋量法的逆运动学数值求解
如果逆运动学方程无解析解时，可采用迭代数值方法求解。即使存在解析解,数值算法也经常用于改善求解的精度。
许多迭代方法可用于非线性方程的求解，’而是将运动学逆解方程转换成现有数值方法可解的形式。为此，将多次使用求解非线性方程的基本方法<牛顿-拉夫森法>。此外，也将引入其他一些优化方法，以应对精确解不存在的情况，并利用这些方法得到最接近真实值的解或者相反情况，存在无穷多个运动学逆解（例如，运动学冗余情况）。为此，需要找到相对某种指标的最优解。因此， 下面首先讨论用于求解非线性方程的牛顿-拉夫森法，接下来讨论优化第一必要条件。
（1）牛顿-拉夫森法
对于给定的微分方程方程，数值求解方程，假设为初值，利用泰勒级数在处展开，并截取到第一项，得：
若只保留到第—阶，令，求解得到：
再将上式求得的值作为初值，代人上述方程中，重复求解，得到下述方程：
上述过程不断迭代，直到满足某个指标值。例如，给定预先设定好的阂值满足：
对于KUKA KR60-3六自由度串联机器人，假设我们用坐标向量及其正向运动学方程表示末端坐标，自然会得到一个从个关节坐标到个末端坐标的非线性向量方程。令为预期的末端坐标，牛顿-拉夫森法中的可以定义成, 目标是找到关节 坐标且保证：
已知初始估计值接近真实解，运动学可以写成泰勒展开的形式，即：
截取泰勒展开第一项，假设可逆，使用下式求解，即：
若正向运动学是的线性函数，这时新的估计值 精确满足。相反，若正向运动学是的非线性函数，就如通常情况,这时新的估计值比更接近真实值，迭代过程不断重复，最终在处收敛。
（2）逆运动学的数值算法
实际上，由于计算效率等原因，经常不采用求逆的方式来求解。可以找到更高效的方法求解线性方程＝中的。例如，对于可逆阵，基于的分解可以用更少的运算得到。或者将替换成Moore-Penrose伪逆形式。
修改上述算法，使之能应用在预期末端位形为而不是坐标向量 的情况。可将关节坐标雅可比替换成末端物体雅可比。注意，由于向量表示的是从当前估计值（由正向运动学得到）到预期末端位形。不能简单地替换成；的伪逆应该作为物体速度旋量。为找到一个合理的对比，我们可以想象为一个速度向量，如果遵循单位时间，可使运动从 到。类似的，我们应该能够找到—个速度旋量，如果遵循单位时间，可使运动从到预期行为。
为找到这个，首先需计算相对物体坐标系的预期位形，即
然后,利用矩阵对数确定即:
由此可以给出逆运动学算法：
（a）初始化：已知，初始估计值，设定;
（b）计算，当或者时：
- 计算
完成一定次数的迭代后，就都能获得真实值的近似解。
（3） 数值解代码实现
def IKinBody(Blist, M, T, thetalist0, eomg, ev): #Computes inverse kinematics in the body frame for an open chain robot
thetalist = np.array(thetalist0).copy() i = 0 maxiterations = 20 Vb = se3ToVec(MatrixLog6(np.dot(TransInv(FKinBody(M, Blist, \ thetalist)), T))) err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \ or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev while err and i < maxiterations: thetalist = thetalist \ + np.dot(np.linalg.pinv(JacobianBody(Blist, \ thetalist)), Vb) i = i + 1 Vb \ = se3ToVec(MatrixLog6(np.dot(TransInv(FKinBody(M, Blist, \ thetalist)), T))) err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \ or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev return (thetalist, not err)
def IKinSpace(Slist, M, T, thetalist0, eomg, ev): #Computes inverse kinematics in the space frame for an open chain robot thetalist = np.array(thetalist0).copy() i = 0 maxiterations = 20 Tsb = FKinSpace(M,Slist, thetalist) Vs = np.dot(Adjoint(Tsb), \ se3ToVec(MatrixLog6(np.dot(TransInv(Tsb), T)))) err = np.linalg.norm([Vs[0], Vs[1], Vs[2]]) > eomg \ or np.linalg.norm([Vs[3], Vs[4], Vs[5]]) > ev while err and i < maxiterations: thetalist = thetalist \ + np.dot(np.linalg.pinv(JacobianSpace(Slist, \ thetalist)), Vs) i = i + 1 Tsb = FKinSpace(M, Slist, thetalist) Vs = np.dot(Adjoint(Tsb), \ se3ToVec(MatrixLog6(np.dot(TransInv(Tsb), T)))) err = np.linalg.norm([Vs[0], Vs[1], Vs[2]]) > eomg \ or np.linalg.norm([Vs[3], Vs[4], Vs[5]]) > ev return (thetalist, not err)
eomg = 0.01ev = 0.01initial = np.array([40.050000, -50.910000, 110.140000, 120.540000, -11.390000, 97.930000])/180*math.piT = np.array([ [0.410150, -0.123729, -0.903586, 1183.631143] , [-0.190306, -0.980556, 0.047886, -1137.765860] , [-0.891942, 0.152317, -0.425722, 756.413634 ], [ 0.000000, 0.000000, 0.000000, 1.000000] ]) = mr.IKinSpace(Slist,M,T,initial,eomg,ev)1.4 机器人运动学
1.4.1 雅可比矩阵
除了关节角度和机器人末端执行器位置之间的关系外，还需要研究关节和末端执行器速度之间的关系。在本节中，推导了这种关系的公式，并研究其结构和性质，还研究了连杆之间力传递的关系。由第2章的公式(2-5)，当给定一组关节角时，可以确定机器人末端的位置与姿态：
其中，，将公式两边进行求导，可以得到：
式中，为雅可比矩阵。
一般情况下，雅可比矩阵通过矢量差乘积法、微分变换法得到。而使用指数积公式表示正运动学，可以更加明确、优雅的推导出雅可比矩阵。在机器人正运动学的指数积公式下，末端的速度的表达式为：
带入机器人正运动学公式，并使用伴随映射将上式写成向量形式，即：
式中，对于任意，与相关联的伴随映射为:
1.4.2 雅可比矩阵代码实现
def JacobianBody(Blist, thetalist): #Computes the body Jacobian for an open chain robot Jb = np.array(Blist).copy().astype(np.float) T = np.eye(4) for i in range(len(thetalist) - 2, -1, -1): T = np.dot(T,MatrixExp6(VecTose3(np.array(Blist)[:, i + 1] \ * -thetalist))) Jb[:, i] = np.dot(Adjoint(T), np.array(Blist)[:, i]) return Jb
def JacobianSpace(Slist, thetalist): #Computes the space Jacobian for an open chain robot Js = np.array(Slist).copy().astype(np.float) T = np.eye(4) for i in range(1, len(thetalist)): T = np.dot(T, MatrixExp6(VecTose3(np.array(Slist)[:, i - 1] \ * thetalist))) Js[:, i] = np.dot(Adjoint(T), np.array(Slist)[:, i]) return Js1.5 涉及的函数说明
1.5.1 空间变换与齐次矩阵
1.计算旋转矩阵的逆invR = RotInv(R)
2.将三维向量转换为3*3反对称矩阵so3mat = VecToso3(omg)
3.将3*3反对称矩阵转换为三维向量omg = so3ToVec(so3mat)
4.从旋转的三维指数坐标中提取出轴线和角度 = AxisAng3(expc3)
5.计算矩阵指数对应的旋转矩阵R = MatrixExp3(so3mat)
6.计算旋转矩阵对应的矩阵对数so3mat = MatrixLog3(R)
7.M到SO（3）旋转矩阵的空间距离的度量d = DistanceToSO3(mat)
8.判断M是否为旋转矩阵judge = TestIfSO3(mat)
9.返回与M最近的旋转矩阵R = ProjectToSO3(mat)
10.将旋转矩阵和位置向量转换为变换矩阵T = RpToTrans(R,p)
11.从变换矩阵中分离出旋转矩阵和位置向量[R,p] = TransToRp(T)
12.变换矩阵求逆invT = TransInv(T)
13.构造与六维向量形式的运动旋量V对应的se3mat = VecTose3(V)
14.构造与se(3)矩阵对应的六维向量形式的运动旋量VV = se3ToVec(se3mat)
15.计算变换矩阵T的6*6的伴随矩阵AdT = Adjoint(T)
16.返回正则化的螺旋轴表达式S = ScrewToAxis(q,s,h)
17.从六位向量形式的指数坐标中提取出螺旋轴和沿轴线移动的距离[S,theta] = AxisAng6(expc6)
18.计算矩阵指数对应的变换矩阵T = MatrixExp6(se3mat)
19.计算变换矩阵对应的矩阵对数se3mat = MatrixLog6(T)
20.M到变换矩阵的空间距离d = DistanceToSE3(mat)
21.判断是否为SE3变换矩阵judge = TestIfSE3(mat)
22.离M最近的变换矩阵T = ProjectToSE3(mat)1.5.2 运动学
#正运动学末端坐标系下。给定初始形位，关节旋量以及关节值，计算末端坐标系T = FKinBody(M,Blist,thetalist)空间坐标系下。给定初始形位，关节旋量以及关节值，计算末端坐标系T = FKinSpace(M,Slist,thetalist)
#雅可比矩阵给定物体坐标系下描述的各关节旋量Blist及关节角，计算物体雅可比JBJb=JacobianBody(Blist,thetalist)给定物体坐标系下描述的各关节旋量Slist及关节角，计算物体雅可比JsJ=JacobianSpace(Slist,thetalist)
#逆运动学［theta11st,success］＝IKinBody（B1ist,M,T,theta1ist,eomg,ev）［theta11st,success］＝IKinSpace（B1ist,M,T,theta1ist,eomg,ev）1.6 参考链接
支持与分享
如果这篇文章对你有帮助，欢迎分享给更多人或打赏支持！