一、冗余自由度

刀具路径由离散的CLS数据组成，

$CLS=(x,y,z,i,j,k)$

其中x,y,z为刀具位置，i,j,k为刀轴方向的单位向量

1、CLS对应的末端姿态

方法一：

引用文献中的方法

Peng J, Ding Y, Zhang G, 等. Smoothness-oriented path optimization for robotic milling processes[J]. Science China Technological Sciences, 2020, 63(9): 1751–1763.

由cls可以生成刀具姿态，但是刀具姿态并不唯一，定义初始机器人任务姿态为：

$o_i=(x,y,z)$ 为刀具姿态矩阵的位置，z轴向量为

$z_i = （i，j，k）$

x轴向量为

$x_i=y_i \times z_i$

y轴向量可以有下式计算得来

$y_i= \frac{z_i \times \overline{o_{i+1} o_{i}} } {\lVert {z_i \times \overline{o_{i+1} o_{i}}} \lVert}\\ 对于最后一个cls点\\ y_i= \frac{z_i \times \overline{o_{i-1} o_{i}} } {\lVert {z_i \times \overline{o_{i-1} o_{i}}} \lVert}\\$

由此可以得到机器人末端初始姿态矩阵，因为绕刀具轴的旋转对铣刀定位没有影响，因此对于一个CLS，存在无数的可行机器人姿态，可由绕刀轴旋转得到。

$T(\gamma_i) = T_i \cdot Rot(\hat{z},\gamma_i)\\ T_i = \begin{bmatrix} x_i&y_i&z_i&o_i\\ 0&0&0&1\end {bmatrix}\\$

其中 $T_i$ 为cls对应的初始末端矩阵， $\gamma \in [-\pi,\pi]$ 为冗余自由度，这样就可以得到CLS任意冗余自由度对应的末端姿态矩阵。

这种方法会导致刀具轨迹弯折时机器人姿态变化较大

方法二：

[1]Zhu W, Qu W, Cao L, 等. An off-line programming system for robotic drilling in aerospace manufacturing[J]. The International Journal of Advanced Manufacturing Technology, 2013, 68(9–12): 2535–2545.

定义与上一个的方法一样， $z_i = [i,j,k]$ ，而 $x_i$ 是三个标准单位向量 $[1,0,0],[0,1,0],[0,0,1]$ 在由 $[x,y,z]和[i,j,k]$ 决定的平面上的最长投影的归一化向量， $y_i$ 是 $z_i$ 和 $x_i$ 的叉积。

方法三:

[1]熊刚. 机器人铣削加工的误差补偿和力控制方法研究[D]. 上海交通大学, 2019.

方法四：

计算刀轴向量和基座标系Z轴之间的变换矩阵

将两个向量之间的变换视为绕某一轴的旋转某一角度的螺旋运动，旋转角为两个向量之间的夹角，而旋转轴则是两个向量之间的叉积。

已知单位向量，将其绕某一轴 $\omega$ 旋转 $\theta$ ，通过使用罗德里格斯公式可以计算其旋转矩阵。

$R(\omega,\theta) =I +sin\theta[\omega]+(1-cos\theta)[\omega]^2$

方法一：
#使用轴角法求取向量之间的变换矩阵，和上一个函数是一样的方法
def rotation_matrix_from_vectors2(vec2):
    vec1 = [0, 0, 1]
    #规范化两个向量
    a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3)
    #求两个向量变换的旋转轴
    v = np.cross(a, b)
    s = np.linalg.norm(v)
    v = v / s
    #求旋转角度
    c = np.dot(a, b)
    theta = math.acos(c)
    #使用mr库求取轴角法的旋转矩阵，用的是罗德里格斯公式
    omegaHat = mr.VecToso3(v)
    so3mat = omegaHat * theta
    matrix = mr.MatrixExp3(so3mat)
    #求矩阵对应的欧拉角
    r = R.from_matrix(matrix)
    euler = r.as_euler('ZYX',degrees=True)
    matrix = r.as_matrix()
    return matrix

方法二：本质上都是一样的
def rotation_matrix_from_vectors(vec1,vec2):
    """ Find the rotation matrix that aligns vec1 to vec2
    :param vec1: A 3d "source" vector
    :param vec2: A 3d "destination" vector
    :return mat: A transform matrix (3x3) which when applied to vec1, aligns it with vec2.
    """
    a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3)
    v = np.cross(a, b)
    c = np.dot(a, b)
    s = np.linalg.norm(v)
    kmat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
    rotation_matrix = np.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2))
    return rotation_matrix

2、末端姿态对应的欧拉角

末端到基座标的旋转角ABC，是绕动坐标系旋转的ZYX欧拉角，KRC中定义角度A、B和C是绕Z、Y和X的旋转角度，旋转顺序也必须遵从：

绕Z轴旋转角度为A
绕Y轴旋转角度为B
绕X轴旋转角度为C

由机器人导论中的方法，对旋转矩阵求逆解

$R_{ZYX}=R_Z(\alpha)R_Y(\beta)R_X(\gamma)=\begin {bmatrix} r11 & r12 &r13\\ r21&r22&r23\\ r31&r32&r33\end {bmatrix}\\ \beta=Atan2(-r_{31},\sqrt{r_{11}^2 + r_{21}^2})\\ \alpha=Atan2(\frac {r_{21}}{c\beta},\frac {r_{11}}{c\beta})\\ \gamma = Atan2(\frac{r_{32}}{c\beta},\frac {r_{33}}{c\beta})$

虽然存在第二个解，但是在上式中取 $\beta$ 的正根以得到单解，满足 $-90\le \beta \le 90$ ，以此获取各种姿态表示法之间一一对应的映射函数。如果等于±90，解的结果就退化了，这个时候一般取 $\alpha = 0$ 。