用Python/Matlab动手推导牛顿-欧拉方程从理论到代码的完整实践刚体动力学是机器人学和控制工程中的核心课题而牛顿-欧拉方程则是描述刚体运动的黄金标准。但面对复杂的矢量运算和旋转矩阵推导许多工程师和学生常常感到无从下手。本文将带你用Python和Matlab这两种工具通过编写可执行代码来直观理解这些抽象概念。1. 准备工作与环境搭建在开始推导之前我们需要准备好计算工具。Python的NumPy和SymPy库以及Matlab的符号计算工具箱将成为我们的得力助手。对于Python用户建议使用Jupyter Notebook进行交互式编程# Python环境配置 import numpy as np import sympy as sp from sympy import symbols, Matrix, diff, simplify # 初始化符号变量 t symbols(t) # 时间变量Matlab用户则可以直接使用Symbolic Math Toolbox% Matlab环境配置 syms t real % 声明时间变量为实型符号2. 坐标系与旋转矩阵的实现理解牛顿-欧拉方程的第一步是掌握坐标系变换。我们需要实现旋转矩阵和反对称矩阵的运算。旋转矩阵的实现以ZYX欧拉角为例# Python实现 def rotation_matrix(alpha, beta, gamma): ZYX顺序的欧拉角旋转矩阵 Rz Matrix([ [sp.cos(alpha), -sp.sin(alpha), 0], [sp.sin(alpha), sp.cos(alpha), 0], [0, 0, 1] ]) Ry Matrix([ [sp.cos(beta), 0, sp.sin(beta)], [0, 1, 0], [-sp.sin(beta), 0, sp.cos(beta)] ]) Rx Matrix([ [1, 0, 0], [0, sp.cos(gamma), -sp.sin(gamma)], [0, sp.sin(gamma), sp.cos(gamma)] ]) return Rz * Ry * Rx对应的Matlab实现% Matlab实现 function R rotation_matrix(alpha, beta, gamma) % ZYX顺序的欧拉角旋转矩阵 Rz [cos(alpha) -sin(alpha) 0; sin(alpha) cos(alpha) 0; 0 0 1]; Ry [cos(beta) 0 sin(beta); 0 1 0; -sin(beta) 0 cos(beta)]; Rx [1 0 0; 0 cos(gamma) -sin(gamma); 0 sin(gamma) cos(gamma)]; R Rz * Ry * Rx; end反对称矩阵的实现# Python实现 def skew_symmetric(v): 将3D向量转换为反对称矩阵 return Matrix([ [0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0] ])% Matlab实现 function S skew_symmetric(v) % 将3D向量转换为反对称矩阵 S [0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0]; end3. 线运动方程的推导与实现基于前面的准备工作我们现在可以开始推导线运动方程。牛顿-欧拉方程的线运动部分描述了刚体的平动动力学。关键推导步骤位置关系r_A r_B C_BU * r_A|B速度关系v_A v_B C_BU * (v_A|B ω_B × r_A|B)加速度关系a_A a_B C_BU * [2(ω_B × v_A|B) ω_B × (ω_B × r_A|B) a_A|B α_B × r_A|B]让我们用代码实现这些关系# Python实现线运动推导 # 定义符号变量 r_A, r_B, r_A_B symbols(r_A r_B r_A_B, clsMatrix) v_A, v_B, v_A_B symbols(v_A v_B v_A_B, clsMatrix) a_A, a_B, a_A_B symbols(a_A a_B a_A_B, clsMatrix) omega_B, alpha_B symbols(omega_B alpha_B, clsMatrix) # 位置关系 position_relation r_A - (r_B C_BU * r_A_B) # 速度关系 velocity_relation v_A - (v_B C_BU * (v_A_B skew_symmetric(omega_B) * r_A_B)) # 加速度关系 coriolis 2 * skew_symmetric(omega_B) * v_A_B centripetal skew_symmetric(omega_B) * skew_symmetric(omega_B) * r_A_B angular_accel skew_symmetric(alpha_B) * r_A_B acceleration_relation a_A - (a_B C_BU * (coriolis centripetal a_A_B angular_accel))% Matlab实现线运动推导 syms r_A r_B r_A_B [3 1] real syms v_A v_B v_A_B [3 1] real syms a_A a_B a_A_B [3 1] real syms omega_B alpha_B [3 1] real % 位置关系 position_relation r_A - (r_B C_BU * r_A_B); % 速度关系 velocity_relation v_A - (v_B C_BU * (v_A_B skew_symmetric(omega_B) * r_A_B)); % 加速度关系 coriolis 2 * skew_symmetric(omega_B) * v_A_B; centripetal skew_symmetric(omega_B) * skew_symmetric(omega_B) * r_A_B; angular_accel skew_symmetric(alpha_B) * r_A_B; acceleration_relation a_A - (a_B C_BU * (coriolis centripetal a_A_B angular_accel));4. 角运动方程的推导与实现角运动方程描述了刚体的转动动力学其推导过程与线运动类似但更为简洁。角运动关键方程角速度关系ω_A ω_B C_BU * ω_A|B角加速度关系α_A α_B C_BU * (ω_B × ω_A|B α_A|B)代码实现如下# Python实现角运动推导 # 定义符号变量 omega_A, omega_A_B symbols(omega_A omega_A_B, clsMatrix) alpha_A symbols(alpha_A, clsMatrix) # 角速度关系 angular_velocity_relation omega_A - (omega_B C_BU * omega_A_B) # 角加速度关系 angular_acceleration_relation alpha_A - (alpha_B C_BU * (skew_symmetric(omega_B) * omega_A_B alpha_A_B))% Matlab实现角运动推导 syms omega_A omega_A_B [3 1] real syms alpha_A [3 1] real % 角速度关系 angular_velocity_relation omega_A - (omega_B C_BU * omega_A_B); % 角加速度关系 angular_acceleration_relation alpha_A - (alpha_B C_BU * (skew_symmetric(omega_B) * omega_A_B alpha_A_B));5. 完整动力学方程的组装与验证现在我们将线运动和角运动方程组合起来形成完整的牛顿-欧拉方程并通过具体案例进行验证。完整动力学方程线运动 F m * a_A角运动 τ I * α_A ω_A × (I * ω_A)其中I是惯性张量。我们可以用代码实现这些方程# Python实现完整动力学方程 m symbols(m) # 质量 I symbols(I, clsMatrix) # 惯性张量 # 外力 F symbols(F, clsMatrix) tau symbols(tau, clsMatrix) # 线运动方程 linear_dynamics F - m * a_A # 角运动方程 angular_dynamics tau - (I * alpha_A skew_symmetric(omega_A) * I * omega_A)% Matlab实现完整动力学方程 syms m real % 质量 syms I [3 3] real % 惯性张量 % 外力 syms F [3 1] real syms tau [3 1] real % 线运动方程 linear_dynamics F - m * a_A; % 角运动方程 angular_dynamics tau - (I * alpha_A skew_symmetric(omega_A) * I * omega_A);验证案例简单旋转刚体考虑一个绕z轴旋转的刚体我们可以通过代码验证我们的实现是否正确# Python验证案例 # 定义具体参数 alpha_val t # 绕z轴旋转角度随时间线性变化 beta_val 0 gamma_val 0 # 计算具体旋转矩阵 C_BU rotation_matrix(alpha_val, beta_val, gamma_val) # 计算角速度 omega_B_val Matrix([0, 0, 1]) # 绕z轴单位角速度 # 验证旋转矩阵导数 dC_BU_dt diff(C_BU, t) S_omega skew_symmetric(omega_B_val) assert simplify(dC_BU_dt - C_BU * S_omega) Matrix.zeros(3,3)% Matlab验证案例 % 定义具体参数 alpha_val t; % 绕z轴旋转角度随时间线性变化 beta_val 0; gamma_val 0; % 计算具体旋转矩阵 C_BU rotation_matrix(alpha_val, beta_val, gamma_val); % 计算角速度 omega_B_val [0; 0; 1]; % 绕z轴单位角速度 % 验证旋转矩阵导数 dC_BU_dt diff(C_BU, t); S_omega skew_symmetric(omega_B_val); assert(all(all(simplify(dC_BU_dt - C_BU * S_omega) zeros(3))));6. 可视化与结果分析为了更直观地理解牛顿-欧拉方程我们可以将结果可视化。Python中可以使用MatplotlibMatlab则有强大的内置绘图功能。Python可视化示例import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D # 模拟刚体运动轨迹 times np.linspace(0, 10, 100) positions [] for time in times: # 计算随时间变化的位置 alpha_t 0.5 * time C rotation_matrix(alpha_t, 0, 0) r C * Matrix([1, 0, 0]) # 初始位置(1,0,0) positions.append(np.array(r, dtypefloat).flatten()) positions np.array(positions) # 绘制轨迹 fig plt.figure() ax fig.add_subplot(111, projection3d) ax.plot(positions[:,0], positions[:,1], positions[:,2]) ax.set_xlabel(X) ax.set_ylabel(Y) ax.set_zlabel(Z) plt.title(刚体运动轨迹) plt.show()Matlab可视化示例% 模拟刚体运动轨迹 times linspace(0, 10, 100); positions zeros(3, length(times)); for i 1:length(times) time times(i); alpha_t 0.5 * time; C rotation_matrix(alpha_t, 0, 0); r C * [1; 0; 0]; % 初始位置(1,0,0) positions(:,i) r; end % 绘制轨迹 figure; plot3(positions(1,:), positions(2,:), positions(3,:)); xlabel(X); ylabel(Y); zlabel(Z); title(刚体运动轨迹); grid on;7. 实际应用与扩展掌握了牛顿-欧拉方程的基本推导和实现后我们可以将其应用到更复杂的场景中比如多刚体系统如机器人手臂的动力学建模。多刚体系统建模的关键点建立每个连杆的坐标系确定相邻坐标系间的变换关系递归计算每个连杆的速度和加速度计算作用在每个连杆上的力和力矩# Python实现简单的两连杆机器人动力学 # 定义连杆参数 m1, m2 symbols(m1 m2) # 质量 l1, l2 symbols(l1 l2) # 长度 I1, I2 symbols(I1 I2, clsMatrix) # 惯性张量 # 定义关节角度 theta1, theta2 symbols(theta1 theta2, clsFunction)(t) omega1 diff(theta1, t) alpha1 diff(omega1, t) omega2 diff(theta2, t) alpha2 diff(omega2, t) # 第一连杆的运动 C01 rotation_matrix(theta1, 0, 0) # 基座到第一连杆的旋转 r1 C01 * Matrix([l1/2, 0, 0]) # 第一连杆质心位置 v1 diff(r1, t) a1 diff(v1, t) # 第二连杆的运动 C12 rotation_matrix(theta2, 0, 0) # 第一到第二连杆的旋转 C02 C01 * C12 r2 C01 * Matrix([l1, 0, 0]) C02 * Matrix([l2/2, 0, 0]) # 第二连杆质心位置 v2 diff(r2, t) a2 diff(v2, t) # 计算动力学方程 # 这里省略了具体的力和力矩计算实际应用中需要完整推导% Matlab实现简单的两连杆机器人动力学 % 定义连杆参数 syms m1 m2 real % 质量 syms l1 l2 real % 长度 syms I1 I2 [3 3] real % 惯性张量 % 定义关节角度 syms theta1(t) theta2(t) omega1 diff(theta1, t); alpha1 diff(omega1, t); omega2 diff(theta2, t); alpha2 diff(omega2, t); % 第一连杆的运动 C01 rotation_matrix(theta1, 0, 0); % 基座到第一连杆的旋转 r1 C01 * [l1/2; 0; 0]; % 第一连杆质心位置 v1 diff(r1, t); a1 diff(v1, t); % 第二连杆的运动 C12 rotation_matrix(theta2, 0, 0); % 第一到第二连杆的旋转 C02 C01 * C12; r2 C01 * [l1; 0; 0] C02 * [l2/2; 0; 0]; % 第二连杆质心位置 v2 diff(r2, t); a2 diff(v2, t); % 计算动力学方程 % 这里省略了具体的力和力矩计算实际应用中需要完整推导通过这种动手实践的方式牛顿-欧拉方程不再是纸上抽象的数学公式而变成了可以直观理解和验证的计算机代码。在实际机器人动力学仿真中这种基于代码的实现方式可以直接用于控制器设计和系统仿真。