Integration of nonlinear observer and unscented Kalman filter for pose estimation in autonomous truck–trailer and container truck

Abstract

This paper introduces a new approach to state estimation called nonlinear observer-unscented Kalman filter (NLO-UKF). The proposed method is designed to improve the accuracy of state estimation in complex systems that are subject to nonlinearity and uncertainty. The key idea of the NLO-UKF is to use a nonlinear observer to correct the projected sigma points based on a measurement, and then update the mean and covariance using the UKF. The paper provides a detailed description of the NLO-UKF algorithm and demonstrates its boundedness. The use of NLO-UKF for pose estimation is presented to compare the effectiveness of the proposed method with other state estimation methods in the simulation of an autonomous truck–trailer system and experimentation with a container truck system. The NLO-UKF demonstrates improved accuracy during steady-state estimation.

Appendices

Derivation of truck trailer model

The schematic of truck–trailer is shown in Fig. 2a. From Fig. 2, The dynamics at point ($x_h$, $y_h$) that affect by the velocity v are obtained as

$$\begin{aligned} \dot{x}_h= & {} v \cos \theta _h \end{aligned}$$

(A1)

$$\begin{aligned} \dot{y}_h= & {} v \sin \theta _h. \end{aligned}$$

(A2)

Due to limitations in steering, wheel rotation, and the inability to move directly sideways in the kinematic of the head truck, the nonholonomic constraints are given by

$$\begin{aligned} \dot{x}_h \sin \theta _h - \dot{y}_h \cos \theta _h= & {} 0 \end{aligned}$$

(A3)

$$\begin{aligned} \dot{x}_f \sin (\theta _h + \delta ) - \dot{y}_f \cos (\theta _h + \delta )= & {} 0, \end{aligned}$$

(A4)

where $x_f$ and $y_f$ is the position of the front wheel and determined by

$$\begin{aligned} x_f= & {} x_h + l_h \cos \theta _h \end{aligned}$$

(A5)

$$\begin{aligned} y_f= & {} y_h + l_h \sin \theta _h. \end{aligned}$$

(A6)

The derivative of $x_f$ and $y_f$ are

$$\begin{aligned} \dot{x}_f= & {} \dot{x}_h + \dot{\theta }_h l_h \sin \theta _h \end{aligned}$$

(A7)

$$\begin{aligned} \dot{y}_f= & {} \dot{y}_h + \dot{\theta }_h l_h \cos \theta _h. \end{aligned}$$

(A8)

Substituting (A7) and (A8) to (A4) yields

$$\begin{aligned}{} & {} (\dot{x}_h + \dot{\theta }_h l_h \sin \theta _h) \sin (\theta _h + \delta ) - (\dot{y}_h + \dot{\theta }_h l_h \cos \theta _h) \nonumber \\{} & {} \qquad \cos (\theta _h + \delta ) = 0 \end{aligned}$$

(A9)

$$\begin{aligned}{} & {} \dot{x}_h \sin (\theta _h + \delta ) - \dot{y}_h \cos (\theta _h + \delta ) - \dot{\theta }_h l_h \cos \delta = 0. \nonumber \\ \end{aligned}$$

(A10)

Then, substitution of (A1) and (A2) into (A10) provides

$$\begin{aligned}{} & {} v \cos \theta _h \sin (\theta _h + \delta ) - v \sin \theta _h \cos (\theta _h + \delta ) \nonumber \\{} & {} - \dot{\theta }_h l_h \cos \delta = 0 \end{aligned}$$

(A11)

$$\begin{aligned}{} & {} v \sin \theta _h - \dot{\theta }_h l_h \cos \delta = 0.\nonumber \\ \end{aligned}$$

(A12)

Thus,

$$\begin{aligned} \dot{\theta }_h= & {} \frac{v \tan \delta }{l_h}. \end{aligned}$$

(A13)

The final result of the kinematic of the head truck is described by (A1), (A2), and (A13).

Next, we derive the kinematic of the trailer part based on Fig. 2a. The dynamic of the trailer at point ($x_t$, $y_t$) that affect by the velocity v and the heading of the head truck is obtained as

$$\begin{aligned} \dot{x}_t= & {} v \cos (\theta _h - \theta _t) \cos \theta _t \end{aligned}$$

(A14)

$$\begin{aligned} \dot{y}_t= & {} v \cos (\theta _h - \theta _t) \sin \theta _t. \end{aligned}$$

(A15)

Nonholonomic constraints are existed at the trailer reference point ($x_t$,$y_t$) and the hitch joint at point ($x_h$,$y_h$) due to limitations in inability to move directly sideways. The nonholonomic constraints are given by

$$\begin{aligned} \dot{x}_h \sin \theta _h - \dot{y}_h \cos \theta _h= & {} 0 \end{aligned}$$

(A16)

$$\begin{aligned} \dot{x}_t \sin \theta _t - \dot{y}_t \cos \theta _t= & {} 0, \end{aligned}$$

(A17)

where $x_h$ and $y_h$ is described by

$$\begin{aligned} x_h= & {} x_t + l_t \cos \theta _t \end{aligned}$$

