基于TOF傳感器的堵板拆裝機(jī)器人水室定位

陳柳妍，王湘江，馮棟彥

（南華大學(xué) 機(jī)械工程學(xué)院，湖南 衡陽(yáng) 421001）

0 引言

隨著科學(xué)技術(shù)的進(jìn)步，對(duì)電力的需求也在不斷增加，國(guó)家提出了大力發(fā)展核能工業(yè)的方針和政策[1]，以滿足我國(guó)核電產(chǎn)業(yè)的迅猛發(fā)展和核電市場(chǎng)的擴(kuò)大。加強(qiáng)核設(shè)備的安全性和可靠度，使操作人員受到的輻射劑量最小化，改善操作人員的工作條件，是中國(guó)核電工業(yè)急需解決的難題。為保障核電站的正常運(yùn)行，必須對(duì)核電站的關(guān)鍵設(shè)施進(jìn)行定期檢修，因此開(kāi)發(fā)智能、性能先進(jìn)、可完全取代人力的核電站機(jī)器人是當(dāng)前的研究熱點(diǎn)，期望核電站機(jī)器人能實(shí)現(xiàn)日常維護(hù)、事故處理、核廢物處理等工作。

蒸汽發(fā)生器的管道堵板工作，是核電站每次大修必須執(zhí)行的工作項(xiàng)目，每次大修至少需完成3次堵板拆裝作業(yè)。據(jù)統(tǒng)計(jì)，每執(zhí)行一次堵板工作約受到3 mSv劑量的照射，給現(xiàn)場(chǎng)的施工人員輻射量控制帶來(lái)了一定的壓力，因此，目前亟需機(jī)器人取代人工作業(yè)完成檢修的工作模式。

在已設(shè)計(jì)的十自由度一次側(cè)堵板拆裝機(jī)器人進(jìn)行堵板作業(yè)時(shí)，由于機(jī)器臂末端執(zhí)行機(jī)構(gòu)的尺寸僅略小于蒸汽發(fā)生器的人孔孔徑，并且機(jī)械臂拆裝堵板時(shí)的末端精度需±0.5 mm，因此對(duì)于一次側(cè)堵板拆裝機(jī)器人的工作位置要求較高。只有確定了機(jī)器人執(zhí)行堵板作業(yè)時(shí)的工作位置，才能方便后續(xù)作業(yè)。為此，必須對(duì)其進(jìn)行精確的定位及運(yùn)動(dòng)軌跡規(guī)劃。此外，為適應(yīng)核電站不同型號(hào)尺寸蒸汽發(fā)生器的堵板作業(yè)，需確定一套便捷準(zhǔn)確、泛用性高、功能完備的堵板機(jī)器人室外定位方案及設(shè)備，并對(duì)此種定位方法進(jìn)行誤差分析以保證測(cè)量精度。

目前室外定位技術(shù)迅速發(fā)展，但由于室內(nèi)環(huán)境通常狹小復(fù)雜，定位結(jié)果往往未能差強(qiáng)人意。用于高精度室內(nèi)定位的技術(shù)一般有：基于WiFi的室內(nèi)定位技術(shù)、慣導(dǎo)傳感器室內(nèi)定位技術(shù)[2]、地磁強(qiáng)度室內(nèi)定位技術(shù)、紅外線定位技術(shù)、超寬帶室內(nèi)定位技術(shù)等。張俊龍等[3]利用位置特征信息識(shí)別通過(guò)WiFi網(wǎng)絡(luò)進(jìn)行定位，達(dá)到基于每個(gè)AP熱定覆蓋面積100 m2的情況下，精度為3 m；原玉磊等[4]使用紅外線和超聲波的定位技術(shù)，最小誤差可以達(dá)到4.43 cm；楊瀟等[5]提出一種基于超寬帶技術(shù)的人員定位方法，最終定位精度提高了5%。孫建偉等[6]通過(guò)多源信息融合室內(nèi)定位的方法減小復(fù)雜空間結(jié)構(gòu)對(duì)室內(nèi)定位精度的影響，得到室內(nèi)定位精度的平均偏差為0.601 m。紅外測(cè)距技術(shù)較為成熟，且使用與結(jié)果分析較為簡(jiǎn)便直接，本文直接采用紅外測(cè)距中的激光位移傳感器進(jìn)行實(shí)驗(yàn)研究。

