聯(lián)合仿真下機(jī)械臂終端約束模型預(yù)測(cè)控制方法

張 蕾，李義帥，王曉華，王文杰

(1.西安工程大學(xué) 電子信息學(xué)院，陜西 西安 710048；2.陜西省人工智能聯(lián)合實(shí)驗(yàn)室 西安工程大學(xué)分部，陜西 西安 710048)

0 引 言

隨著全球新冠疫情的反復(fù)以及人口老齡化所造成的勞動(dòng)力短缺與用工難等問(wèn)題，機(jī)械臂的應(yīng)用需求日益旺盛[1]。機(jī)械臂逐步替代人類生產(chǎn)生活中具有枯燥重復(fù)及惡劣危險(xiǎn)性質(zhì)的工作，而完成這些工作對(duì)機(jī)械臂的性能具有極高的考驗(yàn)，對(duì)機(jī)械臂的本體設(shè)計(jì)制造與控制方法的設(shè)計(jì)都是挑戰(zhàn)。機(jī)械臂采用傳統(tǒng)的瀑布式流程設(shè)計(jì)開(kāi)發(fā)，存在著開(kāi)發(fā)效率低下、成本較高等問(wèn)題，且機(jī)械臂的軌跡跟蹤控制是機(jī)械臂大規(guī)模應(yīng)用的基礎(chǔ)性問(wèn)題[2-3]。

針對(duì)機(jī)械臂的軌跡跟蹤控制問(wèn)題，國(guó)內(nèi)外眾多學(xué)者對(duì)此深入研究并取得頗多建設(shè)性的成果[4-5]。在日常應(yīng)用中，傳統(tǒng)的PID控制方法是機(jī)械臂運(yùn)動(dòng)控制應(yīng)用最廣泛的控制方法之一，但是多關(guān)節(jié)機(jī)械臂存在著非線性耦合現(xiàn)象，其各個(gè)連桿之間存在著相互力作用，會(huì)影響到機(jī)械臂高速運(yùn)動(dòng)時(shí)的控制精度以及穩(wěn)定性，且無(wú)法解決機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)存在的約束問(wèn)題，會(huì)造成較大的跟蹤誤差以及抖動(dòng)現(xiàn)象[6-9]。模型預(yù)測(cè)控制(model predictive control，MPC)是目前主流的先進(jìn)控制方法之一，具備在復(fù)雜條件下處理約束的能力，在機(jī)械臂運(yùn)動(dòng)控制的領(lǐng)域不斷發(fā)展，在學(xué)術(shù)界和工業(yè)界都有著大量的研究與應(yīng)用，已成為解決機(jī)械臂控制問(wèn)題的流行方法[10-11]。HASSAN等針對(duì)柔性單關(guān)節(jié)機(jī)械臂振動(dòng)抑制的問(wèn)題，采用有限元法建模，基于階躍響應(yīng)實(shí)現(xiàn)動(dòng)態(tài)矩陣控制，但階躍響應(yīng)并不能很好地應(yīng)用在多關(guān)節(jié)機(jī)械臂控制[12]；HEDJAR等針對(duì)雙關(guān)節(jié)機(jī)械臂軌跡跟蹤控制問(wèn)題，采用泰勒展開(kāi)法對(duì)機(jī)械臂模型進(jìn)行線性化處理，設(shè)計(jì)固定控制律，同時(shí)加入積分環(huán)節(jié)確保系統(tǒng)的魯棒性，但是對(duì)機(jī)械臂所建立的模型與本體之間的精確度要求高，且未對(duì)約束條件進(jìn)行處理[13]；BELAD等針對(duì)工業(yè)并聯(lián)機(jī)械臂，對(duì)外界噪聲擾動(dòng)進(jìn)行估計(jì)實(shí)現(xiàn)調(diào)整模型預(yù)測(cè)控制方法的參數(shù)，雖然抑制了系統(tǒng)擾動(dòng)造成的誤差，但是忽略了約束條件，并不能應(yīng)用在真實(shí)硬件環(huán)境上[14]。在進(jìn)行控制方法設(shè)計(jì)時(shí)，通常情況采用理論計(jì)算來(lái)研究機(jī)械臂的動(dòng)力學(xué)控制問(wèn)題，但是存在計(jì)算量過(guò)大的弊端，而且專家學(xué)者們對(duì)機(jī)械臂模型進(jìn)行了簡(jiǎn)化，使得理論仿真結(jié)果并不理想[15]。另外，真實(shí)的機(jī)械臂測(cè)試成本較高且在實(shí)驗(yàn)室環(huán)境下存在一定危險(xiǎn)，建立具有更好的技術(shù)基礎(chǔ)設(shè)施的高度復(fù)雜的實(shí)驗(yàn)臺(tái)需要巨大的成本。為克服這些問(wèn)題，可以使用仿真環(huán)境模擬、可視化和驗(yàn)證控制方法。

