遙操作機(jī)器人運(yùn)動(dòng)軌跡規(guī)劃技術(shù)領(lǐng)域，特別是涉及一種利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃方法及系統(tǒng)。

背景技術(shù)：

遙操作機(jī)器人技術(shù)能夠通過(guò)遠(yuǎn)程操作機(jī)器人動(dòng)態(tài)實(shí)現(xiàn)在特定環(huán)境中的作業(yè)任務(wù)。該技術(shù)常應(yīng)用于較難現(xiàn)場(chǎng)人工操作的環(huán)境，如太空作業(yè)、水下作業(yè)、原子能操作任務(wù)、軍事領(lǐng)域、遠(yuǎn)程應(yīng)用、醫(yī)療等情況。通過(guò)人工遠(yuǎn)程操作，產(chǎn)生指令信息，轉(zhuǎn)變成控制信號(hào)，經(jīng)過(guò)處理后，發(fā)送給現(xiàn)場(chǎng)機(jī)器人，實(shí)現(xiàn)具體操作任務(wù)。由于操作設(shè)備的特定屬性，常常需要經(jīng)過(guò)一定的處理和轉(zhuǎn)換后，變成機(jī)器人適應(yīng)的指令，然后產(chǎn)生具體動(dòng)作。

虛擬現(xiàn)實(shí)手柄是一種手持式三維空間位姿捕獲設(shè)備，能夠快速準(zhǔn)確的捕獲當(dāng)前位姿，適合用于主從式的機(jī)器人遙操作。但是，操作虛擬現(xiàn)實(shí)手柄時(shí)，不可避免的會(huì)產(chǎn)生抖動(dòng)、速度或加速度超過(guò)機(jī)器人允許的最大值、位置超出機(jī)器人工作范圍、難以實(shí)現(xiàn)精確的細(xì)微操作、需要產(chǎn)生一定的位姿偏移等問(wèn)題。因此，需要對(duì)虛擬現(xiàn)實(shí)手柄的信號(hào)進(jìn)行預(yù)處理，經(jīng)過(guò)平滑和插補(bǔ)之后，轉(zhuǎn)換成機(jī)器人適應(yīng)的指令信息。

目前的遙操作信號(hào)處理，大多針對(duì)延時(shí)處理、臨場(chǎng)感的構(gòu)建、通信和信息處理、任務(wù)理解與智能規(guī)劃等方面，比較少有關(guān)于操作再現(xiàn)的實(shí)時(shí)性和平滑性的研究。但是，在一些遙操作任務(wù)中，對(duì)實(shí)時(shí)性和平滑性具有較高的要求，如通過(guò)虛擬現(xiàn)實(shí)手柄操作機(jī)器人接打乒乓球、抓取運(yùn)動(dòng)的物體、多機(jī)器人協(xié)同作業(yè)等。nurbs(non-uniformrationalb-splines，非均勻有理b樣條曲線)曲線可以用于擬合跟蹤離散點(diǎn)軌跡，并可以通過(guò)參數(shù)設(shè)置擬合誤差范圍，但是需要多個(gè)采樣點(diǎn)才能擬合一次，且擬合過(guò)程計(jì)算量較大，不能夠同時(shí)保證實(shí)時(shí)性和平滑性。因此，亟需一種從虛擬現(xiàn)實(shí)手柄到機(jī)器人運(yùn)動(dòng)之間的快速變換方法，使機(jī)器人能夠在允許的運(yùn)動(dòng)學(xué)范圍內(nèi)，盡快跟隨手柄的指令信息，同時(shí)避免抖動(dòng)和軌跡曲折等問(wèn)題。

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

為了解決現(xiàn)有技術(shù)中的上述問(wèn)題，即為了解決從虛擬現(xiàn)實(shí)手柄到機(jī)器人運(yùn)動(dòng)之間的快速變換方法，使機(jī)器人能夠在允許的運(yùn)動(dòng)學(xué)范圍內(nèi)，盡快跟隨手柄的指令信息，同時(shí)避免抖動(dòng)和軌跡曲折的問(wèn)題，本發(fā)明提供一種利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃方法及系統(tǒng)。

為實(shí)現(xiàn)上述目的，本發(fā)明提供了如下方案：