1 激光位移傳感器測(cè)量原理

當(dāng)前市面上的機(jī)械式激光雷達(dá)采用了多種測(cè)距方法[7-13]，其主要有兩種：激光三角測(cè)距和TOF測(cè)距。激光三角法[14]的測(cè)量原理是將紅外激光從雷達(dá)測(cè)距模塊中發(fā)射出去，入射到被測(cè)對(duì)象上后，通過(guò)接收透鏡將一部分散射光聚集在線陣圖像傳感器（CCD/CMOS）上。與其相比，TOF激光位移傳感器具有更小的外形尺寸、更寬的動(dòng)態(tài)感應(yīng)范圍和適應(yīng)各種工作環(huán)境的能力，不僅如此，TOF激光位移傳感器還具有低功耗、小尺寸、高精度、高速度、高安全性和遠(yuǎn)程測(cè)量的優(yōu)勢(shì)，是本實(shí)驗(yàn)最理想的測(cè)距方式。

TOF傳感器測(cè)距時(shí)，驅(qū)動(dòng)激光器發(fā)射一束瞬時(shí)功率極大[15]但持續(xù)時(shí)間極短的光脈沖，同時(shí)啟動(dòng)傳感器內(nèi)部的計(jì)時(shí)單元，光脈沖經(jīng)過(guò)發(fā)射電路到測(cè)量物體表面后四散開(kāi)來(lái)。接收光路通過(guò)光電器件將經(jīng)過(guò)接收光路的散射光能量轉(zhuǎn)化為光電流，再經(jīng)過(guò)回波信號(hào)處理電路轉(zhuǎn)化為電壓信號(hào)，經(jīng)過(guò)放大及調(diào)理后產(chǎn)生電脈沖，觸發(fā)計(jì)時(shí)單元停止計(jì)時(shí)。計(jì)時(shí)單元記錄的時(shí)間間隔即為激光脈沖從發(fā)射到返回的時(shí)間，與光速相乘再除以2，就是傳感器到被測(cè)物體的距離。

圖1 TOF激光位移傳感器測(cè)量原理

2 蒸汽發(fā)生器與機(jī)器人空間模型

2.1 蒸汽發(fā)生器與機(jī)器人坐標(biāo)系建立

圖2所示為安裝一次側(cè)堵板的蒸汽發(fā)生器，由于機(jī)械臂實(shí)驗(yàn)環(huán)境極其狹小，在仿真模擬和試驗(yàn)中，唯有當(dāng)大電缸經(jīng)過(guò)人孔軸線時(shí)，機(jī)械臂方能順利通過(guò)人孔進(jìn)行后續(xù)拆裝工作，因此，電缸與人孔軸線重合時(shí)為最佳實(shí)驗(yàn)位置。

圖2 蒸汽發(fā)生器與機(jī)器人空間坐標(biāo)系

蒸發(fā)發(fā)生器模型為半徑1895 mm球體的一部分，因此建立一個(gè)以球心為坐標(biāo)原點(diǎn)的三維坐標(biāo)系xyz，然后在xyz三維坐標(biāo)系基礎(chǔ)上平移dx、dy、dz，以大電缸與小車(chē)平面交點(diǎn)為原點(diǎn)建立三維坐標(biāo)系x′y′z′。

2.2 蒸汽發(fā)生器球面與人孔面方程

在以球面A球心為坐標(biāo)原點(diǎn)的三維坐標(biāo)系下，M1、M2、M3三點(diǎn)的坐標(biāo)如表1所示。

表1 人孔面上3點(diǎn)坐標(biāo)

2.3 擬合人孔平面邊線及圓心求解

如圖3所示，借用Matlab畫(huà)圖工具，根據(jù)球面A與平面B的方程繪制三維圖形，顯然，球面A和平面B相交所得的交線圓C即為人孔平面邊線。

