本發(fā)明涉及機械臂技術(shù)領(lǐng)域，特別是涉及一種機械臂運動控制方法和系統(tǒng)。

背景技術(shù)：

在機械臂實際的工作場景中，可能會存在眾多的障礙物。在運動規(guī)劃問題中，需要在給定的起點，終點之間，規(guī)劃出一條避免碰撞的路徑，從而保證運行的安全。

機械臂的運動規(guī)劃分為離線和在線兩種方式。離線方式可以在機械臂運動前預(yù)先規(guī)劃好運行路徑，但是這需要事前知道工作空間中障礙物的幾何結(jié)構(gòu)和位置姿態(tài)；而在線方式系統(tǒng)一邊通過傳感器獲取實時的環(huán)境數(shù)據(jù)，一邊同步對機械臂的運動進(jìn)行規(guī)劃。一般來說，離線方式效率較高，但是對于一些存在動態(tài)障礙物的工作環(huán)境中，必須使用在線方式。

現(xiàn)有的械臂路徑在線控制方式一般是根據(jù)障礙物的分布實時計算機械臂運動的多個中間點，根據(jù)中間點控制機械臂的運動路徑，這種方式計算量較大，導(dǎo)致機械臂運動控制效率較低。

技術(shù)實現(xiàn)要素：

基于此，有必要針對機械臂運動控制效率較低的問題，提供一種機械臂運動控制方法和系統(tǒng)。

一種機械臂運動控制方法，包括以下步驟：

獲取機械臂運動的起點與終點之間的障礙物的實時位置；

根據(jù)所述實時位置獲取各個障礙物與機械臂關(guān)節(jié)上預(yù)先選定的控制點的最短距離，計算所述最短距離之和；

根據(jù)所述最短距離之和與所述機械臂關(guān)節(jié)的運動速度實時地計算對所述控制點的第一虛擬力以及對機械臂終端的第二虛擬力，根據(jù)所述第一虛擬力和第二虛擬力確定對所述機械臂的實時控制力矩；

根據(jù)所述實時控制力矩對機械臂進(jìn)行實時路徑控制。

一種機械臂運動控制系統(tǒng)，包括：

第一獲取模塊，用于獲取機械臂運動的起點與終點之間的障礙物的實時位置；

第二獲取模塊，用于根據(jù)所述實時位置獲取各個障礙物與機械臂關(guān)節(jié)上預(yù)先選定的控制點的最短距離，計算所述最短距離之和；

計算模塊，用于根據(jù)所述最短距離之和與所述機械臂關(guān)節(jié)的運動速度實時地計算對所述控制點的第一虛擬力以及對機械臂終端的第二虛擬力，根據(jù)所述第一虛擬力和第二虛擬力確定對所述機械臂的實時控制力矩；

路徑控制模塊，用于根據(jù)所述實時控制力矩對機械臂進(jìn)行實時路徑控制。

上述機械臂運動控制方法和系統(tǒng)，能夠根據(jù)障礙物的實時位置獲取各個障礙物與機械臂關(guān)節(jié)上預(yù)先選定的控制點的最短距離之和，根據(jù)該最短距離之和計算第一虛擬力和第二虛擬力，根據(jù)第一虛擬力和第二虛擬力生成實時控制力矩，并根據(jù)實時控制力矩對機械臂進(jìn)行實時路徑控制，在機械臂運動過程中，能夠根據(jù)機械臂與障礙物之間的距離生成一個作用力，將機械臂推離障礙物，避免機械臂撞上障礙物。上述方法和系統(tǒng)計算量小，控制效率高。

附圖說明

圖1為一個實施例的機械臂運動控制方法流程圖；

圖2為一個實施例的機械臂關(guān)節(jié)及控制點的示意圖；

圖3為一個實施例的機械臂運動控制方法的程序流圖；

圖4為一個實施例的機械臂運動控制系統(tǒng)的結(jié)構(gòu)示意圖。

具體實施方式

下面結(jié)合附圖對本發(fā)明的技術(shù)方案進(jìn)行說明。

如圖1所示，本發(fā)明提供一種機械臂運動控制方法，可包括以下步驟：

s1，獲取機械臂運動的起點與終點之間的障礙物的實時位置；

