Dummy-Robot

• 源链接：Dummy-Robot
• 资料整理：不爱学习任同学（B站）

初始化设置

• 首先断电，将机械臂恢复至 Z 型位置：move_j(0, -75, 180, 0, 0, 180)

• 连接上位机，在电机未 enable 的状态下，erase J2/J3 电机的 config

  dummy0.robot.set_enable(0)
  dummy0.robot.joint_2.erase_configs()
  dummy0.robot.joint_3.erase_configs()

• apply 电机当前的位置

  dummy0.robot.joint_2.apply_home_offset()
  dummy0.robot.joint_3.apply_home_offset()

• PID 设置：

  dummy0.robot.joint_2.set_dce_kv(120)
  dummy0.robot.joint_2.set_dce_ki(200)
  dummy0.robot.joint_2.set_dce_kd(300)
  dummy0.robot.joint_2.set_dce_kp(2000)
  
  dummy0.robot.joint_3.set_dce_kv(120)
  dummy0.robot.joint_3.set_dce_ki(200)
  dummy0.robot.joint_3.set_dce_kd(300)
  dummy0.robot.joint_3.set_dce_kp(2000)

• 移动到 7 字型:

  dummy0.robot.set_enable(1)
  dummy0.robot.move_j(0, 0, 90, 0, 0, 180)
  
  # 任意位置
  dummy0.robot.move_j(-50, 60, 90, 70, -20, 180)
  
  # 复位
  dummy0.robot.resting()

机器人运动学简单入门

旋转矩阵和欧拉角

• 三维旋转：欧拉角、四元数、旋转矩阵、轴角之间的转换
• 欧拉角和旋转矩阵之间的转换

源码见： 6dof_kinematic

▶

RotMatToEulerAngle and EulerAngleToRotMat

函数分析：旋转矩阵和欧拉角的转换

这两个函数分别实现了 旋转矩阵（Rotation Matrix） 和 欧拉角（Euler Angles） 之间的转换，适用于 3D 旋转计算，如 机械臂运动、3D 图形变换、机器人姿态计算等。

1. RotMatToEulerAngle() - 旋转矩阵转换为欧拉角
(1) 输入输出

• 输入：
  • _rotationM：一个 3×3 旋转矩阵（列优先存储）。
• 输出：
  • _eulerAngles：存储 欧拉角 （C, B, A），即 （Yaw, Pitch, Roll）。

(2) 计算逻辑

这个函数从旋转矩阵 _rotationM 计算 ZYX 欧拉角（即绕 Z→Y→X 轴旋转）。

(a) 特殊情况处理：Gimbal Lock（万向锁）

• fabs(_rotationM[6]) >= 1.0 - 0.0001 检查是否接近 ±1，判断是否发生万向锁（Pitch = ±90°）。
• 当 _rotationM[6] = -1（向下）：
  • B = π/2
  • A = 0
  • C = atan2f(_rotationM[1], _rotationM[4])
• 当 _rotationM[6] = 1（向上）：
  • B = -π/2
  • A = 0
  • C = -atan2f(_rotationM[1], _rotationM[4])

(b) 正常情况计算

如果未发生万向锁：

• 计算 Pitch：

  $$
  B = \text{atan2}(-R_{32}, \sqrt{R_{11}^2 + R_{21}^2})
  $$

• 计算 Roll：

  $$
  A = \text{atan2}(R_{21} / \cos(B), R_{11} / \cos(B))
  $$

• 计算 Yaw：

  $$
  C = \text{atan2}(R_{32} / \cos(B), R_{33} / \cos(B))
  $$

(c) 结果存储

最终：

$$
\text{Yaw (C)} = eulerAngles[0], \quad \text{Pitch (B)} = eulerAngles[1], \quad \text{Roll (A)} = eulerAngles[2]
$$

