自动驾驶|自动驾驶算法-滤波器系列（五）——高级运动模型在UKF中的应用人工智能|算法

CTRV_UKF, CTRA_UKF

1. 基于CTRV的UKF
- 1.1 CTRV模型
- 1.2 UKF模型
2. 基于CTRA的UKF
- 2.1 CTRA模型

【自动驾驶|自动驾驶算法-滤波器系列（五）——高级运动模型在UKF中的应用】扩展卡尔曼滤波算法是对非线性的系统方程或者观测方程进行泰勒展开并保留其一阶近似项，这样会不可避免引入线性化误差。无迹卡尔曼滤波摒弃了对非线性函数进行线性化的传统做法，采用卡尔曼线性滤波框架，对于一步预测方程，使用无迹变换来处理均值和协方差的非线性传递问题。UKF算法是对非线性函数的概率密度分布进行近似，用一系列确定样本来逼近状态的后延概率密度，而不是对非线性函数进行近似，不需要对雅各比矩阵进行求导。UKF没有忽略高阶项，因此对于非线性分布的统计量有较高的计算精度，有效地克服了扩展卡尔曼滤波的估计精度低、稳定性差的缺陷。
1. 基于CTRV的UKF 1.1 CTRV模型 CTRV模型如下：
选取的状态空间为：
x = ( x , y , v , ψ , ω ) T x=(x,y,v,\psi,\omega)^T x=(x,y,v,ψ,ω)T
上式中变量依次为：x坐标，y坐标，速度，偏航角（与x夹角逆时针为正），角速度。
状态方程为：
{ x k + 1 y k + 1 v k + 1 ψ k + 1 ω k + 1 } = { x k + v k ω k ( sin ? ( ψ k + ω k Δ t ) ? sin ? ( ψ k ) ) y k + v k ω k ( ? cos ? ( ψ k + ω k Δ t ) + cos ? ( ψ k ) ) v k ψ k + ω k Δ t ω k } + { 1 2 υ a , k cos ? ( ψ k ) Δ t 2 1 2 υ a , k sin ? ( ψ k ) Δ t 2 Δ t υ a , k 1 2 υ ψ . . , k Δ t 2 Δ t υ ψ . . , k } , ω k ≠ 0 \left\{\begin{matrix}x_{k+1}\\y_{k+1}\\v_{k+1}\\\psi_{k+1}\\\omega_{k+1}\end{matrix}\right\}= \left\{\begin{matrix} x_k+\frac{v_k}{\omega_{k}}(\sin(\psi_k +\omega_{k} \Delta t)-\sin(\psi_k))\\ y_k+\frac{v_k}{\omega_{k}}(-\cos(\psi_k +\omega_{k} \Delta t)+\cos(\psi_k))\\ v_k\\ \psi_k+\omega_{k}\Delta t\\ \omega_{k} \end{matrix}\right\} + \left\{\begin{matrix} \frac{1}{2}\upsilon _{a,k}\cos(\psi_k)\Delta t^2\\ \frac{1}{2}\upsilon _{a,k}\sin(\psi_k)\Delta t^2\\ \Delta t \upsilon _{a,k}\\ \frac{1}{2}\upsilon _{\mathop{\psi}\limits^{..},k}\Delta t^2\\ \Delta t \upsilon _{\mathop{\psi}\limits^{..},k} \end{matrix}\right\}, \quad \omega_{k}\neq0 ????????????xk+1?yk+1?vk+1?ψk+1?ωk+1??????????????=????????????xk?+ωk?vk??(sin(ψk?+ωk?Δt)?sin(ψk?))yk?+ωk?vk??(?cos(ψk?+ωk?Δt)+cos(ψk?))vk?ψk?+ωk?Δtωk??????????????+????????????21?υa,k?cos(ψk?)Δt221?υa,k?sin(ψk?)Δt2Δtυa,k?21?υψ..?,k?Δt2Δtυψ..?,k??????????????,ωk??=0
{ x k + 1 y k + 1 v k + 1 ψ k + 1 ω k + 1 } = { x k + v k cos ? ( ψ k ) Δ t y k + v k sin ? ( ψ k ) Δ t v k ψ k + ω k Δ t ω k } + { 1 2 υ a , k cos ? ( ψ k ) Δ t 2 1 2 υ a , k sin ? ( ψ k ) Δ t 2 Δ t υ a , k 1 2 υ ψ . . , k Δ t 2 Δ t υ ψ . . , k } , ω k = 0 \left\{\begin{matrix}x_{k+1}\\y_{k+1}\\v_{k+1}\\\psi_{k+1}\\\omega_{k+1}\end{matrix}\right\}= \left\{\begin{matrix} x_k+v_k\cos(\psi_k)\Delta t\\ y_k+v_k\sin(\psi_k)\Delta t\\ v_k\\ \psi_k+\omega_{k}\Delta t\\ \omega_{k} \end{matrix}\right\} + \left\{\begin{matrix} \frac{1}{2}\upsilon _{a,k}\cos(\psi_k)\Delta t^2\\ \frac{1}{2}\upsilon _{a,k}\sin(\psi_k)\Delta t^2\\ \Delta t \upsilon _{a,k}\\ \frac{1}{2}\upsilon _{\mathop{\psi}\limits^{..},k}\Delta t^2\\ \Delta t \upsilon _{\mathop{\psi}\limits^{..},k} \end{matrix}\right\}, \quad \omega_{k}=0 ????????????xk+1?yk+1?vk+1?ψk+1?ωk+1??????????????=????????????xk?+vk?cos(ψk?)Δtyk?+vk?sin(ψk?)Δtvk?ψk?+ωk?Δtωk??????????????+????????????21?υa,k?cos(ψk?)Δt221?υa,k?sin(ψk?)Δt2Δtυa,k?21?υψ..?,k?Δt2Δtυψ..?,k??????????????,ωk?=0
对于 H H H矩阵，根据传感器采集到的数据（x坐标，y坐标，偏航角）以及状态空间的大小，确定 H H H矩阵：
H = { 1 0 0 0 0 0 1 0 0 0 0 0 0 1 0 } H=\left\{\begin{matrix}1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 &0\\ 0 & 0 & 0 & 1 & 0 \end{matrix}\right\} H=????100?010?000?001?000?????
Q Q Q矩阵的推导见不同运动模型的建模和推导，此处直接给出结论：
Q = c o v ( υ ) = E [ ( G u ) ( G u ) T ] = G E [ u 2 ] G T = G { σ a k 2 0 0 σ ψ k . . 2 } G T Q=cov(\upsilon)=E[(Gu)(Gu)^T]=GE[u^2]G^T=G\left\{\begin{matrix}{\sigma_{a_k}}^2 & 0\\0 & {\sigma_{\mathop{\psi_k}\limits^{..}}}^2\end{matrix}\right\}G^T Q=cov(υ)=E[(Gu)(Gu)T]=GE[u2]GT=G{σak??20?0σψk?..??2?}GT
Q = { 1 2 cos ? ( ψ k ) Δ t 2 0 1 2 sin ? ( ψ k ) Δ t 2 0 Δ t 0 0 1 2 Δ t 2 0 Δ t } { σ a k 2 0 0 σ ψ k . . 2 } { 1 2 cos ? ( ψ k ) Δ t 2 0 1 2 sin ? ( ψ k ) Δ t 2 0 Δ t 0 0 1 2 Δ t 2 0 Δ t } T Q=\left\{\begin{matrix} \frac{1}{2}\cos(\psi_k)\Delta t^2 & 0\\ \frac{1}{2}\sin(\psi_k)\Delta t^2 & 0\\ \Delta t & 0\\ 0 & \frac{1}{2}\Delta t^2\\ 0 & \Delta t \end{matrix}\right\} \left\{\begin{matrix}{\sigma_{a_k}}^2 & 0\\0 & {\sigma_{\mathop{\psi_k}\limits^{..}}}^2\end{matrix}\right\} \left\{\begin{matrix} \frac{1}{2}\cos(\psi_k)\Delta t^2 & 0\\ \frac{1}{2}\sin(\psi_k)\Delta t^2 & 0\\ \Delta t & 0\\ 0 & \frac{1}{2}\Delta t^2\\ 0 & \Delta t \end{matrix}\right\}^T Q=????????????21?cos(ψk?)Δt221?sin(ψk?)Δt2Δt00?00021?Δt2Δt?????????????{σak??20?0σψk?..??2?}????????????21?cos(ψk?)Δt221?sin(ψk?)Δt2Δt00?00021?Δt2Δt?????????????T
P P P矩阵的初始化选择单位矩阵 I 5 x 5 I_{5x5} I5x5?初始化。 R R R矩阵为测量噪声矩阵，采用对角矩阵初始化。 Q Q Q矩阵的噪声参数也需要根据后期调参来确定。
1.2 UKF模型 UKF为无迹卡尔曼滤波，采用UT转换将状态转移方程线性化。本文档只有状态转移矩阵为非线性的，观测矩阵可以由要观测的数据直接得到，为线性的。故只需要对状态转移矩阵进行UT变换即可。

