简介：机器人运动学建模是连接理论推导与工程实现的桥梁。以经典教材《机器人建模和控制》的习题为切入点，从标准DH参数建立齐次变换矩阵，到正运动学与基于雅可比矩阵的数值逆运动学求解，再延伸至牛顿-欧拉递推与拉格朗日动力学方程，最后落脚于关节空间PD控制与重力补偿。通过MATLAB仿真逐层验证推导过程，既能加深对运动学和动力学原理的理解，也能为真实机械臂的轨迹跟踪和控制部署提供实践参考。这些经验覆盖了从符号约定混淆、惯性矩阵不正定到重力补偿项符号错误等常见踩坑点，帮助读者真正把书本知识转化为工程能力。
1. 《机器人建模和控制》习题答案：不是抄作业，是把推导变成能跑的代码
在机器人运动学建模这条路上，很多人卡住的不是教材正文，而是课后习题——这些题表面上让你推导雅可比矩阵、算拉格朗日动力学项、调一个 PD 控制参数，实际上是在逼你把数学符号变成能跑的 MATLAB 代码，再对着仿真结果检验每一步是否正确。这本经典教材的习题答案，真正值钱的地方不是那个最终数值，而是推导过程中每一步的物理含义和代码对应关系。我见过不少读者把参考答案抄了一遍、考试也能画出来，但让他把一本书里的二连杆动力学模型移植到 STM32 上驱动舵机 PWM 控制时完全不知道从哪里下手。这篇笔记就是沿着这本书的习题主线，把该有的建模、仿真、控制和踩坑经验一次说透。
2. 机器人运动学建模与 MATLAB 仿真：从 DH 参数到正解代码落地
2.1 标准 DH 表：写变换矩阵前必须明确的四个约定
运动学建模的第一步不是敲代码，而是确定 DH 参数的约定方式。做机器人运动学建模题时，最常见的坑是拿到一个机器人结构图就急着写theta、d、a、alpha四列，结果坐标系的建立方式和教材的约定不一致，后面所有矩阵全部错位。
我一般会在动手前先问自己三个问题：这个机械臂有几个转动关节、几个移动关节？关节轴线的方向是否平行？基座坐标系的原点放在哪里？这些问题直接决定 DH 表怎么写。在标准 DH（SDH）约定下，a_i表示沿x_i轴方向从z_{i-1}移动到z_i的距离，alpha_i表示绕x_i轴从z_{i-1}旋转到z_i的角度；而改进 DH（MDH）约定的平移和旋转顺序完全不同，两套参数不能混用。
写代码时，我建议把 DH 参数直接定义成一个n x 4的矩阵，每一行对应一个关节：
% dh_table: 每行为 [a_i, alpha_i, d_i, theta_i_offset] % theta_offset 是关节变量为0时的初始角度偏移 dh_table = [ 0.0, pi/2, 0.2, 0.0; 0.4, 0.0, 0.0, -pi/2; 0.25, 0.0, 0.0, pi/4; ];这里的theta_i通常作为关节变量，实际角度等于q(i) + theta_offset。把偏移量和变量分离，是为了后续做逆运动学时方便对关节角度求偏导，也避免在每一次正解计算时去改 DH 表本身。
构建单个变换矩阵的函数建议独立成文件：
function T = dh_transform(a, alpha, d, theta) % SDH 标准DH约定下的单关节齐次变换矩阵 T = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta); sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta); 0, sin(alpha), cos(alpha), d; 0, 0, 0, 1]; end这个函数里最容易写错的是第二行第三列的符号。很多人在推导时把sin(theta)*cos(alpha)的负号位置弄反，导致绕x轴旋转部分出现符号错误。对照书本里的旋转矩阵分解，R = Rz(theta) * Rx(alpha)，展开后第二行第三列一定是-cos(theta)*sin(alpha)，不是cos(theta)*sin(alpha)。
2.2 正运动学计算：手写乘法和工具箱对比的差异
正运动学就是把每个关节的变换矩阵按顺序连乘，得到末端执行器相对于基座的位姿。在做习题时，我倾向于先手写矩阵乘法，再用 MATLAB 的 Robotics Toolbox 验证结果，而不是一开始就依赖fkine函数。
function T_end = fkine_total(dh_table, q_offset) % 输入 DH 表和关节变量，返回末端齐次变换矩阵 T_end = eye(4); n = size(dh_table, 1); for i = 1:n a_i = dh_table(i, 1); alpha_i = dh_table(i, 2); d_i = dh_table(i, 3); theta_i = q_offset(i) + dh_table(i, 4); T_i = dh_transform(a_i, alpha_i, d_i, theta_i); T_end = T_end * T_i; end end这里的连乘顺序绝对不能反。T_end * T_i表示在基坐标系下依次左乘每个关节的变换矩阵，而T_i * T_end则是相对于上一关节坐标系进行计算，结果完全不同。做完这一步之后，可以把末端位置T_end(1:3, 4)和姿态矩阵T_end(1:3, 1:3)打印出来，再和 Robotics Toolbox 的fkine输出对比。
对比结果不一致时，不要急着怀疑工具箱有 bug，先检查自己的 DH 约定是否和工具箱内置模型一致。Robotics Toolbox 默认的SerialLink模型很多使用改进 DH，如果教材用的是标准 DH，那同一组参数得到的末端位姿必然对不上。
2.3 逆运动学数值求解：雅可比迭代和奇异位形下的阻尼策略
逆运动学的解析解只在少数几何结构简单的机械臂上存在，比如前三个关节轴线交于一点的球形手腕结构。习题里一旦出现不规则构型，解析推导就会变得非常繁琐，这时候用数值迭代法更实际。
我的做法是基于雅可比矩阵做阻尼最小二乘迭代，把末端位姿误差映射到关节空间。代码结构如下：
function q_sol = ik_dls(T_desired, q_init, dh_table) q = q_init; lambda = 0.02; % 阻尼系数，抑制奇异附近的大步长 max_iter = 200; tol = 1e-8; for k = 1:max_iter T_cur = fkine_total(dh_table, q); e_pos = T_desired(1:3, 4) - T_cur(1:3, 4); e_rot = 0.5 * vex(T_desired(1:3,1:3) * T_cur(1:3,1:3)' - ... T_cur(1:3,1:3) * T_desired(1:3,1:3)'); e = [e_pos; e_rot]; if norm(e) < tol break; end J = jacobian_numeric(dh_table, q); delta_q = (J'*J + lambda^2*eye(6)) \ (J' * e); q = q + delta_q; end q_sol = q; end这里的vex函数把反对称矩阵还原成三维旋转向量，是姿态误差计算的标准操作。阻尼系数lambda的选取很关键，太大会让迭代收敛速度极慢，太小又会在机械臂接近奇异位形时让关节速度突变。
如果不想用数值差分算雅可比，可以直接用解析法建立雅可比矩阵，把每个关节轴的单位向量和作用点到末端的向量算出来，这样迭代速度更快、精度也更高。习题中一般只需要验证逆解的正确性，数值雅可比已经足够，但是走向工程落地时，解析雅可比才是首选。
2.4 用轨迹跟随验证正逆解：关节空间与笛卡尔空间的关系
正解和逆解写完之后，最有效的验证方式不是单点测试，而是让机械臂末端沿一条笛卡尔空间直线运动，把逆解算出的关节轨迹送回正解，看末端位置误差能否控制在毫米级以内。
% 笛卡尔空间直线轨迹采样 t = linspace(0, 1, 20); x_start = [0.3; 0.2; 0.1]; x_end = [0.5; -0.1; 0.4]; q = q_init; traj_pos_error = zeros(length(t), 1); for idx = 1:length(t) x_des = x_start + (x_end - x_start) * t(idx); T_des = eye(4); T_des(1:3, 4) = x_des; q = ik_dls(T_des, q, dh_table); T_cur = fkine_total(dh_table, q); traj_pos_error(idx) = norm(T_cur(1:3, 4) - x_des); end plot(t, traj_pos_error * 1000, 'LineWidth', 1.5);这一步做完，运动学部分的习题就算真正落地了。后面接动力学和控制时，正逆解会作为底层模块反复被调用，所以这里的验证越严格，后面的仿真调试越省力。
3. 动力学与控制联调：从牛顿-欧拉递推到关节 PD 控制的逐层落实
3.1 牛顿-欧拉递推：外推和内推的索引对齐问题
动力学建模有两条主流路线：牛顿-欧拉递推和拉格朗日方程。习题里要求推导动力学方程时，拉格朗日法更直观，但要在 MATLAB 里实现 n 自由度的递推动力学，牛顿-欧拉法在计算效率上明显占优。
牛顿-欧拉法分为两步：从基座到末端的外推，计算每个连杆的角速度、角加速度和线加速度；然后从末端到基座的内推，计算每个关节上的力和力矩。代码实现的经典坑是数组索引错位——外推时用的是第i个连杆的速度，内推时却把第i+1个连杆的力加到第i个关节上。
function tau = ne_dynamics(dh_table, q, qd, qdd, link_mass, link_inertia) n = size(dh_table, 1); omega = zeros(3, n); alpha = zeros(3, n); vdot = zeros(3, n); v = zeros(3, n); F = zeros(3, n); N_moment = zeros(3, n); tau = zeros(n, 1); % 外推：计算各连杆速度和加速度 for i = 1:n % 这里需要从DH表提取旋转轴和位置信息 z_axis = ...; % 根据DH参数计算旋转轴方向 omega(:, i) = ...; % 递推公式 end % 内推：从末端连杆开始计算力和力矩 for i = n:-1:1 F(:, i) = link_mass(i) * vdot(:, i); N_moment(:, i) = link_inertia(:, :, i) * alpha(:, i) + ...; tau(i) = ...; % 投影到关节轴上 end end这道题的难点主要是把书本上的矢量公式转化为计算机可执行的形式，尤其是各个量所在的坐标系要统一。我一般会先在纸上画一棵两连杆的树形结构图，把每一段递推符号在图上标注完再写程序，宁可多花半小时画图，也不要在调试时对着报错信息猜索引从 0 还是从 1 开始。
3.2 拉格朗日法与惯性矩阵 M(q) 的装配验证
拉格朗日方程推导出的动力学方程通常写成M(q)qdd + C(q, qd)qd + G(q) = tau的形式。习题里经常要求计算特定构型下的M(q)矩阵，然后再代入数值验证。
在 MATLAB 里装配M(q)的一个直接办法是基于动能公式T = 0.5 * qd' * M * qd反推，但更通用的做法是建立一个符号表达式，把每个连杆的动能累加起来，然后对qdd求偏导。这里有一个屡试不爽的验证手段：对于任意关节速度向量qd，动能必须是非负的，也就是说M(q)至少是半正定矩阵。如果算出的M(q)存在负的特征值，那动力学方程大概率有误。
syms q1 q2 qd1 qd2 real % 两连杆的动能表达式 T_total = 0.5*(m1*qd1^2 + 2*cos(q2)*qd1*qd2 + m2*qd2^2); M_sym = [diff(T_total, qd1, 2), diff(diff(T_total, qd1), qd2); diff(diff(T_total, qd2), qd1), diff(T_total, qd2, 2)]; M_num = matlabFunction(M_sym);把符号计算结果代入几组随机角度，检查M是否正定，再看M的对称性是否满足，这两关过了，惯性矩阵这部分基本就不会有大的逻辑问题。
3.3 关节空间 PD 控制：重力补偿项到底补什么
动力学方程写好后，最常见的课后习题就是设计关节空间 PD 控制器，让机械臂稳定跟踪一个期望关节角轨迹。控制律的基本形式是tau = Kp*e + Kd*ed + G(q)，其中G(q)是重力补偿项。
很多初学者会在这一步踩坑：不加重力补偿时，系统在平衡点附近会存在稳态误差，这是控制器在对抗重力时力不够导致的；加了重力补偿却把补偿项符号搞反，结果误差更大。判断补偿项是否正确的简单办法是把仿真停在期望位置不动，看各个关节力矩命令是否刚好等于重力力矩。
function tau = pd_control_with_gravity(q_d, qd_d, q, qd, Kp, Kd, g_gravity) e = q_d - q; ed = qd_d - qd; tau = g_gravity(q) + Kp * e + Kd * ed; end仿真中的参数设定也有技巧。Kp和Kd的比例要满足近似临界阻尼的关系，也就是Kd大约等于2*sqrt(Kp)的量级，而不是随意放两个数进去。用这个经验值起步，再微调Kp看系统响应速度和超调量的变化，比盲目搜索参数效率高得多。
3.4 PID 控制与 FOC 无感控制的思想关联：控制器设计为何要分层
做完关节空间 PD 仿真，值得停下来想一想：为什么实际工业机器人里几乎不用一个单层 PD 包打天下，而是要在电流环、速度环、位置环里做级联 PID？这和近期大家讨论的 PMSM 无感 FOC 控制其实是一套逻辑——底层电流环控制电机力矩，中层速度环控制转速，顶层位置环控制关节角度。
在做仿真习题时，把关节空间 PD 直接当作顶层位置环，把电机模型当作被控对象，就会发现在力矩饱和、参数不确定性存在时，单环控制的鲁棒性并不够。这时把仿真里多出来的状态量（比如电机电流或者关节速度）再设计一个内环，控制效果会明显改善。这个分层思路在习题答案里不一定会直接给出来，但工程上几乎处处用到。
4. 习题答案排查手册：仿真失败和实验结果对不上的 5 个踩坑实录
4.1 SDH 与 MDH 符号混淆导致的正解差一个符号
现象：用标准 DH 写程序，和 Robotics Toolbox 的fkine结果对比，末端位置差得不多，但姿态矩阵有几项正负号完全相反。
原因：DH 参数物理意义不同。同一组四元参数，SDH 和 MDH 对应的坐标系变换顺序不同，导致旋转矩阵里的符号分布不同。
解决：在代码注释里明确标注自己采用的是哪套约定，并且在验证阶段用书本上已知的简单构型（比如只有一个旋转关节的连杆）做单关节测试。先把单关节的正解和手算结果对齐，再去算多关节机械臂。
4.2 动力学仿真初始化即发散，惯性矩阵不正定
现象：跑 0.5 秒就报数值错误，qdd变得极大，系统直接发散。
原因：装配出的M(q)不是正定矩阵，或者递推过程中连杆质量、惯性张量数据不一致，导致仿真的能量意涵错误。
解决：先用符号法求出M(q)，检查对称性和特征值。最常见的翻车点是连杆惯性张量写成了基坐标系下的值，而没有转换到质心坐标系；或者质量密度参数单位不统一，几个连杆的质量差了三个数量级。把这些数据逐项打印出来对比，很快就会发现异常位置。
4.3 PD 控制稳态误差一直存在，重力补偿符号反了
现象：关节角度最终稳定在期望值附近但始终差几度，加大Kp只能减小误差却始终消不掉。
原因：仿真模型里包含重力项，而控制律中要么没有加重力补偿，要么补偿项的符号恰好反了。
解决：把仿真中的速度、加速度全部设为 0，只在某一位形下单独检查控制力矩是否等于重力矩。画一个简单判断：期望角度比当前角度大，末端要往上抬，重力矩方向是向下的，补偿项必须和重力矩方向相反才能抵消。
4.4 逆运动学迭代发散，初始猜测和阻尼系数不匹配
现象：迭代到第几步时delta_q突然增大，q直接跳到关节限位之外。
原因：机械臂处于奇异位形附近时，雅可比矩阵接近病态，伪逆放大了末端微小误差，产生大步长关节增量。
解决：给迭代加入阻尼项，lambda至少要从0.01起步；每次迭代后检查关节是否越界，越界则截断到限位值附近重新迭代。另外，初始猜测不要随机取，上一时刻的关节位置在轨迹跟踪中是最自然的初值。
4.5 仿真结果在真实电机上无法复现，控制力矩饱和未建模
现象：仿真里关节跟踪误差完美，但把控制律移植到 STM32 控制的伺服电机上，电机根本跟不上指令，频繁抖动甚至过流报警。
原因：真实电机有最大力矩限制，仿真里的理想执行器没有力矩饱和，导致控制律命令的力矩值超出物理极限。
解决：在仿真里给tau加上max_torque饱和函数，再看控制性能。这一步会让很多人恍然大悟——许多冠以“习题答案”的控制参数，在真实设备上根本没有工程可行性。如果你后续要接 STM32 或舵机 PWM 控制，这一步是必须做的。
5. 验证习题答案的进阶习惯：从离线仿真走向在线控制的最后一公里
习题做完不等于知识点过关，我习惯用三件套来验证自己的答案：代数验证、轨迹验证、硬件对比。代数验证是指把运动学正解反代回逆解程序，看得到的位置是否一致；轨迹验证是指让末端走一条包含加速、减速和圆弧转折的连续轨迹，统计全程跟踪误差；硬件对比则是把控制律部署到真实平台上，哪怕是便宜的两连杆套件和舵机，也能立刻暴露仿真中忽略的摩擦力、惯量不确定性和通信延迟问题。
在写控制代码时，我从一开始就会加一个手动可调的 “torque scaling” 系数，把控制器输出统一乘上一个 0 到 1 的系数，先在数值模型上把系数调到 0.3，验证稳定性，再逐步提高。这种方式能避免超调。遇到调试困难时，先固化所有随机项，把问题缩小到运动学或动力学单一环节再排查。
这套从习题到仿真再到硬件的流程，我保持了很多年。现在每拿到一份新的机械臂模型，还是会从 DH 表开始手写一遍正运动学，不会直接调用现成的函数。因为黑匣子可以帮你得到结果，却不能帮你在出错时定位问题。希望这些踩坑经验能帮你少走一段弯路。
本文还有配套的精品资源，点击获取