針對(duì)機(jī)械臂傳統(tǒng)開(kāi)發(fā)過(guò)程存在的跟蹤誤差較大、無(wú)法處理約束、效率低下、成本較高等問(wèn)題，本文提出一種基于終端約束的機(jī)械臂軌跡跟蹤模型預(yù)測(cè)控制方法。該控制方法通過(guò)設(shè)置目標(biāo)函數(shù)及終端狀態(tài)等式約束條件將機(jī)械臂軌跡跟蹤問(wèn)題轉(zhuǎn)化成模型預(yù)測(cè)控制的數(shù)值規(guī)劃問(wèn)題。再基于模型在環(huán)的仿真思想，采用輕量化低成本設(shè)備樹(shù)莓派Raspberry Pi搭建基于Gazebo-Simulink的機(jī)械臂聯(lián)合仿真平臺(tái)，驗(yàn)證本文所提控制方法的有效性。

1 三關(guān)節(jié)機(jī)械臂

良好的機(jī)器人仿真系統(tǒng)能夠構(gòu)造接近真實(shí)世界的物理環(huán)境，快速測(cè)試機(jī)器人設(shè)計(jì)可靠性以及控制方法的可用性[16-17]。Gazebo是一款開(kāi)源軟件，對(duì)于復(fù)雜的多剛體仿真速度快、實(shí)時(shí)性強(qiáng)，相比于傳統(tǒng)的ADAMS軟件，更適合對(duì)機(jī)械臂進(jìn)行動(dòng)力學(xué)仿真。通過(guò)與Simulink進(jìn)行協(xié)同工作，借助Simulink的圖形化編程可以實(shí)現(xiàn)快速編程，降低開(kāi)發(fā)難度，提高開(kāi)發(fā)效率[18]。

本文研究的被控對(duì)象不是機(jī)械臂的數(shù)學(xué)模型，是Gazebo中的機(jī)械臂物理模型，其模型文件包含了機(jī)械臂的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)參數(shù)，Gazebo中的機(jī)械臂物理模型具備真實(shí)機(jī)械臂的動(dòng)力學(xué)特性，需要根據(jù)動(dòng)力學(xué)方程所呈現(xiàn)出來(lái)的機(jī)械臂系統(tǒng)的驅(qū)動(dòng)力矩與關(guān)節(jié)位移的輸入輸出關(guān)系進(jìn)行控制方法設(shè)計(jì)。

接下來(lái)建立SoildWorks中的虛擬物理樣機(jī)模型，并導(dǎo)出機(jī)械臂的統(tǒng)一機(jī)器人描述格式(unified robot description format ，URDF)文件，為后續(xù)的Gazebo機(jī)械臂物理仿真的實(shí)現(xiàn)提供建?；A(chǔ)；然后，基于拉格朗日法建立三關(guān)節(jié)機(jī)械臂的動(dòng)力學(xué)方程，為后續(xù)進(jìn)行機(jī)械臂軌跡跟蹤控制方法的設(shè)計(jì)提供理論基礎(chǔ)。

1.1 三關(guān)節(jié)機(jī)械臂物理建模

