前言：
在前两篇文章中，基本不涉及机器人学。在这篇文章中，将主要验证运动学正逆解，运动学模型。
机器人学
MDH参数表建立
这里采用MDH，来源于《机器人学导论》这本书。 那么先看看机械臂样子，这是一个符合Pieper准则的机械臂：
在建立MDH时，用手稿画了一版：
可能看的不直观，接下来在SW装配体中查看。
上图也是MDH参数建立完之后的姿态，solidworks中由于坐标的重叠，坐标系重叠在一起不容易看，所以我分为三张图片，展示了7个坐标（1个基坐标系origin_global和6个关节坐标系c1-c6）：
结合以上坐标系原点和通过测量连杆长度，可以得出以下MDH参数表：
| 序号 | α i - 1 | a i -1 | di | θi |
|---|---|---|---|---|
| 1 | 0 | 0 | 0.107 | 0 |
| 2 | pi / 2 | 0 | 0 | 0 |
| 3 | 0 | 0.3 | 0 | 0 |
| 4 | - pi / 2 | 0 | 0.32315 | 0 |
| 5 | pi / 2 | 0 | 0 | 0 |
| 6 | - pi /2 | 0 | 0.0825 | 0 |
运动学正逆解
将这个MDH参数表给AI（Gemini），生成出正逆解函数：
正解函数：
static KinematicResult fk(List<double> jointAnglesDeg) {
jointAnglesDeg[1] = jointAnglesDeg[1] + 90;
jointAnglesDeg[2] = jointAnglesDeg[2] - 90;
if (jointAnglesDeg.length != 6) {
throw Exception("需要6个关节的角度");
}
// 1. 将输入的角度转为弧度用于运算
List<double> th = jointAnglesDeg.map((deg) => _deg2rad(deg)).toList();
// 2. 构建变换矩阵
List<List<double>> t1 = _mdhMatrix(0, 0, d1, th[0]);
List<List<double>> t2 = _mdhMatrix(pi / 2, 0, 0, th[1]);
List<List<double>> t3 = _mdhMatrix(0, a2, 0, th[2]);
List<List<double>> t4 = _mdhMatrix(-pi / 2, 0, d4, th[3]);
List<List<double>> t5 = _mdhMatrix(pi / 2, 0, 0, th[4]);
List<List<double>> t6 = _mdhMatrix(-pi / 2, 0, d6, th[5]);
// 连续相乘求得末端姿态 t06 (行主序)
List<List<double>> t06 = _multiply(
_multiply(_multiply(_multiply(_multiply(t1, t2), t3), t4), t5),
t6,
);
// 3. 构建 Matrix4 变换矩阵 (将行主序转为列主序)
final Matrix4 transformation = Matrix4(
t06[0][0],
t06[1][0],
t06[2][0],
t06[3][0],
t06[0][1],
t06[1][1],
t06[2][1],
t06[3][1],
t06[0][2],
t06[1][2],
t06[2][2],
t06[3][2],
t06[0][3],
t06[1][3],
t06[2][3],
t06[3][3],
);
// 4. 提取位移 Vector3
final Vector3 position = Vector3(t06[0][3], t06[1][3], t06[2][3]);
// 5. 提取姿态 Quaternion
// 利用 vector_math 提供的 getRotation 方法提取 3x3 旋转矩阵，并直接生成四元数
final Quaternion quaternion = Quaternion.fromRotation(
transformation.getRotation(),
);
// 6. 计算 Euler XYZ 欧拉角 (单位: 弧度)
// 采用标准 XYZ 顺规解析，并处理万向锁（奇异点）保护
double sy = sqrt(t06[0][0] * t06[0][0] + t06[1][0] * t06[1][0]);
bool singular = sy < 1e-6; // 检测是否发生万向锁
double ex, ey, ez;
if (!singular) {
ex = atan2(t06[2][1], t06[2][2]);
ey = atan2(-t06[2][0], sy);
ez = atan2(t06[1][0], t06[0][0]);
} else {
// 处于万向锁状态时，强行将 z 轴旋转归零，完全由 x 轴补偿
ex = atan2(-t06[1][2], t06[1][1]);
ey = atan2(-t06[2][0], sy);
ez = 0;
}
final Vector3 eulerXYZ = Vector3(ex, ey, ez);
// 7. 返回结果
return KinematicResult(
position: position,
quaternion: quaternion,
eulerXYZ: eulerXYZ,
transformation: transformation,
);
}
逆解函数：
static List<List<double>> ik(Matrix4 targetPose, List<double>? preJointRad) {
preJointRad?[1] = preJointRad[1] + pi / 2;
preJointRad?[2] = preJointRad[2] - pi / 2;
List<List<double>> solutions = [];
// 1. 提取目标位移和旋转矩阵元素
double px = targetPose.entry(0, 3);
double py = targetPose.entry(1, 3);
double pz = targetPose.entry(2, 3);
double r11 = targetPose.entry(0, 0);
double r12 = targetPose.entry(0, 1);
double r13 = targetPose.entry(0, 2);
double r21 = targetPose.entry(1, 0);
double r22 = targetPose.entry(1, 1);
double r23 = targetPose.entry(1, 2);
double r31 = targetPose.entry(2, 0);
double r32 = targetPose.entry(2, 1);
double r33 = targetPose.entry(2, 2);
// 2. 反推球形腕中心点 (Wrist Center)
double wx = px - d6 * r13;
double wy = py - d6 * r23;
double wz = pz - d6 * r33;
// ==========================================
// 【新增】：肩部奇异点处理 (Shoulder Singularity)
// 跨越北极点： 锁定轴1为上一帧的值
// ==========================================
double rXySq = wx * wx + wy * wy;
List<double> th1List = [];
// 如果腕心水平投影半径极小 (1毫米以内视为奇异)
if (rXySq < 1e-6) {
// 锁定 θ1 为上一帧的角度，此时不再分正反向
double prevTh1 = (preJointRad != null && preJointRad.isNotEmpty)
? preJointRad[0]
: 0.0;
th1List = [prevTh1];
} else {
// 正常区域，产生正反两个解
th1List = [atan2(wy, wx), atan2(-wy, -wx)];
}
// --- 分支 1：肩部姿态 ---
for (double th1 in th1List) {
double r = sqrt(rXySq);
// 如果肩部反向，r 取负值
if ((th1 - atan2(wy, wx)).abs() > 1e-4) {
r = -r;
}
double zPrime = wz - d1;
// ==========================================
// 肘部奇异点处理 (Elbow Singularity)： 可达空间的处理
// 1.如果超过外部可达空间： 无解
// 2.如果点位在于内部折叠导致的死区，大臂与小臂的差值：无解
// ==========================================
double sinTh3 =
(a2 * a2 + d4 * d4 - (r * r + zPrime * zPrime)) / (2 * a2 * d4);
// 数学钳位与物理越界拦截
if (sinTh3 > 1.0) {
if (sinTh3 > 1.001) continue; // 超出最大物理臂展过多，直接抛弃该解
sinTh3 = 1.0; // 浮点误差导致的边界奇异点，强制钳位，防止后续 acos 出现 NaN
} else if (sinTh3 < -1.0) {
if (sinTh3 < -1.001) continue; // 进入内侧极限折叠死区，抛弃该解
sinTh3 = -1.0; // 强制钳位
}
// --- 分支 2：肘部姿态 (2种: 肘朝上 / 肘朝下) ---
double cosTh3Base = sqrt(1 - sinTh3 * sinTh3);
List<double> cosTh3List = [cosTh3Base, -cosTh3Base];
for (double cosTh3 in cosTh3List) {
double th3 = atan2(sinTh3, cosTh3);
double A = a2 - d4 * sinTh3;
double B = d4 * cosTh3;
double th2 = atan2(A * zPrime - B * r, A * r + B * zPrime);
// 计算前置姿态 T03
List<List<double>> t1 = _mdhMatrix(0, 0, d1, th1);
List<List<double>> t2 = _mdhMatrix(pi / 2, 0, 0, th2);
List<List<double>> t3m = _mdhMatrix(0, a2, 0, th3);
List<List<double>> t03 = _multiply(_multiply(t1, t2), t3m);
List<List<double>> r03T = [
[t03[0][0], t03[1][0], t03[2][0]],
[t03[0][1], t03[1][1], t03[2][1]],
[t03[0][2], t03[1][2], t03[2][2]],
];
List<List<double>> rTarget = [
[r11, r12, r13],
[r21, r22, r23],
[r31, r32, r33],
];
List<List<double>> r36 = List.generate(3, (_) => List.filled(3, 0.0));
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
r36[i][j] += r03T[i][k] * rTarget[k][j];
}
}
}
double r36_12 = r36[1][2].clamp(-1.0, 1.0);
// --- 分支 3：腕部姿态 (2种: 姿态正向 / 姿态翻转) ---
double th5Base = acos(r36_12);
List<double> th5List = [th5Base, -th5Base];
for (double th5 in th5List) {
double th4, th6;
double sinTh5 = sin(th5);
// ==========================================
// 【修改】：腕部奇异点处理 (Wrist Singularity)
// ==========================================
if (sinTh5.abs() > 1e-4) {
// 正常姿态：使用 Dart 的 .sign 属性来获取正负符号 (1.0 或 -1.0)
double sign = sinTh5.sign;
th4 = atan2(r36[2][2] * sign, -r36[0][2] * sign);
th6 = atan2(-r36[1][1] * sign, r36[1][0] * sign);
} else {
// 奇异姿态 (Gimbal Lock)(第五轴角度为0时)：4轴和6轴重合
if (preJointRad != null && preJointRad.length >= 6) {
th4 = preJointRad[3]; // 核心：锁定 4轴 为上一帧的角度
th6 = atan2(-r36[0][1], r36[0][0]) - th4; // 让 6轴 承担所有剩余的旋转补偿
} else {
th4 = 0.0;
th6 = atan2(-r36[0][1], r36[0][0]);
}
}
// solutions.add([th1, th2 - pi / 2, th3 + pi / 2, th4, th5, th6]);
solutions.add(
[
th1,
th2 - pi / 2,
th3 + pi / 2,
th4,
th5,
th6,
].map((e) => normalizeAngle(e)).toList(),
);
}
}
}
return solutions;
}
得到以上函数，直接将其集成进flutter app中，并验证：
正解验证：白色小方块即是正解得出的值在空间的位置：
逆解验证：实现机械臂末端沿样条曲线运动的功能：
动力学模型
主要有两种方式： 拉格朗日能量法和递归牛顿欧拉算法（RENA）。在六轴机械臂中，一般选用RENA。这个算法需要两个信息：1. MDH参数表 2. 连杆质量，质心位置和惯性张量
第一个参数已经有了。第二个参数可以通过solidwors软件的“评估质量”功能获得。
特别需要注意的是：评估质量的坐标系需要和MDH参数表的坐标系完全一致。
由于我只有三个电机，所以连杆3开始的评估质量没有携带电机的质量。电机的位置和质量会影响质心和惯性。所以在此需要单独评估：
上方图中的质量只是软件估计的值，打开Bambu studio，去掉支撑，切片一下，即可得到打印件的质量：
将这六张图片和MDH参数交给Gemini，就可以生成出RENA函数了。
/// 递归牛顿-欧拉算法（RNEA）计算函数（无腕部电机轻量测试版）
///
/// 输入参数：
/// - [q]: 各关节角度，单位：弧度 (rad)
/// - [dq]: 各关节角速度，单位：rad/s
/// - [ddq]: 各关节角加速度，单位：rad/s^2
/// - [gBase]: 基座重力加速度向量，默认竖直向下 [0, 0, -9.81] m/s^2
///
/// 返回：各关节所需的驱动力矩 List<double>，单位：N·m
List<double> computeRNEA(
List<double> q,
List<double> dq,
List<double> ddq, {
Vector3? gBase,
}) {
final Vector3 gravity = gBase ?? Vector3(0.0, 0.0, -9.81);
const int n = 6;
// 初始化 6 个连杆的 MDH 参数与【不含腕部电机】的几何惯量数据
final List<LinkParams> links = [
LinkParams(
alphaPrev: 0.0,
aPrev: 0.0,
d: 0.107,
m: 0.4716,
rC: Vector3(0.0000, -0.0323, -0.0018),
),
LinkParams(
alphaPrev: math.pi / 2,
aPrev: 0.0,
d: 0.0,
// m: 0.5741,
m: 0.5395,
rC: Vector3(0.2362, 0.0000, 0.0897),
),
LinkParams(
alphaPrev: 0.0,
aPrev: 0.3,
d: 0.0,
// m: 0.1968,
m: 0.129,
rC: Vector3(0.0000, 0.0113, 0.0054),
),
LinkParams(
alphaPrev: -math.pi / 2,
aPrev: 0.0,
d: 0.32315,
// m: 0.3110,
m: 0.205,
rC: Vector3(-0.0055, 0.0324, -0.1524),
),
LinkParams(
alphaPrev: math.pi / 2,
aPrev: 0.0,
d: 0.0,
// m: 0.1120,
m: 0.09,
rC: Vector3(0.0000, 0.0098, -0.0047),
),
LinkParams(
alphaPrev: -math.pi / 2,
aPrev: 0.0,
d: 0.0,
// m: 0.0041,
m: 0.00,
rC: Vector3(0.0000, -0.0030, 0.0650),
),
];
// Link 1 原点惯量
links[0].iO.m[0][0] = 0.0013;
links[0].iO.m[1][1] = 0.0005;
links[0].iO.m[2][2] = 0.0013;
// Link 2 原点惯量
links[1].iO.m[0][0] = 0.0050;
links[1].iO.m[1][1] = 0.0430;
links[1].iO.m[2][2] = 0.0386;
links[1].iO.m[0][2] = -0.0120;
links[1].iO.m[2][0] = -0.0120;
// Link 3 原点惯量 (无电机)
links[2].iO.m[0][0] = 0.0006;
links[2].iO.m[1][1] = 0.0005;
links[2].iO.m[2][2] = 0.0005;
// Link 4 原点惯量 (无电机)
links[3].iO.m[0][0] = 0.0102;
links[3].iO.m[1][1] = 0.0097;
links[3].iO.m[2][2] = 0.0011;
links[3].iO.m[0][2] = 0.0004;
links[3].iO.m[2][0] = 0.0004;
links[3].iO.m[1][2] = -0.0008;
links[3].iO.m[2][1] = -0.0008;
// Link 5 原点惯量 (无电机)
links[4].iO.m[0][0] = 0.0002;
links[4].iO.m[1][1] = 0.0002;
links[4].iO.m[2][2] = 0.0002;
// Link 6 原点惯量 (保持为 0)
final List<Vector3> w = List.generate(n, (_) => Vector3());
final List<Vector3> dw = List.generate(n, (_) => Vector3());
final List<Vector3> a = List.generate(n, (_) => Vector3());
final List<Vector3> aC = List.generate(n, (_) => Vector3());
final List<Vector3> fC = List.generate(n, (_) => Vector3());
final List<Vector3> nC = List.generate(n, (_) => Vector3());
final List<Matrix3x3> rIPrev = List.generate(n, (_) => Matrix3x3());
Vector3 wPrev = Vector3(0, 0, 0);
Vector3 dwPrev = Vector3(0, 0, 0);
Vector3 aPrev = gravity * -1.0;
final Vector3 zHat = Vector3(0, 0, 1.0);
// ================= 1. 前向递推 (Forward Recursion) =================
for (int i = 0; i < n; i++) {
final double theta = q[i];
final double ct = math.cos(theta);
final double st = math.sin(theta);
final double ca = math.cos(links[i].alphaPrev);
final double sa = math.sin(links[i].alphaPrev);
final Matrix3x3 r = Matrix3x3();
r.m[0][0] = ct;
r.m[0][1] = st * ca;
r.m[0][2] = st * sa;
r.m[1][0] = -st;
r.m[1][1] = ct * ca;
r.m[1][2] = ct * sa;
r.m[2][0] = 0.0;
r.m[2][1] = -sa;
r.m[2][2] = ca;
rIPrev[i] = r;
final Vector3 wInI = r.multiplyVector(wPrev);
final Vector3 dwInI = r.multiplyVector(dwPrev);
w[i] = wInI + zHat * dq[i];
dw[i] = dwInI + wInI.cross(zHat * dq[i]) + zHat * ddq[i];
final Vector3 pInI = Vector3(
links[i].aPrev * ct,
-links[i].aPrev * st,
links[i].d,
);
a[i] =
r.multiplyVector(aPrev) +
dwInI.cross(pInI) +
wInI.cross(wInI.cross(pInI));
aC[i] =
a[i] + dw[i].cross(links[i].rC) + w[i].cross(w[i].cross(links[i].rC));
// 平行轴定理转换惯量张量
final Matrix3x3 iC = Matrix3x3();
final double m = links[i].m;
final double xc = links[i].rC.x;
final double yc = links[i].rC.y;
final double zc = links[i].rC.z;
iC.m[0][0] = links[i].iO.m[0][0] - m * (yc * yc + zc * zc);
iC.m[1][1] = links[i].iO.m[1][1] - m * (xc * xc + zc * zc);
iC.m[2][2] = links[i].iO.m[2][2] - m * (xc * xc + yc * yc);
iC.m[0][1] = links[i].iO.m[0][1] + m * xc * yc;
iC.m[1][0] = iC.m[0][1];
iC.m[0][2] = links[i].iO.m[0][2] + m * xc * zc;
iC.m[2][0] = iC.m[0][2];
iC.m[1][2] = links[i].iO.m[1][2] + m * yc * zc;
iC.m[2][1] = iC.m[1][2];
fC[i] = aC[i] * m;
nC[i] = iC.multiplyVector(dw[i]) + w[i].cross(iC.multiplyVector(w[i]));
wPrev = w[i];
dwPrev = dw[i];
aPrev = a[i];
}
// ================= 2. 后向递推 (Backward Recursion) =================
final List<double> tau = List.filled(n, 0.0);
Vector3 fNext = Vector3(0, 0, 0);
Vector3 nNext = Vector3(0, 0, 0);
for (int i = n - 1; i >= 0; i--) {
Vector3 fCurr = fC[i];
Vector3 nCurr = nC[i] + links[i].rC.cross(fC[i]);
if (i < n - 1) {
final Matrix3x3 rNextCurr = rIPrev[i + 1].transpose();
final Vector3 fNextInCurr = rNextCurr.multiplyVector(fNext);
final Vector3 nNextInCurr = rNextCurr.multiplyVector(nNext);
final Vector3 pNextInCurr = Vector3(
links[i + 1].aPrev,
-links[i + 1].d * math.sin(links[i + 1].alphaPrev),
links[i + 1].d * math.cos(links[i + 1].alphaPrev),
);
fCurr = fCurr + fNextInCurr;
nCurr = nCurr + nNextInCurr + pNextInCurr.cross(fNextInCurr);
}
tau[i] = nCurr.z;
fNext = fCurr;
nNext = nCurr;
}
return tau;
}
将其集成进flutter app验证一下，下图可看出在某个角度下，RENA计算出来的力矩：
将计算的数值应用到实际的机械臂中，做个‘零重力’模式验证一下：
结语
一路的过程挺麻烦的，到此要下笔了，却感觉没啥好写的。这是倒数第二篇，最后还有一篇，是关于vla和手眼标定，还没开始学，到时写完文章应该要明年了。
