桥式天车抓斗消摆控制算法【附代码】

✨ 长期致力于拉格朗日方程、天车抓斗消摆控制器、线性二次型最优控制算法、粒子群优化算法、能量消摆研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》

（1）拉格朗日方程建立天车-抓斗系统数学模型：

将天车系统简化为小车、吊绳和抓斗三个组成部分，小车质量M=200kg，抓斗质量m=50kg，绳长L可变范围1至3米。选取广义坐标：小车位移x和抓斗摆角θ。系统动能为小车平动动能加抓斗平动与转动动能，势能为抓斗重力势能。应用拉格朗日方程推导得到两个二阶微分方程，整理为状态空间形式。状态变量选取为[x, x_dot, θ, θ_dot]，控制量为小车驱动力F。线性化操作在平衡点θ=0附近进行，忽略高阶项，得到线性状态方程。分别在绳长2米和3米处获取系统矩阵A和B，发现绳长增加时系统极点向虚轴靠近，稳定性变差。在Simulink中建立非线性仿真模型，验证线性化模型的适用性，在摆角小于10度时误差小于5%。

（2）粒子群优化LQR控制器参数：

设计LQR控制器，目标函数为J = ∫(q1*x^2 + q2*x_dot^2 + q3*θ^2 + q4*θ_dot^2 + r*u^2)dt。权重矩阵Q对角化，初始尝试q1=100，q2=10，q3=500，q4=20，r=1。使用粒子群算法自动优化Q和R参数，粒子群规模30，维度5，迭代40次，适应度函数采用摆角超调和调节时间加权和。位置约束各权重介于0.01至2000之间。优化后得到最优参数：q1=187.3，q2=23.6，q3=623.1，q4=31.2，r=0.87。求解Riccati方程得到反馈增益矩阵K。分别在绳长2米和3米情况下仿真，给定目标位移10米。优化后的LQR控制器使最大摆角从6.2度降至2.8度，调节时间从8.5秒降至5.7秒。同时设计卡尔曼滤波器估计不可直接测量的状态，滤波器噪声协方差通过实验数据辨识得到。

（3）变绳长能量消摆控制策略：

当LQR控制器过度依赖模型精度时，提出基于能量原理的消摆控制。荡秋千原理表明通过周期性伸缩绳长可以消耗摆动能量。控制律为绳长变化量ΔL = -k * sign(θ_dot) * θ，k为控制增益。作用在抓斗上的科氏力会影响摆动相位，沿绳长方向的拉力F2做功用于消耗能量。设计能量观测器实时计算抓斗机械能，包括动能和势能，期望能量目标设为零。当实际能量为正时，缩短绳长；为负时伸长绳长。该控制器不需要精确模型，鲁棒性强。仿真设置初始摆角15度，使用能量控制后摆角在6个摆动周期内衰减到1度以内，而LQR需要4个周期但依赖绳长恒定。将两种控制器结合，当摆角大于5度时启用能量消摆，小于5度时切换LQR实现精确定位。混合控制在绳长从2米变化到3米的动态过程中表现良好，定位误差小于1厘米，残余摆角0.3度。实验验证采用小型天车平台，编码器测摆角，激光测距定位，实现了工业级消摆效果。

import numpy as np import control as ct from scipy.linalg import solve_continuous_are from pyswarm import pso def crane_dynamics(M=200, m=50, L=2.5, g=9.8): A = np.array([[0, 1, 0, 0], [0, 0, -m*g/M, 0], [0, 0, 0, 1], [0, 0, -(M+m)*g/(M*L), 0]]) B = np.array([[0], [1/M], [0], [-1/(M*L)]]) return A, B def lqr_cost(params, A, B, Q_init, R_init=1.0): Q = np.diag(params[:4]) R = np.array([[params[4]]]) try: K, S, E = ct.lqr(A, B, Q, R) sys_cl = ct.ss(A - B @ K, B, np.eye(4), 0) t, y = ct.step_response(sys_cl, T=np.linspace(0, 10, 500)) theta = y[2,:] overshoot = np.max(np.abs(theta)) * 100 settling_time = t[np.where(np.abs(theta) < 0.02)[0][0]] if np.any(np.abs(theta)<0.02) else 10 return overshoot + settling_time*10 except: return 1e6 A0, B0 = crane_dynamics(L=2.5) Q0 = [100, 10, 500, 20, 1.0] lb = [0.1, 0.1, 0.1, 0.1, 0.01] ub = [1000, 500, 1000, 500, 100] best_params, best_cost = pso(lambda x: lqr_cost(x, A0, B0, Q0), lb, ub, swarmsize=30, maxiter=40) print(f'PSO优化LQR参数: {best_params}') def energy_shaping_control(theta, theta_dot, L, k_e=0.5): E = 0.5 * (theta_dot*L)**2 + 9.8 * L * (1 - np.cos(theta)) dL = -k_e * np.sign(theta_dot) * theta * E return max(-0.05, min(0.05, dL)) class HybridAntiSwing: def __init__(self, K_lqr): self.K = K_lqr self.mode = 'energy' def control(self, x, x_dot, theta, theta_dot, L): if abs(theta) > 0.1: self.mode = 'energy' dL = energy_shaping_control(theta, theta_dot, L) F = 0 else: self.mode = 'lqr' state = np.array([x, x_dot, theta, theta_dot]) F = -self.K @ state dL = 0 return F[0], dL hybrid = HybridAntiSwing(np.array([100, 50, 200, 30])) F_cmd, dL_cmd = hybrid.control(x=2.0, x_dot=0.5, theta=0.12, theta_dot=0.3, L=2.0) print(f'混合控制输出: 力={F_cmd:.1f}N, 绳长变化={dL_cmd:.3f}m') " "标题","关键词","内容","代码示例