---
# System prepended metadata

title: 2022軟體組交接

---

#    2022軟體組交接

##    System

###    STM32

###    Rpi

##    Sensor

###    IMU

###    Pressure Sensor

##    Sonar

##    Output

###    Motor

##    Control

###    Dynamics

####    Full Dynamic Model

- 座標圖示:
![](https://i.imgur.com/PeWy0un.png)

- Dynamic Equation

    $$
    \dot{x}=v \\ 
    m\ddot{x}=mge_3+TR-Be_3-K_v(R)v^2-F_L(R) \\
    \dot{R}=R\hat{\Omega} \\
    J\hat{\Omega}+\Omega\times J\Omega=M-K_{\Omega}(R){\Omega}^2+M_B(R)+M_f(v)
    $$

    - x: 3D vector, position of AUV
    - v: 3D vector, velocity of AUV
    - m: scalar, mass of AUV
    - g: scalar, gravity acceleration constant
    - e~3~: 3D vector, [0 0 1]^T^
    - T: 3D vector, motor force
    - R: 9*9 matrix, rotation matrix of AUV
    - B: scalar, value of buoyancy
    - K~v~\(R\): drag constant, caused by the cross section of AUV --> a function of AUV attitude
    - F~L~\(R\): lift, caused by the shape of AUV --> a function of AUV attitude
    - Ω: angular velocity of AUV
    - J: moment of inertia of AUV
    - M: motor moment
    - K~Ω~\(R\): drag moment constant, a function of AUV attitude
    - M~B~\(R\): moment by buoyancy
    - M~f~: moment by drag force
    
####    Simplified Dynamic Model

- Dynamic Equation

$$
\dot{x}=v \\ 
m\ddot{x}=mge_3+TR-Be_3-K_vv^2 \\
\dot{R}=R\hat{\Omega} \\
J\hat{\Omega}=M+M_B(R)+M_f(v)
$$

- Current Equation

$$
\dot{x}=v \\ 
m\ddot{x}=mge_3+TR-Be_3 \\
\dot{R}=R\hat{\Omega} \\
J\hat{\Omega}=M
$$

###    Error Term

####    Linear

1. **e~x~:** 由兩個1D聲納，測量牆壁計算得到位置，再將前後時刻的位置相減

![](https://i.imgur.com/k9uDRXU.png)


2. **e~v~:** 由DVL計算+聲納計算出的位移微分，兩者融合而成

####    Angular

1. **Roll & Pitch**

    由IMU得到，透過e~R~的公式計算得到，公式如下:

    $$
    e_R=\frac{1}{2}(R_d^TR-R^TR_d)^\bigvee
    $$

2. **Yaw**

    ![](https://i.imgur.com/kOOaRDD.png)

    透過兩個聲納，從上圖的三角關係中，$\theta$ 角已知，$l_1$、$l_2$由聲納得到，可透過餘弦定理算出$l_3$及 $\alpha$ 角
    
###    Control Input