圖3 模擬人孔平面圖

均勻選取12 個(gè)圓C 上的點(diǎn)，設(shè)圓C方程為A·（x2+y2+z2）+Bx+Cy+Dz+E=0，利用Python工具進(jìn)行數(shù)據(jù)擬合得到ABCDE的值如下：

計(jì)算得到圓C半徑約為435，與實(shí)際圓面直徑吻合，圓心坐標(biāo)為（935.9339,728.3192，-1415.1192）。

2.4 機(jī)器人與激光位移傳感器之間的位置變化

大電缸經(jīng)過(guò)人孔平面圓心時(shí)，可認(rèn)為圓心與大電缸軸線頂點(diǎn)M0（x0，y0，z0）重合，大電缸與小車(chē)平面的夾角為α，兩激光位移傳感器安裝在離x′y′z′原點(diǎn)距離d的位置，

表2 圓C數(shù)據(jù)采集表

由平行關(guān)系可得到以下方程組：

圖4 計(jì)算示意圖

2.5 激光位移傳感器的理論測(cè)量結(jié)果

直線AB的方向向量μ=A—→B；

平面B法向量：n=M1M2×M1M3。

由于三維坐標(biāo)系xyz和x′y′z′之間不存在旋轉(zhuǎn)關(guān)系，僅在空間內(nèi)平移了dx、dy、dz：

所以證得直線AB與平面B平行，故在定位實(shí)驗(yàn)中，當(dāng)測(cè)量到兩激光位移傳感器確定的直線與人孔平面平行位置時(shí)（兩傳感器顯示的距離相同），即為機(jī)器人面向蒸汽發(fā)生器人孔面的最佳工作位置。

2.6 基于激光位移傳感器非接觸測(cè)量模型

激光位移傳感器測(cè)量定位裝置主要由基恩士LRTB5000TOF激光位移傳感器、Y200RA200電控旋轉(zhuǎn)工作臺(tái)、滑軌組成。機(jī)械臂為新松機(jī)械臂，具有±0.05 mm的重復(fù)定位精度，對(duì)本實(shí)驗(yàn)的誤差影響可忽略不計(jì)。機(jī)器人的移動(dòng)小車(chē)部分裝有圖5所示的XY方向的移動(dòng)平臺(tái)，因此只需確定機(jī)器人面向蒸汽發(fā)生器的擺放角度，再根據(jù)實(shí)際情況通過(guò)移動(dòng)平臺(tái)調(diào)整大電缸的前后左右位置即可達(dá)到機(jī)械臂最佳工作位置。

圖5 蒸汽發(fā)生器與機(jī)器人裝配圖

實(shí)驗(yàn)時(shí)，先由激光傳感器對(duì)蒸汽發(fā)生器人孔面進(jìn)行測(cè)量[17]，再將測(cè)量數(shù)據(jù)經(jīng)計(jì)算處理后反饋給底部的工作臺(tái)，旋轉(zhuǎn)到所求角度后進(jìn)行后續(xù)的測(cè)量與定位。

根據(jù)計(jì)算機(jī)器人與蒸汽發(fā)生器的空間位置關(guān)系，得到了機(jī)器人相對(duì)蒸汽發(fā)生器的理論擺放位置，即在機(jī)器人的電缸軸線與人孔軸線重合的基礎(chǔ)上，機(jī)械臂后續(xù)的作業(yè)難度會(huì)大大降低，借助激光位移傳感器進(jìn)行測(cè)量計(jì)算即可得到機(jī)器人的實(shí)際擺放位置。

將激光傳感器定位裝置放置于面向蒸汽發(fā)生器人孔的地面上，激光位移傳感器A和B所在位置設(shè)定為點(diǎn)a、b，且a、b之間的距離為g，如圖6所示，通過(guò)調(diào)整定位裝置的朝向和安裝激光位移傳感器的轉(zhuǎn)板角度，使得兩激光位移傳感器發(fā)出的射線m、n均在人孔環(huán)形平面上，形成光點(diǎn)p、q，設(shè)定點(diǎn)a到p的距離為x，點(diǎn)b到q的距離為y。通過(guò)點(diǎn)a、b的直線為l1，通過(guò)光點(diǎn)p、q的直線為l2，直線l1、l2共面并形成夾角β。讀取激光位移傳感器上顯示的x、y的實(shí)時(shí)數(shù)值，按照式（1）計(jì)算夾角β：

