Characteristic
Curvilinear coordination model is an efficient model to consider both longitudinal and lateral motion.
Using Bahnkoordinaten.
Nonlinear Model.
Variables
State Variables:
x = [ s n μ v a δ δ ˙ ] \mathbf{x} = \begin{bmatrix} s \\ n \\ \mu \\ v \\ a \\ \delta \\ \dot{\delta} \end{bmatrix} x= snμvaδδ˙
- ( s ): Progress
- ( n ): Lateral error
- ( μ \mu μ): Local heading
- ( v ): Velocity
- ( a ): Acceleration in projected driving direction
- ( δ \delta δ): Steering angle
- ( δ ˙ \dot{\delta} δ˙ ): Steering rate
Input variable:
u = [ u Jerk u δ ¨ ] = [ v ¨ δ ¨ ] \begin{align*} \mathbf{u} = \begin{bmatrix} u_{\text{Jerk}} \\ u_{\ddot{\delta}} \end{bmatrix} = \begin{bmatrix} \ddot{v} \\ \ddot{\delta} \end{bmatrix} \end{align*} u=[uJerkuδ¨]=[v¨δ¨]
- ( u Jerk u_{\text{Jerk}} uJerk ): Jerk
- ( u δ ¨ u_{\ddot{\delta}} uδ¨ ): Steering accel
PS: use second order derivative, because integral is more accurate than differential, and these two items is related to comfortable, then minimize u.
Curvilinear Coordinates Vehicle Model
The so called arc velocity 𝑠 gives a way of describing the vehicles motion along a reference trajectory parameterized by 𝑠.
convert velocity from vehicle coordinate to trajectory coordinate:
[ s ˙ n ˙ z ˙ ] = [ c o s u − s i n u 0 s i n u c o s u 0 0 0 1 ] = [ v x v y v z ] \begin{bmatrix} \dot{s}\\ \dot{n} \\ \dot{z} \end{bmatrix} = \begin{bmatrix} cos_u & -sin_u & 0 \\ sin_u & cos_u & 0 \\ 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} v_x\\ v_y \\ v_z \end{bmatrix} s˙n˙z˙ = cosusinu0−sinucosu0001 = vxvyvz
a . v s = v x cos ( u ) − v y sin ( u ) = ( R − n ) ϕ ˙ s b . s ˙ = R ϕ ˙ s c . κ = 1 R d . v x = v cos ( β ) e . v y = v sin ( β ) \begin{align*} & a.\ v_s = v_x \cos(u) - v_y \sin(u) = ({R - n}) \dot{\phi}_s \\ & b.\ \dot{s} = R \dot{\phi}_s \\ & c.\ \kappa = \frac{1}{R} \\ & d.\ v_x = v \cos(\beta) \\ & e.\ v_y = v \sin(\beta) \end{align*} a. vs=vxcos(u)−vysin(u)=(R−n)ϕ˙sb. s˙=Rϕ˙sc. κ=R1d. vx=vcos(β)e. vy=vsin(β)
s ˙ = v cos ( β ) cos ( u ) − v sin ( β ) sin ( u ) 1 − n κ = v cos ( β + u ) 1 − n κ \begin{align*} \dot{s} & = \frac{v \cos(\beta) \cos(u) - v \sin(\beta) \sin(u)}{1 - n \kappa}\\ & = \frac{v \cos(\beta + u)}{1 - n \kappa} \end{align*} s˙=1−nκvcos(β)cos(u)−vsin(β)sin(u)=1−nκvcos(β+u)
n ˙ = v x sin ( u ) + v y cos ( u ) = v cos ( β ) sin ( u ) + v sin ( β ) cos ( u ) = v sin ( β + u ) \begin{align*} \dot n & = v_x \sin(u) + v_y \cos(u) \\ & = v \cos(\beta) \sin(u) + v \sin(\beta) \cos(u) \\ & = v \sin(\beta + u) \end{align*} n˙=vxsin(u)+vycos(u)=vcos(β)sin(u)+vsin(β)cos(u)=vsin(β+u)
μ
˙
=
r
−
ϕ
˙
s
=
v
R
v
−
k
v
cos
(
β
+
μ
)
1
−
n
κ
=
v
sin
(
β
)
l
r
−
k
v
cos
(
β
+
μ
)
1
−
n
κ
\begin{align*} \dot{\mu} & = r - {\dot{\phi}_s} \\ &= {\frac{v}{R_v}} - k \frac{v \cos(\beta + \mu)}{1-n\kappa} \\ & = \frac{v \sin(\beta)}{l_r} - k \frac{v \cos(\beta + \mu)}{1-n\kappa} \end{align*}
μ˙=r−ϕ˙s=Rvv−k1−nκvcos(β+μ)=lrvsin(β)−k1−nκvcos(β+μ)
The motion model is a discretized kinematic bicycle model in curvilinear coordinates.
x ˙ = [ s ˙ n ˙ μ ˙ v ˙ a ˙ δ ˙ δ ¨ ] = [ v cos ( μ + β ) 1 − n κ v sin ( μ + β ) v sin ( β ) l r − k v cos ( μ + β ) 1 − n κ a u jerk δ ˙ u δ ¨ ] . \mathbf{\dot{x}} = \begin{bmatrix} \dot{s} \\ \dot{n} \\ \dot{\mu} \\ \dot{v} \\ \dot{a} \\ \dot{\delta} \\ \ddot{\delta} \end{bmatrix}= \begin{bmatrix} \frac{v \cos(\mu + \beta)}{1 - n \kappa} \\ {v \sin(\mu + \beta)} \\ \frac{v \sin(\beta)}{l_r} - k \frac{v \cos(\mu + \beta)}{1 - n \kappa} \\ a \\ \text{u}_{\text{jerk}} \\ \dot{\delta} \\ \text{u}_{\ddot{\delta}} \end{bmatrix} . x˙= s˙n˙μ˙v˙a˙δ˙δ¨ = 1−nκvcos(μ+β)vsin(μ+β)lrvsin(β)−k1−nκvcos(μ+β)aujerkδ˙uδ¨ .
where
β = tan − 1 ( l r l f + l r tan ( δ f ) ) \beta = \tan^{-1} \left(\frac{l_r}{l_f + l_r} \tan(\delta_f)\right) β=tan−1(lf+lrlrtan(δf))
is slip angle
( l_f ) is the length from the front of the car to CoG
( l_r ) is the length from the rear of the car to CoG.
sin ( β ) = l r l f , R = l r sin ( β ) l f sin ( δ f − β ) = R sin ( π 2 − δ f ) l f sin ( δ f ) cos ( β ) − cos ( δ f ) sin ( β ) = l r cos ( δ f ) sin ( β ) l f tan ( δ f ) − tan ( β ) = l r tan ( β ) β = tan − 1 ( l r l f + l r tan ( δ f ) ) \sin(\beta) = \frac{l_r}{l_f} , {R} = \frac{l_r}{ \sin(\beta)} \\ \frac{l_f}{\sin(\delta_f - \beta)} = \frac{R}{\sin(\frac{\pi}{2} -\delta_f)} \\ \frac{l_f}{\sin(\delta_f)\cos( \beta)-\cos(\delta_f)\sin(\beta)} = \frac{l_r}{\cos(\delta_f)\sin( \beta)} \\ \frac{l_f}{\tan(\delta_f) - \tan(\beta)} = \frac{l_r}{\tan(\beta)} \\ \beta = \tan^{-1} \left(\frac{l_r}{l_f + l_r} \tan(\delta_f)\right) sin(β)=lflr,R=sin(β)lrsin(δf−β)lf=sin(2π−δf)Rsin(δf)cos(β)−cos(δf)sin(β)lf=cos(δf)sin(β)lrtan(δf)−tan(β)lf=tan(β)lrβ=tan−1(lf+lrlrtan(δf))