static void RotMatToEulerAngle(const float* _rotationM, float* _eulerAngles)
{
    float A, B, C, cb;

    if (fabs(_rotationM[6]) >= 1.0 - 0.0001)
    {
        if (_rotationM[6] < 0)
        {
            A = 0.0f;
            B = (float) M_PI_2;
            C = atan2f(_rotationM[1], _rotationM[4]);
        } else
        {
            A = 0.0f;
            B = -(float) M_PI_2;
            C = -atan2f(_rotationM[1], _rotationM[4]);
        }
    } else
    {
        B = atan2f(-_rotationM[6], sqrtf(_rotationM[0] * _rotationM[0] + _rotationM[3] * _rotationM[3]));
        cb = cosf(B);
        A = atan2f(_rotationM[3] / cb, _rotationM[0] / cb);
        C = atan2f(_rotationM[7] / cb, _rotationM[8] / cb);
    }

    _eulerAngles[0] = C;
    _eulerAngles[1] = B;
    _eulerAngles[2] = A;
}

2. EulerAngleToRotMat() - 欧拉角转换为旋转矩阵

(1) 输入输出

• 输入：
  • _eulerAngles：欧拉角 (C, B, A) (Yaw, Pitch, Roll)
• 输出：
  • _rotationM：3×3 旋转矩阵。

(2) 计算逻辑

该函数计算 ZYX 旋转矩阵：
$$
R = R_z(C) \cdot R_y(B) \cdot R_x(A)
$$

(a) 欧拉角计算旋转矩阵

定义旋转矩阵：
$$
R_x(A) =
\begin{bmatrix}
1 & 0 & 0 \\
0 & \cos A & -\sin A \\
0 & \sin A & \cos A
\end{bmatrix}
$$

$$
R_y(B) =
\begin{bmatrix}
\cos B & 0 & \sin B \\
0 & 1 & 0 \\
-\sin B & 0 & \cos B
\end{bmatrix}
$$

$$
R_z(C) =
\begin{bmatrix}
\cos C & -\sin C & 0 \\
\sin C & \cos C & 0 \\
0 & 0 & 1
\end{bmatrix}
$$

最终得到：
$$
R = R_z(C) R_y(B) R_x(A)
$$

展开后：
$$
R =
\begin{bmatrix}
\cos A \cos B & \cos A \sin B \sin C - \sin A \cos C & \cos A \sin B \cos C + \sin A \sin C \\
\sin A \cos B & \sin A \sin B \sin C + \cos A \cos C & \sin A \sin B \cos C - \cos A \sin C \\
-\sin B & \cos B \sin C & \cos B \cos C
\end{bmatrix}
$$

代码中的 ca, cb, cc 代表 cos(A), cos(B), cos(C)，sa, sb, sc 代表 sin(A), sin(B), sin(C)，然后填充到旋转矩阵 _rotationM。

static void EulerAngleToRotMat(const float* _eulerAngles, float* _rotationM)
{
    float ca, cb, cc, sa, sb, sc;

    cc = cosf(_eulerAngles[0]);
    cb = cosf(_eulerAngles[1]);
    ca = cosf(_eulerAngles[2]);
    sc = sinf(_eulerAngles[0]);
    sb = sinf(_eulerAngles[1]);
    sa = sinf(_eulerAngles[2]);

    _rotationM[0] = ca * cb;
    _rotationM[1] = ca * sb * sc - sa * cc;
    _rotationM[2] = ca * sb * cc + sa * sc;
    _rotationM[3] = sa * cb;
    _rotationM[4] = sa * sb * sc + ca * cc;
    _rotationM[5] = sa * sb * cc - ca * sc;
    _rotationM[6] = -sb;
    _rotationM[7] = cb * sc;
    _rotationM[8] = cb * cc;
}

DOF6Kinematic 类

这个 DOF6Kinematic 类是一个 六自由度（6-DOF）机械臂的运动学求解器，用于 正向运动学（FK, Forward Kinematics） 和 逆向运动学（IK, Inverse Kinematics） 计算。
主要应用场景：

• FK（正向运动学）：已知关节角度，计算末端执行器的位姿（位置 + 姿态）。
• IK（逆向运动学）：已知末端执行器的位姿，计算关节角度。

▶

详细内容

DOF6Kinematic 采用 Denavit-Hartenberg（DH）参数 来表示机械臂的几何结构：

float DH_matrix[6][4] = {0}; // DH参数表：[关节号][home, d, a, alpha]

