将博客搬至CSDN

对与θH ,由于AC⊥CE ,所以∠CAB=θH'=θH .此时满足

tanθH=tanθH'=Lrt-1'          [hz2] ①

tanθH=tanθH'=Lrt-1'     （1）

可由角速度关系

ωt-1=Vt-1rt-1          ②

可得ω[hz3] =VLtanθHt-1

由于系统中dt 非常小,所以近似为1,dt 内自车中心D 以角速度ω 匀速转动，因此

yaw=yaw'[hz4] +dtω

yawt=yawt-1+Vt-1LtanθHt-1dt

获得当前车辆实际姿态角后，

xt=xt-1+cosyawt-1Vt-1dt

yt=yt-1+sinyawt-1Vt-1dt

在模型中α 为已知量(控制量)

自车运动状态的数学模型如下

xt=xt-1+αtcosθHt-1dt

yt=yt-1+αtsinθHt-1dt

yawt=yawt+Vt-1LtanθHt-1dt

vt=vt-1+αtdt

lqr轨迹跟踪模型

θH ---------------------转向角

yaw ---------------------姿态角

yaw' ---------------------x2 点的目标姿态角

d1 ---------------------t 时刻距x1 点的距离

d2 ---------------------t 时刻距x2 点的距离

D ---------------------自车中心点

lqr原理：t 时刻自车更新当前所在位置D 的坐标并计算出与目标轨迹距离最近的点（），x2 点对应的速度大小，航向角yaw' ，曲率与速度期望值是确定的，在t 时刻自车也存在一个当前状态(state) 实际的航向角yawtVt 与曲率半径，通过当前状态与期望点的误差值进行lqr线性二次计算可得到最佳的输出加速度α 与前轮转向角θH ，从而使自车快速以最小的时间成本到达期望点x2 。此后期望点将不断后移，直到达到终点。

相关数学模型

xt=xt-1+xt-1dt

xt=∆θt-1Vt-1

∆θt=∆θt-1+ωt-1dt

ωt=VLθHt-1

Vt=Vt-1+αt-1dt[hz5] 

其中xt 为当前位置与最近点的距离，xt 表示当前自车与期望目标点距离的变化率，∆θt 表示当前姿态角变化率vt 表示当前速度

令xt=xtxt∆θtωtVt

则 xt=10000 dt0000 0Vt-1100 00dt00 00001 . xt-1xt-1∆θt-1ωt-1θHt-1+ 000VL0 0000dt . θHt-1αt-1

上式称为系统的离散状态空间方程，该方程中所有变量可以完全表示系统当前状态，只要我们在t 时刻对系统施加一个确定的前轮转向角θHt 和确定的加速度αt ，系统就能被改变。称θHtαt 为控制量。

lqr的计算任务是在获取xt 的状态下的状态量后，根据线性算法计算出t 时刻最优的控制量θHα ，使得系统的xt 能更快更平稳地趋于目标轨迹，由于该系统的时间非连续，存在dt ，所以采用离散lqr进行求解

在使用时会出现连续时间与离散时间的不同推导过程，在连续时间下，系统的状态发生变化是上一个状态受到外界干扰或者系统输入量影响而导致的改变。

而对于一个离散时间系统

xt+1=Axt+But,x0=xinit                                                                          （1）[hz8] 

其中，A∈Rn×n ，B∈Rn×m[hz9] 

关于最优问题，就在于如何选择合适的 u0,u1,...[hz10]  ，使得状态量 x0,x1,...  足够小，因此得到好的调节和控制；或者使得 u0,u1,...  足够小，以使用更少的能量。这两个量通常相互制约，如果采用更大的输入u  ，就会驱使状态量 x  更快达到 0。采用线性二次调节原理可以解决这个问题。

这里先给出lqr的计算过程再逐一解释：

• 计算出目标轨迹点，确定迭代参数N ，并得出轨迹中每一离散点的目标切线切线曲率、速度
• 设置迭代初始值PN=Qf，Qf=Q
• 循环迭代：Pt-1=Q+ATPtA-ATPtB(R+BTPtB)-1BTPtA
• 从t=0,…N-1，计算反馈系数kt=(R+BTPt+1B)-1BTPt+1A
• 得到优先的控制量Ut*=-ktxt
• 更新自车状态并判断是否到达终点，若否，则重复第③步。[hz11] 

为了使系统趋于0，我们设定了代价函数[hz12] 

J(U)=k=1N(XkTQXk+ukTRuk)                                              (2)

                                   J(U)=τ=0N-1 (XτTQXτ+uτTRuτ)+XQfXN                            (3)

(2)(3)代价方程是等价的，(2)式将最后一项分离了出来便于观察迭代过程。

一般来说，矩阵Q与矩阵R为对角矩阵，对角矩阵的元素对应着不同的状态控制量的权重，越大说明对该量的重视程度越大。即希望这个量在变化过程中保持较小的值。

其中函数参数 U=(u0,u1,..,uN) ，并且矩阵 Q,Qf,R  为正定矩阵，及

Q=QT≥0,Qf=QfT≥0,R=RT>0

Q 给定状态代价矩阵

Qf 最终状态代价矩阵

R 输入代价矩阵

N ：时间范围 (考虑 )

