---
# System prepended metadata

title: 控制器設計程式講解(geometry controller)
tags: [控制組]

---

# 控制器設計程式講解(geometry controller)
###### tags: `控制組`
writer: 林哲丞
email: snoopy195011@gmail.com

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

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


2. 此function member的用途為將desire的姿態從quarternion的形式轉換成rotation matrix的形式
![](https://i.imgur.com/NzCAV8K.png)

3. 下圖為從quaternion轉換成rotation matrix的過程，其公式可看下一張圖
![](https://i.imgur.com/svbvuvq.png)

4. formula of turning quaternion to rotation matrix
![](https://i.imgur.com/IqkhnWk.jpg)

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向量，詳見下圖
![](https://i.imgur.com/3qT9qEe.png)

6. vee map
![](https://i.imgur.com/EzsBJIk.jpg)

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軟體組交接
![](https://i.imgur.com/hgXBsqZ.png)


8. 控制器得到需要補償的力和力矩後再丟到allocation matrix即可得到每個馬達所需要出的力是多少
![](https://i.imgur.com/UDD17TC.jpg)