本實施例中的實時位置可以是動態(tài)的位置，即，各個障礙物的實時位置可隨時間變化。起點與終點之間是指機械臂從起點ps運動到終點pe的運動路徑上可能經(jīng)過的位置。在一個實施例中，可以通過傳感器獲取障礙物的實時位置，傳感器可以以一定的頻率采集各個障礙物的實時位置。在一個具體實施例中，可以獲取障礙物的運動速度，根據(jù)障礙物的運動速度設(shè)置傳感器采集實時位置的頻率。例如，當(dāng)障礙物的運動速度大于預(yù)設(shè)的速度閾值時，可以以較大的頻率采集實時位置；反之，可以以較小的頻率采集實時位置。這里的“較大”和“較小”可以是相對于某個默認(rèn)的采集頻率的值。進(jìn)一步地，還可以設(shè)置多個閾值檔，當(dāng)障礙物的運動速度大于第一速度閾值時，以第一頻率采集實時位置；當(dāng)障礙物的運動速度大于第二速度閾值時，以第二頻率采集實時位置；……；當(dāng)障礙物的運動速度大于第n速度閾值時，以第n(n為正整數(shù))頻率采集實時位置；其中，第一頻率、第二頻率、……、第n頻率的值依次減小。通過設(shè)置不同的速度閾值與對應(yīng)的采集頻率，既可以保證障礙物位置獲取的時效性，又可以在障礙物位置變換較慢的情況下減少數(shù)據(jù)交互量。

s2，根據(jù)所述實時位置獲取各個障礙物與機械臂關(guān)節(jié)上預(yù)先選定的控制點的最短距離，計算所述最短距離之和；

控制點可以是機械臂關(guān)節(jié)上的一個點，這個點可以預(yù)先選定。機械臂的每個關(guān)節(jié)上均可以設(shè)置一個控制點。一個實施例的機械臂關(guān)節(jié)及控制點的示意圖如圖2所示，機械臂包括三個關(guān)節(jié)，三個關(guān)節(jié)上各有一個選定的控制點p1(q)、p2(q)和p3(q)，o為機械臂工作空間中的障礙物集合，peff(q)為機械臂的終端位置，機械臂終端是指在機械臂末端，用于與環(huán)境交互的設(shè)備，具體取決于機器人的應(yīng)用場景。比說，在機械臂末端安裝一個夾子，那這個夾子就是這個機械臂的終端。參數(shù)q＝[q1,q2,…,qn]表示機械臂各個關(guān)節(jié)的角度。對每個控制點，可分別計算各個障礙物到該控制點的最短距離之和。對于第i個控制點，t時刻各個障礙物到該控制點的最短距離之和可記為：

式中，di,t表示t時刻各個障礙物與第i個控制點的最短距離之和，pi表示第i個機械臂關(guān)節(jié)上的控制點，pt'表示t時刻障礙物的實時位置，oi表示第i個機械臂關(guān)節(jié)的工作空間，k表示機械臂關(guān)節(jié)的個數(shù)。

s3，根據(jù)所述最短距離之和與所述機械臂關(guān)節(jié)的運動速度實時地計算對所述控制點的第一虛擬力以及對機械臂終端的第二虛擬力，根據(jù)所述第一虛擬力和第二虛擬力確定對所述機械臂的實時控制力矩；

根據(jù)機械臂的動力學(xué)，t時刻笛卡爾空間中第i個機械臂關(guān)節(jié)的運動速度與第i個機械臂關(guān)節(jié)的角速度之間存在以下關(guān)系：

式中，j為雅克比矩陣，t表示轉(zhuǎn)置操作，為表示pi(q)對時間的導(dǎo)數(shù)，為qi對時間的導(dǎo)數(shù)，i＝1,2,…,k。

同樣地，笛卡爾空間中的力f與機械臂各個關(guān)節(jié)的力矩π＝[π0,π1,…,πk]之間存在以下關(guān)系：

f＝j(luò)π；(4)

π＝j(luò)^tf；(5)

在t時刻，對于第i個控制點，可生成如下第一虛擬力：

式中，fi,t為t時刻對第i個控制點的第一虛擬力，γ，c，b為預(yù)設(shè)的控制參數(shù)，為t時刻第i個機械臂關(guān)節(jié)的運動速度，di,t表示t時刻各個障礙物與第i個控制點的最短距離之和。假定t時刻第i個機械臂關(guān)節(jié)的角度qi和角速度已知，可通過公式(2)獲得。

進(jìn)一步地，在t時刻，對于機械臂終端，可生成如下第二虛擬力：

其中，et＝pe-peff,t；

