# Research on Obstacle Avoidance Replanning and Trajectory Tracking Control Driverless Ferry Vehicles
## 4 Obstacle Avoidance Algorithm and Transverse Controller Design
### Design of MPC Controller with Planning Module
#### 4.2.3 Linear Discretization of Nonlinear Models  
Here we discuss the linearization of a ferry vehicle's nonlinear dynamics model using a linearization method for state trajectories.  
Let \([\xi_{0},u_{0}]\) be the state quantity and control quantity at a certain time in the system, and let \(\hat{\xi}_{0}(k)\) represent the system state quantity obtained when the system continuously outputs the control quantity, \(u_{0}\) The following relationships exist between them:  
\[\left\{\begin{array}{l}\hat{\xi}_{0}(k+1)=f(\hat{\xi}_{0}(k),u_{0})\\ \hat{\xi}_{0}(0)=\xi_{0}\end{array}\right. \tag{31}\]  
The system of Equation (31) is transformed into a linear time-varying system equation:  
\[\hat{\xi}(k+1)=A(t)\tilde{\xi}+B(t)\widetilde{u} \tag{32}\]  
where \(\tilde{\xi}=\xi-\tilde{\xi}_{0}(k)\), \(A(t)=J_{f}(\xi)\), \(B(t)=J_{f}(u)\), \(\widetilde{u}=u-u_{0}\).  
The discretization of the above formula can be obtained:  
\[\left\{\begin{array}{l}A_{k,t}=I+TA(t)\\ B_{k,t}=TB(t)\end{array}\right. \tag{33}\]  
By combining Formula (32) with Formula (33), you can obtain the following:  
\[\tilde{\xi}(k+1)=A_{k,t}\tilde{\xi}(k)+B_{k,t}\widetilde{u}(k) \tag{34}\]  
where \(\tilde{\xi}=\xi(k)-\hat{\xi}_{0}(k)\), and \(\widetilde{u}=u(k)-u_{0}\).  
According to the above linear discretization method, the nonlinear dynamic equation of the driverless ferry vehicle is linearized, and the linearized equation is obtained as follows:  
\[\dot{\tilde{\xi}}_{dyn}=A_{dyn}(t)\xi_{dyn}(t)+B_{dyn}(t)u_{dyn}(t) \tag{35}\]  
\[\text{in the formula }B_{dyn}(t)=\left.\frac{\partial f_{dyn}}{\partial u}\right|_{ \xi_{0},\mu_{t}}=\left[\frac{2\mathcal{C}_{cf}}{m},\frac{2\mathcal{C}_{cf}(2 \beta_{f-1-\frac{\hat{y}_{t}+\hat{y}_{t}}{\hat{x}_{t}}})}{m},0,\frac{2\mathcal{ C}_{cf}}{l_{c}},0,0\right],A_{dyn}(t)=\frac{\partial f}{\partial u_{dyn}}|_{\xi_{0}, \mu_{t}}\]  
\[=\left[\begin{array}{ccccc}\frac{-2(\mathcal{C}_{cf}+\mathcal{C}_{cr})}{m \mathcal{C}_{t}}&\frac{\partial f_{y}}{\partial x}&0&-\dot{x}_{t}+\frac{2( \mathcal{C}_{cr}-\mathcal{C}_{cf})}{m\mathcal{C}_{t}}&0&0&0\\ \dot{\varphi}-\frac{2\mathcal{C}_{cf}\dot{\varphi}_{f-1-\frac{\hat{y}_{t}}{ \hat{x}_{t}}}}{m\mathcal{C}_{t}}&\frac{\partial f_{z}}{\partial x}&0&\dot{y}_ {t}-\frac{2\mathcal{C}_{cf}\dot{\varphi}_{f-1-\frac{\hat{y}_{t}}{\hat{x}_{t} }}}{m\mathcal{C}_{t}}&0&0\\ 0&0&0&1&0&0\\ \frac{2(\mathcal{C}_{cr}-\mathcal{C}_{cf})}{l_{c}\hat{x}_{t}}&\frac{\partial f _{z}}{\partial x}&0&\frac{-2(\mathcal{C}_{cf}-\mathcal{C}_{cr})}{l_{c}\dot{x }_{t}}&0&0&0\\ \cos(\dot{\varphi}_{t})&\sin(\dot{\varphi}_{t})&\dot{x}_{t}\cos(\dot{\varphi} _{t})-\dot{y}_{t}\sin(\dot{\varphi}_{t})&0&0&0&0\\ -\sin(\dot{\varphi}_{t})&\cos(\dot{\varphi}_{t})&-\dot{y}_{t}\cos(\dot{ \varphi}_{t})-\dot{x}_{t}\sin(\dot{\varphi}_{t})&0&0&0&0\\ \end{array}\right]\]  
Among them, \(\frac{\partial f_{y}}{\partial x}=(2\mathcal{C}_{cf}(\dot{y}_{t}+a\dot{ \varphi}_{r})+2\mathcal{C}_{cr}(\dot{y}_{t}+b\dot{\varphi}_{t}))/m\mathcal{x}_{ t}^{2}-\dot{\varphi}_{t}\), \(\frac{\partial f_{x}}{\partial x}=(2\mathcal{C}_{cf}\delta_{f-1}(\dot{y}_{t} +a\dot{\varphi}_{t}))/(m\mathcal{x}_{t}^{2})\), and \(\frac{\partial f_{y}}{\partial x}=(2\mathcal{C}_{cf}(\dot{y}_{t}+a\dot{\varphi }_{t})-2b\mathcal{C}_{cr}(\dot{y}_{t}-b\dot{\varphi}_{t}))/I_{z}\mathcal{x}_{t}^ {2}\).  
Discrete state-space expressions are created by discretizing the linearized equations derived previously using the first-order difference quotient method:  
\[\xi_{dyn}(k+1)=A_{dyn}(k)\xi_{dyn}(k)+B_{dyn}(k)u_{dyn}(k) \tag{36}\]  
where \(A_{dyn}(k)=I+TA_{dyn}(t)\) is \(B_{dyn}(k)=TB_{dyn}(t)\).