一種基于磁強(qiáng)計的機(jī)器人遙操作裝置及方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于機(jī)器人遙操作技術(shù)領(lǐng)域，涉及一種基于磁強(qiáng)計的機(jī)器人遙操作裝置及 方法，該方法通過磁強(qiáng)計作為末端檢測方法，通過該方法可以檢測出機(jī)器人末端需要達(dá)到 的位置和姿態(tài)。然后用該姿態(tài)作為機(jī)器人末端的目標(biāo)位置進(jìn)行運(yùn)動。該方法可以用于同構(gòu) 機(jī)器人的遙操作系統(tǒng)。
【背景技術(shù)】
[0002] 關(guān)于機(jī)器人遙操作方法，有多種形式，比如專利(CN201410016585.6)提出了一種 空間機(jī)器人遙操作方法，采用三維圖形仿真驗證、執(zhí)行能力驗證雙驗證方法保證空間遙操 作指令安全性;采用預(yù)測仿真驅(qū)動三維模型為操作者提供實時視覺信息，采用雙邊控制技 術(shù)為操作人員提供力覺信息，該方法的計算量大，操作實時性差，并且對控制器的要求較 高。另外專利(CN201210304630.9) -種遙操作機(jī)器人力反饋控制系統(tǒng)及控制方法，該發(fā)明 控制系統(tǒng)能間接提取遙操作機(jī)器人系統(tǒng)的負(fù)載力，實現(xiàn)準(zhǔn)確實時的力反饋，該方法操作不 直觀，很難了解當(dāng)前的操作狀態(tài)。但是從目前來開，還沒有采用基于磁強(qiáng)計的檢測方法來實 現(xiàn)機(jī)器人遙操作。

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

