一種小型水下機(jī)械手位置角度測(cè)量方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及的是一種水下機(jī)械手位置角度測(cè)量方法，具體地說(shuō)是一種為水下機(jī)械 手末端位置精度以及關(guān)節(jié)角度精度測(cè)量方法。
【背景技術(shù)】
[0002] 隨著陸地資源的日漸減少，人類開發(fā)海洋的步伐越來(lái)越快。自主式水下機(jī)器人及 其作業(yè)系統(tǒng)無(wú)人情況下在深海進(jìn)行探測(cè)、開發(fā)的載體；隨著其工作范圍，作業(yè)要求的不斷提 高，對(duì)其作業(yè)工具也提出了更高的要求?；谒聶C(jī)械手作業(yè)任務(wù)高精度的要求，需要對(duì)水 下機(jī)械手運(yùn)動(dòng)控制精度進(jìn)行驗(yàn)證測(cè)量。
[0003] 目前直接以測(cè)量水下機(jī)械手的末端位置角度精度為目的的測(cè)量方法相對(duì)較少，但 相關(guān)論文上描述的方法可以通過(guò)測(cè)量結(jié)果處理間接的得到機(jī)械手末端精度。如中文期刊海 洋工程（THE OCEAN ENGINEERING)刊登的"水下機(jī)械手接近覺系統(tǒng)研究"文章中，根據(jù)水下 機(jī)械手的工作需要，設(shè)計(jì)了一套高精度的水下超聲波定位系統(tǒng)。采用超聲波檢測(cè)距離、三 探頭陣列檢測(cè)方位的方案，使用超低功耗單片機(jī)MSP430精確計(jì)算超聲波往返時(shí)間和控制 三探頭輪流工作，通過(guò)對(duì)三個(gè)探頭的測(cè)量數(shù)據(jù)進(jìn)行處理，實(shí)現(xiàn)目標(biāo)測(cè)距及定位。如中文期 刊北京航空航天大學(xué)學(xué)報(bào)刊登的"基于CCD和超聲的物體位姿檢測(cè)方法及精度分析"文章 中，介紹了用單個(gè)CCD攝像機(jī)和超聲傳感器相結(jié)合檢測(cè)物體位姿的方法。首先由CCD攝像 機(jī)采集一幅圖像，獲取物體上目標(biāo)點(diǎn)在圖像坐標(biāo)系中的理想坐標(biāo)，并由此確定該點(diǎn)投影矢 量（連接物體點(diǎn)和對(duì)應(yīng)圖像點(diǎn)的直線）的方向；然后控制機(jī)器人運(yùn)動(dòng)，使超聲傳感器的z 軸與求得的投影矢量重合；最后用超聲傳感器測(cè)出投影矢量的長(zhǎng)度，確定目標(biāo)點(diǎn)在攝像機(jī) 坐標(biāo)系中的坐標(biāo)。上述文章可通過(guò)各自方法得到末端運(yùn)行坐標(biāo)用以測(cè)量機(jī)械手末端精度， 上述方法所用的高精度超聲探頭以及CCD攝像機(jī)成本較高。

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

