一、基于地面坐标系建模
如下图所示,车辆运动轨迹的计算建立在地面坐标系 O 1 X 1 Y 1 O_1X_1Y_1 O1X1Y1上。
定义车辆的 t 1 t_1 t1时刻的位置为 ( x 1 , y 1 ) (x_1,y_1) (x1,y1),偏航角为 ψ 1 \psi_1 ψ1, t 2 t_2 t2时刻车辆位置为 ( x 2 , y 2 ) (x_2,y_2) (x2,y2),偏航角为 ψ 2 \psi_2 ψ2。
定义车辆转弯半径表示为 R R R,正值代表左转,负值代表右转。
定义车辆旋转中心点表示为 ( x 0 , y 0 ) (x_0,y_0) (x0,y0),如果向左行驶,旋转中心点落在车身左侧,如果向右行驶,旋转中心点落在车身右侧。
定义 R 01 R_{01} R01在 X X X轴和 Y Y Y轴上的投影分别为 H 1 H_1 H1和 L 1 L_1 L1, R 02 R_{02} R02在 X X X轴和 Y Y Y轴上的投影分别为 H 2 H_2 H2和 L 2 L_2 L2。
假设车辆在 Δ t \Delta t Δt内的速度为 V V V,正值代表向前行驶,负值代表向后行驶,车辆在短时间内可以看作是匀速运动。
二、角度关系
假设车辆右向前行驶,则
V
V
V为正值,
R
R
R为负值
ψ
2
=
ψ
1
+
Δ
ψ
\psi_2 = \psi_1 + \Delta\psi
ψ2=ψ1+Δψ
其中
Δ
ψ
\Delta\psi
Δψ计算如下:
Δ
ψ
=
V
∗
Δ
t
R
\Delta\psi = \frac{V*\Delta t }{R}
Δψ=RV∗Δt
故 Δ ψ \Delta\psi Δψ为负值,及 ψ 2 < ψ 1 \psi_2 < \psi_1 ψ2<ψ1。
三、计算投影
考虑到上述假设车辆往右侧行驶,及
R
R
R为负值,故需加上“
−
-
−”号以代表投影距离值。
计算
R
01
R_{01}
R01在
X
X
X轴和
Y
Y
Y轴的投影:
L
1
=
−
R
∗
sin
(
ψ
1
)
H
1
=
−
R
∗
cos
(
ψ
1
)
L_1 = -R*\sin(\psi_1)\\ H_1 = -R*\cos(\psi_1)
L1=−R∗sin(ψ1)H1=−R∗cos(ψ1)
计算
R
02
R_{02}
R02在
X
X
X轴和
Y
Y
Y轴的投影:
L
2
=
−
R
∗
sin
(
ψ
2
)
H
2
=
−
R
∗
cos
(
ψ
2
)
L_2 = -R*\sin(\psi_2)\\ H_2 = -R*\cos(\psi_2)
L2=−R∗sin(ψ2)H2=−R∗cos(ψ2)
四、计算车辆位置
x
2
=
x
1
+
L
1
−
L
2
y
2
=
y
1
+
H
2
−
H
1
x_2 = x_1 + L_1 - L_2\\ y_2 = y_1 + H_2 - H_1
x2=x1+L1−L2y2=y1+H2−H1
带入得:
x
2
=
x
1
+
R
∗
(
sin
(
ψ
2
)
−
sin
(
ψ
1
)
)
y
2
=
y
1
+
R
∗
(
cos
(
ψ
1
)
−
cos
(
ψ
2
)
)
x_2 = x_1 + R * (\sin(\psi_2) - \sin(\psi_1))\\ y_2 = y_1 + R * (\cos(\psi_1) - \cos(\psi_2))
x2=x1+R∗(sin(ψ2)−sin(ψ1))y2=y1+R∗(cos(ψ1)−cos(ψ2))
五、考虑相邻两个时刻的转弯半径不同的情况
假设相邻两个时刻的转弯半径分别为
R
1
R_1
R1和
R
2
R_2
R2,
计算
R
1
R_{1}
R1在
X
X
X轴和
Y
Y
Y轴的投影:
L
1
=
−
R
1
∗
sin
(
ψ
1
)
H
1
=
−
R
1
∗
cos
(
ψ
1
)
L_1 = -R_1*\sin(\psi_1)\\ H_1 = -R_1*\cos(\psi_1)
L1=−R1∗sin(ψ1)H1=−R1∗cos(ψ1)
计算
R
2
R_{2}
R2在
X
X
X轴和
Y
Y
Y轴的投影:
L
2
=
−
R
2
∗
sin
(
ψ
2
)
H
2
=
−
R
2
∗
cos
(
ψ
2
)
L_2 = -R_2*\sin(\psi_2)\\ H_2 = -R_2*\cos(\psi_2)
L2=−R2∗sin(ψ2)H2=−R2∗cos(ψ2)
根据几何关系
x
2
=
x
1
+
L
1
−
L
2
y
2
=
y
1
+
H
2
−
H
1
x_2 = x_1 + L_1 - L_2\\ y_2 = y_1 + H_2 - H_1
x2=x1+L1−L2y2=y1+H2−H1
带入得
x
2
=
x
1
−
R
1
∗
sin
(
ψ
1
)
+
R
2
∗
sin
(
ψ
2
)
y
2
=
y
1
+
R
1
∗
cos
(
ψ
1
)
−
R
2
cos
(
ψ
2
)
x_2 = x_1 - R_1 * \sin(\psi_1) + R_2 * \sin(\psi_2)\\ y_2 = y_1 + R_1 * \cos(\psi_1) - R_2\cos(\psi_2)
x2=x1−R1∗sin(ψ1)+R2∗sin(ψ2)y2=y1+R1∗cos(ψ1)−R2cos(ψ2)