DH 矩阵存储的是：

变量	说明
$ \theta $ (home)	关节角（变量，由控制器调整）
$ d $	关节的偏移量
$ a $	关节间连杆长度
$ \alpha $	关节旋转轴的倾斜角

机械臂的结构参数

struct ArmConfig_t
{
    float L_BASE;    // 基座长度
    float D_BASE;    // 基座高度偏移
    float L_ARM;     // 大臂长度
    float L_FOREARM; // 小臂长度
    float D_ELBOW;   // 肘关节偏移
    float L_WRIST;   // 腕部长度
};

这些变量用于存储 机械臂各段的几何参数。

计算变量

float l_se_2, l_se, l_ew_2, l_ew, atan_e;

这些变量用于计算 肘部和手腕的几何关系，具体作用在 逆向运动学计算 中可能涉及三角函数运算。

主要数据结构

(1) Joint6D_t - 六自由度关节角

struct Joint6D_t
{
    float a[6]; // 六个关节角度（单位: 弧度）
};

• 用于存储 六个关节角度（$\theta_1, \theta_2, …, \theta_6$）。
• 重载 operator- 以支持角度差计算。

(2) Pose6D_t - 末端执行器位姿

struct Pose6D_t
{
    float X{}, Y{}, Z{};  // 位置
    float A{}, B{}, C{};  // 姿态 (Yaw-Pitch-Roll)
    float R[9]{};         // 旋转矩阵
    bool hasR{};          // 是否已计算旋转矩阵
};

• 机器人末端执行器的 位姿（XYZ 位置 + ABC 欧拉角）。
• hasR 表示 R[9] 是否已计算。

(3) IKSolves_t - 逆运动学解

struct IKSolves_t
{
    Joint6D_t config[8];  // 最多有 8 组逆解
    char solFlag[8][3];   // 解的标志
};

由于 6 轴机器人一般有 最多 8 组逆解，因此 config[8] 存储了所有可能的解，并使用 solFlag[8][3] 记录哪些解有效。

主要方法

(1) SolveFK() - 正向运动学

bool SolveFK(const Joint6D_t &_inputJoint6D, Pose6D_t &_outputPose6D);

作用：

• 输入：_inputJoint6D（六个关节角）。
• 输出：_outputPose6D（计算出的末端执行器位姿）。
• 流程：
  1. 计算 变换矩阵 $ T = T_1 \cdot T_2 \cdot … \cdot T_6 $。
  2. 提取末端的 XYZ 位置。
  3. 提取旋转矩阵，并转换为欧拉角（Yaw, Pitch, Roll）。

(2) SolveIK() - 逆向运动学

bool SolveIK(const Pose6D_t &_inputPose6D, const Joint6D_t &_lastJoint6D, IKSolves_t &_outputSolves);

作用：

• 输入：
  • _inputPose6D（目标位姿）。
  • _lastJoint6D（上一帧的关节角，作为参考）。
• 输出：
  • _outputSolves（可能的逆解）。
• 流程：
  1. 解析目标位姿 _inputPose6D，分解 X, Y, Z, A, B, C。
  2. 计算腕部中心点位置 W（通过末端位置反推腕部位置）。
  3. 计算前三个关节的解（主要利用 肘部的几何关系）。
  4. 计算后三个关节的解（使用旋转矩阵求解）。
  5. 返回 0~8 组解，并选择最优解（与 _lastJoint6D 角度最接近）。

构造函数和 DH 参数矩阵

DOF6Kinematic::DOF6Kinematic(float L_BS, float D_BS, float L_AM, float L_FA, float D_EW, float L_WT)

DH矩阵：

这个 DH_matrix 是一个 6×4 的 Denavit-Hartenberg（DH）参数矩阵，表示一个 六自由度机械臂 各个关节的运动参数。转换为矩阵形式如下：

$$
DH_matrix =
\begin{bmatrix}
0 & L_{BASE} & D_{BASE} & -\frac{\pi}{2} \\
-\frac{\pi}{2} & 0 & L_{ARM} & 0 \\
\frac{\pi}{2} & D_{ELBOW} & 0 & \frac{\pi}{2} \\
0 & L_{FOREARM} & 0 & -\frac{\pi}{2} \\
0 & 0 & 0 & \frac{\pi}{2} \\
0 & L_{WRIST} & 0 & 0
\end{bmatrix}
$$