圖6 基于TOF傳感器的測(cè)量原理圖

然后，根據(jù)以上求得的角度β，在電動(dòng)旋轉(zhuǎn)平臺(tái)上輸入step，轉(zhuǎn)動(dòng)旋轉(zhuǎn)臺(tái)，旋轉(zhuǎn)臺(tái)每1600 Step為1°，使直線l1、l2相互平行，同時(shí)，使x=y；當(dāng)x=y時(shí)，即定位完成，故轉(zhuǎn)動(dòng)角度β后的直線l1方向即為一次側(cè)堵板拆裝機(jī)器人的擺放角度。

3 實(shí)驗(yàn)研究

3.1 激光位移傳感器定位實(shí)驗(yàn)

本試驗(yàn)?zāi)康臑橥ㄟ^(guò)測(cè)量?jī)杉す馕灰苽鞲衅鞯饺丝灼矫嫔先我鈨牲c(diǎn)的距離，求得人孔平面與激光位移傳感器所在直線的夾角，通過(guò)轉(zhuǎn)動(dòng)電控旋轉(zhuǎn)臺(tái)使兩者平行，從而確定移動(dòng)小車(chē)初始位置。

實(shí)驗(yàn)開(kāi)始時(shí)，先將激光位移傳感器、內(nèi)置雙軸心直線運(yùn)動(dòng)導(dǎo)軌、電控旋轉(zhuǎn)工作臺(tái)按照設(shè)計(jì)方案組裝，并接通電源；將定位裝置擺放至人孔前方地面，使得激光位移傳感器的測(cè)量范圍在5 m以內(nèi)即可；調(diào)整兩個(gè)TOF激光位移傳感器的角度，使其激光點(diǎn)均在人孔平面上，根據(jù)兩激光位移傳感器測(cè)得的距離與傳感器原定的安裝距離，計(jì)算兩激光位移傳感器所在直線與人孔平面的偏移角度；將所求角度轉(zhuǎn)換為旋轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)步數(shù)并反饋給電控旋轉(zhuǎn)工作臺(tái)，即此時(shí)激光位移傳感器所在位置即為機(jī)器人擺放角度。

為保證后續(xù)實(shí)驗(yàn)準(zhǔn)確性與可靠性，用此實(shí)驗(yàn)方法進(jìn)行多次定位，實(shí)驗(yàn)結(jié)果如表3所示。由于定位裝置擺放位置隨機(jī)，因此實(shí)驗(yàn)結(jié)果中的旋轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)角度與擺放位置不具有線性特征。

表3 定位實(shí)驗(yàn)記錄表

3.2 定位結(jié)果誤差分析

為測(cè)試定位方法的可靠性和重復(fù)性精度，在第一次利用定位裝置確定機(jī)器人的作業(yè)位置后，錄制了一整套機(jī)械臂由人孔進(jìn)入蒸汽發(fā)生器并完成安裝堵板的程序G，在實(shí)驗(yàn)1定位結(jié)果的基礎(chǔ)上運(yùn)行程序G并記錄機(jī)械臂末端位置作為參考標(biāo)準(zhǔn)點(diǎn)。然后將定位裝置多次面對(duì)人孔方向隨機(jī)擺放，用上述定位方法進(jìn)行重復(fù)實(shí)驗(yàn)后運(yùn)行程序G，機(jī)械臂按原路徑完成了一次側(cè)堵板安裝，但幾次試驗(yàn)的機(jī)械臂末端位置存在一定出入，機(jī)械臂末端位置記錄如表4所示。

表4 重復(fù)定位實(shí)驗(yàn)記錄表

