[發(fā)明專利]一種基于虛擬現(xiàn)實(shí)技術(shù)的帶電作業(yè)機(jī)器人機(jī)械臂協(xié)作力反饋主從控制方法及系統(tǒng)

在審

• 申請(qǐng)?zhí)枺?/td>: 201810319664.2
• 申請(qǐng)日: 2018-04-11
• 公開（公告）號(hào): CN108582032A
• 公開（公告）日: 2018-09-28
• 發(fā)明（設(shè)計(jì)）人: 郭健;湯馮煒;郭毓;吳禹均;龔勛;韓若冰
• 申請(qǐng)（專利權(quán)）人: 南京理工大學(xué)
• 主分類號(hào): B25J5/06
• 分類號(hào): B25J5/06;B25J3/00;B25J9/16
• 代理公司: 南京理工大學(xué)專利中心 32203
• 代理人: 孟睿

1.一種基于虛擬現(xiàn)實(shí)技術(shù)的帶電作業(yè)機(jī)器人機(jī)械臂協(xié)作力反饋主從控制方法，其特征在于，操作手、機(jī)械臂和工控機(jī)組成帶電作業(yè)機(jī)器人力反饋主從控制系統(tǒng)；主操作手力反饋控制時(shí)，工控機(jī)根據(jù)機(jī)械臂末端的受力數(shù)據(jù)，計(jì)算出主操作手各個(gè)關(guān)節(jié)的扭矩?cái)?shù)據(jù)，通過主操作手的控制接口，控制主操作手各關(guān)節(jié)輸出扭矩；

所述械臂末端受力數(shù)據(jù)的標(biāo)量為六自由度力/力矩?cái)?shù)據(jù)，包括兩部分：一部分來自于機(jī)械臂與環(huán)境接觸時(shí)產(chǎn)生的受力數(shù)據(jù)，另一部分來自于機(jī)械臂之間相互碰撞而產(chǎn)生的受力數(shù)據(jù)；前述兩部分受力數(shù)據(jù)均采用基于虛擬現(xiàn)實(shí)的技術(shù)獲得，其中，獲得機(jī)械臂之間相互碰撞而產(chǎn)生的受力數(shù)據(jù)的過程為：

步驟1，獲取第二機(jī)械臂六個(gè)關(guān)節(jié)的角度數(shù)據(jù)，根據(jù)第二機(jī)械臂外型模型數(shù)據(jù)，生成第二機(jī)械臂的虛擬現(xiàn)實(shí)數(shù)據(jù)；

步驟2，運(yùn)用第二機(jī)械臂的基坐標(biāo)系到第一機(jī)械臂的基坐標(biāo)系的轉(zhuǎn)換矩陣，將第二機(jī)械臂的虛擬現(xiàn)實(shí)數(shù)據(jù)轉(zhuǎn)換到第一機(jī)械臂的基坐標(biāo)系中；

步驟3，運(yùn)用八分法對(duì)第二機(jī)械臂的虛擬現(xiàn)實(shí)數(shù)據(jù)進(jìn)行逐步劃分，構(gòu)建八叉樹；

步驟4，獲取第一機(jī)械臂末端位姿數(shù)據(jù)，以該末端位姿數(shù)據(jù)為球心，創(chuàng)建包圍球，該包圍球的半徑為所需碰撞檢測(cè)的距離；

步驟5，對(duì)步驟4的包圍球與步驟3的八叉樹進(jìn)行碰撞檢測(cè)，若沒有發(fā)生碰撞，則機(jī)械臂之間的受力數(shù)據(jù)為零，若發(fā)生碰撞，則獲得虛擬現(xiàn)實(shí)場(chǎng)景中發(fā)生碰撞的數(shù)據(jù)點(diǎn)，根據(jù)發(fā)生碰撞的數(shù)據(jù)點(diǎn)坐標(biāo)與第一機(jī)械臂末端位姿數(shù)據(jù)，計(jì)算碰撞深度矢量，然后進(jìn)入步驟6；

步驟6，采用彈簧模型計(jì)算第一機(jī)械臂末端受力數(shù)據(jù)，

F＝-kx

其中，k為彈簧系數(shù)，x為碰撞深度矢量，F(xiàn)為受力數(shù)據(jù)，F(xiàn)為六維矢量，其標(biāo)量為六自由度力/力矩?cái)?shù)據(jù)；

步驟7，將步驟6獲得的六自由度力/力矩?cái)?shù)據(jù)符號(hào)取反獲得第二機(jī)械臂末端的六自由度力/力矩?cái)?shù)據(jù)。