以實(shí)驗(yàn)室與中科深谷公司共同開(kāi)發(fā)的三關(guān)節(jié)機(jī)械臂ROCR3為被控對(duì)象進(jìn)行分析研究，該三關(guān)節(jié)機(jī)械臂本體由連接部件、底座以及末端部件3個(gè)可重組的關(guān)節(jié)模組組成，是典型的三關(guān)節(jié)擬人臂。該三關(guān)節(jié)機(jī)械臂實(shí)體及尺寸參數(shù)如圖1所示。

(a) 三關(guān)節(jié)機(jī)械臂實(shí)體 (b)機(jī)械臂尺寸參數(shù)圖 1 三關(guān)節(jié)機(jī)械臂及尺寸參數(shù)Fig.1 Three-joint manipulator and its parameters

機(jī)械臂必須在滿足本體參數(shù)限制的條件下運(yùn)行，若負(fù)載超限制則會(huì)損壞機(jī)械臂。機(jī)械臂控制方法設(shè)計(jì)必須符合機(jī)械臂的物理特性滿足約束條件，否則也將會(huì)嚴(yán)重?fù)p壞機(jī)械臂，甚至?xí)?duì)使用者的人身安全造成威脅。本文嚴(yán)格參考機(jī)械臂的真實(shí)數(shù)據(jù)，依照機(jī)械臂的CAD文件，通過(guò)SolidWorks建立其虛擬物理樣機(jī)模型。SolidWorks中的虛擬物理樣機(jī)模型包含了機(jī)械臂的質(zhì)量屬性、尺寸等參數(shù)。Solidworks中的虛擬物理樣機(jī)的建立為下一步建立機(jī)械臂的動(dòng)力學(xué)方程做了初步準(zhǔn)備，同時(shí)也為下一步建立Gazebo中的機(jī)械臂物理模型做建模準(zhǔn)備。通過(guò)SolidWorks的sw_urdf_exporter插件配置并導(dǎo)出機(jī)械臂的URDF文件，通過(guò)此插件導(dǎo)出機(jī)械臂的URDF文件，相比基于文本的編程方式建立機(jī)械臂的URDF文件，可以大大提高機(jī)械臂模型建立的效率，節(jié)省時(shí)間并減少建模過(guò)程中的錯(cuò)誤。機(jī)械臂的URDF文件內(nèi)容包含機(jī)械臂的連桿、關(guān)節(jié)名稱、運(yùn)動(dòng)學(xué)參數(shù)、動(dòng)力學(xué)參數(shù)、可視化模型、碰撞檢測(cè)模型等[19]。首先通過(guò)check_urdf命令驗(yàn)證URDF文件的正確性，然后在Gazebo中通過(guò)可視化仿真驗(yàn)證機(jī)械臂的建模正確性。圖2為SolidWorks中的虛擬物理樣機(jī)模型以及Gazebo中的機(jī)械臂物理模型。

(a) SolidWorks虛擬樣機(jī) (b) Gazebo物理模型圖 2 機(jī)械臂物理仿真模型Fig.2 Physical model of manipulator

從圖2可以看出，Gazebo中所建立的機(jī)械臂模型和SolidWorks中的虛擬物理樣機(jī)模型以及真實(shí)機(jī)械臂關(guān)節(jié)及連桿具有一致性，證明Gazebo中的機(jī)械臂物理模型建模正確。

1.2 三關(guān)節(jié)機(jī)械臂動(dòng)力學(xué)方程建立

采用拉格朗日方程法建立三關(guān)節(jié)機(jī)械臂的動(dòng)力學(xué)模型。為建模方便，對(duì)其進(jìn)行簡(jiǎn)似化處理，可得三關(guān)節(jié)機(jī)械臂的簡(jiǎn)化動(dòng)力學(xué)方程[20-21]為

(1)

通過(guò)對(duì)SolidWorks內(nèi)搭建的三關(guān)節(jié)機(jī)械臂的虛擬物理樣機(jī)模型進(jìn)行測(cè)量計(jì)算得到機(jī)械臂動(dòng)力學(xué)方程的參數(shù)，可以減少建模誤差，實(shí)現(xiàn)更精準(zhǔn)的控制，將測(cè)量得到的動(dòng)力學(xué)方程參數(shù)代入機(jī)械臂動(dòng)力學(xué)方程式(1)中可以得到更精確的機(jī)械臂動(dòng)力學(xué)模型，精確的動(dòng)力學(xué)模型的建立使得數(shù)學(xué)模型更接近于真實(shí)機(jī)械臂，從而設(shè)計(jì)符合動(dòng)力學(xué)條件的控制方法。

