# 控制器設計程式講解(geometry controller)
###### tags: `控制組`
writer: 林哲丞
email: [e-mail]

本文章將講述用於2022SAUVC時的控制器(geometry controller)

1. 下圖為constructor，其功用主要是設置控制器的增益參數kx, kv, kR等等
次要功能為初始化與姿態有關的3個陣列
姿態通常以旋轉矩陣(rotation matrix)或是四元數(quarternion)來表示
Rd為desire的機體姿態
R指的是當前的機體姿態，此資訊可由IMU得知
Re指的是姿態誤差

2. 此function member的用途為將desire的姿態從quarternion的形式轉換成rotation matrix的形式

3. 下圖為從quaternion轉換成rotation matrix的過程，其公式可看下一張圖

4. formula of turning quaternion to rotation matrix

5. er_mul這個function最主要是在算姿態誤差，公式如下
$$
e_R=\frac{1}{2}(R_d^TR-R^TR_d)^\bigvee
$$
$$
first矩陣計算的是R_d^TR的部分
$$
$$
second矩陣計算的是R^TR_d的部分
$$
()的右上角有一個很像V的符號，其在數學上叫做vee map，主要功能是將3乘3矩陣轉換成一個3乘1向量，詳見下圖

6. vee map

7. update這個function主要是要得到此時機體總共需要受到哪個方向的力和力矩
qtoR將目前的姿態從quaternion的形式轉換成rotation matrix
er_mul再將姿態誤差算出來，但此時Re是一個3乘3矩陣，我們必須將此藉由vee map轉換成3乘1向量
藉由上圖的vee map公式我們將轉換完的3乘1向量存到eR.x和eR.y中
至於eR.z因為其代表的是yaw angle，沒辦法從IMU中得到
之後若是藉由底部相機即可得yaw angle的誤差，並代到eR.z中
104~110行是藉由物理公式得到的式子，詳見2022軟體組交接

8. 控制器得到需要補償的力和力矩後再丟到allocation matrix即可得到每個馬達所需要出的力是多少