(A18)

$$\begin{aligned} y_h= & {} y_t + l_t \sin \theta _t, \end{aligned}$$

(A19)

and the derivation of $x_h$ and $y_h$ is calculated by

$$\begin{aligned} \dot{x}_h= & {} \dot{x}_t + \dot{\theta }_t l_t \sin \theta _t \end{aligned}$$

(A20)

$$\begin{aligned} \dot{y}_h= & {} \dot{y}_t + \dot{\theta }_t l_t \cos \theta _t. \end{aligned}$$

(A21)

Substituting (A20) and (A21) to (A16) yields

$$\begin{aligned} (\dot{x}_t + \dot{\theta }_t l_t \sin \theta _t) \sin \theta _h - (\dot{y}_t + \dot{\theta }_t l_t \cos \theta _t) \cos \theta _h= & {} 0 \nonumber \\\end{aligned}$$

(A22)

$$\begin{aligned} \dot{x}_t \sin \theta _h - \dot{y}_t \cos \theta _h - l_t \dot{\theta }_t \cos (\theta _h - \theta _t)= & {} 0. \nonumber \\ \end{aligned}$$

(A23)

Then, substitution of (A14) and (A15) to (A23) provides

$$\begin{aligned}{} & {} (v \cos (\theta _h - \theta _t) \cos \theta _t) \sin \theta _h\nonumber \\{} & {} - (v \cos (\theta _h - \theta _t) \sin \theta _t) \cos \theta _h \nonumber \\{} & {} \qquad - l_t \dot{\theta }_t \cos (\theta _h - \theta _t) = 0 \nonumber \\\end{aligned}$$

(A24)

$$\begin{aligned}{} & {} v \cos (\theta _h - \theta _t) \sin (\theta _h - \theta _t) - l_t \dot{\theta }_t \cos (\theta _h - \theta _t) = 0.\nonumber \\ \end{aligned}$$

(A25)

Thus,

$$\begin{aligned} \dot{\theta }_t= & {} \frac{v \sin (\theta _h - \theta _t)}{l_t}. \end{aligned}$$

(A26)

Finally, the kinematic of the trailer is determined by (A14), (A15) , and (A26).

Derivation of container truck model

The velocity of the container truck is on the same wheels as that used for steering as shown in Fig. 2b. From Fig. 2b, the dynamic of the container truck at point ($x_c$, $y_c$) with velocity v and steering angle $\delta$ is obtained as

$$\begin{aligned} \dot{x}_c= & {} v \cos \delta \cos \theta _c \end{aligned}$$

(B27)

$$\begin{aligned} \dot{y}_c= & {} v \cos \delta \sin \theta _c . \end{aligned}$$

(B28)

The container truck follows the nonholonomic constraint by following equations

$$\begin{aligned} \dot{x}_c \sin \theta _c - \dot{y}_c \cos \theta _c= & {} 0 \end{aligned}$$

(B29)

$$\begin{aligned} \dot{x}_s \sin (\theta _c + \delta ) - \dot{y}_s \cos (\theta _c + \delta )= & {} 0, \end{aligned}$$

(B30)

where $x_s$ and $y_s$ are determined by

$$\begin{aligned} x_s= & {} x_c + l_c \cos \theta _c \end{aligned}$$

(B31)

$$\begin{aligned} y_s= & {} y_c + l_c \sin \theta _c. \end{aligned}$$

(B32)

The derivatives of $x_s$ and $y_s$ are obtained as

$$\begin{aligned} \dot{x}_s= & {} \dot{x}_c + \dot{\theta }_c l_c \sin \theta _c \end{aligned}$$

(B33)

$$\begin{aligned} \dot{y}_s= & {} \dot{y}_c + \dot{\theta }_c l_c \cos \theta _c. \end{aligned}$$

(B34)

Substituting (B33) and (B34) to (B30) yields

$$\begin{aligned}{} & {} (\dot{x}_c + \dot{\theta }_c l_c \sin \theta _c) \sin (\theta _c + \delta ) - (\dot{y}_c + \dot{\theta }_c l_c \cos \theta _c) \nonumber \\{} & {} \qquad \cos (\theta _c + \delta ) = 0 \end{aligned}$$

(B35)

$$\begin{aligned}{} & {} \dot{x}_c \sin (\theta _c + \delta ) - \dot{y}_c \cos (\theta _c + \delta ) - \dot{\theta }_c l_c \cos \delta = 0. \nonumber \\ \end{aligned}$$

(B36)

Then, substitution of (B27) and (B28) to (B36) results

$$\begin{aligned}{} & {} (v \cos \delta \cos \theta _c) \sin (\theta _c + \delta ) - v \cos \delta \sin \theta _c \cos (\theta _c + \delta ) \nonumber \\{} & {} \qquad - \dot{\theta }_c l_c \cos \delta = 0. \end{aligned}$$

(B37)

Thus, we have

$$\begin{aligned} \dot{\theta }_c= & {} \frac{v sin\delta }{l_c}. \end{aligned}$$

(B38)

Finally, the kinematic of the container truck is defined by (B27), (B28) , and (B38).