2 聯(lián)合仿真建立與驗(yàn)證

通過(guò)Simulink中的Robotics System工具箱，使Gazebo和Simulink協(xié)同工作，搭建聯(lián)合仿真平臺(tái)，進(jìn)行模型在環(huán)測(cè)試。聯(lián)合仿真的過(guò)程相當(dāng)于閉環(huán)的數(shù)據(jù)交換，實(shí)現(xiàn)機(jī)械臂的實(shí)時(shí)閉環(huán)控制。

該聯(lián)合仿真平臺(tái)由Matlab中的Simulink仿真控制器和Gazebo中的仿真被控對(duì)象機(jī)械臂組成，在Matlab中顯示仿真數(shù)據(jù)曲線，在Gazebo中顯示三維仿真結(jié)果。

2.1 聯(lián)合仿真建立

聯(lián)合仿真的仿真對(duì)象由2部分組成:一部分是控制器模型的仿真，一部分是被控對(duì)象模型的仿真。借助于Simulink庫(kù)中Gazebo Co-Simulation的通信模塊，實(shí)現(xiàn)發(fā)布與訂閱話題，從而實(shí)現(xiàn)Simulink與Gazebo閉環(huán)數(shù)據(jù)交換[22]。

樹(shù)莓派性能優(yōu)異足以完成機(jī)械臂的仿真且成本較低，因此本文采用便攜式的微型單板計(jì)算機(jī)樹(shù)莓派(Raspberry Pi)搭載Gazebo實(shí)現(xiàn)被控對(duì)象機(jī)械臂的仿真。Gazebo中的物理模型機(jī)械臂在圖2中已經(jīng)建立，控制器仿真模型如圖3所示。

圖 3 控制器仿真模型Fig.3 Structure diagram of controller simulation model

圖3中的控制器仿真模型主要分為4部分：期望軌跡輸入模塊可以自定義關(guān)節(jié)期望軌跡，進(jìn)行不同期望軌跡的測(cè)試與對(duì)比；控制器模塊可以通過(guò)S函數(shù)或者已有Simulink模塊庫(kù)設(shè)計(jì)控制方法；仿真效果可視化模塊可以通過(guò)Scope示波器模塊實(shí)時(shí)顯示機(jī)械臂的運(yùn)行狀態(tài)，包括關(guān)節(jié)的角度、角速度以及加速度和力矩變化情況等；Gazebo通信模塊通過(guò)主從分布式的話題消息通信機(jī)制實(shí)現(xiàn)交互通信。

2.2 聯(lián)合仿真有效性驗(yàn)證

(2)

式中：Kp為比例環(huán)節(jié)的參數(shù)項(xiàng)；Ki為積分環(huán)節(jié)的參數(shù)項(xiàng)；Kd為微分環(huán)節(jié)的參數(shù)項(xiàng)；t為時(shí)間參數(shù)；u(t)為輸入控制力矩；r(t)為期望軌跡；q(t)為機(jī)械臂反饋回來(lái)的實(shí)際軌跡；軌跡跟蹤誤差e(t)=r(t)-q(t)。

設(shè)置仿真環(huán)境以及參數(shù)。3個(gè)關(guān)節(jié)的期望軌跡均為y=(π/3)sint，單位為rad，初始角弧度為0 rad，角速度為0 rad/s，Gazebo中機(jī)械臂為站立姿態(tài)。Simulink中機(jī)械臂控制器采樣時(shí)間設(shè)置為Ts1=0.01 s，仿真總時(shí)間設(shè)置為Ts2=20 s。前饋補(bǔ)償PID控制方法的PID參數(shù)分別設(shè)置為Kp=[100 60 10]，Ki=[10 6 1]，Kd=[5 1 0.1]。Gazebo中重力加速度為9.8 m/s2，環(huán)境溫度為26 ℃，風(fēng)力干擾為0 N。首先在樹(shù)莓派上啟動(dòng)Gazebo實(shí)現(xiàn)機(jī)械臂可視化，然后在Simulink中運(yùn)行聯(lián)合仿真，在本文所設(shè)置實(shí)驗(yàn)條件下進(jìn)行多次重復(fù)實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果一致。

