动力学参数设置

基于LQR最优控制算法实现的轨迹跟踪控制建立了基于车辆的质心侧偏角、横摆角速度横向误差航向误差四自由度动力学模型作为控制模型通过最优化航向误差和横向误差实时计算最优的K值计算期望的前轮转角实现轨迹跟踪仿真效果良好有对应的资料最近在调车辆轨迹跟踪算法的时候发现LQR这个老伙计真是越用越香。传统PID在复杂路况下容易手忙脚乱而基于最优控制理论的LQR就像个经验丰富的赛车手总能找到最优雅的控制策略。今天咱们就聊聊怎么用四个关键状态量教车辆学会贴线行驶。先看模型搭建这个硬核环节。咱们把车辆的运动拆解成四个维度质心侧偏角β车身打滑的倔强程度、横摆角速度γ车头左右甩的速率、横向误差e偏离轨道的距离和航向误差ΔΨ车头朝向与路线的夹角。这四兄弟组成了我们的状态向量x[β, γ, e, ΔΨ]^T就像给车辆装了四个传感器实时汇报状态。模型的核心是这个状态方程m 1723.0 # 整车质量 lf 1.232 # 前轮到质心距离 Iz 4175.0 # 横摆转动惯量 C_alpha 80000 # 轮胎侧偏刚度 # 状态矩阵A A np.array([ [-2*C_alpha/(m*vx), 2*(lf*C_alpha)/(m*vx)-1, 0, 0], [2*lf*C_alpha/Iz, -2*lf**2*C_alpha/(Iz*vx), 0, 0], [0, 1, 0, vx], [1, 0, 0, 0] ])这里用到的vx是纵向车速需要实时获取。注意A矩阵第三行那个vx正是它把航向误差和横向位置联系起来——就像骑自行车时车速越快方向稍微偏一点车子横移得就越明显。LQR的魔法在于代价函数的设计。咱们重点关照横向和航向这两个误差项Q矩阵里给它们安排了重兵把守Q np.diag([0, 0, 20, 15]) # 状态权重 R np.array([[0.1]]) # 控制权重这里第三、四个对角元对应e和ΔΨ的惩罚系数相当于在说偏离路线1米比航向偏5度更严重。而R矩阵控制着方向盘转动的代价防止出现抽搐式转向。基于LQR最优控制算法实现的轨迹跟踪控制建立了基于车辆的质心侧偏角、横摆角速度横向误差航向误差四自由度动力学模型作为控制模型通过最优化航向误差和横向误差实时计算最优的K值计算期望的前轮转角实现轨迹跟踪仿真效果良好有对应的资料求解Riccati方程是算法的核心环节。Python里几行代码就能搞定P linalg.solve_continuous_are(A, B, Q, R) K np.dot(np.dot(np.linalg.inv(R), B.T), P)这串计算就像给车辆装了个智能大脑每20ms重新计算一次最优K值。实时更新的K矩阵会告诉控制器现在应该多用点侧偏角反馈还是多关注航向修正最终的前轮转角控制量计算反而简单得不像话delta -np.dot(K, x_current)这个负号是关键相当于发现车辆往左偏就赶紧往右打方向。但别小看这个减法背后是整套动力学模型在支撑——就像老司机下意识的方向修正都是多年经验模型积累的条件反射。在CarSim和Simulink联合仿真中60km/h过发夹弯时横向误差能控制在0.2米以内。有意思的是当路面附着系数突然降低时算法会自动增大航向误差的权重让车辆像猫科动物一样提前调整姿态。不过也发现了问题——在连续S弯中如果车速突变状态矩阵的vx参数滞后会导致控制量震荡后续准备用卡尔曼滤波做个车速估计试试。调参时有个小技巧把Q矩阵的非重点项设成非零但很小的值比如0.01能避免数值计算中的奇异问题。就像做菜时加点水防止粘锅虽不主要影响味道但少了就会翻车。