本文是對文獻 [1] 的復現。
更新日志
- Github 倉庫
- 2021/02/03
- 第一次上傳
- 2021/04/18
- 修改貼圖路徑為相對路徑
- 在 Model Properties 中手動添加模型裝配數據腳本回調函數，解決中文路徑問題
- 添加 2018a - 2020b 版本仿真模型文件
- 添加參考文獻文件
- 2021/02/03
- 文章本身
- 2021/04/28
- 讀者 Q&A 更新
- 2021/04/28
關於阻抗控制
阻抗控制的提出可以追溯到 1984 年，Neville Hogan 在 American Control Conference 發表的 Impedance Control：An Approach to Manipulation ，次年，Hogan N. 又在 Journal of Dynamic Systems Measurement and Control-transactions of The Asme 發表了阻抗控制經典三部曲 —— Impedance Control：An Approach to Manipulation, Part I --Theory, Part II -- Implementation, Part III -- Applications。
圖為 2021/01/21 時博主做的組會 Presentation
博主理解的阻抗控制，實際其控制的目標是調節機器人的末端位置（笛卡爾坐標空間）和末端作用力之間的動態關系，以保證機器人在適當的柔順運動過程中進行軌跡跟蹤，至於是更加柔順還是更注重軌跡跟蹤精度則是由阻抗控制模型（算法）式（1）進行調節的。為了更好的理解阻抗模型各項的作用，可以對該阻抗模型進行簡化，例如 \(f_e =k(x_d-x)\)， 這個時候說明我們只考慮了控制機器人末端與環境之間的位置關系（由胡克定律，此時兩者之間就像只有一個彈簧連接一樣）；同樣地，我們讓 \(f_e = b(\dot{x}_d -\dot{x}) + k(x_d-x)\)，這個時候就同時考慮了控制機器人末端與環境之間的位置關系和速度關系（此時兩者之間應該是一個彈簧-阻尼系統）；進一步同時考慮考慮位移-速度-加速度時(即彈簧-阻尼-質量系統)，則得到了 \(f_e = m(\ddot{x}_d -\ddot{x}) + b(\dot{x}_d -\dot{x}) + k(x_d-x)\) 這樣完整的阻抗控制模型了。
當然，在這里所做的簡單陳述只是為了更好地理解阻抗控制的核心思想，其內容遠不止博主上面所說的這些（自己其實也學得稀里糊塗的），謹記。
離散化阻抗控制控制器的實現
這里使用了簡化的阻抗控制模型，即要求機器人末端執行器在 X/Y/Z 三個方向的運動是解耦的，這樣我們便可以對三個方向分別進行控制，也便於我們調節阻抗參數。
對於阻抗模型 \(f_e = m(\ddot{x}_d -\ddot{x}) + b(\dot{x}_d -\dot{x}) + k(x_d-x)\)，我們可以采用下面的方式進行離散化：
離散化阻抗控制器
function [Xr,Xr_dot,Xr_dotdot] = fcnlmp(Xd,Xd_dot,Xd_dotdot,f)
m =1;b = 200;k=1200;T = 0.005; % 參數初始化
persistent Xr_pre Xr_dot_pre;% 持久變量 https://ww2.mathworks.cn/help/matlab/ref/persistent.html
if isempty(Xr_pre)
Xr_pre = 0;
end
if isempty(Xr_dot_pre)
Xr_dot_pre = 0;
end
Xr_dotdot_pre = Xd_dotdot + 1/m*(-f + b*(Xd_dot - Xr_dot_pre) + k*(Xd - Xr_pre)); % 上一采樣時間 nT 機器人末端的實際運動加速度
Xr = Xr_pre + Xr_dot_pre*T; % 當前采樣時間 (n+1)T 機器人末端的實際運動位置
Xr_dot = Xr_dot_pre + Xr_dotdot_pre*T;% 當前采樣時間 (n+1)T 機器人末端的實際運動速度
Xr_dotdot = Xr_dotdot_pre; % 當前采樣時間 (n+1)T 機器人末端的實際運動加速度
Xr_dot_pre = Xr_dot; % 上一采樣時間 nT 機器人末端的實際運動速度
Xr_pre = Xr; % 上一采樣時間 nT 機器人末端的實際運動位置
end
圖為 2021/01/21 時博主做的組會 Presentation
基於阻抗控制的工業機器人軌跡跟蹤系統仿真
本項目已開源，【倉庫地址】： ER-16-Simulation 。
■ 環境說明
參考文獻 [1]，對埃夫特公司的 ER-16 系列六軸機器人進行 Simulink/Simscape 仿真，環境說明如下：
| 類別 | 下載地址 |
|---|---|
| ER16 系列機器人 3D-Model | 埃夫特智能裝備股份有限公司 |
| Matlab 2020b （版本 9.9） | MathWorks 官網 |
| Simulink （版本 10.2） | Matlab 菜單欄 -> 附加功能 -> 搜索安裝即可 |
| Simscape（版本 5.0） | Matlab 菜單欄 -> 附加功能 -> 搜索安裝即可 |
| Simscape Multibody（版本 7.2 ） | Matlab 菜單欄 -> 附加功能 -> 搜索安裝即可 |
| Simscape Multibody Link （版本 7.2） | 參考文章 《Matlab模型可視化...》 安裝即可 |
■ 系統說明
完整的軌跡跟蹤系統模型模型如下，其中涉及了機器人本體模型的搭建，離散化阻抗控制器的實現，以及末端軌跡生成器的設計。
機器人本體模型的搭建 是從 Solidworks 導出 xml 文件，再在 Matlab 中用 SimMechanics Link 讀取該 xml 文件從而獲取裝配信息得到的，相比於手動裝配確實省事了不少。SimMechanics Link 的使用可以參考文章 Matlab模型可視化仿真:SimMechanics Link的安裝與使用。機器人模型子系統呈現如下：
離散化阻抗控制器 是直接利用了之前的成果，將其拓展到 X/Y/Z 三個維度即可。
對於機器人的運動控制，一個很棘手的問題就是逆運動學求解，這里的處理方式是在機器人末端執行器的頂端額外添加一個關節（Bushing Joint 模塊），將 X/Y/Z 三個方向的位置信息作為輸入，由該關節 “牽” 着這個龐大的六軸機器人運動（有牽着鼻子走內味了嗷🤭），至於六個軸該怎么動就交給擁有強大的計算能力的 Matlab 自己去算吧！這樣放養式的逆運動學求解最大的毛病就是多解的問題，再加上 Simscape 這個工具貌似沒有碰撞檢測，所以不加以限制機器人運動起來就會變成 “奇形種” hhhhh。博主是將第五關節模塊）的運動設置成了必須精准計算（Accurate）情況才有所好轉的！
阻抗控制器 + 運動控制子系統 實現如下：
末端軌跡生成器 需要生成 X/Y/Z 三個方向的位置提供給阻抗控制器，根據期望軌跡的不同，生成方式也有很多，這里只進行了圓周和直線軌跡的模擬，所以實現方式就比較簡單，利用極坐標和插值就可以了。
function Traj = traj(Mode,Clock,Stoptime)
if Mode == 0 % 圓周軌跡
theta = 2*pi*Clock/Stoptime;
Xd = 0.2*cos(theta)-0.2;
Yd = 0;
Zd = 0.2*sin(theta);
Traj = [Xd;Yd;Zd];
else % 直線軌跡
Xd = 0;
Yd = 0;
Zd = 0.2*Clock/Stoptime;
Traj = [Xd;Yd;Zd];
end
end
■ 結果展示
給定參考軌跡（包括圓周軌跡和直線軌跡兩類），在機器人運動過程中施加外力干擾（借助信號發生器實現），機器人在進行軌跡跟蹤的同時做出主動柔順運動。
后記
作為本科階段最后一門專業課程，《機器人動力學與控制》這門課也沒逃過感覺也沒學到啥的命運，腦子里倒是清晰地記得老師反復念叨的那句 —— “別問我為什么要這么干，人家就是這樣搞的，記公式就完事了！” 像極了期末抱佛腳的自己🤭
經過四年的磨練，越發覺得一門課程學得好不好還是得看自己是不是真的能拿出來用了 —— 微機那些 8086 什么的沒學會，實驗學的匯編倒是用得挺溜；學完數電甚至能在 Minecraft 里面像模像樣的玩起 “數電科技”😄 多虧了這些相關的實驗和平時做的項目，自己才掌握了不少本領，也幫助我在和研究生導師面談的時候有話可聊（有逼可吹）🌹 希望看到這篇文章的外校內校的學弟學妹們也能認真對待自己的實驗~🐕
項目開源
【倉庫地址】： https://github.com/Beta-y/ER-16-Simulation
參考文獻
【1】陳明,甘亞輝,戴先中,等.基於阻抗控制的工業機器人軌跡跟蹤系統設計[C].//中國人工智能學會.第十二屆中國智能機器人大會論文集.2017:324-331.
【2】Hogan N. Impedance control - An approach to manipulation. I- Theory. II - Implementation. III - Applications[J]. AsmeTransactions Journal of Dynamic Systems & Measurement Control B, 1984, 107(1):304-313.
【3】李正義.機器人與環境間力/位置控制技術研究與應用[D].湖北:華中科技大學,2011. DOI:10.7666/d.d186146.
【4】楊振.基於阻抗控制的機器人柔順性控制方法研究[D].江蘇:東南大學,2005. DOI:10.7666/d.y943306.
Q&A
- 讀者 “杜楠楠” 郵箱留言 2021年4月22日（星期四）上午10 : 19
" 同學，您好！我也開始在做阻抗控制方面的研究，最近在CSDN上看到了您的文章“基於阻抗控制的工業機器人軌跡跟蹤系統 Simulink/Simscape 仿真”，並成功復現了您開源的代碼。這里有幾個問題想請教您。
1、在這篇文章中是不是沒有針對阻抗環境修正后的位置xr的伺服控制（實際位置x跟蹤xr，利用PID之類的），因為在plot文件中直接輸出的就是xr的信號。
2、CAD軟件導入Simulink的xml文件是不包含模型的物理信息（質量、質心、慣性矩等）嗎？因為我看知乎中“Matlab模型可視化仿真:SimMechanics Link的安裝與使用”這篇文章是這么說的。另外看您的datafile文件中通過代碼的形式定義了各個部件的物理信息。
3、下一個問題就是，既然這個仿真模型中沒有基於動力學模型，那為什么還要定義模型的物理信息呢？
4、另外，關於bushing joint環節，哪個輸出是逆運動學解出來的關節位置變量呢？這部分看得比較暈。在阻抗控制這方面，前期一直在看論文，代碼能力不強，因此一直苦於仿真的實現。所以十分感謝您的共享，獲益匪淺。期待您的回復。再次感謝。"
1.博主回復 2021年4月22日（星期四）上午10 : 55
“ 同學你好，很高興收到你的來信，你提的幾個問題可以說是問到了點子上，那我就我簡單對它們解釋一下吧。
問題1. 修正后的 Xr 的位置實際上就是機器人末端的實際位置 X，之所以沒有加額外的伺服控制是因為這里直接借助了 Simscape 的輸入期望關節的 Motion 自動解算其控制力矩 Force 的功能，這個你可以雙擊 BushingJoint 在 X/Y/Z Prismatic Primitive->Acutation 中找到；
問題2. xml 文件可以理解成整個機器人系統的裝配信息，其中包含了各個零件之間的配合關系，當然也包含了它們的物理信息，你也可以用任意的文本編輯器 (比如記事本) 打開這個文件找到這些信息。至於 Datafile 文件的作用是將 xml 文件翻譯成 Matlab 可讀的 m 語言形式供 Simulink 仿真模型使用，其中也包含了裝配信息和物理信息；
問題3. 這里是一個資源選取的問題，SolidWorks 導出 xml 文件時默認是將所有信息一並導出的，而我們確實只用到了裝配信息但是實際確實沒有使用到物理信息，你甚至可以將各個 Solid 模塊的質量、密度設置成任意值，這都不會影響到我們的控制；
問題4.這個問題我在博客中用了一個很接地氣的方式表述了一下 —— “牽着鼻子走”；之所以這樣形容是因為確實我們沒有做各個關節的逆運動學工作，而是依靠 Simscape 的自動求解功能完成的，這一點與問題1類似，你可以在機械臂各個關節的同一屬性 Acutation 中找到 Torque = None, Motion = Automatically caculated 之類的設置。實際上 Bushing Joint 是我們設置的一個虛擬關節用於表征末端執行器的位置，實際上並不存在這一對象，我們所做的只是告知末端執行器應該往哪里走（Xr），然后借助 Torque -> Automatically caculated 讓其到達指定位置，而其余幾個臂的運動是由 Simscape 根據末端執行器的實際位置進行解算的。以上是所有問題的回復，希望對你有幫助。