圖4(a)、(b)、(c)依次為前饋補(bǔ)償PID控制方法應(yīng)用在機(jī)械臂關(guān)節(jié)1、2、3的實(shí)際關(guān)節(jié)軌跡跟蹤期望軌跡的位置響應(yīng)曲線以及關(guān)節(jié)角度跟蹤誤差曲線。圖中藍(lán)色為真實(shí)值，紅色為參考值。

(a) 關(guān)節(jié)1

(b) 關(guān)節(jié)2

(c) 關(guān)節(jié)3圖 4 前饋補(bǔ)償PID控制方法下關(guān)節(jié)位置響應(yīng) 及關(guān)節(jié)角度跟蹤誤差曲線Fig.4 Joint position response and joint angle tracking error curve under feedforward compensation PID control method

從圖4(a)～(c)可以看出，在三關(guān)節(jié)機(jī)械臂上應(yīng)用前饋補(bǔ)償PID控制方法時(shí)，關(guān)節(jié)1在2.05 s左右跟蹤誤差趨于穩(wěn)態(tài)，波形平穩(wěn)后關(guān)節(jié)1跟蹤誤差在[-0.002 8，0.003 3]之間；關(guān)節(jié)2在2.48 s左右跟蹤誤差趨于穩(wěn)態(tài)，波形平穩(wěn)后關(guān)節(jié)2跟蹤誤差在[-0.063 3，0.065 6]之間；關(guān)節(jié)3在2.48 s左右跟蹤誤差趨于穩(wěn)態(tài)，波形平穩(wěn)后關(guān)節(jié)3跟蹤誤差在[-0.115 9，0.116 5]之間。可以得出，盡管各關(guān)節(jié)跟蹤誤差最后收斂在一定范圍內(nèi)，但是機(jī)械臂運(yùn)行初期誤差波動(dòng)較大，在Gazebo中也能觀察到對(duì)應(yīng)的抖動(dòng)現(xiàn)象；受到各關(guān)節(jié)互相作用力的影響，關(guān)節(jié)2、3的跟蹤誤差較大。

圖5為三關(guān)節(jié)機(jī)械臂的控制力矩變化曲線。u1、u2、u3分別代表機(jī)械臂關(guān)節(jié)1、2、3的力矩變化曲線。

圖 5 關(guān)節(jié)1、2、3控制力矩曲線Fig.5 Control torque curve of joint 1, 2 and 3

從圖5可以看出，各關(guān)節(jié)力矩在運(yùn)行初期振蕩明顯，波動(dòng)較大，進(jìn)入穩(wěn)態(tài)后呈周期性變化，無(wú)明顯振蕩現(xiàn)象，且受本身機(jī)械結(jié)構(gòu)影響，關(guān)節(jié)2的峰值力矩最大，關(guān)節(jié)1的峰值力矩最小，關(guān)節(jié)3的峰值力矩較小。結(jié)合圖4(a)～(c)可以得出，運(yùn)行初期關(guān)節(jié)力矩頻繁調(diào)節(jié)導(dǎo)致振蕩現(xiàn)象，造成初始誤差波動(dòng)，從而導(dǎo)致Gazebo中機(jī)械臂存在抖動(dòng)現(xiàn)象，在控制力矩進(jìn)入穩(wěn)態(tài)后，跟蹤誤差也隨之進(jìn)入穩(wěn)態(tài)，呈現(xiàn)周期性變化。

