基于LQR和UKF的軟體機器人無模型軌跡跟蹤控制

摘要：針對軟體機器人精確建模和控制問題提出一種新穎的非線性估計和控制策略，用于控制二維氣動軟體機器人的動態性能。采用基于Koopman算子的數據驅動方法建立二維氣動軟體機器人的線性模型。利用無跡卡爾曼濾波器（UKF）進行傳感器數據濾波和系統狀態估計，同時利用線性二次型調節器（LQR）來實現軌跡跟蹤的最優控制。仿真和實驗比較結果一致表明，所提方法在軌跡跟蹤性能方面優于另兩種方法。

關鍵詞：軟體機器人；Koopman算子；LQR控制；無跡卡爾曼濾波器

中圖分類號：TP242

Model-free Trajectory Tracking Control of Soft Robots Based on LQR and UKF

GUAN Shengchuang1 LIU Yujun2 YANG Qinghao1 LIU Zhaobing^1*

1.School of Mechanical and Electrical Engineering，Wuhan University of Technology，Wuhan，430000

2.Department of Electrical and Electronic Engineering，University of Nottingham Ningbo China，Ningbo，Zhejiang，315000

Abstract： A novel nonlinear estimation and control strategy for controlling the dynamic performance of a 2D pneumatic soft robot was proposed to address the problems of accurate modelling and control of soft robots. Firstly， a linear model of the 2D pneumatic soft robot was established using a Koopman operator-based approach. Then， the UKF was proposed for sensor data filtering and system state estimation， while the LQR was used for optimal control of trajectory tracking. Simulation and experimental results consistently show that the strategy herein performs better than other two strategies" in terms of trajectory tracking.

Key words： soft robot； Koopman operator； linear quadratic regulator（LQR） control； unscented Kalman filter（UKF）

0 引言

在過去的幾年里，由高度可變形和柔性材料制成的軟體機器人已經成為一個新興的研究領域^［1］。與傳統的剛性機器人相比，它們表現出非凡的物理彈性、靈活性和安全性，能夠輕松適應復雜環境中的任務需求，這為機器人的應用開辟了更多的可能性^［2］。 精確的模型有助于更好地理解軟體機器人的行為，以及根據模型設計適當的控制器。軟體機器人的建模方法可以分為兩類：基于物理解析建模和無模型的數據驅動建模^［3］。

理論上，軟體機器人具有無限的自由度和強非線性，因此基于物理解析建模的方法一般需要強有力的假設。相比之下，只依賴實驗數據的數據驅動的建模方法更適合軟體機器人。

Koopman算子理論提供了一種數據驅動的建模方法，既避免了物理簡化假設，同時建立了面向控制的線性模型^［4-5］。這種方法利用Koopman算子的線性結構，根據輸入輸出數據構建非線性受控動態系統的線性模型，再使用已建立的線性控制方法就可以對其進行控制。BRUDER等^［4］利用Koopman算子理論對三腔式螺紋管型氣動軟體機器人進行建模，并使用MPC控制器實現軌跡跟蹤控制。NARASINGAM等^［5］將Koopman算子理論應用在水力壓裂過程的模型識別和反饋控制中，數值實驗表明，Koopman線性模型與真實系統表現出了良好的一致性。

另外，集成在軟體機器人上的傳感器很容易受環境的干擾，這會導致傳感器反饋的數據帶有噪聲甚至不穩定^［6］。無跡卡爾曼濾波器（unsecanted Kalman filter， UKF）可以通過已有的系統狀態空間模型和系統觀測模型來估計系統的狀態，同時可以過濾傳感器中的噪聲^［7］。經過UKF處理后數據傳遞給LQR控制器，可以計算出系統的最優控制律。

綜上所述，本文針對軟體機器人建模控制復雜的問題，將基于Koopman算子的數據驅動方法應用于二維氣動軟體機器人并建立系統的線性模型；基于該模型設計了線性二次型調節器（LQR）；提出了無跡卡爾曼濾波器（UKF）來估計系統狀態并過濾傳感器反饋數據中的噪聲，并設計了對比實驗。

1 基于Koopman算子的建模方法

1.1 Koopman算子理論

首先考慮一個具有輸入的離散非線性被控動力系統：