其中：

• 第一列（θi）：关节角（home 位置，初始角度）
• 第二列（di）：连杆的 平移 偏移量
• 第三列（ai）：连杆长度
• 第四列（αi）：连杆的 旋转 偏移量（相邻连杆的扭转角）

机械臂物理参数：

• L_BASE：基座长度（Base Length）
• D_BASE：基座高度偏移（Base Offset）
• L_ARM：上臂长度（Arm Length）
• L_FORERM：前臂长度（Forearm Length）
• D_ELBOW：肘关节偏移（Elbow Offset）
• L_WRIST：腕部长度（Wrist Length）

正向运动学

正运动学（Forward Kinematics，FK）是机械臂运动学中的一个重要部分，它是描述机械臂在给定的关节角度（或位置）下，末端执行器（例如，抓手或工具）的位姿（位置和姿态）的数学模型。与逆运动学相反，正运动学的目的是从已知的关节变量推导出末端执行器的位置和姿态，而逆运动学则是从目标末端执行器的位置和姿态推导出关节变量。

▶

FK 的作用

1. 关节配置与末端执行器之间的关系：
   正运动学提供了关节角度或位置与末端执行器在空间中位置之间的映射关系。通过正运动学的计算，给定关节的角度或位置，我们可以快速得出末端执行器的位置和姿态。这在机械臂运动控制中非常重要，因为我们需要根据关节的状态来确定机械臂的末端执行器在哪里以及它如何朝向目标。
2. 运动轨迹规划：
   在运动规划中，正运动学被用来描述机械臂从某个初始位置到达目标位置时，每个关节需要如何运动，以及对应末端执行器的运动轨迹。通过正运动学的计算，可以描述整个运动过程中的末端执行器位置变化，从而规划和调整机械臂的运动路径。
3. 姿态控制：
   在很多应用中，机械臂不仅要到达某个目标位置，还需要具有特定的姿态（即末端执行器的方向）。正运动学能够帮助我们理解给定关节角度如何影响末端执行器的姿态，从而提供精确的姿态控制。
4. 仿真和调试：
   正运动学是机械臂仿真和调试的基础。在设计和开发机械臂时，正运动学帮助我们预测机械臂的行为，确保关节的配置能在物理上有效地实现末端执行器的目标位置。

六自由度机械臂正向运动学（FK）公式推导，正向运动学（Forward Kinematics, FK）是指：
已知 六自由度机械臂的 关节角度（Joint6D_t），
求解 末端执行器（End-Effector）在基坐标系中的 位置和姿态（Pose6D_t）。

我们使用 Denavit-Hartenberg (DH) 变换 建模机械臂运动，并推导出正向运动学公式。

单个关节的 DH 变换。每个关节的变换矩阵由 Denavit-Hartenberg 公式 定义：

$$
T_i =
\begin{bmatrix}
\cos\theta_i & -\cos\alpha_i \sin\theta_i & \sin\alpha_i \sin\theta_i & a_i \cos\theta_i \\
\sin\theta_i & \cos\alpha_i \cos\theta_i & -\sin\alpha_i \cos\theta_i & a_i \sin\theta_i \\
0 & \sin\alpha_i & \cos\alpha_i & d_i \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

其中：

• $ \theta_i $：关节变量（旋转角）
• $ d_i $：关节的平移偏移
• $ a_i $：连杆长度
• $ \alpha_i $：连杆扭转角

6 自由度机械臂的总变换矩阵可以表示为：

$$
T_{06} = T_0^1 \cdot T_1^2 \cdot T_2^3 \cdot T_3^4 \cdot T_4^5 \cdot T_5^6
$$

其中：

• $ T_0^1 $ 表示从 基坐标系 到 关节 1 的变换矩阵
• $ T_1^2 $ 表示从 关节 1 到 关节 2 的变换矩阵
• ……
• $ T_5^6 $ 表示从 关节 5 到 末端执行器 的变换矩阵
• 最终得到 $ T_{06} $ 即末端执行器相对于基坐标系的位姿矩阵

