本發(fā)明屬于手術機器人領域，具體涉及一種主從式遙操作手術機器人多臂協(xié)同控制系統(tǒng)，屬于人機交互領域。

背景技術：

主從式遙操作手術機器人控制系統(tǒng)通常由一個主手操作端和若干個從手執(zhí)行臂組成。從手執(zhí)行臂安裝于手術臺旁邊，在其末端可以安裝內窺鏡以及各種手術器械，通過微小創(chuàng)口到達病人體內病灶處。醫(yī)生操作主手操作，即可控制從手末端器械完成各種手術操作，為外科醫(yī)生提供了傳統(tǒng)手術的操作環(huán)境，可以協(xié)助醫(yī)生完成更精細的手術動作，減少手術時由于疲勞產(chǎn)生的誤操作或手部震顫造成的損傷。多臂機器人是髙階強耦合的復雜非線性系統(tǒng)，同時由于工作環(huán)境的多變性，對機器人系統(tǒng)控制方法的魯棒性、控制系統(tǒng)的實時性提出了很髙要求?；趯伪蹤C器人的運動學優(yōu)化求解，無法滿足機器人末端受環(huán)境約束并需要與環(huán)境交互的應用需求。在針對非結構化環(huán)境設計的多臂協(xié)作機器人比傳統(tǒng)的單臂機器人具有更加靈活的特點，能實現(xiàn)人與機器人之間以及各機器人之間的互動功能。目前國外已研制出主從式遙操作手術機器人，但尚不具備力反饋功能及多從臂協(xié)同控制功能。

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

有鑒于此，本發(fā)明提供一種主從式遙操作手術機器人多臂協(xié)同控制系統(tǒng)，利用滑?？刂葡到y(tǒng)及多運動控制器并行解算，可以實現(xiàn)各機械臂之間的協(xié)同作業(yè)功能。

本發(fā)明的目的是通過這樣的技術方案實現(xiàn)的，一種主從式遙操作手術機器人多臂協(xié)同控制系統(tǒng)，用于對多臂手術機器人進行控制，多臂手術機器人包括主手操作端和從手執(zhí)行端，主手操作端為7dof串聯(lián)式機器人，從手執(zhí)行端由兩個7dof串聯(lián)式機械臂和兩個6dof串聯(lián)式機械臂組成，每個機械臂末端均安裝多維力傳感器和運動控制器，其特征在于：該控制系統(tǒng)包括工業(yè)pc系統(tǒng)和底層控制系統(tǒng)，所述工業(yè)pc系統(tǒng)包括工業(yè)pc、以太網(wǎng)、can總線適配卡；所述底層控制系統(tǒng)包括數(shù)字量與模擬量采集卡、伺服驅動器；工業(yè)pc利用以太網(wǎng)接受來自其他系統(tǒng)的信號，利用can總線適配卡連接伺服驅動器控制機械臂運動，利用數(shù)字量/模擬量采集卡采集和處理來自多維力傳感器的信號；各機械臂接收主手操作端傳遞過來的位置控制指令，通過逆運動學解算得到各關節(jié)期望位置；通過多維力傳感器感知與環(huán)境間的接觸力及機械臂之間的接觸力，經(jīng)過逆動力學求解得到各關節(jié)力和力矩信息，并將接觸力反饋至手術機器人主手；通過絕對值位置傳感器獲取各關節(jié)實際位置；由工業(yè)pc協(xié)調各運動控制器的數(shù)據(jù)傳遞及信號傳遞，完成多機械臂協(xié)同控制功能。

進一步，該手術機器人多臂協(xié)同控制系統(tǒng)包括滑模控制器、自適應模糊邏輯控制器和非線性觀測器；所述滑?？刂破鳎糜诰_獲得系統(tǒng)的動力學參數(shù)；所述自適應模糊邏輯控制器，用于提高控制精度；所述非線性觀測器，用于補償外部環(huán)境對系統(tǒng)穩(wěn)定新和精度的影響。