xk+1=F（xk，uk）（1）

xk∈XRn" uk∈URm

式中：xk、uk分別為動態系統在第k個采樣時間的狀態和控制輸入；F為系統狀態由xk隨時間步長k演化到xk+1的函數。

在狀態空間上定義一個標量可觀測函數g：X×U→R，用來表示狀態和輸入之間的關系。在無限維函數空間F中，動力系統的狀態演化由Koopman算子K ：F→F控制，Koopman算子K將可觀測量沿系統軌跡的動力學定義為

KggF（2）

式中：“”為復合函數操作符。

1.2 Koopman算子的近似

理論上，Koopman算子是無限維的，在實際應用中需要求解其有限維的近似值。為此，利用最近開發的EDMD算法^［8］為受控系統構建Koopman算子的有限維近似，EDMD算法流程如下。

1）定義系統的提升函數：

φ（x，u）=（φ1（x，u），φ2（x，u），…，φN（x，u））T（3）

式中：x、u分別為系統的狀態和控制輸入；φi（x，u）為線性無關的基函數，i∈1，2，…，N。

定義提升后的狀態為

φ（xk）=（φ1（xk），φ2（xk），…，φN（xk））T（4）

2）構建Koopman算子線性模型：

φ（xk+1）=Aφ（xk）+Buk

zk=Cφ（xk）（5）

式中：zk為對原非線性系統狀態xk的估計；A、B和C表示線性常數矩陣。

3）準備包含p個系統狀態的數據集：

X=（x1，x2，…，xp）

Y=（y2，y3，…，yp+1）

U=（u1，u2，…，up）（6）

yi+1=F（xi，ui）" i∈1，2，…，p

4）準備提升系統狀態數據集：

φXY=φYφXUT

φXX=φXUφXUT（7）

φX=（φ（x1），φ（x2），…，φ（xp））

φY=（φ（y2），φ（y3），…，φ（yp+1））

5）Koopman線性模型中的A、B和C矩陣由求解最小二乘問題獲得：

minA，B∑pk=1‖φ（yk+1）－Aφ（xk）－Buk‖22

minC∑pk=1‖xk-Cφ（xk）‖22（8）

φ（yk+1）=（φ1（yk+1），φ2（yk+1），…，φN（yk+1））T

φ（xk）=（φ1（xk），φ2（xk），…，φN（xk））T

式（8）的最小二乘問題的解為

［A，B］=φXYφXX

C=Xφ（X）（9）

式中：“" ”表示偽逆。

至此，獲得了氣動軟體機器人的Koopman線性模型表達式（式（5））的參數矩陣，可以用于后續控制器的設計。

2 軟體機器人最優控制策略

目前的研究中基于Koopman線性模型的控制器大多是根據模型的控制方法設計的。基于Koopman線性模型的控制方法使用較多的是LQR和MPC控制器。相比之下，LQR控制器計算量更小，具有良好的穩定性和魯棒性^［9］。

二維氣動軟體機器人系統已經用Koopman線性模型（式（5））表示，系統模型和控制率都滿足線性相關，兩者之間的關系可以表達為

uk=－K（φ（xk）－φ（xr））（10）

式中：φ（xr）為參考值的提升狀態；K為LQR的狀態反饋增益。

LQR狀態反饋增益的選擇標準是：使得系統的性能代價函數實現極小值。具體地，基于Koopman線性模型的代價函數的形式如下：

J=∑∞k=0（Δφ）TQ（Δφ）+uTkRuk（11）

式中：Δφ=φ（xk）－φ（xr）；Q、R分別表示關于狀態和輸入的權重矩陣，是對稱正定矩陣。

LQR反饋控制律K就是在式（11）的約束條件下求出其最優控制律uk，并使得代價函數J的值最小。

基于Koopman模型的二維氣動軟體機器人軌跡跟蹤控制系統反饋增益矩陣K的形式如下：

K=R^－1BTP（12）

其中，P為常量矩陣，其具體值可以在MATLAB中通過求解 Riccat 方程進行計算得出：

ATP+PA－PBR^－1BTP+Q=0（13）

3 無跡卡爾曼濾波器

本文利用UKF處理非線性系統的優勢，實現對氣動軟體機器人的狀態估計和傳感器反饋數據中噪聲過濾，然后將處理過的數據用于LQR控制器。