一種利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃方法，所述規(guī)劃方法包括：

在第一個(gè)采樣周期內(nèi)，獲取虛擬現(xiàn)實(shí)手柄的初始手柄位姿和機(jī)器人的初始位姿，并獲取所述虛擬現(xiàn)實(shí)手柄的當(dāng)前手柄位姿；

對(duì)所述初始手柄位姿和當(dāng)前手柄位姿進(jìn)行預(yù)處理得到虛擬現(xiàn)實(shí)手柄的位姿增量，并變換為機(jī)器人的位姿增量；

根據(jù)所述機(jī)器人的初始位姿及位姿增量確定機(jī)器人的目標(biāo)位姿；

將表征所述機(jī)器人的目標(biāo)位姿的離散點(diǎn)連成折線，在任意相鄰的兩條折線之間插入過(guò)渡段軌跡進(jìn)行平滑處理，得到平滑曲線，其中，所述平滑曲線包括位置曲線和姿態(tài)曲線；

對(duì)所述平滑曲線進(jìn)行位姿插補(bǔ)，得到離散的插補(bǔ)點(diǎn)，所述插補(bǔ)點(diǎn)的連線為所述機(jī)器人的運(yùn)行軌跡。

可選的，所述規(guī)劃方法還包括：

根據(jù)所述插補(bǔ)點(diǎn)進(jìn)行逆運(yùn)動(dòng)學(xué)計(jì)算，確定關(guān)節(jié)控制指令，以控制機(jī)器人按照所述運(yùn)行軌跡運(yùn)動(dòng)。

可選的，根據(jù)初始手柄位姿和當(dāng)前手柄位姿進(jìn)行預(yù)處理得到手柄的位姿增量，并轉(zhuǎn)換為機(jī)器人的位姿增量，具體包括：

通過(guò)低通濾波器對(duì)所述當(dāng)前手柄位姿vh進(jìn)行低通濾波，獲得濾波后的手柄位姿v′h；

根據(jù)以下公式，計(jì)算手柄虛擬現(xiàn)實(shí)手柄的位姿增量

其中，初始手柄位姿為vhp0是虛擬現(xiàn)實(shí)手柄的初始位置向量，vho0是虛擬現(xiàn)實(shí)手柄的初始姿態(tài)向量；δvhp為虛擬現(xiàn)實(shí)手柄的位置增量，δvho為虛擬現(xiàn)實(shí)手柄的姿態(tài)增量，

根據(jù)以下公式，對(duì)所述虛擬現(xiàn)實(shí)手柄的位姿增量按縮放比例進(jìn)行縮放，確定所述機(jī)器人的位姿增量

其中，δvrp為機(jī)器人的位置增量，δvro為機(jī)器人的姿態(tài)增量，rp為位置的縮放比例，ro為姿態(tài)的縮放比例，rp和ro是對(duì)角矩陣。

可選的，所述位置向量由三維坐標(biāo)數(shù)據(jù)組成，所述姿態(tài)向量由章動(dòng)角、旋進(jìn)角和自轉(zhuǎn)角三個(gè)角度數(shù)據(jù)組成；

所述低通濾波器對(duì)所述位置向量的三維坐標(biāo)數(shù)據(jù)的濾波頻率相同，對(duì)姿態(tài)向量中的三個(gè)角度數(shù)據(jù)的濾波頻率相同。

可選的，所述根據(jù)所述機(jī)器人的初始位姿及位姿增量確定機(jī)器人的目標(biāo)位姿，具體包括：

根據(jù)以下公式，確定所述機(jī)器人的目標(biāo)位姿

其中，vrp表示機(jī)器人的目標(biāo)位置，vro表示機(jī)器人的目標(biāo)姿態(tài)；表示機(jī)器人的初始位姿，vrp0表示機(jī)器人的初始位置，vro0表示機(jī)器人的初始姿態(tài)；表示機(jī)器人的位姿增量，δvrp為機(jī)器人的位置增量，δvro為機(jī)器人的姿態(tài)增量；表示偏移量，voffsetp為位置偏移量，voffseto為姿態(tài)偏移量。

