别再死记硬背公式了！用Python/Matlab动手推导牛顿-欧拉方程（附完整代码）
用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', cls=Matrix) v_A, v_B, v_A_B = symbols('v_A v_B v_A_B', cls=Matrix) a_A, a_B, a_A_B = symbols('a_A a_B a_A_B', cls=Matrix) omega_B, alpha_B = symbols('omega_B alpha_B', cls=Matrix) # 位置关系 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', cls=Matrix) alpha_A = symbols('alpha_A', cls=Matrix) # 角速度关系 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', cls=Matrix) # 惯性张量 # 外力 F = symbols('F', cls=Matrix) tau = symbols('tau', cls=Matrix) # 线运动方程 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中可以使用Matplotlib，Matlab则有强大的内置绘图功能。
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, dtype=float).flatten()) positions = np.array(positions) # 绘制轨迹 fig = plt.figure() ax = fig.add_subplot(111, projection='3d') 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', cls=Matrix) # 惯性张量 # 定义关节角度 theta1, theta2 = symbols('theta1 theta2', cls=Function)(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); % 计算动力学方程 % 这里省略了具体的力和力矩计算，实际应用中需要完整推导通过这种动手实践的方式，牛顿-欧拉方程不再是纸上抽象的数学公式，而变成了可以直观理解和验证的计算机代码。在实际机器人动力学仿真中，这种基于代码的实现方式可以直接用于控制器设计和系统仿真。