顾青等：基于非线性模型预测控制的自动泊车路径跟踪 .949· 慢，控制周期T被设置为0.1s,即控制频率为10Hz. 跟踪目标点的选取主要基于车辆当前位置与参 转向角增量△δ通过下式计算： 考速度，假设参考路径为参数化的函数g(x,y,), △6=T8 (3) 则第0个跟踪目标点满足： 式中，δ为转向角速度速度.对于该值的约束，北京 d(x(tlt),(tt))=d(x(tlt),g(x,y,0)) 理工大学的龚建伟教授团队进行了基于实际车辆的 (10) 实验测试[)，实验结果测得该值的上下限设置为 其中，d为求解距离的函数. ±9.4°s-1 预测时域内的其余跟踪目标点满足： 转向角δ的上下限也根据龚建伟教授团队的测 s(x(t+ilt),x(t+i-1lt))=Tud, 试结果[16]设置，其值为±25°. i=1,2,…,Np (11) 综合上述约束，并将角度转化为弧度，基于非线 其中，3。为两个跟踪目标点之间参考路径的弧长， 性模型预测控制的自动泊车路径跟踪控制系统的约 v为人为设定的参考速度 束条件为： 式(9)~(11)即自动泊车路径跟踪控制器 -0.1ms-1≤△v≤0.1ms-1 的预测模型.基于上述模型，滚动优化目标函数 可设置为： -0.0164rad≤△6≤0.0164rad (4) -0.44rad≤6≤0.44rad J(e(t),u(t))= le(t+1lt)+ 2自动泊车路径跟踪控制器设计 I△u(t+i)la2+ ‖δ(t+ilt)l2,+ 设计非线性模型预测制器的核心是建立预测模 型和设计滚动优化函数.在路径跟踪控制器中，预 llella (12) 测模型的作用是根据车辆当前的位姿状态和未来可 式(12)中，第一个惩罚项是误差惩罚项，第二 能的控制输入，预测未来可能的位姿状态，所以基于 个惩罚项的作用是使自动泊车路径尽可能地平稳， 车辆的运动学模型(1)建立预测模型，将(1)向量化 第三个惩罚项的功能是防止微小扰动导致转向轮的 可得： 转向角过于频繁地变化，e为松弛因子，Q1、Q2、Q3 和Q,为权重系数矩阵. x=f(x,u) (5) 其中： △u(t+ilt)为控制量增量： (x=[x y 0]T Au(t+ilt)=u(t+ilt)-u(t+i-1lt), (u=[v6]r (6) i=1,2,…,Np (13) 至此，自动泊车路径跟踪控制问题即转化为如 采用欧拉法(Euler method)对式(5)进行离散 化处理，假设当前时刻为t: 下多约束二次规划问题： x(t+1lt)=x(tlt)+Tf(x(tlt),u(tlt))(7) minJ(e(t),u(t))= 40tE 假设预测时域为N。,控制时域为N。,则在预测 时域内，每一时刻车辆的位姿状态为： Ie+1i)I6+龙 △u(t+ilt)I,+ x(t+1lt)=x(ilt)+Tf(x(tlt),u(tlt)) Iδ(t+ilt)a,+Isla4 x(t+N +1lt)=x(t+Nlt)+Tf(x(t+N lt),u(t+Nlt)) s.t. -0.1ms1≤△w≤0.1ms1 -0.0164rad≤△δ≤0.0164rad r(t+N,l)=x(t+N。-llt)+Tx(t+N。-lt),u(t+N。lt) -0.44rad≤6≤0.44rad (8) (14) 预测时域内车辆位姿与参考路径之间的偏差即 求解式(14)可得一个最优控制序列，其中第一 预测获得的误差： 个元素即控制器在下一时刻输出的控制变量. e(t+ilt)=x(t+ilt)-x(t+ilt), 3仿真验证 i=1,2,…,Np (9) 其中，x即参考路径上跟踪目标点的位姿状态 基于非线性模型预测控制的自动泊车路径跟踪 信息. 控制器通过MATLAB/Simulink和PreScan进行联合顾 青等: 基于非线性模型预测控制的自动泊车路径跟踪 慢,控制周期 T 被设置为0郾 1 s,即控制频率为10 Hz. 转向角增量 驻啄 通过下式计算: 驻啄 = T 啄 · (3) 式中,啄 · 为转向角速度速度. 对于该值的约束,北京 理工大学的龚建伟教授团队进行了基于实际车辆的 实验测试[16] ,实验结果测得该值的上下限设置为 依 9郾 4毅·s - 1 . 转向角 啄 的上下限也根据龚建伟教授团队的测 试结果[16]设置,其值为 依 25毅. 综合上述约束,并将角度转化为弧度,基于非线 性模型预测控制的自动泊车路径跟踪控制系统的约 束条件为: - 0郾 1 m·s - 1臆驻v臆0郾 1 m·s - 1 - 0郾 0164 rad臆驻啄臆0郾 0164 rad - 0郾 44 rad臆啄臆 ì î í ïï ïï 0郾 44 rad (4) 2 自动泊车路径跟踪控制器设计 设计非线性模型预测制器的核心是建立预测模 型和设计滚动优化函数. 在路径跟踪控制器中,预 测模型的作用是根据车辆当前的位姿状态和未来可 能的控制输入,预测未来可能的位姿状态,所以基于 车辆的运动学模型(1)建立预测模型,将(1)向量化 可得: x · = f(x,u) (5) 其中: x = [x y 兹] T u = [v 啄] { T (6) 采用欧拉法(Euler method) 对式(5) 进行离散 化处理,假设当前时刻为 t: x(t + 1 | t) = x(t | t) + Tf(x(t | t),u(t | t)) (7) 假设预测时域为 Np ,控制时域为 Nc,则在预测 时域内,每一时刻车辆的位姿状态为: x(t +1|t) = x(t|t) + Tf(x(t|t),u(t|t)) 左 x(t +Nc +1|t) = x(t +Nc |t) + Tf(x(t +Nc |t),u(t +Nc |t)) 左 x(t +Np |t) = x(t +Np -1|t) + Tf(x(t +Np -1|t),u(t +Nc |t)) (8) 预测时域内车辆位姿与参考路径之间的偏差即 预测获得的误差: e(t + i | t) = x(t + i | t) - xref(t + i | t), i = 1,2,…,Np (9) 其中,xref 即参考路径上跟踪目标点的位姿状态 信息. 跟踪目标点的选取主要基于车辆当前位置与参 考速度,假设参考路径为参数化的函数 g( x,y,兹), 则第 0 个跟踪目标点满足: d(x(t | t),xref(t | t)) = dmin (x(t | t),g(x,y,兹)) (10) 其中,d 为求解距离的函数. 预测时域内的其余跟踪目标点满足: sg(xref(t + i | t),xref(t + i - 1 | t)) = Tvref, i = 1,2,…,Np (11) 其中,sg 为两个跟踪目标点之间参考路径的弧长, vref为人为设定的参考速度. 式(9) ~ ( 11 ) 即自动泊车路径跟踪控制器 的预测模型. 基于上述模型,滚动优化目标函数 可设置为: J(e(t),u(t)) = 移 Np i = 1 椰e(t + 1 | t)椰2 Q1 + 移 Np i = 1 椰驻u(t + i | t)椰2 Q2 + 移 Np i = 1 椰啄(t + i | t)椰2 Q3 + 椰着椰2 Q4 (12) 式(12) 中,第一个惩罚项是误差惩罚项,第二 个惩罚项的作用是使自动泊车路径尽可能地平稳, 第三个惩罚项的功能是防止微小扰动导致转向轮的 转向角过于频繁地变化,着 为松弛因子,Q1 、Q2 、Q3 和 Q4 为权重系数矩阵. 驻u(t + i | t)为控制量增量: 驻u(t + i | t) = u(t + i | t) - u(t + i - 1 | t), i = 1,2,…,Np (13) 至此,自动泊车路径跟踪控制问题即转化为如 下多约束二次规划问题: min u(t),着 J(e(t),u(t)) = 移 Np i = 1 椰e(t + 1 | t)椰2 Q1 + 移 Np i = 1 椰驻u(t + i | t)椰2 Q2 + 移 Np i = 1 椰啄(t + i | t)椰2 Q3 + 椰着椰2 Q4 s. t. - 0郾 1 m·s - 1臆驻v臆0郾 1 m·s - 1 - 0郾 0164 rad臆驻啄臆0郾 0164 rad - 0郾 44 rad臆啄臆0郾 44 rad (14) 求解式(14)可得一个最优控制序列,其中第一 个元素即控制器在下一时刻输出的控制变量. 3 仿真验证 基于非线性模型预测控制的自动泊车路径跟踪 控制器通过 MATLAB / Simulink 和 PreScan 进行联合 ·949·