可選的，所述在任意相鄰的兩條折線之間插入過(guò)渡段軌跡進(jìn)行平滑處理，具體包括：

對(duì)于所述位置曲線，采用三次貝塞爾曲線過(guò)渡，設(shè)置過(guò)渡點(diǎn)與終點(diǎn)的距離dl為可調(diào)節(jié)參數(shù)，用于調(diào)節(jié)位置的平滑程度；

對(duì)于所述姿態(tài)曲線，采用球面貝塞爾曲線過(guò)渡，設(shè)置過(guò)渡點(diǎn)與終點(diǎn)的旋轉(zhuǎn)角dθ為過(guò)渡參數(shù)，用于調(diào)節(jié)姿態(tài)的平滑程度；

其中，在所述三次貝塞爾曲線和球面貝塞爾曲線中，均選取兩個(gè)過(guò)渡點(diǎn)和相鄰指令的交點(diǎn)為控制頂點(diǎn)，相鄰指令交點(diǎn)選為重合頂點(diǎn)，構(gòu)造三次曲線。

可選的，對(duì)所述平滑曲線進(jìn)行位姿插補(bǔ)具體包括：

根據(jù)位置曲線的始末點(diǎn)位置、速度和中間點(diǎn)運(yùn)動(dòng)約束條件，計(jì)算位置曲線的插補(bǔ)時(shí)間tp；以及根據(jù)姿態(tài)曲線的始末點(diǎn)位置、角速度和中間點(diǎn)運(yùn)動(dòng)約束條件，計(jì)算姿態(tài)曲線的插補(bǔ)時(shí)間to；

比較tp和to的大小，確定插補(bǔ)時(shí)間；

按插補(bǔ)時(shí)間插補(bǔ)位置曲線和姿態(tài)曲線，得到離散的插補(bǔ)點(diǎn)。

可選的，所述比較tp和to的大小，確定插補(bǔ)時(shí)間，具體包括：

如果tp<to，則時(shí)間to為插補(bǔ)時(shí)間；

如果tp≥to，則時(shí)間tp為插補(bǔ)時(shí)間。

可選的，所述插補(bǔ)時(shí)間小于采樣周期內(nèi)的采樣時(shí)間。

根據(jù)本發(fā)明提供的具體實(shí)施例，本發(fā)明公開(kāi)了以下技術(shù)效果：

本發(fā)明利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃方法通過(guò)獲取虛擬現(xiàn)實(shí)手柄的當(dāng)前手柄位姿，可實(shí)時(shí)跟隨操作指令；通過(guò)計(jì)算機(jī)器人的位姿增量、目標(biāo)位姿，確定離散點(diǎn)；進(jìn)一步在離散點(diǎn)形成的折線中插入過(guò)渡段軌跡進(jìn)行平滑處理、插補(bǔ)，從而可快速準(zhǔn)確的確定機(jī)器人的運(yùn)行軌跡。

為實(shí)現(xiàn)上述目的，本發(fā)明還提供了如下方案：

一種利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃系統(tǒng)，所述規(guī)劃系統(tǒng)包括：

獲取單元，用于在一個(gè)采樣周期內(nèi)，獲取虛擬現(xiàn)實(shí)手柄的初始手柄位姿和機(jī)器人的初始位姿，并獲取所述虛擬現(xiàn)實(shí)手柄的當(dāng)前手柄位姿；

預(yù)處理單元，用于根據(jù)手柄初始位姿和當(dāng)前位姿進(jìn)行預(yù)處理得到虛擬現(xiàn)實(shí)手柄的位姿增量；并變換為機(jī)器人的位姿增量；

確定單元，用于根據(jù)所述機(jī)器人的初始位姿及位姿增量確定機(jī)器人的目標(biāo)位姿；

平滑處理單元，用于將表征所述機(jī)器人的目標(biāo)位姿的離散點(diǎn)連成折線，在任意相鄰的兩條折線之間插入過(guò)渡段軌跡進(jìn)行平滑處理，得到平滑曲線，其中，所述平滑曲線包括位置曲線和姿態(tài)曲線；

插補(bǔ)單元，用于對(duì)所述平滑曲線進(jìn)行位姿插補(bǔ)，得到離散的插補(bǔ)點(diǎn)，所述插補(bǔ)點(diǎn)的連線為所述機(jī)器人的運(yùn)行軌跡。