UKF由無跡變換（unscented transform，UT）和線性卡爾曼濾波算法組成。無跡變化的本質是高斯分布，具體而言：在初始狀態數據中根據一定的標準獲得采樣點，采樣點的均值、協方差與初始狀態一致；將采樣點代入非線性狀態空間，得到映射的非線性狀態空間數據集，計算UT后的均值和協方差。高斯分布的優勢在于可以獲得非線性系統精確到三階的UT后的后驗均值和協方差。

為了方便推導，假設一個離散非線性系統：

xk=G（xk－1，uk－1）+wk

zk=g（xk）+vk（14）

wk～（0，Qk）vk～（0，Rk）（15）

式中：G、g分別為非線性狀態方程函數和觀測方程函數；wk、vk分別為狀態轉換和觀測噪聲;（0，Qk）表示均值為0，協方差為Qk;（0，Rk）表示均值為0，協方差為Rk。

系統初始狀態的均值和協方差分別為x+0=E（x0）和P+0=E（［x0－x+0］［x0－x+0］T），其中，x0為系統的初始狀態。

具體地，UKF算法流程如下。

1）從高斯過濾中選擇sigma點：

x^^（i）k－1=x^+k－1+nP+k－1

x^^（n+i）k－1=x^+k－1－nP+k－1（16）

式中：x^^（i）k－1為狀態sigma點;x^+k－1為后驗狀態估計;P+k－1為后驗協方差;n為系統狀態的維度，i=1，2，…，n。

2）通過系統狀態轉換函數傳播sigma點：

x^^（i）k=G（x^^（i）k－1，uk）+wk（17）

3）從映射的sigma點中提取高斯狀態的預測均值和協方差：

x^－k=12n∑2ni=1x^^（i）k

P－k=12n∑2ni=1（Δx）（Δx）T+Qk－1（18）

式中：x^－k為預測狀態估計；P－k為預測協方差，Δx=x^^（i）k－x^－k。

4）從預測的高斯分布中選一組新的sigma點：

x^^（i）k=x^－k+nP－k

x^^（n+i）k=x^－k－nP－k（19）

5）通過測量方程傳播sigma點以生成預測測量值z^^（i）k：

z^^（i）k=g（x^^（i）k）+vk（20）

6）確定k時間處的預測測量值z^k，預測測量值的協方差Pz以及x^－k和z^k之間的交叉協方差Pxz：

z^k=12n∑2ni=1z^^（i）k

Pz=12n∑2ni=1（Δz）（Δz）T+Rk

Pxz=12n∑2ni=1（Δx）（Δz）T（21）

Δz=z^^（i）k－z^k

7）通過卡爾曼濾波方程提取濾波后，UKF的卡爾曼增益Kk、系統狀態x^+k和相應的協方差P+k可估計為

Kk=Pxz（Pz）^－1

x^+k=x^－k+Kk（zk－z^k）

P+k=P－k－KkPzKTk（22）

4 實驗平臺和數據處理

4.1 實驗平臺

氣動軟體機器人使用商用熔融沉積模型（FDM）打印機（FlashForge Creator Pro）進行制造，打印的二維氣動軟體機器人的總體尺寸為108 mm×14 mm×25 mm，如圖1所示。

圖2展示了實驗的基本裝置，主要包括：Arduino Mega 2560電路板，用于與彎曲傳感器的交互，以收集反饋信息；SMC VQ110U-5M電磁閥，接收來自Arduino板的60Hz脈沖寬度調制（PWM）信號，控制氣體流量以改變SPA的內部壓力；彎曲傳感器，測量二維氣動軟體機器人的彎曲角度；上位機，向控制器發出角度指令，并實時監測二維氣動軟體機器人的角度。在MATLAB/Simulink中記錄和顯示傳感器反饋的數據。Arduino板的模擬輸出為0～255，對應的氣壓是0～120 kPa。

4.2 數據獲取與處理

本文使用的二維氣動軟體機器人只有一個氣道，因此可以定義為單輸入單輸出模型，輸入為0～255的模擬信號，輸出為彎曲角度，通過實驗獲取10 000組輸入輸出數據對，用于構建Koopman線性模型。

