基于STM32F103C8的DLQR控制算法的C編程

關鍵詞：STM32F103C8；DLQR；雙線性變換；動態黎卡提方程；CMSIS-DSP函數庫；Kei15.32中圖分類號： TP273+ .5 文獻標識碼：A 文章編號：2096-4706（2025）16-0029-05

C Programming of DLQR Control Algorithm Based on STM32F103C8

WANG Changshi （Temperature MeasurementandControlLaboratoryofNanchang DeyouIndustrialCo.，Ltd.，Nanchang33o025，China）

Abstract： Implementing a more complex Discrete Linear Quadratic Regulator （DLQR）control algorithm on alowerend MCUchip（lowoperating frequency，lowFlashandRAMcapacity，noFPU，andlowcost）isapracticaland minorproblem in the MCU control field in recent years.The project innovatively completed the independent Clanguage program design of DLQRcontrolalgorithmo STM32F103C8.Thethree main pointsareas follows.Firstly，theCMSIS-DSPfunctionlibraryis addedtothegeneraldevelopmenttoolKeil5.32ofSTM32F1o3C8MCU.Secondly，thebilinearmethodisusedtorealizethe discretizationofthecontinuousstateequation byCprogramming.Finaly，thebackwarditeration method isused to solve the dynamic Riccati equation，and then the discrete state feedback gain matrix K_n is obtained. The product test shows that the DLQR controlalgorithm programbasedonthe Clanguage design isalmostidentical to theprogram basedonMATLABdesign on the STM32F103C8 chip.

Keywords： STM32F103C8; DLQR; bilinear transformation; dynamic Riccati equation; CMSIS-DSP function library： Keil 5.32

0 引言

