基于慣性測(cè)量單元的機(jī)械臂及其控制方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種基于慣性測(cè)量單元的機(jī)械臂控制方法,尤其是通過(guò)慣性測(cè)量單元 測(cè)量控制者手臂姿態(tài)來(lái)控制機(jī)械臂運(yùn)動(dòng)的機(jī)械臂控制方法。
【背景技術(shù)】
[0002] 目前,現(xiàn)有的機(jī)械臂一般通過(guò)編程控制,或者使用按鈕、搖桿等外部設(shè)備進(jìn)行控 制。前者多用于工業(yè)生產(chǎn)中完成重復(fù)性動(dòng)作;后者主要用于有人操作的機(jī)械臂,如微創(chuàng)手術(shù) 機(jī)器人、排爆機(jī)器人等,操作人員通過(guò)控制方法發(fā)送動(dòng)作指令,控制機(jī)械臂的運(yùn)動(dòng)。但是使 用按鈕、搖桿等方法控制機(jī)械臂,需要經(jīng)過(guò)專(zhuān)業(yè)訓(xùn)練,操作難度大、控制效率低。
【發(fā)明內(nèi)容】
[0003] 為了克服現(xiàn)有機(jī)械臂控制方法操作難度大、控制效率低的不足,本發(fā)明提供一種 基于慣性測(cè)量單元的機(jī)械臂及其控制方法,該控制方法使用慣性測(cè)量單元感知操作人員的 手臂動(dòng)作,經(jīng)過(guò)計(jì)算后對(duì)機(jī)械臂發(fā)送控制指令,操作人員手臂動(dòng)作直接映射到機(jī)械臂上,操 作簡(jiǎn)單、直觀、效率高。
[0004] 本發(fā)明解決其技術(shù)問(wèn)題所采用的技術(shù)方案如下: 一種基于慣性測(cè)量單元的機(jī)械臂,包括機(jī)械臂和機(jī)械臂控制器, 所述的機(jī)械臂控制器包括慣性測(cè)量單元和數(shù)據(jù)融合計(jì)算單元,在操作人員的上臂、小 臂和手腕上分別安裝慣性測(cè)量單元,所述的慣性測(cè)量單元包括磁阻傳感器、加速度計(jì)、角速 率陀螺儀,能夠分別測(cè)量載體的航向角、加速度、運(yùn)動(dòng)角速率,慣性測(cè)量單元將測(cè)量數(shù)據(jù)輸 入到數(shù)據(jù)融合計(jì)算單元,數(shù)據(jù)融合計(jì)算單元解算后的載體姿態(tài)角數(shù)據(jù)輸出到所述的機(jī)械 臂。
[0005] 所述的機(jī)械臂使用舵機(jī)作為驅(qū)動(dòng)動(dòng)力,具有6個(gè)自由度,其中腕關(guān)節(jié)舵機(jī)、小臂旋 轉(zhuǎn)舵機(jī)、小臂彎曲舵機(jī)、上臂旋轉(zhuǎn)舵機(jī)、肩關(guān)節(jié)彎曲舵機(jī)、肩關(guān)節(jié)旋轉(zhuǎn)舵機(jī)依次相連,兩個(gè)肩 關(guān)節(jié)舵機(jī)固定于底座上。
[0006] 所述的數(shù)據(jù)融合姿態(tài)計(jì)算單元為ATmega8單片機(jī)。
[0007] -種所述的機(jī)械臂的控制方法,所述的慣性測(cè)量單元測(cè)量操作人員手臂的姿態(tài), 然后將姿態(tài)數(shù)據(jù)發(fā)送到數(shù)據(jù)融合計(jì)算單元,計(jì)算出機(jī)械臂各電機(jī)的旋轉(zhuǎn)角度,然后發(fā)送到 機(jī)械臂的電機(jī)控制器,控制機(jī)械臂做出與操作人員手臂動(dòng)作相同的動(dòng)作。
[0008] 所述的數(shù)據(jù)融合計(jì)算單元采用計(jì)算空間向量夾角的方法來(lái)得到載體姿態(tài)角數(shù)據(jù), 使用姿態(tài)余弦矩陣來(lái)表示載體坐標(biāo)系各軸相對(duì)導(dǎo)航坐標(biāo)系的角度,根據(jù)加速度計(jì)和磁阻傳 感器的數(shù)據(jù),修正陀螺儀數(shù)據(jù)抖動(dòng)產(chǎn)生的角度偏移。
[0009] 所述的機(jī)械臂使用舵機(jī)作為驅(qū)動(dòng)動(dòng)力,具有6個(gè)自由度,其中腕關(guān)節(jié)舵機(jī)、小臂旋 轉(zhuǎn)舵機(jī)、小臂彎曲舵機(jī)、上臂旋轉(zhuǎn)舵機(jī)、肩關(guān)節(jié)彎曲舵機(jī)、肩關(guān)節(jié)旋轉(zhuǎn)舵機(jī)依次相連,兩個(gè)肩 關(guān)節(jié)舵機(jī)固定于底座上。
[0010] 本發(fā)明的有益效果: 使用本發(fā)明方法控制機(jī)械臂,機(jī)械臂跟隨操作人員的手臂動(dòng)作而運(yùn)動(dòng),操作人員的動(dòng) 作直接表現(xiàn)在機(jī)械臂的運(yùn)動(dòng)上,這種控制方法相較于使用按鍵、搖桿等方法更直觀,操作難 度低,控制效率高,即使沒(méi)有經(jīng)過(guò)專(zhuān)業(yè)訓(xùn)練,也能完成對(duì)機(jī)械臂的控制。
[0011] 通過(guò)慣性測(cè)量單元測(cè)量操作人員手臂的運(yùn)動(dòng),直接控制機(jī)械臂運(yùn)動(dòng),機(jī)械臂動(dòng)作 與操作人員手臂動(dòng)作一致,操作難度低、控制效率高。
【附圖說(shuō)明】
[0012] 下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明進(jìn)一步說(shuō)明; 圖1是慣性測(cè)量單元硬件架構(gòu)示意圖; 圖2是六自由度機(jī)械臂結(jié)構(gòu)與電機(jī)設(shè)置部位示意圖; 圖3是人手臂在西北天地理坐標(biāo)中的抽象化示意圖; 圖4是機(jī)械臂控制器與機(jī)械臂硬件架構(gòu)。
【具體實(shí)施方式】
[0013] 圖1為本發(fā)明一個(gè)實(shí)施例的慣性測(cè)量單元硬件架構(gòu)示意圖,慣性測(cè)量單元集成了 磁阻傳感器、加速度計(jì)、角速率陀螺儀,能夠測(cè)量載體的航向角、載體加速度、載體運(yùn)動(dòng)角速 率,傳感器將測(cè)量數(shù)據(jù)通過(guò)數(shù)據(jù)接口輸入到數(shù)據(jù)融合解算單元,在數(shù)據(jù)融合解算單元中,通 過(guò)對(duì)三個(gè)傳感器數(shù)據(jù)進(jìn)行數(shù)據(jù)融合,計(jì)算后的載體姿態(tài)角數(shù)據(jù)輸出,比如通過(guò)串口或者其 他已知的方式;本實(shí)施例可以使用ATmegaS單片機(jī)作為數(shù)據(jù)融合姿態(tài)解算單元,通過(guò)I 2C接 口讀取角速率陀螺儀、加速度計(jì)、磁阻傳感器的數(shù)據(jù),使用姿態(tài)余弦矩陣來(lái)表示載體坐標(biāo)系 各軸相對(duì)導(dǎo)航坐標(biāo)系的角度,使用加速度計(jì)和磁阻傳感器,修正陀螺儀數(shù)據(jù)抖動(dòng)產(chǎn)生的角 度偏移。
[0014] 本發(fā)明中的機(jī)械臂結(jié)構(gòu)設(shè)計(jì)可以采用如圖2所示,使用舵機(jī)作為驅(qū)動(dòng)動(dòng)力,具有6 個(gè)自由度,其中腕關(guān)節(jié)舵機(jī)1、小臂旋轉(zhuǎn)舵機(jī)2、小臂彎曲舵機(jī)3、上臂旋轉(zhuǎn)舵機(jī)4、肩關(guān)節(jié)彎 曲舵機(jī)5、肩關(guān)節(jié)旋轉(zhuǎn)舵機(jī)6依次相連,兩個(gè)肩關(guān)節(jié)舵機(jī)固定于底座7上。
[0015] 將慣性測(cè)量數(shù)據(jù)轉(zhuǎn)換成機(jī)械臂舵機(jī)角度的方法,可以采用了計(jì)算空間向量夾角的 方法來(lái)實(shí)現(xiàn)。如圖3所示,本實(shí)施例將人手臂的上臂、小臂、手掌以肩關(guān)節(jié)為原點(diǎn)0,肘關(guān)節(jié) 為C點(diǎn),腕關(guān)節(jié)為B點(diǎn),手指前端為A點(diǎn),抽象成0C、CB、BA三個(gè)首尾相連、長(zhǎng)度為1個(gè)單位 的線段,通過(guò)慣性測(cè)量單元可以測(cè)得三線段在坐標(biāo)系中的俯仰角Θ、偏航角φ、滾轉(zhuǎn)角γ, 使用Θ和φ通過(guò)式1.1可得三線段的空間向量〇C、CB、BA。使用式1.2至1.7計(jì)算出Ml 至M6的6個(gè)機(jī)械臂舵機(jī)的旋轉(zhuǎn)角度。
【主權(quán)項(xiàng)】
1. 一種基于慣性測(cè)量單元的機(jī)械臂,其特征是,包括機(jī)械臂和機(jī)械臂控制器, 所述的機(jī)械臂控制器包括慣性測(cè)量單元和數(shù)據(jù)融合計(jì)算單元,在操作人員的上臂、小 臂和手腕上分別安裝慣性測(cè)量單元,所述的慣性測(cè)量單元包括磁阻傳感器、加速度計(jì)、角速 率陀螺儀,能夠分別測(cè)量載體的航向角、加速度、運(yùn)動(dòng)角速率,慣性測(cè)量單元將測(cè)量數(shù)據(jù)輸 入到數(shù)據(jù)融合計(jì)算單元,數(shù)據(jù)融合計(jì)算單元解算后的載體姿態(tài)角數(shù)據(jù)輸出到所述的機(jī)械 臂。
2. 根據(jù)權(quán)利要求1所述的機(jī)械臂,其特征是,所述的機(jī)械臂使用舵機(jī)作為驅(qū)動(dòng)動(dòng)力,具 有6個(gè)自由度,其中腕關(guān)節(jié)舵機(jī)(1 )、小臂旋轉(zhuǎn)舵機(jī)(2 )、小臂彎曲舵機(jī)(3 )、上臂旋轉(zhuǎn)舵機(jī) (4)、肩關(guān)節(jié)彎曲舵機(jī)(5 )、肩關(guān)節(jié)旋轉(zhuǎn)舵機(jī)(6 )依次相連,兩個(gè)肩關(guān)節(jié)舵機(jī)固定于底座(7 ) 上。
3. 根據(jù)權(quán)利要求1所述的機(jī)械臂,其特征是,所述的數(shù)據(jù)融合姿態(tài)計(jì)算單元為 ATmega8單片機(jī)。
4. 一種根據(jù)權(quán)利要求1所述的機(jī)械臂的控制方法,其特征是,所述的慣性測(cè)量單元測(cè) 量操作人員手臂的姿態(tài),然后將姿態(tài)數(shù)據(jù)發(fā)送到數(shù)據(jù)融合計(jì)算單元,計(jì)算出機(jī)械臂各電機(jī) 的旋轉(zhuǎn)角度,然后發(fā)送到機(jī)械臂的電機(jī)控制器,控制機(jī)械臂做出與操作人員手臂動(dòng)作相同 的動(dòng)作。
5. 根據(jù)權(quán)利要求4所述的機(jī)械臂的控制方法,其特征是,所述的數(shù)據(jù)融合計(jì)算單元采 用計(jì)算空間向量夾角的方法來(lái)得到載體姿態(tài)角數(shù)據(jù),使用姿態(tài)余弦矩陣來(lái)表示載體坐標(biāo)系 各軸相對(duì)導(dǎo)航坐標(biāo)系的角度,根據(jù)加速度計(jì)和磁阻傳感器的數(shù)據(jù),修正陀螺儀數(shù)據(jù)抖動(dòng)產(chǎn) 生的角度偏移。
6. 根據(jù)權(quán)利要求4所述的機(jī)械臂的控制方法,其特征是,所述的機(jī)械臂使用舵機(jī)作為 驅(qū)動(dòng)動(dòng)力,具有6個(gè)自由度,其中腕關(guān)節(jié)舵機(jī)(1)、小臂旋轉(zhuǎn)舵機(jī)(2)、小臂彎曲舵機(jī)(3)、上 臂旋轉(zhuǎn)舵機(jī)(4 )、肩關(guān)節(jié)彎曲舵機(jī)(5 )、肩關(guān)節(jié)旋轉(zhuǎn)舵機(jī)(6 )依次相連,兩個(gè)肩關(guān)節(jié)舵機(jī)固定 于底座(7)上。
【專(zhuān)利摘要】本發(fā)明公開(kāi)了一種基于慣性測(cè)量單元的機(jī)械臂及其控制方法,包括機(jī)械臂和機(jī)械臂控制器,所述的機(jī)械臂控制器包括慣性測(cè)量單元和數(shù)據(jù)融合計(jì)算單元,在操作人員的上臂、小臂和手腕上分別安裝慣性測(cè)量單元,所述的慣性測(cè)量單元包括磁阻傳感器、加速度計(jì)、角速率陀螺儀,能夠分別測(cè)量載體的航向角、加速度、運(yùn)動(dòng)角速率,慣性測(cè)量單元將測(cè)量數(shù)據(jù)輸入到數(shù)據(jù)融合計(jì)算單元,數(shù)據(jù)融合計(jì)算單元解算后的載體姿態(tài)角數(shù)據(jù)輸出到所述的機(jī)械臂。本發(fā)明通過(guò)慣性測(cè)量單元測(cè)量操作人員手臂的運(yùn)動(dòng),直接控制機(jī)械臂運(yùn)動(dòng),機(jī)械臂動(dòng)作與操作人員手臂動(dòng)作一致,操作難度低、控制效率高。
【IPC分類(lèi)】B25J9-18
【公開(kāi)號(hào)】CN104772756
【申請(qǐng)?zhí)枴緾N201510037521
【發(fā)明人】遲天陽(yáng), 劉力上, 董利達(dá)
【申請(qǐng)人】杭州師范大學(xué)
【公開(kāi)日】2015年7月15日
【申請(qǐng)日】2015年1月26日