0 引言

隨著全球化的推進(jìn)，世界各國(guó)間的經(jīng)濟(jì)聯(lián)系變得更加緊密，海上運(yùn)輸作為國(guó)際貿(mào)易的主要運(yùn)輸方式之一，承擔(dān)著大量貨物和資源的運(yùn)輸任務(wù)，因此需求量不斷增加。大型船舶搭載的人員和物資數(shù)量眾多，海上環(huán)境復(fù)雜多變，在船舶航行條件下安全開(kāi)展艙內(nèi)物資轉(zhuǎn)運(yùn)作業(yè)面臨重大挑戰(zhàn)，因此研究高效、安全的船用物資搬運(yùn)設(shè)備對(duì)于船舶物流保障具有重要意義。

船舶艙室內(nèi)通?？臻g有限，而且設(shè)備、物資布局復(fù)雜緊湊，物資轉(zhuǎn)運(yùn)路徑上往往會(huì)存在阻擋物，有些阻擋物可能難以從其高度方向跨越或者轉(zhuǎn)運(yùn)物資需要在狹小范圍內(nèi)進(jìn)行回轉(zhuǎn)。本文選擇PLC作為控制器，利用ST語(yǔ)言和梯形圖設(shè)計(jì)實(shí)現(xiàn)一種電力驅(qū)動(dòng)的碼垛機(jī)械臂運(yùn)動(dòng)控制算法，可使機(jī)械臂末端掛載的轉(zhuǎn)運(yùn)物資跟隨操作人員移動(dòng)路徑，在船舶艙室內(nèi)靈活避障，提高物資轉(zhuǎn)運(yùn)效率。

1機(jī)械臂結(jié)構(gòu)模型

本文中的船用機(jī)械臂其機(jī)械結(jié)構(gòu)由一級(jí)臂及其驅(qū)動(dòng)裝置、二級(jí)臂及其驅(qū)動(dòng)裝置、升降驅(qū)動(dòng)裝置、抓取臂、操作面板和中心立柱等組成。中心立柱底部與船體基座連接，抓取臂負(fù)責(zé)固定連接待起吊轉(zhuǎn)運(yùn)的物資。使用人員通過(guò)選擇操作面板布置的方向選擇按鈕控制一、二級(jí)臂的回轉(zhuǎn)和俯仰，使機(jī)械臂掛載負(fù)載跟隨操作人員的移動(dòng)路徑起吊轉(zhuǎn)運(yùn)。

機(jī)械臂結(jié)構(gòu)抽象后的運(yùn)動(dòng)學(xué)模型視圖如圖1所示， M 點(diǎn)是機(jī)械臂抓取轉(zhuǎn)運(yùn)物資的重心，將其在三維空間內(nèi)的坐標(biāo)視為轉(zhuǎn)運(yùn)物資的位置。

機(jī)械臂尺寸、位置的計(jì)量單位均為毫米， l_ED 表示一級(jí)臂長(zhǎng)度， l_EP 為一級(jí)臂最大起升高度。二級(jí)驅(qū)動(dòng)與一級(jí)臂的機(jī)械連接方式可使二級(jí)臂在船體艙室發(fā)生傾斜搖擺時(shí)始終與艙室地板面保持平行， l_BA 表示二級(jí)臂長(zhǎng)度。對(duì)抓取臂與二級(jí)臂的連接點(diǎn)A在三維空間上的連續(xù)運(yùn)動(dòng)軌跡 （x，y，z） 做二維平面的\"切片\"采樣，采樣平面與艙室地板面平行，從而得到連接點(diǎn)A在采樣時(shí)刻 垂直方向的高度 z₀ 以及在采樣二維平面內(nèi)的坐標(biāo)位置 （x₀，y₀） ，在采樣平面內(nèi)根據(jù)機(jī)械臂結(jié)構(gòu)的投影尺寸和掛載物資跟隨速度基于機(jī)械臂正逆運(yùn)動(dòng)學(xué)求解機(jī)械臂各個(gè)驅(qū)動(dòng)轉(zhuǎn)動(dòng)的速度。

