M00031-机械臂凯恩方法建模控制算法
机械臂动力学建模这玩意儿，说难不难说简单也不简单。今天咱们来点干货，聊聊怎么用凯恩方法搞机械臂建模。这方法比拉格朗日方程省事多了，特别是关节多的机械臂，能少算一堆偏导数。
先看个二连杆的例子热热身。假设机械臂在平面运动，每个关节都是旋转的。咱们先定义广义坐标θ₁和θ₂，这俩就是关节角度。凯恩法的核心是找偏速度，说白了就是各质点速度对广义速度的线性组合。
import sympy as sp θ1, θ2, θ1_dot, θ2_dot = sp.symbols('theta_1 theta_2 theta1_dot theta2_dot') l1, l2 = 0.5, 0.4 # 连杆长度 r1 = sp.Matrix([l1/2*sp.cos(θ1), l1/2*sp.sin(θ1)]) r2 = sp.Matrix([l1*sp.cos(θ1) + l2/2*sp.cos(θ1+θ2), l1*sp.sin(θ1) + l2/2*sp.sin(θ1+θ2)]) # 质心速度 v1 = r1.diff(θ1)*θ1_dot + r1.diff(θ2)*θ2_dot v2 = r2.diff(θ1)*θ1_dot + r2.diff(θ2)*θ2_dot这段代码把质心速度拆成了广义速度的线性组合。注意sympy的微分操作会自动处理变量依赖关系，比手工推导靠谱多了。特别是当连杆数量增加时，这种符号计算优势更明显。
M00031-机械臂凯恩方法建模控制算法
动力学方程的关键在广义主动力和广义惯性力。咱们先处理惯性力部分：
m1, m2, I1, I2 = 2.0, 1.5, 0.3, 0.2 # 质量和转动惯量 g = 9.8 # 动能计算 T1 = 0.5*m1*v1.dot(v1) + 0.5*I1*θ1_dot**2 T2 = 0.5*m2*v2.dot(v2) + 0.5*I2*(θ1_dot + θ2_dot)**2 T_total = T1 + T2 # 广义惯性力 Q_inertia = [] for q_dot in [θ1_dot, θ2_dot]: Q = (sp.diff(T_total, q_dot).diff(t) - sp.diff(T_total, q)) Q_inertia.append(Q.simplify())这里有个坑：时间导数操作需要特别注意符号变量的时间相关性。实际建模时得用sympy的Derivative类处理，或者像这样通过链式法则展开。
再看看控制算法实现。PD控制配上动力学前馈是常用套路：
// 伪代码示例 void controlLoop() { MatrixXd M = computeInertiaMatrix(); // 惯性矩阵 VectorXd C = computeCoriolis(); // 科氏力项 VectorXd G = computeGravity(); // 重力项 VectorXd q_error = q_desired - q_current; VectorXd q_error_dot = q_dot_desired - q_dot_current; torque = M*(Kp*q_error + Kd*q_error_dot) + C + G; }重点在前馈补偿部分——M矩阵乘PD项这个操作，相当于用动力学模型预测需要的加速度。这种结构比单纯PID性能好得多，特别是在高速运动时。
最后说个实战经验：模型参数不准怎么办？可以留出参数在线辨识接口。比如把M、C、G矩阵中的质量参数换成可调节变量，配合最小二乘法实时更新。具体实现时注意数值稳定性，必要时加个遗忘因子。
)
