大家好，又见面了，我是你们的朋友全栈君。如果您正在找激活码,请点击查看最新教程,关注关注公众号 “全栈程序员社区” 获取激活教程,可能之前旧版本教程已经失效.最新Idea2022.1教程亲测有效,一键激活。

Jetbrains全系列IDE使用 1年只要46元 售后保障 童叟无欺

一级倒立摆装置

文章目录

简介与建模

绪论

​ 倒立摆是自动控制系统中重要的模型，无论理论还是实际都具有重要意义，首先对于自控理论中一些算法的测试，如将经典控制理论的PID控制或是现代控制理论的LQR算法的全状态反馈应用于装置控制器中，就能测试算法的鲁棒性等性能，第二就是在实际工程中有许多的项目都是以倒立摆为模型，先在倒立摆上测试在应用于实际生产的，如火箭在上升中能够保持姿态的平稳不变等。

​ 虽然倒立摆的控制不易，但本身的结构简单，要实现的目的也清晰。无论是直线还是旋转倒立摆，总体都分为转动部分和摆杆部分，目的就是能让摆杆迅速摆起到竖直位置并保持稳定另外在尽可能大的外界干扰的情况下能保持稳定。对于总体的研究分为两类，首先是起摆控制(即是对静止状态快速起摆至稳定位置)，然后是稳摆控制(即是让系统稳定于稳定状态)。

​ 其中稳摆控制一般简化为线性控制，而起摆控制使用非线性控制器比较困难，由于能力和篇幅有限，本文探讨稳摆控制。

直线倒立摆的简化数学模型和系统分析

​ 对于研究一个自控系统来说，首先我们需要建立数学模型，并利用数学工具实现对系统分析和校正，实现控制器设计。倒立摆本身为一个非线性系统，我们需要对其进行分析是困难的，所以这里我们以简单的直线倒立摆为例子，实现对摆杆稳定位置的线性化建模。

​ 倒立摆简易图所示，首先对系统进行受力分析。对推车和摆杆在水平方向上的合力求和，可以得到以下运动方程,其中b是摩擦系数：

$$\begin{gathered} M\ddot{x}+b\dot{x}+N=F \\ N=m\ddot{x}+ml{\dot{\theta }}^{2}sin\theta -ml{\dot{\theta }}^{2}cos\theta \end{gathered}$$

​ 对推车和摆杆在竖直方向上的合力求和，可以得到以下运动方程
$$\begin{gathered} P-mg=ml\ddot{\theta }sin\theta +ml{\ddot{\theta }}^{2}cos\theta \\ Plsin\theta +Nlcos\theta =I\ddot{\theta } \\ I=m{l}^2/3 \end{gathered}$$
​ 对上面的 公式进行一定的简化
$$\begin{gathered} cos\theta \approx 1 \\ sin\theta \approx \theta \\ {\dot{\theta }}^2\approx 0 \end{gathered}$$
​ 分别结合前两个公式和后三个公式得到下面方程
$$\begin{gathered} (I+m{l}^2)\ddot{\theta }-mgl\theta =ml\ddot{x} \\ (M+m)\ddot{x}+b\dot{x}-ml\ddot{\theta }=F=u \end{gathered}$$

​ 若以F为输入，小车的位置、速度，摆杆的位置、速度为状态变量的话，可以得到以下的倒立摆的state-space,为了方便之后的仿真，这里参考项目直接设定具体数值。