可選的，所述規(guī)劃系統(tǒng)還包括：

運(yùn)算單元，用于根據(jù)所述插補(bǔ)點(diǎn)進(jìn)行逆運(yùn)動(dòng)學(xué)計(jì)算，確定關(guān)節(jié)控制指令，以控制機(jī)器人按照所述運(yùn)行軌跡運(yùn)動(dòng)。

根據(jù)本發(fā)明提供的具體實(shí)施例，本發(fā)明公開(kāi)了以下技術(shù)效果：

本發(fā)明利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃系統(tǒng)通過(guò)獲取單元，獲取虛擬現(xiàn)實(shí)手柄的當(dāng)前手柄位姿，可實(shí)時(shí)跟隨操作指令；通過(guò)預(yù)處理單元及確定單元，計(jì)算機(jī)器人的位姿增量、目標(biāo)位姿，確定離散點(diǎn)；進(jìn)一步，通過(guò)平滑處理單元及插補(bǔ)單元在離散點(diǎn)形成的折線中插入過(guò)渡段軌跡進(jìn)行平滑處理、插補(bǔ)，從而可快速準(zhǔn)確的確定機(jī)器人的運(yùn)行軌跡。

附圖說(shuō)明

圖1為本發(fā)明利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃方法的流程圖；

圖2為本發(fā)明利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃方法中機(jī)器人的位姿增量的流程圖；

圖3為位置過(guò)渡示意圖；

圖4為姿態(tài)過(guò)渡示意圖；

圖5為本發(fā)明利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃方法中位姿插補(bǔ)的流程圖；

圖6為s型曲線插補(bǔ)時(shí)間長(zhǎng)度的示意圖；

圖7為虛擬現(xiàn)實(shí)手柄原信號(hào)與處理后的機(jī)器人運(yùn)行軌跡的示意圖；

圖8為本發(fā)明利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃系統(tǒng)的模塊結(jié)構(gòu)示意圖。

符號(hào)說(shuō)明：

獲取單元—1，預(yù)處理單元—2，確定單元—3，平滑處理單元—4，插補(bǔ)單元—5，運(yùn)算單元—6。

具體實(shí)施方式

下面參照附圖來(lái)描述本發(fā)明的優(yōu)選實(shí)施方式。本領(lǐng)域技術(shù)人員應(yīng)當(dāng)理解的是，這些實(shí)施方式僅僅用于解釋本發(fā)明的技術(shù)原理，并非旨在限制本發(fā)明的保護(hù)范圍。

本發(fā)明提供一種利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃方法，通過(guò)獲取虛擬現(xiàn)實(shí)手柄的當(dāng)前手柄位姿，可實(shí)時(shí)跟隨操作指令；通過(guò)計(jì)算機(jī)器人的位姿增量、目標(biāo)位姿，確定離散點(diǎn)；進(jìn)一步在離散點(diǎn)形成的折線中插入過(guò)渡段軌跡進(jìn)行平滑處理、插補(bǔ)，從而可快速準(zhǔn)確的確定機(jī)器人的運(yùn)行軌跡。

為使本發(fā)明的上述目的、特征和優(yōu)點(diǎn)能夠更加明顯易懂，下面結(jié)合附圖和具體實(shí)施方式對(duì)本發(fā)明作進(jìn)一步詳細(xì)的說(shuō)明。

如圖1所示，本發(fā)明利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃方法：

步驟100：在第一個(gè)采樣周期內(nèi)，獲取虛擬現(xiàn)實(shí)手柄的初始手柄位姿和機(jī)器人的初始位姿，并獲取所述虛擬現(xiàn)實(shí)手柄的當(dāng)前手柄位姿；

步驟200：對(duì)所述初始手柄位姿和當(dāng)前手柄位姿進(jìn)行預(yù)處理得到虛擬現(xiàn)實(shí)手柄的位姿增量，并變換為機(jī)器人的位姿增量；

步驟300：根據(jù)所述機(jī)器人的初始位姿及位姿增量確定機(jī)器人的目標(biāo)位姿；