近一年多來，實驗室一直在探索基于STM32F103C8（以下簡稱F103）的DLQR控制技術。這涉及三方面技術：F103、DLQR以及F103與DLQR的結合。DLQR研究于2024年7月取得突破[1-3]。后續曾嘗試采用Python +F103^[4] 及Eigen+IAR/Keil+F103[5]實現DLQR控制，但均未如愿，主要原因是兩者均要求高端MCU（如STM32F405RZ，簡稱F405），這在多數產品中并不可取，因為性價比過低一一F405市場價是F103的9倍多；其次，參考文獻（以下簡稱文獻）[的方法引入了Python語言，文獻[2]則引入了 C++ 的高級運算庫（含線性代數運算）Eigen，兩者均增加了編程難度且延長了開發周期。直至2024年9月，在了解到CMSIS-DSP庫函數[后， Fl03+ DLQR的問題才得以圓滿解決。實際上，該問題不僅存在于本實驗室，在MCU控制業界也較為普遍。2024年初曾使用一款基于LQR算法的一階直線倒立擺嵌入式（F103）自控教學產品[1]。不知為何，該產品先脫離F103在電腦上用MATLAB編程運行DLQR控制算法求出 K_n 值，再將 K_n 值嵌入F103的程序中，以其作為入口參數，依據公式（12）所示的最優控制律輸出控制量 u_n 。

本文創新在于不脫離F103離線操作，也不依賴MATLAB編程求解 K_n 值，而是直接在F103上通過Keil（含CMSIS-DSP庫函數）這一常規工具，依據DLQR控制原理自主采用C語言編程，一次性完成包括求解 K_n 值在內的倒立擺F103全部程序（在所選產品的原程序基礎上增加了求解 K_n 的程序）。

1 DLQR原理要點

LQR（LinearQuadratic Regulator）是最優控制理論的一部分。其核心控制思想是：所定義的成本函數（Cost function，或稱目標函數） J 包含終端項和積分項（見公式（2）），積分項中又包含 2～3 項，且所有項均為二次型（quadratic）標量函數；同時要求系統動態方程為線性（見公式（1））。 J 中必須包含系統的狀態變量 x 和輸入（或控制）變量 u （見公式（1））。 J 的實際物理意義是系統關鍵測量值或狀態值（如位移、速度、角度和角速度等）與期望值（或設定值）之間的誤差總和。LQR的目標是尋找最優（或較小）的輸入量 u ，使 J 達到最小（即誤差最小且 u 較小）。LQR通過狀態反饋計算 u 值（見公式（3）），這涉及求解狀態反饋增益矩陣 K ，而 K 的求解需先解關于矩陣 P 的黎卡提微分方程（見方程（4））—這是LQR的核心內容。

LQR按系統的連續／離散特性、軌跡終端（積分上限）的有限／無限性分為4類[。本文所用的是有限離散LQR，因其具有較完善的動態黎卡提方程（DYRE）迭代解法。

DLQR即離散化的LQR。

1.1有限時域連續線性系統

時間 t∈[t₀ ， t_f] ， t₀ ： t_f 是初始時間和終端時間。

1.1.1建立控制系統的狀態方程模型

該模型也稱為動態方程[8]，因其4個矩陣反映了控制系統的動態特征，如式（1）所示。式中， x 為狀態變量， 是 x 對時間的一階導數，類似地還有下文的d、 為輸出變量； u 為輸入控制變量； A，B 、c 和 D 均為常數矩陣。以一階直線倒立擺[1為例， x 可為 4×1 維列向量，即 ，其中 d d、 θ 1 分別為擺桿的位移、速度、角度和角速度，上標T表示轉置（下同）； y 為 2×1 維輸出矢量 （y=[dθ]^T） ：u 為 1×1 維輸入矢量（標量），即電機的工作電壓控制量。此時， A 為 4×4 方陣， B 為 4×1 列向量，c 為 2×4 矩陣， 為 2×1 列向量。具體建模過程及參數取值詳見文獻[4]，此處不做贅述。

y=Cx+Du

1. 1.2 系統的二次型成本（cost）函數

函數如式（2）所示，式中等號右邊4個二次項（或成本項）依次為系統的終端態（或穩態）、狀態、輸入及狀態與輸入交叉（cross，或稱相互影響）二次項，均為標量函數，代表4種誤差。 F 二 ， R 和 N 為其中的加權（或稱懲罰）矩陣，其作用是對控制系統的性能指標進行側重。要求 和 F 為半正定矩陣，R 為正定矩陣， N 應使（ Q-NR^?-1N^?T. ）陣為半正定矩陣[10]，且均為實對稱常數矩陣。 F 和 N 有時可忽略（即令 F=0 ， N=0 ，下同），具體視系統設計需求而定。F ： 、 R 和 N 的具體取值詳見文獻[1]。

1.1.3 系統的最優控制率

最優控制量（ Φ_u 值）的計算如公式（3）所示。式中，u 為標量， K 為狀態反饋增益矩陣，其中的實對稱正定陣 P^[8] 必須是方程（4）的解。 _A-BK 稱為閉環系統狀態反饋矩陣。LQR的設計目的是求出 κ ，進而通過較小的 u 值使式（2）中的 J 最小。

1.1.4黎卡提矩陣微分方程（MDRE）

LQR之所以引入該方程（式（4））[]，是因為它為系統的動態方程、成本函數最小化及最優控制解（ Φ_u 值）之間提供了關鍵聯系。

這是一個關于 P 矩陣的一階常微分方程，是LQR控制的核心內容。通過該方程可解出 P ，進而利用增益式（3）求出增益矩陣 K ，最終得到最優控制量 u 。

LQR全狀態反饋控制系統的結構圖如圖1所示，圖中dx/dt=x

圖1LQR全狀態反饋控制系統結構圖

1.2有限時域連續線性系統離散化

1.2.1 狀態方程模型的離散化

對式（1）的 A ， B 、 c 和 矩陣采用雙線性法（bilinear或Tustin）進行離散化，得到 A_d ： B_d ，C_d ， D_d ，即[3]：

D_d=CB_d+D

式中， α=T/2 ， T 為采樣時間， I 為單位矩陣，-1表示求逆。由此得到離散的狀態方程[12]：

選擇雙線性法的原因是其可保持控制系統原有的動態特性。

1.2.2 離散目標函數

式（7）中[13]， Q，R 已是離散化的。 n 為離散（采樣）序列號，是正整數。 H 對應式（2）中 t_f 的離散時間，此時需做出最優控制決策，即使 J 最小化：

1.2.3 后向迭代動態黎卡提方程

式（8）由式（4）離散化而來：

P_n-1=Q+A_d^TP_nA_d-A_d^TP_nB_d（R+B_d^TP_nB_d）^-1B_d^TP_nA_d

1.2.4計算離散的 K 值和 u 控制律

離散的 K 值為：

其中， P_n 是方程（8）的迭代解， K_n 的更多論述見公式（12）。

離散的 u 控制律為：

u_n=-K_nx_n

其中， K_n 、 x_n 為矩陣量， u_n 為標量。

此式與式（3）對應，更多論述見式（6）和（12）。

2在 F103+Kei∣5.32 上加入CMSIS-DSP函數庫

CMSIS-DSP是一個C語言函數庫，專為基于Cortex-M和Cortex-A架構的處理器芯片設計。它包含多種數字信號處理（DSP）函數，其中有矩陣運算。與此前使用過的另一種更通用的DSP函數庫C++ Eigen相比，前者更具針對性，因此更易于嵌入MCU（如F103）的程序中，且編譯后的代碼容量小、執行速度快，但編程語句相對復雜。

在Keil環境下，CMSIS-DSP庫的加入方法參見文獻[6]。

3 算法與編程

本節論述參照有C語言格式。

3.1用CMSIS-DSP庫函數編程的注意點

有以下幾點[14]：

1）定義一個矩陣，需用3個語句：

第一，arm_matrix_instance_f32 A ；//聲明一個不定矩陣 A （204

第二，float32_t D_A 0，0，0}; // 指定矩陣的數據單元和初始值，可為非0值，視需求而定

第三，arm_mat_init_f32（amp;A，4，4， D_A） 11 將 A 陣初始化為 4×4 的矩陣并賦值。矩陣 A 以結構體形式存在，包括矩陣的行數numRows、列數numCols和數據指針pData（指向 D_A^? ），因此 A 和 D_A 必須嚴格一一對應，即矩陣 A 橫向拉直后即為一維數組 D_A

2） n×m 階矩陣的行、列值均從0開始，即行數numRows∈[0， n -1]、列數numCols∈[0，m-1]。

3）若要給 4×4 矩陣 A 的第2行第2列賦值3.4，操作如下：

第一，arm_matrix_instance f32^*S; //聲明一個矩陣指針

第二， S=amp;A;/1 指向 A 矩陣的數據區第三， SpData[5]=3.4;// 結束

4）CMSIS-DSP庫函數中沒有矩陣變量賦值語句（即 A=B ），可用矩陣加法函數替代。例如：

arm_mat_add_f32（amp;B，amp;Z_44，amp;A）; 1/A 、 B 和 Z_44為同維矩陣， Z_-44 必須是全0矩陣

雖然也可用語句：

arm_copy_f32 （D_B，D_A，16） ;//復制數據

實現 A=B ，但該語句的代碼容量比上一句多出80字節，故不宜采用。

5）運算結果不能直接返回給操作數。

此外，為確保程序穩定運行、避免跑飛，應簡化程序運行邏輯，不在一條語句中進行矩陣的混合運算。

3.2計算 ?_Ad ！ B_d，C_d，D_d

依據（5）式計算，程序如下：

先按3.1.1所述方法定義并初始化9個矩陣A、B、A_d ！ I， Templ、Temp2、Temp3、Temp4和 Z_-44 ，然后通過矩陣運算計算 A_d 值，程序如下：

arm_matrix_instance_ Γ32，B，A_d，I， Templ，Temp2，Te 聲明9個矩陣

以下僅以矩陣 A 一 _I 和 Z_-44 為例建立對應的數據單元：

float32_t D_A[16]={0，0，0，0，0，0，0，0，0，0，0，0，0，0，0};// 對應 4×4A 陣的數據單元

float32_t D_l[16]= {1，0，0，0，0，1，0，0，0，0，1，0，0，0，0，1};//對應 4×4 單位陣 I 的數據單元

float32_t D_z[16]={0，0，0，0，0，0，0，0，0，0，0，0，0，0，0};/∪ 對應 4×4 全0陣 Z_-44 的數據單元

以下僅以矩陣 A ！ I 為例做矩陣的初始化。

arm_mat_init_f3 2（amp;A，4，4，D_?A） 11 初始化 A 陣

arm_mat_init_f32 （amp;I，4，4，D_I） 11 初始化 I 陣armmat init Γ32（amp;Z44，4，4，D_?Z） ： 11 初始化全0陣

Z_44

以下做矩陣運算： arm_mat_scale_f32（amp;A，α，amp;Templ）;//矩陣的數乘 arm_mat_add_f32（amp;Templ，amp;Z_44，amp;Temp3）;//讓 矩陣Temp 3= Temp1 arm_mat_add_f32（amp;I，amp;Temp3，amp;Templ）;//求矩 陣和 arm_mat_sub_f32（amp;I，amp;Temp3，amp;Temp2）;//求矩 陣差 arm_mat_inverse_f32（amp;Temp2，amp;Temp4）;//求矩 陣逆 arm_mat_mult_f32（amp;Templ，amp;Temp4，amp;Ad）;//求矩 陣積

類似地，可計算式（5）中的其他3個矩陣量B_d 、 C_d 和 D_d 。該DLQR控制算法中的其他矩陣運算也可參照此方法。所用庫函數的詳情參見文獻[14]。

3.3用迭代法求解動態黎卡提方程

方程（8）的迭代算法如下：1）變量定義：uintl6_tIterNum;//迭代次數float IterTolEr; 11 迭代容許誤差uintl6_tIterNumUpLim; // 迭代次數上限值uint8_t IterAbFlag; 11 迭代異常標志float P_eMA ：0 11 定義為 P_err 陣中數值最大元素的絕對值以下聲明矩陣：arm_matrix_instance_f32 P_n，P_n-1，P_err，Q，R 0 //P_err 矩陣定義為 P_n-1 與 P_n 二陣的差值2）賦初值：P_n=Q;/I 矩陣 、 R 的初值見文獻[1]IterNum ₁₌₀ IterTolE r=0.01 // 視具體的精度要求而定IterNumUpLin 1=10000;11 視具體情況而定IterAbFlag ρ₌₀ 3）按式（8）計算 P_n-1 。4）計算 P_em=P_n-1-P_n 。5）找出矩陣 P_err 中最大元素并取絕對值，得到PeMAo6）IterNum ⁺⁺ 。7） P_n=P_n-1;// 迭代一次。8）若 IterTolEr 且IterNumlt; IterNumUpLim，則返回3）進入循環；否則：若P_eMA? IterTolEr，結束迭代循環，得到解 P_n=P_n-1 ：若IterNum ? IterNumUpLim，表明迭代異常，置IterAbFlag =1 后退出迭代循環。

迭代流程圖如圖2所示，本項目中，當迭代128次時，迭代即完成。

圖2DYRE后向迭代算法程序流程圖

3.4計算 K_n 值和控制率 u_n

依據式（9）參照3.2方法編程，結果為：K_n=[k₁k₂k₃k₄]=[-64.0730743-51.815502 -109.779 419-19.073 093 4] （11）依據式（10），可得： （12）-109.7794190n-19.07309340這里 d_n 、 θ_n 和 分別為 d_n θ_n 和 的采樣值。至此，在F103上的DLQR控制算法的C編程全部完成，完整的代碼見文獻[1]。

4二種語言程序比較測試

將上述自主C語言編程求解 K_n 的程序加入文獻[1]中的F103+Keil5.32原程序“直線倒立擺源碼”并進行測試。與MATLAB編程結果（作為參考）相比，在相同模型參數下，最終所得 K_n 值非常接近，如表1所示。最大示值相對誤差的絕對值γ僅為0.00162% ，出現在C語言編程的 k₃ 上， γ=|（- 109.779 419-（-109.781 199 15））/-109.779 419|=0.00162% ；二者對倒立擺的運行結果也一致，如圖3所示。

表1 K_n 值比較

圖3正在運行改進后程序的倒立擺

5結論

測試表明，上述在STM32F103C8上不依賴MATLAB語言編程，而是直接通過自主C語言編程實現DLQR控制算法是完全有效的。這也為業界在較低端MCU上運行高端CMSIS-DSP庫函數提供了更多可借鑒的工程實例。

參考文獻：

[1]王昌世.在STM32F103c8上做DLQR最優控制算法 的C編程資料[EB/OL].[2024-12-14].https：//www.elecfans.com/ soft/78/223/2025/202507236848377.html.

[2] Technicianl3/DLQR.discreteLQR controller based on C++ [EB/OL].[2024-06-29].https：//github.com/Technician13/ DLQR.

[3] Stack Exchange.Bilinear transformation of continuous time state space system [EB/OL].[2025-02-09].https：//dsp stackexchange.com/questions/45o42/bilinear-transformation-ofcontinuous-time-state-space-system.

[4] bilibili.使用micropython開發 STM32[EB/OL].[2024- 08-25].https：//www.bilibili.com/opus/711217627732639785.