[發(fā)明專利]一種三自由度轉(zhuǎn)動手控器及其控制方法

有效

• 申請?zhí)枺?/td>: 202110377517.2
• 申請日: 2021-04-08
• 公開（公告）號: CN113103263B
• 公開（公告）日: 2021-12-07
• 發(fā)明（設(shè)計）人: 張得禮;王志國;周星雨;張宇;張文奇;韓亮亮;楊健;楊永常;谷程鵬;卓新;胡雪平;王苑
• 申請（專利權(quán)）人: 南京航空航天大學(xué);上海宇航系統(tǒng)工程研究所
• 主分類號: B25J13/02
• 分類號: B25J13/02;A61B34/30
• 代理公司: 南京天華專利代理有限責(zé)任公司 32218
• 代理人: 莫英妍

1.一種三自由度轉(zhuǎn)動手控器，其特征在于，包括第一驅(qū)動裝置、第二驅(qū)動裝置、第三驅(qū)動裝置、操作手柄(1)、力傳感器(2)、法蘭盤(4)和十字鉸支架(3)，法蘭盤(4)固定在操作臺上，力傳感器(2)安裝在法蘭盤(4)上，第一驅(qū)動裝置下端通過套筒(8)安裝在力傳感器(2)上，第一驅(qū)動裝置包括：Z軸電機(jī)(7)、套筒(8)、Z軸聯(lián)軸器(11)和Z軸軸承箱(9)，套筒(8)包圍在Z軸電機(jī)(7)外，Z軸電機(jī)(7)動力輸出端與Z軸聯(lián)軸器(11)的一端連接，Z軸聯(lián)軸器(11)外套裝有Z軸軸承箱(9)，Z軸軸承箱(9)與套筒(8)頂端連接，Z軸聯(lián)軸器(11)的另一端連接X軸節(jié)叉(10)的節(jié)叉桿，X軸節(jié)叉(10)的一個節(jié)叉頭套在X軸第二軸承箱(21)上，X軸第二軸承箱(21)安裝在十字鉸支架(3)的D端，X軸節(jié)叉(10)的另一個節(jié)叉頭套在X軸第一軸承箱(13)上，X軸第一軸承箱(13)安裝在十字鉸支架(3)的B端；使能按鈕(6)安裝在操作手柄(1)上，操作手柄(1)通過手柄桿(24)與Y軸節(jié)叉(16)固定連接；所述十字鉸支架(3)的四端順時針設(shè)置A、B、C、D端，A、B、C、D端均為階梯軸，十字鉸支架(3)A端連接Y軸節(jié)叉(16)，十字鉸支架(3)B端連接第二驅(qū)動裝置和X軸節(jié)叉(10)；十字鉸支架(3)C端連接第三驅(qū)動裝置和Y軸節(jié)叉(16)；十字鉸支架(3)D端與X軸節(jié)叉(10)連接；第二驅(qū)動裝置包括X軸電機(jī)(12)、X軸第一軸承箱(13)和X軸聯(lián)軸器(14)，X軸電機(jī)(12)的動力輸出端通過X軸聯(lián)軸器(14)與十字鉸支架(3)的B端連接，X軸聯(lián)軸器(14)外套裝有X軸第一軸承箱(13)，X軸第一軸承箱(13)外套有X軸節(jié)叉(10)的一個叉頭；第三驅(qū)動裝置包括Y軸電機(jī)(15)、Y軸第一軸承箱(22)和Y軸聯(lián)軸器(17)，Y軸電機(jī)(15)的動力輸出端通過Y軸聯(lián)軸器(17)與十字鉸支架(3)的C端連接，Y軸聯(lián)軸器(17)外套裝有Y軸第一軸承箱(22)，Y軸第一軸承箱(22)外套裝有Y軸節(jié)叉(16)的一個叉頭，Y軸節(jié)叉(16)的另一個叉頭套在Y軸第二軸承箱(23)上，Y軸第二軸承箱(23)安裝在十字鉸支架(3)的A端。

2.根據(jù)權(quán)利要求1所述的三自由度轉(zhuǎn)動手控器，其特征在于，所述力傳感器(2)為六位力傳感器。

3.根據(jù)權(quán)利要求1所述的三自由度轉(zhuǎn)動手控器，其特征在于，所述三自由度轉(zhuǎn)動手控器外側(cè)有防護(hù)罩(18)。

4.根據(jù)權(quán)利要求3所述的三自由度轉(zhuǎn)動手控器，其特征在于，所述使能按鈕(6)通過螺紋連接安裝在操作手柄(1)上，操作手柄(1)軸向開有螺紋盲孔，操作手柄(1)側(cè)面開有安裝使能按鈕(6)的螺紋孔，操作手柄(1)上的螺紋盲孔和側(cè)面的螺紋孔相連通；所述操作手柄(1)的螺紋盲孔與手柄桿(24)一端的外螺紋配合，操作手柄(1)通過螺紋連接固定在手柄桿(24)的一端，手柄桿(24)的另一端通過螺紋連接固定在Y軸節(jié)叉(16)的中部；手柄桿(24)從操作手柄(1)到Y(jié)軸節(jié)叉(16)的方向，依次在手柄桿(24)外側(cè)圓周上安裝有風(fēng)琴防護(hù)罩軸承(20)和防護(hù)罩軸承(25)，防護(hù)罩軸承(25)將手柄桿(24)和防護(hù)罩(18)連接在一起。

