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

關(guān)鍵詞：STM32F103C8；DLQR；雙線性變換；動(dòng)態(tài)黎卡提方程；CMSIS-DSP函數(shù)庫(kù)；Kei15.32中圖分類(lèi)號(hào)： TP273+ .5 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：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 引言

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

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

1 DLQR原理要點(diǎn)

LQR（LinearQuadratic Regulator）是最優(yōu)控制理論的一部分。其核心控制思想是：所定義的成本函數(shù)（Cost function，或稱(chēng)目標(biāo)函數(shù)） J 包含終端項(xiàng)和積分項(xiàng)（見(jiàn)公式（2）），積分項(xiàng)中又包含 2～3 項(xiàng)，且所有項(xiàng)均為二次型（quadratic）標(biāo)量函數(shù)；同時(shí)要求系統(tǒng)動(dòng)態(tài)方程為線性（見(jiàn)公式（1））。 J 中必須包含系統(tǒng)的狀態(tài)變量 x 和輸入（或控制）變量 u （見(jiàn)公式（1））。 J 的實(shí)際物理意義是系統(tǒng)關(guān)鍵測(cè)量值或狀態(tài)值（如位移、速度、角度和角速度等）與期望值（或設(shè)定值）之間的誤差總和。LQR的目標(biāo)是尋找最優(yōu)（或較?。┑妮斎肓?u ，使 J 達(dá)到最?。凑`差最小且 u 較小）。LQR通過(guò)狀態(tài)反饋計(jì)算 u 值（見(jiàn)公式（3）），這涉及求解狀態(tài)反饋增益矩陣 K ，而 K 的求解需先解關(guān)于矩陣 P 的黎卡提微分方程（見(jiàn)方程（4））—這是LQR的核心內(nèi)容。

LQR按系統(tǒng)的連續(xù)／離散特性、軌跡終端（積分上限）的有限／無(wú)限性分為4類(lèi)[。本文所用的是有限離散LQR，因其具有較完善的動(dòng)態(tài)黎卡提方程（DYRE）迭代解法。

DLQR即離散化的LQR。

1.1有限時(shí)域連續(xù)線性系統(tǒng)

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

1.1.1建立控制系統(tǒng)的狀態(tài)方程模型

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

y=Cx+Du

1. 1.2 系統(tǒng)的二次型成本（cost）函數(shù)

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

1.1.3 系統(tǒng)的最優(yōu)控制率

最優(yōu)控制量（ Φ_u 值）的計(jì)算如公式（3）所示。式中，u 為標(biāo)量， K 為狀態(tài)反饋增益矩陣，其中的實(shí)對(duì)稱(chēng)正定陣 P^[8] 必須是方程（4）的解。 _A-BK 稱(chēng)為閉環(huán)系統(tǒng)狀態(tài)反饋矩陣。LQR的設(shè)計(jì)目的是求出 κ ，進(jìn)而通過(guò)較小的 u 值使式（2）中的 J 最小。

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

LQR之所以引入該方程（式（4））[]，是因?yàn)樗鼮橄到y(tǒng)的動(dòng)態(tài)方程、成本函數(shù)最小化及最優(yōu)控制解（ Φ_u 值）之間提供了關(guān)鍵聯(lián)系。

這是一個(gè)關(guān)于 P 矩陣的一階常微分方程，是LQR控制的核心內(nèi)容。通過(guò)該方程可解出 P ，進(jìn)而利用增益式（3）求出增益矩陣 K ，最終得到最優(yōu)控制量 u 。

LQR全狀態(tài)反饋控制系統(tǒng)的結(jié)構(gòu)圖如圖1所示，圖中dx/dt=x

圖1LQR全狀態(tài)反饋控制系統(tǒng)結(jié)構(gòu)圖

1.2有限時(shí)域連續(xù)線性系統(tǒng)離散化

1.2.1 狀態(tài)方程模型的離散化

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

D_d=CB_d+D

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

選擇雙線性法的原因是其可保持控制系統(tǒng)原有的動(dòng)態(tài)特性。

1.2.2 離散目標(biāo)函數(shù)

式（7）中[13]， Q，R 已是離散化的。 n 為離散（采樣）序列號(hào)，是正整數(shù)。 H 對(duì)應(yīng)式（2）中 t_f 的離散時(shí)間，此時(shí)需做出最優(yōu)控制決策，即使 J 最小化：

1.2.3 后向迭代動(dòng)態(tài)黎卡提方程

式（8）由式（4）離散化而來(lái)：

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計(jì)算離散的 K 值和 u 控制律

離散的 K 值為：

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

離散的 u 控制律為：

u_n=-K_nx_n

其中， K_n 、 x_n 為矩陣量， u_n 為標(biāo)量。

此式與式（3）對(duì)應(yīng)，更多論述見(jiàn)式（6）和（12）。

2在 F103+Kei∣5.32 上加入CMSIS-DSP函數(shù)庫(kù)

CMSIS-DSP是一個(gè)C語(yǔ)言函數(shù)庫(kù)，專(zhuān)為基于Cortex-M和Cortex-A架構(gòu)的處理器芯片設(shè)計(jì)。它包含多種數(shù)字信號(hào)處理（DSP）函數(shù)，其中有矩陣運(yùn)算。與此前使用過(guò)的另一種更通用的DSP函數(shù)庫(kù)C++ Eigen相比，前者更具針對(duì)性，因此更易于嵌入MCU（如F103）的程序中，且編譯后的代碼容量小、執(zhí)行速度快，但編程語(yǔ)句相對(duì)復(fù)雜。

在Keil環(huán)境下，CMSIS-DSP庫(kù)的加入方法參見(jiàn)文獻(xiàn)[6]。

3 算法與編程

本節(jié)論述參照有C語(yǔ)言格式。

3.1用CMSIS-DSP庫(kù)函數(shù)編程的注意點(diǎn)

有以下幾點(diǎn)[14]：

1）定義一個(gè)矩陣，需用3個(gè)語(yǔ)句：

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

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

第三，arm_mat_init_f32（amp;A，4，4， D_A） 11 將 A 陣初始化為 4×4 的矩陣并賦值。矩陣 A 以結(jié)構(gòu)體形式存在，包括矩陣的行數(shù)numRows、列數(shù)numCols和數(shù)據(jù)指針pData（指向 D_A^? ），因此 A 和 D_A 必須嚴(yán)格一一對(duì)應(yīng)，即矩陣 A 橫向拉直后即為一維數(shù)組 D_A

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

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

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

第二， S=amp;A;/1 指向 A 矩陣的數(shù)據(jù)區(qū)第三， SpData[5]=3.4;// 結(jié)束

4）CMSIS-DSP庫(kù)函數(shù)中沒(méi)有矩陣變量賦值語(yǔ)句（即 A=B ），可用矩陣加法函數(shù)替代。例如：

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

雖然也可用語(yǔ)句：

arm_copy_f32 （D_B，D_A，16） ;//復(fù)制數(shù)據(jù)

實(shí)現(xiàn) A=B ，但該語(yǔ)句的代碼容量比上一句多出80字節(jié)，故不宜采用。

5）運(yùn)算結(jié)果不能直接返回給操作數(shù)。

此外，為確保程序穩(wěn)定運(yùn)行、避免跑飛，應(yīng)簡(jiǎn)化程序運(yùn)行邏輯，不在一條語(yǔ)句中進(jìn)行矩陣的混合運(yùn)算。

3.2計(jì)算 ?_Ad ！ B_d，C_d，D_d

依據(jù)（5）式計(jì)算，程序如下：

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

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

以下僅以矩陣 A 一 _I 和 Z_-44 為例建立對(duì)應(yīng)的數(shù)據(jù)單元：

float32_t D_A[16]={0，0，0，0，0，0，0，0，0，0，0，0，0，0，0};// 對(duì)應(yīng) 4×4A 陣的數(shù)據(jù)單元

float32_t D_l[16]= {1，0，0，0，0，1，0，0，0，0，1，0，0，0，0，1};//對(duì)應(yīng) 4×4 單位陣 I 的數(shù)據(jù)單元

float32_t D_z[16]={0，0，0，0，0，0，0，0，0，0，0，0，0，0，0};/∪ 對(duì)應(yīng) 4×4 全0陣 Z_-44 的數(shù)據(jù)單元

以下僅以矩陣 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

以下做矩陣運(yùn)算： arm_mat_scale_f32（amp;A，α，amp;Templ）;//矩陣的數(shù)乘 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）;//求矩 陣積

類(lèi)似地，可計(jì)算式（5）中的其他3個(gè)矩陣量B_d 、 C_d 和 D_d 。該DLQR控制算法中的其他矩陣運(yùn)算也可參照此方法。所用庫(kù)函數(shù)的詳情參見(jiàn)文獻(xiàn)[14]。

3.3用迭代法求解動(dòng)態(tài)黎卡提方程

方程（8）的迭代算法如下：1）變量定義：uintl6_tIterNum;//迭代次數(shù)float IterTolEr; 11 迭代容許誤差uintl6_tIterNumUpLim; // 迭代次數(shù)上限值uint8_t IterAbFlag; 11 迭代異常標(biāo)志float P_eMA ：0 11 定義為 P_err 陣中數(shù)值最大元素的絕對(duì)值以下聲明矩陣：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 的初值見(jiàn)文獻(xiàn)[1]IterNum ₁₌₀ IterTolE r=0.01 // 視具體的精度要求而定IterNumUpLin 1=10000;11 視具體情況而定IterAbFlag ρ₌₀ 3）按式（8）計(jì)算 P_n-1 。4）計(jì)算 P_em=P_n-1-P_n 。5）找出矩陣 P_err 中最大元素并取絕對(duì)值，得到PeMAo6）IterNum ⁺⁺ 。7） P_n=P_n-1;// 迭代一次。8）若 IterTolEr 且IterNumlt; IterNumUpLim，則返回3）進(jìn)入循環(huán)；否則：若P_eMA? IterTolEr，結(jié)束迭代循環(huán)，得到解 P_n=P_n-1 ：若IterNum ? IterNumUpLim，表明迭代異常，置IterAbFlag =1 后退出迭代循環(huán)。

迭代流程圖如圖2所示，本項(xiàng)目中，當(dāng)?shù)?28次時(shí)，迭代即完成。

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

3.4計(jì)算 K_n 值和控制率 u_n

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

4二種語(yǔ)言程序比較測(cè)試

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

表1 K_n 值比較

圖3正在運(yùn)行改進(jìn)后程序的倒立擺

5結(jié)論

測(cè)試表明，上述在STM32F103C8上不依賴(lài)MATLAB語(yǔ)言編程，而是直接通過(guò)自主C語(yǔ)言編程實(shí)現(xiàn)DLQR控制算法是完全有效的。這也為業(yè)界在較低端MCU上運(yùn)行高端CMSIS-DSP庫(kù)函數(shù)提供了更多可借鑒的工程實(shí)例。

參考文獻(xiàn)：

[1]王昌世.在STM32F103c8上做DLQR最優(yōu)控制算法 的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開(kāi)發(fā) STM32[EB/OL].[2024- 08-25].https：//www.bilibili.com/opus/711217627732639785.