基于關節(jié)力的空間六維力測量方法
【技術領域】
[0001] 本發(fā)明涉及一種機器人的力和力矩測量方法，尤其涉及一種基于關節(jié)力的空間六 維力測量方法。
【背景技術】
[0002] 現(xiàn)有的工業(yè)、軍事中，廣泛運用到機器人，機器人在運動過程中所受到的六維力和 力矩測量要求越來越高，而且環(huán)境也越來越復雜，通過現(xiàn)有的測量技術已經(jīng)遠遠不能滿足 環(huán)境、頻率、耦合以及簡便等要求，比如：機器人在惡劣的環(huán)境中進行作業(yè)時，其末端與環(huán)境 接觸的地方產(chǎn)生的力或者力矩會對操作的運動軌跡、精度等產(chǎn)生重大影響，如將現(xiàn)有測量 方式中的力傳感器或速度傳感器安裝在操作末端上，由于末端所述的環(huán)境復雜多變，高溫 高壓或者強光燈極端環(huán)境將嚴重影響力傳感器或者速度傳感器的測量精度，同時對傳感器 提出了極高的要求，而且裝配復雜，操作準備的工作量大，然而，現(xiàn)有技術都需要借助上述 的傳感器來進行測量。
[0003] 因此，需要提出一種對機器人的六維力和力矩的測量方法，能夠在極端環(huán)境中對 機器人傳感器的六維力和力矩進行準確測量，并且能夠適應惡劣的工況環(huán)境，可靠性強。

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