綜合以上仿真結(jié)果可以得出，在聯(lián)合仿真中，Gazebo中的機(jī)械臂按照期望軌跡運(yùn)動(dòng)，Simulink中獲取到了Gazebo中機(jī)械臂反饋回來(lái)的數(shù)據(jù)，實(shí)現(xiàn)了閉環(huán)內(nèi)聯(lián)合仿真軟件之間的實(shí)時(shí)數(shù)據(jù)交互。結(jié)果證明聯(lián)合仿真的流程與設(shè)計(jì)思路是正確的，該聯(lián)合仿真是成功有效的。本文所搭建的聯(lián)合仿真平臺(tái)能夠更直觀地驗(yàn)證控制效果，是一種更接近于真實(shí)環(huán)境的仿真模型，彌補(bǔ)了傳統(tǒng)開(kāi)發(fā)方法的不足，具有成本低、快速驗(yàn)證的優(yōu)點(diǎn)。

但是在三關(guān)節(jié)機(jī)械臂上應(yīng)用前饋補(bǔ)償PID控制方法存在著控制精度較差，響應(yīng)速度較慢以及初期力矩變化振蕩的問(wèn)題。

3 基于終端約束的模型預(yù)測(cè)控制設(shè)計(jì)

為解決傳統(tǒng)的前饋補(bǔ)償PID控制方法在三關(guān)節(jié)機(jī)械臂上應(yīng)用時(shí)存在的控制精度較差以及無(wú)法處理約束條件等問(wèn)題，本文提出一種基于終端約束的機(jī)械臂軌跡跟蹤模型預(yù)測(cè)控制方法。采用雅可比線性化方法對(duì)機(jī)械臂動(dòng)力學(xué)方程進(jìn)行線性化處理[10]，再進(jìn)行離散化得到機(jī)械臂的離散線性動(dòng)力學(xué)方程，最后設(shè)計(jì)基于終端約束的模型預(yù)測(cè)控制方法(model predictive control method with terminal constraints，TMPC)實(shí)現(xiàn)軌跡跟蹤控制。

3.1 機(jī)械臂動(dòng)力學(xué)方程線性離散化

(3)

接下來(lái)對(duì)式(3)進(jìn)行雅可比線性化處理，根據(jù)上文中的狀態(tài)向量，求出其分量的導(dǎo)數(shù)函數(shù)f的雅可比子矩陣，其表達(dá)式為

(4)

首先定義線性化后的狀態(tài)向量為xM，控制向量為uM，則該系統(tǒng)方程為

(5)

式(5)中的常數(shù)項(xiàng)可以用D(D∈R+)來(lái)表示，則系統(tǒng)線性化后的方程為

(6)

根據(jù)連續(xù)離散化的基本約定，可以得到離散時(shí)間線性化狀態(tài)空間方程的簡(jiǎn)化形式，即

(7)

3.2 終端約束模型預(yù)測(cè)控制方法設(shè)計(jì)

預(yù)測(cè)控制的核心思想是根據(jù)被控對(duì)象的系統(tǒng)模型建立預(yù)測(cè)模型，并設(shè)計(jì)包含控制輸入量與預(yù)測(cè)誤差量的目標(biāo)函數(shù)，添加約束條件后通過(guò)求解最小化目標(biāo)函數(shù)的數(shù)值規(guī)劃問(wèn)題求得最優(yōu)控制輸入序列，從而進(jìn)一步實(shí)現(xiàn)機(jī)械臂軌跡跟蹤控制。

在預(yù)測(cè)模型建立的過(guò)程中，假設(shè)在時(shí)刻k時(shí)，給定一組控制輸入U(xiǎn)(k)，預(yù)測(cè)模型向后預(yù)測(cè)Np步可以得到k時(shí)刻的預(yù)測(cè)輸入輸出以及狀態(tài)序列。

預(yù)測(cè)模型的輸出序列為

(8)

預(yù)測(cè)模型的狀態(tài)序列為

(9)

預(yù)測(cè)模型的控制輸入序列為

(10)

給定的參考軌跡期望輸入序列為

(11)

預(yù)測(cè)模型的誤差狀態(tài)序列為

E(k)=Y(k+1|k)-R(k)=

(12)

