一種主從式水下機(jī)械臂的延時(shí)控制方法
【專利摘要】本發(fā)明涉及機(jī)器人技術(shù)領(lǐng)域，提供的是一種能夠保證主從式水下機(jī)械臂在作業(yè)環(huán)境惡劣、傳輸通訊延時(shí)條件下，實(shí)現(xiàn)水下機(jī)械臂主手與從手的協(xié)調(diào)控制的主從式水下機(jī)械臂的延時(shí)控制方法。本發(fā)明包括：給定水下機(jī)械臂的結(jié)構(gòu)參數(shù)，根據(jù)水下機(jī)械臂的運(yùn)動(dòng)學(xué)模型，對(duì)主從機(jī)械臂進(jìn)行時(shí)延分析；采集水動(dòng)力因素，根據(jù)水下機(jī)械臂動(dòng)力學(xué)模型，驗(yàn)證機(jī)械臂性能是否滿足模型需求；對(duì)主手動(dòng)作信號(hào)進(jìn)行延時(shí)處理，將前n個(gè)時(shí)刻關(guān)節(jié)控制信號(hào)緩存在水下控制器中，然后基于多次冪曲線擬合方法計(jì)算出從手n+1時(shí)刻的關(guān)節(jié)理想運(yùn)動(dòng)位置。本發(fā)明解決了水下機(jī)械臂作業(yè)環(huán)境惡劣、傳輸通訊延時(shí)造成的水下機(jī)械臂工作間斷性等難題。
【專利說明】
一種主從式水下機(jī)械臂的延時(shí)控制方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及機(jī)器人技術(shù)領(lǐng)域，提供的是一種能夠保證主從式水下機(jī)械臂在作業(yè)環(huán) 境惡劣、傳輸通訊延時(shí)條件下，實(shí)現(xiàn)水下機(jī)械臂主手與從手的協(xié)調(diào)控制的主從式水下機(jī)械 臂的延時(shí)控制方法。
【背景技術(shù)】
[0002] 由于水下環(huán)境復(fù)雜，存在海水壓力、能見度低、溫度下降等不利于人類工作的環(huán)境 條件，可用主從水下多功能機(jī)械臂配合作業(yè)型水下機(jī)器人代替人類在深海等危險(xiǎn)環(huán)境工 作，水下機(jī)械臂可進(jìn)行海底勘探開采，水下管道維修，石油管道鋪建等工作。水下機(jī)械臂不 僅降低人類水下作業(yè)操作的危險(xiǎn)，還滿足人類擴(kuò)展了感知、對(duì)未知領(lǐng)域探索的需求。因此設(shè) 計(jì)主從水下機(jī)械臂控制系統(tǒng)具有重要的研究和實(shí)際工程意義。
[0003] 許多專家在水下機(jī)械臂的運(yùn)動(dòng)控制方面提出了很多方法。開關(guān)控制是水下機(jī)械臂 最簡(jiǎn)單也是最古老的控制方式，水面控制器由操作平臺(tái)開關(guān)按鈕組成，水面操作人員通過 操作對(duì)應(yīng)關(guān)節(jié)的開關(guān)開合來控制水下機(jī)械臂從手各關(guān)節(jié)的運(yùn)動(dòng)，操作人員通過觀察水下攝 像器傳輸回的圖像信息來判斷從手關(guān)節(jié)的運(yùn)動(dòng)位置，由上述可知，水下機(jī)械臂開關(guān)控制方 式為開環(huán)控制，并且開關(guān)控制的從手運(yùn)動(dòng)速度固定，所以控制精度主要依賴于操作人員的 經(jīng)驗(yàn)判斷，水下作業(yè)工作效率有限，并且多關(guān)節(jié)協(xié)助控制時(shí)對(duì)操作人員難度較大。
[0004] 相較于開關(guān)控制，速度控制的最大優(yōu)點(diǎn)就是關(guān)節(jié)運(yùn)動(dòng)速度的可控性。理想條件下， 關(guān)節(jié)從初始位置到目標(biāo)位置肯定經(jīng)過先加速、再勻速、再減速的過程，那么開關(guān)控制的關(guān)節(jié) 運(yùn)動(dòng)速度固定，這樣在最后很難準(zhǔn)確的停在目標(biāo)位置，而速度控制可以高效的調(diào)整關(guān)節(jié)運(yùn) 動(dòng)。速度控制方式通過比例閥箱控制各關(guān)節(jié)油液流量從而控制機(jī)械臂從手各關(guān)節(jié)的運(yùn)動(dòng)速 度。但是速度控制和開關(guān)控制有一個(gè)共同的缺點(diǎn)，那就是對(duì)水面操作人員的要求較高。速度 控制采用操作桿作為水面控制器，操作桿的操作方向與關(guān)節(jié)的運(yùn)動(dòng)方向可能不同，如大臂 關(guān)節(jié)為上下俯仰運(yùn)動(dòng)、操作桿操作方向?yàn)樽笥疫\(yùn)動(dòng)，這樣會(huì)增加操作人員工作量，并且不能 做到同時(shí)控制多個(gè)關(guān)節(jié)的速度。速度控制雖然解決了開關(guān)控制從手運(yùn)動(dòng)精準(zhǔn)度不高的問 題，但仍沒解決實(shí)際控制中操控人員操控困難的問題。
[0005] 隨著技術(shù)發(fā)展，考慮前兩種控制方式的弊端，研究人員設(shè)計(jì)位置反饋的控制方式， 也稱為主從控制方式，通過將操作桿改進(jìn)成從手縮小版模型進(jìn)行位置反饋，使得操作人員 可以形象化得知從手位置信息，不用像開關(guān)控制和速度控制似得在頭腦中構(gòu)建機(jī)械臂運(yùn)動(dòng) 模型，這樣可以避免操作對(duì)機(jī)械臂的損傷，并且方便操控人員同時(shí)控制多個(gè)關(guān)節(jié)運(yùn)動(dòng)。主從 控制方式使得操作人員操作水下機(jī)械臂時(shí)有人臂一體的感覺，有效地提高操作人員工作效 率。并且位置反饋控制也具有速度控制的優(yōu)點(diǎn)，通過操作主手發(fā)出的速度指令，也可以將速 度指令值用控制裝置經(jīng)積分變換為位置信息，進(jìn)行位置反饋控制。
[0006] 但是在水下作業(yè)實(shí)際控制中，水下機(jī)械臂在深海作業(yè)時(shí)，機(jī)械臂主手控制信號(hào)傳 輸距離遠(yuǎn)，傳統(tǒng)的485總線通訊傳輸速度慢，所以傳輸延時(shí)時(shí)間長，這樣就造成從手接收控 制信號(hào)滯后于水面操作，水下機(jī)械臂從手在工作中出現(xiàn)滯后、間斷運(yùn)動(dòng)等不良現(xiàn)象，影響水 面操作人員對(duì)從手位置的判斷，從而使操作人員的操作難度加大，進(jìn)而影響完成水下任務(wù) 的作業(yè)效率。
【發(fā)明內(nèi)容】
[0007] 本發(fā)明目的在于提供一種形式簡(jiǎn)單，減少計(jì)算量，能夠按需要得到優(yōu)化解，具有通 用性和快速性的主從式水下機(jī)械臂的延時(shí)控制方法。
[0008] 本發(fā)明的目的是這樣實(shí)現(xiàn)的：
[0009] 本發(fā)明包括以下步驟：
[0010] (1)給定水下機(jī)械臂的結(jié)構(gòu)參數(shù)，根據(jù)水下機(jī)械臂的運(yùn)動(dòng)學(xué)模型，對(duì)主從機(jī)械臂進(jìn) 行時(shí)延分析；
[0011] (2)采集水動(dòng)力因素，根據(jù)水下機(jī)械臂動(dòng)力學(xué)模型，驗(yàn)證機(jī)械臂性能是否滿足模型 需求；
[0012] (3)對(duì)主手動(dòng)作信號(hào)進(jìn)行延時(shí)處理，將前n個(gè)時(shí)刻關(guān)節(jié)控制信號(hào)緩存在水下控制器 中，然后基于多次冪曲線擬合方法計(jì)算出從手n+1時(shí)刻的關(guān)節(jié)理想運(yùn)動(dòng)位置；
[0013] (4)分析機(jī)械臂主手是否存在急停變向緊急操作以及機(jī)械臂從手是否存在洋流干 擾因素，通過延時(shí)控制環(huán)節(jié)和洋流反饋校正環(huán)節(jié)的判斷，實(shí)現(xiàn)主從水下機(jī)械臂平穩(wěn)連續(xù)控 制；
[0014] (5)根據(jù)運(yùn)動(dòng)學(xué)模型、動(dòng)力學(xué)模型以及延時(shí)控制算法建立控制系統(tǒng)模型，并基于該 模型進(jìn)行仿真，驗(yàn)證延時(shí)控制方法的有效性及可靠性。
[0015] 步驟2所述的水下機(jī)械臂動(dòng)力學(xué)模型在不考慮水動(dòng)力因素時(shí)，是運(yùn)用牛頓一歐拉 方程對(duì)機(jī)械臂進(jìn)行動(dòng)力學(xué)建模的，其中水下機(jī)械臂在任一運(yùn)動(dòng)時(shí)刻，將機(jī)械臂各個(gè)關(guān)節(jié)角 度變量qi、速度變量么、加速度變量矣，i = 1，. . .n作為已知條件，解算各關(guān)節(jié)的力矩變量Qi， i = l，.. .n，根據(jù)牛頓一歐拉法首先向前迭代計(jì)算機(jī)械臂的各連桿的速度和加速度，其中基 座初始運(yùn)動(dòng)狀態(tài)是確定的，如果基座固定，則基坐標(biāo)角速度 13 ?〇 = 〇,角加速度根據(jù)牛 頓一歐拉法向后迭代計(jì)算機(jī)械臂的各關(guān)節(jié)的力及力矩；初始條件是力矩^心力 &+i 冰下機(jī)械臂末端點(diǎn)可在機(jī)械臂工作空間內(nèi)自由活動(dòng)，則MdAnd為零；
[0016] (2.1)向前迭代解算機(jī)械臂各連桿的關(guān)節(jié)角度變量、速度變量：
[0024]其中，i+1coi+1為第i + 1個(gè)關(guān)節(jié)的角速度,11為+:1為第i+1個(gè)關(guān)節(jié)的角加速度;Ri+1,i為 第i個(gè)關(guān)節(jié)到第i+1個(gè)關(guān)機(jī)的旋轉(zhuǎn)矩陣為第i+1個(gè)關(guān)節(jié)的線加速度;為第i個(gè)關(guān)節(jié)的 質(zhì)心處的線加速度;U第i+1個(gè)關(guān)節(jié)自身的角速度;1ei為第i個(gè)關(guān)節(jié)的旋轉(zhuǎn)軸線;1+11 1+1為 第i+1個(gè)連桿的長度矢量;ipiSi坐標(biāo)系到i連桿質(zhì)心處的位置矢量;11+1為第i+1個(gè)關(guān)節(jié)到 質(zhì)心坐標(biāo)原點(diǎn)處的慣性張量;m 1+1為第i+1個(gè)關(guān)節(jié)的質(zhì)量;1+1F1+1為第i+1個(gè)關(guān)節(jié)的慣性力； 1+ iMl+1為第i+1個(gè)關(guān)節(jié)的慣性力矩；
[0025] (2.2)向后迭代解算機(jī)械臂關(guān)節(jié)的力?:和力矩弘i=n，n-l，? ? ?，1:
[0029] 其中，1為1坐標(biāo)系到i連桿受力點(diǎn)的位置矢量;％為第i個(gè)關(guān)節(jié)的關(guān)節(jié)力/馬，為 第i個(gè)關(guān)節(jié)相對(duì)于i坐標(biāo)系的關(guān)節(jié)力矩;QMi為第i個(gè)關(guān)節(jié)相對(duì)于i-1坐標(biāo)系的關(guān)節(jié)力矩；
[0030] 當(dāng)機(jī)械臂連桿關(guān)節(jié)的重量作為考慮因素時(shí)，設(shè)固定的機(jī)器人基座受到的 支撐作用相當(dāng)向上的重力加速度g，考慮水動(dòng)力因素時(shí)，選擇瑞雷耗散函數(shù)描述粘性阻尼和 流體阻力下物體的運(yùn)動(dòng)情況：
[0031] 在拉格朗日動(dòng)力學(xué)算法中引入瑞雷耗散函數(shù)：
[0033]可得帶有瑞雷耗散函數(shù)的拉格朗日方程：
[0035] 其中qs為廣義位置向量，&為相應(yīng)的速度向量，T為拉格朗日函數(shù)；
[0036] 由于牛頓一歐拉方法求出不含水動(dòng)力變量的力矩
[0037] 貝1J
[0039]對(duì)六自由度水下機(jī)械臂推導(dǎo)分析，各關(guān)節(jié)相對(duì)于前一關(guān)節(jié)的速度表示為(i = 1,2,3.....6)，根據(jù)剛體定軸轉(zhuǎn)動(dòng)定理，得出各關(guān)節(jié)平動(dòng)速度，進(jìn)而得到各關(guān)節(jié)的質(zhì)心點(diǎn)的 運(yùn)動(dòng)絕對(duì)速度°vi(i = l，2,3……6);
[0040]求解機(jī)械臂各個(gè)關(guān)節(jié)的耗散函數(shù)巾i(i = 1，. .6)，
[0042]得到水下機(jī)械臂系統(tǒng)的耗散函數(shù)為傘=傘1+傘2+傘3+傘4+傘5+傘6,將瑞雷耗散函數(shù)帶 入拉格朗日方程進(jìn)行推導(dǎo)，求導(dǎo)瑞雷耗散函數(shù)巾的廣義速率i，參數(shù)代入方程-? 即可求得水動(dòng)力阻尼系數(shù)。
[0043]步驟3所述的冪曲線擬合方法包括：曲線擬合是通過實(shí)驗(yàn)獲得有限對(duì)測(cè)試數(shù)據(jù) (Xl，yi)，利用這些數(shù)據(jù)來求取近似函數(shù)y = f(x)的，其中X為輸出量，y為被測(cè)物理量，延時(shí)控 制中時(shí)間為輸出量，關(guān)節(jié)角度為被測(cè)物理量;將曲線擬合法應(yīng)用到主從水下機(jī)械臂延時(shí)控 制中，延時(shí)環(huán)節(jié)時(shí)間設(shè)置為兩個(gè)參數(shù)周期，將這種方法稱為兩周期延時(shí)，加上由于485總線 傳輸造成的信號(hào)延時(shí)，從手的滯后主手三個(gè)周期運(yùn)動(dòng)，在主手運(yùn)動(dòng)i+3時(shí)刻，由于485通信信 號(hào)的延時(shí)，i+3時(shí)刻水下控制器剛接收到主手i+2時(shí)刻位置，控制器得知三個(gè)時(shí)刻的位置點(diǎn)， 即i+3時(shí)刻的從手位置、i+1時(shí)刻的主手控制信號(hào)、i+2時(shí)刻的主手控制信號(hào)，根據(jù)關(guān)節(jié)位置 時(shí)間信息，提前判斷出從手的運(yùn)動(dòng)變化趨勢(shì)，從而利用兩周期延時(shí)控制方法，計(jì)算出下一時(shí) 刻從手理想運(yùn)動(dòng)點(diǎn)；
[0044] 其中主手關(guān)節(jié)i在tk-2時(shí)控制角度值為0k-2，在控制角度值為0H，從手關(guān)節(jié)在 tk時(shí)刻實(shí)際角度值為0k，得到坐標(biāo)分別為(tk-2，0 k-2)、（tk-Jk-〇和(tk，0k)，根據(jù)關(guān)節(jié)曲線運(yùn) 動(dòng)擬合方程，得到t k+1時(shí)刻的角度預(yù)測(cè)值為0k+1:
[0045] 0k+i = 0k+v(k)XT+aT2/2
[0046] 其中，a為關(guān)節(jié)的角加速度，T為傳輸周期0.1s;v(k)為k時(shí)刻關(guān)節(jié)i轉(zhuǎn)動(dòng)速度；
[0047]由勻加速度方程得到：
[0049]進(jìn)行計(jì)算，得到^+1時(shí)刻的從手關(guān)節(jié)角度的理想值
[0051] 最終由當(dāng)前時(shí)刻從手關(guān)節(jié)角度和延時(shí)兩個(gè)時(shí)刻的主手關(guān)節(jié)角度來計(jì)算下一時(shí)刻 從手關(guān)節(jié)速度、角速度，從而得到理想的運(yùn)動(dòng)角度。
[0052] 步驟(4)所述的延時(shí)控制環(huán)節(jié)包括:對(duì)主手運(yùn)動(dòng)曲線進(jìn)行分析，從而判斷機(jī)械臂主 手是否存在變向，加速，減速的突變運(yùn)動(dòng)環(huán)節(jié)，進(jìn)而對(duì)從手進(jìn)行運(yùn)動(dòng)控制；
[0053] 其中第k步主手輸出目標(biāo)值x(k)與第k+1步主手輸出目標(biāo)值x(k+l)之間的角度差 為e(k)=X(k+l)-x(k)，對(duì)該角度差進(jìn)行判斷分析，根據(jù)角度差范圍的不同，控制從手下一 步如何運(yùn)動(dòng)：
[0054]當(dāng)|e(k)|〈0.2°時(shí)，主手控制變化，處于從手運(yùn)動(dòng)能力范圍內(nèi)，不存在緊急操作，延 時(shí)環(huán)節(jié)正常工作，利用多周期延時(shí)算法得出從手下一時(shí)刻的理想運(yùn)動(dòng)點(diǎn)；
[0055]當(dāng)| e(k) | >0.2°時(shí)，此時(shí)角度差超出機(jī)械臂校正能力范圍，主手出現(xiàn)急停避障運(yùn) 動(dòng)；機(jī)械臂停止運(yùn)動(dòng)，保持當(dāng)前位置，直到主手信號(hào)從新回到從手停止?fàn)顟B(tài)后，重新開始運(yùn) 動(dòng)。
[0056] 洋流反饋校正環(huán)節(jié)主要判斷是否存在洋流干擾對(duì)機(jī)械臂從手運(yùn)動(dòng)造成影響，如果 洋流影響超出了機(jī)械臂校正運(yùn)動(dòng)能力范圍，認(rèn)為洋流干擾影響過大，從手機(jī)械臂停止當(dāng)前 運(yùn)動(dòng)，直到機(jī)械臂主手從新回到機(jī)械臂從手停止位置，機(jī)械臂從手繼續(xù)跟隨主手運(yùn)動(dòng);如果 洋流影響較小，影響處于機(jī)械臂運(yùn)動(dòng)能力范圍內(nèi)，機(jī)械臂根據(jù)主手目標(biāo)位置進(jìn)行恰當(dāng)?shù)奈?置校正，再進(jìn)行延時(shí)環(huán)節(jié)控制，使得機(jī)械臂從手平穩(wěn)運(yùn)動(dòng)；
[0057] 其中第k步主手關(guān)節(jié)輸出目標(biāo)值x(k)與從手關(guān)節(jié)實(shí)際運(yùn)動(dòng)位置.小纟丨之間的誤差為 = A# ：) ，對(duì)該誤差進(jìn)行判斷分析，根據(jù)誤差范圍的不同，決定從手之后如何運(yùn)動(dòng)；
[0058] 當(dāng)| e(k) |〈0 ? 02°時(shí)，校正后的從手輸出值a: (/〇 = .Y(幻，無需校正，保持從手當(dāng)前運(yùn) 動(dòng)狀態(tài)；
[0059] 當(dāng)0 ? 02°〈 | e(k) |〈0 ? 2°時(shí)，校正后的從手輸出值x及)= i(f) + e(f)，誤差在機(jī)械臂 校正范圍能力內(nèi)，對(duì)從手當(dāng)前位置進(jìn)行校正；
[0060] 當(dāng)|6(1〇|>0.2°時(shí)，校正后從手輸出值'(幻=4幻，誤差超出機(jī)械臂校正能力范 圍，機(jī)械臂停止運(yùn)動(dòng)，保持當(dāng)前位置，直到主手信號(hào)從新回到從手停止?fàn)顟B(tài)后，重新開始運(yùn) 動(dòng)。
[0061] 本發(fā)明的有益效果在于：
[0062] 通過結(jié)合主從機(jī)械臂的結(jié)構(gòu)特點(diǎn)和工作方式，將延時(shí)控制方法引入到主從水下機(jī) 械臂的運(yùn)動(dòng)控制中，該方法能夠直觀快速的實(shí)現(xiàn)主從式水下機(jī)械臂的平滑穩(wěn)定控制。基于 冪曲線擬合的延時(shí)控制算法，通過對(duì)已知位置信息多次迭代的方式對(duì)從手進(jìn)行理想關(guān)節(jié)位 置確定，該方法能夠有效實(shí)現(xiàn)水下從機(jī)械臂運(yùn)動(dòng)軌跡平滑，避免洋流干擾和操作不當(dāng)造成 運(yùn)動(dòng)抖動(dòng)和停頓。基于主從機(jī)械臂的延遲控制方法，為遠(yuǎn)程遙操作方式工作的機(jī)器人控制 方法提供了借鑒，具有一定通用性和實(shí)用性。最終本發(fā)明解決了水下機(jī)械臂作業(yè)環(huán)境惡劣、 傳輸通訊延時(shí)造成的水下機(jī)械臂工作間斷性等難題。該方法形式簡(jiǎn)潔，具有很高的精度和 求解速度，滿足水下機(jī)械臂運(yùn)動(dòng)曲線平滑穩(wěn)定的作業(yè)需要。
【附圖說明】
[0063] 圖1主從機(jī)械臂的控制系統(tǒng)流程圖；
[0064] 圖2水下機(jī)械臂的桿件關(guān)系圖；
[0065]圖3曲線擬合法示意圖；
[0066]圖4洋流校正判斷；
[0067] 圖5 Simulink控制系統(tǒng)圖；
[0068]圖6水下機(jī)械臂結(jié)構(gòu)簡(jiǎn)圖；
[0069]圖7洋流干擾模擬圖；
[0070] 圖8洋流干擾的主從機(jī)械臂單關(guān)節(jié)兩周期延時(shí)控制仿真圖。
【具體實(shí)施方式】
[0071] 下面結(jié)合附圖對(duì)本發(fā)明做進(jìn)一步描述。
[0072] 本發(fā)明涉及主從水下機(jī)械臂控制系統(tǒng)設(shè)計(jì)領(lǐng)域，提供了一種主從式水下機(jī)械臂的 延時(shí)控制方法。本發(fā)明以主從式水下機(jī)械臂的時(shí)延分析入手，利用冪曲線擬合的延時(shí)控制 算法，確定主從式機(jī)械臂協(xié)調(diào)控制的過程，通過運(yùn)動(dòng)學(xué)模型的建立、延時(shí)控制環(huán)節(jié)和洋流反 饋環(huán)節(jié)的校正、動(dòng)力學(xué)校核，快速找到水下機(jī)械臂從手各關(guān)節(jié)優(yōu)化位置，能夠保證主從式水 下機(jī)械臂在作業(yè)環(huán)境惡劣、傳輸通訊延時(shí)條件下，實(shí)現(xiàn)水下機(jī)械臂主手與從手的協(xié)調(diào)控制。 本發(fā)明解決的主從式機(jī)械臂協(xié)調(diào)控制的通用性問題，不依賴機(jī)器人構(gòu)形，形式簡(jiǎn)單，降低了 求解的難度，減少計(jì)算量，能夠按需要得到優(yōu)化解，具有通用性和快速性，滿足給定目標(biāo)任 務(wù)和環(huán)境對(duì)主從式水下機(jī)械臂的協(xié)調(diào)控制的要求。
[0073] 步驟1:給定水下機(jī)械臂的結(jié)構(gòu)參數(shù)，建立機(jī)械臂的運(yùn)動(dòng)學(xué)模型，同時(shí)計(jì)算出主手 到從手的通信延時(shí)時(shí)間。
[0074]步驟2:考慮水動(dòng)力因素，建立水下機(jī)械臂動(dòng)力學(xué)模型，用以驗(yàn)證機(jī)械臂性能是否 滿足模型需求，并且可以應(yīng)用于實(shí)際控制操作中。
[0075]步驟3:對(duì)主手動(dòng)作信號(hào)進(jìn)行延時(shí)處理，將前n個(gè)時(shí)刻關(guān)節(jié)控制信號(hào)緩存在水下控 制器中，然后基于多次冪曲線擬合方法計(jì)算出從手n+1時(shí)刻的關(guān)節(jié)理想運(yùn)動(dòng)位置。
[0076] 步驟4:分析機(jī)械臂主手是否存在急停變向等緊急操作以及機(jī)械臂從手是否存在 洋流干擾等因素，通過延時(shí)控制環(huán)節(jié)和洋流反饋校正環(huán)節(jié)的判斷，實(shí)現(xiàn)主從水下機(jī)械臂平 穩(wěn)連續(xù)控制。
[0077] 步驟5:根據(jù)運(yùn)動(dòng)學(xué)模型、動(dòng)力學(xué)模型以及延時(shí)控制算法可以建立控制系統(tǒng)模型， 并基于該模型進(jìn)行仿真，驗(yàn)證延時(shí)控制方法的有效性及可靠性。
[0078] 實(shí)施例1，結(jié)合附圖1，本發(fā)明的方法包括如下步驟：
[0079] 步驟1:給定水下機(jī)械臂的結(jié)構(gòu)參數(shù)，建立機(jī)械臂的運(yùn)動(dòng)學(xué)模型，同時(shí)計(jì)算出主手 到從手的通信延時(shí)時(shí)間。
[0080] 步驟2:考慮水動(dòng)力因素，建立水下機(jī)械臂動(dòng)力學(xué)模型，用以驗(yàn)證機(jī)械臂性能是否 滿足模型需求，并且可以應(yīng)用于實(shí)際控制操作中。
[0081 ]步驟3:對(duì)主手動(dòng)作信號(hào)進(jìn)行延時(shí)處理，將前n個(gè)時(shí)刻關(guān)節(jié)控制信號(hào)緩存在水下控 制器中，然后基于多次冪曲線擬合方法計(jì)算出從手n+1時(shí)刻的關(guān)節(jié)理想運(yùn)動(dòng)位置。
[0082]步驟4:分析機(jī)械臂主手是否存在急停變向等緊急操作以及機(jī)械臂從手是否存在 洋流干擾等因素，通過延時(shí)控制環(huán)節(jié)和洋流反饋校正環(huán)節(jié)的判斷，實(shí)現(xiàn)主從水下機(jī)械臂平 穩(wěn)連續(xù)控制。
[0083]步驟5:根據(jù)運(yùn)動(dòng)學(xué)模型、動(dòng)力學(xué)模型以及延時(shí)控制算法可以建立控制系統(tǒng)模型， 并基于該模型進(jìn)行仿真，驗(yàn)證延時(shí)控制方法的有效性及可靠性。
[0084]實(shí)施例2,結(jié)合附圖2,建立水下機(jī)械臂的動(dòng)力學(xué)模型。首先不考慮水動(dòng)力因素，運(yùn) 用牛頓一歐拉方程對(duì)機(jī)械臂進(jìn)行動(dòng)力學(xué)建模。
[0085] 水下機(jī)械臂在任一運(yùn)動(dòng)時(shí)刻，將機(jī)械臂各個(gè)關(guān)節(jié)角度變量9:、速度變量么、加速度 變量成，i = l,. . .n作為已知條件，解算各關(guān)節(jié)的力矩變量Qi，i = l,. . .n。根據(jù)牛頓一歐拉法 首先向前迭代計(jì)算機(jī)械臂的各連桿的速度和加速度。假設(shè)基座初始運(yùn)動(dòng)狀態(tài)是確定的，如 果基座固定，則卻=〇。之后根據(jù)牛頓一歐拉法向后迭代計(jì)算機(jī)械臂的各關(guān)節(jié)的力 及力矩。初始條件是，巧 +1 = 。假設(shè)水下機(jī)械臂末端點(diǎn)可在機(jī)械臂工作空間 內(nèi)自由活動(dòng)，則Mend、Fend為零。
[0086] (1)向前迭代解算機(jī)械臂各連桿的qi、4 / = 0.1…n-1-.
[0094] 其中：i+1?i+1為第i+1個(gè)關(guān)節(jié)的角速度；
[0095] ;+>+1為第i+1個(gè)關(guān)節(jié)的角加速度；
[0096] R1+1>1為第i個(gè)關(guān)節(jié)到第i+1個(gè)關(guān)機(jī)的旋轉(zhuǎn)矩陣；
[0097] f+S>f+1為第i+1個(gè)關(guān)節(jié)的線加速度；
[0098] Vci為第i個(gè)關(guān)節(jié)的質(zhì)心處的線加速度；
[0099] 么+,為第i+1個(gè)關(guān)節(jié)自身的角速度；
[0100] iei為第i個(gè)關(guān)節(jié)的旋轉(zhuǎn)軸線；
[0101] i+1li+1為第i+1個(gè)連桿的長度矢量；
[0102] ipi為i坐標(biāo)系到i連桿質(zhì)心處的位置矢量；
[0103] 11+1為第i+1個(gè)關(guān)節(jié)到質(zhì)心坐標(biāo)原點(diǎn)處的慣性張量；
[0104] mi+1為第i+1個(gè)關(guān)節(jié)的質(zhì)量；
[0105] i+1Fi+1為第i+1個(gè)關(guān)節(jié)的慣性力；
[0106] 1+1M1+1為第i+1個(gè)關(guān)節(jié)的慣性力矩。
[0107] (2)向后迭代解算機(jī)械臂關(guān)節(jié)的力?:和力矩弘i=n，n-l，? ? ?，1:
[0111]其中"hi為i坐標(biāo)系到i連桿受力點(diǎn)的位置矢量；
[0112] 為第i個(gè)關(guān)節(jié)的關(guān)節(jié)力；
[0113] 為第i個(gè)關(guān)節(jié)相對(duì)于i坐標(biāo)系的關(guān)節(jié)力矩；
[0114] 為第i個(gè)關(guān)節(jié)相對(duì)于i-1坐標(biāo)系的關(guān)節(jié)力矩。
[0115]當(dāng)機(jī)械臂連桿關(guān)節(jié)的重量作為考慮因素時(shí)，可設(shè)°匕=§，固定的機(jī)器人基座受到 的支撐作用相當(dāng)向上的重力加速度g。這樣處理與各模塊重力的影響完全一樣。有上述推導(dǎo) 可得機(jī)械臂動(dòng)力學(xué)模型。
[0116]考慮水動(dòng)力因素時(shí)，選擇瑞雷耗散函數(shù)描述粘性阻尼和流體阻力下物體的運(yùn)動(dòng)情 況。具體求解機(jī)械臂的瑞雷耗散函數(shù)過程如下：
[0117]在拉格朗日動(dòng)力學(xué)算法中引入瑞雷耗散函數(shù)
[0119]可得帶有瑞雷耗散函數(shù)的拉格朗日方程
[0121]其中qs為廣義位置向量，為相應(yīng)的速度向量，T為拉格朗日函數(shù)。
[0122] 由于牛頓一歐拉方法求出不含水動(dòng)力變量的力矩
[0123] 貝 IJ
[0125] 對(duì)六自由度水下機(jī)械臂推導(dǎo)分析，各關(guān)節(jié)相對(duì)于前一關(guān)節(jié)的速度表示為i_lvi(i = 1,2,3.....6)。根據(jù)剛體定軸轉(zhuǎn)動(dòng)定理，可得出各關(guān)節(jié)平動(dòng)速度，進(jìn)而可以得到各關(guān)節(jié)的 質(zhì)心點(diǎn)的運(yùn)動(dòng)絕對(duì)速度Vi (i = 1，2，3……6)。
[0126] 求解機(jī)械臂各個(gè)關(guān)節(jié)的耗散函數(shù)巾i(i = 1，. .6)，
[0128] 由此可得水下機(jī)械臂系統(tǒng)的耗散函數(shù)為巾=巾巾2+巾3+巾4+巾5+巾6。將瑞雷耗 散函數(shù)帶入拉格朗日方程進(jìn)行推導(dǎo)，求導(dǎo)瑞雷耗散函數(shù)巾的廣義速率4，參數(shù)代入方程
即可求得水動(dòng)力阻尼系數(shù)。
[0129] 實(shí)施例3,結(jié)合附圖3,曲線擬合是通過實(shí)驗(yàn)獲得有限對(duì)測(cè)試數(shù)據(jù)(Xl，yi)，利用這些 數(shù)據(jù)來求取近似函數(shù)y = f (x)。式中x為輸出量，y為被測(cè)物理量。延時(shí)控制中時(shí)間為輸出量， 關(guān)節(jié)角度為被測(cè)物理量。
[0130] 將曲線擬合法應(yīng)用到主從水下機(jī)械臂延時(shí)控制中，延時(shí)環(huán)節(jié)時(shí)間設(shè)置為兩個(gè)參數(shù) 周期，將這種方法稱為兩周期延時(shí)。加上由于485總線傳輸造成的信號(hào)延時(shí)，從手的滯后主 手三個(gè)周期運(yùn)動(dòng)，流程分析:在主手運(yùn)動(dòng)i+3時(shí)刻，由于485通信信號(hào)的延時(shí)，i+3時(shí)刻水下控 制器剛接收到主手i+2時(shí)刻位置，由此控制器得知三個(gè)時(shí)刻的位置點(diǎn)，即i+3時(shí)刻的從手位 置(根據(jù)水下控制閉環(huán)反饋得知）、i+l時(shí)刻的主手控制信號(hào)、i+2時(shí)刻的主手控制信號(hào)。根據(jù) 關(guān)節(jié)位置時(shí)間信息，可以提前判斷出從手的運(yùn)動(dòng)變化趨勢(shì)，比如變向、急停、變速等情況，從 而利用兩周期延時(shí)控制方法，可以計(jì)算出下一時(shí)刻從手理想運(yùn)動(dòng)點(diǎn)。
[0131] 假定主手關(guān)節(jié)i在tk-2時(shí)控制角度值為0k-2，在控制角度值為0H，從手關(guān)節(jié)在 tk時(shí)刻實(shí)際角度值為0k，則可得其坐標(biāo)分別為(tk-2，0 k-2)、（tk-10k-〇和(tk，0k)，根據(jù)關(guān)節(jié)曲 線運(yùn)動(dòng)擬合方程，可得t k+1時(shí)刻的角度預(yù)測(cè)值為0k+1，其值為
[0132] 0k+i = 0k+v(k) XT+aT2/2 (15)
[0133] 其中，a是關(guān)節(jié)的角加速度，T為傳輸周期0.1s;v(k)為k時(shí)刻關(guān)節(jié)i轉(zhuǎn)動(dòng)速度。
[0134] 由勻加速度方程可以推出
[0136]綜合(15)、（16)式進(jìn)行計(jì)算，得到^+1時(shí)刻的從手關(guān)節(jié)角度的理想值
[0138]這就是兩周期延時(shí)的計(jì)算公式。由當(dāng)前時(shí)刻從手關(guān)節(jié)角度和延時(shí)兩個(gè)時(shí)刻的主手 關(guān)節(jié)角度來計(jì)算下一時(shí)刻從手關(guān)節(jié)速度、角速度，從而得到理想的運(yùn)動(dòng)角度。
[0139] 實(shí)施例4,結(jié)合附圖4,延時(shí)控制使得水下機(jī)械臂從手滯后于主手運(yùn)動(dòng)，使得控制器 得知主手的后續(xù)運(yùn)動(dòng)軌跡，可以得出從手運(yùn)動(dòng)的理想值，但實(shí)際操作中存在急停變向等緊 急操作，所以需要對(duì)主手運(yùn)動(dòng)曲線進(jìn)行分析，從而判斷機(jī)械臂主手是否存在變向，加速，減 速的突變運(yùn)動(dòng)環(huán)節(jié)，進(jìn)而對(duì)從手進(jìn)行運(yùn)動(dòng)控制。
[0140] 假設(shè)第k步主手輸出目標(biāo)值x(k)與第k+1步主手輸出目標(biāo)值x(k+l)之間的角度差 為e(k)=X(k+l)-x(k)，對(duì)該角度差進(jìn)行判斷分析，根據(jù)角度差范圍的不同，控制從手下一 步如何運(yùn)動(dòng)。
[0141 ]當(dāng)| e(k) |〈0.2°時(shí)，主手控制變化，處于從手運(yùn)動(dòng)能力范圍內(nèi)，不存在避障、急速變 向等緊急操作，延時(shí)環(huán)節(jié)正常工作，利用多周期延時(shí)算法得出從手下一時(shí)刻的理想運(yùn)動(dòng)點(diǎn)。
[0142] 當(dāng)| e(k) | >0.2°時(shí)，此時(shí)角度差超出機(jī)械臂校正能力范圍，此狀況是由主手出現(xiàn)急 停避障運(yùn)動(dòng)。機(jī)械臂停止運(yùn)動(dòng)，保持當(dāng)前位置，直到主手信號(hào)從新回到從手停止?fàn)顟B(tài)后，重 新開始運(yùn)動(dòng)。
[0143] 洋流反饋校正環(huán)節(jié)的特征是計(jì)算從手實(shí)際運(yùn)動(dòng)位置和主手相應(yīng)時(shí)刻目標(biāo)位置的 差值，判讀示意圖如附圖4所示，主要判斷是否存在洋流干擾對(duì)機(jī)械臂從手運(yùn)動(dòng)造成影響， 如果洋流影響超出了機(jī)械臂校正運(yùn)動(dòng)能力范圍，認(rèn)為洋流干擾影響過大，從手機(jī)械臂停止 當(dāng)前運(yùn)動(dòng)，直到機(jī)械臂主手從新回到機(jī)械臂從手停止位置，機(jī)械臂從手繼續(xù)跟隨主手運(yùn)動(dòng)， 反之，如果洋流影響較小，影響處于機(jī)械臂運(yùn)動(dòng)能力范圍內(nèi)，機(jī)械臂根據(jù)主手目標(biāo)位置進(jìn)行 恰當(dāng)?shù)奈恢眯Ｕ?，再進(jìn)行延時(shí)環(huán)節(jié)控制，使得機(jī)械臂從手平穩(wěn)運(yùn)動(dòng)。
[0144] 假設(shè)第k步主手關(guān)節(jié)輸出目標(biāo)值x(k)與從手關(guān)節(jié)實(shí)際運(yùn)動(dòng)位置i(/c)之間的誤差為 以人：>=a# :>;，對(duì)該誤差進(jìn)行判斷分析，根據(jù)誤差范圍的不同，決定從手之后如何運(yùn)動(dòng)。
[0145] 當(dāng)|e(k)|〈0.02°時(shí)，校正后的從手輸出值'(幻=.說），誤差不大，無需校正，保持 從手當(dāng)前運(yùn)動(dòng)狀態(tài)。
[0146] 當(dāng)0 ? 02°〈 | e(k) |〈0 ? 2°時(shí)，校正后的從手輸出值A(chǔ): (/f) = x(/〇 ，誤差在機(jī)械臂 校正范圍能力內(nèi)，對(duì)從手當(dāng)前位置進(jìn)行校正。
[0147] 當(dāng)| e(k) | >0 ? 2°時(shí)，校正后從手輸出值⑷=x(/〇，此時(shí)誤差超出機(jī)械臂校正能力 范圍，此狀況是由于洋流干擾過大造成。機(jī)械臂停止運(yùn)動(dòng)，保持當(dāng)前位置，直到主手信號(hào)從 新回到從手停止?fàn)顟B(tài)后，重新開始運(yùn)動(dòng)。
[0148] 實(shí)施5，結(jié)合圖5，水下機(jī)械臂是一個(gè)六輸入六輸出的系統(tǒng)，控制系統(tǒng)包括控制算法 部分，運(yùn)動(dòng)學(xué)模型部分及動(dòng)力學(xué)模型部分，設(shè)定七功能機(jī)械臂各個(gè)關(guān)節(jié)的運(yùn)動(dòng)軌跡，運(yùn)動(dòng)軌 跡為連續(xù)的曲線，對(duì)其進(jìn)行分段處理，每隔單位時(shí)間根據(jù)控制算法對(duì)控制信號(hào)進(jìn)行采樣，運(yùn) 用M文件編寫控制算法。
[0149] 根據(jù)延時(shí)控制算法，可以得出新的關(guān)節(jié)運(yùn)動(dòng)目標(biāo)點(diǎn)。將新的運(yùn)動(dòng)目標(biāo)點(diǎn)帶入運(yùn)動(dòng) 學(xué)模型中，并封裝在M文件中，解算出各關(guān)節(jié)的運(yùn)動(dòng)軌跡。
[0150]動(dòng)力學(xué)模型封裝在相應(yīng)的M文件中，得到各關(guān)節(jié)運(yùn)動(dòng)軌跡后，對(duì)各個(gè)關(guān)節(jié)進(jìn)行動(dòng)力 學(xué)力矩分析，從而得出各個(gè)關(guān)節(jié)的相關(guān)控制力矩。
[0151] Simul ink控制系統(tǒng)框圖如附圖5所示。
[0152] 實(shí)施6,結(jié)合附圖6,舉例進(jìn)行驗(yàn)證，舉例水下七功能機(jī)械臂的關(guān)節(jié)參數(shù)如表1所示， 七功能水下機(jī)械臂結(jié)構(gòu)簡(jiǎn)圖如附圖6所示。
[0153] 表1七功能機(jī)械臂各桿參數(shù)
[0155] 運(yùn)用Pro/E工具對(duì)多功能水下機(jī)械臂進(jìn)行模型搭建，從而解算出機(jī)械臂各連桿的 重心向量、質(zhì)量和轉(zhuǎn)動(dòng)慣性矩等參數(shù)，七功能機(jī)械臂各關(guān)節(jié)參數(shù)如下表2、3所示。
[0156] 表2七功能機(jī)械臂各連桿質(zhì)量及重心向量
[0158] 表3七功能機(jī)械臂各連桿慣性矩
[0159]
[0160] 假設(shè)六自由度水下機(jī)械臂各關(guān)節(jié)初始狀態(tài)為:qz = [ 0，0，0，0，0，0 ]
[0162] 采用相同的一組隨機(jī)數(shù)作為洋流干擾，洋流干擾如附圖7所示。主手操作人員操作 主手單關(guān)節(jié)，先慢后快的增大關(guān)節(jié)角度，突然急停變向，操作主手關(guān)節(jié)重新回到從手停止處 后，繼續(xù)操作關(guān)節(jié)角度增加，之后改變關(guān)節(jié)角度變化方向，操作關(guān)節(jié)角度勻速減小。
[0163] 洋流干擾的單關(guān)節(jié)兩周期延時(shí)控制仿真如附圖8所示。
【主權(quán)項(xiàng)】
1. 一種主從式水下機(jī)械臂的延時(shí)控制方法，其特征在于，包括以下步驟： (1) 給定水下機(jī)械臂的結(jié)構(gòu)參數(shù)，根據(jù)水下機(jī)械臂的運(yùn)動(dòng)學(xué)模型，對(duì)主從機(jī)械臂進(jìn)行時(shí) 延分析； (2) 采集水動(dòng)力因素，根據(jù)水下機(jī)械臂動(dòng)力學(xué)模型，驗(yàn)證機(jī)械臂性能是否滿足模型需 求； (3) 對(duì)主手動(dòng)作信號(hào)進(jìn)行延時(shí)處理，將前n個(gè)時(shí)刻關(guān)節(jié)控制信號(hào)緩存在水下控制器中， 然后基于多次冪曲線擬合方法計(jì)算出從手n+1時(shí)刻的關(guān)節(jié)理想運(yùn)動(dòng)位置； (4) 分析機(jī)械臂主手是否存在急停變向緊急操作以及機(jī)械臂從手是否存在洋流干擾因 素，通過延時(shí)控制環(huán)節(jié)和洋流反饋校正環(huán)節(jié)的判斷，實(shí)現(xiàn)主從水下機(jī)械臂平穩(wěn)連續(xù)控制； (5) 根據(jù)運(yùn)動(dòng)學(xué)模型、動(dòng)力學(xué)模型以及延時(shí)控制算法建立控制系統(tǒng)模型，并基于該模型 進(jìn)行仿真，驗(yàn)證延時(shí)控制方法的有效性及可靠性。2. 根據(jù)權(quán)利要求1所述的一種主從式水下機(jī)械臂的延時(shí)控制方法，其特征在于：步驟 (2)所述的水下機(jī)械臂動(dòng)力學(xué)模型在不考慮水動(dòng)力因素時(shí)，是運(yùn)用牛頓一歐拉方程對(duì)機(jī)械 臂進(jìn)行動(dòng)力學(xué)建模的，其中水下機(jī)械臂在任一運(yùn)動(dòng)時(shí)刻，將機(jī)械臂各個(gè)關(guān)節(jié)角度變量qi、速 度變量#、加速度變量氣，i = l，. . .n作為已知條件，解算各關(guān)節(jié)的力矩變量Qi，i = l，. . .n， 根據(jù)牛頓一歐拉法首先向前迭代計(jì)算機(jī)械臂的各連桿的速度和加速度，其中基座初始運(yùn)動(dòng) 狀態(tài)是確定的，如果基座固定，則基坐標(biāo)角速度％() = 〇,角加速度4=〇;根據(jù)牛頓一歐拉法 向后迭代計(jì)算機(jī)械臂的各關(guān)節(jié)的力及力矩;初始條件是力矩力巧水 下機(jī)械臂末端點(diǎn)可在機(jī)械臂工作空間內(nèi)自由活動(dòng)，則MmcKF-為零； (2.1) 向前迭代解算機(jī)械臂各連桿的關(guān)節(jié)角度變量、速度變量：其中，i+1 i+1為第i+1個(gè)關(guān)節(jié)的角速度嚴(yán)1為+1為第i+1個(gè)關(guān)節(jié)的角加速度;Ri +1, i為第i個(gè) 關(guān)節(jié)到第i+1個(gè)關(guān)機(jī)的旋轉(zhuǎn)矩陣廣為第i+1個(gè)關(guān)節(jié)的線加速度;為第i個(gè)關(guān)節(jié)的質(zhì)心 處的線加速度;為第i+1個(gè)關(guān)節(jié)自身的角速度; 1&為第i個(gè)關(guān)節(jié)的旋轉(zhuǎn)軸線;1+111+1為第i+ 1個(gè)連桿的長度矢量;ipiSi坐標(biāo)系到i連桿質(zhì)心處的位置矢量^1 1+1為第i + 1個(gè)關(guān)節(jié)到質(zhì)心 坐標(biāo)原點(diǎn)處的慣性張量;m1+1為第i+1個(gè)關(guān)節(jié)的質(zhì)量;1+1F 1+1為第i+1個(gè)關(guān)節(jié)的慣性力;1+1M1+1 為第i+1個(gè)關(guān)節(jié)的慣性力矩； (2.2) 向后迭代解算機(jī)械臂關(guān)節(jié)的力Fi和力矩Qi i = n,n-l, ? ? ? ,1: ^M^'Ad^-R + ^FJiA QM,=Kx；MJ^elA 其中，1為1坐標(biāo)系到i連桿受力點(diǎn)的位置矢量;％為第i個(gè)關(guān)節(jié)的關(guān)節(jié)力/焉；為第i個(gè) 關(guān)節(jié)相對(duì)于i坐標(biāo)系的關(guān)節(jié)力矩;QMi為第i個(gè)關(guān)節(jié)相對(duì)于i_l坐標(biāo)系的關(guān)節(jié)力矩； 當(dāng)機(jī)械臂連桿關(guān)節(jié)的重量作為考慮因素時(shí)，設(shè)= g，固定的機(jī)器人基座受到的支撐作 用相當(dāng)向上的重力加速度g，考慮水動(dòng)力因素時(shí)，選擇瑞雷耗散函數(shù)描述粘性阻尼和流體阻 力下物體的運(yùn)動(dòng)情況： 在拉格朗日動(dòng)力學(xué)算法中引入瑞雷耗散函數(shù)：可得帶有瑞雷耗散函數(shù)的拉格朗日方程：其中qs為廣義位置向量，I為相應(yīng)的速度向量，T為拉格朗日函數(shù)；由于牛頓一歐拉方法求出不含水動(dòng)力變量的 則對(duì)六自由度水下機(jī)械臂推導(dǎo)分析，各關(guān)節(jié)相對(duì)于前一關(guān)節(jié)的速度表示為(i = 1，2， 3……6)，根據(jù)剛體定軸轉(zhuǎn)動(dòng)定理，得出各關(guān)節(jié)平動(dòng)速度，進(jìn)而得到各關(guān)節(jié)的質(zhì)心點(diǎn)的運(yùn)動(dòng) 絕對(duì)速度°vi(i = l，2,3……6); 求解機(jī)械臂各個(gè)關(guān)節(jié)的耗散函數(shù)巾i(i = 1，. . 6)，得到水下機(jī)械臂系統(tǒng)的耗散函數(shù)為1+ 2+巾3+巾4+巾5+巾6，將瑞雷耗散函數(shù)帶 入拉格朗日方程進(jìn)行推導(dǎo)，求導(dǎo)瑞雷耗散函數(shù)巾的廣義速率&，參數(shù)代入 即可求得水動(dòng)力阻尼系數(shù)。3.根據(jù)權(quán)利要求1所述的一種主從式水下機(jī)械臂的延時(shí)控制方法，其特征在于：步驟 (3)所述的冪曲線擬合方法包括：曲線擬合是通過實(shí)驗(yàn)獲得有限對(duì)測(cè)試數(shù)據(jù)(Xl，yi)，利用這 些數(shù)據(jù)來求取近似函數(shù)y = f (x)的，其中x為輸出量，y為被測(cè)物理量，延時(shí)控制中時(shí)間為輸 出量，關(guān)節(jié)角度為被測(cè)物理量;將曲線擬合法應(yīng)用到主從水下機(jī)械臂延時(shí)控制中，延時(shí)環(huán)節(jié) 時(shí)間設(shè)置為兩個(gè)參數(shù)周期，將這種方法稱為兩周期延時(shí)，加上由于485總線傳輸造成的信號(hào) 延時(shí)，從手的滯后主手三個(gè)周期運(yùn)動(dòng)，在主手運(yùn)動(dòng)i+3時(shí)刻，由于485通信信號(hào)的延時(shí)，i+3時(shí) 刻水下控制器剛接收到主手i+2時(shí)刻位置，控制器得知三個(gè)時(shí)刻的位置點(diǎn)，即i+3時(shí)刻的從 手位置、i + 1時(shí)刻的主手控制信號(hào)、i+2時(shí)刻的主手控制信號(hào)，根據(jù)關(guān)節(jié)位置時(shí)間信息，提前 判斷出從手的運(yùn)動(dòng)變化趨勢(shì)，從而利用兩周期延時(shí)控制方法，計(jì)算出下一時(shí)刻從手理想運(yùn) 動(dòng)點(diǎn)； 其中主手關(guān)節(jié)i在tk-2時(shí)控制角度值為0k-2，在控制角度值為0!^，從手關(guān)節(jié)在t k時(shí) 亥嫉際角度值為知，得到坐標(biāo)分別為⑷-以^以^^滴^^丄根據(jù)關(guān)節(jié)曲線運(yùn)動(dòng)擬 合方程，得到tk+1時(shí)刻的角度預(yù)測(cè)值為0 k+1: 0k+i = 9k+v(k) XT+aT2/2 其中，a為關(guān)節(jié)的角加速度，T為傳輸周期0.1 s; v (k)為k時(shí)刻關(guān)節(jié)i轉(zhuǎn)動(dòng)速度； 由勻加速度方程得到：進(jìn)行計(jì)算，得到^+1時(shí)刻的從手關(guān)節(jié)角度的理想值最終由當(dāng)前時(shí)刻從手關(guān)節(jié)角度和延時(shí)兩個(gè)時(shí)刻的主手關(guān)節(jié)角度來計(jì)算下一時(shí)刻從手 關(guān)節(jié)速度、角速度，從而得到理想的運(yùn)動(dòng)角度。4.根據(jù)權(quán)利要求1所述的一種主從式水下機(jī)械臂的延時(shí)控制方法，其特征在于：步驟 (4)所述的延時(shí)控制環(huán)節(jié)包括:對(duì)主手運(yùn)動(dòng)曲線進(jìn)行分析，從而判斷機(jī)械臂主手是否存在變 向，加速，減速的突變運(yùn)動(dòng)環(huán)節(jié)，進(jìn)而對(duì)從手進(jìn)行運(yùn)動(dòng)控制； 其中第k步主手輸出目標(biāo)值x(k)與第k+1步主手輸出目標(biāo)值x(k+l)之間的角度差為e (k) = x(k+l)_x(k)，對(duì)該角度差進(jìn)行判斷分析，根據(jù)角度差范圍的不同，控制從手下一步如 何運(yùn)動(dòng)： 當(dāng)|e(k)|〈0.2°時(shí)，主手控制變化，處于從手運(yùn)動(dòng)能力范圍內(nèi)，不存在緊急操作，延時(shí)環(huán) 節(jié)正常工作，利用多周期延時(shí)算法得出從手下一時(shí)刻的理想運(yùn)動(dòng)點(diǎn)； 當(dāng)|e(k)| >0.2°時(shí)，此時(shí)角度差超出機(jī)械臂校正能力范圍，主手出現(xiàn)急停避障運(yùn)動(dòng);機(jī) 械臂停止運(yùn)動(dòng)，保持當(dāng)前位置，直到主手信號(hào)從新回到從手停止?fàn)顟B(tài)后，重新開始運(yùn)動(dòng)。 洋流反饋校正環(huán)節(jié)主要判斷是否存在洋流干擾對(duì)機(jī)械臂從手運(yùn)動(dòng)造成影響，如果洋流 影響超出了機(jī)械臂校正運(yùn)動(dòng)能力范圍，認(rèn)為洋流干擾影響過大，從手機(jī)械臂停止當(dāng)前運(yùn)動(dòng)， 直到機(jī)械臂主手從新回到機(jī)械臂從手停止位置，機(jī)械臂從手繼續(xù)跟隨主手運(yùn)動(dòng);如果洋流 影響較小，影響處于機(jī)械臂運(yùn)動(dòng)能力范圍內(nèi)，機(jī)械臂根據(jù)主手目標(biāo)位置進(jìn)行恰當(dāng)?shù)奈恢眯?正，再進(jìn)行延時(shí)環(huán)節(jié)控制，使得機(jī)械臂從手平穩(wěn)運(yùn)動(dòng)； 其中第k步主手關(guān)節(jié)輸出目標(biāo)值x(k)與從手關(guān)節(jié)實(shí)際運(yùn)動(dòng)位置4/()之間的誤差為 e(fc) = ，對(duì)該誤差進(jìn)行判斷分析，根據(jù)誤差范圍的不同，決定從手之后如何運(yùn)動(dòng)； 當(dāng)|e(k)|〈0.02°時(shí)，校正后的從手輸出值.= ，無需校正，保持從手當(dāng)前運(yùn)動(dòng)狀 態(tài)； 當(dāng)0.02°〈 | e(k) |〈0.2°時(shí)，校正后的從手輸出值，誤差在機(jī)械臂校正 范圍能力內(nèi)，對(duì)從手當(dāng)前位置進(jìn)行校正； 當(dāng)I e(k) | >0 ? 2°時(shí)，校正后從手輸出值.Y<.⑷=a?⑷，誤差超出機(jī)械臂校正能力范圍，機(jī)械 臂停止運(yùn)動(dòng)，保持當(dāng)前位置，直到主手信號(hào)從新回到從手停止?fàn)顟B(tài)后，重新開始運(yùn)動(dòng)。
【文檔編號(hào)】G05B13/04GK106054599SQ201610352534
【公開日】2016年10月26日
【申請(qǐng)日】2016年5月25日
【發(fā)明人】魏延輝, 羅姍姍, 姚貴鵬, 喬金鶴, 張皓淵, 李寧波, 郭銳, 洪國慶
【申請(qǐng)人】哈爾濱工程大學(xué)