力控牽引和擺位的多自由度機(jī)械臂控制裝置及其控制方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及多自由度機(jī)械臂控制技術(shù)，具體設(shè)及力控牽引和擺位的多自由度機(jī)械 臂控制裝置及其控制方法。
【背景技術(shù)】
[0002] 多自由度機(jī)械臂W其定位重復(fù)精度高、可編程、易操作等優(yōu)點(diǎn)，已在工業(yè)設(shè)備制 造、航空航天、醫(yī)療衛(wèi)生等領(lǐng)域得到廣泛應(yīng)用。工業(yè)多自由度機(jī)械臂系統(tǒng)常用的控制方法有 兩種:一是采用"示教模式"，即操作人員首先通過機(jī)械臂教導(dǎo)器控制機(jī)械臂末端(常安裝有 工具)沿一定的工作軌跡運(yùn)動(dòng)，并使機(jī)械臂控制器W預(yù)先確定的采樣頻率"記錄"工作軌跡 中每個(gè)采樣軌跡點(diǎn)對(duì)應(yīng)的機(jī)械臂運(yùn)動(dòng)狀態(tài)(包括瞬時(shí)速度、瞬時(shí)加速度、各關(guān)節(jié)的瞬時(shí)角度 信息等），在實(shí)際控制中機(jī)械臂控制器W預(yù)先選定的插補(bǔ)方式控制機(jī)械臂末端依次通過上 述采樣軌跡點(diǎn)，復(fù)現(xiàn)示教時(shí)的工作運(yùn)動(dòng)軌跡，完成相應(yīng)的工作任務(wù);二是采用"離線編程模 式"，即在機(jī)械臂控制器中按照特定工作任務(wù)要求預(yù)先寫入控制程序，在實(shí)際工況中機(jī)械臂 將按照程序中的步驟單步循環(huán)運(yùn)行，實(shí)現(xiàn)預(yù)定工作任務(wù)。
[0003] 在一些應(yīng)用場(chǎng)景中，有時(shí)需要機(jī)械臂末端能夠按照操作人員的意愿方便地、實(shí)時(shí) 地調(diào)整空間位姿，例如醫(yī)療應(yīng)用中的機(jī)械臂扶持、康復(fù)訓(xùn)練應(yīng)用中的人機(jī)交互。在運(yùn)些應(yīng)用 中一方面在操作人員進(jìn)行機(jī)械臂控制時(shí)，通過對(duì)末端工具施加作用力實(shí)現(xiàn)對(duì)末端工具的牽 引操作;另一方面在無(wú)外部控制輸入時(shí)，機(jī)械臂能夠克服自身及其工具重力等因素影響，保 持當(dāng)前位姿，實(shí)現(xiàn)精確定位。另外，在預(yù)先給定機(jī)械臂末端目標(biāo)點(diǎn)位姿信息時(shí)，有時(shí)候?yàn)榱?機(jī)械臂緊急避障等目的，需要機(jī)械臂在運(yùn)動(dòng)過程中也能夠通過外力控制實(shí)時(shí)更新運(yùn)動(dòng)軌 跡。
[0004] 現(xiàn)有的機(jī)械臂控制系統(tǒng)絕大多數(shù)不能實(shí)現(xiàn)外力作用下控制機(jī)械臂的運(yùn)動(dòng)，而只能 通過教導(dǎo)模式與離線編程模式兩種控制方法預(yù)先規(guī)劃好機(jī)械臂運(yùn)動(dòng)軌跡。在上述應(yīng)用場(chǎng)景 中，應(yīng)用兩類控制模式實(shí)現(xiàn)機(jī)械臂控制所存在的不足主要體現(xiàn)在：
[0005] 1.示教模式下，特定工作任務(wù)中要求操作人員控制機(jī)械臂對(duì)部分位姿點(diǎn)精確到 達(dá)，同時(shí)也要考慮到各種可能引發(fā)安全隱患的因素，因此示教軌跡選取W及示教操作過程 較為費(fèi)時(shí)。離線編程模式下，前期需要將工作任務(wù)轉(zhuǎn)化為機(jī)器程序，后期根據(jù)實(shí)際工況條件 會(huì)有多次的程序修改與運(yùn)行調(diào)試，系統(tǒng)維護(hù)成本也較高。
[0006] 2.當(dāng)機(jī)械臂末端工作軌跡改變時(shí)，往往需要操作人員對(duì)運(yùn)動(dòng)軌跡進(jìn)行重新示教或 者預(yù)編程，也使多自由度機(jī)械臂在個(gè)性化方案應(yīng)用方面受到了極大限制。
[0007] 3.機(jī)械臂在運(yùn)行過程中，無(wú)法通過檢測(cè)外部作用力做出相應(yīng)控制指令的調(diào)整，因 此無(wú)法實(shí)現(xiàn)力控牽引功能;在機(jī)械臂運(yùn)動(dòng)軌跡中意外遇到障礙物時(shí)，無(wú)法通過外力作用實(shí) 時(shí)更新機(jī)械臂的運(yùn)動(dòng)軌跡，容易造成意外事故。
[000引專利CN103347659A提出了一種傳感器中繼控制裝置，能夠通過對(duì)力傳感器信息進(jìn) 行解算控制機(jī)械臂各關(guān)節(jié)電動(dòng)機(jī)的運(yùn)動(dòng)，但是也不能實(shí)現(xiàn)對(duì)機(jī)械臂系統(tǒng)的力控牽引功能。

【發(fā)明內(nèi)容】

[0009] 基于W上現(xiàn)有技術(shù)中多自由度機(jī)械臂控制中存在的問題，本發(fā)明提出了一種力控 牽引和擺位的多自由度機(jī)械臂控制裝置及其控制方法。
[0010] 本發(fā)明的一個(gè)目的在于提出一種力控牽引和擺位的多自由度機(jī)械臂控審曠置。
[0011] 本發(fā)明的力控牽引和擺位的多自由度機(jī)械臂控制裝置包括:上位機(jī)、多自由度機(jī) 械臂系統(tǒng)和六自由度力傳感器系統(tǒng);其中，多自由度機(jī)械臂系統(tǒng)包括多自由度機(jī)械臂和機(jī) 械臂控制器;六自由度力傳感器系統(tǒng)包括力信號(hào)傳感器和力信號(hào)控制器;上位機(jī)包括力信 號(hào)控制器通訊模塊、末端工具重力補(bǔ)償模塊、力-位及力-速控制算法模塊、位姿坐標(biāo)解算模 塊、機(jī)械臂控制模塊和圖形用戶界面模塊;上位機(jī)分別與機(jī)械臂控制器和力信號(hào)控制器聯(lián) 接;力信號(hào)傳感器具有相對(duì)且平行的安裝平面和工具平面，安裝平面通過安裝連接件固定 在機(jī)械臂末端，在工具平面上安裝末端工具，保持末端工具與力信號(hào)傳感器之間的相對(duì)固 定;機(jī)械臂末端坐標(biāo)系巧nd}的Z軸與力信號(hào)傳感器坐標(biāo)系{FT-Sensor}的Z軸共線且同向， 機(jī)械臂末端坐標(biāo)系巧nd}的Y軸與力信號(hào)傳感器坐標(biāo)系{FT-Sensor}的Y軸平行;外部作用力 作用在末端工具上，力信號(hào)傳感器檢測(cè)到外力，外力包括外部作用力和末端工具的重力，將 外力信號(hào)轉(zhuǎn)化為模擬電信號(hào)傳輸至力信號(hào)控制器;力信號(hào)控制器將模擬信號(hào)轉(zhuǎn)化為數(shù)字信 號(hào)后傳輸至上位機(jī);上位機(jī)的力信號(hào)控制器通訊模塊接收數(shù)字信號(hào)得到外力的值，末端工 具重力補(bǔ)償模塊解算出實(shí)際作用在力信號(hào)傳感器上的外部作用力沿力信號(hào)傳感器坐標(biāo)系 IFT-SensoHS軸的分力與分力矩;力-位及力-速控制算法模塊將外部作用力的信息轉(zhuǎn)化 為在力信號(hào)傳感器坐標(biāo)系{FT-Sensor}下的機(jī)械臂末端的位姿偏移量與速度值;位姿坐標(biāo) 解算模塊將在力信號(hào)傳感器坐標(biāo)系{FT-Sensor}的機(jī)械臂末端的位姿偏移量與速度值轉(zhuǎn)換 成在機(jī)械臂基坐標(biāo)系{Robot}下的機(jī)械臂末端的位姿偏移量與速度值，并將當(dāng)前采樣時(shí)刻 解算得到的位姿偏移量與當(dāng)前采樣時(shí)刻的預(yù)期位姿坐標(biāo)值進(jìn)行矢量相加，得到機(jī)械臂末端 的下一采樣時(shí)刻在機(jī)械臂基坐標(biāo)系{Robot}下的預(yù)期位姿坐標(biāo)值;機(jī)械臂控制模塊按照機(jī) 械臂末端的下一采樣時(shí)刻在機(jī)械臂基坐標(biāo)系{Robot}下的預(yù)期位姿坐標(biāo)值控制機(jī)械臂末端 運(yùn)動(dòng)，帶動(dòng)末端工具到達(dá)預(yù)期指定的位姿，實(shí)現(xiàn)基于力控牽引和擺位控制。
[0012] 多自由度機(jī)械臂系統(tǒng)包括多自由度機(jī)械臂和機(jī)械臂控制器;多自由度機(jī)械臂具有 多個(gè)移動(dòng)副或旋轉(zhuǎn)副組成的串聯(lián)結(jié)構(gòu)。機(jī)械臂控制器包括狀態(tài)檢測(cè)模塊和運(yùn)動(dòng)控制模塊。
[0013] 六自由度力傳感器系統(tǒng)包括能夠檢測(cè)六自由度力/力矩分量信息的力信號(hào)傳感器 W及能夠處理六自由度力/力矩信息的力信號(hào)控制器。力信號(hào)控制器包括模數(shù)轉(zhuǎn)換模塊和 偏置校正模塊。
[0014] 機(jī)械臂末端具有圓環(huán)狀的法蘭盤，法蘭盤上設(shè)置有法蘭盤定位銷槽。力信號(hào)傳感 器與機(jī)械臂末端相對(duì)的面為安裝平面，安裝平面具有圓環(huán)，圓環(huán)上設(shè)置有圓環(huán)定位銷槽;力 信號(hào)傳感器與末端工具相對(duì)的面為工具平面，在工具平面上直接固定安裝末端工具。安裝 連接件為可拆卸連接件，具有相對(duì)且平行的機(jī)械臂末端安裝面和力信號(hào)傳感器安裝面;機(jī) 械臂末端安裝面與機(jī)械臂末端固定并唯一確定安裝連接件的方向；力信號(hào)傳感器安裝面與 力信號(hào)傳感器固定并唯一確定力信號(hào)傳感器的安裝方向。機(jī)械臂末端安裝面的中屯、具有機(jī) 械臂末端安裝面定中屯、凸臺(tái)，邊緣設(shè)置機(jī)械臂末端安裝面定位銷槽;定中屯、凸臺(tái)嵌在法蘭 盤的圓環(huán)中，法蘭盤定位銷槽與機(jī)械臂末端安裝面定位銷槽通過定位銷對(duì)齊并固定;力信 號(hào)傳感器安裝面的中屯、具有力信號(hào)傳感器安裝面定中屯、凸臺(tái)，邊緣設(shè)置力信號(hào)傳感器安裝 面定位銷槽;力信號(hào)傳感器安裝面定中屯、凸臺(tái)嵌在力信號(hào)傳感器的圓環(huán)中，力信號(hào)傳感器 上的圓環(huán)定位銷槽與力信號(hào)傳感器安裝面定位銷槽通過定位銷對(duì)齊并固定。定中屯、凸臺(tái)的 中屯、點(diǎn)與定位銷的連線分別平行于機(jī)械臂末端坐標(biāo)系的Y軸和力信號(hào)傳感器坐標(biāo)系{FT-Sensor}的Y軸;安裝連接件通過定中屯、凸臺(tái)和定位銷，將機(jī)械臂末端與力信號(hào)傳感器連接， 機(jī)械臂末端坐標(biāo)系巧nd}的Z軸與力信號(hào)傳感器坐標(biāo)系{FT-Sensor}的Z軸共線且同向，機(jī)械 臂末端坐標(biāo)系巧nd}的Y軸與力信號(hào)傳感器坐標(biāo)系{FT-Sensor}的Y軸平行，并唯一確定安裝 方向；機(jī)械臂末端與力信號(hào)傳感器通過安裝連接件W唯一方式固定，使兩坐標(biāo)系XYZS軸兩 兩對(duì)應(yīng)平行，兩坐標(biāo)系原點(diǎn)連線與兩坐標(biāo)系Z軸=者共線且同向。
[0015] 本發(fā)明的另一個(gè)目的在于提出一種力控牽引和擺位的多自由度機(jī)械臂的控制方 法。
[0016] 本發(fā)明的力控牽引的多自由度機(jī)械臂控制方法，包括W下步驟：
[0017] 1)開機(jī)通電，上位機(jī)對(duì)力信號(hào)控制器通訊模塊、末端工具重力補(bǔ)償模塊、力-位及 力-速控制算法模塊、位姿坐標(biāo)解算模塊、機(jī)械臂控制模塊和圖形用戶界面模塊進(jìn)行初始 化，機(jī)械臂控制器與力信號(hào)控制器分別進(jìn)行初始化，上位機(jī)通過機(jī)械臂控制器使機(jī)械臂末 端移動(dòng)到初始位姿，在該位姿狀態(tài)下對(duì)力信號(hào)控制器進(jìn)行偏置校正；
[0018] 2)外部作用力作用在末端工具上，力信號(hào)傳感器檢測(cè)到外力，外力包括外部作用 力和末端工具的重力，將外力信號(hào)轉(zhuǎn)化為模擬電信號(hào)傳輸至力信號(hào)控制器；
[0019] 3)上位機(jī)向力信號(hào)控制器發(fā)送力信號(hào)請(qǐng)求指令，力信號(hào)控制器將模擬信號(hào)轉(zhuǎn)化為 數(shù)字信號(hào)后傳輸至上位機(jī)；
[0020] 4)上位機(jī)的力信號(hào)控制器通訊模塊接收數(shù)字信號(hào)得到外力的值；
[0021] 5)上位機(jī)向機(jī)械臂控制器發(fā)送機(jī)械臂狀態(tài)請(qǐng)求指令，讀取機(jī)械臂當(dāng)前位姿，并通 過末端工具重力補(bǔ)償模塊，采用末端工具重力補(bǔ)償算法，解算出實(shí)際作用在力信號(hào)傳感器 上的外部作用力沿力信號(hào)傳感器坐標(biāo)系{FT-Sensor}S軸的分力與分力矩；
[0022] 6)力-位及力-速控制算法模塊采用力-位姿偏移控制算法和力-速控制算法，將外 部作用力的信息轉(zhuǎn)化為在力信號(hào)傳感器坐標(biāo)系{FT-Sensor}下的機(jī)械臂末端的位姿偏移量 與速度值；
[0023] 7)位姿坐標(biāo)解算模塊采用位姿坐標(biāo)點(diǎn)解算算法，將在力信號(hào)傳感器坐標(biāo)系{FT-Sensor}的機(jī)械臂末端的位姿偏移量轉(zhuǎn)換成在機(jī)械臂基坐標(biāo)系{Robot}下的機(jī)械臂末端的 位姿偏移量，用位姿偏移量修正當(dāng)前采樣時(shí)刻的預(yù)期位姿坐標(biāo)值，得到機(jī)械臂末端的下一 采樣時(shí)刻在機(jī)械臂基坐標(biāo)系{Robot}下的預(yù)期位姿坐標(biāo)值；
[0024] 8)機(jī)械臂控制模塊按照機(jī)械臂末端的下一采樣時(shí)刻在機(jī)械臂基坐標(biāo)系{Robot}下 的預(yù)期位姿坐標(biāo)值控制機(jī)械臂末端運(yùn)動(dòng)，帶動(dòng)末端工具到達(dá)預(yù)期指定的位姿，實(shí)現(xiàn)力控牽 引控制。
[0025] 其中，在步驟5)中，末端工具重力補(bǔ)償算法具體包括:在每一采樣時(shí)刻下，上位機(jī) 通過機(jī)械臂控制器得到機(jī)械臂