文章图片

选取sigma点
计算2n+1个Sigma点，即采样点，这里的n为状态的维数，本文档中n为5.
X ( 0 ) = X ? i = 0 X^{(0)} = \mathop{X}\limits^- \quad i=0 X(0)=X??i=0
X ( i ) = X ? + ( ( n + λ ) P ) i , i = 1 ～ n X^{(i)} = \mathop{X}\limits^- + (\sqrt{(n+\lambda)P)}_i, i=1 \sim n X(i)=X??+((n+λ)P) ?i?,i=1～n
X ( i ) = X ? ? ( ( n + λ ) P ) i , i = n + 1 ～ 2 n X^{(i)} = \mathop{X}\limits^- - (\sqrt{(n+\lambda)P)}_i, i=n+1 \sim 2n X(i)=X???((n+λ)P) ?i?,i=n+1～2n
式中， P T ( P ) = P , ( P ) i \sqrt{P}^T(\sqrt{P}) = P, (\sqrt{P})_i P ?T(P ?)=P,(P ?)i?表示矩阵方根的第i列。

X ? \mathop{X}\limits^- X?? 取上一个状态的后验估计，取上一个状态的后验估计，取上一个状态的后验估计，P$为上一时刻的协方差矩阵。

设置权重值
ω m ( n ) = λ n + λ \omega_m^{(n)} = \frac{\lambda}{n+\lambda} ωm(n)?=n+λλ?
ω c ( n ) = λ n + λ + ( 1 ? α 2 + β ) \omega_c^{(n)} = \frac{\lambda}{n+\lambda} + (1-\alpha^2+\beta) ωc(n)?=n+λλ?+(1?α2+β)
ω m ( i ) = ω c ( i ) = 1 2 ( n + λ ) , i = 1 ～ 2 n \omega_m^{(i)} = \omega_c^{(i)} = \frac{1}{2(n+\lambda)}, i = 1 \sim 2n ωm(i)?=ωc(i)?=2(n+λ)1?,i=1～2n