2 正運(yùn)動(dòng)學(xué)

機(jī)械臂（機(jī)器人）運(yùn)動(dòng)學(xué)從幾何角度描述和研究機(jī)械臂末端位置與關(guān)節(jié)驅(qū)動(dòng)變量的關(guān)系，由機(jī)械臂關(guān)節(jié)空間到機(jī)械臂工作空間的映射稱為正運(yùn)動(dòng)學(xué)[2-6]。選取三維空間運(yùn)行軌跡 t₀ 時(shí)刻的二維平面采樣

如圖2所示。

2.1一級(jí)臂等效投影長(zhǎng)度 l_1Proj 及轉(zhuǎn)運(yùn)物資高度 h_mount

夾角 （lEc2+2-lr2），其中l(wèi)是電動(dòng)缸本體加其絲杠伸縮長(zhǎng)度，如圖1所示。根據(jù)機(jī)械臂尺寸以及三角形余弦定理可求得角度 ∠KEF 和 ∠GEH 數(shù)值，設(shè)定 θ_pitch 為一級(jí)臂俯仰角度，則 θ_pitch=∠KEF+ ∠GEH+∠FEG-90^°;l_lnealProj 是一級(jí)臂在艙室地板面實(shí)際投影長(zhǎng)度， ，進(jìn)一步求得 l_1Proj ，即

一級(jí)臂與中心立柱夾角 ∠DEH=180^°-（∠KEF+ ∠GEH+∠FEG） ，轉(zhuǎn)運(yùn)物資高度 h_mount=l_EP-（l_EDcos∠DEH+ l_CB+l_AM） 。

2.2轉(zhuǎn)運(yùn)物資在采樣平面的投影坐標(biāo)位置

定義一級(jí)臂等效投影按順時(shí)針?lè)较蚺cY軸的夾角為一級(jí)臂轉(zhuǎn)動(dòng)角度 θ₁ ，二級(jí)臂投影以二級(jí)驅(qū)動(dòng)為中心點(diǎn) （R 點(diǎn)）按順時(shí)針?lè)较蚺c一級(jí)臂的夾角為二級(jí)臂轉(zhuǎn)動(dòng)角度 ，如圖2所示。

X^′O^′Y^′ 坐標(biāo)系是以抓取臂與二級(jí)臂連接點(diǎn)A的投影為原點(diǎn) （O^′） 的坐標(biāo)系，其 X^′ 軸垂直于二級(jí)臂， Y^′ 軸平行于二級(jí)臂且指向二級(jí)驅(qū)動(dòng)R。 X^′O^′Y^′ 坐標(biāo)系原點(diǎn)（連接點(diǎn)A）也可認(rèn)為是轉(zhuǎn)運(yùn)物資在二維平面上的位置，其在XOY基座坐標(biāo)系的位置是 （x₀，y₀） 。向量O^′d 是機(jī)械臂轉(zhuǎn)運(yùn)物資的運(yùn)動(dòng)方向。

當(dāng) θ₂lt;180^° 時(shí)， ∠ORO^′=θ₂ 當(dāng) θ₂gt;180^° 時(shí)， ∠ORO^′= 360^°-θ₂ 。 X^′O^′Y^′ 坐標(biāo)系原點(diǎn)到XOY坐標(biāo)系原點(diǎn)距離 ，已知 l₀₀，.l_1Proj，l₂ ，根據(jù)三角形余弦定理可求得角度 值， θ₄=θ₂-β 。故求得采樣時(shí)刻二維平面 X^′O^′Y^′ 坐標(biāo)系原點(diǎn)在XOY坐標(biāo)系的位置 x₀=l_oo^′ sin θ₄ y₀=l_OO^′ cos θ₄ 。

3 逆運(yùn)動(dòng)學(xué)

機(jī)械臂（機(jī)器人）逆運(yùn)動(dòng)學(xué)即已知機(jī)械臂末端位置姿態(tài)，計(jì)算機(jī)械臂對(duì)應(yīng)位置的全部關(guān)節(jié)變量[2-]。

