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

【發(fā)明內容】

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