变换矩阵 $ T_{06} $ 可以拆分成：

$$
T_{06} =
\begin{bmatrix}
R_{06} & P_{06} \\
0_{1 \times 3} & 1
\end{bmatrix}
$$

其中：

• $ R_{06} $ 是 旋转矩阵，描述 末端执行器的方向
• $ P_{06} $ 是 位移向量，描述 末端执行器的位置

▶

代码分析

SolveFK 函数实现了六自由度机械臂的正向运动学（Forward Kinematics, FK）计算，即从关节角度计算末端执行器的位置和方向。
分析如下：

1. 代码核心逻辑
该函数的主要任务是：

1. 转换输入关节角度（单位从度转为弧度，并加上 DH 参数中的 θ 偏移量）。
2. 计算各个关节的旋转矩阵 R(i)（基于 DH 变换公式）。
3. 逐步累乘旋转矩阵，计算从基座到末端执行器的旋转矩阵。
4. 计算各个连杆在基坐标系下的位移（依次累加各关节的位移）。
5. 计算最终的末端位姿（X, Y, Z, A, B, C），并存入 _outputPose6D。

角度转换

for (int i = 0; i < 6; i++)
    q_in[i] = _inputJoint6D.a[i] / RAD_TO_DEG;

• q_in[i]：从输入角度（单位：度）转换为弧度。

