S-R-S七自由度机器人逆解

参考：M. Shimizu, H. Kakuya, W. -K. Yoon, K. Kitagaki and K. Kosuge, "Analytical Inverse Kinematic Computation for 7-DOF Redundant Manipulators With Joint Limits and Its Application to Redundancy Resolution," in IEEE Transactions on Robotics, vol. 24, no. 5, pp. 1131-1142, Oct. 2008, doi: 10.1109/TRO.2008.2003266.

     七自由度S-R-S机械臂是一种与人手臂自然结构产生的自由度一致的机械臂构型，其结构分为肩部，肘部和腕部，其中肩部，肘部与腕部分别由三个相交轴旋转副构成，可以视作一个球铰，肘部由一个旋转副组成，故称为S-R-S机械臂（球面铰-旋转副-球面铰）。

        根据七转动副模型，写出S-R-S机械臂的D-H参数表，其中d_bs  d_se  d_ew d_wt 分别表示从肩关节到肘关节的长度，从肘关节到腕关节的长度与从腕关节到工具点的长度。

       由于S-R-S具有七个自由度，是冗余自由度的机构，所以采用臂角φ来描述这一冗余自由度，臂角定义为由肩部，肘部与腕部组成的臂平面与参考平面之间的夹角。当确定腕部相对于肩部的位姿后，再确定臂角即可获得确定的手臂位姿。

    若假设末端的位姿分别由 x_d07给出，则可以根据给出的末端位姿计算出机器人各关节角。

1>肘关节角度的计算     当腕关节的位姿固定则可以容易的计算出肘关节的角度：

 

2>计算参考关节角：关节角需要满足的条件为：

        其中参考关节角为θ_3等于零的时候所确定的，并且为了避免奇异，参考平面的 θ_1需为0，此时上式中仅有 未知，所以可以将参考肩关节角度计算出来从而确定参考关节角。

3>计算肩关节角度：当根据参考关节角确定了臂角后，可以计算出肩关节角

4>计算腕关节 腕关节相对于肘关节的旋转矩阵由下式给出：

以上就是M. Shimizu论文中第二章求逆解的主要内容，下面是python中的实现（参数是我随便取的，臂角的优化可以看看论文中接下来的几张，讲的很细致，代码中的臂角我是随便取的，还有代码中没有考虑关节角限制，代码写的有点拉，不想改了）

import numpy as np

# 导入关节参数
Dbs = 50
Dse = 200
Dew = 250
Dwt = 50
# 导入末端位姿与自定义手臂角
Xd07 = np.array([120, 80, 50]).reshape(3, 1)
fai = 30
# 计算末端相对于基座的旋转矩阵
AX = 45
AY = 30
AZ = 33
C1 = np.cos(AX)
C2 = np.cos(AY)
C3 = np.cos(AZ)
S1 = np.sin(AX)
S2 = np.sin(AY)
S3 = np.sin(AZ)
Rd07 = np.array([[C1 * C2 * C3 - S1 * S3, -C3 * S1 - C1 * C2 * S3, C1 * S2],
                 [C1 * S3 + C2 * C3 * S1, C1 * C3 - C2 * S1 * S3, S1 * S2], [-C3 * S2, S2 * S3, C2]])
