[MEMO]移動体の位置制御

システム

移動体の制御点の位置を $\boldsymbol{x}$，入力を $\boldsymbol{u}$ として，以下のように定義する．

\begin{align}
\boldsymbol{x_b} = 
\begin{bmatrix}
x_b & y_b 
\end{bmatrix}^\mathrm{T}
\qquad
\boldsymbol{u} = 
\begin{bmatrix}
v & \omega 
\end{bmatrix}^\mathrm{T}
\end{align}

$\boldsymbol{x_b}$ と $\boldsymbol{u}$ の関係は，移動体の姿勢（ヨー角）を $\theta_b$，移動体の回転中心から制御点の距離を $l_b$ として，
（簡単のため，移動体の回転中心と制御点は機体の中心軸にあるとする）

\begin{align}
\begin{bmatrix}
v \\ 
l \omega 
\end{bmatrix}
=
\begin{bmatrix}
cos \theta_b & sin \theta_b \\
- sin \theta_b & cos \theta_b 
\end{bmatrix}
\begin{bmatrix}
\dot{x}_b \\ 
\dot{y}_b 
\end{bmatrix}
\end{align}

制御

目標位置を $\boldsymbol{x_b^d} = \begin{bmatrix} x_b^d & y_b^d \end{bmatrix}^\mathrm{T}$，目標速度を $\boldsymbol{\dot{x}_b^d} = \begin{bmatrix} \dot{x}_b^d & \dot{y}_b^d \end{bmatrix}^\mathrm{T}$ とすると，

\begin{align}
\begin{bmatrix}
v \\ 
l \omega 
\end{bmatrix}
=
\begin{bmatrix}
cos \theta_b & sin \theta_b \\
- sin \theta_b & cos \theta_b 
\end{bmatrix}
\left(
K_d
\begin{bmatrix}
\dot{x}_b^d \\ 
\dot{y}_b^d 
\end{bmatrix}
- K_p
\begin{bmatrix}
x - x_b^d \\ 
y - y_b^d 
\end{bmatrix}
\right)
\end{align}

として，PD制御する．