式中，feff,t為t時刻對機械臂終端的第二虛擬力，α，γ，c，g，b為預(yù)設(shè)的控制參數(shù)，pe為機械臂運動的終點位置，peff,t為t時刻機械臂終端的位置，為t時刻第i個機械臂關(guān)節(jié)的運動速度，di,t表示t時刻各個障礙物與第i個控制點的最短距離之和，e為自然常數(shù)。

相應(yīng)地，在t時刻，對第i個機械臂關(guān)節(jié)的實時控制力矩可記為：

式中，πt為t時刻對機械臂的實時控制力矩，為t時刻第i個控制點對應(yīng)的雅克比矩陣，fi,t為t時刻對第i個控制點的第一虛擬力，feff,t為t時刻對機械臂終端的第二虛擬力，k表示機械臂關(guān)節(jié)的個數(shù)，t表示轉(zhuǎn)置操作。

s4，根據(jù)所述實時控制力矩對機械臂進(jìn)行實時路徑控制。

根據(jù)上述實時控制力矩，可以驅(qū)動機械臂各個電機運行，實現(xiàn)對機械臂各個關(guān)節(jié)的控制。一個實施例的機械臂運動控制方法的程序流圖如圖3所示。

通過上述方式，本發(fā)明可以根據(jù)障礙物的實時位置獲取各個障礙物與機械臂關(guān)節(jié)上預(yù)先選定的控制點的最短距離之和，根據(jù)該最短距離之和計算第一虛擬力和第二虛擬力，根據(jù)第一虛擬力和第二虛擬力生成實時控制力矩，并根據(jù)實時控制力矩對機械臂進(jìn)行實時路徑控制，在機械臂運動過程中，能夠根據(jù)機械臂與障礙物之間的距離生成一個作用力，將機械臂推離障礙物，避免機械臂撞上障礙物。

本發(fā)明具有以下優(yōu)點：

(1)采用在線控制方式，能夠在存在動態(tài)障礙物的工作環(huán)境中有效地對機械臂進(jìn)行運動控制。

(2)無需反復(fù)計算機械臂運動的中間點，減少了計算量，提高了機械臂運動控制的效率。

如圖4所示，本發(fā)明還提供一種機械臂運動控制系統(tǒng)，可包括：

第一獲取模塊10，用于獲取機械臂運動的起點與終點之間的障礙物的實時位置；

本實施例中的實時位置可以是動態(tài)的位置，即，各個障礙物的實時位置可隨時間變化。起點與終點之間是指機械臂從起點ps運動到終點pe的運動路徑上可能經(jīng)過的位置。在一個實施例中，可以通過傳感器獲取障礙物的實時位置，傳感器可以以一定的頻率采集各個障礙物的實時位置。在一個具體實施例中，可以獲取障礙物的運動速度，根據(jù)障礙物的運動速度設(shè)置傳感器采集實時位置的頻率。例如，當(dāng)障礙物的運動速度大于預(yù)設(shè)的速度閾值時，可以以較大的頻率采集實時位置；反之，可以以較小的頻率采集實時位置。這里的“較大”和“較小”可以是相對于某個默認(rèn)的采集頻率的值。進(jìn)一步地，還可以設(shè)置多個閾值檔，當(dāng)障礙物的運動速度大于第一速度閾值時，以第一頻率采集實時位置；當(dāng)障礙物的運動速度大于第二速度閾值時，以第二頻率采集實時位置；……；當(dāng)障礙物的運動速度大于第n速度閾值時，以第n(n為正整數(shù))頻率采集實時位置；其中，第一頻率、第二頻率、……、第n頻率的值依次減小。通過設(shè)置不同的速度閾值與對應(yīng)的采集頻率，既可以保證障礙物位置獲取的時效性，又可以在障礙物位置變換較慢的情況下減少數(shù)據(jù)交互量。

第二獲取模塊20，用于根據(jù)所述實時位置獲取各個障礙物與機械臂關(guān)節(jié)上預(yù)先選定的控制點的最短距離，計算所述最短距離之和；

控制點可以是機械臂關(guān)節(jié)上的一個點，這個點可以預(yù)先選定。機械臂的每個關(guān)節(jié)上均可以設(shè)置一個控制點。一個實施例的機械臂關(guān)節(jié)及控制點的示意圖如圖2所示，機械臂包括三個關(guān)節(jié)，三個關(guān)節(jié)上各有一個選定的控制點p1(q)、p2(q)和p3(q)，o為機械臂工作空間中的障礙物集合，peff(q)為機械臂的終端位置，機械臂終端是指在機械臂末端，用于與環(huán)境交互的設(shè)備，具體取決于機器人的應(yīng)用場景。比說，在機械臂末端安裝一個夾子，那這個夾子就是這個機械臂的終端。參數(shù)q＝[q1,q2,…,qn]表示機械臂各個關(guān)節(jié)的角度。對每個控制點，可分別計算各個障礙物到該控制點的最短距離之和。對于第i個控制點，t時刻各個障礙物到該控制點的最短距離之和可記為：

