变附着系数AGV横摆稳定性控制【附程序】

✨ 长期致力于无人搬运车、横摆稳定性、变附着系数、路面附着系数估计、直接横摆力矩控制研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》

（1）自适应无迹卡尔曼滤波路面附着系数观测器设计：

针对AGV在变附着系数路面行驶时轮胎与路面摩擦特性变化的问题，设计一种自适应无迹卡尔曼滤波算法用于实时估计路面附着系数。基于Dugoff轮胎模型建立整车七自由度动力学模型，包括纵向、横向、横摆和四个车轮旋转自由度。状态变量选取为纵向车速、横向车速、横摆角速度、四个车轮转速以及路面附着系数，共9维。测量量为轮速传感器和IMU提供的纵向加速度、横向加速度和横摆角速度。UKF的Sigma点个数为2n+1=19，采用比例修正对称采样。自适应机制包括两部分：一是基于协方差匹配原理计算预测误差的实时协方差，与理论协方差比较，构建自适应加权因子动态调整过程噪声协方差矩阵；二是在过程噪声更新中引入遗忘因子λ=0.97，限制陈旧测量数据的权重。在Carsim中设置变附着系数工况：车辆从干燥沥青（μ=0.85）行驶30米后进入湿滑冰雪路面（μ=0.25），车速36km/h。观测器估计的附着系数在进入湿滑路面后0.4秒内从0.85下降到0.28，平均绝对误差不超过6%。与标准UKF相比，自适应UKF的估计波动幅度减小45%，收敛速度提升30%。

（2）分层式横摆稳定性控制器架构与滑模附加横摆力矩计算：

采用分层控制架构，上层为路面附着系数观测器，中层为横摆稳定性控制器，下层为转矩分配器。中层控制器基于滑模控制算法设计，以横摆角速度误差和质心侧偏角误差的线性组合为滑模面。车辆参考模型根据路面附着系数实时调整横摆角速度期望值，上限为μ*g/v。滑模趋近律采用指数趋近加等速趋近，切换增益根据横摆角速度误差绝对值自适应调节。控制器输出附加横摆力矩，范围为-2000N·m至2000N·m。在Simulink中搭建控制器模型，与Carsim联合仿真。测试工况为角阶跃转向（方向盘转角90°，车速60km/h），路面从高附突变为低附。突变瞬间，无控制时车辆横摆角速度超调达到0.42rad/s并出现大幅摆动；滑模控制介入后横摆角速度峰值0.28rad/s，超调量降低33%，且能迅速跟踪参考值，调节时间0.6秒。

（3）最优转矩分配与实车双移线验证：

下层转矩分配器采用最优分配方法，以轮胎利用率最小为目标，约束为总附加横摆力矩和纵向驱动力需求。将期望附加横摆力矩转换为四个车轮的驱动/制动转矩指令，同时考虑电机转矩限幅和路面附着系数限制。分配问题转化为二次规划求解，在嵌入式控制器中以每10ms周期求解。在Carsim/Simulink中搭建双移线仿真工况（ISO 3888-1），路面附着系数按0.8-0.4-0.8变化。对比无控制和有控制的结果：无控制时车辆在第二次变线时发生侧滑，轨迹偏离超过车道线1.2米；所提控制器使车辆始终保持在车道内，最大横摆角速度偏差0.09rad/s。搭建实车AGV样机（尺寸1.2m×0.8m，四轮独立驱动），在试验场进行双移线实验，路面铺设低附薄膜模拟冰面。实验数据记录显示，AUKF观测器估计附着系数与实测值误差小于14%，横摆角速度跟踪误差平均0.07rad/s，超调量小于10%，验证了控制算法的实际有效性。

import numpy as np from filterpy.kalman import UnscentedKalmanFilter, MerweScaledSigmaPoints class AdaptiveUKF: def __init__(self, dt, dim_x=9, dim_z=6): self.dt = dt self.dim_x = dim_x self.dim_z = dim_z points = MerweScaledSigmaPoints(n=dim_x, alpha=0.1, beta=2, kappa=1) self.ukf = UnscentedKalmanFilter(dim_x=dim_x, dim_z=dim_z, dt=dt, hx=self.hx, fx=self.fx, points=points) self.ukf.Q = np.eye(dim_x) * 0.01 self.ukf.R = np.eye(dim_z) * 0.1 self.forgetting = 0.97 self.cov_match_window = [] def fx(self, x, dt): # 离散化状态方程简化 vx, vy, yaw_rate, w1, w2, w3, w4, mu = x[0], x[1], x[2], x[3], x[4], x[5], x[6], x[7] # 车辆动力学简化 ax = -0.1 * vx ay = -0.2 * vy yaw_acc = -0.5 * yaw_rate x_new = x.copy() x_new[0] = vx + ax*dt x_new[1] = vy + ay*dt x_new[2] = yaw_rate + yaw_acc*dt x_new[3:7] = x[3:7] + np.random.randn(4)*0.1 x_new[7] = mu # 附着系数保持不变，由测量更新调整 return x_new def hx(self, x): # 测量量：ax, ay, yaw_rate, 轮速 return np.array([x[0]*0.1, x[1]*0.1, x[2], x[3], x[4], x[5]]) def adaptive_update(self, z): self.ukf.predict() # 计算预测误差协方差匹配 y = z - self.hx(self.ukf.x) S = self.ukf.S gamma = y @ np.linalg.inv(S) @ y.T self.cov_match_window.append(gamma) if len(self.cov_match_window) > 10: self.cov_match_window.pop(0) avg_gamma = np.mean(self.cov_match_window) if avg_gamma > 3.0: # 自适应调整Q self.ukf.Q *= (1 + 0.1*(avg_gamma-3)) self.ukf.update(z, self.hx) return self.ukf.x class YawStabilityController: def __init__(self): self.k1 = 15.0 self.k2 = 10.0 self.adaptive_gain = True def sliding_surface(self, yaw_err, beta_err): return yaw_err + 0.2 * beta_err def compute_moment(self, yaw_err, beta_err, yaw_rate, vx, mu): s = self.sliding_surface(yaw_err, beta_err) # 自适应增益 k_eta = 5.0 + 2.0 * abs(yaw_err) Mz = -k_eta * np.sign(s) - 8.0 * s # 限幅 Mz = np.clip(Mz, -2000, 2000) return Mz def torque_distribution_qp(Mz, Fx_des, tire_forces_max): # 二次规划分配，简化为解析解 # 轮距1.2m，前轴后轴各两个轮 delta_T = Mz / 0.6 # 左右差动力 # 分配至四个轮 T_fl = Fx_des/4 - delta_T/2 T_fr = Fx_des/4 + delta_T/2 T_rl = Fx_des/4 - delta_T/2 T_rr = Fx_des/4 + delta_T/2 return np.clip([T_fl, T_fr, T_rl, T_rr], -tire_forces_max, tire_forces_max)