本發(fā)明涉及機(jī)器人技術(shù)的雙臂協(xié)作控制領(lǐng)域，尤其涉及一種基于雙臂機(jī)器人協(xié)同操作的柔順控制方法及系統(tǒng)。

背景技術(shù)：

單臂機(jī)器人的操作任務(wù)簡(jiǎn)單、靈巧性不足，與傳統(tǒng)單個(gè)機(jī)械臂相比，雙臂機(jī)器人更能適應(yīng)工廠中為人類設(shè)計(jì)的工作空間，同時(shí)雙臂可以完成裝配、搬運(yùn)等復(fù)雜的操作任務(wù)。雙臂機(jī)器人在具有較高的靈活性和任務(wù)適應(yīng)性的同時(shí)，也帶來(lái)了更加復(fù)雜的建模、規(guī)劃以及控制等問(wèn)題。而雙臂機(jī)器人的研究重點(diǎn)則為雙臂協(xié)調(diào)完成復(fù)雜的載荷搬運(yùn)以及裝配等任務(wù)，由于雙臂在進(jìn)行協(xié)調(diào)操作時(shí)會(huì)形成閉鏈約束，同時(shí)雙臂機(jī)器人系統(tǒng)在建模與控制上存在一定誤差，如果不對(duì)末端操作力進(jìn)行控制，很有可能會(huì)對(duì)負(fù)載甚至機(jī)械臂自身造成意想不到的破壞。

為了增強(qiáng)機(jī)械臂與環(huán)境的交互與感知，目前大多數(shù)機(jī)械臂均在關(guān)節(jié)或是末端安裝有力傳感器，通過(guò)阻抗控制方法或是力位混合控制方法即可實(shí)現(xiàn)機(jī)械臂的力柔順操作。但是針對(duì)雙臂協(xié)同搬運(yùn)載荷等典型任務(wù)，尤其是雙臂機(jī)器人與目標(biāo)負(fù)載形成閉鏈約束后的雙臂協(xié)同運(yùn)動(dòng)規(guī)劃與柔順操作問(wèn)題，尚未提出安全可靠的控制方法。

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

為了解決上述技術(shù)問(wèn)題，本發(fā)明的目的是提供一種基于雙臂機(jī)器人協(xié)同操作的柔順控制方法及系統(tǒng)。

本發(fā)明所采用的技術(shù)方案是：

本發(fā)明提供一種基于雙臂機(jī)器人協(xié)同操作的柔順控制方法，包括以下步驟：

雙臂協(xié)同控制模塊根據(jù)目標(biāo)負(fù)載的期望運(yùn)動(dòng)軌跡和期望力建立動(dòng)力學(xué)模型，通過(guò)滿足雙臂閉鏈約束方程的協(xié)調(diào)位置分解和公共力分解得到各個(gè)機(jī)械臂的末端期望位姿和末端期望力；

單臂控制模塊完成對(duì)所述期望任務(wù)指令的解析與執(zhí)行，得到機(jī)械臂各個(gè)關(guān)節(jié)的期望角度，通過(guò)基于位置的阻抗控制實(shí)現(xiàn)機(jī)械臂的力柔順操作；

驅(qū)動(dòng)模塊根據(jù)規(guī)劃的期望關(guān)節(jié)角度，驅(qū)動(dòng)所述機(jī)械臂完成任務(wù)軌跡。

作為該技術(shù)方案的改進(jìn)，所述步驟還包括系統(tǒng)對(duì)雙臂之間公共力的分解。

作為該技術(shù)方案的改進(jìn)，所述對(duì)公共力的分解的方法包括采用主從分解模式。

作為該技術(shù)方案的改進(jìn)，所述主從分解模式包括通過(guò)給定其中一機(jī)械臂的末端期望操作力/力矩F_ea＝[f_ex,f_ey,f_ez,τ_ex,τ_ey,τ_ez]^T，進(jìn)而可求解得到另一機(jī)械臂的末端期望力/力矩為其中，F(xiàn)_L表示負(fù)載所受環(huán)境的廣義力；表示負(fù)載所受地球的廣義重力，F(xiàn)_IL表示負(fù)載的廣義慣性力，Γ_a、Γ_b分別代表兩個(gè)機(jī)械臂的抓取矩陣。

作為該技術(shù)方案的改進(jìn)，所述對(duì)公共力的分解的方法包括采用共享分解模式。

進(jìn)一步地，所述共享分解模式包括將兩個(gè)機(jī)械臂同等看待，采用偽逆法求解負(fù)載受力平衡方程，可得到：

其中，Γ_a、Γ_b分別代表兩個(gè)機(jī)械臂的抓取矩陣，F(xiàn)_ea、F_eb分別為兩機(jī)械臂的末端期望操作力/力矩，F(xiàn)_L表示負(fù)載所受環(huán)境的廣義力；表示負(fù)載所受地球的廣義重力；F_IL表示負(fù)載的廣義慣性力。

進(jìn)一步地，所述單臂控制模塊采用基于位置的阻抗控制，通過(guò)機(jī)械臂的導(dǎo)納特性將末端的力、力矩誤差等效為輸入的位置與姿態(tài)增量，實(shí)現(xiàn)機(jī)械臂的力柔順操作。