[0004] 本發(fā)明的目的是提供一種測(cè)量過(guò)程方便簡(jiǎn)單、計(jì)算處理過(guò)程精簡(jiǎn)且測(cè)量成本低， 能有效解決水下機(jī)械手末端位置角度精度測(cè)量的小型水下機(jī)械手位置角度測(cè)量方法。
[0005] 本發(fā)明的目的是這樣實(shí)現(xiàn)的：
[0006] (1)首先建立該方法測(cè)量時(shí)所用的基坐標(biāo)系以及測(cè)量坐標(biāo)系；以機(jī)械手安裝固定 點(diǎn)為原點(diǎn)建立機(jī)械手基坐標(biāo)系（O-XYZ)，機(jī)械手完全展開時(shí)末端點(diǎn)與安裝固定點(diǎn)連線方向 為X軸，縱向垂直于X軸方向?yàn)閅軸，水平面垂直于X軸方向?yàn)閆軸，坐標(biāo)系X、Y、Z軸的建 立遵循右手法則；將三個(gè)拉繩位移傳感器安裝組成三維位置測(cè)量裝置，傳感器之間的相對(duì) 位置在測(cè)量過(guò)程中不變并且安裝位置在空間中呈"〈"型固定，基于該裝置可建立一測(cè)量坐 標(biāo)系O-XcYcZc,坐標(biāo)系X、Y、Z軸與基坐標(biāo)系相同。
[0007] (2)組成三維位置測(cè)量裝置的各拉繩位移傳感器兩兩之間相對(duì)位置為a、b、c (a、 b、c長(zhǎng)度可由拉繩位移傳感器測(cè)量），可知①、②、③拉繩位移傳感器在坐標(biāo)系O-XcYcZ c中的 位置分別為：①：[0, 0, -acos ( Θ )];②：[0, asin( Θ )，0];③：[0, 0, -acos ( Θ )-b]，其中： (3〇3(0) = (&2+132-(^/ 2&13，&、13、(;為各傳感器之間安裝的相對(duì)位置。由于三維位置測(cè)量裝 置的三根測(cè)量線末端與機(jī)械手末端連接，機(jī)械手末端運(yùn)動(dòng)到測(cè)量坐標(biāo)系下任意點(diǎn)，傳感器 可以得到相應(yīng)的拉伸長(zhǎng)度。
[0008] (3)給定空間基坐標(biāo)系中A[px，py，pz](理論值），針對(duì)機(jī)械手的自由度為 i(i多2)，通過(guò)傳統(tǒng)的逆運(yùn)動(dòng)學(xué)方法求解得出機(jī)械手各關(guān)節(jié)的關(guān)節(jié)角0。到Θ i，即各關(guān)節(jié)角 轉(zhuǎn)動(dòng)角度的期望值，并將機(jī)械手各關(guān)節(jié)轉(zhuǎn)動(dòng)到期望值角度，使機(jī)械手末端運(yùn)動(dòng)到期望位置， 此時(shí)三維位置測(cè)量裝置中各傳感器拉伸值為Q、L2、L3;由于測(cè)量坐標(biāo)系O-XJcZp Q、L2、L3均以確定，因此可建立如下所示方程組：
[0009] pcx2+pcy2+ (pcz+acos ( Θ ))2= L !2
[0010] Pcx2+ (pcy-asin ( Θ )) 2+pcz2= L 22
[0011] Pcx2+Pcy2+ (pcz+acos ( θ ) +b)2= L 32
[0012] 通過(guò)對(duì)上述方程組進(jìn)行求解，可計(jì)算得到點(diǎn)A在測(cè)量坐標(biāo)系0-&\4中的位置 [Pcx，Pcy，Pcz](頭際值）。
[0013] (4)測(cè)量坐標(biāo)系0-&\4通過(guò)坐標(biāo)系旋轉(zhuǎn)變換以及平移轉(zhuǎn)換可得到基坐標(biāo)系 0-ΧΥΖ，其旋轉(zhuǎn)變換矩陣和平移轉(zhuǎn)換矩陣分別為 在測(cè)量坐標(biāo)系O-XeYeZe中的位置[p ^ Pq，PtJ可通過(guò)下式轉(zhuǎn)換到基坐標(biāo)系O-XYZ下表示：
[0015] 式中：AC為點(diǎn)A在測(cè)量坐標(biāo)系O-X JcZc下矩陣A c= [p pcy p Jτ，A。為點(diǎn)A在基 坐標(biāo)系O-XYZ下矩陣A。= [pM Pciy ρJt，并通過(guò)此變換矩陣式即可得出實(shí)際值在基坐標(biāo)系 O-XYZ 下坐標(biāo)[Pcix，Pciy, PtJ。
[0016] (5)在基坐標(biāo)系下，將解算出來(lái)的點(diǎn)A坐標(biāo)實(shí)際值[pM，Pciy，ρ J與理論值[px，py， Pz]比較，即可得出末端位置定位誤差，同時(shí)關(guān)節(jié)角度誤差P可通過(guò)下式計(jì)算得出：
%兩點(diǎn)之間的誤差距離，即為末 端位置定位誤差，理論值點(diǎn)A[px，py，pz]到原點(diǎn)0距離
[0019] 三維位置測(cè)量裝置（測(cè)量坐標(biāo)系）可以整體移動(dòng)以適應(yīng)作業(yè)空間較大的機(jī)械手末 端精度測(cè)量。
[0020] 本發(fā)明適用于水下機(jī)械手關(guān)節(jié)為轉(zhuǎn)動(dòng)副，求解過(guò)程中當(dāng)存在多重解情況，應(yīng)采用 "最短行程"準(zhǔn)則擇優(yōu)選取，即保證每個(gè)關(guān)節(jié)的轉(zhuǎn)動(dòng)量變化最小。
[0021] 需進(jìn)行多次測(cè)量，得到多組機(jī)械手末端定位誤差，并求取平均定位誤差，再求取關(guān) 節(jié)角度定位誤差⑦.α
[0022] 水下機(jī)械手位置角度的測(cè)量方法同樣適用于陸地機(jī)械手使用。
[0023] 本發(fā)明提供了 一種有效解決水下機(jī)械手末端位置角度精度測(cè)量的方法，該方法測(cè) 量過(guò)程方便簡(jiǎn)單、計(jì)算處理過(guò)程精簡(jiǎn)且測(cè)量成本低；測(cè)量原理為通過(guò)本專利的三維測(cè)量裝 置得到機(jī)械手末端在測(cè)量坐標(biāo)系下的實(shí)際值坐標(biāo)，但可將測(cè)量的實(shí)際值坐標(biāo)通過(guò)坐標(biāo)轉(zhuǎn)換 到理論值基坐標(biāo)系中，比較得到機(jī)械手末端運(yùn)動(dòng)的位置誤差。
[0024] 本發(fā)明的有益效果為：
[0025] 1、提供一種機(jī)械手末端位置角度精度測(cè)量方法，由于測(cè)量裝置拉繩位移傳感器能 適用于水下測(cè)量，所以該方法適用于水下作業(yè)機(jī)械手。
[0026] 2、該方法測(cè)量過(guò)程簡(jiǎn)單、計(jì)算處理過(guò)程精簡(jiǎn)、測(cè)量所使用拉繩位移傳感器成本低； 而且能快速的進(jìn)行多次測(cè)量，求取平均誤差；測(cè)量坐標(biāo)系可以整體移動(dòng)以適應(yīng)作業(yè)空間較 大的機(jī)械手末端精度測(cè)量。
[0027] 3、通過(guò)該測(cè)量方法的平均誤差結(jié)果，可通過(guò)后續(xù)的計(jì)算處理得到機(jī)械手各關(guān)節(jié)角 度定位精度。
【附圖說(shuō)明】
[0028] 圖1本發(fā)明測(cè)量方法流程圖；
[0029] 圖2三維測(cè)量裝置安裝測(cè)量原理圖；
[0030] 圖3坐標(biāo)系轉(zhuǎn)換原理圖；
[0031] 圖4機(jī)械手關(guān)節(jié)角度誤差計(jì)算原理圖；
[0032] 圖5三維測(cè)量裝置移動(dòng)測(cè)量原理圖。
【具體實(shí)施方式】
[0033] 下面結(jié)合附圖對(duì)本發(fā)明專利做進(jìn)一步詳述。
[0034] 如圖1所示，本發(fā)明測(cè)量方法的測(cè)量流程和實(shí)現(xiàn)步驟為：
[0035] 1)首先基于本發(fā)明的測(cè)量原理，建立該方法測(cè)量時(shí)所用的基坐標(biāo)系以及測(cè)量坐標(biāo) 系；以機(jī)械手安裝固定點(diǎn)為原點(diǎn)建立機(jī)械手基坐標(biāo)系（O-XYZ)，將三個(gè)拉繩位移傳感器安 裝組成三維位置測(cè)量裝置，并基于該裝置可建立一測(cè)量坐標(biāo)系0-WZ。?；鴺?biāo)系（O-XYZ) 以機(jī)械手完全展開時(shí)末端點(diǎn)與安裝固定點(diǎn)連線方向?yàn)閄軸，縱向垂直于X軸方向?yàn)閅軸，水 平面垂直于X軸方向?yàn)閆軸，坐標(biāo)系X、Y、Z軸的建立遵循右手法則；測(cè)量坐標(biāo)系O-XeYcZ c, 坐標(biāo)系X、Y、Z軸與基坐標(biāo)系相同。三維位置測(cè)量裝置的三根測(cè)量線末端與機(jī)械手末端連 接，機(jī)械手末端運(yùn)動(dòng)到測(cè)量坐標(biāo)系下任意點(diǎn)，傳感器可以得到相應(yīng)的拉伸長(zhǎng)度。
[0036]