根據(jù)模型預(yù)測(cè)控制的最優(yōu)性能指標(biāo)定義，選擇預(yù)測(cè)輸出序列與參考軌跡期望輸入序列的誤差量和控制輸入量作為性能指標(biāo)，則TMPC控制器的目標(biāo)函數(shù)可設(shè)置為

(13)

式中：e(k+i|k)=y(k+i|k)-r(k+i|k)；Q為TMPC控制器的誤差權(quán)重對(duì)角矩陣；R為TMPC控制器的控制輸入權(quán)重對(duì)角矩陣，均為正定矩陣。不同的權(quán)重矩陣的設(shè)置將會(huì)影響被控對(duì)象的動(dòng)態(tài)性能。

為使機(jī)械臂能夠跟蹤期望軌跡，本文考慮加入終端狀態(tài)等式約束條件，定義預(yù)測(cè)誤差狀態(tài)序列E(k)處于終端約束內(nèi)，為終端狀態(tài)量。特別地，強(qiáng)制E(k)=0即零終端情況下會(huì)導(dǎo)致預(yù)測(cè)控制問(wèn)題無(wú)可行解[24]。故本文在每個(gè)預(yù)測(cè)時(shí)域的(k+Np)終端時(shí)刻，令E(k+Np|k)=0即預(yù)測(cè)的最后序列使得預(yù)測(cè)誤差收斂于零，此時(shí)系統(tǒng)無(wú)需再調(diào)節(jié)控制輸入即預(yù)測(cè)控制輸入量U(k+Np|k)=0，保證機(jī)械臂在每個(gè)預(yù)測(cè)時(shí)域內(nèi)能夠有效地跟蹤期望軌跡。則所提TMPC的優(yōu)化控制問(wèn)題可以描述為

(14)

滿足x(k+1)=Ax(k)+Bu(k)+c，y(k)=Cx(k),同時(shí)umin(k+i|k)≤u(k+i|k)≤umax(k+i|k),i=0,1,2,…,Np-1；ymin(k+i|k)≤y(k+i|k)≤ymax(k+i|k),i=1,2,…,Np；這2個(gè)式子為控制輸入與輸出的約束條件。本文所提控制方法具體的實(shí)現(xiàn)流程分為3個(gè)步驟。

1) 根據(jù)系統(tǒng)約束與終端約束條件求解最優(yōu)性能指標(biāo)目標(biāo)函數(shù)J，計(jì)算最優(yōu)控制輸入量U*(k)。

2) 將U*(k)序列中第一個(gè)控制向量u*(k|k)作用于被控對(duì)象機(jī)械臂可以得到測(cè)量值x(k+1|k)。

3) (k+1)時(shí)刻，令系統(tǒng)平衡點(diǎn)xr=x(k+1|k)，更新預(yù)測(cè)模型系統(tǒng)狀態(tài)量，返回第1)步，實(shí)現(xiàn)滾動(dòng)優(yōu)化。

在Matlab中可以通過(guò)調(diào)用Hybrid Toolbox工具箱實(shí)現(xiàn)TMPC控制器的設(shè)計(jì)，該工具箱支持自動(dòng)生成C++代碼，實(shí)現(xiàn)嵌入式開(kāi)發(fā)與測(cè)試，數(shù)值規(guī)劃問(wèn)題的求解可以使用YALMIP工具箱調(diào)用其他開(kāi)源求解器，例如CasADi等，加快求解速度[25]。

4 TMPC控制方法聯(lián)合仿真實(shí)驗(yàn)

為驗(yàn)證本文所提TMPC控制方法的有效性，在聯(lián)合仿真環(huán)境下對(duì)三關(guān)節(jié)機(jī)械臂進(jìn)行了仿真研究，仿真環(huán)境參數(shù)設(shè)置同上文2.2節(jié)。圖6為本文所提出的TMPC控制方法作用在三關(guān)節(jié)機(jī)械臂上關(guān)節(jié)1、2、3的實(shí)際關(guān)節(jié)軌跡跟蹤期望軌跡的位置響應(yīng)曲線以及關(guān)節(jié)角度跟蹤誤差曲線。圖中藍(lán)色代表真實(shí)值，紅色代表參考值。

