船用碼垛機械臂運動控制算法設計

0 引言

隨著全球化的推進，世界各國間的經濟聯系變得更加緊密，海上運輸作為國際貿易的主要運輸方式之一，承擔著大量貨物和資源的運輸任務，因此需求量不斷增加。大型船舶搭載的人員和物資數量眾多，海上環境復雜多變，在船舶航行條件下安全開展艙內物資轉運作業面臨重大挑戰，因此研究高效、安全的船用物資搬運設備對于船舶物流保障具有重要意義。

船舶艙室內通常空間有限，而且設備、物資布局復雜緊湊，物資轉運路徑上往往會存在阻擋物，有些阻擋物可能難以從其高度方向跨越或者轉運物資需要在狹小范圍內進行回轉。本文選擇PLC作為控制器，利用ST語言和梯形圖設計實現一種電力驅動的碼垛機械臂運動控制算法，可使機械臂末端掛載的轉運物資跟隨操作人員移動路徑，在船舶艙室內靈活避障，提高物資轉運效率。

1機械臂結構模型

本文中的船用機械臂其機械結構由一級臂及其驅動裝置、二級臂及其驅動裝置、升降驅動裝置、抓取臂、操作面板和中心立柱等組成。中心立柱底部與船體基座連接，抓取臂負責固定連接待起吊轉運的物資。使用人員通過選擇操作面板布置的方向選擇按鈕控制一、二級臂的回轉和俯仰，使機械臂掛載負載跟隨操作人員的移動路徑起吊轉運。

機械臂結構抽象后的運動學模型視圖如圖1所示， M 點是機械臂抓取轉運物資的重心，將其在三維空間內的坐標視為轉運物資的位置。

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

2 正運動學

機械臂（機器人）運動學從幾何角度描述和研究機械臂末端位置與關節驅動變量的關系，由機械臂關節空間到機械臂工作空間的映射稱為正運動學[2-6]。選取三維空間運行軌跡 t₀ 時刻的二維平面采樣

如圖2所示。

2.1一級臂等效投影長度 l_1Proj 及轉運物資高度 h_mount

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

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

2.2轉運物資在采樣平面的投影坐標位置

定義一級臂等效投影按順時針方向與Y軸的夾角為一級臂轉動角度 θ₁ ，二級臂投影以二級驅動為中心點 （R 點）按順時針方向與一級臂的夾角為二級臂轉動角度 ，如圖2所示。

X^′O^′Y^′ 坐標系是以抓取臂與二級臂連接點A的投影為原點 （O^′） 的坐標系，其 X^′ 軸垂直于二級臂， Y^′ 軸平行于二級臂且指向二級驅動R。 X^′O^′Y^′ 坐標系原點（連接點A）也可認為是轉運物資在二維平面上的位置，其在XOY基座坐標系的位置是 （x₀，y₀） 。向量O^′d 是機械臂轉運物資的運動方向。

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

3 逆運動學

機械臂（機器人）逆運動學即已知機械臂末端位置姿態，計算機械臂對應位置的全部關節變量[2-]。

3.1 逆運動學建模

假定抓取臂在采樣平面內的跟隨速度是 v （單位： mm/s） ，1s移動位移為 Δs ，目標位置是基座坐標系坐標為 （x₁，y₁） 的 d 點， R^′ 是目標位置對應的二級驅動，如圖3所示。令 |Od|=Δs ，通過齊次變換可求得 （x₀ y₀） 到 （x₁，y₁） 的映射關系，即：

式中： θ₃ 是在 X^′O^′Y^′ 坐標系下機械臂轉運物資的運動跟隨方向； γ 是 X^′O^′Y^′ 坐標系 X^′ 軸相對于XOY坐標系 X 軸旋轉的角度。

求得坐標 （x₁，y₁） 值后可進一步求出采樣平面內目標位置到機械臂基座原點之間距離 的長度。

二級驅動對應夾角 ∠R^′Od= arccos l2+l2-l2，則一級驅動對應角度θ'=∠dOY+2l1Projlod∠R^′Od 在單位間隔時間內掛載物資從當前位置 O^′ 點到目標位置 d 點，二級驅動應轉動角度 Δθ₂=θ₂-θ₂' ，對應角速度 ω₂ ；一級驅動應轉動角度 Δθ₁=θ₁-θ₁' ，對應角速度 ω₁ 已知 ω₁°ω₂ 以及一、二級驅動減速器減速比，可進一步求得一、二級驅動伺服電機設定轉速。

3.2 跟隨速度的S形曲線加減速過程

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

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

式中： v_set 是當前給定速度； v_start 是起始給定速度； v_end 是目標給定速度；參數 flex 用來表征S形曲線的拉伸變化特性，即體現S形曲線的平滑程度，具體而言，flex值越大，曲線越陡峭，flex值越小，曲線越平滑； Num 表示給定加減速設定的總次數；i是當前速度給定的次數，i從0開始，到2Num結束。

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

4運動控制程序實現

本文中的運動跟隨控制算法將三維空間內掛載物資的轉運路徑分解為垂直方向和二維采樣平面上的方向。根據機械臂驅動位置基于機器人學正運動學建模求解掛載物資坐標位置，結合掛載物資給定跟隨速度求解機械臂逆運動學解，從而得到機械臂各個驅動伺服電機的設定轉速。運動控制算法作為核心功能模塊嵌入機械臂控制程序。運動控制算法在PLC的一個掃描周期內執行流程圖如圖5所示。

5 結論與展望

本文針對如何提高船舶艙室物資轉運作業效率這一現實問題，提出一種船舶艙室內安裝機械臂的跟隨控制算法，該機械臂在實際應用中尤其是在空間狹小的艙室內搬運物資上取得了良好的效果。展望未來，在已有基礎上，本型船用碼垛搬運機械臂將更加注重設備的柔性化和協同化：柔性化方面，機械臂將結合智能傳感器，更好地感知周圍環境，從而具備更強的適應性和靈活性；協同化方面，機械臂將通過無線網絡與其他自動化設備實現更緊密的協作，共同構建高效、智能的船舶物流轉運系統。

[參考文獻]

[1]王果，馮紅旭，劉蘇琪，等.基于Stateflow的船舶物資轉運系統的建模方法[J].計算機仿真，2019，36（11）：114-120.

[2]戰強.機器人學：機構、運動學、動力學及運動規劃[M].北京：清華大學出版社，2019.

[3]郝光鵬，寧志剛.柑橘采摘機械臂控制算法研究[J].現代農業裝備，2019，40（1：46-51.

[4]張光輝，王耀南.基于C/S結構的空間機械臂控制系統軟件架構設計[J].計算機應用，2014，34（10）：3059-3064.

[5]何萬賢，方輝，曾志康，等.特定自動采摘設備的機械臂位置控制算法的設計與實現[J].農業與技術，2023，43（15）：37-42.

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