$$\{\begin{array}{l}\left[\begin{array}{c}\dot{x}\\ \ddot{x}\\ \dot{\theta }\\ \ddot{\theta }\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& -0.1818& 2.673& 0\\ 0& 0& 0& 0\\ 0& -0.4545& 31.18& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \theta \\ \dot{\theta }\end{array}\right]+\left[\begin{array}{c}0\\ 1.8182\\ 0\\ 4,5455\end{array}\right]u\\ \\ \\ y=\left[\begin{array}{c}x\\ \theta \end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \theta \\ \dot{\theta }\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]u\end{array}$$

其中详细的推到可以查看参考2和5

​ 到这里就得到了倒立摆稳定时数学模型了，接下来就是对于系统的分析了，这里需要知道系统的稳定性¹、能控性²进行探讨。

这里直接利用MATLAB进行计算和结果输出：

//系统数学模型的建立和性能分析
clear;clc;
%创建数学模型
A=[0 1 0 0; 0 -0.1818 2.673 0; 0 0 0 1; 0 -0.4545 31.18 0];
B=[0 1.18 0 4.545]';
C=[1 0 0 0;0 0 1 0];
D=[0 0]';
states = {'x' 'x_dot' 'phi' 'phi_dot'};
inputs = {'u'};
outputs = {'x'; 'phi'};
sys_state=ss(A,B,C,D,'statename',states,'inputname',inputs,'outputname',outputs);
%查看特征值，发现存在存在正实部，存在一个极点在S平面的右半平面，系统开环不稳定
[V,D1]=eig(A);
disp('特征值为：')
diag(D1)
%判断系统能观能控性，发现能观能控，我们必须设计控制器来才能进行平衡位置的稳定控制
CONT=ctrb(A,B);
a=rank(CONT);
if a==4
   disp('4  能控')
else
   disp('非能控')
end
clear a;
OBSER=obsv(A,C);
b=rank(OBSER);
if b==4
   disp('4  能观')
else
   disp('非能观')
end
clear b;

​ 由结果可知，系统是开环不稳定，即是欠驱动系统，但是又是可观可控性的，所以的我们可以对系统进行校正，选择合适的控制器结构(如串联还是反馈，全反馈还是半反馈)，采用合适的算法设计控制器参数，最终实现对倒立摆系统的稳定控制。

​

仿真部分

​ 在状态空间中设计线性控制器适合使用全状态反馈校正，即是将系统中全部的状态乘上对应的反馈增益然后反馈回去。其中控制器的设计结构如图所示,其中K就是反馈增益矩阵。

​ 其中反馈增益可以通过多种控制理论得到。如有期望闭环极点时，可以利用极点配置法。本文将利用简单常用的LQR最优控制来确定各状态的反馈增益LQR算法³。其具体做法就是

1. 由系统本身和经验选择Q,R;
2. 求解黎卡提方程；
3. 得到增益矩阵K

LQR算法具体设计和Q、R矩阵设定详细可以查看参考6

其中求解方程部分直接用MATLAB的lqr()函数代替，其他详细代码也可以查看参考3

MATLAB模型建立与仿真

//MATLAB控制器的设计和性能测试
%这里我们利用LQR控制设计全反馈状态线性控制器,建立闭环状态空间
Q = C'*C;
R = 1;
K1 = lqr(A,B,Q,R);
Ac = (A-B*K1);
Bc = B;
Cc = C;
Dc = D;
states = {'x' 'x_dot' 'phi' 'phi_dot'};
inputs = {'r'};
outputs = {'x'; 'phi'};
sys_cl = ss(Ac,Bc,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);

%利用阶跃信号得到系统性能
t = 0:0.01:5;
r =0.2*ones(size(t));%0.2幅值的阶跃信号
[y,t,x]=lsim(sys_cl,r,t);%闭环系统的阶跃响应
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','小车距离 (m)')
set(get(AX(2),'Ylabel'),'String','摆杆角度 (radians)')
title('闭环系统1的阶跃响应')

%换出不同权重得到不同反馈增益得到的阶跃信号
Q(1,1) = 5000;
Q(3,3) = 100;
K2 = lqr(A,B,Q,R);
Ac = (A-B*K2);
figure(2);
sys_cl = ss(Ac,Bc,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);
[y,t,x]=lsim(sys_cl,r,t);
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','小车距离 (m)')
set(get(AX(2),'Ylabel'),'String','摆杆角度 (radians)')
title('闭环系统2的阶跃响应')

%换成R矩阵变换得到不同反馈增益得到的阶跃信号
Q = C'*C;
R = 10;
K3 = lqr(A,B,Q,R);
Ac = (A-B*K3);
states = {'x' 'x_dot' 'phi' 'phi_dot'};
inputs = {'r'};
outputs = {'x'; 'phi'};
sys_cl = ss(Ac,Bc,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);
figure(3);
[y,t,x]=lsim(sys_cl,r,t);
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','小车距离 (m)')
set(get(AX(2),'Ylabel'),'String','摆杆角度 (radians)')
title('闭环系统3的阶跃响应')

​ 显然在三个不同权重下，系统都能达到稳定，且只要在合适的参数的线性控制器下还可以达到不错的效果，下面利用Simulink对控制器进一步仿真。

Simulink模型建立与仿真

​ 可见如果参数调得好就可以得到超调量小、响应速度快的结果，因此可以进一步进行硬件的搭建。

实物部分

​ 上面已经对直线倒立摆进行仿真稳定控制了，则对于稳摆控制采用LQR算法的全状态反馈控制器设计。对于搭建的实物旋转倒立摆，唯一的不同是在实现控制器时，需要进行模型和物理设备之间的转换，其中F变成电机的转矩，小车的位移和速度变成水平旋转的角度 和角速度。即是存在一个电机转矩使得倒立摆稳定。
$$\tau =\left[\begin{array}{cccc}k1& k2& k3& k4\end{array}\right]\left[\begin{array}{c}\theta \\ \dot{\theta }\\ \varphi \\ \dot{\varphi }\end{array}\right]$$

所有的机械结构和PCB都在参考1的Github链接

机械部分

​ 对于机械外壳部分，参考模型导出切片文件，利用光固化打印机实现部件打印并组装起来。

电路部分

​ 本次的装置主要利用主控制器和传感器实现整体的闭环控制，主控接收传感器的数据进行状态机判别然后对驱动进行控制。其中为了达到精准控制，电机采用了无刷电机，而控制方法采用了矢量控制。

其中原理图的设计可以参考STM32教程和各芯片手册；PCB参考教程

矢量控制原理可以查看参考7

主控和驱动板部分

​ 本次主控采用STM32F103C8T6，驱动芯片采用DRV8313，编码器芯片采用MA730。STM32利用时钟配置3个PWM波形传输给驱动芯片，然后驱动电机，其中驱动方案采用三相半桥式。利用2个SPI⁴接口连接两个编码器，实现对摆杆、电机的角度和速度获取。另外外接出JATG和串口。

具体驱动方案和芯片的电路设计可以参考DRV8313的数据手册

摆杆编码板部分

​ 电路设计和主控板一样，利用外部接口和主控板链接

具体角度获取和编码原理和电路设计可以参考MA730的数据手册

电源板部分

​ 利用USB供电和串口通讯，提供的5V利用升压和降压电路，实现对STM32和驱动芯片的供电。另外外接JATG，方便接ST-link烧录。

具体升压原理和电路实现可以参考MT3608数据手册

代码部分

主函数部分，其余工程代码查看参考1

/*
倒立摆装置；
获取传感器数据，判断状态。驱动电机想要的力矩，实现倒立摆的摆起和稳定
*/
#include "Arduino.h"
#include <SimpleFOC.h>

#include "SPI.h"

#define LED_PIN PC13

float target_angle = 1.92; //摆杆原始角度，手动测量，数据归一化，不要太靠近0或者6.28
float bar_vel,bar_angle,bar_vel_filtered;//摆杆和滤波值
float base_vel,base_angle,base_vel_filtered;//电机和滤波值
float error=0;//误差 error=_normalizeAngle(bar_angle)-target_angle;
float angle_kp; //调试增加比例
float threshold = 5;//死区补偿，不然力矩过大
float dead_force =0.0;//死区力量
float theta,theta_dot,costheta;//theta是定义为摆的角度，摆竖直向下的时候为0，逆时针为正,用于非线性摆起
float target_force = 0;  //力矩

//初始设定
LowPassFilter filter = LowPassFilter(0.001);
LowPassFilter base_filter = LowPassFilter(0.01);
MagneticSensorSPI sensor = MagneticSensorSPI(MA730_SPI, PA4);//电机的
MagneticSensorSPI ma730 = MagneticSensorSPI(MA730_SPI, PB12);//摆杆的
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(PA8, PA9, PA10, PA0);
Commander command = Commander(Serial2);


void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
void setup() 
	{
	angle_kp = 1.4;//-2.52;//-10;//尝试不同比例
	pinMode(PC13, OUTPUT);//LED灯引脚模式定义
	
	sensor.init(&SPI);//电机
	ma730.init(&SPI_2);//摆杆
	motor.linkSensor(&sensor);

//电机控制配置
  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 12;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);
 // aligning voltage [V]
  motor.voltage_sensor_align = 3;
  // set motion control loop to be used
  motor.torque_controller = TorqueControlType::voltage;
  motor.controller = MotionControlType::torque;
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
	// velocity PI controller parameters
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 10;
  motor.PID_velocity.D = 0;
  // default voltage_power_supply
  motor.voltage_limit = 6;
  // jerk control using voltage voltage ramp
  // default value is 300 volts per sec  ~ 0.3V per millisecond
  // velocity low pass filtering time constant
  motor.LPF_velocity.Tf = 0.01;
  motor.PID_velocity.output_ramp = 1000;
  // contoller configuration
  // default parameters in defaults.h
  // velocity PI controller parameters
//  motor.PID_velocity.P = 0.1;
//  motor.PID_velocity.I = 1.5;
//  motor.PID_velocity.D = 0.0000003;
//  motor.LPF_velocity.Tf = 0.05;
//  motor.P_angle.P = 14;
motor.velocity_limit = 50;
//  //motor.voltage_limit = 4;

  // velocity low pass filtering time constant
  // the lower the less filtered
  //motor.LPF_velocity.Tf = 0.01f;

  // angle P controller
  //motor.P_angle.P = 20;
  // maximal velocity of the position control
  //motor.velocity_limit = 20;

  // use monitoring with serial
  Serial2.begin(115200);
  // comment out if not needed
  motor.useMonitoring(Serial2);

  motor.sensor_direction=Direction::CW;

  // initialize motor
  motor.init();
  // align sensor and start FOC
  motor.initFOC();

  // add target command T
  command.add('T', doTarget, "target angle");

  Serial2.println(F("Motor ready."));
  Serial2.println(F("Set the target angle using serial terminal:"));
  _delay(500);
  
//取一下前面的数据，不然滤波可能有问题
  for(int j=0;j<1000;j++){
    bar_angle = ma730.getAngle();
    bar_vel=ma730.getVelocity();
    base_angle=sensor.getAngle();
    base_vel = sensor.getVelocity();
    bar_vel_filtered=filter(bar_vel);
    base_vel_filtered=base_filter(base_vel); 
  }
		
  Serial2.println("state,error,vel,f_vel,base_vel,angle");

	
	
	}