3.1 逆運(yùn)動(dòng)學(xué)建模

假定抓取臂在采樣平面內(nèi)的跟隨速度是 v （單位： mm/s） ，1s移動(dòng)位移為 Δs ，目標(biāo)位置是基座坐標(biāo)系坐標(biāo)為 （x₁，y₁） 的 d 點(diǎn)， R^′ 是目標(biāo)位置對(duì)應(yīng)的二級(jí)驅(qū)動(dòng)，如圖3所示。令 |Od|=Δs ，通過(guò)齊次變換可求得 （x₀ y₀） 到 （x₁，y₁） 的映射關(guān)系，即：

式中： θ₃ 是在 X^′O^′Y^′ 坐標(biāo)系下機(jī)械臂轉(zhuǎn)運(yùn)物資的運(yùn)動(dòng)跟隨方向； γ 是 X^′O^′Y^′ 坐標(biāo)系 X^′ 軸相對(duì)于XOY坐標(biāo)系 X 軸旋轉(zhuǎn)的角度。

求得坐標(biāo) （x₁，y₁） 值后可進(jìn)一步求出采樣平面內(nèi)目標(biāo)位置到機(jī)械臂基座原點(diǎn)之間距離 的長(zhǎng)度。

二級(jí)驅(qū)動(dòng)對(duì)應(yīng)夾角 ∠R^′Od= arccos l2+l2-l2，則一級(jí)驅(qū)動(dòng)對(duì)應(yīng)角度θ'=∠dOY+2l1Projlod∠R^′Od 在單位間隔時(shí)間內(nèi)掛載物資從當(dāng)前位置 O^′ 點(diǎn)到目標(biāo)位置 d 點(diǎn)，二級(jí)驅(qū)動(dòng)應(yīng)轉(zhuǎn)動(dòng)角度 Δθ₂=θ₂-θ₂' ，對(duì)應(yīng)角速度 ω₂ ；一級(jí)驅(qū)動(dòng)應(yīng)轉(zhuǎn)動(dòng)角度 Δθ₁=θ₁-θ₁' ，對(duì)應(yīng)角速度 ω₁ 已知 ω₁°ω₂ 以及一、二級(jí)驅(qū)動(dòng)減速器減速比，可進(jìn)一步求得一、二級(jí)驅(qū)動(dòng)伺服電機(jī)設(shè)定轉(zhuǎn)速。

3.2 跟隨速度的S形曲線加減速過(guò)程

跟隨速度的S形曲線加減速過(guò)程，是指在運(yùn)動(dòng)控制中，依據(jù)S形曲線函數(shù)模型，對(duì)跟隨速度在加速與減速階段進(jìn)行精細(xì)化調(diào)控。在加速階段，速度并非瞬間躍升，而是以平滑的S形曲線逐步提升，使得加速度從初始值緩慢增大至設(shè)定值；減速階段則反之，通過(guò)控制加速度的變化，實(shí)現(xiàn)速度的平穩(wěn)降低，確保運(yùn)動(dòng)過(guò)程的平穩(wěn)性和精確性。在伺服電機(jī)控制系統(tǒng)中，應(yīng)用S形曲線控制算法可有效降低負(fù)載所承受的沖擊，顯著提升系統(tǒng)的控制精度與響應(yīng)速度。該算法的核心在于將加減速過(guò)程進(jìn)行分段精細(xì)化控制，通過(guò)漸進(jìn)式地調(diào)整加減速的大小，使速度變化更加平滑。其實(shí)現(xiàn)方式通常是基于速度與位置函數(shù)，運(yùn)用插值計(jì)算方法，規(guī)劃電機(jī)在每個(gè)運(yùn)動(dòng)階段的狀態(tài)，從而實(shí)現(xiàn)對(duì)電機(jī)運(yùn)行的精準(zhǔn)控制。

抓取臂在采樣平面上的給定跟隨速度（單位：mm/s） 所對(duì)應(yīng)的S形曲線生成公式如下：