2.如權(quán)利要求1所述基于虛擬現(xiàn)實(shí)技術(shù)的帶電作業(yè)機(jī)器人機(jī)械臂協(xié)作力反饋主從控制方法，其特征在于，獲得機(jī)械臂與環(huán)境接觸時(shí)產(chǎn)生的受力數(shù)據(jù)的過程為：

步驟1，運(yùn)用八分法對(duì)機(jī)械臂作業(yè)場(chǎng)景對(duì)應(yīng)的虛擬現(xiàn)實(shí)場(chǎng)景進(jìn)行逐步劃分，構(gòu)建八叉樹；

步驟2，獲取機(jī)械臂末端位姿數(shù)據(jù)，以該末端位姿數(shù)據(jù)為球心，創(chuàng)建包圍球，設(shè)置包圍球的半徑為所需碰撞檢測(cè)的距離；

步驟3，對(duì)步驟2的包圍球與步驟1的八叉樹進(jìn)行碰撞檢測(cè)，若沒有發(fā)生碰撞，則機(jī)械臂末端的六自由度力/力矩?cái)?shù)據(jù)為零，若發(fā)生碰撞，則獲取虛擬現(xiàn)實(shí)場(chǎng)景中發(fā)生碰撞的數(shù)據(jù)點(diǎn)，根據(jù)發(fā)生碰撞的數(shù)據(jù)點(diǎn)坐標(biāo)與機(jī)械臂末端位姿數(shù)據(jù)，計(jì)算碰撞深度矢量；

步驟4，根據(jù)碰撞深度矢量計(jì)算機(jī)械臂末端六自由度力/力矩?cái)?shù)據(jù)，即采用彈簧阻尼模型計(jì)算反饋力，

F＝-kx-cv

其中，k為彈簧系數(shù)，x為碰撞深度矢量，c為阻尼系數(shù)，v為機(jī)械臂末端速度矢量，F(xiàn)為反饋力，其標(biāo)量為六自由度力/力矩?cái)?shù)據(jù)。

3.如權(quán)利要求1所述基于虛擬現(xiàn)實(shí)技術(shù)的帶電作業(yè)機(jī)器人機(jī)械臂協(xié)作力反饋主從控制方法，其特征在于，對(duì)主操作手靜力學(xué)建模，得到機(jī)械臂末端六自由度力/力矩?cái)?shù)據(jù)到主操作手各個(gè)關(guān)節(jié)扭矩?cái)?shù)據(jù)的轉(zhuǎn)換關(guān)系，工控機(jī)根據(jù)所述轉(zhuǎn)換關(guān)系計(jì)算出主操作手各個(gè)關(guān)節(jié)的扭矩?cái)?shù)據(jù)。

4.如權(quán)利要求1所述基于虛擬現(xiàn)實(shí)技術(shù)的帶電作業(yè)機(jī)器人機(jī)械臂協(xié)作力反饋主從控制方法，其特征在于，機(jī)械臂位姿控制時(shí)，工控機(jī)根據(jù)主操作手各旋轉(zhuǎn)關(guān)節(jié)的角度數(shù)據(jù)，計(jì)算出機(jī)械臂末端速度矢量的期望值，通過機(jī)械臂的控制接口，按照所述期望值控制機(jī)械臂運(yùn)動(dòng)；獲取機(jī)械臂末端速度矢量期望值的方法為：

運(yùn)用D-H建模方法對(duì)主操作手建模，得出主操作手的各個(gè)旋轉(zhuǎn)關(guān)節(jié)的角度數(shù)據(jù)到主操作手末端位姿數(shù)據(jù)的轉(zhuǎn)換關(guān)系；工控機(jī)接收主操作手發(fā)送的各個(gè)旋轉(zhuǎn)關(guān)節(jié)的角度數(shù)據(jù)，根據(jù)所述轉(zhuǎn)換關(guān)系計(jì)算得到主操作手末端位姿數(shù)據(jù)，然后將主操作手末端位姿數(shù)據(jù)通過微分運(yùn)算得到主操作手末端速度數(shù)據(jù)；根據(jù)主操作手末端位姿數(shù)據(jù)和末端速度數(shù)據(jù)，使用速度前饋PID控制器，計(jì)算得到機(jī)械臂末端速度矢量期望值。

5.如權(quán)利要求1至4所述的任意一種基于虛擬現(xiàn)實(shí)技術(shù)的帶電作業(yè)機(jī)器人機(jī)械臂協(xié)作力反饋主從控制方法，其特征在于，所述主操作手和機(jī)械臂均為一個(gè)或者多個(gè)自由度機(jī)械臂結(jié)構(gòu)。