航天器機(jī)械臂柔性隨動(dòng)控制重力補(bǔ)償方法
【專利摘要】本發(fā)明公開了一種航天器機(jī)械臂的柔性隨動(dòng)控制重力補(bǔ)償方法，柔性隨動(dòng)控制中將六維力傳感器設(shè)置在機(jī)械臂末端與負(fù)載之間，該方法通過控制機(jī)械臂使負(fù)載處于多個(gè)不同的空間姿態(tài)，在負(fù)載無外力作用的情況下，記錄每個(gè)空間姿態(tài)下六維力傳感器的測量數(shù)據(jù)并根據(jù)力與力矩的物理關(guān)系，由測量數(shù)據(jù)計(jì)算得到負(fù)載的重力大小G及負(fù)載重心的坐標(biāo)；并根據(jù)負(fù)載的受力情況對重力的影響進(jìn)行補(bǔ)償。本發(fā)明的方法，用機(jī)械臂系統(tǒng)本身測量負(fù)載的重力和重心位置，設(shè)計(jì)的算法計(jì)算出六維力傳感器測得6個(gè)分量的重力補(bǔ)償值，解決了機(jī)械臂柔性隨動(dòng)控制中的重力補(bǔ)償問題。
【專利說明】航天器機(jī)械臂柔性隨動(dòng)控制重力補(bǔ)償方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于機(jī)械臂的控制【技術(shù)領(lǐng)域】，具體來說，本發(fā)明涉及一種機(jī)械臂柔性隨動(dòng)控制中補(bǔ)償負(fù)載重力的方法。
【背景技術(shù)】
[0002]航天器研制具有單件小批量的特點(diǎn)，目前的裝配作業(yè)大量依賴人工操作，并輔以吊具、升降車、架梯等簡易工具進(jìn)行不同航天器的裝配工作。這種裝配方式存在一定的局限性，對于大尺寸、大重量零部件人工安裝困難，對于狹小空間內(nèi)零部件的安裝人工難以操作，對于安裝精度要求較高的零部件，人工安裝難以控制安裝精度。
[0003]為解決航天器復(fù)雜工況的裝配難題，提高裝配質(zhì)量、裝配效率和安全性，希望引入機(jī)械臂，采用柔性隨動(dòng)控制方法對航天器進(jìn)行裝配。所述的機(jī)械臂柔性隨動(dòng)控制方法為:人手直接作用于安裝在機(jī)械臂末端的負(fù)載，在機(jī)械臂末端法蘭與負(fù)載之間安裝有六維力傳感器，用于感知作用其上的力與力矩信息，控制系統(tǒng)進(jìn)一步獲得人手作用的力與力矩信息，以此作為輸入通過一定的控制算法控制機(jī)械臂運(yùn)動(dòng)，使負(fù)載跟隨人手運(yùn)動(dòng)。
[0004]在上述機(jī)械臂柔性隨動(dòng)控制方法中，六維力傳感器安裝在機(jī)械臂末端法蘭與負(fù)載之間，其感知到的力與力矩信息是負(fù)載重力與人手施力的綜合作用結(jié)果，需要進(jìn)行重力補(bǔ)償，即將負(fù)載重力的作用分量從六維力傳感器得到的力與力矩參數(shù)中減去，進(jìn)而獲得人手作用產(chǎn)生的力與力矩信息，作為輸入?yún)?shù)用于控制機(jī)械臂的運(yùn)動(dòng)。
[0005]機(jī)械臂在運(yùn)動(dòng)過程中，如果其末端姿態(tài)發(fā)生變化，則六維力傳感器與負(fù)載的空間姿態(tài)也隨之變化，從六維力傳感器得到的力與力矩參數(shù)中負(fù)載重力的作用分量也發(fā)生了變化，要在機(jī)械臂末端姿態(tài)不斷變化的情況下實(shí)現(xiàn)柔性隨動(dòng)控制，需要在機(jī)械臂運(yùn)動(dòng)過程中，實(shí)時(shí)獲得負(fù)載在當(dāng)前姿態(tài)下的重力作用分量，并進(jìn)行重力補(bǔ)償。
[0006]本發(fā)明即根據(jù)機(jī)械臂柔性隨動(dòng)控制的需要，提出了一種航天器機(jī)械臂柔性隨動(dòng)控制重力補(bǔ)償方法。

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

