基于LQR的无人驾驶车辆横纵向线性二次型调节器【附代码】
✨ 本团队擅长数据搜集与处理、建模仿真、程序设计、仿真代码、EI、SCI写作与指导，毕业论文、期刊论文经验交流。
✅ 专业定制毕设、代码
✅如需沟通交流，查看文章底部二维码
（1）基于自适应灰狼优化的LQR权重在线调节：
针对LQR控制器中状态权重矩阵Q和输入权重R通常依赖经验设定的问题，引入改进灰狼优化算法对权重参数进行在线寻优。将无人驾驶汽车的横向偏差、航向角偏差、方向盘转角以及转角变化率作为LQR的状态量，并采用动态自适应sigmoid函数更新灰狼算法的收敛因子，初期探索范围广，后期精细搜索。将每次控制周期内实际轨迹与参考轨迹的累积横向误差和转向能耗作为灰狼优化器的适应度函数，在Matlab/Simulink仿真中，每0.1秒进行一次小种群迭代，粒子数量设定为15个，最大迭代10代。经过高速公路双移线工况的变速实验，该自适应LQR相比固定权重LQR的最大横向跟踪误差降低22%，航向角误差标准差减小0.018 rad，且在不同车速下均能保持平稳转向，无过度振荡。
（2）遗传算法优化的双层纵向PID控制架构：
构建上层速度调节与下层加速度执行的两级纵向控制器。上层控制器根据期望速度与实际速度的偏差产生期望加速度指令，采用遗传算法对PID的比例、积分、微分系数和低通滤波器时间常数共四个参数进行整定。遗传算法的染色体采用实数编码，目标函数由速度跟踪的积分绝对误差、超调量惩罚和加速度变化率惩罚三项加权构成，权重分别取0.5、0.3、0.2。为降低在线计算负载，优化过程在Matlab工作空间离线进行，生成一组关于速度偏差和车速的系数映射表，实际运行中通过查表插值获取。下层控制器将期望加速度转换为驱动或制动信号，标定过程采用基于最小二乘的节气门与制动压力分段线性映射。在Carsim中进行的匀加速至80 km/h和正弦速度跟踪测试中，该纵向控制器的速度均方根误差为0.12 m/s，加速度曲线平顺无抖动，乘客冲击度指标小于2.0 m/s³。
（3）横纵向协同控制与预瞄‑反馈复合机制：
为实现泊车和超车等复杂场景下的整体运动控制，整合横向LQR与纵向PID，搭建预瞄‑反馈复合控制器。预瞄控制器利用道路曲率和当前车速计算前馈方向盘转角，降低LQR的稳态误差；反馈通道则实时补偿因侧风、轮胎磨损等引起的扰动。横向LQR与纵向PID通过车速和曲率的耦合约束协同工作，在弯道处，纵向速度规划根据横向加速度限幅自动降低期望车速，而横向控制器的预测时域也会随车速动态调节。在Carsim与Simulink联合仿真中构建了低速直角转弯泊车和高速超车变道两个典型场景，前者的最终停车位置误差小于0.04 m，后者在超车过程中侧向加速度最大值仅为2.1 m/s²，横摆角速度响应迅速且无衰减振荡，有效提升了整车轨迹跟踪精度与行驶安全性。"
"import numpy as np
from scipy.linalg import solve_continuous_are
# 自适应灰狼优化LQR权重更新
class GreyWolfLQR:
def __init__(self, n=15, dim=4):
self.n = n; self.dim = dim
self.alpha_pos = np.zeros(dim); self.beta_pos = np.zeros(dim); self.delta_pos = np.zeros(dim)
def optimize(self, fitness_func, bounds):
wolves = np.random.rand(self.n, self.dim)*(bounds[:,1]-bounds[:,0])+bounds[:,0]
a = 2; convergence = []
for t in range
(10):
for i in range(self.n):
fitness = fitness_func(wolves[i])
# 更新alpha/beta/delta...
a = 2*(1-t/10) # 灰狼线性收敛，此处可改为自适应sigmoid
for i in range(self.n):
r1, r2 = np.random.rand(self.dim)
A, C = 2*a*r1-a, 2*r2
D = np.abs(C*self.alpha_pos - wolves[i])
wolves[i] = self.alpha_pos - A*D
return self.alpha_pos
# LQR求解
def compute_lqr_gain(A, B, Q, R):
P = solve_continuous_are(A, B, Q, R)
K = np.linalg.inv(R) @ B.T @ P
return K
# 纵向遗传优化PID（离线）
def pid_fitness(params, speed_ref, speed_act):
Kp, Ki, Kd, N = params
error = speed_ref - speed_act
integral = np.cumsum(error)*0.01; derivative = np.gradient(error)/0.01
u = Kp*error + Ki*integral + Kd*derivative
iau = np.sum(np.abs(error)*0.01)
overshoot = max(speed_act.max()-speed_ref,0)*100
return iau*0.5 + overshoot*0.3 + np.std(np.gradient(u))*0.2
如有问题，可以直接沟通
👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