双环PID实现倒立摆实验

东亚_劲夫

已于 2025-08-19 15:56:54 修改

阅读量291

点赞数 11

CC 4.0 BY-SA版权

分类专栏： PID 文章标签：算法

于 2025-08-18 21:17:33 首次发布

本文链接：https://blog.csdn.net/m0_75103637/article/details/150496010

PID 专栏收录该内容

3 篇文章

订阅专栏

文章目录

一、倒立摆的介绍
二、倒立摆控制

一、倒立摆的介绍

下图是倒立摆的稳定状态，通过下面的编码器，左右转动横杆，使得活动杆左右摇摆，将摆杆摆至顶端，然后由双环 PID 的输出值控制摆杆竖直向上，通过角度传感器判断摆杆杆是否在顶端或是在左右两边。
在这里插入图片描述

二、倒立摆控制

控制倒立摆的摆杆使用双环 PID，分别是：

内环角度环：保持摆杆稳定倒立，输出 PWM 直接控制电机
外环位置环：保持横杆的位置固定在一个地方，输出角度为内环的目标角度，并加上中心角度

2.1.启动和停止

为了防止参数还没调试完成，横杆左右乱摆造成危险，因此调参数之前需要设计启动和停止，本次实验通过按下 KEY1 启动和停止，定义一个变量说明状态，或是摆杆不再用户设定的中心区间就会停止输出动力，下面是代码演示：

uint8_t RunState;

int main()
{
	KeyNum = Key_GetNum();		
	if (KeyNum == 1)			
	{
		RunState = !RunState;	//控制程序启动和停止
	}
}

void TIM1_UP_IRQHandler(void)
{
	if (TIM_GetITStatus(TIM1, TIM_IT_Update) == SET)
	{
		/*摆杆倒下自动停止PID程序*/
		if (! (Angle > CENTER_ANGLE - CENTER_RANGE
			&& Angle < CENTER_ANGLE + CENTER_RANGE))	//如果角度值超过了规定的中心区间
		{
			RunState = 0;			//运行状态变量置0，自动停止PID程序
		}
		/*根据运行状态执行PID程序或者停止*/
		if (RunState)				
		{
			//运行状态
		}
		else						//如果运行状态为0
		{
			Motor_SetPWM(0);		//不执行PID程序且电机PWM直接设置为0，电机停止
		}
		TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
	}
}

2.2.稳定状态

在中断里面每 1ms 取一次角度传感器值：

Angle = AD_GetValue();		//获取角度传感器的角度值
Speed = Encoder_Get();		//获取电机的速度值
Location += Speed;			//获取电机的位置值

2.2.1.内环 PID

首先配置内环 PID 的三项，内环 PID 属于是定位置控制，以 PD 为主，I 为辅，首先将 PID 三项初始化为 0，再把摆杆竖起来，调节 Kp 项，给 Kp 调一点值，如果摆杆向右倾斜，横杆也向右移动，那说明设计的极性是正确的，如果相反，只需要在输出值取个负数即可，Kp 越大反应越快，加大 Kp 直到反应迅速，同时没有剧烈抖动；再设置 Kd 项，逐渐加大 Kd 项，直到有明显的稳定即可：

#define CENTER_ANGLE		2010	//目标中心角度，通过多次调试得到目标中心角度，如果目标中心角度过小或者过大，会导致横杆为了平衡会顺时针旋转或者逆时针旋转

PID_t AnglePID = {			//内环角度环PID结构体变量，定义的时候同时给部分成员赋初值
	.Target = CENTER_ANGLE,	//角度环目标值初值设定为中心角度值
	
	.Kp = 0.3,				//比例项权重
	.Ki = 0.01,				//积分项权重
	.Kd = 0.4,				//微分项权重
	
	.OutMax = 100,			//输出限幅的最大值
	.OutMin = -100,			//输出限幅的最小值
};

Count1 ++;				//计次自增
if (Count1 >= 5)		//如果计次5次，则if成立，即if每隔5ms进一次
{
	Count1 = 0;			//计次清零，便于下次计次
	
	/*以下进行角度环PID控制*/
	AnglePID.Actual = Angle;		//内环为角度环，实际值为角度值
	PID_Update(&AnglePID);			//调用封装好的函数，一步完成PID计算和更新
	Motor_SetPWM(AnglePID.Out);		//角度环的输出值给到电机PWM
}

