基于阻抗控制的工業機器人軌跡跟蹤系統 Simulink/Simscape 仿真

??本文是對文獻 [1] 的復現，

關于阻抗控制

??阻抗控制的提出可以追溯到 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 =k(x_d-x) fe?=k(xd??x)， 這個時候說明我們只考慮了控制機器人末端與環境之間的位置關系（由胡克定律，此時兩者之間就像只有一個彈簧連接一樣）；同樣地，我們讓 f e = b ( x ˙ d ? x ˙ ) + k ( x d ? x ) f_e = b(\dot{x}_d -\dot{x}) + k(x_d-x) fe?=b(x˙d??x˙)+k(xd??x)，這個時候就同時考慮了控制機器人末端與環境之間的位置關系和速度關系（此時兩者之間應該是一個彈簧-阻尼系統）；進一步同時考慮考慮位移-速度-加速度時(即彈簧-阻尼-質量系統)，則得到了 f e = m ( x ¨ d ? x ¨ ) + b ( x ˙ d ? x ˙ ) + k ( x d ? x ) f_e = m(\ddot{x}_d -\ddot{x}) + b(\dot{x}_d -\dot{x}) + k(x_d-x) fe?=m(x¨d??x¨)+b(x˙d??x˙)+k(xd??x) 這樣完整的阻抗控制模型了，
??當然，在這里所做的簡單陳述只是為了更好地理解阻抗控制的核心思想，其內容遠不止博主上面所說的這些（自己其實也學得稀里糊涂的），謹記，

離散化阻抗控制控制器的實作

??這里使用了簡化的阻抗控制模型，即要求機器人末端執行器在 X/Y/Z 三個方向的運動是解耦的，這樣我們便可以對三個方向分別進行控制，也便于我們調節阻抗引數，
??對于阻抗模型 f e = m ( x ¨ d ? x ¨ ) + b ( x ˙ d ? x ˙ ) + k ( x d ? x ) f_e = m(\ddot{x}_d -\ddot{x}) + b(\dot{x}_d -\dot{x}) + k(x_d-x) fe?=m(x¨d??x¨)+b(x˙d??x˙)+k(xd??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 仿真，環境說明如下：

■ 系統說明

??完整的軌跡跟蹤系統模型模型如下，其中涉及了機器人本體模型的搭建，離散化阻抗控制器的實作，以及末端軌跡生成器的設計，

??機器人本體模型的搭建 是從 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.