[0007]本發(fā)明的目的在于提供一種航天器機(jī)械臂柔性隨動(dòng)控制重力補(bǔ)償方法，能夠在機(jī)械臂運(yùn)動(dòng)過程中，實(shí)時(shí)得到負(fù)載在當(dāng)前姿態(tài)下六維力傳感器各分量的重力補(bǔ)償值，通過重力補(bǔ)償實(shí)時(shí)獲得人手或其他外力作用產(chǎn)生的力與力矩信息，實(shí)現(xiàn)機(jī)械臂的柔性隨動(dòng)控制。
[0008]為了實(shí)現(xiàn)上述目的，本發(fā)明采用了如下的技術(shù)方案:
[0009]一種航天器機(jī)械臂柔性隨動(dòng)控制重力補(bǔ)償方法，其中柔性隨動(dòng)控制中將六維力傳感器設(shè)置在機(jī)械臂末端與負(fù)載之間，該方法包括如下步驟:
[0010]I)控制機(jī)械臂使負(fù)載處于多個(gè)不同的空間姿態(tài)，在負(fù)載無外力作用的情況下，記錄每個(gè)空間姿態(tài)下六維力傳感器的測量數(shù)據(jù)；
[0011]2)根據(jù)力與力矩的物理關(guān)系，由步驟I)的測量數(shù)據(jù)計(jì)算得到負(fù)載的重力大小G及負(fù)載重心在六維力傳感器自身坐標(biāo)系中的坐標(biāo)(X，1，Z)；[0012]3)在負(fù)載有外力作用的情況下，實(shí)時(shí)根據(jù)重力在六維力傳感器的自身坐標(biāo)系中的方向，將重力在六維力傳感器的自身坐標(biāo)系的三個(gè)坐標(biāo)軸上投影得到三個(gè)重力分量Gx、Gy、Gz，分別作為六維力傳感器的自身坐標(biāo)系中X、Y、Z軸力分量的重力補(bǔ)償值；
[0013]4)由公式 MGx=Gz Xy — Gy X z, MGy=GxX z — Gz Xx, MGz=GyXx — GxXy,計(jì)算得到六維力傳感器自身坐標(biāo)系中X、Y、Z軸上由重力引起的三個(gè)力矩分量MGx、MGy、MGz，分別作為六維力傳感器自身坐標(biāo)系中X、Y、Z軸上力矩分量的補(bǔ)償值；
[0014]5)在負(fù)載有外力作用的情況下，六維力傳感器測得的三個(gè)力分量為Fx、Fy、Fz，三個(gè)力矩分量為Mx、My、Mz，補(bǔ)償?shù)玫酵饬υ诹S力傳感器自身坐標(biāo)系中的作用力為(Fx —Gx, Fy - Gy, Fz — Gz )，作用力矩為(Mx — MGx, My — MGy, Mz — MGz )，重力補(bǔ)償完成。
[0015]上述技術(shù)方案步驟I)中，所述六維力傳感器每次測量得到的數(shù)據(jù)有六個(gè)分量，包括在六維力傳感器自身坐標(biāo)系中的三個(gè)坐標(biāo)軸方向的力分量，以及三個(gè)坐標(biāo)軸方向的力矩分量。所述六維力傳感器自身坐標(biāo)系是與六維力傳感器自身固連的空間直角坐標(biāo)系。
[0016]上述技術(shù)方案步驟I)中，所述負(fù)載無外力作用指的是負(fù)載連接在六維力傳感器上，僅受自身重力及與六維力傳感器的連接支持力，不受其它外部作用力。
[0017]上述技術(shù)方案步驟2)中，假設(shè)步驟I)對N個(gè)不同的負(fù)載姿態(tài)測量得到N組六維力傳感器數(shù)據(jù)，第i組的3個(gè)力分量為Fx1、Fy1、Fzi，3個(gè)力矩分量為Mx1、My1、Mzi，在負(fù)載無外力作用的情況下，測得的力與力矩分量都由負(fù)載重力引起，則負(fù)載重力大小為:G= (Fxi2+Fyi2+Fzi2)1/2ο設(shè)負(fù)載重心在六維力傳感器坐標(biāo)系中的坐標(biāo)為(x, y, z),在該坐標(biāo)系下根據(jù)力與力矩的關(guān)系有:
[0018]Mxi=FziXy — FyiXz
[0019]Myi=FxiXz — FziXx
[0020]Mzi=FyiXx — FxiXy
[0021]取多個(gè)不同的負(fù)載姿態(tài)測量得到的多組六維力傳感器數(shù)據(jù)，根據(jù)上面的方程組，采用一定的數(shù)學(xué)方法，如最小二乘法，即可求得負(fù)載重心在六維力傳感器坐標(biāo)系中的坐標(biāo)
(X, Y，Z)o
[0022]上述技術(shù)方案步驟3)中，在柔性隨動(dòng)控制過程中，機(jī)械臂末端姿態(tài)會(huì)發(fā)生變化，六維力傳感器坐標(biāo)系相對于大地坐標(biāo)系的姿態(tài)也會(huì)隨之變化，但負(fù)載重力始終豎直向下，因此負(fù)載重力在六維力傳感器坐標(biāo)系中的方向也在隨之變化。經(jīng)過對機(jī)械臂安放姿態(tài)的標(biāo)定，機(jī)械臂控制系統(tǒng)容易實(shí)時(shí)得到重力方向與六維力傳感器坐標(biāo)系X、Y、Z軸的夾角(α，β，Y)，則可計(jì)算得到負(fù)載重力在六維力傳感器坐標(biāo)系3個(gè)坐標(biāo)軸上的投影為:Gx=GXcos a，Gy=GXcos β , Gz=GXcos Y , Gx、Gy、Gz即是六維力傳感器測得的力分量數(shù)據(jù)Fx、Fy、Fz中由于負(fù)載重力產(chǎn)生的部分。
[0023]上述技術(shù)方案步驟4)中，計(jì)算得到的MGx、MGy、MGz是六維力傳感器測得的力矩分量數(shù)據(jù)Mx、My、Mz中由于負(fù)載重力產(chǎn)生的部分。
[0024]上述技術(shù)方案步驟5)中，若在柔性隨動(dòng)控制人手對負(fù)載進(jìn)行操作，(Fx - Gx, Fy 一Gy, Fz - Gz)可被認(rèn)為是人手作用產(chǎn)生的力的分量，(Mx 一 MGx7My 一 MGy7Mz 一 MGz)可被認(rèn)為是人手作用產(chǎn)生的力距的分量，根據(jù)(Fx — Gx, Fy — Gy, Fz — Gz)與(Mx — MGx,My —MGy, Mz — MGz)可控制機(jī)械臂跟隨人手運(yùn)動(dòng)。
[0025]本發(fā)明的航天器機(jī)械臂柔性隨動(dòng)控制重力補(bǔ)償方法，用機(jī)械臂系統(tǒng)本身測量負(fù)載的重力和重心位置，設(shè)計(jì)的算法可計(jì)算出六維力傳感器測得6個(gè)分量的重力補(bǔ)償值，解決了機(jī)械臂柔性隨動(dòng)控制中的重力補(bǔ)償問題。
【專利附圖】