將7次實(shí)驗(yàn)的期望值作為標(biāo)準(zhǔn)點(diǎn)A，坐標(biāo)為（x0，y0，z0），其他7組機(jī)械臂末端位置坐標(biāo)一次記錄為B（x1，y1，z1）、C

此定位方法的重復(fù)誤差為1.183 mm。

一般來(lái)說(shuō)，相對(duì)誤差更能反映測(cè)量的可信度，相對(duì)誤差設(shè)為δ，絕對(duì)誤差為Δ=1.183，L為整體尺寸，機(jī)器人的最大有效工作半徑為1400 mm，因此

計(jì)算結(jié)果表明，基于該定位方法下的重復(fù)定位精度較高，在實(shí)際實(shí)驗(yàn)研究中具有一定的可靠性。

3.3 測(cè)量結(jié)果不確定性分析

從測(cè)量數(shù)學(xué)模型可以看出，測(cè)量誤差、激光位移傳感器系統(tǒng)誤差、機(jī)械臂系統(tǒng)誤差是影響測(cè)量精度的重要因素。

首先，計(jì)算引用到的公式等存在近似限制，使用儀器時(shí)操作不規(guī)范，均會(huì)使實(shí)際測(cè)量與理論結(jié)果有所出入；其次，TOF激光位移傳感器系統(tǒng)自身存在一定的技術(shù)限制[18]。當(dāng)被測(cè)量對(duì)象的表面特別明亮且離TOF傳感器的距離非常近時(shí)，傳感器會(huì)向接收機(jī)散射多余的光線，從而產(chǎn)生偽影和一些不需要的反射。尤其是當(dāng)TOF傳感器的工作環(huán)境是晴朗的室外時(shí)，強(qiáng)烈的太陽(yáng)光線會(huì)加速傳感器的像素飽和，使其不能準(zhǔn)確快速地檢測(cè)到物體反射的實(shí)際光。再有，當(dāng)TOF傳感器測(cè)量的目標(biāo)面為轉(zhuǎn)角或者凹部時(shí)，會(huì)產(chǎn)生多次反射，給測(cè)量數(shù)據(jù)帶來(lái)很大的誤差，即造成測(cè)量結(jié)果的不確定性。

在驗(yàn)證定位結(jié)果準(zhǔn)確性時(shí)，利用了機(jī)械臂走G程序，記錄末端位置分析定位誤差的方法，誤差分析過(guò)程中，應(yīng)考慮機(jī)械臂本身存在的系統(tǒng)誤差[19]。

4 結(jié)論

本文提出了一種基于TOF激光位移傳感器的一次側(cè)堵板拆裝機(jī)器人的水室定位方法，涉及核電站自動(dòng)化檢修技術(shù)領(lǐng)域。在已設(shè)計(jì)的十自由度一次側(cè)堵板拆裝機(jī)器人進(jìn)行堵板拆裝作業(yè)時(shí)，由于機(jī)械臂尺寸略小于蒸汽發(fā)生器的人孔孔徑，對(duì)于機(jī)器人的工作位置要求較高。因此只有確定了機(jī)器人執(zhí)行堵板作業(yè)時(shí)的工作位置，才能方便機(jī)械臂進(jìn)入人孔并且為后續(xù)作業(yè)提供相對(duì)較大的活動(dòng)范圍和操作空間。首先建立蒸汽發(fā)生器與機(jī)器人的空間位置模型，得出機(jī)器人的移動(dòng)小車(chē)部分必須平行人孔平面的結(jié)論；再利用設(shè)計(jì)的激光位移傳感器定位裝置對(duì)小車(chē)的位置進(jìn)行測(cè)量與校正；最后通過(guò)測(cè)量機(jī)械臂末端的空間坐標(biāo)變化來(lái)驗(yàn)證此定位方法的重復(fù)性精度和可靠性。經(jīng)驗(yàn)證，此方法的相對(duì)重復(fù)定位誤差為0.0845%，可以為機(jī)器人提供較為理想的工作位置，有效降低機(jī)器人的操作難度，提高拆裝堵板的工作效率。