式：
[0077] 0 (t)=曰。+曰11+曰2*2+曰313
[0078] 運(yùn)動(dòng)軌跡的關(guān)節(jié)速度與加速度為：
[0081] 代入相應(yīng)的約束方程，得到有關(guān)系數(shù)a。，曰1，曰2,曰3,可得：
[0082] a〇 = 00
[0083] 日1 = 0
[0086] 由此確定了時(shí)間與角度的關(guān)系，當(dāng)進(jìn)行沖洗時(shí)，車體相對(duì)于絕緣子的具體是一定 的，設(shè)為，可得絕緣桿的長(zhǎng)度1 =d/sin0，帶入公式0 (t)=曰。+曰1*+曰2*2+曰3*3,可W得到?jīng)_ 洗過(guò)程中隨時(shí)間的變化關(guān)系：
[0087]
[008引對(duì)（1，d，0。，0f)進(jìn)行插值，生成水沖洗機(jī)械臂對(duì)絕緣子串的上下方向的沖洗軌 跡。
[0089] 路徑點(diǎn)上的關(guān)節(jié)速度可W根據(jù)需要設(shè)定，由此，確定H次多項(xiàng)式的方法就與直線 插補(bǔ)中所述完全相同，只是速度約束條件變?yōu)椋?br>[0090] 0 '（0) = 0 0'
[0091] 0，（tf)=0f'
[0092] 確定H次多項(xiàng)式的四個(gè)方程為：
[0097]0。，0f，e'。，e'f分別為起始點(diǎn)和時(shí)間參變量tf時(shí)刻的角度和角速度， 曰〇,曰1，曰2,曰3為待定系數(shù)。
[0098] 求解方程組，可得到H次多項(xiàng)式的系數(shù)：
[0099] a〇 = 00
[0100] 曰1 =白 〇'
[0101]
[0102]
[0103] 0。，0f，0 '。，0 'f分別為起始點(diǎn)和時(shí)間參變量tf時(shí)刻的角度和角速度， 曰0,曰1，曰2,曰3為待定系數(shù)。
[0104] 由此確定的H次多項(xiàng)式描述了起始點(diǎn)與終止點(diǎn)具有任意給定位置和速度的運(yùn)動(dòng) 軌跡，剩下的問(wèn)題就是如何確定路徑點(diǎn)上的關(guān)節(jié)速度，在系統(tǒng)中為了保證每個(gè)路徑點(diǎn)上的 加速度連續(xù)，控制系統(tǒng)會(huì)根據(jù)要求自動(dòng)選擇路徑點(diǎn)的速度。為了保證路徑點(diǎn)的加速度連續(xù)， 可W設(shè)法用兩條H次曲線在路徑點(diǎn)處按照一定規(guī)則連接起來(lái)拼湊成所要求的軌跡，其約束 條件是；連接處不僅速度連續(xù)，而且加速度也連續(xù)。
[0105]設(shè)所經(jīng)過(guò)的路徑點(diǎn)處的關(guān)節(jié)角度為0y，與該點(diǎn)相鄰的前后兩點(diǎn)的關(guān)節(jié)速度分別 為e。和0g。從e。到ey的插值H次多項(xiàng)式為：
[0106] 日（t) =ai〇+aiit+ai2t2+ai3t3
[0107] 從ey到eg的插值H次多項(xiàng)式為：
[010 引 日（t)=曰20+曰211+曰2212+曰2313
[0109] 上述兩個(gè)H次多項(xiàng)式的時(shí)間區(qū)間分別為[0, /,」和!0, 對(duì)送兩個(gè)多項(xiàng)式的約 束是：
[011引a。，i= 1，2,j= 0, 1，2, 3，為待定系數(shù)，tfi,tf2為時(shí)間參數(shù)。
[0119] W上約束條件組成了含有8個(gè)未知數(shù)的8個(gè)線性方程，對(duì)于tfi=tf2 =tf的情況， 送個(gè)方程的解為：
[0120] 曰1。=白 0
[0121] 曰11 = 0

[0128] a。，i=1，2,j=0, 1，2, 3，為待定系數(shù)，tf,tfi,tf2為時(shí)間參數(shù)。
[0129] 由此我們將得到的系數(shù)帶入0 (t) =aie+aiit+ai2t2+ai3t3,就可W得到關(guān)節(jié)角度隨 時(shí)間變化的關(guān)系。
[0130] 對(duì)狂1，Yi，0。，0f)進(jìn)行插值，生成對(duì)水沖洗機(jī)器人對(duì)朝向機(jī)器人一側(cè)的絕緣子串 進(jìn)行清洗的運(yùn)動(dòng)軌跡。
[0131] 為確保噴頭能夠?qū)Τ驒C(jī)器人一側(cè)的絕緣子串進(jìn)行清洗，即圖2中①-②， ②-⑨，⑨-④的中間過(guò)渡過(guò)程，采用逐點(diǎn)比較法的圓弧插補(bǔ)方式。進(jìn)行圓弧插補(bǔ)時(shí)，通常 W圓必為原點(diǎn)，根據(jù)圓弧起點(diǎn)與終點(diǎn)的坐標(biāo)值來(lái)進(jìn)行插補(bǔ)。
[013引設(shè)圓弧插補(bǔ)的圓弧起點(diǎn)坐標(biāo)為狂。，Y。)，終點(diǎn)坐標(biāo)為狂。，Y。），對(duì)于圓弧上任一點(diǎn) 狂1，Yi)，有恥+Yi2 =R2,令F=Xi2+Yi2-R2為偏差函數(shù)。當(dāng)F> 0時(shí)，該點(diǎn)在圓外，向-X方 向運(yùn)動(dòng)一步；當(dāng)F< 0時(shí)，該點(diǎn)在圓弧內(nèi)，向巧方向運(yùn)動(dòng)一步；為使運(yùn)動(dòng)繼續(xù)下去，將F= 0 歸入F> 0的情況，插補(bǔ)運(yùn)動(dòng)始終沿著圓弧并向終點(diǎn)運(yùn)動(dòng)。圓弧插補(bǔ)的判別計(jì)算可采用如 下的迭加運(yùn)算：
[013引設(shè)當(dāng)前點(diǎn)化，Yi)對(duì)應(yīng)的偏差函數(shù)為
[0134] Fi=Xi2+Yi2-R2
[0135] 當(dāng)噴槍沿-X方向走一步后
[0136] Fw=狂i-1)2+Yi2-r2 =Fi_2Xi+1
[0137] 當(dāng)噴槍沿巧方向走一步后
[013 引 Fw=Xi2+ 燈1+1) 2-r2 =Fi巧Yi+1
[0139] 終點(diǎn)判別可由n=IXg-X。I+1Yg-Y。I判別，每走一步使n=n+1,直至n= 0為止。
[0140] W上為第一象限逆圓弧的插補(bǔ)，其它象限的順逆圓弧的插補(bǔ)原理與第一象限相 似。
[0141] 把路徑點(diǎn)也看作是"起始點(diǎn)"和"終止點(diǎn)"，對(duì)送些路徑點(diǎn)求解逆運(yùn)動(dòng)學(xué)，得到相應(yīng) 的關(guān)節(jié)矢量值，然后確定所要求的H次多項(xiàng)式插值函數(shù)，把路徑點(diǎn)平滑的連接起來(lái)，但是送 些"起始點(diǎn)"和"終止點(diǎn)"的關(guān)節(jié)運(yùn)動(dòng)速度不再是零。
[0142] 進(jìn)行水沖洗時(shí)，調(diào)節(jié)出水速度，使雷諾數(shù)滿足47<氏<105,形成卡口潤(rùn)街。振動(dòng)頻率 與流體速度成正比，與阻流體的正面寬度成反比，調(diào)節(jié)水槍出水口徑，實(shí)現(xiàn)對(duì)絕緣子串振動(dòng) 頻率的調(diào)節(jié)，W產(chǎn)生良好的沖洗效果。
[0143] 通過(guò)W上步驟，提供了一種變電站水沖洗機(jī)器人的水沖洗控制方式，該方法能夠 實(shí)現(xiàn)水沖洗機(jī)器人沖洗作業(yè)的精準(zhǔn)控制，保證沖洗的質(zhì)量，降低帶電作業(yè)的安全隱患，提高 作業(yè)效率。
【主權(quán)項(xiàng)】
1. 基于激光測(cè)距的變電站水沖洗機(jī)器人的水沖洗控制方法，其特征是，包括： 步驟一：根據(jù)安裝在沖洗平臺(tái)上的激光測(cè)距儀和監(jiān)控?cái)z像頭采集距離及監(jiān)控的圖像得 到當(dāng)前機(jī)器人與絕緣子之間的相對(duì)距離，判斷該相對(duì)距離是否是安全距離，若是，進(jìn)行下一 步，若不是，調(diào)整位置，直至該相對(duì)距離為安全距離；確定路徑規(guī)劃的起始狀態(tài)； 步驟二：采用直線插補(bǔ)方式實(shí)現(xiàn)水沖洗機(jī)械臂對(duì)絕緣子串的上下方向的自動(dòng)沖洗； 步驟三：采用逐點(diǎn)比較法的圓弧插補(bǔ)方式實(shí)現(xiàn)噴頭能夠?qū)Τ驒C(jī)器人一側(cè)的絕緣子串 進(jìn)行清洗； 步驟四：在進(jìn)行水沖洗時(shí)，通過(guò)調(diào)節(jié)水槍出水口徑及出水速度，形成卡門渦街并實(shí)現(xiàn)對(duì) 絕緣子串振動(dòng)頻率的調(diào)節(jié)。2. 如權(quán)利要求1所述的基于激光測(cè)距的變電站水沖洗機(jī)器人的水沖洗控制方法，其特 征是，所述步驟二中，具體過(guò)程為： (2-1)在沖洗過(guò)程中采集得到起始時(shí)刻及終止時(shí)刻操作臂的關(guān)節(jié)角度； (2-2)對(duì)于單個(gè)關(guān)節(jié)的平穩(wěn)運(yùn)動(dòng)，軌跡函數(shù)0 (t)至少滿足四個(gè)約束條件，根據(jù)約束條 件對(duì)應(yīng)的約束方程唯一的確定一個(gè)三次多項(xiàng)式； (2-3)根據(jù)該三次多項(xiàng)式得到運(yùn)動(dòng)軌跡的關(guān)節(jié)速度與加速度，將運(yùn)動(dòng)軌跡的關(guān)節(jié)速度 與加速度代入約束方程得到三次多項(xiàng)式的系數(shù)，該系數(shù)確定了沖洗時(shí)間與沖洗角度的關(guān) 系； (2-4)根據(jù)絕緣桿的長(zhǎng)度及三次多項(xiàng)式得到?jīng)_洗過(guò)程中絕緣桿的長(zhǎng)度隨時(shí)間的變化關(guān) 系，對(duì)（l，d，0。，0f)進(jìn)行插值，生成水沖洗機(jī)械臂對(duì)絕緣子串的上下方向的沖洗軌跡。3. 如權(quán)利要求2所述的基于激光測(cè)距的變電站水沖洗機(jī)器人的水沖洗控制方法，其特 征是，所述約束條件，其中兩個(gè)是起始點(diǎn)和終止點(diǎn)對(duì)應(yīng)的關(guān)節(jié)角度： 0 (〇) =0 〇 ； 0 (tf) = 0f ； 其中，9。，9f為起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)角度。4. 如權(quán)利要求2所述的基于激光測(cè)距的變電站水沖洗機(jī)器人的水沖洗控制方法，其特 征是，為了滿足關(guān)節(jié)運(yùn)動(dòng)速度的連續(xù)性要求，另外還有兩個(gè)約束條件，即起始點(diǎn)與終止點(diǎn)的 關(guān)節(jié)速度要求，規(guī)定： 9 ' (0) = 0 ； 0 '（tf) = 0 ; 其中，e' (〇)，e' (tf)為起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)角速度。5. 如權(quán)利要求2所述的基于激光測(cè)距的變電站水沖洗機(jī)器人的水沖洗控制方法，其特 征是，所述四個(gè)邊界的約束條件唯一的確定了一個(gè)三次多項(xiàng)式： 9 (t) =a〇+a1t+a2t2+a3t3 ； 其中，0 (t)為時(shí)間參變量t時(shí)刻的關(guān)節(jié)角度，a。，ai，a2,a3為待定系數(shù)。6. 如權(quán)利要求2所述的基于激光測(cè)距的變電站水沖洗機(jī)器人的水沖洗控制方法，其特 征是，所述運(yùn)動(dòng)軌跡的關(guān)節(jié)速度與加速度為： 9' (t) =a1+2a2t+3a3t2 9" (t) = 2a2+6a3t〇7. 如權(quán)利要求2所述的基于激光測(cè)距的變電站水沖洗機(jī)器人的水沖洗控制方法，其特 征是，所述將運(yùn)動(dòng)軌跡的關(guān)節(jié)速度與加速度代入相應(yīng)的約束方程，得到有關(guān)系數(shù)a。，ai，a2， a3 : a〇 = 0 〇 巳工=0由此確定了時(shí)間與角度的關(guān)系，當(dāng)進(jìn)行沖洗時(shí)，車體相對(duì)于絕緣子的距離是一定的，設(shè) 為d，可得絕緣桿的長(zhǎng)度1 =d/sin0，帶入公式0 (t) =ao+ait+aj^+a；^3,可以得到?jīng)_洗過(guò) 程中絕緣桿的長(zhǎng)度隨時(shí)間的變化關(guān)系：對(duì)（l，d，0。，0f)進(jìn)行插值，生成水沖洗機(jī)械臂對(duì)絕緣子串的上下方向的沖洗軌跡。8. 如權(quán)利要求1所述的基于激光測(cè)距的變電站水沖洗機(jī)器人的水沖洗控制方法，其特 征是，所述步驟三中，逐點(diǎn)比較法的圓弧插補(bǔ)方式具體過(guò)程如下： 圓弧起點(diǎn)坐標(biāo)為（X。，Y。），終點(diǎn)坐標(biāo)為（Xe，YJ，對(duì)于圓弧上任一點(diǎn)（XdYJ，有=R2,令F=X^+Y^-R2為偏差函數(shù)，當(dāng)F> 0時(shí)，該點(diǎn)在圓外，向-X方向運(yùn)動(dòng)一步；當(dāng)F< 0 時(shí)，該點(diǎn)在圓弧內(nèi)，向+Y方向運(yùn)動(dòng)一步；為使運(yùn)動(dòng)繼續(xù)下去，將F= 0歸入F> 0的情況，插 補(bǔ)運(yùn)動(dòng)始終沿著圓弧并向終點(diǎn)運(yùn)動(dòng)。9. 如權(quán)利要求7所述的基于激光測(cè)距的變電站水沖洗機(jī)器人的水沖洗控制方法，其特 征是，圓弧插補(bǔ)的判別計(jì)算可采用如下的迭加運(yùn)算： 設(shè)當(dāng)前點(diǎn)（X^Yi)對(duì)應(yīng)的偏差函數(shù)為Fx = X^+Y^-R2 當(dāng)噴槍沿-X方向走一步后： F1+1 = (X-D^-R2 =F-2X.+1 當(dāng)噴槍沿+Y方向走一步后： F1+1 =XZ+CY.+I)2-^ =F1+2Y1+1 終點(diǎn)判別可由n= |Xe-X。| +1Ye-Y。|判別，每走一步使n=n+1，直至n= 0為止。10. 如權(quán)利要求1所述的基于激光測(cè)距的變電站水沖洗機(jī)器人的水沖洗控制方 法，其特征是，所述步驟四中，卡門渦街形成的條件：在流體中的圓柱體，其雷諾數(shù)滿足 47〈R6〈105,雷諾數(shù)用來(lái)表征流體流動(dòng)情況，以R6表示，R6 =Pvl/n，其中v、P、n分別為 流體的流速、密度與黏性系數(shù)，1為一特征長(zhǎng)度。
【專利摘要】本發(fā)明公開了基于激光測(cè)距的變電站水沖洗機(jī)器人的水沖洗控制方法，根據(jù)安裝在沖洗平臺(tái)上的激光測(cè)距儀和監(jiān)控?cái)z像頭采集距離及監(jiān)控的圖像得到當(dāng)前機(jī)器人與絕緣子之間的相對(duì)距離，判斷該相對(duì)距離是否是安全距離，若是，進(jìn)行下一步，若不是，調(diào)整位置，直至該相對(duì)距離為安全距離；確定路徑規(guī)劃的起始狀態(tài)；采用直線插補(bǔ)方式實(shí)現(xiàn)水沖洗機(jī)械臂對(duì)絕緣子串的上下方向的自動(dòng)沖洗；采用逐點(diǎn)比較法的圓弧插補(bǔ)方式實(shí)現(xiàn)噴頭能夠?qū)Τ驒C(jī)器人一側(cè)的絕緣子串進(jìn)行清洗；在進(jìn)行水沖洗時(shí)，通過(guò)調(diào)節(jié)水槍出水口徑及出水速度，形成卡門渦街并實(shí)現(xiàn)對(duì)絕緣子串振動(dòng)頻率的調(diào)節(jié)。本發(fā)明使機(jī)器人工作在安全距離，使沖洗軌跡的起點(diǎn)和終點(diǎn)定位準(zhǔn)確，提高沖洗的質(zhì)量和效率。
【IPC分類】B08B3/02, B08B13/00
【公開號(hào)】CN105013735
【申請(qǐng)?zhí)枴緾N201410459390
【發(fā)明人】王振利, 韓磊, 魯守銀, 慕世友, 任杰, 傅孟潮, 譚林, 李健, 呂曦晨, 張海龍, 王濱海, 李建祥, 趙金龍, 陳強(qiáng), 高郎宏
【申請(qǐng)人】山東魯能智能技術(shù)有限公司
【公開日】2015年11月4日
【申請(qǐng)日】2014年9月10日