---
tags: TI instaspin
---

# 阻抗控制簡介
Impedance control is a type of control strategy used in robotics to achieve compliant interactions with the environment. It involves controlling the mechanical impedance of a robot, which is the ratio of force to displacement at a given point in the robot's workspace. The mechanical impedance can be thought of as the robot's resistance to external forces.

In impedance control, the desired impedance is specified as a function of time, and the robot's controller adjusts the robot's motion to achieve that impedance. This allows the robot to be compliant and adapt to changes in the environment, while still maintaining a desired level of stability.

Impedance control can be used for a variety of applications, including human-robot interaction, manipulation of delicate objects, and robot locomotion on uneven terrain. It is particularly useful in situations where precise control of forces and motion is important, but where the environment is uncertain or changing.
>from CHATGPT

:pushpin:阻抗控制的實際應用
<iframe width="560" height="315" src="https://www.youtube.com/embed/KJ8s1BUHoks" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

## 阻抗控制原理
我們可以用一個彈簧來簡單介紹阻抗控制，假設一個彈簧，根據虎克定律假設彈簧係數為(1/100) ，F=(1/100)*X，其位置控制架構可表示如下圖
現在考慮一種情況，在彈簧運動過程中，突加一個外力 Fe 。對於位置控制器來講這是一個外擾，這個外力將使得彈簧的位置發生偏移。控制器獲得負回授信號之後，輸出的控制量起到壓制外擾的效果，用更強有力的控制量把本該偏移的位移掰回來，而這兩者之間是一個對抗的關系，如果系統剛性很大且控制器增益很高時，系統會即時傳回相應對抗力量。
若對系統進行阻抗控制如下圖，以彈簧為例，當施加一個外力Fe彈簧所呈現的物理意義是彈簧會產生一個與所施加外力相等的反作用力，並且根據外力的大小產生適當的位移。
將上述彈簧控制換成馬達，可繪製系統方塊圖如下，透過Load Torque Observer可以估出馬達所受到的外力。而根據上述阻抗控制的概念，馬達會產生相應的反作用力，與受到外力之後該表現的速度。
此時還能透過調整Reference Model的參數改變實現電子慣量。

如圖所示則為參考模式控制的另一類控制形式,此一架構進行的為速度控制,其中 Reference Model 僅一階,僅剩 J 與 B 可調,此一系統仍採用原 IP 控制架構,調整 Reference Model 內參數將改變馬達輸出特性,改變 J 可以實現所謂的電子飛輪效果,改變 B 則可控制馬達之制動剎車力,而此一架構則可用於實現健身車或電動輪椅的阻、助力控制上。

## MATLAB模擬

模擬如圖，整個，其中底層FOC架構為無感測HFI驅動。


* Speed loop速度迴路(黃色部分)由InstaSPIN-FOC支援

* T_pedal則模擬人類踩踏力道

* Torque scope(綠色)比較輸入pedal輸入torque與observer輸出

* T_observer(紫色)分別輸入電流與速度命令去估出扭矩

* Reference model 將扭矩訊號轉成速度訊號，並且能透過調整J和B達到點子飛輪的效果

### 模擬結果(speed and torque)
黃色為命令，藍色為回授

### PI V.S. IP in Speed Loop
:dart: [PI controller V.S. IP controller](https://www.irjet.net/archives/V4/i10/IRJET-V4I1089.pdf)

## Torque observer

Observer簡單來說就是一個Model base的虛擬控制系統，當系統的受控系統為虛擬的Model參數時，此時我們會稱該系統的控制器為Observer。
下圖 為observer狀態表示式的經典架構，where A, B, C and Ke represent characteristic matrix, input matrix, output matrix and observer gain matrix。(現代控制學)
在一個系統當中，實際上並非所有物理量都能夠對其進行量測。以馬達物理系統為例，馬達在轉動時我們僅能得知馬達轉速，無法得到馬達所受到的轉矩，或是輸出的轉矩。此時就能透過所謂的觀察器(Observer)或稱估測器(Estimator)，算出馬達的轉矩。
下圖是一個簡單的馬達外部轉矩觀測器，控制使用IP控制器，注意本專案是使用PICONTROLLER。

### 頻寬設計考量
目前最高頻寬設計為sampling frequency的1/20倍
在最高頻寬下sliding mode的方法低速高扭會超級震，然後HFI的模式會很吵。
->根據系統速度採用變動頻寬的方式。

### S-DOMAIN實現
使用TI提供的PI控制器模板直接在頻域使用C語言進行實現。

//! \brief     Runs the PID controller for torque observer
//! \param[in] handle      The PID controller handle
//! \param[in] refValue    The reference value to the controller
//! \param[in] fbackValue  The feedback value to the controller
//! \param[in] pOutValue   The pointer to the controller output value
static inline void PID_run_torque_ob(PID_Handle handle,const _iq refValue_speed,
                                     const _iq fback_Iq,_iq *pOutValue)

## Reference model
TI instaspin中只有提供PI控制器的模板，所以這邊預計採用數位控制[pole-zero mapping](https://en.wikipedia.org/wiki/Matched_Z-transform_method)的方式另外進行實現，但不考慮採用[tustin](https://en.wikipedia.org/wiki/Bilinear_transform)以簡化計算量，且因為馬達控制相對來說頻寬較低，也較不追求嚴格的響應規格。
以下是計算過程
以下是LUT實現架構

需要實現差分方程
$$
U(k) = e^{\frac{-J}{B}T}U(k-1)+\frac{1-\frac{-J}{B}T}{2B}E(k)+\frac{1-\frac{-J}{B}T}{2B}E(k-1)
$$

實作時需搭配[TI IQ LIB](/B-CFcyocS6uEyB4Rd_E9_A)所提供的運算，且專案中使用IQ24，須注意overflow議題。

//! \brief     Runs the PID controller for Reference model
//! \param[in] handle      The PID controller handle
//! \param[in] J,B    The controlable J, B input
//! \param[in] e_n  The input value for the controller
//! \param[in] d_n   The pointer to the controller output value(kRPM)
static inline void Refmodel(PID_Handle handle,const _iq J,const _iq B,
                            const _iq e_n,_iq *d_n)

## REFERENCE
https://zhuanlan.zhihu.com/p/126338809
https://blog.csdn.net/a735148617/article/details/108564836
胡家勝，阻抗控制於力覺回饋控制應用之設計與實現，2003