步驟400：將表征所述機(jī)器人的目標(biāo)位姿的離散點(diǎn)連成折線，在任意相鄰的兩條折線之間插入過(guò)渡段軌跡進(jìn)行平滑處理，得到平滑曲線，其中，所述平滑曲線包括位置曲線和姿態(tài)曲線；

步驟500：對(duì)所述平滑曲線進(jìn)行位姿插補(bǔ)，得到離散的插補(bǔ)點(diǎn)，所述插補(bǔ)點(diǎn)的連線為所述機(jī)器人的運(yùn)行軌跡；

步驟600：根據(jù)所述插補(bǔ)點(diǎn)進(jìn)行逆運(yùn)動(dòng)學(xué)計(jì)算，確定關(guān)節(jié)控制指令，以控制機(jī)器人按照所述運(yùn)行軌跡運(yùn)動(dòng)。

在步驟100中，在每個(gè)采樣周期，虛擬現(xiàn)實(shí)手柄的服務(wù)器實(shí)時(shí)獲得手柄位姿vh，并通過(guò)tcp/ip通信傳給機(jī)器人的控制器。采樣周期不超過(guò)150ms，可根據(jù)操作的精細(xì)度調(diào)整。

記錄并保存虛擬現(xiàn)實(shí)手柄的第一個(gè)采樣周期的初始手柄位姿vh0和機(jī)器人的初始位姿vr0，獲取所述虛擬現(xiàn)實(shí)手柄的當(dāng)前手柄位姿vh。

其中，vh＝{x,y,z,a,b,c},{x,y,z}表示三維坐標(biāo)數(shù)據(jù)組成的位置向量，{a,b,c}表示由章動(dòng)角、旋進(jìn)角和自轉(zhuǎn)角組成的歐拉角向量(即姿態(tài)向量)。

所述初始手柄位姿為vhp0是虛擬現(xiàn)實(shí)手柄的初始位置向量，vho0是虛擬現(xiàn)實(shí)手柄的初始姿態(tài)向量；所述機(jī)器人的初始位姿為vrp0表示機(jī)器人的初始位置，vro0表示機(jī)器人的初始姿態(tài)。

如圖2所示，在步驟200中，所述對(duì)所述初始手柄位姿和當(dāng)前手柄位姿進(jìn)行預(yù)處理得到手柄的位姿增量，并變換為機(jī)器人的位姿增量，具體包括：

步驟201：通過(guò)低通濾波器對(duì)所述當(dāng)前手柄位姿vh進(jìn)行低通濾波，獲得濾波后的手柄位姿v′h；

步驟202：根據(jù)公式(1)，計(jì)算手柄虛擬現(xiàn)實(shí)手柄的位姿增量

δvhp為虛擬現(xiàn)實(shí)手柄的位置增量，δvho為虛擬現(xiàn)實(shí)手柄的姿態(tài)增量。

根據(jù)公式(2)，對(duì)手柄虛擬現(xiàn)實(shí)手柄的位姿增量按縮放比例進(jìn)行縮放，確定所述機(jī)器人的位姿增量

其中，δvrp為機(jī)器人的位置增量，δvro為機(jī)器人的姿態(tài)增量，rp為位置的縮放比例，ro為姿態(tài)的縮放比例，rp和ro是對(duì)角矩陣。

其中，所述低通濾波器(lowpassfilter)對(duì)所述位置向量的三維坐標(biāo)數(shù)據(jù)的濾波頻率相同fp，對(duì)姿態(tài)向量中的三個(gè)角度數(shù)據(jù)的濾波頻率相同fo。一般地，濾波頻率約為采樣頻率的一半。

如公式(3)所示，所述低通濾波的處理結(jié)果為：

v′h＝lowpassfilter(vh,fp,fo)(3)

在步驟400中，所述根據(jù)所述機(jī)器人的初始位姿及位姿增量確定機(jī)器人的目標(biāo)位姿，具體包括：

根據(jù)公式(4)，確定所述機(jī)器人的目標(biāo)位姿

