[發明專利]基于六維力/力矩傳感器的機器人力跟隨運動控制方法

有效

• 申請號: 201310200246.9
• 申請日: 2013-05-24
• 公開（公告）號: CN103279037A
• 公開（公告）日: 2013-09-04
• 發明（設計）人: 張鐵;林君健
• 申請（專利權）人: 華南理工大學
• 主分類號: G05B13/04
• 分類號: G05B13/04
• 代理公司: 廣州市華學知識產權代理有限公司 44245
• 代理人: 蔡茂略

1.基于六維力/力矩傳感器的機器人力跟隨運動控制方法，其特征在于包括下述步驟：

（1）通過反饋力/力矩傳感器當前姿態并利用以下公式對力/力矩傳感器的力和力矩數值進行補償：

Fxc=r31kxzG+Fx0Fyc=r32kyzG+Fy0Fzc=r33G+Fz0Mxc=Mx0Myz=My0Mzc=Mz0]]>

式中，r₃₁、r₃₂、r₃₃分別從力傳感器當前姿態T=r11r12r13r21r22r23r31r32r33]]>中獲得；

（2）先以力傳感器x軸朝上作為初始位置1，測得當前力傳感器的數據為F_x1，F_y1，F_z1，M_x1，M_y1，M_z1；在初始位置1基礎上繞z軸逆時針旋轉90°得到位置2，測得當前力傳感器的變量為F_x2，F_y2，F_z2，M_x2，M_y2，M_z2；在初始位置2基礎上繞z軸逆時針旋轉90°得到位置3，測得當前力傳感器的變量為F_x3，F_y3，F_z3，M_x3，M_y3，M_z3；在初始位置3基礎上繞z軸逆時針旋轉90°得到位置4，測得當前力傳感器的變量為F_x4，F_y4，F_z4，M_x4，M_y4，M_z4；在初始位置1基礎上繞y軸逆時針旋轉90°得到位置5，測得當前力傳感器的變量為F_x5，F_y5，F_z5，M_x5，M_y5，M_z5；在初始位置1基礎上繞y軸順時針旋轉90°得到位置6，測得當前力傳感器的變量為F_x6，F_y6，F_z6，M_x6，M_y6，M_z6

通過以下公式獲得相應的初值：

Fx0=(Fx1+Fx3)/2Fy0=(Fy2+Fy4)/2Fz0=(Fz5+Fz6)/2]]>

Mx0=(Mx2+Mx4+Mx5+Mx6)/4My0=(My1+My3+My5+My6)/4Mz0=(Mz1+Mz2+Mz3+Mz4)/4]]>

算出初值后，計算x軸、y軸和z軸分別測得的重力值：

Gx=|Fx3-Fx1|/2Gy=|Fy2-Fy4|/2Gz=|Fz6-Fz5|/2]]>

根據這個重力值計算出傳感器各軸線性度之間的偏差關系

kxz=Gx/Gzkyz=Gy/Gz]]>

以G_z為重力基準，即G=G_z

根據當前力/力矩傳感器的姿態，利用步驟（1）中的公式算出力/力矩傳感器當前的姿態的零位值F_xc、F_yc、F_zc、M_xc、M_yc、M_zc，通過以上公式，對力/力矩傳感器的基本參數進行標定；

（3）利用步驟（1）中的公式分別求出步驟（2）中所測得的6個位置的零位值，并與測量值進行比較，通過下面公式算出此時各個測量量的誤差最大絕對值：

ηf=maxi=16|Fni-Fnc|,(n=x,y,z)]]>

ηm=maxi=16|Mni-Mnc|,(n=x,y,z)]]>

式中F_ni、M_ni表示傳感器測量回來的量，F_nc、M_nc表示相應通道計算出來的補償量；各通道誤差的最大絕對值分別記為η_fx、η_fy、η_fz、η_mx、η_my、η_mz

取穩定系數ε_fx、ε_fy、ε_fz（均大于1，建議選1.5～2.0），令力/力矩傳感器的當前零位穩定區間為：

δfx=ηfxϵfxδfy=ηfyϵfyδfz=ηfzϵfzδmx=ηmxϵmxδmy=ηmyϵmyδmz=ηmzϵmz]]>

則力/力矩傳感器的力端零位穩定區間分別為力矩端的零位穩定區間分別為[M_xc-δ_mx,M_xc+δ_mx]、[M_zc-δ_mz,M_zc+δ_mz]

通過上述方法獲得對力/力矩傳感器當前姿態下的力和力矩數值穩定區間進行設置；

（4）根據步驟（1）、（2）和（3）設計得到力跟隨的控制方法力端為：

uf=kfc(Fm-Fc+δ)Fm<Fc-δ0Fc-δ<Fm<Fc+δkfc(Fm-Fc-δ)Fm>Fc+δ]]>

式中F_m表示在某個力方向上施加力后的測量值，F_c表示在該方向上對力的補償量，δ表示該方向上的穩定區間；

同理，力矩端的控制方法為：

um=kmc(Mm-Mc+δ)Mm<Mc-δ0Mc-δ<Mm<Mc+δkmc(Mm-Mc-δ)Mm>Mc+δ]]>

式中M_m表示在某個力矩方向上施加力后的測量值，M_c表示在該方向上對力的補償量，δ表示該方向上的穩定區間；

控制值將反饋到機器人上進行力跟隨控制，并通過以下公式進行：

q·=J-1u]]>

其中J為機器人當前的雅可比矩陣，u為力和力矩的控制輸入，表示為

u=ufum=ufxufyufzumxumyumz]]>

為機器人各關節的速度，算出各關節的速度后，利用該速度對機器人各關節進行控制，使其作力跟隨運動，每當機器人姿態發生改變，都需要使用步驟（2）中的方法對傳感器的零位值重新標定。