[0004] 有鑒于此，本發(fā)明的目的是提供一種基于關節(jié)力的空間六維力測量方法，能夠在 極端環(huán)境中對機器人傳感器的六維力和力矩進行準確測量，并且能夠適應惡劣的工況環(huán) 境，可靠性強。
[0005] 本發(fā)明提供的一種基于關節(jié)力的空間六維力測量方法，包括上平臺和下平臺，所 述下平臺由六個電機分別獨立驅(qū)動的六個驅(qū)動桿與上平臺連接形成并聯(lián)機構，六個電機形 成六個關節(jié)，所述上平臺和下平臺分別具有六個連接點；包括如下步驟：
[0006] SL建立坐標系
[0007] 在上平臺建立工件坐標系oi-xlylzl,在下平臺建立基坐標系o-xyz，Bi和Ai分 別為上下平臺對應的六個連接點，其中：下平臺的各連接點在基坐標系中的向量為（Aix， Aiy，Aiz)，上平臺的各連接點在基坐標系中的向量表示為（Bix，Biy，Biz),在工件坐標系 中的向量表示為（bix，biy，biz),工件坐標系在基坐標系中的位置矢量為01，Ii為上平 臺和下平臺對應點的長度，上平臺相對于下平臺的位姿乂(1：)、7(1:)、2(1:)、0 !￡(1：)、0"1：)、 Qz(t)表示為 x、y、z、θχ、0y、θζ;
[0008] S2.運動學逆解
[0009] 當上平臺的位姿改變時，根據(jù)平面與平面上點的關系求出此時新點的坐標值Bi，
[0010] Bi= Rb i+Oi (I. I)
[0011] 其中，
[0012]
【主權項】
1. 一種基于關節(jié)力的空間六維力測量方法，其特征在于：包括上平臺和下平臺，所述 下平臺由六個電機分別獨立驅(qū)動的六個驅(qū)動桿與上平臺連接形成并聯(lián)結(jié)構，六個電機形成 六個關節(jié)，所述上平臺和下平臺分別具有六個連接點；包括如下步驟：
51. 建立坐標系 在上平臺的中心建立工件坐標系01-xlylzl,在下平臺的中心建立基坐標系〇-xyz，Bi 和Ai分別為上下平臺對應的六個連接點，其中：下平臺的各連接點在基坐標系中的向量為 (八11，417,412)，上平臺的各連接點在基坐標系中的向量表示為（1^1，1^7，1^2)，在工件坐 標系中的向量表示為（bix，biy，biz),工件坐標系在基坐標系中的位置矢量為01，Ii為上 平臺和下平臺對應點的長度，上平臺相對于下平臺的位姿x(t)、y(t)、z(t)、Θ x(t)、Θ y(t)、 Qz(t)表示為 x、y、z、θχ、0y、θζ;
52. 運動學逆解 當上平臺的位姿改變時，根據(jù)平面與平面上點的關系求出此時新點的坐標值Bi， Bi= Rb i+Oi (I. I) 其中，
R為上平臺姿態(tài)的旋轉(zhuǎn)矩陣，O1為上平臺上的工件坐標系相對于下平臺基坐標系中的 位移矢量，1^為B i點在工件坐標系中的位置矢量； 驅(qū)動桿在基坐標系o-xyz中的位置矢量為Li = B ^Ai = Rb ^O1-Ai，其中，（i = 1，2, · · ·，6)， 驅(qū)動桿的實時長度可表示稱被測運動問題位姿參數(shù)的函數(shù)：
Bpbi*別為上平臺頂點在基坐標系、工件坐標系中的位置向量，Ai為下平臺頂點在基 坐標系中的位置向量，將被測物體的六個位姿參數(shù)代入到（1.2)式中，可求得驅(qū)動桿的長 度；
53. 運動學正解 正解方程由逆解方程變化而來： Ii2= (Bi-Ai) (Bi-Ai)T，其中Bi, 別表示上平臺和下平臺的連接點在基坐標系中的坐 標； fi (Bi) = fi (x, y, ζ, θ χ, Θ y, Θ z) = (Rb^O1-Ai) (Rb^O1-Ai) ^li2= 0, i = I, 2,. . . , 6 (1.3)； 首先令叉1，叉2，叉3，叉4，叉5，叉 6=(叉，7,2,0!￡，0 7，02)且初始點 (x，y，ζ，θ χ，Θ y，θ z) = (〇, 〇, 〇, 〇, 〇, 〇)，然后將 & (Bi) (i = 1，2, · · ·，6)在 &附近進行 Taylor展開，取其一階線性部分可得
式（1.4)中為Xi的線性方程組，對應的系數(shù)矩陣為J1且系數(shù)矩陣對應的第i行第j 列元素為通過求系數(shù)矩陣1的逆矩陣以及解方程組即可求得上平臺的位
J， 姿； S4.力Jacobi矩陣的求解 機構的速度Jacobi矩陣J為： J= [eT (RbXe)T1 (1.5) 對于六個關節(jié)驅(qū)動力（或力矩）組成的關節(jié)矢量為 τ = [^1, τ2, τ3, τ4, τ5, τ6]Τ 關節(jié)矢量與運動平臺的廣義操作力矢量F = [f\，f2, f3, f4, f5, f6]T具有如下關系： τ =JT(q)F (1.6) 力Jacobi矩陣土是靜平衡狀態(tài)下，關節(jié)力向操作力映射的線性關系，即F = Jft，則
2. 根據(jù)權利要求1所述基于基于關節(jié)力的空間六維力測量方法，其特征在于：所述測 量方法還包括步驟S5:關節(jié)電機驅(qū)動電壓測量與驅(qū)動力的計算： 六個電機共同驅(qū)動上平臺運動，電機輸出的驅(qū)動力Fml與驅(qū)動電壓u有如下關系： Fml(s)=L[Fj (1.8) U(s) = L[u] (1. 9)
其中，G(S)表示從驅(qū)動電壓到輸出驅(qū)動力的傳遞函數(shù)，驅(qū)動桿的輸出驅(qū)動力：
關驅(qū)動力矢里 τ - diag (Fmll, Fml2, Fml3, Fml4, Fml5, Fml6) (1.12) 然后通過動力學正解，則可以得到力矢量F = Jft。
3. 根據(jù)權利要求1所述基于關節(jié)力的空間六維力測量方法，其特征在于：所述上平臺 和下平臺為六邊形或圓形，上平臺位于下平臺的正上方。
【專利摘要】本發(fā)明提供的一種基于關節(jié)力的空間六維力測量方法，包括上平臺和下平臺，所述下平臺由六個電機分別獨立驅(qū)動的六個驅(qū)動桿與上平臺連接，六個電機形成六個關節(jié)，所述上平臺和下平臺分別具有六個連接點，通過建立坐標系、運動學逆解、運動學正解以及Jacobi矩陣的求解；能夠在極端環(huán)境中對機器人末端的六維力和力矩進行準確測量計算，并且能夠適應惡劣的工況環(huán)境，可靠性強。
【IPC分類】G01L5-16
【公開號】CN104634498
【申請?zhí)枴緾N201510057991
【發(fā)明人】劉飛, 趙云花, 謝更新, 熊輝, 詹涵菁, 張元勛
【申請人】重慶大學
【公開日】2015年5月20日
【申請日】2015年2月4日