其中，vrp表示機(jī)器人的目標(biāo)位置，vro表示機(jī)器人的目標(biāo)姿態(tài)；表示機(jī)器人的初始位姿，vrp0表示機(jī)器人的初始位置，vro0表示機(jī)器人的初始姿態(tài)；表示機(jī)器人的位姿增量，δvrp為機(jī)器人的位置增量，δvro為機(jī)器人的姿態(tài)增量；表示偏移量，voffsetp為位置偏移量，voffseto為姿態(tài)偏移量。

如圖3和圖4所示，在步驟400中，所述在任意相鄰的兩條折線之間插入過(guò)渡段軌跡進(jìn)行平滑處理，具體包括：

對(duì)于所述位置曲線，采用三次貝塞爾曲線過(guò)渡，設(shè)置過(guò)渡點(diǎn)與終點(diǎn)的距離dl為可調(diào)節(jié)參數(shù)，用于調(diào)節(jié)位置的平滑程度；

對(duì)于所述姿態(tài)曲線，采用球面貝塞爾曲線過(guò)渡，設(shè)置過(guò)渡點(diǎn)與終點(diǎn)的旋轉(zhuǎn)角dθ為過(guò)渡參數(shù)，用于調(diào)節(jié)姿態(tài)的平滑程度。

過(guò)渡方法通過(guò)參數(shù)調(diào)節(jié)與原軌跡的貼合程度，調(diào)節(jié)過(guò)渡段與原軌跡的誤差。

在所述三次貝塞爾曲線和球面貝塞爾曲線中，均選取兩個(gè)過(guò)渡點(diǎn)和相鄰指令的交點(diǎn)為控制頂點(diǎn)，相鄰指令交點(diǎn)選為重合頂點(diǎn)，構(gòu)造三次曲線。

其中，根據(jù)如下公式(5)和公式(6)確定所述三次貝塞爾曲線：

其中，pi為控制頂點(diǎn)。

根據(jù)公式(7)確定所述球面貝塞爾曲線：

其中，q0,i為控制頂點(diǎn)姿態(tài)的四元數(shù)表示，i＝0，1，...n，n＝3。

在步驟500中，所述位姿插補(bǔ)方法為按位置和姿態(tài)分別插補(bǔ)，根據(jù)始末點(diǎn)的位置和速度約束，以及中間過(guò)程點(diǎn)的速度、加速度、加加速度約束，分別規(guī)劃。

如圖5所示，對(duì)所述平滑曲線進(jìn)行位姿插補(bǔ)具體包括：

根據(jù)位置曲線的始末點(diǎn)位置、速度和中間點(diǎn)運(yùn)動(dòng)約束條件，計(jì)算位置曲線的插補(bǔ)時(shí)間tp；以及根據(jù)姿態(tài)曲線的始末點(diǎn)位置、角速度和中間點(diǎn)運(yùn)動(dòng)約束條件，計(jì)算姿態(tài)曲線的插補(bǔ)時(shí)間to；比較tp和to的大小，確定插補(bǔ)時(shí)間；按插補(bǔ)時(shí)間插補(bǔ)位置曲線和姿態(tài)曲線，得到離散的插補(bǔ)點(diǎn)。

在本實(shí)施例中，采用s型加減速算法分別計(jì)算各自的插補(bǔ)時(shí)間tp和to。插補(bǔ)時(shí)間為從起始點(diǎn)到目標(biāo)點(diǎn)的路徑所需要的時(shí)間，對(duì)應(yīng)圖4中的時(shí)長(zhǎng)t。經(jīng)過(guò)時(shí)間t，位移和速度同時(shí)到達(dá)目標(biāo)點(diǎn)的位移和速度。

其中，所述比較tp和to的大小，確定插補(bǔ)時(shí)間，具體包括：如果tp<to，則時(shí)間to為插補(bǔ)時(shí)間；如果tp≥to，則時(shí)間tp為插補(bǔ)時(shí)間。

此外，當(dāng)插補(bǔ)時(shí)間小于時(shí)間閾值，無(wú)法同時(shí)滿足運(yùn)動(dòng)學(xué)約束(速度中間點(diǎn)運(yùn)動(dòng)約束條件或角速度中間點(diǎn)運(yùn)動(dòng)約束條件)和時(shí)間約束(插補(bǔ)時(shí)間大于或等于時(shí)間閾值)時(shí)，在保證加速度和加加速度約束的前提下，允許速度小于終點(diǎn)速度。