另一方面，本發(fā)明還提供一種基于雙臂機(jī)器人協(xié)同操作的柔順控制系統(tǒng)，包括：雙臂協(xié)同控制模塊、驅(qū)動(dòng)模塊和單臂控制模塊；

所述雙臂協(xié)同控制模塊用于執(zhí)行步驟根據(jù)目標(biāo)負(fù)載的期望運(yùn)動(dòng)軌跡和期望力建立動(dòng)力學(xué)模型，通過(guò)滿足雙臂閉鏈約束方程的協(xié)調(diào)位置分解和公共力分解得到各個(gè)機(jī)械臂的末端期望位姿和末端期望力；

所述單臂控制模塊用于執(zhí)行步驟完成對(duì)所述期望任務(wù)指令的解析與執(zhí)行，得到機(jī)械臂各個(gè)關(guān)節(jié)的期望角度，通過(guò)基于位置的阻抗控制實(shí)現(xiàn)機(jī)械臂的力柔順操作；

所述驅(qū)動(dòng)模塊用于執(zhí)行步驟根據(jù)規(guī)劃的期望關(guān)節(jié)角度，驅(qū)動(dòng)所述機(jī)械臂完成任務(wù)軌跡。

本發(fā)明的有益效果是：本發(fā)明提供的基于雙臂機(jī)器人協(xié)同操作的柔順控制方法及系統(tǒng)，針對(duì)雙臂協(xié)同搬運(yùn)載荷等典型任務(wù)，提出了目標(biāo)負(fù)載操作的閉鏈雙臂協(xié)調(diào)柔順控制方法，可以確保雙臂機(jī)器人在對(duì)負(fù)載進(jìn)行閉鏈約束下的協(xié)同操作的同時(shí)，將機(jī)械臂的末端操作力控制在期望的范圍。為了求解負(fù)載力平衡方程，采用主從式和共享式策略進(jìn)行負(fù)載的公共力分解，得到兩個(gè)機(jī)械臂的期望操作力，進(jìn)而提出了雙臂協(xié)同操作的主從式力柔順控制方法和共享式力柔順控制方法。單臂控制器采用基于位置的阻抗控制，僅需要進(jìn)行機(jī)械臂末端操作力的檢測(cè)以及關(guān)節(jié)空間的位置控制即可，降低了對(duì)硬件平臺(tái)的要求。

本發(fā)明為雙臂協(xié)同操作提供了一種安全可靠的控制方案，確保雙臂機(jī)器人在對(duì)負(fù)載進(jìn)行閉鏈約束下的協(xié)同操作時(shí)，將機(jī)械臂的末端操作力控制在期望的范圍。本方案可以廣泛適用于成熟的工業(yè)機(jī)械臂產(chǎn)品以及自主研發(fā)的機(jī)械臂實(shí)驗(yàn)平臺(tái)，用以完成目標(biāo)負(fù)載搬運(yùn)以及裝配等雙臂協(xié)作任務(wù)。

附圖說(shuō)明

下面結(jié)合附圖對(duì)本發(fā)明的具體實(shí)施方式作進(jìn)一步說(shuō)明：

圖1是本發(fā)明第一實(shí)施例的雙臂機(jī)器人操作目標(biāo)載荷所形成的閉鏈約束示意圖；

圖2是本發(fā)明第二實(shí)施例的雙臂操作時(shí)的負(fù)載受力分析示意圖；

圖3是本發(fā)明第三實(shí)施例的基于位置的阻抗控制示意圖；

圖4是本發(fā)明第四實(shí)施例的目標(biāo)負(fù)載操作的閉鏈雙臂協(xié)調(diào)柔順控制示意圖；

圖5是本發(fā)明第五實(shí)施例的控制方法流程示意圖。

具體實(shí)施方式

需要說(shuō)明的是，在不沖突的情況下，本申請(qǐng)中的實(shí)施例及實(shí)施例中的特征可以相互組合。

本發(fā)明提供一種基于雙臂機(jī)器人協(xié)同操作的柔順控制方法，包括以下步驟：

雙臂協(xié)同控制模塊根據(jù)目標(biāo)負(fù)載的期望運(yùn)動(dòng)軌跡和期望力建立動(dòng)力學(xué)模型，通過(guò)滿足雙臂閉鏈約束方程的協(xié)調(diào)位置分解和公共力分解得到各個(gè)機(jī)械臂的末端期望位姿和末端期望力；

單臂控制模塊完成對(duì)所述期望任務(wù)指令的解析與執(zhí)行，得到機(jī)械臂各個(gè)關(guān)節(jié)的期望角度，通過(guò)基于位置的阻抗控制實(shí)現(xiàn)機(jī)械臂的力柔順操作；

驅(qū)動(dòng)模塊根據(jù)規(guī)劃的期望關(guān)節(jié)角度，驅(qū)動(dòng)所述機(jī)械臂完成任務(wù)軌跡。

作為該技術(shù)方案的改進(jìn)，所述步驟還包括系統(tǒng)對(duì)雙臂之間公共力的分解。