式中，di,t表示t時刻各個障礙物與第i個控制點的最短距離之和，pi表示第i個機械臂關(guān)節(jié)上的控制點，pt'表示t時刻障礙物的實時位置，oi表示第i個機械臂關(guān)節(jié)的工作空間，k表示機械臂關(guān)節(jié)的個數(shù)。

計算模塊30，用于根據(jù)所述最短距離之和與所述機械臂關(guān)節(jié)的運動速度實時地計算對所述控制點的第一虛擬力以及對機械臂終端的第二虛擬力，根據(jù)所述第一虛擬力和第二虛擬力確定對所述機械臂的實時控制力矩；

根據(jù)機械臂的動力學(xué)，t時刻笛卡爾空間中第i個機械臂關(guān)節(jié)的運動速度與第i個機械臂關(guān)節(jié)的角速度之間存在以下關(guān)系：

式中，j為雅克比矩陣，t表示轉(zhuǎn)置操作，為表示pi(q)對時間的導(dǎo)數(shù)，為qi對時間的導(dǎo)數(shù)，i＝1,2,…,k。

同樣地，笛卡爾空間中的力f與機械臂各個關(guān)節(jié)的力矩π＝[π0,π1,…,πk]之間存在以下關(guān)系：

f＝j(luò)π；(4)

π＝j(luò)^tf；(5)

在t時刻，對于第i個控制點，可生成如下第一虛擬力：

式中，fi,t為t時刻對第i個控制點的第一虛擬力，γ，c，b為預(yù)設(shè)的控制參數(shù)，為t時刻第i個機械臂關(guān)節(jié)的運動速度，di,t表示t時刻各個障礙物與第i個控制點的最短距離之和。假定t時刻第i個機械臂關(guān)節(jié)的角度qi和角速度已知，可通過公式(2)獲得。

進(jìn)一步地，在t時刻，對于機械臂終端，可生成如下第二虛擬力：

其中，et＝pe-peff,t；

式中，feff,t為t時刻對機械臂終端的第二虛擬力，α，γ，c，g，b為預(yù)設(shè)的控制參數(shù)，pe為機械臂運動的終點位置，peff,t為t時刻機械臂終端的位置，為t時刻第i個機械臂關(guān)節(jié)的運動速度，di,t表示t時刻各個障礙物與第i個控制點的最短距離之和，e為自然常數(shù)。

相應(yīng)地，在t時刻，對第i個機械臂關(guān)節(jié)的實時控制力矩可記為：

式中，πt為t時刻對機械臂的實時控制力矩，為t時刻第i個控制點對應(yīng)的雅克比矩陣，fi,t為t時刻對第i個控制點的第一虛擬力，feff,t為t時刻對機械臂終端的第二虛擬力，k表示機械臂關(guān)節(jié)的個數(shù)，t表示轉(zhuǎn)置操作。

路徑控制模塊40，用于根據(jù)所述實時控制力矩對機械臂進(jìn)行實時路徑控制。

根據(jù)上述實時控制力矩，可以驅(qū)動機械臂各個電機運行，實現(xiàn)對機械臂各個關(guān)節(jié)的控制。一個實施例的機械臂運動控制方法的程序流圖如圖3所示。

通過上述方式，本發(fā)明可以根據(jù)障礙物的實時位置獲取各個障礙物與機械臂關(guān)節(jié)上預(yù)先選定的控制點的最短距離之和，根據(jù)該最短距離之和計算第一虛擬力和第二虛擬力，根據(jù)第一虛擬力和第二虛擬力生成實時控制力矩，并根據(jù)實時控制力矩對機械臂進(jìn)行實時路徑控制，在機械臂運動過程中，能夠根據(jù)機械臂與障礙物之間的距離生成一個作用力，將機械臂推離障礙物，避免機械臂撞上障礙物。

本發(fā)明具有以下優(yōu)點：

(1)采用在線控制方式，能夠在存在動態(tài)障礙物的工作環(huán)境中有效地對機械臂進(jìn)行運動控制。