Q,R ：分别设定状态偏差和输入的相对权重

R>0 ：意味着任何非零输入都增加 J 的代价

xτTQxτ ：衡量状态偏差

uτTRuτ ：衡量输入大小

xNTQfxN ：衡量最终状态偏差

因此，关于 LQR 问题就是找出使得代价函数 J(U)  最小的一组控制输入(u0,u1,...,uN-1)lqr  。

求解：

       求解时使用动态规划法[hz13] 解决

首先定义一个值函数 Vt:Rn→R , 其中 ：t=(0,…,N)

Vt(z)=minut,…,uN-1(τ=tN-1 (xτTQxτ+uτtRuτ)+xNTQfxN)                                （4）

如果设置 xt=z ，根据公式 (1) 的关系，xτ+1=Axτ+Buτ , 并且 τ=t,…,N 。

 Vt(z) 可以表示在 t  时刻，从状态 z  开始的 LQR 最小代价值

 V0(x0) 表示在 0 时刻，从状态 x0  开始的 LQR 最小代价值

 Vt 可以表示为二次型的形式，即 VT(z)=zTPtz , 其中 Pt=PtT≥0

。当 t=N  时，代价值函数为：

VN(z)=zTQfz                                                                                          （5）

因此 。PN=Qf

根据动态规划原理，等式 (4) 可以写成如下递归关系式：

              Vt(z)=minw(zTQz+wTRw+Vt+1(Az+Bw))                                            （6）

其中，

zTQz+wTRw ：如果 , 则代表 ut=w  时刻产生的代价值；

Vt+1(Az+Bw) ：代表从 t+1  时刻开始，引起的最小代价值；

提取等式 (6) 中与 w  无关的选项得

Vt(z)=zTQz+minw(wTRw+Vt+1(Az+Bw))                                               （7）

等式 (7) 描述了 Vt(z)  与 Vt+1(z)  之间的递归关系。

最优控制率 ut  可以表示为[hz14] 

             utlqr=argminw(wTRw+Vt+1(Az+Bw))                                                    （8）

求极值

假设 Vt+1=zTPt+1z  , 并且 Pt+1=Pt+1T≥0

, 等式 (10) 可以进一步转化为 Pt+1  的形式：

             Vt(z)=zTQz+minw(wTRw+(Az+Bw)TPt+1(Az+Bw))                          （9）

为了求最小值，对 w  求导，导数为零的点即为最值点。

 2wTR+2(Az+Bw)TPt+1B=0                                                               （10）

推导等式 (10)，求取 w :

wTR+zTATPt+1B+wTBTPt+1B=0wT(R+BTPt+1B)=-zTATPt+1B(合并同类项并移项[hz15] )(R+BTPt+1B)Tw=-BTPt+1TAz(转置)(R+BTPt+1B)w=-BTPt+1Az(Pt+1=Pt+1T,R=RT)w=-(R+BTPt+1B)-1BTPt+1Az(矩阵求逆) (11)

由等式 (11) 可知，最优输入为

             w*=-(R+BTPt+1B)-1BTPt+1Az                                                              （12）

将等式 (12) 带入等式 (9) 得

 Vt(z)=zTQz+w*TRw*+(Az+Bw*)TPt+1(Az+Bw*)                           （13）

对等式 (13) 化简得

 Vt(z)=zTQz+w*TRw*+(Az+Bw*)TPt+1(Az+Bw*)=zTQz+w*TRw*+zTATPt+1Az+2zTATPt+1Bw*+w*TBTPt+1Bw*=zTQz+zTATPt+1Az+w*T(R+BTPt+1B)w*+2zTATPt+1Bw*=zTQz+zTATPt+1Az+zTATPt+1B(R+BTPt+1B)-1(R+BTPt+1B)(R+BTPt+1B)-1BTPt+1Az-2zTATPt+1B(R+BTPt+1B)-1BTPt+1Az=zTQz+zTATPt+1Az+zTATPt+1B(R+BTPt+1B)-1BTPt+1Az-2zTATPt+1B(R+BTPt+1B)-1BTPt+1Az=zTQz+zTATPt+1Az-zTATPt+1B(R+BTPt+1B)-1BTPt+1Az=zT(Q+ATPt+1A-ATPt+1B(R+BTPt+1B)-1BTPt+1A)z=zTPtz[hz16]  （14）

上述公式化简过程中，由于 Pt+1=Pt+1T,R=RT

，所以 ((R+BTPt+1B)-1)T=(R+BTPt+1B)-1  。

由等式 (14) 可知

 Pt=Q+ATPt+1A-ATPt+1B(R+BTPt+1B)-1BTPt+1A                                    （15）

求解过程

关于 LQR 的求解过程，可以采用动态规划算法，依据上述公式 (18) 的递归关系，反向递推，求出满足一定条件的最小代价值。

确定迭代范围 N

设置迭代初始值 PN=Qf

循环迭代，t=N,…,1

Pt-1=Q+ATPt+1A-ATPt+1B(R+BTPt+1B)-1BTPt+1A

则反馈系数 Kt=-(R+BTPt+1B)-1BTPt+1A ，对于时间 t=0,…,N-1

优化的控制量utlqr=Ktxt