式中，下标m为均值，c为协方差，上标为第几个采样点。参数 λ = α 2 ( n + κ ) ? n \lambda = \alpha^2(n + \kappa) -n λ=α2(n+κ)?n是一个缩放比例参数，用来降低总的预测误差，a的选取控制了采样点的分布状态， κ \kappa κ为待选参数，其具体数值虽然没有界限，但通常应确保矩阵 ( n + λ ) P (n+\lambda)P (n+λ)P为半正定矩阵。待选参数 β ≥ 0 \beta \ge 0 β≥0是一个非负的权系数，它可以合并方程中高阶项的动差，这样就可以把高阶项的影响包括在内。要确保权重值之和为1。

预测，计算均值和协方差矩阵
计算系统状态量的一步预测及协方差矩阵。它由Sigma点集的预测值加权求和得到。将选取的Sigma点通过非线性系统得到预测的结果。UKF不同于传统的卡尔曼滤波算法，利用一组Sigma点的预测，并计算对它们加权求均值，得到系统状态量的一步预测。

X ^ ( k + 1 ∣ k ) = ∑ i = 0 2 n ω ( i ) X ( i ) ( k + 1 ∣ k ) \hat{X}(k+1|k)=\sum_{i=0}^{2n}\omega^{(i)}X^{(i)}(k+1|k) X^(k+1∣k)=i=0∑2n?ω(i)X(i)(k+1∣k) P ( k + 1 ∣ k ) = ∑ i = 0 2 n ω ( i ) [ X ^ ( k + 1 ∣ k ) ? X ( i ) ( k + 1 ∣ k ) ] [ X ^ ( k + 1 ∣ k ) ? X ( i ) ( k + 1 ∣ k ) ] T + Q P(k+1|k) = \sum_{i=0}^{2n}\omega^{(i)}[\hat{X}(k+1|k)-X^{(i)}(k+1|k)][\hat{X}(k+1|k)-X^{(i)}(k+1|k)]^T+Q P(k+1∣k)=i=0∑2n?ω(i)[X^(k+1∣k)?X(i)(k+1∣k)][X^(k+1∣k)?X(i)(k+1∣k)]T+Q