【附圖說明】
[0026]圖1為航天器機(jī)械臂柔性隨動(dòng)控制重力補(bǔ)償方法適用的機(jī)械臂柔性隨動(dòng)控制的系統(tǒng)不意圖。
[0027]圖2為本發(fā)明的重力補(bǔ)償方法中六維力傳感器坐標(biāo)系中負(fù)載重力的作用示意圖。
[0028]I 一機(jī)械臂、2 —六維力傳感器、3 —被操作件、4 一操作者、5 —手、6 —夾具、7 —機(jī)
械臂末端法蘭。
【具體實(shí)施方式】
[0029]以下介紹的是作為本發(fā)明所述內(nèi)容的【具體實(shí)施方式】，下面通過【具體實(shí)施方式】對本發(fā)明的所述內(nèi)容作進(jìn)一步的闡明。當(dāng)然，描述下列【具體實(shí)施方式】只為示例本發(fā)明的不同方面的內(nèi)容，而不應(yīng)理解為限制本發(fā)明范圍。
[0030]實(shí)施方式1:
[0031]如圖1所示，在本發(fā)明的重力補(bǔ)償方法使用的機(jī)械臂柔性隨動(dòng)控制系統(tǒng)中，六維力傳感器2安裝在機(jī)械臂I的末端法蘭7與夾具6之間，六維力傳感器是一種業(yè)內(nèi)人員熟知的成熟的傳感器產(chǎn)品，容易購買獲得(如ATI公司的六維力傳感器)，其被操作件3被夾具6夾持，被操作件3與夾具6共同構(gòu)成了六維力傳感器2的負(fù)載,負(fù)載重力為G,方向?yàn)樨Q直向下，操作者4用手5推動(dòng)被操作件3，對被操作件施加的力為Fh。
[0032]六維力傳感器2檢測到的力與力矩信息是負(fù)載重力G與手5作用力Fh共同作用的結(jié)果，要實(shí)現(xiàn)對機(jī)械臂I的柔性隨動(dòng)控制，需要解算出手5作用力Fh對六維力傳感器2造成的力分量與力矩分量。
[0033]如圖2所示，六維力傳感器2的坐標(biāo)系有X、Y、Z三個(gè)坐標(biāo)軸，負(fù)載重力為G，負(fù)載重心在六維力傳感器2的坐標(biāo)系中的坐標(biāo)為(X，y, z)，負(fù)載重力G在X、Y、Z軸方向的作用分力分別為Gx、Gy、Gz，負(fù)載重力G對X、Y、Z軸的作用力矩分別為MGx、MGy、MGz，根據(jù)力與力矩的關(guān)系，參照圖2易得到:
[0034]
【權(quán)利要求】
1.一種航天器機(jī)械臂的柔性隨動(dòng)控制重力補(bǔ)償方法，其中柔性隨動(dòng)控制中將六維力傳感器設(shè)置在機(jī)械臂末端與負(fù)載之間，該方法包括如下步驟: 1)控制機(jī)械臂使負(fù)載處于多個(gè)不同的空間姿態(tài)，在負(fù)載無外力作用的情況下，記錄每個(gè)空間姿態(tài)下六維力傳感器的測量數(shù)據(jù)； 2)根據(jù)力與力矩的物理關(guān)系，由步驟I)的測量數(shù)據(jù)計(jì)算得到負(fù)載的重力大小G及負(fù)載重心在六維力傳感器自身坐標(biāo)系中的坐標(biāo)(X, y, z)； 3)在負(fù)載有外力作用的情況下，實(shí)時(shí)根據(jù)重力在六維力傳感器的自身坐標(biāo)系中的方向，將重力在六維力傳感器的自身坐標(biāo)系的三個(gè)坐標(biāo)軸上投影得到三個(gè)重力分量Gx、Gy、Gz，分別作為六維力傳感器的自身坐標(biāo)系中X、Y、Z軸力分量的重力補(bǔ)償值； 4)由公式MGx=Gz Xy — Gy X z, MGy=GxX z — Gz Xx, MGz=GyXx — GxXy,計(jì)算得到六維力傳感器自身坐標(biāo)系中X、Y、Z軸上由重力引起的三個(gè)力矩分量MGx、MGy、MGz，分別作為六維力傳感器自身坐標(biāo)系中X、Y、Z軸上力矩分量的補(bǔ)償值； 5)在負(fù)載有外力作用的情況下，六維力傳感器測得的三個(gè)力分量為Fx、Fy,Fz，三個(gè)力矩分量為Mx、My、Mz，補(bǔ)償?shù)玫酵饬υ诹S力傳感器自身坐標(biāo)系中的作用力為(Fx — Gx，F(xiàn)y — Gy，F(xiàn)z — Gz )，作用力矩為(Mx — MGx，My — MGy，Mz — MGz )，重力補(bǔ)償完成。
2.如權(quán)利要求1所述的柔性隨動(dòng)控制重力補(bǔ)償方法，其中，所述六維力傳感器每次測量得到的數(shù)據(jù)有六個(gè)分量，包括在六維力傳感器自身坐標(biāo)系中的三個(gè)坐標(biāo)軸方向的力分量，以及三個(gè)坐標(biāo)軸方向的力矩分量，所述六維力傳感器自身坐標(biāo)系是與六維力傳感器自身固連的空間直角坐標(biāo)系。
3.如權(quán)利要求1所述的柔性隨動(dòng)控制重力補(bǔ)償方法，其中，所述負(fù)載無外力作用指的是負(fù)載連接在六維力傳感器上，僅受自身重力及與六維力傳感器的連接支持力，不受其它外部作用力。
4.如權(quán)利要求1所述的柔性隨動(dòng)控制重力補(bǔ)償方法，其中，假設(shè)步驟I)對N個(gè)不同的負(fù)載姿態(tài)測量得到N組六維力傳感器數(shù)據(jù)，第i組的3個(gè)力分量為Fx1、Fy1、Fzi，3個(gè)力矩分量為Mx1、My1、Mzi，在負(fù)載無外力作用的情況下，測得的力與力矩分量都由負(fù)載重力引起，則負(fù)載重力大小為:G=(Fxi2+Fyi2+Fzi2)1/2。設(shè)負(fù)載重心在六維力傳感器坐標(biāo)系中的坐標(biāo)為(X，y, z)，在該坐標(biāo)系下根據(jù)力與力矩的關(guān)系有:
Mxi=Fzi Xy — Fyi X z
Myi=Fxi X z — Fzi Xx
Mzi=Fyi Xx — Fxi Xy 取多個(gè)不同的負(fù)載姿態(tài)測量得到的多組六維力傳感器數(shù)據(jù)，根據(jù)上面的方程組，采用一定的數(shù)學(xué)方法，如最小二乘法，即可求得負(fù)載重心在六維力傳感器坐標(biāo)系中的坐標(biāo)(X，y,Z)o
5.如權(quán)利要求1所述的柔性隨動(dòng)控制重力補(bǔ)償方法，其中，在柔性隨動(dòng)控制過程中，機(jī)械臂末端姿態(tài)會(huì)發(fā)生變化，六維力傳感器坐標(biāo)系相對于大地坐標(biāo)系的姿態(tài)也會(huì)隨之變化，但負(fù)載重力始終豎直向下，負(fù)載重力在六維力傳感器坐標(biāo)系中的方向也在隨之變化，經(jīng)過對機(jī)械臂安放姿態(tài)的標(biāo)定，機(jī)械臂控制系統(tǒng)容易實(shí)時(shí)得到重力方向與六維力傳感器坐標(biāo)系X、Y、Z軸的夾角(α，β，Y)，則可計(jì)算得到負(fù)載重力在六維力傳感器坐標(biāo)系3個(gè)坐標(biāo)軸上的投影為:Gx=GXcos a，Gy=GXcos β , Gz=GXcos Y , Gx、Gy、Gz即是六維力傳感器測得的力分量數(shù)據(jù)Fx、Fy、Fz中由于負(fù)載重力產(chǎn)生的部分。
6.如權(quán)利要求1所述的柔性隨動(dòng)控制重力補(bǔ)償方法，步驟4)中，計(jì)算得到的MGx、MGy、MGz是六維力傳感器測得的力矩分量數(shù)據(jù)Mx、My、Mz中由于負(fù)載重力產(chǎn)生的部分。
7.如權(quán)利要求1所述的柔性隨動(dòng)控制重力補(bǔ)償方法，步驟5)中，若在柔性隨動(dòng)控制人手對負(fù)載進(jìn)行操作，(Fx - Gx,Fy - Gy,Fz 一 Gz)認(rèn)為是人手作用產(chǎn)生的力的分量，(Mx 一MGx, My - MGy, Mz 一 MGz)認(rèn)為是人手作用產(chǎn)生的力距的分量，根據(jù)(Fx — Gx, Fy 一 Gy,Fz 一 Gz)與(Mx — MGx, My 一 MGy, Mz 一 MGz)控制機(jī)械臂跟隨人手運(yùn)動(dòng)。
【文檔編號(hào)】B25J13/08GK103600354SQ201310552492
【公開日】2014年2月26日 申請日期:2013年11月8日 優(yōu)先權(quán)日:2013年11月8日
【發(fā)明者】劉宏陽, 傅浩, 衛(wèi)月娥, 張立建, 胡瑞欽, 唐賴穎, 易旺民, 萬畢樂, 孫繼鵬, 布仁, 孫剛, 張成立, 郭靜然 申請人:北京衛(wèi)星環(huán)境工程研究所