對數據進行歸一化預處理，歸一化處理后的數據更易準確捕捉自變量與因變量的相關性，從而提高模型預測精度，因此處理后的數據更適合建立Koopman模型。通過均方根誤差（RMSE）來計算狀態軌跡的擬合程度：

ERMSE=∑pi=1（yreal－yref）2p（23）

式中：yreal、yref分別為狀態的測量值和參考值;p為收集的測量點的個數。

5 結果與討論

5.1 仿真結果

在MATLAB/Simulink中搭建Koopman線性模型、編寫UKF濾波器和LQR控制器仿真模型。為驗證所設計控制器的可行性，分析比較了三種控制策略，分別為基于解析模型的LQR控制策略（A-FL-LQR）、基于Koopman線性模型的LQR控制策略（K-LQR）、基于Koopman線性模型的UKF估計和LQR控制策略（K-UKF-LQR）。

仿真輸入的參考信號為正弦波和三角波，輸出為彎曲角度，圖3～圖5所示分別為A-FL-LQR、K-LQR和K-UKF-LQR對三角波和正弦波的軌跡跟蹤仿真結果。

表1給出了正弦波和三角波參考信號下三種控制策略的仿真軌跡跟蹤結果，可以看出，相較于A-FL-LQR，在三角波輸入下，K-LQR和K-UKF-LQR的跟蹤精度分別提高了42.34%、64.11%；在正弦波輸入下 K-LQR和K-UKF-LQR的跟蹤精度分別提高了40.78%、62.61%。這說明無論是三角波輸入還是正弦波輸入，K-UKF-LQR控制策略的跟蹤效果都有明顯提升。

5.2 實驗結果

在二維氣動軟體機器人控制實驗平臺上進行了對比實驗，在同樣的三角波和正弦波輸入下，A-FL-LQR、K-LQR和K-UKF-LQR的軌跡跟蹤效果見圖6～圖8。

表2給出了在正弦波和三角波參考信號下三種控制策略的實驗軌跡跟蹤結果。相較于A-FL-LQR，在三角波輸入下，K-LQR和K-UKF-LQR的跟蹤精度分別提高了41.68%、54.23%；在正弦波輸入下，K-LQR和K-UKF-LQR的跟蹤精度分別提高了37.73%、51.70%。這說明K-UKF-LQR的軌跡跟蹤誤差最小，控制策略的控制精度最優。

圖9分別展示了二維氣動軟體機器人在15°、20°、25°、35°時的彎曲狀況。

5.3 結果討論

由表1和表2可知，A-FL-LQR控制策略的RMSE值更大，軌跡跟蹤誤差較大。這是因為 LQR 對模型精度有較高依賴，而構建解析模型通常需要先了解系統的物理方程、參數以及必要的簡化假設，才能進行建模推導。然而，真實系統往往具有復雜的非線性和難以精確建模的因素，導致解析模型難以精確描述系統行為。相比之下，Koopman 線性模型基于大量實驗數據構建，能夠更好地適應實際情況。此外，Koopman 線性模型通過將狀態空間映射到無限維函數空間，可以更準確地捕捉系統的非線性動態行為。從實驗結果可以得出，基于Koopman模型的控制策略具有較高的控制精度。此外，傳感器受環境干擾導致反饋的數據中帶有噪聲的影響，也會影響控制效果，在控制策略中加入UKF后，K-UKF-LQR的RMSE值明顯降低了，表明UKF可以有效過濾傳感器反饋數據中的噪聲并估計系統狀態，用于求解最優控制。綜上所述，該結果證明了提出的K-UKF-LQR控制策略的有效性。

值得注意的是，仿真結果與實驗結果的RMSE值有差距，這可能包含以下幾種原因：

1）本實驗中的彎曲角度是由彎曲傳感器RB-02S046測量的。當彎曲角度不同時，傳感器會輸出不同的電阻值，后面再通過人為標定來確認不同彎曲角度對應的電阻值，在這個過程中會人為引入一些誤差。

2）從實驗圖6～圖8中可以看出，在參考信號的波峰和波谷時會出現較大的波動。這是因為電磁閥的開關突然變大或變小，會導致氣腔內的氣壓忽大忽小，從而產生一些波動，對控制效果產生一定的影響。此外，軟體機器人的超彈性材料產生的機械阻尼增大了控制策略的超調。