作為該技術(shù)方案的改進(jìn)，所述對(duì)公共力的分解的方法包括采用主從分解模式。

作為該技術(shù)方案的改進(jìn)，所述主從分解模式包括通過(guò)給定其中一機(jī)械臂的末端期望操作力/力矩F_ea＝[f_ex,f_ey,f_ez,τ_ex,τ_ey,τ_ez]^T，進(jìn)而可求解得到另一機(jī)械臂的末端期望力/力矩為其中，F(xiàn)_L表示負(fù)載所受環(huán)境的廣義力；表示負(fù)載所受地球的廣義重力，F(xiàn)_IL表示負(fù)載的廣義慣性力，Γ_a、Γ_b分別代表兩個(gè)機(jī)械臂的抓取矩陣。

作為該技術(shù)方案的改進(jìn)，所述對(duì)公共力的分解的方法包括采用共享分解模式。

進(jìn)一步地，所述共享分解模式包括將兩個(gè)機(jī)械臂同等看待，采用偽逆法求解負(fù)載受力平衡方程，可得到：

其中，Γ_a、Γ_b分別代表兩個(gè)機(jī)械臂的抓取矩陣，F(xiàn)_ea、F_eb分別為兩機(jī)械臂的末端期望操作力/力矩，F(xiàn)_L表示負(fù)載所受環(huán)境的廣義力；表示負(fù)載所受地球的廣義重力；F_IL表示負(fù)載的廣義慣性力。

進(jìn)一步地，所述單臂控制模塊采用基于位置的阻抗控制，通過(guò)機(jī)械臂的導(dǎo)納特性將末端的力、力矩誤差等效為輸入的位置與姿態(tài)增量，實(shí)現(xiàn)機(jī)械臂的力柔順操作。

另一方面，本發(fā)明還提供一種基于雙臂機(jī)器人協(xié)同操作的柔順控制系統(tǒng)，包括：雙臂協(xié)同控制模塊、驅(qū)動(dòng)模塊和單臂控制模塊；

所述雙臂協(xié)同控制模塊用于執(zhí)行步驟根據(jù)目標(biāo)負(fù)載的期望運(yùn)動(dòng)軌跡和期望力建立動(dòng)力學(xué)模型，通過(guò)滿足雙臂閉鏈約束方程的協(xié)調(diào)位置分解和公共力分解得到各個(gè)機(jī)械臂的末端期望位姿和末端期望力；

所述單臂控制模塊用于執(zhí)行步驟完成對(duì)所述期望任務(wù)指令的解析與執(zhí)行，得到機(jī)械臂各個(gè)關(guān)節(jié)的期望角度，通過(guò)基于位置的阻抗控制實(shí)現(xiàn)機(jī)械臂的力柔順操作；

所述驅(qū)動(dòng)模塊用于執(zhí)行步驟根據(jù)規(guī)劃的期望關(guān)節(jié)角度，驅(qū)動(dòng)所述機(jī)械臂完成任務(wù)軌跡。

本發(fā)明公開(kāi)了一種目標(biāo)負(fù)載操作的閉鏈雙臂協(xié)調(diào)柔順控制方法，主要包括：建立雙臂機(jī)器人系統(tǒng)的動(dòng)力學(xué)模型，考慮閉鏈約束關(guān)系，得到雙臂機(jī)器人的協(xié)調(diào)運(yùn)動(dòng)學(xué)以及動(dòng)力學(xué)方程；以目標(biāo)負(fù)載為研究對(duì)象，建立負(fù)載的內(nèi)力平衡方程；采用主從式和共享式策略進(jìn)行負(fù)載的公共力分解，得到兩個(gè)機(jī)械臂的期望操作力，進(jìn)而提出了雙臂協(xié)同操作的主從式力柔順控制方法和共享式力柔順控制方法。雙臂機(jī)器人的實(shí)時(shí)操作力由安裝在機(jī)械臂末端的六維力/力矩傳感器得到，各個(gè)機(jī)械臂的力柔順操作通過(guò)基于位置的阻抗控制實(shí)現(xiàn)。本發(fā)明適用于存在閉鏈強(qiáng)耦合約束的雙臂緊協(xié)調(diào)操作，在保證雙臂協(xié)調(diào)運(yùn)動(dòng)的同時(shí)，將末端操作力控制在期望的范圍內(nèi)，為雙臂機(jī)器人的閉鏈操作問(wèn)題提供了一種安全有效的控制方法。

目標(biāo)負(fù)載操作的閉鏈雙臂協(xié)調(diào)柔順控制方法，包括雙臂協(xié)同控制模塊、單臂控制模塊、驅(qū)動(dòng)模塊，其還可包括傳感器信息融合與時(shí)延后處理等模塊。其中：雙臂協(xié)同控制模塊(雙臂協(xié)同控制器)首先由任務(wù)規(guī)劃得到負(fù)載的期望運(yùn)動(dòng)軌跡和期望力作為輸入，進(jìn)一步通過(guò)滿足雙臂閉鏈約束方程的協(xié)調(diào)位置分解和公共力分解得到各個(gè)機(jī)械臂的末端期望位姿輸入和末端期望力輸入；