進一步，所述滑?？刂破鞯目刂坡蔀棣樱溅觘q+τsm，其中τsm為滑?？刂祈棧?imgfile="bda0001272912810000021.gif"wi="700"he="58"img-content="drawing"img-format="gif"orientation="portrait"inline="no"/>為機器人的動力學等效模型，m(q)為慣量矩陣，j^-1(q)為逆雅可比矩陣，為機器人末端笛卡爾空間加速度，為雅可比矩陣的導數(shù)，為關節(jié)空間速度，為慣性項和科氏項，g(q)為重力項；選擇滑模面其中s為6×1維向量，c為正定對角矩陣，e＝qid-qi，表示e的導數(shù)，qid為關節(jié)期望位置，qi為關節(jié)實際位置；所述滑模項為k1為6×6的正定對角矩陣；m(q)為慣性矩陣；j^t(q)為雅可比矩陣的轉秩矩陣，s＝[s1,s2,…,si]為模糊邏輯輸入量，γ＝[γ1,γ2,…,γi]為模糊邏輯輸出量。

進一步，所述非線性觀測器的模型為：

其中k1與k2是正定矩陣，表示外部轉矩擾動，表示外部轉矩擾動的估計，為關節(jié)速度誤差向量，為對關節(jié)速度誤差的估計量，表示關節(jié)加速度誤差；m^-1表示逆慣量矩陣，c表示慣性項和科氏項，表示關節(jié)空間速度，g表示重力項，為關節(jié)輸入加速度，τ∈r⁶為關節(jié)輸入力矩。

由于采用了上述技術方案，本發(fā)明具有如下的優(yōu)點：

本發(fā)明完成了硬件架構的搭建和機械臂運動學算法和多臂協(xié)同控制軟件開發(fā)，實現(xiàn)了手術機器人的多臂協(xié)同控制功能。

附圖說明

為了使本發(fā)明的目的、技術方案和優(yōu)點更加清楚，下面將結合附圖對本發(fā)明作進一步的詳細描述，其中：

圖1為本發(fā)明主從式手術機器人控制系統(tǒng)示意圖；

圖2為本發(fā)明動力學控制策略示意圖；

圖3為本發(fā)明多臂協(xié)同作業(yè)控制系統(tǒng)示意圖。

具體實施方式

以下將結合附圖，對本發(fā)明的優(yōu)選實施例進行詳細的描述；應當理解，優(yōu)選實施例僅為了說明本發(fā)明，而不是為了限制本發(fā)明的保護范圍。

附圖1為本發(fā)明的控制系統(tǒng)硬件架構，主手操作端和從手執(zhí)行端，主手操作端為7dof串聯(lián)式機器人，從手執(zhí)行端由兩個7dof串聯(lián)式機械臂和兩個6dof串聯(lián)式機械臂組成，每個機械臂末端均安裝多維力傳感器。所述的機械臂的控制系統(tǒng)硬件包括工業(yè)pc系統(tǒng)和底層控制系統(tǒng)。所述工業(yè)pc系統(tǒng)包括可視化人機界面、工業(yè)pc、以太網(wǎng)卡、can總線適配卡；所述底層控制系統(tǒng)包括多個運動控制器、數(shù)字量與模擬量采集卡、伺服驅動器及電機、多維力傳感器、絕對式編碼器。工業(yè)pc利用以太網(wǎng)接受來自其他系統(tǒng)的信號，利用can總線適配卡連接伺服驅動器控制機械臂運動，利用數(shù)字量/模擬量采集卡采集和處理來自多維力傳感器與其他傳感器的信號。

附圖3為本發(fā)明的多臂協(xié)同作業(yè)控制系統(tǒng)示意圖，各機械臂接收主手操作端傳遞過來的位置控制指令，通過逆運動學解算得到各關節(jié)期望位置；通過多維力傳感器感知與環(huán)境間的接觸力及機械臂之間的接觸力，經(jīng)過逆動力學求解得到各關節(jié)力和力矩信息，并將接觸力反饋至手術機器人主手；通過絕對值位置傳感器獲取各關節(jié)實際位置；采用滑?？刂破鲗Ω麝P節(jié)進行控制，實現(xiàn)多臂協(xié)同作業(yè)控制。

