基于串级pid控制系统的两轴无人机云台设计

写在前面:本文主要介绍自制简易无人机两轴云台的串级pid控制系统的设计思路,有关机械结构等其他内容不做赘述

一.云台硬件组成
二.串级pid控制系统设计
三.C程序实现

一.云台硬件组成

两轴云台所需要的用的硬件有
1.两个直流无刷电机:分别做云台的yaw轴与pitch轴的执行机构。
2.主控板:这里选用stm32f1r8(一般的32芯片的性能都足以支撑此开发)。
3.陀螺仪:用作角速度反馈,这里选用mpu6050足够。
4.编码器:用作角度反馈,这里使用直流无刷电机自带的编码器,优点是误差较小且驱动方便。

二.串级pid控制系统设计

硬件平台搭好之后,那么进入正题。

自稳云台的重点在于自稳,何谓自稳呢?就是在人工不干预或受到一定的扰动的情况下,云台仍能保持着一定的姿态。如何实现自稳呢?这就需要依赖于pid控制器的帮助。

用的最为广泛且发展最成熟的控制器毫无疑问是pid控制,可是pid控制器也可以分为很多类别,这其中我们最为熟知的就是单回路pid控制,而串级pid控制,顾名思义就是两个单回路pid控制系统互相嵌套形成的pid控制器,是在简单pid上发展起来的,与简单pid控制器相比,串级控制系统的控制质量有显著的提高。

以下为本云台设计的串级pid控制模型
在这里插入图片描述

这一控制流程可以通过分析云台从“垂头丧气地耷拉”到“精神抖擞地挺立”经历的“艰难险阻”以一探究竟
1.电机没有电流值输入的情况时,两个电机无力矩作用,云台垂头丧气地埋着头,这时,你希望电机可以达到一定的角度并保持不动(即流程图中的期望角度)让云台抬起头,而电机编码器反馈回来的角度值与你期望的角度值一定是有差距的,这个差距就是角度误差(且在此时角度误差最大)。

2.角度误差形成后,电机则需要转动去缩小这个误差,既然要转动必然要有转动角速度,有了期望角度,那么期望角速度如何给定呢?不需要我们自己去给定,让角度PID控制器帮我们实现,如图,就是用角度误差作为角度PID控制器的输入值,输出值作为期望的角速度值。
这一过程也很好地体现了串级pid控制系统的最大的优势,即通过一个控制量去自动控制另外一个控制量。当角度误差较大时,那么期望的角速度也大,调节速度就很快,反之,云台到达期望位置时,角度误差几乎为0,那么期望角速度也为0,云台保持不动。

3.有了期望的角速度值,那么我们就只要利用这个值去驱动电机即可,而此角速度值一般较小,若直接作为驱动电机电流值就较小,所以一般用角速度PID控制器的P环节对此值做放大(也有可能缩小,具体依照程序实现)。经过角速度PID控制器的值可以用于驱动电机以改变云台姿态。

4.云台姿态在不断地改变,于是陀螺仪与编码器作为传感反馈的重要作用便体现出来,不断地反馈电机的角度值和角速度值,如此便形成了闭环,即为上图所示的控制流程图。

三.程序设计

角度环与角速度环pid控制器实现
以下两段程序是角度环程序实现
两个电机编码器反馈回电机角度在程序中即为motor_info[0].rotor_angle与motor_info[1].rotor_angle)
输入电机角度设定值(程序中为2250与2400)

pid_calc(&yaw_Angle_pid  ,2250,motor_info[0].rotor_angle); 
pid_calc(&pitch_Angle_pid ,2400,motor_info[1].rotor_angle);

以下两段程序是角度环程序实现
由陀螺仪反馈回yaw轴以及pitch轴的角速度在程序中即为Speed_yaw和Speed_pit)
输入设定值即为角度环输出 (程序中为yaw_Angle_pid.output与pitch_Angle_pid.output)

pid_calc(&yaw_speed_pid  ,yaw_Angle_pid.output,Speed_yaw);
pid_calc(&pitch_speed_pid,pitch_Angle_pid.output,Speed_pit);

其中yaw_Angle_pid与pitch_Angle_pid分别为yaw轴与pitch轴的角度pid控制器结构体,yaw_speed_pid与pitch_speed_pid分别为yaw轴与pitch轴的角速度pid控制器结构体
附:程序中用到的pid控制器计算函数与pid控制器参数结构体如下
(1)pid控制器参数结构体

typedef struct _pid_struct_t
{
  float kp;
  float ki;
  float kd;
  float i_max;
  float out_max;
  
  float ref;      // target value
  float fdb;      // feedback value
  float err[2];   // error and last error

  float p_out;
  float i_out;
  float d_out;
  float output;
}pid_struct_t;

(2)pid控制器计算函数

float pid_calc(pid_struct_t *pid, float ref, float fdb)    //
{
  pid->ref = ref;  //设定值
  pid->fdb = fdb;  //反馈值
  pid->err[1] = pid->err[0];           //err[0]为当前误差
  pid->err[0] = pid->ref - pid->fdb;   //err[1]为上一次误差
  
  pid->p_out  = pid->kp * pid->err[0];                   //p控制器
  pid->i_out += pid->ki * pid->err[0];                    //I控制器
  pid->d_out  = pid->kd * ( pid->err[0] - pid->err[1] );  //D控制器
  LIMIT_MIN_MAX(pid->i_out, -pid->i_max, pid->i_max);     //此函数用于过滤由于扰动造成的过大或过小值
  
  pid->output = pid->p_out + pid->i_out + pid->d_out;     //pid控制器输出
  LIMIT_MIN_MAX(pid->output, -pid->out_max, pid->out_max);
  return pid->output;
}

需要stm32源程序或有指正意见的朋友可评论或私聊交流!

  • 0
    点赞
  • 89
    收藏
    觉得还不错? 一键收藏
  • 11
    评论
评论 11
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值