詢問我們 價格垂詢 遠程視頻 資料下載 0510-8815 8845
控制器的位置環程序為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解耦;
Copyright ? 無錫斯帝爾科技有限公司 2005-2014 All Rights Reserved 蘇ICP備13017180號
微信公眾平臺 斯帝爾CEO想傾聽斯帝爾科技 您的聲音
Powered by MetInfo 5.2 ©2008-2020 MetInfo Inc.