for (int i = 0; i < 6; i++)
{
    q[i] = q_in[i] + DH_matrix[i][0];  // 加上DH参数中的θ偏移

• q[i]：加上 Denavit-Hartenberg (DH) 参数中的 θ 偏移量，得到真实关节角度。

计算每个关节的旋转矩阵

cosq = cosf(q[i]);
sinq = sinf(q[i]);
cosa = cosf(DH_matrix[i][3]);  // DH参数中的alpha
sina = sinf(DH_matrix[i][3]);  

• cosq, sinq 计算的是关节角 q[i] 的余弦和正弦值。
• cosa, sina 计算的是 DH 参数中的 α（扭转角） 的余弦和正弦值。

R[i][0] = cosq;
R[i][1] = -cosa * sinq;
R[i][2] = sina * sinq;
R[i][3] = sinq;
R[i][4] = cosa * cosq;
R[i][5] = -sina * cosq;
R[i][6] = 0.0f;
R[i][7] = sina;
R[i][8] = cosa;

• 计算每个关节的旋转矩阵 R[i]，用于后续矩阵相乘。

计算总旋转矩阵

MatMultiply(R[0], R[1], R02, 3, 3, 3);
MatMultiply(R02, R[2], R03, 3, 3, 3);
MatMultiply(R03, R[3], R04, 3, 3, 3);
MatMultiply(R04, R[4], R05, 3, 3, 3);
MatMultiply(R05, R[5], R06, 3, 3, 3);

• 依次相乘关节的旋转矩阵 R[i]，得到从 **基坐标系到末端执行器的旋转矩阵 R06**。

计算各关节的位移

MatMultiply(R[0], L1_base, L0_bs, 3, 3, 1);
MatMultiply(R02, L2_arm, L0_se, 3, 3, 1);
MatMultiply(R03, L3_elbow, L0_ew, 3, 3, 1);
MatMultiply(R06, L6_wrist, L0_wt, 3, 3, 1);

• 计算各个连杆的位置变换，将机械臂各部分的位移量转换到基坐标系中：
  • L0_bs：基座相对于基坐标系的偏移。
  • L0_se：肩关节相对于基坐标系的偏移。
  • L0_ew：肘关节相对于基坐标系的偏移。
  • L0_wt：腕关节相对于基坐标系的偏移。

计算末端执行器的位置

for (int i = 0; i < 3; i++)
    P06[i] = L0_bs[i] + L0_se[i] + L0_ew[i] + L0_wt[i];

• P06[0]、P06[1]、P06[2] 分别表示末端执行器在基坐标系中的 X、Y、Z 坐标。

计算末端执行器的姿态

RotMatToEulerAngle(R06, &(P06[3]));

• 通过 RotMatToEulerAngle 函数，把旋转矩阵 R06 转换为 欧拉角 (A, B, C)，表示末端的旋转角度。

存储计算结果

_outputPose6D.X = P06[0];
_outputPose6D.Y = P06[1];
_outputPose6D.Z = P06[2];
_outputPose6D.A = P06[3] * RAD_TO_DEG;
_outputPose6D.B = P06[4] * RAD_TO_DEG;
_outputPose6D.C = P06[5] * RAD_TO_DEG;
memcpy(_outputPose6D.R, R06, 9 * sizeof(float));

• 计算的 X, Y, Z 位置赋值给 _outputPose6D。
• 计算的 A, B, C 角度转换回度数并存储。
• 旋转矩阵 R06 直接存入 _outputPose6D.R，可以用于更精确的姿态计算。

返回计算成功

bool
DOF6Kinematic::SolveFK(const DOF6Kinematic::Joint6D_t &_inputJoint6D, DOF6Kinematic::Pose6D_t &_outputPose6D)
{
    float q_in[6];
    float q[6];
    float cosq, sinq;
    float cosa, sina;
    float P06[6];
    float R06[9];
    float R[6][9];
    float R02[9];
    float R03[9];
    float R04[9];
    float R05[9];
    float L0_bs[3];
    float L0_se[3];
    float L0_ew[3];
    float L0_wt[3];

    for (int i = 0; i < 6; i++)
        q_in[i] = _inputJoint6D.a[i] / RAD_TO_DEG;

    for (int i = 0; i < 6; i++)
    {
        q[i] = q_in[i] + DH_matrix[i][0];
        cosq = cosf(q[i]);
        sinq = sinf(q[i]);
        cosa = cosf(DH_matrix[i][3]);
        sina = sinf(DH_matrix[i][3]);

        R[i][0] = cosq;
        R[i][1] = -cosa * sinq;
        R[i][2] = sina * sinq;
        R[i][3] = sinq;
        R[i][4] = cosa * cosq;
        R[i][5] = -sina * cosq;
        R[i][6] = 0.0f;
        R[i][7] = sina;
        R[i][8] = cosa;
    }

    MatMultiply(R[0], R[1], R02, 3, 3, 3);
    MatMultiply(R02, R[2], R03, 3, 3, 3);
    MatMultiply(R03, R[3], R04, 3, 3, 3);
    MatMultiply(R04, R[4], R05, 3, 3, 3);
    MatMultiply(R05, R[5], R06, 3, 3, 3);

    MatMultiply(R[0], L1_base, L0_bs, 3, 3, 1);
    MatMultiply(R02, L2_arm, L0_se, 3, 3, 1);
    MatMultiply(R03, L3_elbow, L0_ew, 3, 3, 1);
    MatMultiply(R06, L6_wrist, L0_wt, 3, 3, 1);

    for (int i = 0; i < 3; i++)
        P06[i] = L0_bs[i] + L0_se[i] + L0_ew[i] + L0_wt[i];

    RotMatToEulerAngle(R06, &(P06[3]));

    _outputPose6D.X = P06[0];
    _outputPose6D.Y = P06[1];
    _outputPose6D.Z = P06[2];
    _outputPose6D.A = P06[3] * RAD_TO_DEG;
    _outputPose6D.B = P06[4] * RAD_TO_DEG;
    _outputPose6D.C = P06[5] * RAD_TO_DEG;
    memcpy(_outputPose6D.R, R06, 9 * sizeof(float));

    return true;
}

逆向运动学

代码见：SolveIK

逆运动学（Inverse Kinematics，IK）是机械臂控制中的一个关键问题，主要涉及如何根据机械臂末端执行器（例如，抓手、工具等）的目标位姿（位置和姿态）来计算各个关节的角度（或位置）。这种求解在机器人系统中具有非常重要的作用。

▶

IK 的重要性

• 任务驱动控制
  机械臂的主要任务是完成各种复杂的动作和操作，如抓取物体、焊接、打磨、装配等。这些操作通常要求末端执行器到达特定的空间位置并具有特定的朝向。为了使机械臂能够完成这些任务，必须根据目标位置和姿态来计算相应的关节角度，从而实现预期的末端执行器运动。逆运动学正是解决这个问题的核心工具。通过逆运动学求解，我们可以从目标位置反推机械臂各个关节的角度，进而指引机械臂如何运动到目标点。
• 灵活性和多样性
  机械臂通常具有多个关节，因此其工作空间（可达空间）非常复杂。逆运动学允许我们从目标点出发，探索所有可能的关节配置，从而为机器人提供多种可行的路径来实现目标。例如，对于一个三自由度的机械臂，可能存在多种不同的角度配置能够使末端执行器到达同一个位置和姿态。逆运动学的求解让机械臂能够根据不同的任务需求（如避免障碍物、优化路径、减少能耗等）选择最佳的关节配置。
• 解决冗余自由度
  机械臂通常具有比末端执行器需求更多的自由度。例如，一个6自由度的机械臂可以到达目标点，但也可能有多种不同的关节配置可以实现这一目标。逆运动学可以帮助我们选择合适的关节角度来执行任务，而不仅仅是使末端执行器到达目标位置。通过逆运动学，我们不仅仅是在寻找一个解，而是考虑如何优化这些解，选择最适合任务的姿态。例如，避免机械臂与环境的碰撞、减少不必要的运动或者提高操作精度等。
• 精确控制
  对于工业自动化、医疗机器人等高精度要求的应用，机械臂需要精确到达特定的目标位置和姿态。逆运动学提供了一种精确控制的方法，可以帮助系统从目标位置反向计算出精确的关节角度。在很多情况下，我们不能简单地控制每个关节的动作，而是必须通过末端执行器的精确控制来完成任务。逆运动学提供的关节角度求解对于确保任务执行的精度至关重要。
• 机器人与环境的交互
  在复杂的环境中，机械臂不仅需要到达目标位置，还需要与环境进行交互，例如抓取物体、避开障碍物或与其他机器人协调。逆运动学求解可以帮助机械臂在考虑障碍物和环境限制的情况下找到合适的姿态和路径。例如，在机器人抓取物体时，除了目标位置外，机械臂还需要考虑物体的位置、姿态以及机器人和物体的接触方式。逆运动学在这种场景中非常重要，因为它可以提供多个可行解，从而确保机械臂能够适应不同的操作环境。
• 简化运动规划
  逆运动学的求解是运动规划的前提。通过计算目标位姿的逆解，系统可以根据各个关节的角度来规划机械臂的运动轨迹。这样，机器人就可以从起始位置通过一系列平滑的运动（通过轨迹规划）到达目标位置。这种方法通过减少机器人操作的复杂度，帮助设计更简单且高效的运动路径。
• 实时控制
  对于实时控制任务（如手术机器人、自动化生产线等），逆运动学是必要的。机械臂需要在有限的时间内快速响应并执行动作。通过逆运动学求解，可以在不需要依赖复杂的运动学建模和计算的情况下迅速计算出目标关节角度，达到实时控制的要求。

▶

IK 和 FK 对比

特性	正运动学 (Forward Kinematics)	逆运动学 (Inverse Kinematics)
输入	给定关节角度（或位置）	给定末端执行器的目标位置和姿态（目标位姿）
输出	计算末端执行器的位置和姿态	计算各个关节的角度（或位置），使末端执行器到达目标位置和姿态
问题类型	是一个直接问题，从已知的关节参数计算末端执行器的位置和姿态	是一个反问题，从已知的目标末端执行器位置推算出关节参数
计算复杂度	一般较为简单，计算量小，通常是通过几何或矩阵运算直接推导的	计算复杂，可能有多个解或者无解，需要考虑多种可能的配置
解的唯一性	一般有唯一解，给定关节角度，末端执行器的位姿是唯一的	可能有多个解，也可能没有解，取决于目标位置和机械臂的自由度
适用场景	适用于已知关节角度时的末端执行器定位、仿真、姿态控制等	适用于控制末端执行器到达目标位置时，计算所需的关节角度或配置
精确度要求	高精度的正运动学计算能提供精确的末端执行器位置和姿态	精确的逆运动学求解对于路径规划、避障和任务执行非常关键
应用场景	机器人控制、路径规划、姿态控制、仿真和调试	机器人任务规划、路径优化、冗余自由度求解、协作任务等

代码分析略~

串口控制

• 上位机代码见 CLiTools
• 固件中的控制：MoveJ，MoveL 则是先求最优解再调用 MoveJ

▶

MoveJ / MoveL

bool DummyRobot::MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6)
//函数实现了控制虚拟机器人按指定关节角度移动的功能
{
    DOF6Kinematic::Joint6D_t targetJointsTmp(_j1, _j2, _j3, _j4, _j5, _j6);
    bool valid = true;
    // 创建目标关节角度对象

    for (int j = 1; j <= 6; j++)
    {
        if (targetJointsTmp.a[j - 1] > motorJ[j]->angleLimitMax ||
            targetJointsTmp.a[j - 1] < motorJ[j]->angleLimitMin)
            valid = false;
            //[j - 1] 表示数组索引,将 j 转换为从0开始的索引
    }
    // 检查目标关节角度是否在限制范围内

    if (valid)
    {
        DOF6Kinematic::Joint6D_t deltaJoints = targetJointsTmp - currentJoints;
        uint8_t index;
        float maxAngle = AbsMaxOf6(deltaJoints, index);
        float time = maxAngle * (float) (motorJ[index + 1]->reduction) / jointSpeed;
         // 计算关节运动的角度差和最大角速度
        for (int j = 1; j <= 6; j++)
        {
            dynamicJointSpeeds.a[j - 1] =
                abs(deltaJoints.a[j - 1] * (float) (motorJ[j]->reduction) / time * 0.1f); //0~10r/s
        }

        jointsStateFlag = 0;
        targetJoints = targetJointsTmp;
        // 根据角速度计算动态关节速度

        return true;
    }

    return false;
}