3）仿真中添加的噪聲是一種理想的高斯白噪聲，但在現實的實驗中會受到電磁閥開/關閥門大小頻繁切換帶來的噪聲影響，傳感器數據反饋時的噪聲以及環境中的未知擾動的影響也會對實驗效果產生影響。

雖然實驗和仿真的數值結果上有差距，但從總體的趨勢上實驗結果和仿真結果是相符的。

6 結語

本文采用數據驅動方法描述二維氣動軟體機器人系統的動態行為，建立了基于Koopman算子的線性模型，結合非線性估計和濾波方法（UKF）以及最優控制（LQR）的方法對二維氣動軟體機器人進行軌跡跟蹤控制。仿真和實驗結果表明，與K-LQR控制策略相比，所提出的K-UKF-LQR控制策略可以更準確地控制二維氣動軟體機器人的軌跡，在數據處理和系統狀態估計方面表現良好，驗證了所提控制策略的有效性。

參考文獻：

［1］ LIU Zhaobing， PENG Kerui， HAN Lvpeng， et al. Modeling and Control of Robotic Manipulators Based on Artificial Neural Networks：a Review［J］. Iranian Journal of Science and Technology， Transactions of Mechanical Engineering， 2023， 47（4）：1307-1347.

［2］ 徐豐羽， 孟凡昌， 范保杰， 等. 軟體機器人驅動、建模與應用研究綜述［J］. 南京郵電大學學報（自然科學版）， 2019， 39（3）：64-75.

XU Fengyu， MENG Fanchang， FAN Baojie， et al. Review of Driving Methods， Modeling and Application in Soft Robots［J］. Journal of Nanjing University of Posts and Telecommunications （Natural Science Edition）， 2019， 39（3）：64-75.

［3］ SCHEGG P， DURIEZ C. Review on Generic Methods for Mechanical Modeling， Simulation and Control of Soft Robots［J］. PLoS One， 2022， 17（1）：e0251059.

［4］ BRUDER D， FU Xun， GILLESPIE R B， et al. Data-driven Control of Soft Robots Using Koopman Operator Theory［J］. IEEE Transactions on Robotics， 2021， 37（3）：948-961.

［5］ NARASINGAM A， KWON J S I. Application of Koopman Operator for Model-based Control of Fracture Propagation and Proppant Transport in Hydraulic Fracturing Operation［J］. Journal of Process Control， 2020， 91：25-36.

［6］ CAO Guizhou， HUO Benyan， YANG Lei， et al. Model-based Robust Tracking Control without Observers for Soft Bending Actuators［J］. IEEE Robotics and Automation Letters， 2021， 6（3）：5175-5182.

［7］ XAVIER M S， FLEMING A J， YONG Y K. Nonlinear Estimation and Control of Bending Soft Pneumatic Actuators Using Feedback Linearization and UKF［J］. IEEE/ASME Transactions on Mechatronics， 2022， 27（4）：1919-1927.

［8］ WILLIAMS M O， KEVREKIDIS I G， ROWLEY C W. A Data-driven Approximation of the Koopman Operator：Extending Dynamic Mode Decomposition［J］. Journal of Nonlinear Science， 2015， 25（6）：1307-1346.

［9］ NAIDU D S. Optimal Control Systems［M］. Boca Raton：CRC Press， 2018.

（編輯 王旻玥）

基金項目：國家大學生創新創業訓練計劃（024104970）

作者簡介：

關勝闖，男，1998年生，碩士研究生。研究方向為軟體機器人建模與控制。E-mail：giky0915@163.com。

劉兆冰*（通信作者），男，1983年生，副教授。研究方向為軟體機器人建模與控制。E-mail：zhaobingliu@whut.edu.cn。

本文引用格式：

關勝闖，柳宇鈞，楊清昊，等.基于LQR和UKF的軟體機器人無模型軌跡跟蹤控制［J］. 中國機械工程，2025，36（3）：570-575.

GUAN Shengchuang， LIU Yujun， YANG Qinghao， et al. Model-free Trajectory Tracking Control of Soft Robots Based on LQR and UKF［J］. China Mechanical Engineering， 2025， 36（3）：570-575.