單臂控制模塊(單臂控制器)完成對(duì)任務(wù)指令的解析與執(zhí)行，得到機(jī)械臂各個(gè)關(guān)節(jié)的期望角度，通過(guò)基于位置的阻抗控制實(shí)現(xiàn)機(jī)械臂的力柔順操作；

驅(qū)動(dòng)模塊(底層硬件)根據(jù)規(guī)劃的期望關(guān)節(jié)角度，驅(qū)動(dòng)機(jī)械臂完成任務(wù)軌跡；

傳感器信息融合與時(shí)延后處理模塊采集雙臂機(jī)器人系統(tǒng)各個(gè)傳感器的信息，對(duì)信號(hào)間的時(shí)差和時(shí)延進(jìn)行處理，最后同時(shí)反饋給上位機(jī)控制軟件。

其中，雙臂協(xié)同控制模塊完成雙臂之間的協(xié)調(diào)位置分解與公共力分解考慮雙臂對(duì)目標(biāo)負(fù)載操作時(shí)所形成的閉鏈約束，可以得到如下雙臂協(xié)調(diào)運(yùn)動(dòng)學(xué)方程以及各個(gè)機(jī)械臂的動(dòng)力學(xué)方程如下：

其中，表示坐標(biāo)系Oy相對(duì)于坐標(biāo)系Ox的位姿齊次變換矩陣；

為Arm-k的廣義質(zhì)量矩陣；

為Arm-k的非線性哥氏力和離心力矢量；

分別為Arm-k的重力項(xiàng)；

分別為Arm-k的雅克比；

分別為Arm-k的關(guān)節(jié)驅(qū)動(dòng)力矩。

進(jìn)行雙臂協(xié)調(diào)操作時(shí)負(fù)載的受力平衡分析，得到操作力約束方程如下：

上式可簡(jiǎn)寫(xiě)為：其中F_L表示負(fù)載所受環(huán)境的廣義力；表示負(fù)載所受地球的廣義重力；F_IL表示負(fù)載的廣義慣性力，Γ_a、Γ_b分別代表兩個(gè)機(jī)械臂的抓取矩陣。

根據(jù)負(fù)載受力平衡方程進(jìn)行負(fù)載操作的公共力分解，然而6個(gè)負(fù)載受力平衡方程包含12個(gè)未知數(shù)，無(wú)法獨(dú)立求解，故采用主從模式或共享模式兩種模式對(duì)其進(jìn)行分解。

1)主從分解模式

定義其中一個(gè)機(jī)械臂為主臂(記為Arm-a)，產(chǎn)生主動(dòng)運(yùn)動(dòng)；另一個(gè)機(jī)械臂為從臂(記為Arm-b)，跟隨主臂做被動(dòng)運(yùn)動(dòng)。

給定主臂的期望末端操作力/力矩：F_ea＝[f_ex,f_ey,f_ez,τ_ex,τ_ey,τ_ez]^T；

可以求解得到從臂的末端期望力/力矩：

其進(jìn)一步可給出如下定義。

主從式力柔順控制策略：根據(jù)操作任務(wù)需求或機(jī)械臂負(fù)載能力，定義雙臂機(jī)器人系統(tǒng)中的主臂和從臂；給定主臂的期望運(yùn)動(dòng)軌跡和末端操作力，通過(guò)雙臂的閉鏈約束方程和負(fù)載力平衡方程計(jì)算得到從臂的期望運(yùn)動(dòng)軌跡和末端操作力；最后通過(guò)獨(dú)立的單臂柔順控制器完成雙臂協(xié)同操作任務(wù)。上述控制流程即定義為雙臂協(xié)同主從式力柔順控制。

2)共享分解模式

共享模式將兩個(gè)機(jī)械臂同等看待，采用偽逆法求解負(fù)載受力平衡方程，可以得到：

其中，Γ_a、Γ_b分別代表兩個(gè)機(jī)械臂的抓取矩陣，F(xiàn)_ea、F_eb分別為兩機(jī)械臂的末端期望操作力/力矩，F(xiàn)_L表示負(fù)載所受環(huán)境的廣義力；G_L表示負(fù)載所受地球的廣義重力；F_IL表示負(fù)載的廣義慣性力。

進(jìn)一步給出如下定義：

共享式力柔順控制策略：直接對(duì)雙臂的閉鏈約束方程和負(fù)載力平衡方程進(jìn)行分解，得到兩個(gè)機(jī)械臂的末端期望運(yùn)動(dòng)軌跡和期望操作力，達(dá)到雙臂綜合最優(yōu)的效果；通過(guò)獨(dú)立的單臂柔順控制器完成雙臂協(xié)同操作任務(wù)。上述控制流程即定義為雙臂協(xié)同共享式力柔順控制。

進(jìn)一步，單臂控制模塊采用基于位置的阻抗控制，硬件上只需要機(jī)械臂末端操作力的檢測(cè)以及關(guān)節(jié)空間的位置控制；通過(guò)機(jī)械臂的導(dǎo)納特性將末端的力、力矩誤差等效為輸入的位置與姿態(tài)增量，即可實(shí)現(xiàn)機(jī)械臂的力柔順操作。

