[發(fā)明專利]阻抗控制方法、裝置、阻抗控制器和機(jī)器人

有效

• 申請?zhí)枺?/td>: 202010650259.6
• 申請日: 2020-07-08
• 公開（公告）號(hào): CN111730599B
• 公開（公告）日: 2021-09-07
• 發(fā)明（設(shè)計(jì)）人: 黃荔群;任曉雨;趙明國;熊友軍
• 申請（專利權(quán)）人: 深圳市優(yōu)必選科技股份有限公司
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16
• 代理公司: 北京超凡宏宇專利代理事務(wù)所(特殊普通合伙) 11463
• 代理人: 梁韜

1.一種阻抗控制方法，其特征在于，包括：

獲取機(jī)械臂當(dāng)前在關(guān)節(jié)空間的關(guān)節(jié)運(yùn)動(dòng)信息和關(guān)節(jié)受力信息以及末端受到的實(shí)際交互力，根據(jù)所述關(guān)節(jié)運(yùn)動(dòng)信息通過正運(yùn)動(dòng)學(xué)求解所述末端在任務(wù)空間的實(shí)際運(yùn)動(dòng)信息；所述實(shí)際運(yùn)動(dòng)信息包括所述末端當(dāng)前的實(shí)際位置和實(shí)際速度，所述末端的期望信息包括所述末端當(dāng)前的期望速度、期望加速度和修正期望軌跡；

利用當(dāng)前環(huán)境信息和在所述任務(wù)空間預(yù)先規(guī)劃的末端期望交互力計(jì)算所述末端的所述修正期望軌跡，根據(jù)所述修正期望軌跡與所述實(shí)際位置之間的位置偏差、所述期望速度和所述實(shí)際速度之間的速度偏差和所述實(shí)際交互力基于彈簧-質(zhì)量-阻尼模型計(jì)算在所述任務(wù)空間的阻尼控制量；將所述關(guān)節(jié)受力信息由所述關(guān)節(jié)空間轉(zhuǎn)換為所述任務(wù)空間，根據(jù)所述阻尼控制量、所述期望加速度和轉(zhuǎn)換后的關(guān)節(jié)受力信息計(jì)算在所述任務(wù)空間的加速度控制量；根據(jù)所述任務(wù)空間的所述加速度控制量基于雅克比矩陣計(jì)算得到所述機(jī)械臂在所述關(guān)節(jié)空間的阻抗控制力矩；

構(gòu)建所述機(jī)械臂的動(dòng)力學(xué)方程，根據(jù)所述動(dòng)力學(xué)方程中的非線性項(xiàng)確定補(bǔ)償力矩；

根據(jù)所述阻抗控制力矩和所述補(bǔ)償力矩對所述機(jī)械臂進(jìn)行關(guān)節(jié)力矩控制。

2.根據(jù)權(quán)利要求1所述的阻抗控制方法，其特征在于，所述動(dòng)力學(xué)方程為所述機(jī)械臂在所述任務(wù)空間的動(dòng)力學(xué)方程，所述任務(wù)空間的動(dòng)力學(xué)方程的構(gòu)建包括：

構(gòu)建所述機(jī)械臂在所述關(guān)節(jié)空間的動(dòng)力學(xué)方程；

基于各關(guān)節(jié)與所述末端之間的運(yùn)動(dòng)關(guān)系將所述關(guān)節(jié)空間的動(dòng)力學(xué)方程轉(zhuǎn)換為所述任務(wù)空間的動(dòng)力學(xué)方程。

3.根據(jù)權(quán)利要求1或2所述的阻抗控制方法，其特征在于，所述當(dāng)前環(huán)境信息包括當(dāng)前的環(huán)境等效剛度和環(huán)境位置；所述利用當(dāng)前環(huán)境信息和在所述任務(wù)空間預(yù)先規(guī)劃的末端期望交互力計(jì)算所述末端的修正期望軌跡，包括：

根據(jù)當(dāng)前的所述環(huán)境等效剛度、所述環(huán)境位置和所述末端期望交互力按照如下公式計(jì)算得到所述末端的修正期望軌跡；

其中，表示修正期望軌跡；x_f表示環(huán)境位置；K_eq表示環(huán)境等效剛度；F_ref表示末端期望交互力。

4.根據(jù)權(quán)利要求1所述的阻抗控制方法，其特征在于，所述關(guān)節(jié)運(yùn)動(dòng)信息包括各關(guān)節(jié)的角位移和角速度；所述根據(jù)所述關(guān)節(jié)運(yùn)動(dòng)信息通過正運(yùn)動(dòng)學(xué)求解所述末端在任務(wù)空間的實(shí)際運(yùn)動(dòng)信息，包括：

根據(jù)所述各關(guān)節(jié)的角位移和角速度通過正運(yùn)動(dòng)學(xué)求解所述末端在所述任務(wù)空間的所述實(shí)際位置和所述實(shí)際速度；

所述機(jī)械臂各關(guān)節(jié)的所述角位移q、所述角速度所述末端的所述實(shí)際位置x，所述實(shí)際速度滿足如下公式：

x＝f(q)；

其中，f(q)表示關(guān)節(jié)角位移與末端位置之間的映射關(guān)系；J為雅克比矩陣。

5.根據(jù)權(quán)利要求2所述的阻抗控制方法，其特征在于，所述構(gòu)建所述機(jī)械臂在所述關(guān)節(jié)空間的動(dòng)力學(xué)方程，包括：

根據(jù)獲取的所述關(guān)節(jié)空間的所述關(guān)節(jié)運(yùn)動(dòng)信息計(jì)算所述機(jī)械臂受到的作用力向量、離心力與科式力矩陣、以及重力向量；

根據(jù)所述作用力向量、所述離心力與科式力矩陣、所述重力向量、所述實(shí)際交互力和所述關(guān)節(jié)受力信息構(gòu)建所述機(jī)械臂在關(guān)節(jié)空間的動(dòng)力學(xué)方程。

6.根據(jù)權(quán)利要求5所述的阻抗控制方法，其特征在于，所述任務(wù)空間的動(dòng)力學(xué)方程中的所述非線性項(xiàng)包括含所述離心力與科式力矩陣的第一非線性項(xiàng)和含所述重力向量的第二非線性項(xiàng)；

將所述第一非線性項(xiàng)和所述第二非線性項(xiàng)進(jìn)行疊加，以作為所述任務(wù)空間中進(jìn)行非線性補(bǔ)償?shù)难a(bǔ)償力矩。