[0003] 本發(fā)明的技術(shù)解決問題:克服現(xiàn)有技術(shù)的不足，提供一種基于磁強(qiáng)計的機(jī)器人遙 操作裝置及方法，解決了遙操作機(jī)器人末端位置感知和計算，并將感知的位姿作為遙操作 機(jī)器人執(zhí)行機(jī)器人的目標(biāo)位置進(jìn)行機(jī)器人的控制，實現(xiàn)機(jī)器人的遙操作。
[0004] 本發(fā)明的技術(shù)方案是:一種基于磁強(qiáng)計的機(jī)器人遙操作裝置，包括空間磁場發(fā)生 器、基于磁強(qiáng)計和動態(tài)傾角儀的遙操作末端感知器、執(zhí)行機(jī)械臂；空間磁場發(fā)生器包括左矩 形線圈組和右矩形線圈組，以及上述兩組線圈產(chǎn)生的非均勻磁場;遙操作末端感知器包括 三軸磁強(qiáng)計和二維動態(tài)傾角儀，其中三軸磁強(qiáng)計敏感正交三個方向磁場的大小，二維動態(tài) 傾角儀檢測整個末端感知器相對于水平面的方位角和俯仰角;執(zhí)行機(jī)械臂用于進(jìn)行任務(wù)執(zhí) 行的機(jī)械臂系統(tǒng);遙操作末端感知器置于空間磁場發(fā)生器的產(chǎn)生的非均勻磁場范圍內(nèi)并按 需要進(jìn)行移動位置和變化姿態(tài)，末端感知器時刻檢測當(dāng)前位置的三個垂直方向的磁場強(qiáng) 度，給出末端感知器在非均勻磁場中的位置和姿態(tài)，將該位置和姿態(tài)作為執(zhí)行機(jī)械臂的目 標(biāo)位置用來驅(qū)動執(zhí)行機(jī)械臂進(jìn)行運(yùn)動達(dá)到需要的位置和姿態(tài)。
[0005] -種基于磁強(qiáng)計的機(jī)器人遙操作方法，步驟如下：
[0006] 1)通過精密測量對該磁場與空間位置關(guān)系進(jìn)行精確標(biāo)定，得到磁場矢量與空間位 置的函數(shù)關(guān)系;磁場矢量空間坐標(biāo)系中任意位置的磁場由磁場總強(qiáng)度F、水平分量H、磁傾角 I、磁偏角D表示;其中磁傾角I表示水平面和磁場向量F之間的夾角，在水平面以下為負(fù)，磁 偏角D表示磁場向量在水平面的投影與正向的夾角，磁北極向為正；
[0007] 2)通過三軸磁強(qiáng)計的三個探頭分別敏感得到三個探頭方向上的磁場強(qiáng)度大小 Mcx，Mcy和Mcz;
[0008] 3)建立如下坐標(biāo)系：
[0009] 31)建立磁場矢量空間坐標(biāo)系OdXdYdZd，其Xd軸方向為北，Yd軸方向為東，Zd軸方向 為天；
[0010] 32)建立磁北極坐標(biāo)系OmXmYmZm，其Xm軸為磁北向，Zm軸為重力反方向，Ym軸符合左 手定則；
[0011] 33)建立載體坐標(biāo)系OjXjYjZj，其中Xj軸為載體前進(jìn)方向，Zj為載體水平前進(jìn)時的 指向天的方向，Yj軸為載體右向；
[0012] 4)利用末端感知器中的二維動態(tài)傾角儀，實時得到載體坐標(biāo)系滾動角史和俯仰角 Y ；
[0013] 5)根據(jù)以下公式計算得到偏航角Θ:
[0014]
[0015] Θ =arctan(Yh/Xh)；
[0016] 6)將MCX，MCy和Mcz帶入下式得到當(dāng)前位置的位置坐標(biāo)，即水平分量H，磁傾角I和 磁偏角D;
[0017]
[0018]
[0019] 7)根據(jù)步驟1)得到的磁場矢量與空間位置的函數(shù)關(guān)系以及步驟6)得到的當(dāng)前位 置的位置坐標(biāo)加上滾動角口和俯仰角γ和偏航角Θ，即可提出末端感知器的位置和姿態(tài)。
[0020] 8)將末端感知器的位置和姿態(tài)作為執(zhí)行機(jī)械臂的目標(biāo)位置給執(zhí)行機(jī)械臂，則機(jī)械 臂根據(jù)自身的運(yùn)動算法達(dá)到目標(biāo)位置，實現(xiàn)遙操作。
[0021] 本發(fā)明與現(xiàn)有技術(shù)相比具有以下優(yōu)點(diǎn)：
[0022]本發(fā)明采用了磁強(qiáng)計和二維動態(tài)傾角儀來檢查遙操作原邊末端感知器的位置和 姿態(tài)，并給出其在已知磁場中的位置和姿態(tài)，通過算法將其轉(zhuǎn)化為執(zhí)行機(jī)械臂的目標(biāo)位置 和姿態(tài)。該方法可以有效的減少常規(guī)視覺遙操作的大時延問題。同時該方法通過原邊的執(zhí) 行操作來進(jìn)行機(jī)械臂的位置和軌跡導(dǎo)引，避免了采用力反饋遙操作方式不直觀的缺點(diǎn)。而 且該方法能夠同時適應(yīng)同構(gòu)型遙操作機(jī)器人系統(tǒng)和異構(gòu)型遙操作機(jī)器人系統(tǒng)。采用同構(gòu)型 機(jī)器人遙操作系統(tǒng)將具有更加直觀的特點(diǎn)。
【附圖說明】
[0023]圖1為空間磁場發(fā)生器組成示意圖；
[0024] 圖2為末端感知器組合示意圖；
[0025] 圖3為各坐標(biāo)系變換關(guān)系；
[0026] 圖4為磁場分解示意圖。
【具體實施方式】
[0027] 本專利主要是提出了一種采用磁強(qiáng)計作為空間位姿測量并對機(jī)械臂進(jìn)行搖操作 的裝置及方法。該裝置主要包括以下部分:空間磁場發(fā)生器，基于磁強(qiáng)計和動態(tài)傾角儀的遙 操作末端感知器，執(zhí)行機(jī)械臂。其中空間磁場發(fā)生器用來在操作臺處產(chǎn)生空間磁場。如圖1 所示，本專利涉及的空間磁場發(fā)生器包括左矩形線圈組1、右矩形線圈組2、以及由上述兩組 線圈產(chǎn)生的非均勻磁場3。該磁場可以通過精密測量可以對該磁場與空間位置關(guān)系進(jìn)行精 確標(biāo)定，即可以得到磁場矢量與空間位置的函數(shù)關(guān)系或?qū)?yīng)關(guān)系。
[0028]遙操作末端感知器主要有三軸磁強(qiáng)計和二維動態(tài)傾角儀組成，其中三軸磁強(qiáng)計可 以敏感正交三個方向磁場的大小，二維動態(tài)傾角儀可以檢測整個末端感知器相對于水平面 的方位角和俯仰角。根據(jù)兩種聯(lián)合解算，可以求得末端感知器在操作平臺坐標(biāo)系內(nèi)的位置 和姿態(tài)。如圖2所示，三軸磁強(qiáng)計的三個探頭的軸分別于載體坐標(biāo)系三軸平行，其中Xj軸為 載體前進(jìn)方向，Zj為載體水平前進(jìn)時的指向天的方向，Yj軸為載體右向。其中磁強(qiáng)計的三個 探頭分別定義為Mcx，Mcy和Mcz，分別可以敏感載體坐標(biāo)系Xj，Yj和Zj軸上的磁場強(qiáng)度大小。 而二維動態(tài)傾角儀安裝于載體坐標(biāo)系OjXjYj平面內(nèi)，這樣傾角儀可以實時輸出載體坐標(biāo)系 滾動角和俯仰角。
[0029] 首先建立如下坐標(biāo)系：
[0030] 1)建立磁場矢量空間坐標(biāo)系OdXdYdZd，其Xd軸方向為北，Yd軸方向為東，Zd軸方向為 天；
[0031] 2)建立磁北極坐標(biāo)系OmXmYmZm，其Xm軸為磁北向，Zm軸為重力反方向，Ym軸符合左手 定則；
[0032] 3)建立載體坐標(biāo)系O