计算卡尔曼增益矩阵，并更新状态和协方差
观测系统是线性的，故观测方程不需要线性化，直接将先验期望带入即可。

卡尔曼增益矩阵公式如下：
K k = P k ? H T ( H P k ? H T + R ) ? 1 K_k={P_k^-}H^T(H{P_k^-}H^T+R)^{-1} Kk?=Pk??HT(HPk??HT+R)?1
P k ? P_k^- Pk??即为上面得到的预测的协方差。
方差更新：
P k = ( I ? K k H ) P k ? P_k=(I-K_kH)P_k^- Pk?=(I?Kk?H)Pk??
在传统UKF滤波算法中，需要对每个采样点进行非线性变换，计算量比较大，而且数值计算的误差也比较明显，估计误差协方差矩阵的非负定性和对称性会因此受到影响，从而影响滤波算法的收敛速度以及稳定性。为了提高滤波算法的滤波效率以及滤波精度，在递推运算过程中采用协方差矩阵的平方根来代替传统算法计算过程中的协方差矩阵，这种方法称为平方根无迹卡尔曼滤波算法。
2. 基于CTRA的UKF 2.1 CTRA模型 CTRA模型如下：
选取的状态空间为：
x = ( x , y , ψ , v , a , ω ) T x=(x,y,\psi,v,a,\omega)^T x=(x,y,ψ,v,a,ω)T
上式子中变量依次为：x坐标，y坐标，偏航角，速度，加速度，角速度。
状态方程为：
{ x k + 1 y k + 1 ψ k + 1 v k + 1 a k + 1 ω k + 1 } = { x k y k ψ k v k a k ω k } + { a ω 2 ( cos ? ( ψ k + ω Δ t ) ? cos ? ( ψ k ) ) + 1 ω ( ( v k + a Δ t ) sin ? ( ψ k + ω Δ t ) ? v k sin ? ( ψ k ) ) a ω 2 ( sin ? ( ψ k + ω Δ t ) ? sin ? ( ψ k ) ) ? 1 ω ( ( v k + a Δ t ) cos ? ( ψ k + ω Δ t ) ? v k cos ? ( ψ k ) ) ω Δ t a Δ t 0 0 } + { 1 6 a . Δ t 3 cos ? ( ψ k + 1 2 ω . Δ t 2 ) 1 6 a . Δ t 3 sin ? ( ψ k + 1 2 ω . Δ t 2 ) 1 2 ω . Δ t 2 1 2 a . Δ t 2 a . Δ t ω . Δ t } \left\{\begin{matrix}x_{k+1}\\y_{k+1}\\\psi_{k+1}\\v_{k+1}\\a_{k+1}\\\omega_{k+1}\end{matrix}\right\} =\left\{\begin{matrix}x_{k}\\y_{k}\\\psi_{k}\\v_{k}\\a_{k}\\\omega_{k}\end{matrix}\right\} + \left\{\begin{matrix} \frac{a}{\omega ^2}(\cos(\psi_k+\omega \Delta t)-\cos(\psi_k))+\frac{1}{\omega}((v_k+a\Delta t)\sin(\psi_k+\omega\Delta t)-v_k\sin(\psi_k))\\ \frac{a}{\omega ^2}(\sin(\psi_k+\omega \Delta t)-\sin(\psi_k))-\frac{1}{\omega}((v_k+a\Delta t)\cos(\psi_k+\omega\Delta t)-v_k\cos(\psi_k))\\ \omega \Delta t\\ a\Delta t\\ 0\\ 0 \end{matrix}\right\} \\ \quad + \left\{\begin{matrix} \frac{1}{6}\mathop{a}\limits^. \Delta t^3 \cos(\psi_k+\frac{1}{2}\mathop{\omega}\limits^. \Delta t^2)\\ \frac{1}{6}\mathop{a}\limits^. \Delta t^3 \sin(\psi_k+\frac{1}{2}\mathop{\omega}\limits^. \Delta t^2)\\ \frac{1}{2}\mathop{\omega}\limits^.\Delta t^2\\ \frac{1}{2}\mathop{a}\limits^.\Delta t^2\\ \mathop{a}\limits^. \Delta t\\ \mathop{\omega}\limits^. \Delta t \end{matrix}\right\} ????????????????xk+1?yk+1?ψk+1?vk+1?ak+1?ωk+1??????????????????=????????????????xk?yk?ψk?vk?ak?ωk??????????????????+????????????????ω2a?(cos(ψk?+ωΔt)?cos(ψk?))+ω1?((vk?+aΔt)sin(ψk?+ωΔt)?vk?sin(ψk?))ω2a?(sin(ψk?+ωΔt)?sin(ψk?))?ω1?((vk?+aΔt)cos(ψk?+ωΔt)?vk?cos(ψk?))ωΔtaΔt00?????????????????+????????????????61?a.Δt3cos(ψk?+21?ω.Δt2)61?a.Δt3sin(ψk?+21?ω.Δt2)21?ω.Δt221?a.Δt2a.Δtω.Δt?????????????????
对于 H H H矩阵，根据传感器采集到的数据（x坐标，y坐标，偏航角）以及状态空间的大小，确定 H H H矩阵：
H = { 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 } H=\left\{\begin{matrix}1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 &0 & 0\\ 0 & 0 & 1 & 0 & 0 & 0\end{matrix}\right\} H=????100?010?001?000?000?000?????
针对 Q Q Q矩阵和 R R R矩阵中的未知参数，需要后期调参来确定。
Q Q Q矩阵为过程噪声的协方差矩阵，通过分析对系统的影响量，得到 Q Q Q矩阵的数学表达式：
Q = c o v ( υ ) = E [ ( G u ) ( G u ) T ] = G E [ u 2 ] G T = G { σ a k . 2 0 0 σ ω . 2 } G T Q=cov(\upsilon)=E[(Gu)(Gu)^T]=GE[u^2]G^T=G\left\{\begin{matrix}{\mathop{\sigma a_k}\limits^{.}}^2 & 0\\0 & {\sigma_{\mathop{\omega}\limits^{.}}}^2\end{matrix}\right\}G^T Q=cov(υ)=E[(Gu)(Gu)T]=GE[u2]GT=G{σak?.?20?0σω.?2?}GT
Q = { 1 6 a . Δ t 3 cos ? ( ψ k + 1 2 ω . Δ t 2 ) 1 6 a . Δ t 3 sin ? ( ψ k + 1 2 ω . Δ t 2 ) 1 2 ω . Δ t 2 1 2 a . Δ t 2 a . Δ t ω . Δ t } Q = \left\{\begin{matrix} \frac{1}{6}\mathop{a}\limits^. \Delta t^3 \cos(\psi_k+\frac{1}{2}\mathop{\omega}\limits^. \Delta t^2)\\ \frac{1}{6}\mathop{a}\limits^. \Delta t^3 \sin(\psi_k+\frac{1}{2}\mathop{\omega}\limits^. \Delta t^2)\\ \frac{1}{2}\mathop{\omega}\limits^.\Delta t^2\\ \frac{1}{2}\mathop{a}\limits^.\Delta t^2\\ \mathop{a}\limits^. \Delta t\\ \mathop{\omega}\limits^. \Delta t \end{matrix}\right\} Q=????????????????61?a.Δt3cos(ψk?+21?ω.Δt2)61?a.Δt3sin(ψk?+21?ω.Δt2)21?ω.Δt221?a.Δt2a.Δtω.Δt?????????????????