轉載：拋光機器人力位混合控制思路與方式
2022-09-13 22:35:07 0次
如果我們對位置閉環，那就無法控制力。如果我們對力閉環，那就無法控制位置。那么，如何實現位置和力的同時控制呢？就像我們的人手一樣。于是，于是就有了阻抗控制的概念，即我們用位置閉環去控制機械臂末端，但是末端具有一定的剛度，與外界接觸時會表現出力的特征。這種剛度可以從機械設計上實現，比如柔性關節SEA。也可以從控制算法上實現，比如阻抗/導納控制。
我們統稱為“阻抗控制”；一般的，阻抗控制是直接控制力（電流）的方式；導納控制是通過控制位置來控制力；無論是阻抗控制還是導納控制，都可以調節阻抗特征的，只是阻抗控制無法模擬高剛度，導納控制無法模擬低剛度，而且這種限制并不來源于理論，而是由于控制量不相同，工程中的傳感器測量有誤差和延時導致；
理論分析：任何物體間的接觸，都可以等效為兩個“阻尼質量彈簧”模型，之間的接觸舉個例子，人擦玻璃，玻璃的彈簧剛度K可以認為是無窮大（沒有彈性），人手的剛度K就需要比較小（有彈性），這樣才能適應玻璃表面，完成擦拭工作；再比如，拿刀切肉，肉的剛度K可以認為是很小，刀的剛度認為是很大，這樣才能讓肉隨著刀的運動，而變化；現實世界里，我們人與外界接觸的剛度往往不是固定的，而是變化的，舉個例子，拍籃球，手在剛接觸球的時候是偏柔性的，讓手隨著球而動，然后，手會變得剛性起來，讓球隨著手而動，這個剛度的控制，是人對自己手臂的主動控制，我們也希望機器人在處理力控任務的時候，能有這種自由控制剛度的能力；為什么需要這個“阻尼質量彈簧”模型呢？ 擦玻璃的話，給玻璃施加一個恒力不就行了嗎？想象一下，如果現在機械臂是對一塊玻璃施加一個恒力F，然后突然撤開玻璃會怎樣呢？
顯然機械臂會失控，以最快速度向F的方向運動；但是如果有了“阻尼質量彈簧（BMK）”模型，當MBK三個參數調節得當的時候，機械臂就會以平滑的運動回到虛擬彈簧的原點；在這種情況下，我們控制機械臂與環境的接觸，是通過控制機械臂末端的四個參數：虛擬質量M，虛擬阻尼B，虛擬剛度K，虛擬彈簧原點x0；控制這四個參數，我們可以讓機械臂對外呈現各種力特征，來滿足打磨拋光，精密裝配，協作交互等需求現在我們目標明確了，要設計一個BMK模型，對機械臂進行控制，接下來怎么做呢？還是按照圖9-6來分析，其中f是施加到系統的外力，xdot是速度，xdotdot是加速度，這是一個典型的二階系統，有以下公式來描述系統的運動：
即已知MBK以及測得的外力、速度、位置，我們可以求出當前需要的加速度；這個加速度就是我們要控制的量；那怎么控制加速度？由于我們是離散系統，通過將xd積分兩次，就得到了位置，所以，最終我們可以通過控制位置x，去實現這個模型，而且在伺服系統中，控制量通常是位置環，所以這種方式是合理的；那么既然最后都要控制位置x，那為什么剛開始不直接把公式，化成位置的表達式呢？因為化成位置的表達式，式中有加速度項，需要測量加速度項，而通常伺服系統中位置信息是由編碼器得到的，而編碼器只能獲得位置反饋，其速度反饋是通過對位置的微分得到，而加速度是對速度微分得到，這個時候，由于經過了兩次微分，得到的加速度信息往往是噪聲非常大的，不適合使用；可以看到，我們這種方法，實際上，是通過控制位置來控制力的，俗稱“導納控制”；其實常用的還有另一種方法，是直接控制力，俗稱“阻抗控制”，但這需要有關節力傳感器的反饋，也要進行動力學建模，并且要能直接控制伺服驅動器的電流環，實現成本較大，這里就不敘述了；
一個例子：
控制器的位置環程序為3ms周期，其中包括一個導納控制模塊
導納控制模塊如下：
//x軸導納模型
xdd = (fx0 - Bx*vx_now - Kx*(xnow-xbase))/Mx;
//加速度積分得到速度
xd = xd + xdd;
//y軸導納模型
ydd = (fy0 - By*vy_now - Ky*(ynow-ybase))/My;
//加速度積分得到速度
yd = yd + ydd;
//將導納模型的位置偏移計入，得到新的x軸y軸位置
xt = xd + xnow;
yt = yd + ynow;分別計算出x和y軸的導納模型，將位置偏移量更新到新的目標位置
其中，fx0是笛卡爾坐標系中x軸向的外力，Bx是x軸向的阻尼值，vx_now是末端在x軸向的瞬時速度，Kx是x軸向的剛度，xnow是末端在x軸的瞬時位置，xbase是x軸的虛擬阻抗原點，Mx是x軸向的質量值。
通過導納模型計算出加速度，由于此模塊，是與位置環同周期的，在離散數學中，積分可簡化為加法（類似于PID，如果不明白可以自行百度一下）
那么，怎樣得到fx0呢？我使用了一個末端的六維力傳感器，對傳感器受的力進行坐標變換，得到x軸和y軸的外力fx0和fy0；
如何得到vx_now？首先從驅動器獲得關節速度，通過雅克比變換，得到末端的x軸和y軸速度；
如何得到xnow？首先從驅動器獲得關節位置，通過正運動學計算，得到末端的x和y軸位置
所以，大概是這樣：
我們假設機械臂的末端是一個阻尼質量彈簧系統，當有了外力作用時，我們通過模型計算，這個虛擬的MBK系統的加速度，速度，位置應處于什么狀態。最后，通過直接篡改位置環，去模擬一個MBK系統；
并且，你可以看出，x和y軸的MBK系統是解耦的，各自獨立計算的。由于我的實驗機械臂，只具有在xy軸平面內運動的能力，所以只考慮了x和y軸，一般的六自由度機械臂，應分別對x，y，z，rx，ry，rz六個自由度進行MBK解耦；