void loop() {
	
	int i=0;//状态1的循环参数
	int state=1;//首先设定状态为1
	ma730.update();
	sensor.update();

//对于摆来说，磁钢在安装的时候可能方向不一样，所以摆的角度的正方向不确定，需要调整角度和速度的方向为逆时针(正视图)
 //对于基座来说，由于光电编码器的AB两线和单片机的连接顺序不定，基座的正方向也不确定，需要调整角度和速度的方向为逆时针(俯视图)
 //需要根据正方向自行调节下面的代码
  bar_angle = -ma730.getAngle();
  bar_vel = -ma730.getVelocity();
  base_vel = -sensor.getVelocity();
  bar_vel_filtered=filter(bar_vel); 
  base_vel_filtered=base_filter(base_vel);
  error=_normalizeAngle(bar_angle)-target_angle;

  theta=3.1415+error; 
  theta_dot=bar_vel_filtered;
  costheta=cos(theta); //提前计算，方便后边使用
	
  digitalWrite(PC13, LOW);
  //一共四个状态机
  switch(state){
    case 1: //idle 空状态，当摆竖直朝下的时候，给一个脉冲，不然energy shaping法无法启动，摆不能自己摆起来
      if(i<100){
        _delay(1);
        target_force=0.7; 
        i++;
      }
      else
        state=2;
      break;
    case 2: //swing up 使用energy shaping法摆起来，接近平衡位置的时候切换状态到LQR
      target_force = 0.0065*theta_dot*costheta*(-8.5*(1+costheta)+0.0075*theta_dot*theta_dot); 
      if(abs(error)<0.1)
        state=3;
      break;
    case 3: //这里的参数是理论加调试改动的，结构上使用的是LQR的结构
		digitalWrite(PC13, HIGH);
		target_force =base_vel_filtered*0.05*angle_kp-1.5*error*angle_kp-0.1*bar_vel_filtered*angle_kp;
      if(abs(error)>0.25)
        state=4;
      break;
    case 4: //swing down 就是Swing up的逆向使用，为了更快的让摆竖直向下
		target_force = -0.007*theta_dot*costheta*costheta*costheta*costheta*(-8.5*(1+costheta)+0.0075*theta_dot*theta_dot);
		if(abs(theta)<0.2&&abs(theta_dot)<0.2)
			{
				state=1;
				i=0;
			}
		break;
  }
  
//如果力矩方向和基座方向不一致，需要取反
target_force=-target_force;

//死区补偿
  if(target_force>threshold)
    target_force=threshold;
  else if(target_force<-threshold)
    target_force=-threshold;
  
//串口输出数据
	Serial2.print(state); Serial2.print('\t');
	Serial2.print(error); Serial2.print('\t');
	Serial2.print(bar_vel); Serial2.print('\t');
	Serial2.print(bar_vel_filtered); Serial2.print('\t');
	Serial2.print(base_vel_filtered); Serial2.print('\t');
	Serial2.println(_normalizeAngle(bar_angle));
	motor.loopFOC();//力矩输出
//注释掉了校准力矩 
	if(target_force>0)target_force+=dead_force;
	else if(target_force<0)target_force-=dead_force;
//转速过大就保护
	if(abs(base_vel_filtered)>50) target_force=0;
	motor.move(target_force);
	
}


int main(void)
{
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    GPIO_JTAG_Disable();
    Delay_Init();
    ADCx_Init(ADC1);
    setup();
    for(;;)loop();
}

结言

本文首先由意义引出对倒立摆的研究，然后确立研究方向，然后利用控制学步骤，先建立模型->分析系统->设计控制器->仿真效果->利用电路实现控制器和其他实物的搭建。本文是帮助和利用开源项目下做出来的，其中有许多错误和不足的部分，但希望能够或多或少给你带来帮助，也希望批评指正。

参考:

1. 项目GitHub链接

2. 魏帅. 直线倒立摆的控制算法研究[D].西安电子科技大学,2020.

3. 倒立摆仿真部分

4. 倒立摆装置项目参考部分

5. B站现代控制理论知识

6. 刘 豹,唐 万 生. 现代控制理论[Ｍ]. 北京:机 械 工 业 出 版 社, 2006

7. FOC参考