5.根據(jù)權(quán)利要求4所述的三自由度轉(zhuǎn)動手控器，其特征在于，塔型風(fēng)琴防護(hù)罩(19)小口端與風(fēng)琴防護(hù)罩軸承(20)連接，塔型風(fēng)琴防護(hù)罩(19)大口端與防護(hù)罩(18)通過螺釘連接。

6.根據(jù)權(quán)利要求1所述的三自由度轉(zhuǎn)動手控器，其特征在于，所述與十字鉸支架(3)A端連接的Y軸節(jié)叉(16)的節(jié)叉頭上安裝有配重裝置(5)，配重裝置(5)的重量與第二驅(qū)動裝置的重量相等。

7.根據(jù)權(quán)利要求1所述的三自由度轉(zhuǎn)動手控器的控制方法，其特征在于，包括以下步驟：

步驟一，接通電源將三自由度轉(zhuǎn)動手控器打開，將當(dāng)前位置作為操作手柄(1)的初始位置，從而完成操作手柄(1)的初始化；

步驟二，三自由度轉(zhuǎn)動手控器底部的力傳感器(2)讀取到X，Y，Z三個方向上的扭力τ_x、τ_y、τ_z，將讀取到的力數(shù)值通過濾波算法進(jìn)行平滑處理，并通過力控算法計算得到電機(jī)目標(biāo)位置，在X軸上的坐標(biāo)tarposition_x、在Y軸上的坐標(biāo)tarposition_y、在Z軸上的坐標(biāo)tarposition_z，下發(fā)給對應(yīng)的X軸電機(jī)(12)、Y軸電機(jī)(15)和Z軸電機(jī)(7),使電機(jī)產(chǎn)生與使用者施加給操作手柄(1)力方向相反的力，從而達(dá)到力反饋的效果，即

tarposition_x＝K_px·τ_x

tarposition_y＝K_py·τ_y

tarposition_z＝K_pz·τ_z

其中比例系數(shù)K_px、K_py、K_pz為手柄轉(zhuǎn)動到限位時期望力矩與對應(yīng)電機(jī)脈沖的比值；

步驟三，對目標(biāo)位置進(jìn)行平滑處理以防止手柄出現(xiàn)抖動現(xiàn)象，以10ms為一個周期，Δ_px.py.pz表示兩個周期之間可以接受的最大電機(jī)位置差，位置差數(shù)值在Δ_px.py.pz內(nèi)電機(jī)運動較為順滑，位置差數(shù)值在Δ_px.py.pz外電機(jī)運動頓挫感強(qiáng)抖動明顯，若上個計算周期的目標(biāo)位置與本計算周期的目標(biāo)位置的差小于最大值Δ_px.py.pz，即

|tarposition_x,y,z-tarposition′_x,y,z|Δ_px.py.pz

則本計算周期的目標(biāo)位置保持不變，其中tarposition_x,y,z為當(dāng)前目標(biāo)位置，tarposition′_x,y,z為上個計算周期目標(biāo)位置；

若上個計算周期的目標(biāo)位置與本計算周期的目標(biāo)位置的差大于最大值Δ_px.py.pz，即

|tarposition_x,y,z-tarposition′_x,y,z|Δ_px.py.pz

則令本計算周期的目標(biāo)位置等于上個計算周期的目標(biāo)位置加Δ_px.py.pz，即

tarposition_x.y.z＝tarposition′_x,y,z+Δ_px.py.pz；

步驟四，對手柄控制機(jī)器人下達(dá)速度指令，對機(jī)器人的笛卡爾空間姿態(tài)進(jìn)行加減速規(guī)劃，Δ_vx.vy.vz表示兩個周期之間可以接受的最大速度指令差，速度指令差在Δ_vx.vy.vz范圍內(nèi)，被控機(jī)器人運動較為平滑，速度指令差在Δ_vx.vy.vz范圍外，被控機(jī)器人可能在運動時會出現(xiàn)抖動的情況，若本周期的速度指令與上周期的速度指令的差值小于Δ_vx.vy.vz，即

|V_x,y,z-V′_x,y,z|Δ_vx.vy.vz

則本周期速度指令保持不變，其中V_x,y,z為當(dāng)前速度指令速度，V′_x,y,z為上個計算周期速度指令速度；

若本周期的速度指令與上個計算周期的速度指令的差值大于Δ_vx.vy.vz，即

|V_x,y,z-V′_x,y,z|Δ_vx.vy.vz

則令本周期速度指令等于上個計算周期速度指令加Δ_vx.vy.vz,即

V_x.y.z＝V′_x,y,z+Δ_vx.vy.vz；

步驟五，對實際速度指令進(jìn)行限速處理，當(dāng)實際速度指令大于最大速度，令實際速度指令等于最大速度；

步驟六，將最終計算出的實際速度指令下發(fā)給目標(biāo)裝置進(jìn)行控制。