主從式遙操作手術機器人多臂協(xié)同控制系統(tǒng)包括滑?？刂破?、自適應模糊邏輯控制器和非線性觀測器；所述滑?？刂破?，用于精確獲得系統(tǒng)的動力學參數(shù)；所述自適應模糊邏輯控制器，用于提高控制精度；所述非線性觀測器，用于補償外部環(huán)境對系統(tǒng)穩(wěn)定新和精度的影響。

所述滑?？刂破鞯目刂坡蔀棣樱溅觘q+τsm，其中τsm為滑?？刂祈?，為機器人的動力學等效模型，m(q)為慣量矩陣，j^-1(q)為逆雅可比矩陣，為機器人末端笛卡爾空間加速度，為雅可比矩陣的導數(shù)，為關節(jié)空間速度，為慣性項和科氏項，g(q)為重力項；選擇滑模面表示e的導數(shù)，其中s為6×1維向量，c為正定對角矩陣，e＝qid-qi，qid為關節(jié)期望位置，qi為關節(jié)實際位置；滑模項τsm可設計為k1,k2為6×6的正定對角矩陣。

引入模糊邏輯控制策略，模糊邏輯輸出量γ只與s有關，γ＝flc(s)，flc(s)為模糊語言決策集的函數(shù)，s＝[s1,s2,…,si]為模糊邏輯輸入量，γ＝[γ1,γ2,…,γi]為模糊邏輯輸出量，定義表示當前參數(shù)，表示初始參數(shù)，表示的估計值，ψki(si)表示模糊基函數(shù)，為不確定性補償項；于是可以將滑模項τsm修正為k1,k2為6×6的正定對角矩陣；m(q)∈r^6×6,為慣性矩陣；j^t(q)為雅可比矩陣的轉秩矩陣，s＝[s1,s2,…,si]為模糊邏輯輸入量。

所述非線性觀測器的模型為：其中k1與k2是正定矩陣，表示外部轉矩擾動的估計，表示關節(jié)加速度誤差，為關節(jié)速度誤差向量，為對關節(jié)速度誤差的估計量，；為加速度，m^-1表示逆慣量矩陣，c表示慣性項和科氏項，表示關節(jié)速度，g表示重力項，表示外部轉矩擾動，τ∈r⁶為關節(jié)輸入力矩。

所述非線性觀測器的設計方法為，定義位置誤差e＝q-qd，q表示關節(jié)當前位置，qd表示關節(jié)目標位置，求一階導數(shù)和二階導數(shù)為qd,為關節(jié)角位置、速度與加速度；定義為關節(jié)速度誤差向量，為對關節(jié)速度誤差的估計量；定義為對外部干擾力矩的估計，為干擾誤差的估計；將所述e,的表達式代入動力學方程得到基于迭代算法的非線性觀測器設計為其中k1與k2是正定矩陣。

所述的動力學方程為：

其中為關節(jié)角位置、速度加速度，m(q)∈r⁶×⁶為慣性矩陣，為科氏力和向心力矩陣，g(q)∈r⁶為重力矩陣，τ∈r⁶為關節(jié)輸入力矩，τd∈r⁶為機械臂所受外力矩，將代入機械臂的關節(jié)動力學方程得到將機械臂末端期望位姿代替得到動力學等效方程為

由機械臂的dh參數(shù)建立串聯(lián)機器人的齊次變換矩陣，將機械臂的正運動學方程表示為x(t)＝φ(q)，x(t)∈r⁶，q(t)∈r⁶，其中x(t)為笛卡爾空間中機械臂末端位姿，q(t)為關節(jié)空間中各關節(jié)位置；對正運動學方程求導得到速度方程為其中，為雅可比矩陣；對速度方程求導得到加速度方程于是

本發(fā)明提出了一種主從式遙操作手術機器人多臂協(xié)同控制系統(tǒng)，按照所述流程搭建控制系統(tǒng)硬件并編寫控制程序實現(xiàn)了多臂協(xié)同作業(yè)控制功能。

以上所述僅為本發(fā)明的優(yōu)選實施例，并不用于限制本發(fā)明，顯然，本領域的技術人員可以對本發(fā)明進行各種改動和變型而不脫離本發(fā)明的精神和范圍。這樣，倘若本發(fā)明的這些修改和變型屬于本發(fā)明權利要求及其等同技術的范圍之內，則本發(fā)明也意圖包含這些改動和變型在內。