機(jī)械臂的導(dǎo)納特性的數(shù)學(xué)模型可以表示為：

其中：M為機(jī)械臂的慣性系數(shù)；B為機(jī)械臂的阻尼系數(shù)；K為機(jī)械臂的剛度系數(shù)；x、x_e分別表示機(jī)械臂末端的實(shí)際位姿向量和期望位姿向量；F為機(jī)械臂末端操作力偏差；

對(duì)上式進(jìn)行拉普拉斯變換，當(dāng)已知操作力誤差時(shí)，可以求得與末端操作力偏差對(duì)應(yīng)的位姿偏差如下：

其中雙臂協(xié)同控制器由雙臂之間的協(xié)調(diào)位置分解與公共力分解組成。考慮雙臂對(duì)目標(biāo)負(fù)載操作時(shí)所形成的閉鏈約束，可以得到如下雙臂協(xié)調(diào)運(yùn)動(dòng)學(xué)方程以及各個(gè)機(jī)械臂的動(dòng)力學(xué)方程如下：

其中，表示坐標(biāo)系Oy相對(duì)于坐標(biāo)系Ox的位姿齊次變換矩陣；

為Arm-k的廣義質(zhì)量矩陣；

為Arm-k的非線性哥氏力和離心力矢量；

分別為Arm-k的重力項(xiàng)；

分別為Arm-k的雅克比；

分別為Arm-k的關(guān)節(jié)驅(qū)動(dòng)力矩。

進(jìn)行雙臂協(xié)調(diào)操作時(shí)負(fù)載的受力平衡分析，得到操作力約束方程如下：

上式簡(jiǎn)寫(xiě)為：其中F_L：負(fù)載所受環(huán)境的廣義力；負(fù)載所受地球的廣義重力；F_IL：負(fù)載的廣義慣性力，Γ_a、Γ_b分別代表兩個(gè)機(jī)械臂的抓取矩陣。

根據(jù)負(fù)載受力平衡方程進(jìn)行負(fù)載操作的公共力分解，然而6個(gè)負(fù)載受力平衡方程包含12個(gè)未知數(shù)，無(wú)法獨(dú)立求解，故采用主從模式與共享模式兩種模式對(duì)其進(jìn)行分解。

1)主從分解模式

定義其中一個(gè)機(jī)械臂為主臂(記為Arm-a)，產(chǎn)生主動(dòng)運(yùn)動(dòng)；另一個(gè)機(jī)械臂為從臂(記為Arm-b)，跟隨主臂做被動(dòng)運(yùn)動(dòng)。

給定主臂的期望末端操作力/力矩：F_ea＝[f_ex,f_ey,f_ez,τ_ex,τ_ey,τ_ez]^T；

可以求解得到從臂的末端期望力/力矩：

進(jìn)一步給出如下定義：

主從式力柔順控制策略：根據(jù)操作任務(wù)需求或機(jī)械臂負(fù)載能力，定義雙臂機(jī)器人系統(tǒng)中的主臂和從臂；給定主臂的期望運(yùn)動(dòng)軌跡和末端操作力，通過(guò)雙臂的閉鏈約束方程和負(fù)載力平衡方程計(jì)算得到從臂的期望運(yùn)動(dòng)軌跡和末端操作力；最后通過(guò)獨(dú)立的單臂柔順控制器完成雙臂協(xié)同操作任務(wù)。上述控制流程即定義為雙臂協(xié)同主從式力柔順控制。

2)共享分解模式

共享模式將兩個(gè)機(jī)械臂同等看待，采用偽逆法求解負(fù)載受力平衡方程，可以得到：

進(jìn)一步給出如下定義：

共享式力柔順控制策略：直接對(duì)雙臂的閉鏈約束方程和負(fù)載力平衡方程進(jìn)行分解，得到兩個(gè)機(jī)械臂的末端期望運(yùn)動(dòng)軌跡和期望操作力，達(dá)到雙臂綜合最優(yōu)的效果；通過(guò)獨(dú)立的單臂柔順控制器完成雙臂協(xié)同操作任務(wù)。上述控制流程即定義為雙臂協(xié)同共享式力柔順控制。

單臂控制器采用基于位置的阻抗控制，硬件上只需要機(jī)械臂末端操作力的檢測(cè)以及關(guān)節(jié)空間的位置控制；通過(guò)機(jī)械臂的導(dǎo)納特性將末端的力、力矩誤差等效為輸入的位置與姿態(tài)增量，即可實(shí)現(xiàn)機(jī)械臂的力柔順操作。

(1)雙臂動(dòng)力學(xué)建模以及負(fù)載力平衡分析

參照?qǐng)D1，雙臂空間機(jī)器人系統(tǒng)的一般運(yùn)動(dòng)學(xué)模型由基座、n_a自由度的串聯(lián)機(jī)械臂(記為Arm-a)和n_b自由度的串聯(lián)機(jī)械臂(記為Arm-b)組成。其中，各個(gè)變量定義如下：

B₀：為作為空間機(jī)器人基座的航天器平臺(tái)；

B_L：為雙臂協(xié)同操作的目標(biāo)負(fù)載；