(2)無需反復(fù)計算機械臂運動的中間點，減少了計算量，提高了機械臂運動控制的效率。

本發(fā)明的機械臂運動控制系統(tǒng)與本發(fā)明的機械臂運動控制方法一一對應(yīng)，在上述機械臂運動控制方法的實施例闡述的技術(shù)特征及其有益效果均適用于機械臂運動控制系統(tǒng)的實施例中，特此聲明。

在流程圖中表示或在此以其他方式描述的邏輯和/或步驟，例如，可以被認(rèn)為是用于實現(xiàn)邏輯功能的可執(zhí)行指令的定序列表，可以具體實現(xiàn)在任何計算機可讀介質(zhì)中，以供指令執(zhí)行系統(tǒng)、裝置或設(shè)備(如基于計算機的系統(tǒng)、包括處理器的系統(tǒng)或其他可以從指令執(zhí)行系統(tǒng)、裝置或設(shè)備取指令并執(zhí)行指令的系統(tǒng))使用，或結(jié)合這些指令執(zhí)行系統(tǒng)、裝置或設(shè)備而使用。就本說明書而言，“計算機可讀介質(zhì)”可以是任何可以包含、存儲、通信、傳播或傳輸程序以供指令執(zhí)行系統(tǒng)、裝置或設(shè)備或結(jié)合這些指令執(zhí)行系統(tǒng)、裝置或設(shè)備而使用的裝置。

計算機可讀介質(zhì)的更具體的示例(非窮盡性列表)包括以下：具有一個或多個布線的電連接部(電子裝置)，便攜式計算機盤盒(磁裝置)，隨機存取存儲器(ram)，只讀存儲器(rom)，可擦除可編輯只讀存儲器(eprom或閃速存儲器)，光纖裝置，以及便攜式光盤只讀存儲器(cdrom)。另外，計算機可讀介質(zhì)甚至可以是可在其上打印所述程序的紙或其他合適的介質(zhì)，因為可以例如通過對紙或其他介質(zhì)進(jìn)行光學(xué)掃描，接著進(jìn)行編輯、解譯或必要時以其他合適方式進(jìn)行處理來以電子方式獲得所述程序，然后將其存儲在計算機存儲器中。

應(yīng)當(dāng)理解，本發(fā)明的各部分可以用硬件、軟件、固件或它們的組合來實現(xiàn)。在上述實施方式中，多個步驟或方法可以用存儲在存儲器中且由合適的指令執(zhí)行系統(tǒng)執(zhí)行的軟件或固件來實現(xiàn)。例如，如果用硬件來實現(xiàn)，和在另一實施方式中一樣，可用本領(lǐng)域公知的下列技術(shù)中的任一項或他們的組合來實現(xiàn)：具有用于對數(shù)據(jù)信號實現(xiàn)邏輯功能的邏輯門電路的離散邏輯電路，具有合適的組合邏輯門電路的專用集成電路，可編程門陣列(pga)，現(xiàn)場可編程門陣列(fpga)等。

在本說明書的描述中，參考術(shù)語“一個實施例”、“一些實施例”、“示例”、“具體示例”、或“一些示例”等的描述意指結(jié)合該實施例或示例描述的具體特征、結(jié)構(gòu)、材料或者特點包含于本發(fā)明的至少一個實施例或示例中。在本說明書中，對上述術(shù)語的示意性表述不一定指的是相同的實施例或示例。而且，描述的具體特征、結(jié)構(gòu)、材料或者特點可以在任何的一個或多個實施例或示例中以合適的方式結(jié)合。

以上所述實施例的各技術(shù)特征可以進(jìn)行任意的組合，為使描述簡潔，未對上述實施例中的各個技術(shù)特征所有可能的組合都進(jìn)行描述，然而，只要這些技術(shù)特征的組合不存在矛盾，都應(yīng)當(dāng)認(rèn)為是本說明書記載的范圍。

以上所述實施例僅表達(dá)了本發(fā)明的幾種實施方式，其描述較為具體和詳細(xì)，但并不能因此而理解為對發(fā)明專利范圍的限制。應(yīng)當(dāng)指出的是，對于本領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明構(gòu)思的前提下，還可以做出若干變形和改進(jìn)，這些都屬于本發(fā)明的保護(hù)范圍。因此，本發(fā)明專利的保護(hù)范圍應(yīng)以所附權(quán)利要求為準(zhǔn)。