本發(fā)明涉及機器人系統(tǒng)的機械臂的控制系統(tǒng)、控制方法及一種手術(shù)機器人。
背景技術(shù)：
：微創(chuàng)傷外科手術(shù)機器人系統(tǒng)中，各個關(guān)節(jié)的精度決定著整個機器人系統(tǒng)的運動精度和工作性能，而在整個系統(tǒng)中，主操作手與從機械臂的跟隨性能是保證系統(tǒng)精度的關(guān)鍵。主操作手是醫(yī)生手持的操作器，也可以簡稱“主操作手”或“主手”。從機械臂是病人端機器人安裝的機械臂，手術(shù)時會插入人體內(nèi)部執(zhí)行具體手術(shù)操作，可以簡稱為從手。主從機械臂之間的跟隨效果是通過主從控制來實現(xiàn)的，其目的是通過坐標變換和運動映射，采取特定的控制策略，保證主操作手在人眼坐標系下的運動與從機械臂在應(yīng)用場合坐標系下的運動精確對應(yīng)。在手術(shù)過程中，主從控制的中斷操作是不可避免的，例如更換從機械臂上的手術(shù)器械。然而，連接中斷后，由于主操作手的位置或者姿態(tài)被調(diào)整，其在人眼坐標系下的坐標不再與從機械臂在內(nèi)窺鏡坐標系下的坐標一致，因此在重新建立與從機械臂的連接時，主從機械臂之間的運動映射發(fā)生了變化。為了保證跟隨效果，主手的位置和姿態(tài)需要根據(jù)從機械臂的位置和姿態(tài)進行輕微的調(diào)整，從而確保主從兩端運動映射的準確性和一致性。否則，從機械臂為了跟隨主手的運動和姿態(tài)，其姿態(tài)會跟隨主手的姿態(tài)發(fā)生突變。目前，基于前饋控制器和PD控制器的主操作手在主動控制狀態(tài)下的控制系統(tǒng)，由于PD控制本質(zhì)上是對偏差進行操作，當偏差為零時，控制作用也為零，在有摩擦和重力等非線性項作用的情況下，偏差不會為0，同時主操作手的伺服剛度一般較小，因此并不能夠很好地消除靜態(tài)誤差。而且從機械臂開始跟隨主操作手姿態(tài)時，由于靜態(tài)誤差的作用，從機械臂會發(fā)生位置和姿態(tài)的突變，在手術(shù)中容易造成危險的后果。技術(shù)實現(xiàn)要素：本發(fā)明的目的在于提供一種機械臂的控制系統(tǒng)及其控制方法，以提高其控制性能。本發(fā)明還提供一種手術(shù)機器人包括機械臂和該控制系統(tǒng)，該控制系統(tǒng)用于控制該機械臂。一種用于機械臂的控制系統(tǒng)，包括滑?？刂破?，所述滑?？刂破饔糜诮邮諜C械臂上各關(guān)節(jié)位置誤差信號e和速度誤差信號并根據(jù)滑?？刂坡奢敵隽刂噶?，用于控制各關(guān)節(jié)的電機動作，以驅(qū)動機械臂的各個關(guān)節(jié)達到設(shè)計位置，其中，所述滑?？刂坡捎筛鶕?jù)機械臂的動力學(xué)模型以及一快速趨近律得出，所述快速趨近律為根據(jù)所述機械臂上各關(guān)節(jié)的關(guān)節(jié)位置到終端滑模面的距離設(shè)定，所述終端滑模面與各關(guān)節(jié)位置誤差和速度誤差相關(guān)，所述距離是機械臂的各關(guān)節(jié)位置在狀態(tài)空間中到該終端滑模面的距離，所述狀態(tài)空間的狀態(tài)變量包括所述位置誤差和速度誤差。在一實施例中，所述控制系統(tǒng)還包括第一疊加器和第二疊加器，所述第一疊加器的輸入端接收機械臂上各關(guān)節(jié)的實際位置信號q和理想位置信號qd，并輸出位置誤差信號e；所述第二疊加器的輸入端接收機械臂上各關(guān)節(jié)的實際速度信號v和理想速度信號vd，并輸出輸出誤差信號在一實施例中，所述控制系統(tǒng)還包括微分器，所述微分器的輸入端接入機械臂各關(guān)節(jié)上的位置傳感器發(fā)送的位置信號q，并輸出實際速度信號v。在一實施例中，所述控制系統(tǒng)還包括限制器，所述限制器的輸入端與滑?？刂破鞯妮敵龆诉B接，對控制滑?？刂破鞯牧刂噶钸M行飽和限制，輸出最終的力矩指令。在一實施例中，所述控制系統(tǒng)還包括前饋補償裝置和第三疊加器，所述前饋補償裝置的輸入端接收機械臂上各關(guān)節(jié)的實際位置信號q和實際速度信號v，前饋補償裝置的輸出端、滑?？刂破鞯妮敵龆伺c第三疊加器輸入端連接，第三疊加器的輸出端與限制器的輸入端連接，所述限制器對第三疊加器輸出的力矩指令進行飽和限制，并輸出最終的力矩指令。在一實施例中，所述距離設(shè)定為：D=e+λe·+μeγ,]]>其中，D為所述距離，e為位置誤差，為速度誤差，λ,μ和γ是相應(yīng)于機械臂的系統(tǒng)要求的可選擇性參數(shù)。在一實施例中，λ>0,μ>0,1<γ<2在一實施例中，λ＝2,μ＝1,γ＝1.5在一實施例中，所述快速趨近律選擇為：u＝-ηsign(D)-ρD，其中，η和ρ是相應(yīng)于機械臂系統(tǒng)要求的可選擇性參數(shù)。在一實施例中，所述快速趨近律選擇為：u＝-ηsat(D)-ρD，其中，sat(D)=+1,D>δDδ,|D|≤δ-1,D<-δ,]]>δ>0是切換邊界，η和ρ是相應(yīng)于機械臂系統(tǒng)要求的可選擇性參數(shù)。。在一實施例中，η>0，ρ>0。在一實施例中，η＝3，ρ＝1。在一實施例中，所述動力學(xué)模型表達為：H(q)q··+C(q,q·)q·+G(q)=τ,]]>其中，H是n×n的對稱正定矩陣，為關(guān)節(jié)空間慣性矩陣，是關(guān)節(jié)位置q相關(guān)的函數(shù)；C是n×n的矩陣，與離心力和科氏力相關(guān)，是關(guān)節(jié)實際位置q與關(guān)節(jié)實際速度的函數(shù)；G是重力項的矢量；τ是控制律的輸出，即控制律的操作對象的輸入；n為機械臂的關(guān)節(jié)數(shù)。一種機械臂的控制方法包括：提供一個與機械臂上各關(guān)節(jié)位置誤差和速度誤差相關(guān)的終端滑模面，計算機械臂的各關(guān)節(jié)位置在狀態(tài)空間中到終端滑模面的距離，所述狀態(tài)空間的狀態(tài)變量包括所述位置誤差和速度誤差；針對各關(guān)節(jié)位置到終端滑模面的距離，設(shè)計一個快速趨近律；根據(jù)機械臂的動力學(xué)模型以及所述快速趨近律得出滑模控制律的完整表達；根據(jù)所述滑塊控制律輸出力矩指令，用于控制各關(guān)節(jié)的電機動作，以驅(qū)動機械臂的各個關(guān)節(jié)達到設(shè)計位置。一種手術(shù)機器人，包括醫(yī)生端機械臂和所述的控制系統(tǒng)，所述的控制系統(tǒng)用于控制所述醫(yī)生端機械臂。一種手術(shù)機器人，包括病人端機械臂和所述的控制系統(tǒng)，所述的控制系統(tǒng)用于控制所述病人端機械臂。本發(fā)明的控制方法，結(jié)構(gòu)簡單，響應(yīng)速度快，并且可以解決傳統(tǒng)PD控制跟隨精度不能滿足要求的問題，具有消除穩(wěn)態(tài)誤差的控制效果，避免了重連時誤差較大時產(chǎn)生的姿態(tài)突變問題。本發(fā)明的優(yōu)選實施例基于滑模面距離函數(shù)和快速趨近律提高了控制器跟蹤精度，其中的快速趨近律，用飽和函數(shù)代替符號函數(shù)，可以有效地消除或減弱系統(tǒng)處于滑動模態(tài)時的震顫。本發(fā)明的優(yōu)選實施例中滑模面距離函數(shù)采用了冪函數(shù)和線性函數(shù)，提高了系統(tǒng)在滑模面運行時的平滑性。附圖說明本發(fā)明的上述的以及其他的特征、性質(zhì)和優(yōu)勢將通過下面結(jié)合附圖和實施例的描述而變得更加明顯，其中：圖1為本發(fā)明一比較例的機械臂的控制系統(tǒng)的結(jié)構(gòu)框圖；圖2為本發(fā)明一實施例中的機械臂的控制系統(tǒng)的結(jié)構(gòu)框圖；圖3為本發(fā)明一實施例中機械臂的滑?？刂破鞯慕Y(jié)構(gòu)框圖；圖4為本發(fā)明一實施例中機械臂的滑?？刂品椒ǖ牧鞒虉D。具體實施方式本發(fā)明人通過對手術(shù)機器人系統(tǒng)的控制系統(tǒng)的研究，發(fā)現(xiàn)在手術(shù)過程中，主從控制的中斷操作是不可避免的，具體應(yīng)用場景主要有三種：情況一是當主操作手已到達動作極限，而從機械臂并未到達極限時，醫(yī)生需要采取中斷操作，主動斷開主操作手與從機械臂的關(guān)聯(lián)，調(diào)整主操作手位置后，與從機械臂重新建立連接；情況二是當手術(shù)中需要完成更換器械的動作前后，需要采取中斷和重連的操作；情況三為當需要調(diào)整內(nèi)窺鏡位置時，也要求醫(yī)生采取與從機械臂的中斷操作。然而，連接中斷后，由于主操作手的位置或者姿態(tài)被人手調(diào)整，其在人眼坐標系下的坐標不再與從機械臂在內(nèi)窺鏡坐標系下的坐標一致，因此在重新建立與從機械臂的連接時，主從機械臂之間的運動映射發(fā)生了變化，為了保證跟隨效果，主操作手的位置和姿態(tài)需要根據(jù)從機械臂的位置和姿態(tài)進行輕微的調(diào)整，從而確保主從兩端運動映射的準確性和一致性。如果缺少這一調(diào)整環(huán)節(jié)，當重新建立連接時，從機械臂為了跟隨主操作手的運動和姿態(tài)，其姿態(tài)會跟隨主操作手的姿態(tài)發(fā)生突變。而此時從機械臂的末端器械仍處于病人的腹腔內(nèi)，這樣的突變很可能會造成手術(shù)失誤，引起嚴重的后果。由于該問題很難用精確的機器人動力學(xué)模型來描述，并且具有非線性特性，加上微創(chuàng)傷手術(shù)中對從機械臂末端器械的動作要求極其精微，傳統(tǒng)的PD關(guān)節(jié)控制器很難滿足其高精度的要求。例如醫(yī)生調(diào)整主手后與從機械臂重新建立連接，此時主手在人眼坐標系下的末端姿態(tài)為：RM=r11r12r13r21r22r23r31r32r33]]>而從機械臂在內(nèi)窺鏡坐標系下的末端姿態(tài)為：Rs=r′11r′12r′13r′21r′22r′23r′31r′32r′33]]>中斷重連時主操作手與從機械臂姿態(tài)不對應(yīng)會引發(fā)的從機械臂姿態(tài)突變問題。如圖1所示的控制系統(tǒng)，為基于前饋控制器和PD控制器的主操作手在主動控制狀態(tài)下的控制系統(tǒng)結(jié)構(gòu)框圖。主操作手的前饋補償環(huán)節(jié)1-1是對摩擦補償和慣性力補償，主操作手的控制環(huán)節(jié)采用PD控制器1-2，對位置偏差e及速度偏差進行負反饋控制，保證主從跟隨效果，飽和限制環(huán)節(jié)1-3是限制輸出力矩不大于允許的最大力矩。而在中斷重連時，主操作手需要根據(jù)從機械臂的姿態(tài)進行主動跟隨調(diào)整，由于PD控制本質(zhì)上是對偏差進行操作，當偏差為零時，控制作用也為零，在有摩擦和重力等非線性項作用的情況下，偏差不會為0，同時主操作手的伺服剛度一般較小，因此并不能夠很好地消除靜態(tài)誤差。而且當重建連接，從機械臂開始跟隨主操作手姿態(tài)時，由于靜態(tài)誤差的作用，從機械臂會發(fā)生位置和姿態(tài)的突變，在手術(shù)中容易造成危險的后果。為了更好地解決這個問題，避免手術(shù)安全事故的出現(xiàn)，本發(fā)明提出基于滑?？刂频臋C械臂控制系統(tǒng)，由于該控制系統(tǒng)無需精確的動力學(xué)模型，并且具有響應(yīng)快、精度高、魯棒性好的特點，因此可以有效地提高調(diào)整過程的系統(tǒng)性能，保證主操作手姿態(tài)能夠精準的跟隨從機械臂姿態(tài)，從而避免機械臂產(chǎn)生大的運動或者抖動，對整個機器人控制安全有非常重要的意義。后述的機械臂以主從機器人系統(tǒng)的主手為例，但本發(fā)明也適合于其他應(yīng)用場合中的機械臂及其控制方法，例如主從手術(shù)機器人病人端的機械臂(又稱“從手”)。下面結(jié)合具體實施例和附圖對本發(fā)明作進一步說明，在以下的描述中闡述了更多的細節(jié)以便于充分理解本發(fā)明，但是本發(fā)明顯然能夠以多種不同于此描述的其它方式來實施，本領(lǐng)域技術(shù)人員可以在不違背本發(fā)明內(nèi)涵的情況下根據(jù)實際應(yīng)用情況作類似推廣、演繹，因此不應(yīng)以此具體實施例的內(nèi)容限制本發(fā)明的保護范圍。圖2為本發(fā)明一實施例中的機械臂的控制系統(tǒng)的結(jié)構(gòu)框圖，主操作手基于前饋控制器和滑?？刂破魈幱诒粍诱{(diào)整狀態(tài)下。圖1、圖2所示的結(jié)構(gòu)相似，區(qū)別在于在正常的主從控制狀態(tài)下，圖2所示的結(jié)構(gòu)采用滑?？刂破?-2來提高主操作手的性能，達到高精度跟隨的控制目的，最終保證手術(shù)安全。在圖2中，前饋補償環(huán)節(jié)(前饋補償裝置)2-1利用位置信息qd和速度信息vd，來對受控主操作手的非線性耦合系統(tǒng)提供摩擦補償和慣性力補償，使控制器的控制效果更精確。本發(fā)明對前饋補償環(huán)節(jié)2-1沒有特別的要求，采用本領(lǐng)域現(xiàn)有技術(shù)皆可。系統(tǒng)檢測到的實際位置信號q和計算得到的理想位置信號qd通過第一疊加器求得該時刻的位置誤差信號e，由位置信號微分得來的實際速度信號v和理想速度信號vd通過第二疊加器求得該時刻的速度誤差信號兩個偏差信號輸入至滑模控制器2-2中進行處理生成滑?？刂菩盘枺c前饋補償2-1輸出的補償信號作疊加，即通過第三疊加器進行疊加，隨后輸入到飽和限制環(huán)節(jié)(限制器)2-3中。飽和限制環(huán)節(jié)2‐3是用來對控制器輸出的力矩命令進行限制，比如限制其輸出力矩不超出電機可提供的最大力矩，或限制其輸出力矩不大于該關(guān)節(jié)允許的最大力矩等。如圖3所示的是圖2中控制系統(tǒng)中滑?？刂破鞯慕Y(jié)構(gòu)框圖。在模塊3‐1中建立主操作手的動力學(xué)模型：H(q)q··+C(q,q·)q·+G(q)=τ---(1)]]>其中，H是n×n的對稱正定矩陣，稱為關(guān)節(jié)空間慣性矩陣，是關(guān)節(jié)位置q相關(guān)的函數(shù)；C是n×n的矩陣，與離心力和科氏力相關(guān)，是關(guān)節(jié)位置q與關(guān)節(jié)速度的函數(shù)；G是重力項的矢量；τ是控制律的輸出，即控制律的操作對象的輸入；n為機械臂的關(guān)節(jié)數(shù)。在模塊3‐2中，建立滑模面環(huán)節(jié)3-2，其是用于接收輸入的位置誤差信號e和速度誤差信號并設(shè)計一個與e和相關(guān)的函數(shù)D，來計算關(guān)節(jié)信息在狀態(tài)空間中到滑模面的距離，狀態(tài)空間的定義是以n個狀態(tài)變量作為基底組成的n維空間。其中，狀態(tài)變量是指一組能確定系統(tǒng)在時間域中的行為或運動信息的變量，在本發(fā)明的實施例中可以用位置偏差e和速度偏差作為兩個狀態(tài)變量，由此就可定義一個二維的狀態(tài)空間。而控制的目的是消除偏差，使位置偏差e和速度偏差均為零，也就是讓該狀態(tài)空間的兩個狀態(tài)變量在滑?？刂破鞯淖饔孟碌竭_次此狀態(tài)空間的原點。選取到滑模面的距離函數(shù)D如下：D=e+λe·+μeγ---(2)]]>其中，λ,μ和γ都是可根據(jù)相應(yīng)機械臂的系統(tǒng)要求進行選擇的參數(shù)，一般可讓λ>0,μ>0,1<γ<2，e是位置偏差，在本發(fā)明的一實施例中，λ＝2,μ＝1,γ＝1.5。在模塊3‐3中的快速趨近律環(huán)節(jié)用于接收到滑模面的距離函數(shù)D，并提供一個快速趨近律u，使得狀態(tài)空間中的兩個狀態(tài)變量到滑模面的距離，即關(guān)節(jié)的位置偏差和速度偏差均趨向于零。這是通過設(shè)計控制函數(shù)，保證系統(tǒng)在滑模面上運行?？焖仝吔蓇是一個與距離函數(shù)D的絕對值相關(guān)的，適用于主操作手的單調(diào)函數(shù)，可選擇為：u＝-ηsign(D)-ρD(3)其中，η和ρ是可根據(jù)相應(yīng)機械臂的系統(tǒng)要求進行選擇的參數(shù)，可讓η>0，ρ>0。在本發(fā)明的一實施例中，η＝3，ρ＝1。為了消除或減弱系統(tǒng)處于滑動模態(tài)時的震顫，可以用如下的飽和函數(shù)sat(D)替換上式中的符號函數(shù)sign(D)：sat(D)=+1,D>δDδ,|D|≤δ-1,D<-δ---(4)]]>其中δ>0是切換邊界。在模塊3‐4中的滑?？刂破鳝h(huán)節(jié)是利用模塊3‐1中的動力學(xué)模型和模塊3‐3中的快速趨近律得到的控制函數(shù)，計算出可應(yīng)用于該主操作手的完整的滑?？刂坡桑?tau;=H(q)[-ηsign(D)-ρD]+C(q,q·)q·+G(q)---(5)]]>在模塊3‐5中限制環(huán)節(jié)對應(yīng)于圖2中的飽和限制環(huán)節(jié)2-3，是對模塊3-4輸出的力矩進行飽和限制，目的是限制其輸出力矩不超出電機可提供的最大力矩，或限制其輸出力矩不大于該關(guān)節(jié)允許的最大力矩等。該環(huán)節(jié)的輸出信號τsat，為最終的輸出力矩指令，輸出信號至作用于機器人主操作手各個關(guān)節(jié)的伺服電機，驅(qū)動各關(guān)節(jié)達到理想位置。圖4所示為本發(fā)明的一實施例中，用于主操作手的滑?？刂品椒ǖ恼w流程圖。如圖所示，首先，在步驟4-1中建立主操作手的動力學(xué)模型，具體動力學(xué)方程見公式(1)，可由剛性機械臂的歐拉‐拉格朗日方程推導(dǎo)得出。在步驟4-2中，關(guān)節(jié)在狀態(tài)空間中的位置到滑模面的距離函數(shù)D被計算得出，此距離函數(shù)的不是唯一的，其中一種見公式(2)。在步驟4-3中，根據(jù)步驟4-2中的距離函數(shù)D，可計算得出使主操作手運行至滑模面快速趨近律，其設(shè)計方法可是應(yīng)用的機器人系統(tǒng)而定，其中一種方案見公式(3)。隨后在步驟4-4中，設(shè)計得到的快速趨近律被帶入動力學(xué)方程，從而得到應(yīng)用于機器人系統(tǒng)完整的滑?？刂坡?，即公式(5)，求得系統(tǒng)為達到理想控制效果所需要的輸出力矩。由于該結(jié)果只由計算得出，可能不完全適合當前系統(tǒng)采用的電機、負載或應(yīng)用場景等，因此在步驟4‐5中對上一步輸出的力矩加以限制，防止其超出系統(tǒng)期望的力矩。最后，在步驟4-6中，控制系統(tǒng)的最終的力矩指令被輸出，作用于主操作手的各個關(guān)節(jié)，達到期望的控制目的。通過以上控制策略，主操作手的控制系統(tǒng)的跟蹤性能得到提升，中斷重連過程中從機械臂姿態(tài)突變的情況可以被有效地避免，手術(shù)的安全性得到了提高。本發(fā)明雖然以較佳實施例公開如上，但其并不是用來限定本發(fā)明，任何本領(lǐng)域技術(shù)人員在不脫離本發(fā)明的精神和范圍內(nèi)，都可以做出可能的變動和修改。因此，凡是未脫離本發(fā)明技術(shù)方案的內(nèi)容，依據(jù)本發(fā)明的技術(shù)實質(zhì)對以上實施例所作的任何修改、等同變化及修飾，均落入本發(fā)明權(quán)利要求所界定的保護范圍之內(nèi)。當前第1頁1 2 3