无人驾驶技术——CTRV模型
- 1. 一次运动模型
-
- 1.1 等速模型
- 1.2 匀加速模型
- 2. 二次运动模型
-
- 2.1 恒定转弯率和速度模型
无人驾驶的物体跟踪运动模型有很多种，包括：
1 2 3 4 5 6 | 恒定速度模型：CV----Constant Velocity 恒定加速度模型：CA----Constant Acceleration 恒定转弯率和速度模型：CTRV----Constant Turn Rate and Velocity 恒定转弯率和加速度模型：CTRA----Constant Turn Rate and Acceleration 恒定转向角和速度模型：CSAV----Constant Steering Angle and Velocity 常曲率和加速度模型：CCA----Constant Curvature and Acceleration |
1. 一次运动模型
一次运动模型也称线性运动模型，有：
- 恒定速度模型（Constant Velocity, CV）
- 恒定加速度模型（Constant Acceleration, CA）
这些线性运动模型假定目标是直线运动的，并不考虑物体的转弯。
1.1 等速模型
等速模型是用于物体跟踪的最基本的运动模型之一：
- 状态空间可以表示为：
x
?
(
t
)
=
(
x
y
v
x
v
y
)
T
\vec{x}(t)=\left(\begin{array}{llll} x & y & v_{x} & v_{y} \end{array}\right)^{T}
x
x
?
(
t
+
Δ
t
)
=
(
x
(
t
)
+
Δ
t
v
x
y
(
t
)
+
Δ
t
v
y
v
x
v
y
)
\vec{x}(t+\Delta t)=\left(\begin{array}{c} x(t)+\Delta t v_{x} \\ y(t)+\Delta t v_{y} \\ v_{x} \\ v_{y} \end{array}\right)
x
- 状态变量表示方式 II：
C
V
X
=
(
x
y
?
v
)
T
\begin{array}{llll} \mathrm{CV}_{\mathbf{X}}=\left(\begin{array}{llll} x & y & \vartheta & v \end{array}\right)^{\mathrm{T}} \end{array}
CVX?=(x?y???v?)T?
四个状态变量依次为横坐标，纵坐标，与 x 轴夹角（逆时针为正），线速度。
状态转移方程：x
k
+
1
=
x
k
+
(
v
cos
?
(
?
)
T
k
v
sin
?
(
?
)
T
k
0
0
)
\mathbf{x}_{k+1}=\mathbf{x}_{k}+\left(\begin{array}{c} v \cos (\vartheta) T_{k} \\ v \sin (\vartheta) T_{k} \\ 0 \\ 0 \end{array}\right)
xk+1?=xk?+?????vcos(?)Tk?vsin(?)Tk?00??????
- 局限：
假设速度是常量，我们实际上简化了车辆实际移动的形式，因为大多数车辆道路是有拐弯的，但速度是常量的模型会无法正确预测拐弯车辆。
1.2 匀加速模型
- 状态变量：
C
A
X
=
(
x
y
?
v
a
)
T
\begin{array}{lllll} \mathrm{CA}_{\mathrm{X}}=\left(\begin{array}{lllll} x & y & \vartheta & v & a \end{array}\right)^{\mathrm{T}} \end{array}
CAX?=(x?y???v?a?)T?
四个状态变量依次为横坐标，纵坐标，与 x 轴夹角（逆时针为正），线速度，加速度。
状态转移方程：x
k
+
1
=
x
k
+
(
(
v
k
T
k
+
a
2
T
k
2
)
cos
?
(
?
)
(
v
k
T
k
+
a
2
T
k
2
)
sin
?
(
?
)
0
a
T
k
0
)
\mathbf{x}_{k+1}=\mathbf{x}_{k}+\left(\begin{array}{c} \left(v_{k} T_{k}+\frac{a}{2} T_{k}^{2}\right) \cos (\vartheta) \\ \left(v_{k} T_{k}+\frac{a}{2} T_{k}^{2}\right) \sin (\vartheta) \\ 0 \\ a T_{k} \\ 0 \end{array}\right)
xk+1?=xk?+???????(vk?Tk?+2a?Tk2?)cos(?)(vk?Tk?+2a?Tk2?)sin(?)0aTk?0????????
2. 二次运动模型
- 恒定转(弯)率和速度模型（Constant Turn Rate and Velocity，CTRV）
- 恒定转(弯)率和加速度模型（Constant Turn Rate and Acceleration，CTRA）
CTRA多用于机载追踪系统，这些二次运动模型大多假定速度 v 和偏航角速度
ω
\omega
ω 没有关系。因此，在这类运动模型中，由于偏航角速度
ω
\omega
ω 测量的扰动，即使车辆没有移动，运动模型下的角速度也会发生非常细微的变化。
为了解决这个问题，速度 v 和 偏航角速度
ω
\omega
ω 的关联可以通过设定转向角
Φ
\Phi
Φ 恒定来建立，这样就引出了恒定转向角和速度模型（Constant Steering Angle and Velocity，CSAV），另外，速度可以被假定为线性变化的，进而引出了常曲率和加速度模型（Constant Curvature and Acceleration，CCA）。
2.1 恒定转弯率和速度模型
CVTR 是 CV 的一般形式，当角速度
ω
=
0
\omega=0
ω=0 时，就是 CV。CTRV 模型假设对象沿直线前进，同时还能以固定的转弯速率和恒定的速度移动。
- 状态变量：
x
=
[
p
x
p
y
v
ψ
ψ
˙
]
x=\left[\begin{array}{c} p_{x} \\ p_{y} \\ v \\ \psi \\ \dot{\psi} \end{array}\right]
x=???????px?py?vψψ˙?????????
五个状态变量依次为：横坐标---->纵坐标---->速度大小---->偏航角---->角速度