分別表示Arm-a和Arm-b的第i個(gè)連桿；

為Arm-a和Arm-b的第i個(gè)關(guān)節(jié)；

為Arm-a和Arm-b的第i個(gè)關(guān)節(jié)的關(guān)節(jié)角度；

當(dāng)進(jìn)行雙臂協(xié)調(diào)操作時(shí)，雙臂機(jī)器人與目標(biāo)操作物形成閉鏈約束，因此已知目標(biāo)操作物的期望位姿軌跡^BT_L，可以根據(jù)閉鏈約束方程分別得到兩個(gè)機(jī)械臂末端在各自參考坐標(biāo)系下的表示：

其中，表示機(jī)械臂k的末端工具坐標(biāo)系相對(duì)于基座坐標(biāo)系的齊次變換矩陣；表示機(jī)械臂k的基座坐標(biāo)系相對(duì)于雙臂機(jī)器人本體坐標(biāo)系下的齊次變換矩陣；表示目標(biāo)負(fù)載的本體坐標(biāo)系相對(duì)于機(jī)械臂k末端工具坐標(biāo)系的齊次變換矩陣；fkine表示機(jī)械臂的正運(yùn)動(dòng)學(xué)方程；

對(duì)任意給定任務(wù)，根據(jù)式(1)-(4)，通過(guò)逆運(yùn)動(dòng)學(xué)方程便可以得到雙臂機(jī)器人系統(tǒng)協(xié)調(diào)操作時(shí)各個(gè)關(guān)節(jié)的控制輸入。

假設(shè)雙臂機(jī)器人基座平臺(tái)與雙臂不產(chǎn)生同時(shí)運(yùn)動(dòng)，雙臂協(xié)調(diào)操作時(shí)可以將雙臂機(jī)器人系統(tǒng)中的各個(gè)機(jī)械臂近似為基座固定的機(jī)械臂時(shí)，得到各個(gè)機(jī)械臂的動(dòng)力學(xué)方程如下：

其中：M_k(k＝a,b)為對(duì)稱正定的慣量矩陣，C_k為科氏力/離心力項(xiàng)，G_k為動(dòng)力學(xué)方程中的重力項(xiàng)；τ_k為關(guān)節(jié)的控制力矩，為機(jī)械臂操作時(shí)所受到的外力，本發(fā)明中特指雙臂與負(fù)載形成閉鏈時(shí)機(jī)械臂末端受到的接觸力，分別為兩機(jī)械臂的雅克比矩陣。

參照?qǐng)D2，為雙臂同時(shí)抓持目標(biāo)負(fù)載時(shí)的受力情況。對(duì)雙臂協(xié)調(diào)操作時(shí)負(fù)載的受力平衡進(jìn)行分析，得到操作力/力矩約束方程。

其中：

f_L,τ_L為環(huán)境施加給目標(biāo)載荷的外力、外力矩；

-f_ea,-τ_ea為Arm-a末端施加給載荷的力、力矩(與載荷施加給Arm-a末端的力f_ea、力矩τ_ea互為作用力及反作用力)；

-f_eb,-τ_eb為Arm-b末端施加給載荷的力、力矩(與載荷施加給Arm-b末端的力f_eb、力矩τ_eb互為作用力及反作用力)；

r_L、r_a、r_b分別為力f_L、f_ea、f_eb作用點(diǎn)到載荷質(zhì)心的矢量

v_L,ω_L為負(fù)載質(zhì)心坐標(biāo)系的線速度和角速度；

M_L為負(fù)載的質(zhì)量；

I_L為負(fù)載質(zhì)心轉(zhuǎn)動(dòng)慣量；

G_L＝M_Lg為負(fù)載所受的重力；

最后可得到：

雙臂協(xié)調(diào)力柔順控制的關(guān)鍵是如何將common點(diǎn)的廣義力、力矩等效到兩個(gè)臂的末端。將式(9)簡(jiǎn)寫(xiě)為：

式(10)中：

F_L：負(fù)載所受環(huán)境的廣義力；

負(fù)載所受地球的廣義重力；

F_IL：負(fù)載的廣義慣性力。

(2)負(fù)載操作的公共力分解方法

對(duì)于給定的任務(wù)，F(xiàn)_L、F_IL可求(作為已知項(xiàng))。然而，式(10)包含了12個(gè)未知數(shù)(F_ea、F_eb分別為6個(gè))，且僅有6個(gè)方程，無(wú)法獨(dú)立求解。為了求解欠定的負(fù)載力平衡方程(10)，分別采用主從式和共享式策略進(jìn)行負(fù)載的公共力分解，得到兩個(gè)機(jī)械臂的期望操作力，進(jìn)而提出了雙臂協(xié)同操作的主從式力柔順控制方法和共享式力柔順控制方法。

1)主從分解模式

主從分解模式定義其中一個(gè)機(jī)械臂為主臂(記為Arm-a)，產(chǎn)生主動(dòng)運(yùn)動(dòng)；另一個(gè)機(jī)械臂為從臂(記為Arm-b)，跟隨主臂做被動(dòng)運(yùn)動(dòng)。