2.2.2.外环 PID

内环参数的好坏能直接影响到倒立摆能否立起来，外环参数的好坏只是影响位置固定的速度和精准度。下面代码里的式子- LocationPID.Out需要为负，经实验观察，如果设置为正，首先开始设置 Kp 时，手动让摆杆向上并且顺时针移动横杆，发现输出值为负，并作用到角度传感器的目标值，使其减小，角度传感器的目标值在减小意味着位置就需要顺时针移动去接住摆杆下垂，如此重复，横杆就 360° 转起来了，这样形成一个正反馈，因此需要把- LocationPID.Out定义为负，让目标位置尽量接近中心位置；接着再调节 Kd 项，如果 Kd 项数值不够大，横杆就会来回加速摆，因此 Kd 项给到 4。

PID_t LocationPID = {		//外环位置环PID结构体变量，定义的时候同时给部分成员赋初值
	.Target = 0,			//位置环目标值初值设定为0
	
	.Kp = 0.4,				//比例项权重
	.Ki = 0,				//积分项权重
	.Kd = 4,				//微分项权重
	
	.OutMax = 100,			//输出限幅的最大值
	.OutMin = -100,			//输出限幅的最小值
};

Count2 ++;				//计次自增
if (Count2 >= 50)		//如果计次50次，则if成立，即if每隔50ms进一次
{
	Count2 = 0;			//计次清零，便于下次计次
	
	/*以下进行位置环PID控制*/
	LocationPID.Actual = Location;	//外环为位置环，实际值为位置值
	PID_Update(&LocationPID);		//调用封装好的函数，一步完成PID计算和更新
	AnglePID.Target = CENTER_ANGLE - LocationPID.Out;	//外环的输出值作用于内环的目标值，组成串级PID结构
}

2.3.启摆

按下按键，让机器自动启摆，将摆杆摆至最高位置，并稳定下来。启摆的实现类似荡秋千，当秋千荡至最高时，稍微给一点力，即可荡至对面方向，这样来回加速，最终到达或是经过最高点。本次实验的启摆的实现：当摆杆到某一侧的最高点，抖动一下再往下走，下面是各个状态转移：

RunState = 0，停止状态，PWM 输出为 0
RunState = 1，判断状态，根据角度值判断摆杆在左侧区间或是右侧区间，确定下一步要转入的状态

如果摆杆位于右侧区间且处于最高点位置：

RunState = 21，向左摆动，PWM 输出正值
RunState = 22，延时，计数计时
RunState = 23，延时结束，向右摆动，PWM 输出负值
RunState = 24，延时，计数计时

接着转到 RunState = 1 进行判断，如果摆杆位于左侧区间且处于最高点位置：

RunState = 31，向右摆动，PWM 输出正值
RunState = 32，延时，计数计时
RunState = 33，延时结束，向左摆动，PWM 输出负值
RunState = 34，延时，计数计时

如果摆杆摆至中心区间，执行 PID 控制程序，进行控制摆杆稳定：

RunState = 1，判断状态，如果摆杆进入中心区间，判断进入 RunState = 4。
RunState = 4，执行 PID 控制程序

为了启动的时候避开盲区，按下启动按键就进入 RunState = 21 状态或是 RunState = 31 状态，下面是在左边或是右边区间时的代码实现：

#define START_PWM			35	//启摆时电机转动的力度
#define START_TIME			100			//启摆时电机施加瞬时驱动力的时间，一般在80~120之间

if (RunState == 21)		//当前处于状态21，向左摆动
{
	Motor_SetPWM(START_PWM);	//电机左转，旋转力度由START_PWM指定
	CountTime = START_TIME;		//初始化计数计时器，时间由START_TIME指定
	RunState = 22;				//随后状态自动转入22
}
else if (RunState == 22)		//当前处于状态22，延时
{
	CountTime --;				//计数计时变量递减
	if (CountTime == 0)			//如果减到0了，说明计时时间到
	{
		RunState = 23;			//随后状态转入23
	}
}
else if (RunState == 23)		//当前处于状态23，向右摆动
{
	Motor_SetPWM(-START_PWM);	//电机右转
	CountTime = START_TIME;		//初始化计数计时器
	RunState = 24;				//随后状态自动转入24
}
else if (RunState == 24)		//当前处于状态24，延时
{
	CountTime --;				//计数计时变量递减
	if (CountTime == 0)			//如果减到0了，说明计时时间到
	{
		Motor_SetPWM(0);		//向左施加瞬时驱动力过程结束，电机停止
		RunState = 1;			//随后状态转入1，继续下一次判断
	}
}