下面以一具體實(shí)施例詳細(xì)介紹本發(fā)明利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃方法：

如圖2所示，經(jīng)過(guò)對(duì)虛擬現(xiàn)實(shí)手柄位姿采樣后，獲取六維向量vh＝{x,y,z,a,b,c}；通過(guò)低通濾波器分別對(duì)六維向量的每個(gè)參數(shù)進(jìn)行低通濾波，并對(duì)濾波后的軌跡做縮放和偏移處理，確定機(jī)器人的目標(biāo)位姿。

將表征所述機(jī)器人的目標(biāo)位姿的離散點(diǎn)連成折線，在任意相鄰的兩條折線之間插入過(guò)渡段軌跡進(jìn)行平滑處理，得到平滑曲線。如圖3所示，在相鄰直線段ab和bc之間插入了一段過(guò)渡軌跡p1p2。其中，p1b的長(zhǎng)度是可調(diào)節(jié)參數(shù)。由點(diǎn)p1、點(diǎn)b、點(diǎn)b、點(diǎn)p2構(gòu)成貝塞爾曲線的四個(gè)控制頂點(diǎn)。如圖4所示，在相鄰旋轉(zhuǎn)軌跡ab和bc之間插入了一段姿態(tài)過(guò)渡軌跡p1p2。其中，p1b的角度值是可調(diào)節(jié)參數(shù)。由點(diǎn)p1、點(diǎn)b、點(diǎn)b、點(diǎn)p2構(gòu)成球面貝塞爾曲線的四個(gè)控制頂點(diǎn)。

對(duì)所述平滑曲線進(jìn)行位姿插補(bǔ)，得到離散的插補(bǔ)點(diǎn)。本發(fā)明的位姿插補(bǔ)過(guò)程如圖5所示。首先位置和姿態(tài)分別進(jìn)行計(jì)算，得到位置插補(bǔ)時(shí)間tp和姿態(tài)插補(bǔ)時(shí)間to。插補(bǔ)時(shí)間對(duì)應(yīng)圖6中的時(shí)間長(zhǎng)度t，位移和速度同時(shí)到達(dá)目標(biāo)點(diǎn)的位移和速度。然后比較tp和to的大小。如果tp<to，則按照時(shí)間to進(jìn)行插補(bǔ)；如果tp≥to，則按照時(shí)間tp進(jìn)行插補(bǔ)。

經(jīng)過(guò)插補(bǔ)后得到的機(jī)器人的運(yùn)動(dòng)軌跡如圖7所示。畫(huà)圈的點(diǎn)表示采樣的離散點(diǎn)，相鄰點(diǎn)之間對(duì)應(yīng)采樣時(shí)間。點(diǎn)劃線上的點(diǎn)表示插補(bǔ)點(diǎn)，相鄰點(diǎn)之間對(duì)應(yīng)插補(bǔ)時(shí)間，插補(bǔ)時(shí)間小于采樣周期內(nèi)的采樣時(shí)間。

根據(jù)所述插補(bǔ)點(diǎn)進(jìn)行逆運(yùn)動(dòng)學(xué)計(jì)算，確定關(guān)節(jié)控制指令，以控制機(jī)器人按照所述運(yùn)行軌跡運(yùn)動(dòng)。

所述位姿離散點(diǎn)的采集、濾波、平滑過(guò)渡和位姿插補(bǔ)等均運(yùn)行在機(jī)器人控制器上，實(shí)時(shí)計(jì)算完成，控制機(jī)器人按照給定運(yùn)動(dòng)學(xué)約束盡快跟隨虛擬現(xiàn)實(shí)手柄的操作。

本發(fā)明利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃方法利用虛擬現(xiàn)實(shí)手柄獲得的離散數(shù)據(jù)點(diǎn)生成機(jī)器人實(shí)時(shí)軌跡的規(guī)劃方法，能夠?qū)⑻摂M現(xiàn)實(shí)手柄的六維向量變換到滿足機(jī)器人運(yùn)動(dòng)學(xué)要求的連續(xù)插補(bǔ)點(diǎn)，實(shí)現(xiàn)軌跡平滑、速度超限、抖動(dòng)、位置超限、精確操作、位姿偏移等情況的處理。