給定主臂的期望末端操作力/力矩：F_ea＝[f_ex,f_ey,f_ez,τ_ex,τ_ey,τ_ez]^T；

可以求解得到從臂的末端期望力/力矩：

給出如下定義：

主從式力柔順控制策略：根據(jù)操作任務(wù)需求或機(jī)械臂負(fù)載能力，定義雙臂機(jī)器人系統(tǒng)中的主臂和從臂；給定主臂的期望運(yùn)動(dòng)軌跡和末端操作力，通過(guò)雙臂的閉鏈約束方程和負(fù)載力平衡方程計(jì)算得到從臂的期望運(yùn)動(dòng)軌跡和末端操作力；最后通過(guò)獨(dú)立的單臂柔順控制器完成雙臂協(xié)同操作任務(wù)。上述控制流程即定義為雙臂協(xié)調(diào)主從式力柔順控制。

2)共享分解模式

共享模式將兩個(gè)機(jī)械臂同等看待，采用偽逆法求解負(fù)載受力平衡方程，可以得到：

進(jìn)一步給出如下定義：

共享式力柔順控制策略：直接對(duì)雙臂的閉鏈約束方程和負(fù)載力平衡方程進(jìn)行分解，得到兩個(gè)機(jī)械臂的末端期望運(yùn)動(dòng)軌跡和期望操作力，達(dá)到雙臂綜合最優(yōu)的效果；通過(guò)獨(dú)立的單臂柔順控制器完成雙臂協(xié)同操作任務(wù)。上述控制流程即定義為雙臂協(xié)同共享式力柔順控制。

主從式雙臂協(xié)同力柔順控制策略優(yōu)先確定出主臂的末端操作力，因此主臂具有較高的優(yōu)先級(jí)；而從臂則可以看作為滿足雙臂協(xié)同操作任務(wù)的輔助機(jī)械臂，其末端操作力由負(fù)載的受力平衡方程計(jì)算得出。另外，對(duì)于雙臂異構(gòu)或雙臂負(fù)載能力不一致的協(xié)調(diào)操作情況，主從式力柔順控制可以體現(xiàn)出更強(qiáng)的優(yōu)越性，通過(guò)分別設(shè)置雙臂不同的操作力控制目標(biāo)，實(shí)現(xiàn)對(duì)載荷的協(xié)同操作。

共享式雙臂協(xié)同力柔順控制策略直接對(duì)負(fù)載受力平衡方程進(jìn)行分解，同時(shí)考慮兩個(gè)機(jī)械臂末端的操作力大小，實(shí)時(shí)控制閉鏈狀態(tài)下負(fù)載內(nèi)力在期望的范圍內(nèi)；控制中以實(shí)現(xiàn)負(fù)載的期望運(yùn)動(dòng)為最高優(yōu)先級(jí)，雙臂在滿足閉鏈約束方程的條件下進(jìn)行協(xié)同控制，尤其適用于對(duì)負(fù)載運(yùn)動(dòng)軌跡精度要求較高的場(chǎng)合。

(3)單臂基于位置的阻抗控制方法

當(dāng)雙臂末端的期望位置與期望操作力確定后，可對(duì)各個(gè)機(jī)械臂進(jìn)行力柔順控制操作。

參照?qǐng)D3，單臂控制器采用基于位置的阻抗控制，基于位置的阻抗控制通過(guò)機(jī)械臂的導(dǎo)納特性將末端的力、力矩誤差等效為輸入的位置與姿態(tài)增量，對(duì)機(jī)械臂進(jìn)行閉環(huán)控制。硬件上只需要機(jī)械臂末端操作力的檢測(cè)以及關(guān)節(jié)空間的位置控制；通過(guò)機(jī)械臂的導(dǎo)納特性將末端的力、力矩誤差等效為輸入的位置與姿態(tài)增量，即可實(shí)現(xiàn)機(jī)械臂的力柔順操作。

通過(guò)給定與實(shí)際系統(tǒng)相對(duì)應(yīng)的質(zhì)量M、阻尼B與剛度K，便可以在自由或約束空間進(jìn)行柔順控制。其數(shù)學(xué)模型可以用下面的微分方程來(lái)描述：

如果將其應(yīng)用到雙臂機(jī)器人的從臂上，當(dāng)檢測(cè)到末端力誤差F時(shí)，等效到輸入端位置信號(hào)為Δx，進(jìn)行拉普拉斯變換可以得到：

通過(guò)上式得到機(jī)械臂末端的位姿增量，與雙臂協(xié)調(diào)位置分解得到的機(jī)械臂期望位姿進(jìn)行疊加，從而實(shí)現(xiàn)單個(gè)機(jī)械臂的力柔順控制操作。

(4)雙臂主從柔順控制方法

參照?qǐng)D4，基于上述雙臂協(xié)調(diào)位置分解以及公共力分解策略，本發(fā)明提出一種目標(biāo)負(fù)載操作的閉鏈雙臂協(xié)調(diào)柔順控制方法及系統(tǒng)。主要由雙臂協(xié)同控制器、單臂控制器、底層硬件實(shí)現(xiàn)以及傳感器信息融合與時(shí)延后處理等模塊組成，其中：

