C算法
基于 STM32F103C8T6 的二轮平衡车 LQR 控制器设计
基于 STM32F103C8T6 的二轮平衡车 LQR 控制器设计。通过对比 PID 与 LQR 控制器的差异，阐述了 LQR 作为白盒实验对物理模型的依赖。详细说明了在 MATLAB 中建立系统状态方程、设定性能指标 Q 与 R 矩阵、求解 Riccati 代数方程获取反馈增益矩阵 K 的过程。最后给出了 STM32 端利用 K 值计算加速度的代码实现及参数微调策略，验证了 LQR 算法在平衡车控制中的可行性。
基于 STM32F103C8T6 的二轮平衡车 LQR 控制器设计。通过对比 PID 与 LQR 控制器的差异，阐述了 LQR 作为白盒实验对物理模型的依赖。详细说明了在 MATLAB 中建立系统状态方程、设定性能指标 Q 与 R 矩阵、求解 Riccati 代数方程获取反馈增益矩阵 K 的过程。最后给出了 STM32 端利用 K 值计算加速度的代码实现及参数微调策略，验证了 LQR 算法在平衡车控制中的可行性。
PID 控制器和 LQR 控制器各有优劣。LQR 控制器更依赖物理模型的建立，属于白盒实验，需要知道系统内部参数再进行调参；而 PID 控制器通常被视为黑盒实验，即不需要精确的物理模型，根据输出响应直接调参即可。虽然 PID 也有白盒情况，但工程上多为经验调参。
在二轮平衡小车的项目中，落地 LQR 线性二次型算法。
与卡尔曼滤波算法验证不同，LQR 控制器的设计最终需要反馈增益矩阵 K 值。K 值是给定值，不是在代码里在线计算，而是离线进行仿真计算。因此，需要先离线在 MATLAB 中进行仿真，再将 K 值代入到代码里进行在线验证。
因为是离线控制，所以推导 K 的代码没有在 STM32 中。输入参数为举例，并不代表本项目实际使用的输入参数。在 MATLAB 代码中，建立系统方程的步骤如下：
m =0.035; r =0.0672/2; i =0.5*m*r^2; M =0.757-2*m; L =0.5*0.0903; J_p =(1/12)*M*(0.0903^2+0.0530^2); d =0.1612; J_delta =(1/12)*M*(0.0930^2+0.0530^2); g =9.8; Q_eq = J_p*M+(J_p+M*L^2)*(2*m+2*i/r^2); A_23 =-(M^2*L^2*g)/Q_eq; A_43 = M*L*g*(M+2*m+2*i/r^2)/Q_eq; B_21 =(J_p+M*L^2+M*L*r)/(Q_eq*r); B_22 = B_21; B_41 =-(M*L/r+M+2*m+2*i/r^2)/Q_eq; B_42 = B_41; B_61 =1/(r*(m*d+i*d/r^2+2*J_delta/d)); B_62 =-B_61; A =[010000;00 A_23 000;000100;00 A_43 000;000001;000000]; B =(i/r)*[00; B_21 B_22;00; B_41 B_42;00; B_61 B_62];
状态方程中，状态转移矩阵内 A 和输入矩阵 B 的具体推导过程需参考相关理论资料。
性能泛函包含两项，一是系统的积累跟踪误差，二是控制能量。显然它着重权衡和惩罚大的跟踪积累误差和大的控制能量。泛函 J 取得极小值的含义是：在没有过大控制能量消耗的前提下，使系统输出量 Y(t) 尽量接近理想输出量 r(t)，即跟踪误差最小。根据以上条件，最优控制存在且唯一。
Q =[100000000;000000;000000;000100000;000010000;000000]; R =[10;01];
Q 的设置体现控制优先级：代码中 Q(1,1)=1000（位移误差）、Q(4,4)=1000（俯仰角速度误差）、Q(5,5)=1000（转向角误差），说明优先保证这三个状态的精度。R 的设置限制控制量：R=eye(2) 表示左 / 右轮力的代价相同，避免电机输出过大（如急加速导致的不稳定）。
若有最优解，则 Riccati 代数方程=0，可求解 P。求解 P 后可求解 K。
K =lqr(A,B,Q,R);% 调用 MATLAB 的 lqr 函数，输入 A、B、Q、R，输出 6×2 增益矩阵 K
在 MATLAB 代码底层函数中可以看到具体实现逻辑。
对于连续时间系统，基于状态方程中的微分方程，性能函数为求积分，对应的 Riccati 代数方程为特定形式。对于离散时间系统，基于状态方程中的差分方程，性能指标是离散求和形式，底层会调用离散 Riccati 方程的求解逻辑。
求解 K 后，可求解 u。
最终，STM32 端的代码核心只有这两行：
L_accel=-(K1*x_pose+K2*(x_speed-Target_x_speed)+K3*(angle_x-Target_angle_x)+K4*gyro_x+K5*angle_z+K6*(gyro_z-Target_gyro_z));
R_accel=-(K1*x_pose+K2*(x_speed-Target_x_speed)+K3*(angle_x-Target_angle_x)+K4*gyro_x-K5*angle_z-K6*(gyro_z-Target_gyro_z));
由于理论模型与实际小车存在误差（如摩擦、传感器噪声），仿真得到的 K 可能需要微调：
验证二轮平衡小车中，联立代码，分析理论，验证 LQR 线性二次型控制器算法的可行性。
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online
将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML转Markdown 互为补充。 在线工具，Markdown转HTML在线工具，online
将 HTML 片段转为 GitHub Flavored Markdown，支持标题、列表、链接、代码块与表格等；浏览器内处理，可链接预填。 在线工具，HTML转Markdown在线工具，online
通过删除不必要的空白来缩小和压缩JSON。 在线工具，JSON 压缩在线工具，online