本發(fā)明利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃方法兼具以下幾方面特性：可針對(duì)不同應(yīng)用目的調(diào)節(jié)虛擬現(xiàn)實(shí)手柄數(shù)據(jù)的采樣周期，滿足不同的精細(xì)程度；針對(duì)虛擬現(xiàn)實(shí)手柄操作設(shè)置了特定頻率的濾波范圍，將噪聲信號(hào)濾除了，保留了具有人工操作意圖的低頻信號(hào)；可以通過(guò)設(shè)定位置和姿態(tài)的過(guò)渡參數(shù)，調(diào)節(jié)與原軌跡的偏離距離和光滑程度，適合不同的應(yīng)用需求；位姿插補(bǔ)方法能夠滿足始末點(diǎn)和中間點(diǎn)的運(yùn)動(dòng)學(xué)約束，當(dāng)不允許反向運(yùn)動(dòng)時(shí)，可能不存在位置和姿態(tài)的相同運(yùn)動(dòng)時(shí)間解，能夠自主求取近似解，舍棄終點(diǎn)速度約束，保證位置和姿態(tài)能夠同時(shí)跟隨；在得到下一個(gè)采樣點(diǎn)時(shí)，規(guī)劃方法能立刻插補(bǔ)出運(yùn)動(dòng)軌跡，具有較好的實(shí)時(shí)性。

此外，本發(fā)明還提供一種利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃系統(tǒng)。如圖8所示，本發(fā)明利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃系統(tǒng)包括獲取單元1、預(yù)處理單元2、確定單元3、平滑處理單元4、插補(bǔ)單元5及運(yùn)算單元6。

其中，所述獲取單元1在第一個(gè)采樣周期內(nèi)，獲取虛擬現(xiàn)實(shí)手柄的初始手柄位姿和機(jī)器人的初始位姿，并獲取所述虛擬現(xiàn)實(shí)手柄的當(dāng)前手柄位姿；所述預(yù)處理單元2對(duì)所述初始手柄位姿和當(dāng)前手柄位姿進(jìn)行預(yù)處理得到虛擬現(xiàn)實(shí)手柄的位姿增量，并轉(zhuǎn)換為機(jī)器人的位姿增量；所述確定單元3根據(jù)所述機(jī)器人的初始位姿及位姿增量確定機(jī)器人的目標(biāo)位姿；所述平滑處理單元4將表征所述機(jī)器人的目標(biāo)位姿的離散點(diǎn)連成折線，在任意相鄰的兩條折線之間插入過(guò)渡段軌跡進(jìn)行平滑處理，得到平滑曲線，其中，所述平滑曲線包括位置曲線和姿態(tài)曲線；所述插補(bǔ)單元5對(duì)所述平滑曲線進(jìn)行位姿插補(bǔ)，得到離散的插補(bǔ)點(diǎn)，所述插補(bǔ)點(diǎn)的連線為所述機(jī)器人的運(yùn)行軌跡；所述運(yùn)算單元6根據(jù)所述插補(bǔ)點(diǎn)進(jìn)行逆運(yùn)動(dòng)學(xué)計(jì)算，確定關(guān)節(jié)控制指令，以控制機(jī)器人按照所述運(yùn)行軌跡運(yùn)動(dòng)。

相對(duì)于現(xiàn)有技術(shù)，本發(fā)明利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃系統(tǒng)與上述利用虛擬現(xiàn)實(shí)手柄確定機(jī)器人軌跡的規(guī)劃方法的有益效果相同，在此不再贅述。

至此，已經(jīng)結(jié)合附圖所示的優(yōu)選實(shí)施方式描述了本發(fā)明的技術(shù)方案，但是，本領(lǐng)域技術(shù)人員容易理解的是，本發(fā)明的保護(hù)范圍顯然不局限于這些具體實(shí)施方式。在不偏離本發(fā)明的原理的前提下，本領(lǐng)域技術(shù)人員可以對(duì)相關(guān)技術(shù)特征作出等同的更改或替換，這些更改或替換之后的技術(shù)方案都將落入本發(fā)明的保護(hù)范圍之內(nèi)。