雙臂協(xié)同控制器首先由任務(wù)規(guī)劃得到負(fù)載的期望運(yùn)動(dòng)軌跡和期望力作為輸入，進(jìn)一步通過(guò)滿足雙臂閉鏈約束的協(xié)調(diào)位置分解方程和公共力分解方程得到兩個(gè)機(jī)械臂的末端期望位姿輸入和末端期望力輸入；其中公共力分解可以采用主從分解和共享分解兩種模式。

單臂控制器完成對(duì)任務(wù)指令的解析與執(zhí)行，得到機(jī)械臂各個(gè)關(guān)節(jié)的期望角度，通過(guò)基于位置的阻抗控制實(shí)現(xiàn)機(jī)械臂的力柔順操作；首先雙臂協(xié)同控制器得到的各個(gè)機(jī)械臂的期望位姿和期望力作為單臂控制的輸入；計(jì)算期望操作力與傳感器反饋的末端操作力的偏差，通過(guò)阻抗控制將操作力偏差等效為機(jī)械臂末端位姿的修正量；機(jī)械臂末端期望位姿與修正量疊加，通過(guò)逆運(yùn)動(dòng)學(xué)方程求解得到對(duì)應(yīng)的各個(gè)關(guān)節(jié)期望角度，控制各個(gè)關(guān)節(jié)運(yùn)動(dòng)，進(jìn)而實(shí)現(xiàn)機(jī)械臂的柔順操作。

底層硬件根據(jù)規(guī)劃的期望關(guān)節(jié)角度，驅(qū)動(dòng)機(jī)械臂完成任務(wù)軌跡；傳感器信息融合與時(shí)延后處理模塊采集雙臂機(jī)器人系統(tǒng)各個(gè)傳感器的信息(包括機(jī)械臂各個(gè)關(guān)節(jié)的位置和速度信息以及機(jī)械臂末端六維力傳感器的操作力信息)，對(duì)信號(hào)進(jìn)行濾波和時(shí)延處理，最后同時(shí)反饋給上位機(jī)控制軟件。

一種基于雙臂機(jī)器人協(xié)同操作的柔順控制系統(tǒng)，包括：雙臂協(xié)同控制模塊、驅(qū)動(dòng)模塊和單臂控制模塊；

所述雙臂協(xié)同控制模塊用于執(zhí)行步驟根據(jù)目標(biāo)負(fù)載的期望運(yùn)動(dòng)軌跡和期望力建立動(dòng)力學(xué)模型，通過(guò)滿足雙臂閉鏈約束方程的協(xié)調(diào)位置分解和公共力分解得到各個(gè)機(jī)械臂的末端期望位姿和末端期望力；

所述單臂控制模塊用于執(zhí)行步驟完成對(duì)所述期望任務(wù)指令的解析與執(zhí)行，得到機(jī)械臂各個(gè)關(guān)節(jié)的期望角度，通過(guò)基于位置的阻抗控制實(shí)現(xiàn)機(jī)械臂的力柔順操作；

所述驅(qū)動(dòng)模塊用于執(zhí)行步驟根據(jù)規(guī)劃的期望關(guān)節(jié)角度，驅(qū)動(dòng)所述機(jī)械臂完成任務(wù)軌跡。

參照?qǐng)D5，是本發(fā)明第五實(shí)施例的控制方法流程示意圖。一種基于雙臂機(jī)器人協(xié)同操作的柔順控制方法，包括以下步驟：

雙臂協(xié)同控制模塊根據(jù)目標(biāo)負(fù)載的期望運(yùn)動(dòng)軌跡和期望力建立動(dòng)力學(xué)模型，通過(guò)滿足雙臂閉鏈約束方程的協(xié)調(diào)位置分解和公共力分解得到各個(gè)機(jī)械臂的末端期望位姿和末端期望力；

單臂控制模塊完成對(duì)所述期望任務(wù)指令的解析與執(zhí)行，得到機(jī)械臂各個(gè)關(guān)節(jié)的期望角度，通過(guò)基于位置的阻抗控制實(shí)現(xiàn)機(jī)械臂的力柔順操作；

驅(qū)動(dòng)模塊根據(jù)規(guī)劃的期望關(guān)節(jié)角度，驅(qū)動(dòng)所述機(jī)械臂完成任務(wù)軌跡。

本發(fā)明中公開(kāi)的目標(biāo)負(fù)載操作的閉鏈雙臂協(xié)調(diào)柔順控制方法，可以確保雙臂機(jī)器人在對(duì)負(fù)載進(jìn)行閉鏈約束下的協(xié)同操作的同時(shí)，將機(jī)械臂的末端操作力控制在期望的范圍。

以上是對(duì)本發(fā)明的較佳實(shí)施進(jìn)行了具體說(shuō)明，但本發(fā)明創(chuàng)造并不限于所述實(shí)施例，熟悉本領(lǐng)域的技術(shù)人員在不違背本發(fā)明精神的前提下還可做出種種的等同變形或替換，這些等同的變形或替換均包含在本申請(qǐng)權(quán)利要求所限定的范圍內(nèi)。