用MATLAB实战二连杆动力学从建模到Newton-Euler算法全解析在机器人动力学研究中二连杆系统是最基础却最具教学意义的模型之一。它如同动力学领域的Hello World既能完整呈现多体系统动力学的核心概念又避免了过于复杂的数学表达。对于机械工程、自动化专业的学生和工程师而言掌握二连杆动力学不仅能够夯实理论基础更能为后续研究更复杂的机械臂系统打下坚实基础。MATLAB作为工程计算的标准工具其符号计算和矩阵运算能力特别适合动力学算法的实现与验证。本文将带领读者从零开始通过MDHModified Denavit-Hartenberg和SDHStandard Denavit-Hartenberg两种建模方法完整实现二连杆系统的Newton-Euler动力学计算。不同于单纯的理论推导我们将重点关注如何将数学公式转化为可执行的MATLAB代码并解释每一行代码背后的物理意义。1. 动力学基础与环境准备1.1 二连杆系统的基本假设在开始建模前我们需要明确几个基本假设这些假设将直接影响后续的数学模型和计算复杂度质量集中假设将连杆的质量视为集中在末端点相当于把连杆简化为无质量的杆件加上末端质点。这一假设大大简化了惯性张量的计算。平面运动限制系统仅在二维平面内运动重力沿y轴负方向作用。刚性连杆不考虑连杆的弹性变形。理想关节忽略关节摩擦和间隙等非线性因素。这些假设使得我们可以专注于动力学算法的核心流程而不被复杂的实际情况所干扰。在实际工程中当这些简化假设不成立时可以采用更精细的模型进行修正。1.2 MATLAB环境配置为了运行后续的动力学仿真代码需要确保MATLAB安装了以下工具包% 检查必要工具包是否安装 toolboxes ver; hasSymbolic any(strcmp({toolboxes.Name}, Symbolic Math Toolbox)); hasRobotics any(strcmp({toolboxes.Name}, Robotics System Toolbox)); if ~hasSymbolic error(需要安装Symbolic Math Toolbox以进行符号计算); end % 虽然不是必须的但Robotics Toolbox提供了有用的辅助函数 if hasRobotics disp(检测到Robotics System Toolbox将使用其中的变换函数); else disp(未检测到Robotics System Toolbox将使用自定义变换函数); end对于没有Robotics System Toolbox的用户我们需要自定义一些基本的空间变换函数。以下是两个关键函数的实现function [R, p] tr2rt(T) % 从齐次变换矩阵提取旋转矩阵和位置向量 R T(1:3,1:3); p T(1:3,4); end function S skew(v) % 向量到反对称矩阵的转换 S [0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0]; end2. MDH参数建模与实现2.1 MDH参数表建立Modified Denavit-HartenbergMDH参数法是一种广泛使用的机器人建模方法。对于我们的二连杆系统MDH参数表可以如下定义连杆α (rad)a (m)θ (rad)d (m)100q1020a1q20末端0a200在MATLAB中我们可以用符号变量表示这些参数syms q1 q2 qp1 qp2 qpp1 qpp2 a1 a2 m1 m2 g real; DH [0 0 q1 0; % 连杆1 0 a1 q2 0; % 连杆2 0 a2 0 0]; % 末端2.2 正向运动学计算基于MDH参数我们可以计算各连杆间的变换矩阵。MDH的变换矩阵公式为for i 1:size(DH,1) alpha DH(i,1); a DH(i,2); theta DH(i,3); d DH(i,4); T{i} [cos(theta) -sin(theta) 0 a; sin(theta)*cos(alpha) cos(theta)*cos(alpha) -sin(alpha) -d*sin(alpha); sin(theta)*sin(alpha) cos(theta)*sin(alpha) cos(alpha) d*cos(alpha); 0 0 0 1]; end2.3 速度与加速度递推Newton-Euler算法的核心在于速度/加速度的正向递推和力的反向递推。正向递推从基座开始逐步计算各连杆的角速度、角加速度和线加速度% 初始化基座速度和加速度 w{1} [0;0;0]; dotw{1} [0;0;0]; dotv{1} [0;g;0]; % 正向速度递推 for i 1:size(q,1) [R_prev, ~] tr2rt(T{i}); w{i1} R_prev * w{i} qp(i)*[0;0;1]; dotw{i1} R_prev * dotw{i} R_prev * skew(w{i}) * qp(i)*[0;0;1] qpp(i)*[0;0;1]; dotv{i1} R_prev * (skew(dotw{i})*r{i} skew(w{i})*skew(w{i})*r{i} dotv{i}); dotvc{i1} skew(dotw{i1})*Tcm{i}(1:3,4) skew(w{i1})*skew(w{i1})*Tcm{i}(1:3,4) dotv{i1}; end2.4 反向力递推反向递推从末端开始计算各关节所受的力和力矩% 末端力/力矩初始化 f{size(q,1)1} [0;0;0]; n_f{size(q,1)1} [0;0;0]; for i size(q,1):-1:1 [R_next, ~] tr2rt(T{i1}); f{i} R_next * f{i1} m(i)*dotvc{i1}; n_f{i} Ic{i}*dotw{i1} skew(w{i1})*Ic{i}*w{i1} ... R_next * n_f{i1} skew(Tcm{i}(1:3,4))*m(i)*dotvc{i1} ... skew(r{i1})*R_next*f{i1}; n_f{i} simplify(n_f{i}); end3. SDH参数建模与实现3.1 SDH与MDH的参数差异Standard Denavit-HartenbergSDH参数法与MDH的主要区别在于坐标系附着方式和参数定义。对于同一二连杆系统SDH参数表为连杆α (rad)a (m)θ (rad)d (m)10a1q1020a2q20MATLAB中的实现差异主要体现在变换矩阵的计算上% SDH变换矩阵计算 for i 1:size(DH,1) alpha DH(i,1); a DH(i,2); theta DH(i,3); d DH(i,4); T{i1} [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]; end3.2 SDH下的递推算法调整SDH方法下的速度递推需要特别注意坐标系变换的顺序% SDH正向速度递推 for i 1:size(q,1) [R_prev_to_next, ~] tr2rt(T{i1}); w{i1} R_prev_to_next * w{i} qp(i)*[0;0;1]; dotw{i1} R_prev_to_next * (dotw{i} skew(w{i})*qp(i)*[0;0;1]) qpp(i)*[0;0;1]; dotv{i1} R_prev_to_next * dotv{i} skew(dotw{i1})*r{i} skew(w{i1})*skew(w{i1})*r{i}; end4. 两种建模方法的对比与分析4.1 计算结果的等价性尽管MDH和SDH的建模过程有所不同但最终得到的动力学方程应当是一致的。通过MATLAB的符号计算我们可以验证这一点% 比较MDH和SDH的关节力矩结果 diff_n1 simplify(n_f_MDH{1} - n_f_SDH{1}); diff_n2 simplify(n_f_MDH{2} - n_f_SDH{2}); disp([关节1力矩差异 char(diff_n1)]); disp([关节2力矩差异 char(diff_n2)]);理论上上述差异应当为零向量验证了两种方法的等价性。4.2 实现复杂度比较从实现角度看两种方法各有特点MDH优势坐标系附着更直观特别是对于串联机器人递推公式形式更统一更适合教学和理解SDH优势历史更悠久更多经典教材采用某些情况下参数更简洁部分商业软件采用SDH标准4.3 数值验证案例为了进一步验证我们的符号推导可以代入具体数值进行计算% 参数赋值 params [a1 a2 m1 m2 g]; values [1.0 1.0 1.0 1.0 9.8]; % 单位m, kg, m/s² % 运动状态赋值 q_val [pi/4 pi/3]; % 关节角度 qp_val [0.5 -0.3]; % 关节速度 qpp_val [0.1 0.2]; % 关节加速度 % 计算具体力矩值 tau_MDH subs([n_f_MDH{1}(3); n_f_MDH{2}(3)], ... [q1 q2 qp1 qp2 qpp1 qpp2 params], ... [q_val qp_val qpp_val values]); disp(MDH计算的关节力矩); disp(double(tau_MDH));5. 扩展与应用5.1 从二连杆到多连杆系统掌握了二连杆系统的动力学计算后扩展到n连杆系统主要涉及参数表的扩展为每个新增连杆添加一行DH参数递推循环的扩展将固定循环次数改为基于连杆数量的变量惯性参数的完善考虑连杆的完整惯性张量而非质点近似5.2 计算效率优化当需要高频计算动力学时可以考虑以下优化策略代码生成利用MATLAB Coder将符号表达式转换为C代码并行计算某些递推步骤可以并行化简化模型在精度允许的情况下减少自由度% 代码生成示例需要MATLAB Coder func matlabFunction([n_f{1}(3); n_f{2}(3)], ... Vars, {[q1;q2], [qp1;qp2], [qpp1;qpp2], [a1;a2], [m1;m2], g}, ... Outputs, {tau});5.3 实际工程应用在实际机器人控制中Newton-Euler算法常用于动态模型前馈提高轨迹跟踪精度力矩控制直接控制关节力矩而非位置/速度系统辨识通过测量数据估计惯性参数仿真验证验证控制算法的性能在工业机器人、外骨骼、航天机械臂等领域这些应用尤为重要。例如在高速拾放作业中考虑动力学效应的控制可以显著提高定位精度和节拍时间。
手把手教你用MATLAB验证二连杆动力学：从MDH/SDH建模到Newton-Euler公式完整流程
发布时间：2026/5/15 14:45:35
用MATLAB实战二连杆动力学从建模到Newton-Euler算法全解析在机器人动力学研究中二连杆系统是最基础却最具教学意义的模型之一。它如同动力学领域的Hello World既能完整呈现多体系统动力学的核心概念又避免了过于复杂的数学表达。对于机械工程、自动化专业的学生和工程师而言掌握二连杆动力学不仅能够夯实理论基础更能为后续研究更复杂的机械臂系统打下坚实基础。MATLAB作为工程计算的标准工具其符号计算和矩阵运算能力特别适合动力学算法的实现与验证。本文将带领读者从零开始通过MDHModified Denavit-Hartenberg和SDHStandard Denavit-Hartenberg两种建模方法完整实现二连杆系统的Newton-Euler动力学计算。不同于单纯的理论推导我们将重点关注如何将数学公式转化为可执行的MATLAB代码并解释每一行代码背后的物理意义。1. 动力学基础与环境准备1.1 二连杆系统的基本假设在开始建模前我们需要明确几个基本假设这些假设将直接影响后续的数学模型和计算复杂度质量集中假设将连杆的质量视为集中在末端点相当于把连杆简化为无质量的杆件加上末端质点。这一假设大大简化了惯性张量的计算。平面运动限制系统仅在二维平面内运动重力沿y轴负方向作用。刚性连杆不考虑连杆的弹性变形。理想关节忽略关节摩擦和间隙等非线性因素。这些假设使得我们可以专注于动力学算法的核心流程而不被复杂的实际情况所干扰。在实际工程中当这些简化假设不成立时可以采用更精细的模型进行修正。1.2 MATLAB环境配置为了运行后续的动力学仿真代码需要确保MATLAB安装了以下工具包% 检查必要工具包是否安装 toolboxes ver; hasSymbolic any(strcmp({toolboxes.Name}, Symbolic Math Toolbox)); hasRobotics any(strcmp({toolboxes.Name}, Robotics System Toolbox)); if ~hasSymbolic error(需要安装Symbolic Math Toolbox以进行符号计算); end % 虽然不是必须的但Robotics Toolbox提供了有用的辅助函数 if hasRobotics disp(检测到Robotics System Toolbox将使用其中的变换函数); else disp(未检测到Robotics System Toolbox将使用自定义变换函数); end对于没有Robotics System Toolbox的用户我们需要自定义一些基本的空间变换函数。以下是两个关键函数的实现function [R, p] tr2rt(T) % 从齐次变换矩阵提取旋转矩阵和位置向量 R T(1:3,1:3); p T(1:3,4); end function S skew(v) % 向量到反对称矩阵的转换 S [0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0]; end2. MDH参数建模与实现2.1 MDH参数表建立Modified Denavit-HartenbergMDH参数法是一种广泛使用的机器人建模方法。对于我们的二连杆系统MDH参数表可以如下定义连杆α (rad)a (m)θ (rad)d (m)100q1020a1q20末端0a200在MATLAB中我们可以用符号变量表示这些参数syms q1 q2 qp1 qp2 qpp1 qpp2 a1 a2 m1 m2 g real; DH [0 0 q1 0; % 连杆1 0 a1 q2 0; % 连杆2 0 a2 0 0]; % 末端2.2 正向运动学计算基于MDH参数我们可以计算各连杆间的变换矩阵。MDH的变换矩阵公式为for i 1:size(DH,1) alpha DH(i,1); a DH(i,2); theta DH(i,3); d DH(i,4); T{i} [cos(theta) -sin(theta) 0 a; sin(theta)*cos(alpha) cos(theta)*cos(alpha) -sin(alpha) -d*sin(alpha); sin(theta)*sin(alpha) cos(theta)*sin(alpha) cos(alpha) d*cos(alpha); 0 0 0 1]; end2.3 速度与加速度递推Newton-Euler算法的核心在于速度/加速度的正向递推和力的反向递推。正向递推从基座开始逐步计算各连杆的角速度、角加速度和线加速度% 初始化基座速度和加速度 w{1} [0;0;0]; dotw{1} [0;0;0]; dotv{1} [0;g;0]; % 正向速度递推 for i 1:size(q,1) [R_prev, ~] tr2rt(T{i}); w{i1} R_prev * w{i} qp(i)*[0;0;1]; dotw{i1} R_prev * dotw{i} R_prev * skew(w{i}) * qp(i)*[0;0;1] qpp(i)*[0;0;1]; dotv{i1} R_prev * (skew(dotw{i})*r{i} skew(w{i})*skew(w{i})*r{i} dotv{i}); dotvc{i1} skew(dotw{i1})*Tcm{i}(1:3,4) skew(w{i1})*skew(w{i1})*Tcm{i}(1:3,4) dotv{i1}; end2.4 反向力递推反向递推从末端开始计算各关节所受的力和力矩% 末端力/力矩初始化 f{size(q,1)1} [0;0;0]; n_f{size(q,1)1} [0;0;0]; for i size(q,1):-1:1 [R_next, ~] tr2rt(T{i1}); f{i} R_next * f{i1} m(i)*dotvc{i1}; n_f{i} Ic{i}*dotw{i1} skew(w{i1})*Ic{i}*w{i1} ... R_next * n_f{i1} skew(Tcm{i}(1:3,4))*m(i)*dotvc{i1} ... skew(r{i1})*R_next*f{i1}; n_f{i} simplify(n_f{i}); end3. SDH参数建模与实现3.1 SDH与MDH的参数差异Standard Denavit-HartenbergSDH参数法与MDH的主要区别在于坐标系附着方式和参数定义。对于同一二连杆系统SDH参数表为连杆α (rad)a (m)θ (rad)d (m)10a1q1020a2q20MATLAB中的实现差异主要体现在变换矩阵的计算上% SDH变换矩阵计算 for i 1:size(DH,1) alpha DH(i,1); a DH(i,2); theta DH(i,3); d DH(i,4); T{i1} [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]; end3.2 SDH下的递推算法调整SDH方法下的速度递推需要特别注意坐标系变换的顺序% SDH正向速度递推 for i 1:size(q,1) [R_prev_to_next, ~] tr2rt(T{i1}); w{i1} R_prev_to_next * w{i} qp(i)*[0;0;1]; dotw{i1} R_prev_to_next * (dotw{i} skew(w{i})*qp(i)*[0;0;1]) qpp(i)*[0;0;1]; dotv{i1} R_prev_to_next * dotv{i} skew(dotw{i1})*r{i} skew(w{i1})*skew(w{i1})*r{i}; end4. 两种建模方法的对比与分析4.1 计算结果的等价性尽管MDH和SDH的建模过程有所不同但最终得到的动力学方程应当是一致的。通过MATLAB的符号计算我们可以验证这一点% 比较MDH和SDH的关节力矩结果 diff_n1 simplify(n_f_MDH{1} - n_f_SDH{1}); diff_n2 simplify(n_f_MDH{2} - n_f_SDH{2}); disp([关节1力矩差异 char(diff_n1)]); disp([关节2力矩差异 char(diff_n2)]);理论上上述差异应当为零向量验证了两种方法的等价性。4.2 实现复杂度比较从实现角度看两种方法各有特点MDH优势坐标系附着更直观特别是对于串联机器人递推公式形式更统一更适合教学和理解SDH优势历史更悠久更多经典教材采用某些情况下参数更简洁部分商业软件采用SDH标准4.3 数值验证案例为了进一步验证我们的符号推导可以代入具体数值进行计算% 参数赋值 params [a1 a2 m1 m2 g]; values [1.0 1.0 1.0 1.0 9.8]; % 单位m, kg, m/s² % 运动状态赋值 q_val [pi/4 pi/3]; % 关节角度 qp_val [0.5 -0.3]; % 关节速度 qpp_val [0.1 0.2]; % 关节加速度 % 计算具体力矩值 tau_MDH subs([n_f_MDH{1}(3); n_f_MDH{2}(3)], ... [q1 q2 qp1 qp2 qpp1 qpp2 params], ... [q_val qp_val qpp_val values]); disp(MDH计算的关节力矩); disp(double(tau_MDH));5. 扩展与应用5.1 从二连杆到多连杆系统掌握了二连杆系统的动力学计算后扩展到n连杆系统主要涉及参数表的扩展为每个新增连杆添加一行DH参数递推循环的扩展将固定循环次数改为基于连杆数量的变量惯性参数的完善考虑连杆的完整惯性张量而非质点近似5.2 计算效率优化当需要高频计算动力学时可以考虑以下优化策略代码生成利用MATLAB Coder将符号表达式转换为C代码并行计算某些递推步骤可以并行化简化模型在精度允许的情况下减少自由度% 代码生成示例需要MATLAB Coder func matlabFunction([n_f{1}(3); n_f{2}(3)], ... Vars, {[q1;q2], [qp1;qp2], [qpp1;qpp2], [a1;a2], [m1;m2], g}, ... Outputs, {tau});5.3 实际工程应用在实际机器人控制中Newton-Euler算法常用于动态模型前馈提高轨迹跟踪精度力矩控制直接控制关节力矩而非位置/速度系统辨识通过测量数据估计惯性参数仿真验证验证控制算法的性能在工业机器人、外骨骼、航天机械臂等领域这些应用尤为重要。例如在高速拾放作业中考虑动力学效应的控制可以显著提高定位精度和节拍时间。