else if (RunState == 31)		//当前处于状态31，向右摆动
{
	Motor_SetPWM(-START_PWM);	//电机右转
	CountTime = START_TIME;		//初始化计数计时器
	RunState = 32;				//随后状态自动转入32
}
else if (RunState == 32)		//当前处于状态32，延时
{
	CountTime --;				//计数计时变量递减
	if (CountTime == 0)			//如果减到0了，说明计时时间到
	{
		RunState = 33;			//随后状态转入33
	}
}
else if (RunState == 33)		//当前处于状态33，向左摆动
{
	Motor_SetPWM(START_PWM);	//电机左转
	CountTime = START_TIME;		//初始化计数计时器
	RunState = 34;				//随后状态自动转入34
}
else if (RunState == 34)		//当前处于状态24，延时
{
	CountTime --;				//计数计时变量递减
	if (CountTime == 0)			//如果减到0了，说明计时时间到
	{
		Motor_SetPWM(0);		//向右施加瞬时驱动力过程结束，电机停止
		RunState = 1;			//随后状态转入1，继续下一次判断
	}
}
else if (RunState == 4)			//当前处于状态4，倒立摆
{
	//2.2.2.内环的程序
}

下面是 RunState = 1 判断状态的代码实现：

else if (RunState == 1)		//当前处于状态1，判断状态
{
	/*计次分频*/
	Count0 ++;				//计次自增
	if (Count0 >= 40)		//如果计次40次，则if成立，即if每隔40ms进一次
	{
		Count0 = 0;			//计次清零，便于下次计次
		
		/*获取本次、上次、上上次的角度值，以40ms为间隔，连续采样3次角度值*/
		Angle2 = Angle1;
		Angle1 = Angle0;
		Angle0 = Angle;
		
		/*判断摆杆当前是否处于右侧最高点*/
		if (Angle0 > CENTER_ANGLE + CENTER_RANGE		//3次角度值均位于右侧区间
		 && Angle1 > CENTER_ANGLE + CENTER_RANGE
		 && Angle2 > CENTER_ANGLE + CENTER_RANGE
		 && Angle1 < Angle0								//且中间一次角度值是最小的，表示摆杆处于右侧最高点
		 && Angle1 < Angle2)
		{
			RunState = 21;	//状态转入21，执行向左施加瞬时驱动力的程序
		}
		
		/*判断摆杆当前是否处于左侧最高点*/
		if (Angle0 < CENTER_ANGLE - CENTER_RANGE		//3次角度值均位于左侧区间
		 && Angle1 < CENTER_ANGLE - CENTER_RANGE
		 && Angle2 < CENTER_ANGLE - CENTER_RANGE
		 && Angle1 > Angle0								//且中间一次角度值是最大的，表示摆杆处于左侧最高点
		 && Angle1 > Angle2)
		{
			RunState = 31;				//状态转入31，执行向右施加瞬时驱动力的程序
		}
		
		/*判断摆杆当前是否进入中心区间*/
		if (Angle0 > CENTER_ANGLE - CENTER_RANGE		//2次角度值均位于中心区间
		 && Angle0 < CENTER_ANGLE + CENTER_RANGE
		 && Angle1 > CENTER_ANGLE - CENTER_RANGE
		 && Angle1 < CENTER_ANGLE + CENTER_RANGE)
		{
			/*一些变量初始化归零，避免错误的初值影响PID程序运行*/
			Location = 0;				//实际位置归零
			AnglePID.ErrorInt = 0;		//角度环误差积分归零
			LocationPID.ErrorInt = 0;	//位置环误差积分归零
			
			/*启摆完成*/
			RunState = 4;				//状态转入4，执行PID控制程序
		}
	}
}