bool DummyRobot::MoveL(float _x, float _y, float _z, float _a, float _b, float _c)
// 将机器人移动到指定的笛卡尔空间姿态
{
    DOF6Kinematic::Pose6D_t pose6D(_x, _y, _z, _a, _b, _c);
      // 步骤 1：准备目标笛卡尔姿态
    DOF6Kinematic::IKSolves_t ikSolves{};
    DOF6Kinematic::Joint6D_t lastJoint6D{};

    dof6Solver->SolveIK(pose6D, lastJoint6D, ikSolves);
    //步骤 2：计算逆运动学（IK）解

    bool valid[8];
    int validCnt = 0;

    for (int i = 0; i < 8; i++)
    {
        valid[i] = true;

        for (int j = 1; j <= 6; j++)
        {
            if (ikSolves.config[i].a[j - 1] > motorJ[j]->angleLimitMax ||
                ikSolves.config[i].a[j - 1] < motorJ[j]->angleLimitMin)
            {
                valid[i] = false;
                continue;
            }
        }

        if (valid[i]) validCnt++;
    }
     // 步骤 3：检查IK解的有效性

    if (validCnt)
    {
        float min = 1000;
        uint8_t indexConfig = 0, indexJoint = 0;
         // 步骤 4：如果至少有一个有效的IK解，请选择最佳解并移动机器人
        for (int i = 0; i < 8; i++)
        {
            if (valid[i])
            {
                for (int j = 0; j < 6; j++)
                    lastJoint6D.a[j] = ikSolves.config[i].a[j];
                DOF6Kinematic::Joint6D_t tmp = currentJoints - lastJoint6D;
                float maxAngle = AbsMaxOf6(tmp, indexJoint);
                if (maxAngle < min)
                {
                    min = maxAngle;
                    indexConfig = i;
                }
                // 选择具有最小关节运动的IK解
            }
        }

        return MoveJ(ikSolves.config[indexConfig].a[0], ikSolves.config[indexConfig].a[1],
                     ikSolves.config[indexConfig].a[2], ikSolves.config[indexConfig].a[3],
                     ikSolves.config[indexConfig].a[4], ikSolves.config[indexConfig].a[5]);
    }
     // 将机器人移动到所选的IK解

    return false;
      // 未找到有效的IK解
}