本文介绍了如何利用带编码器的直流减速电机、角位移传感器和PID控制实现倒立摆的稳定平衡，包括硬件设备选择、ADC数据采集与处理、双环PID控制策略以及电机驱动的详细代码。
1.硬件部分
先给大家看看硬件部分：先是一个直流减速电机，一定要买带编码器的直流减速电机，然后是角位移传感器，就是图中黄色的，来识别摆杆的角位移，然后一个tb6612。板子随意，没特别要求，大体的结构如下图（我主要负责软件，硬件大家可以去了解一下其他大佬的）
自动起摆效果如图
旋转倒立摆
在正式开始讲解之前，我先提一下倒立摆的注意事项，
首先，要保持硬件的稳定性如果硬件一直摇晃的话，那么会很难调出来，甚至一直调不出来，这里给大家参考两个结构，一个是短轴电机，一个是长轴电机。
2.软件部分
我们需要采取两个环才能更好的控制倒立摆，一个是角位移传感器提供的角度环，一个是编码器的位置环。即要使用连个pid，假如只用角度环，那摆杆将左右摇摆，并且当偏转角较大时会一直向一个方向偏，从而无法保持平衡，也就是当偏转恨小的时候，我们不需要过大的pid控制，可以使用位置环，即两个不同的pid。
1.adc采集角位移数据：
先配置adc，因为只有一个脚，所以我们只需要开启单通道采集就行
我使用了一个定时器中断：间隔是5ms这里和大家说一下psc和rcc的配置关系：
计算赫兹：72*10^6除以(psc+1)(arr+1)=hz 时间分之一就是赫兹‘
在这里我还对采集到的adc值做了平均化处理，最大限度的避免了误差的出现。
/**************************************************************************
函数功能：单位时间读取ADC
入口参数：TIM
返回 值：ADC
**************************************************************************/
uint16_t ADC_Read(void)
{
int sum = 0, max = 0, min = 4095,adc_result = 0;
uint8_t i=0;
HAL_ADC_Start(&hadc1); //启动ADC转换
HAL_ADC_PollForConversion(&hadc1, 12); //等待转换完成，50为最大等待时间，单位为ms
//平滑均值滤波
for(i=1;i<=5;i++)
{
adc[i] = HAL_ADC_GetValue(&hadc1);
sum += adc[i];
if(adc[i] > max) max = adc[i];
if(adc[i] < min) min = adc[i];
}
adc_result = (sum - max - min) / 3.0;
// sum = KalmanFilter(adc);
return adc_result;
}
/**************************************************************************
函数功能：卡尔曼滤波
入口参数：inData
返回 值：inData
**************************************************************************/
//卡尔曼滤波
int KalmanFilter(int inData)
{
static float prevData = 0; //先前数值
static float p = 10, q = 0.1, r = 10, kGain = 0; // q控制误差 r控制响应速度
p = p + q;
kGain = p / ( p + r ); //计算卡尔曼增益
inData = prevData + ( kGain * ( inData - prevData ) ); //计算本次滤波估计值
p = ( 1 - kGain ) * p; //更新测量方差
prevData = inData;
return inData; //返回滤波值
}
大家可以看到我还写了一个卡尔曼滤波，大家可以看看效果，是一个非常好的滤波方式：
至于我为什么没有用，是因为我的控制函数已经用了很多flash了，而我又用的stm32c8t6，他的flash非常少，如果大家用更好的芯片可以试一试这个滤波方法，真的非常不错，就是太占flash了
2.直流减速电机的驱动
将定时器设置为编码器模式，因为减速电机有ab连个通道，通过两个口波形的不同可以判断其速度，这就是控制倒立摆的速度环。不了解的小伙伴可以去看看原理讲解http://t.csdnimg.cn/zzHwC
我将左右和控制pwm波的函数封装到一起，方便后面使用 ，这里需要注意加上电机的死区值
/**************************************************************************
函数功能：电机转动控制函数
入口参数：闭环控制最终输出值
**************************************************************************/
void Set_Pwm(int Moto1)
{
if(Moto1>0) AIN1 = 0,AIN2 = 1;
else AIN1 = 1,AIN2 = 0;
if(Moto1) /* 控制器有输出 */
{
Moto1 = abs(Moto1) + dead_area;/* 取绝对值 + 死区电压*/
Limit(&Moto1,max_rpm); /* 限幅 */
}
else /* 无输出，直接关闭驱动 */
{
Moto1 = 0;
}
__HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_2,Moto1);
Contrl();
}
/**************************************************************************
函数功能：限制电机速度
入口参数：闭环控制最终输出值
**************************************************************************/
void Limit(int *motoA,int maxrpm)
{
if(*motoA>maxrpm)*motoA=maxrpm;//最大7200
if(*motoA<-maxrpm)*motoA=-maxrpm;
}
然后是采集编码器的值，为我们的位置环提供数据，采集后记得附零，以免被覆盖
uint32_t encoder_speed(void)
{
uint32_t speed;
speed=(short)__HAL_TIM_GET_COUNTER(&htim3);//获得编码器脉冲
__HAL_TIM_SET_COUNTER(&htim3,0); //编码器脉冲清0
return speed;
} 现在我们角位移和编码器两个值都采集到了，可以开始进行控制函数的编写，这里运用的是直立环和位置环，之所以是位置环而不是速度环，是因为速度环无法稳定在原地，而且使用位置环更方便写最后一个。
4.控制摆杆（两环pid）
首先我们先将摆杆用手拿到竖直位置，观看adc的值为多少，这时候值作为我们的目标值。
在开始做题之前，我们先将两个pid给写好
1.直立环：
int Gray_PID(int adc_value,int aim_value)
{
static int last_bias=0;
int bias=0, abs_bias = 0;
bias = aim_value - adc_value;
if(bias > 4096) bias=4096;
//线性kp
abs_bias = abs(bias);
// if(bias > -280 && bias < 0 ) Gray_KP = 0.45* (( -(1.5 /1500) * abs_bias)+ 15.2);
// else Gray_KP = 0.45* (( -(1.5 /1500) * abs_bias)+ 18.3);
if(bias > -850 && bias < 0 ) Gray_KP = 0.5* (( -(1.5 /1500) * abs_bias)+ 29.7);//37.2
else Gray_KP = 0.5* (( -(1.5 /1500) * abs_bias)+ 29.8);//37.3
int res = Gray_KP*bias-Gray_KD*(bias-last_bias);
last_bias = bias;
return res;
}大家看见我的直立环和标准的有点不一样，是因为我在进行调试过程中发现当偏差值过大时，无法进行拟合，所以我将kp的值改为线性kp，那么当偏差过大的时候，kp的值也会随着加大，从而实现追钟，调参的问题后面再说，下面是位置环。
2.位置环：
int GetVelocity(int Encoder_Cnt,int AimVelocity)
{
static int index = 0;
// 1.计算速度偏差 //
Encoder_Err = (AimVelocity-Encoder_Cnt); //速度目标值//Encoder_Left+Encoder_Right 最大是 160 7200 /（160 * 50%） = 90
// 2.对速度偏差进行--低通滤波--
// low_out = (1-a)*Ek+a*low_out_last
Encoder = Encoder_Err*0.3 + Encoder_last*0.7;// 使得波形更加平滑
Encoder_last = Encoder; // 防止速度过大影响直立环的正常工作
// 3. 对速度偏差进行指数衰减
// if(adc_value <aim_angle+20 && adc_value>aim_angle-20)
// {
// float decay_factor = 0.005; // 控制衰减速度的因子
// Encoder_Integral = Encoder_Integral * (1 - decay_factor) + Encoder * decay_factor; // 指数衰减
// Encoder_Integral = 0;
// }
// 4.积分限幅
if(Encoder_Integral>250) Encoder_Integral=250;
if(Encoder_Integral<-320) Encoder_Integral=-320;
if(Moto_Flag == 1)
{
Encoder_Integral=0; //===电机关闭后或者复位清除积分
Moto_Flag=0;
}
//积分分离
if(abs(Encoder)>5)
{
index=0;
}
else
{
index = 1;
// 3.对速度偏差积分出位移,遥控的速度通过积分融入速度控制器，减缓速度突变对直立控制的影响
Encoder_Integral += Encoder;
//5.速度环控制输出
Velocity=Encoder*kp+index*Encoder_Integral*ki-kd*(Encoder-Encoder_last);
Velocity = Velocity>3000?3000:Velocity;
Velocity = Velocity<-3000?-3000:Velocity;
}
return Velocity;
}这里作了积分分离处理，其他的都是常规写法。为什么要积分分离，我们后面再讲。
ok，完成·以上其全部工作之后，我们就可以开始愉快的做题了
3.开始做题
（1）摆杆从处于自然下垂状态（摆角0°）开始，驱动电机带动旋转臂作往复旋转使
摆杆摆动，并尽快使摆角达到或超过-60°~ +60°；
这个就不多说的，在定时器里面定义一个变量加加，达到几秒左转几秒右转就行，大家看看代码参考一下就行：
if (mode_flag==1)
{
// kp=kp_val;
// ki=kp_val/200;
kp=3;
ki=0.00017;
kd=0.4;
dead_area=660;
mode_time_flag++;
if (mode_time_flag <= 30)
{
Velocity_out=GetVelocity(Encoder_Cnt,60);
Moto1= Velocity_out;
}
else if (mode_time_flag > 30)
{
if (mode_time_flag > 60)
mode_time_flag=0;
Velocity_out=GetVelocity(Encoder_Cnt,-60);
Moto1= Velocity_out;
}
}（2）从摆杆处于自然下垂状态开始，尽快增大摆杆的摆动幅度，直至完成圆周运
动；
这道题也可以用第一问的方法做，但是很不稳定，而且这道题做得好不好，直接关系到你自动起摆得速度，我这里采用得办法是当他进入下方一小段区间的时候1，进行向相反的方向转，从而快速转一周，参考代码如下：
static int number = 0;
up++;
dead_area=680;
if(adc_value < 3750 && adc_value > 3630)//进行方向调转
{
number ++;
if(number == 1)
{
mode_time_flag++;
up = 0;
}
}
if(up < 20)
{
if (mode_time_flag%2 == 0)
{
Moto1 = 2000;
// Velocity_out=GetVelocity(Encoder_Cnt,60);
// Moto1= Velocity_out;
}
else
{
Moto1 = -2000;
// Velocity_out=GetVelocity(Encoder_Cnt,-60);
// Moto1= Velocity_out;
}
}
else
{
Moto1 = 0;
number = 0;
}
}（3）在摆杆处于自然下垂状态下，外力拉起摆杆至接近165°位置，外力撤除同时，
启动控制旋转臂使摆杆保持倒立状态时间不少于5s；期间旋转臂的转动角度不大于90°。
这道题也是自动起摆的关键，也是所有题中最难的部分，这里写好了，下面三道题都不会有困难，我也会在这道题进行详细的解说，包括如何调pid的参数
首先还是给大家看看参考代码：
else if (mode_flag==3)
{
if(aim_angle - adc_value>0)
{
dead_area=230;
}
else
{
dead_area = 300;
}
// kp=-255;//-199
// ki=-3;//-0.4
// kd=-15;
kp=-465;//
ki=-3.1;//
kd=-20;
// mode_balance_time_flag++;
// if(mode_balance_time_flag>3)
// {
// Encoder_Cnt=0;
// mode_balance_time_flag=0;
// }
if (adc_value >aim_angle+850 || adc_value<aim_angle-850)//超出范围
{
AIN1=0;
AIN2=0;
Moto1=0;
Moto_Flag=1;
Encoder_Cnt=0;
mode_balance_time_flag=0;
Position_control_flag=0;
Velocity_out=0;
Vertical_out=0;
position_count=0;
}
else
{
Velocity_out=GetVelocity(Encoder_Cnt,0);//速度环
if(adc_value <aim_angle+300 && adc_value>aim_angle-300 )//在范围内&& adc_value > aim_angle+200 && adc_value <aim_angle-200
{
mode_balance_time_flag++;
if (Position_control_flag ==0)
{
if(mode_balance_time_flag>5)
{
mode_balance_time_flag=0;
Position_control_flag =1;
//Encoder_Cnt=0;
}
}
}
if (Position_control_flag==1)
{
position_count++;
if(position_count<3)
{
Velocity_out=GetVelocity(Encoder_Cnt,0);
}
else
position_count=0;
}
Vertical_out=Gray_PID(adc_value,aim_angle);
// //角速度
// float speed = (adc_value - last_adc_value)/10;
// last_adc_value = adc_value;
//
Moto1 = Vertical_out+Velocity_out;// + 1.2*speed
}
这里是只有在上方才会启动平衡代码
首先是进行直立环的参数调试：1.先调整kp，kp调好的效果是低平抖动，但是回正速度非常快，极性千万不要调反了
2.然后调整kd，kd跳到可以使他高频抖动时候再乘以0.8就好了
调好的直立环是可以进行直立的，但是会随着误差的增加越转越快，直到倒下，如果大家的·1直立环可以保持3圈左右不到，那么直立环就可以了，就不要去动他了
总的来说直立环还好，当时40多分钟就弄好了，最关键的是位置环，让他立在原地这里我弄了几天才弄好，主要每次加上位置环他就不行，下面说一下我调位置环的一些经验
首先这个位置环是正反馈，因为直立环之所以会倒下就是因为有一个偏始终到不了，而速度环的作用就是补足这一小点。
首先来调kp，kp调好的效果是他还是会一直旋转，但是他不会一直加速，因为·有kp的作用，他会一直旋转，但是不会掉下来。这样就说明kp差不多了
然后是积分项，这里是重灾区，大家看见我写了一个积分分离，我试了很多处理积分的问题，发现积分分离是比较好的效果
在他误差过大就给他置零，比定时清零和指数衰减好得多。然后就是积分上下限，大家可以用vofa把积分项打印出来，（不会vofa的可以去搜一搜），应该是一个类似正选函数的一个波形，上下限取正选函数的做大只和最小值（分别加一点和减一点1），然后调整ki，调好的话应该是可以立在原地的
接下来是kd，上面调好之后在原地应该会有一点抖动，这时候就需要kd使其变得丝滑起来
总之上述只是大概方法，大家可以根据实际情况进行调整
（1）从摆杆处于自然下垂状态开始，控制旋转臂作往复旋转运动，尽快使摆杆摆起
倒立，保持倒立状态时间不少于10s；
这个起摆接下来就好了，我们只需要将第二问和手动起摆联系起来就可以实现了
代码如下：
/**************************************************************************
功 能: 起摆函数
输 入: 无
返 回 值: 无
**************************************************************************/
void Auto_run(void)
{
static int number = 0;
up++;
dead_area=680;
if(adc_value < 3750 && adc_value > 3630)//进行方向调转
{
number ++;
if(number == 1)
{
mode_time_flag++;
up = 0;
}
}
if(adc_value <aim_angle+300 && adc_value>aim_angle-300 )//保持平衡
{
if(aim_angle - adc_value>0)
{
dead_area=250;
}
else
{
dead_area = 320;
}
kp=-465;//
ki=-3.1;//
kd=-20;
Velocity_out=GetVelocity(Encoder_Cnt,0);//速度环
if(adc_value <aim_angle+300 && adc_value>aim_angle-300 )//在范围内&& adc_value > aim_angle+200 && adc_value <aim_angle-200
{
mode_balance_time_flag++;
if (Position_control_flag ==0)
{
if(mode_balance_time_flag>5)
{
mode_balance_time_flag=0;
Position_control_flag =1;
//Encoder_Cnt=0;
}
}
}
if (Position_control_flag==1)
{
position_count++;
if(position_count<3)
{
Velocity_out=GetVelocity(Encoder_Cnt,0);
}
else
position_count=0;
}
Vertical_out=Gray_PID(adc_value,aim_angle);
Moto1 = Vertical_out+Velocity_out;
}
else
{
if(up < 20)
{
if (mode_time_flag%2 == 0)
{
Moto1 = 2000;
// Velocity_out=GetVelocity(Encoder_Cnt,60);
// Moto1= Velocity_out;
}
else
{
Moto1 = -2000;
// Velocity_out=GetVelocity(Encoder_Cnt,-60);
// Moto1= Velocity_out;
}
}
else
{
Moto1 = 0;
number = 0;
}
}
}（2）在摆杆保持倒立状态下，施加干扰后摆杆能继续保持倒立或2s内恢复倒立状
态；
这个没什么好说的，就看你的参数调的得好不好
（3）在摆杆保持倒立状态的前提下，旋转臂作圆周运动，并尽快使单方向转过角度
达到或超过360°；
这个就更简单了，在位置环不停得加就好了
参考代码如下
/**************************************************************************
功 能: 旋转一周
输 入: 无
返 回 值: 无
**************************************************************************/
void revil(void)
{
static int stc = 0,article = 0;
if(stc < 20)
{
article++;
if(article==100)
{
stc = stc+1;
article = 0;
}
}
if(adc_value <aim_angle+300 && adc_value>aim_angle-300 )//保持平衡
{
if(aim_angle - adc_value>0)
{
dead_area=250;
}
else
{
dead_area = 320;
}
kp=-465;//
ki=-3.1;//
kd=-20;
Velocity_out=GetVelocity(Encoder_Cnt,0);//速度环
if(adc_value <aim_angle+300 && adc_value>aim_angle-300 )//在范围内&& adc_value > aim_angle+200 && adc_value <aim_angle-200
{
mode_balance_time_flag++;
if (Position_control_flag ==0)
{
if(mode_balance_time_flag>5)
{
mode_balance_time_flag=0;
Position_control_flag =1;
//Encoder_Cnt=0;
}
}
}
if (Position_control_flag==1)
{
position_count++;
if(position_count<3)
{
Velocity_out=GetVelocity(Encoder_Cnt,stc);
}
else
position_count=0;
}
Vertical_out=Gray_PID(adc_value,aim_angle);
Moto1 = Vertical_out+Velocity_out;
}
}
如果有问题可以在评论区留下，或者直接私信我
通过网盘分享的文件：PIDTEXT - 副本.zip 链接: 百度网盘 请输入提取码百度网盘为您提供文件的网络备份、同步和分享服务。空间大、速度快、安全稳固，支持教育网加速，支持手机端。注册使用百度网盘即可享受免费存储空间
https://pan.baidu.com/s/1X3mf-0iWm9LHlrzC04YQZw?pwd=nwrw 提取码: nwrw
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