(a) 關(guān)節(jié)1

(b) 關(guān)節(jié)2

(c) 關(guān)節(jié)3圖 6 關(guān)節(jié)位置響應(yīng)曲線及關(guān)節(jié)角度跟蹤誤差曲線Fig.6 Joint position response curve and joint angle tracking error curve

從圖6(a)～(c)可以看出，在0.31 s左右，關(guān)節(jié)1、2、3的跟蹤誤差進(jìn)入穩(wěn)態(tài)，呈周期性變化。波形平穩(wěn)后，關(guān)節(jié)1跟蹤誤差在[-0.005 5，0.000 3]之間，關(guān)節(jié)2跟蹤誤差在[-0.008 9，0.001 8]之間，關(guān)節(jié)3跟蹤誤差在[-0.002 8，0.035 8]之間。

在本文所設(shè)置的仿真參數(shù)與條件下，應(yīng)用所提控制方法，與圖4(a)～(c)中應(yīng)用前饋補(bǔ)償PID控制方法進(jìn)行對(duì)比后可以發(fā)現(xiàn)，各關(guān)節(jié)跟蹤誤差均有減小，并且Gazebo中機(jī)械臂按照預(yù)定的期望軌跡進(jìn)行平穩(wěn)運(yùn)動(dòng)。各個(gè)關(guān)節(jié)調(diào)節(jié)時(shí)間較快，實(shí)際位置在較快的時(shí)間內(nèi)跟蹤上了期望位置，且運(yùn)行穩(wěn)定后關(guān)節(jié)1、2跟蹤誤差在10-3rad左右，有著較好的控制精度。

圖7為本文所提出的TMPC控制方法作用在三關(guān)節(jié)機(jī)械臂關(guān)節(jié)1、2、3的控制力矩曲線圖。

圖 7 關(guān)節(jié)1、2、3控制力矩曲線Fig.7 Control torque curve of joint 1,2 and 3

從圖7可以看出，雖然各關(guān)節(jié)初始控制力矩較大，但是處于約束條件內(nèi)，能夠?qū)崿F(xiàn)快速調(diào)節(jié)，進(jìn)入穩(wěn)態(tài)后呈周期性變化，相比于圖5中應(yīng)用前饋補(bǔ)償PID控制方法，本文所提控制方法的力矩變化曲線初期振蕩減少且調(diào)節(jié)快速，結(jié)合圖6(a)～(c)可以得出，運(yùn)行初期控制力矩快速調(diào)節(jié)使得軌跡跟蹤快速響應(yīng)，且跟蹤誤差較小。

綜合以上結(jié)果可以得出：應(yīng)用本文所提控制方法時(shí)，機(jī)械臂在更短的時(shí)間跟蹤上期望位置并趨于平穩(wěn)，且運(yùn)行平穩(wěn)后的跟蹤誤差更小，跟蹤精度有所提高，控制力矩變化波動(dòng)較小，初始階段的振蕩現(xiàn)象明顯減弱，證明本文所提的TMPC控制方法在三關(guān)節(jié)機(jī)械臂的軌跡跟蹤上更具有優(yōu)勢(shì)。

5 結(jié) 語(yǔ)

本文采用Gazebo與Simulink搭建三關(guān)節(jié)機(jī)械臂聯(lián)合仿真平臺(tái)，并提出一種基于終端約束的機(jī)械臂軌跡跟蹤模型預(yù)測(cè)控制方法。所搭建的聯(lián)合仿真平臺(tái)將控制方法、可視化工具以及通信標(biāo)準(zhǔn)高效地集成，降低了機(jī)械臂初期開(kāi)發(fā)成本，提高了開(kāi)發(fā)效率；所提出的軌跡跟蹤控制方法在機(jī)械臂軌跡跟蹤控制性能上有所提升，能夠?qū)崿F(xiàn)快速調(diào)節(jié)，降低了跟蹤誤差，提高了控制平穩(wěn)性。本文所搭建的聯(lián)合仿真平臺(tái)與所提控制方法，具有一定的工程應(yīng)用與理論研究?jī)r(jià)值。