# 生成参数向量Lbs，Lse，Lew，Lwt
Lbs0 = np.array([0, 0, Dbs]).reshape(3, 1)
Lse3 = np.array([0, -Dse, 0]).reshape(3, 1)
Lew4 = np.array([0, 0, Dew]).reshape(3, 1)
Lwt7 = np.array([0, 0, Dwt]).reshape(3, 1)
# 计算Xsw0
Xsw0 = Xd07 - Lbs0 - np.matmul(Rd07, Lwt7)
Xsw0I = (np.linalg.norm(Xsw0)) ** 2
# 计算肘关节角度
cA4 = (Xsw0I - Dse ** 2 - Dew ** 2) / (2 * Dse * Dew)
A4 = np.degrees(np.arccos(cA4))
# 计算参考关节角度（计算A20）后缀0代表与参考平面重合时的手腕方向参数
R34 = np.array([[np.cos(A4), -np.sin(A4) * np.cos(A4), np.sin(A4)], [np.sin(A4), 0, -np.cos(A4)], [0, 1, 0]])
Vec = Lse3 + np.matmul(R34, Lew4)
v0 = Vec[0]
v1 = Vec[1]
v2 = Vec[2]
s0 = Xsw0[0]
s1 = Xsw0[1]
b = v0 ** 2 + v0 * v2 + v1 * s1 + v1 ** 2 + v2 * v2 - v0 * v2
c = v0 * s0 + s1 * v0 + v0 * v1 + s1 * v2 + v1 * v2
cA20 = (c / b)
A20 = np.degrees(np.arccos(cA20))
# 计算Xsw0的单位向量Usw0与其反对称矩阵,其中Usw0X为向量Usw0的反对称矩阵
Usw0 = Xsw0 / (np.linalg.norm(Xsw0))
a = Usw0[0]
b = Usw0[1]
c = Usw0[2]
# Usw0X是Usw0的反对称矩阵
Usw0X = np.array([[0, -c, b], [c, 0, -a], [-b, a, 0]], dtype=object)
# 计算As，Bs，Cs矩阵，其中R030为R03在参考关节处的旋转矩阵
R030 = np.array([[np.cos(A20), -np.sin(A20), -(np.sin(A20) * np.cos(A20))], [0, 0, 1], [-np.sin(A20), -np.cos(A20), 0]],
                dtype=object)
As = np.matmul(Usw0X, R030)
power = np.linalg.matrix_power(Usw0X, 2)
Bs = -(np.matmul(Usw0X, R030))
Cs = np.matmul((np.matmul(Usw0, Usw0.T)), R030)
# 求解肩关节角度即A1，A2，A3
A1 = np.degrees(np.arctan(
    (-As[1, 1] * np.sin(fai) - Bs[1, 1] * np.cos(fai) - Cs[1, 1]) / (
            -As[0, 1] * np.sin(fai) - Bs[0, 1] * np.cos(fai) - Cs[0, 1])
))
A2 = np.degrees(np.arccos(
    -As[2, 1] * np.sin(fai) - Bs[2, 1] * np.cos(fai) - Cs[2, 1]
))
A3 = np.degrees(np.arctan(
    (As[2, 2] * np.sin(fai) + Bs[2, 2] * np.cos(fai) + Cs[2, 2]) / (
            -As[2, 0] * np.sin(fai) - Bs[2, 0] * np.cos(fai) - Cs[2, 0])
))
# 求解腕关节参数即求解A5，A6,A7
R34 = np.array([[np.cos(A4), -(np.sin(A4) * np.cos(A4)), np.sin(A4)], [np.sin(A4), 0, 1], [0, 1, 0]])
R34T = R34.T
Aw = np.matmul(np.matmul(R34T, As.T), Rd07)
Bw = np.matmul(np.matmul(R34T, Bs.T), Rd07)
Cw = np.matmul(np.matmul(R34T, Cs.T), Rd07)
A5 = np.degrees(np.arctan(
    (Aw[1, 2] * np.sin(fai) + Bw[1, 2] * np.cos(fai) + Cw[1, 2]) / (
            Aw[0, 2] * np.sin(fai) + Bw[0, 2] * np.cos(fai) + Cw[0, 2])
))
A6 = np.degrees(np.arccos(
    Aw[2, 2] * np.sin(fai) + Bw[2, 2] * np.cos(fai) + Cw[2, 2]
))
A7 = np.degrees(np.arctan(
    (Aw[2, 1] * np.sin(fai) + Bw[2, 1] * np.cos(fai) + Cw[2, 1]) / (
            -Aw[2, 1] * np.sin(fai) - Bw[2, 0] * np.cos(fai) - Cw[2, 1])
))
# 输出计算角度，单位为角度值
Angles = np.array([A1, A2, A3, A4, A5, A6, A7], dtype=object)