平衡车怎么控制
倒立摆模型是控制领域最常见的物理模型之一，比如现在已经被人类驯服的各种平衡车，其简化模型就是一级倒立摆模型。1 基本概念1.1 LQRLQR (linear quadratic regulator)的全称为线性二次型调节器，可由其得到线性反馈的最优控制规律，易于构成闭环最优控制。其本质是基于全状态反馈控制（FSFB）来寻找最优矩阵的一种方式，因此LQR也是全状态反馈控制的一种。
Q、R就是需要设计的半正定矩阵和正定矩阵。对于线性系统的控制器设计问题，如果其性能指标是状态变量和(或)控制变量的二次型函数的积分，则这种动态系统的最优化问题称为线性系统二次型性能指标的最优控制问题，简称为线性二次型最优控制问题或线性二次问题。
线性二次型问题的最优解可以写成统一的解析表达式和实现求解过程的规范化，并可简单地采用状态线性反馈控制律构成闭环最优控制系统，能够兼顾多项性能指标，因此得到特别的重视，为现代控制理论中发展较为成熟的一部分。而且MATLAB的应用为LQR理论仿真创造了条件。
1.2 SimscapeSimscape是Simulink中的一个模块，可用于迅速创建物理系统的模型。通过 Simscape，可以基于物理连接直接相连模块框图建立物理组件模型。通过将基础组件依照原理图装配，为电机、桥式整流器、液压致动器和制冷系统等系统建模。
图片来自mathworks图图准备了mathworks提供的Simscape入门手册，下载方式见文末！
快速入门与帮助手册2 倒立摆模型介绍倒立摆模型是控制领域最常见的物理模型之一，比如现在已经被人类驯服的各种平衡车，其简化模型就是一级倒立摆模型:
我们可以简化为如下图所示的形式，简单来说就是通过调控使杆直立不倒。
倒立摆模型在对倒立摆的稳定性控制过程中能有效反映控制理论中的许多关键问题，如非线性、鲁棒性、正定问题等，因此倒立摆常常被用来检验新控制理论和算法的正确性及其在实际应用中的有效性。
忽略空气阻力后，通过现代控制理论进行建模。对于该系统，选取状态矢量x = [x_1,x_2,x_3,x_4]^T ，分别是质量块位置x_1 ，质量块速度x_2 ，摆杆角度x_3以及摆杆角速度x_4；摆杆角度初始状态与零位偏离一个小角度，即x_3(t_0)不为0；通过给质量块施加一定的水平作用力，可使倒立摆直立，因而输入u 可以选定为施加在质量块上的外力；控制的目的是让摆杆能尽量稳定在初始位置，且倒立摆水平方向的位移往往存在限制，因此可将系统输出y设定为质量块位置x_1及摆杆角度x_3。
选取位移、速度、角度、角速度作为状态变量，如式（1-1）所示：
根据牛顿定理推导可得式（1-2）：
写作状态空间方程形式，得到：
3 倒立摆系统的simulink仿真模型建立由此可在simulink中建立控制系统的仿真模型。整体的程序框图如图所示：
倒立摆系统仿真模型的程序框图利用LQR问题的一般求解方法：
?(1) 设计性能指标泛函J，确定加权矩阵Q,R ;?(2) 根据Riccati方程求解P矩阵;?(3) 代入获得最优控制u*(t) 。
先设计一个能量函数。最优的控制轨迹应该使得该能量函数最小。选取如下形式的能量函数：
其中S ，Q ，R 矩阵为加权矩阵，体现了各要素对过程影响的重要性；要求 S ，Q 为半正定矩阵，R 为正定矩阵，通常使用对角阵；矩阵范数越大，则其对应的状态变量会以更快的速度趋近稳态。
一般地，对于稳定系统而言，稳态时的状态唯一确定，因此不对x(t_f)作要求，即令S=0 。
确定Q,R加权矩阵后代入Ricatti方程：
求解矩阵P ；
则，最优控制u*(t)满足：
当P求解后代入即可得到u*(t)。
值得一提的是，u*(t) 是关于x(t) 的线性函数，也可以视作为初始系统加入了负反馈后构成闭环系统，这说明LQR控制问题本质的即是控制器设计。记原系统为L[A,B,C,D] ， 则引入LQR控制器后的系统为L[A-BK,B,C,D] ，系统框图如下：
全状态反馈控制系统至此， 即可将LQR控制器应用于实际控制模型。
4 可视化平台搭建利用MATLAB Simulink中的Simscape Multibody模块进行3D仿真平台搭建。
Simscape Multibody模块首先确定系统的参数，仿真系统的参数如下表所示：
表1仿真系统的参数
编写程序计算系统的状态方程，并利用MATLAB当中的LQR函数计算出反馈矩阵 k，部分程序代码如下：
function [k, A, B, C, D] = BalanceCar_Model(M_c,m_l,J,b,l) %参数表通过输入得到 [A,B,C,D]=calc_SSM(M_c,m_l,J,b,l); [G,k]=Observability_Controllability_analze(A,B,C,D);endfunction [A,B,C,D]=calc_SSM(M,m,J,b,l)%计算状态方程 g=9.8; p=J*(M+m)+M*m*l*l; A=[ 0 1 0 0; 0 -(J+m*l*l)*b/p (m*m*g*l*l)/p 0; 0 0 0 1; 0 -(m*l*b)/p m*g*l*(M+m)/p 0;]; B=[ 0 0; (J+m*l*l)/p 0; 0 0; m*l/p 0;]; C=[ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1;]; D=[0 0; 0 0; 0 0; 0 0;];endfunction [G,k]=Observability_Controllability_analze(A,B,C,D) %计算 LQR 矩阵 R=[1 0;0 1]; Q=[1 0 0 0; 0 0 0 0; 0 0 10 0; 0 0 0 0;]; %Declare lqr as extrinsic coder.extrinsic(‘lqr’); k=zeros(2,4); [k S e]=lqr(A,B,Q,R);%计算状态反馈矩阵 G=A-B*k;end之后搭建simulink仿真程序:
引入LQR线性二次型最优控制后的仿真系统通过 Simscape Multibody 创建模型，包括坐标系、机构配置器、求解器、模型。通过转换器与模拟框图连接:
Simscape Multibody模型构成具体模型构成如下：主体结构包括小车（简化为滑块）、摆杆、导轨。
具体几何模型与对应部件为方便定位摆杆的转轴和施加转动副，在小车上设置摆杆轴几何形状。模型整体设置输出量为小车位置、小车速度、摆杆角度、摆杆角速度。输入量为加速力（与力关联）。
将上述Multibody模型封装成子系统，设置输入面板，外接模拟电路，如下图所示。
封装子系统外接模拟电路输入可变参数：
?摆杆重0.0923kg，?惯量0.004212kg·m2，?质心轴距0.185m，?小车重0.2275kg，
给初始扰动为10°，观察小车的状态改变情况。
仿真开始时，模型输入矫正力，使小车沿着摆杆初始偏移方向发生偏离，以消除杆相对于小车的转动，经过4s后，小车基本回到原来的位置：
倒立摆平台可视化效果至此，完成了基于LQR控制的倒立摆simscape-simulink仿真。
作者：Richard Young｜编辑：图图
有关PID仿真的部分推荐阅读此文章：《simulink入门与pid控制》
?完整文档&模型，请后台回复LQR;?后台回复全家桶获取所有作品;?图图的MATLAB交流群：1129425848；?欢迎投稿：后台回复投稿，稿费￥ 100 起；?更多干货教程请点击阅读原文