式中： v_set 是當(dāng)前給定速度； v_start 是起始給定速度； v_end 是目標(biāo)給定速度；參數(shù) flex 用來(lái)表征S形曲線的拉伸變化特性，即體現(xiàn)S形曲線的平滑程度，具體而言，flex值越大，曲線越陡峭，flex值越小，曲線越平滑； Num 表示給定加減速設(shè)定的總次數(shù)；i是當(dāng)前速度給定的次數(shù)，i從0開(kāi)始，到2Num結(jié)束。

生成的速度S形曲線圖形如圖4所示。

4運(yùn)動(dòng)控制程序?qū)崿F(xiàn)

本文中的運(yùn)動(dòng)跟隨控制算法將三維空間內(nèi)掛載物資的轉(zhuǎn)運(yùn)路徑分解為垂直方向和二維采樣平面上的方向。根據(jù)機(jī)械臂驅(qū)動(dòng)位置基于機(jī)器人學(xué)正運(yùn)動(dòng)學(xué)建模求解掛載物資坐標(biāo)位置，結(jié)合掛載物資給定跟隨速度求解機(jī)械臂逆運(yùn)動(dòng)學(xué)解，從而得到機(jī)械臂各個(gè)驅(qū)動(dòng)伺服電機(jī)的設(shè)定轉(zhuǎn)速。運(yùn)動(dòng)控制算法作為核心功能模塊嵌入機(jī)械臂控制程序。運(yùn)動(dòng)控制算法在PLC的一個(gè)掃描周期內(nèi)執(zhí)行流程圖如圖5所示。

5 結(jié)論與展望

本文針對(duì)如何提高船舶艙室物資轉(zhuǎn)運(yùn)作業(yè)效率這一現(xiàn)實(shí)問(wèn)題，提出一種船舶艙室內(nèi)安裝機(jī)械臂的跟隨控制算法，該機(jī)械臂在實(shí)際應(yīng)用中尤其是在空間狹小的艙室內(nèi)搬運(yùn)物資上取得了良好的效果。展望未來(lái)，在已有基礎(chǔ)上，本型船用碼垛搬運(yùn)機(jī)械臂將更加注重設(shè)備的柔性化和協(xié)同化：柔性化方面，機(jī)械臂將結(jié)合智能傳感器，更好地感知周圍環(huán)境，從而具備更強(qiáng)的適應(yīng)性和靈活性；協(xié)同化方面，機(jī)械臂將通過(guò)無(wú)線網(wǎng)絡(luò)與其他自動(dòng)化設(shè)備實(shí)現(xiàn)更緊密的協(xié)作，共同構(gòu)建高效、智能的船舶物流轉(zhuǎn)運(yùn)系統(tǒng)。

[參考文獻(xiàn)]

[1]王果，馮紅旭，劉蘇琪，等.基于Stateflow的船舶物資轉(zhuǎn)運(yùn)系統(tǒng)的建模方法[J].計(jì)算機(jī)仿真，2019，36（11）：114-120.

[2]戰(zhàn)強(qiáng).機(jī)器人學(xué)：機(jī)構(gòu)、運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)及運(yùn)動(dòng)規(guī)劃[M].北京：清華大學(xué)出版社，2019.

[3]郝光鵬，寧志剛.柑橘采摘機(jī)械臂控制算法研究[J].現(xiàn)代農(nóng)業(yè)裝備，2019，40（1：46-51.

[4]張光輝，王耀南.基于C/S結(jié)構(gòu)的空間機(jī)械臂控制系統(tǒng)軟件架構(gòu)設(shè)計(jì)[J].計(jì)算機(jī)應(yīng)用，2014，34（10）：3059-3064.

[5]何萬(wàn)賢，方輝，曾志康，等.特定自動(dòng)采摘設(shè)備的機(jī)械臂位置控制算法的設(shè)計(jì)與實(shí)現(xiàn)[J].農(nóng)業(yè)與技術(shù)，2023，43（15）：37-42.

[6]李煥，李海博，樊慶文，等.一種并聯(lián)二自由度機(jī)械臂的控制算法研究[J].機(jī)械，2017，44（11）：19-22.