精华内容
下载资源
问答
  • 智能小车PID算法资料,一些自己用的7788的资料
  • PID-小车类-智能小车PID算法
  • 采用模糊 PID 控制算法,实现了对方向和速度的优化控制,即采用模糊 PD 算法智能小车方向进行控制,采用模糊 PID 算法对速度进行控制。该方案运用于智能车控制系统,克服了传统 PID 控制的不足,通过模糊规则进行...
  • 以STM32F103C8T6为控制器,L298N驱动两个直流电机,通过3个反射式红外传感器采集数据,采用两节3.2V锂电池串联作为电源的巡线小车。车上搭在了其他模块,如:超声波测距模块、显示屏模块等。 程序为C语言编写。 ...
  • Arduino智能小车(三):PID算法简介

    千次阅读 2021-01-24 13:32:04
    本篇文章是对PID算法的原理进行了一些探讨,并对其在Arduino智能车中的使用做了简单的介绍。如有纰漏,烦请指出???? 前两篇文章地址: Arduino智能小车(一):编码马达 Arduino智能小车(二):编码马达的使用 PID...

    本篇文章是对PID算法的原理进行了一些探讨,并对其在Arduino智能车中的使用做了简单的介绍。如有纰漏,烦请指出😜

    前两篇文章地址:
    Arduino智能小车(一):编码马达
    Arduino智能小车(二):编码马达的使用

    PID是控制里面最为经典的一个算法,从智能小车,到扫地机器人,再到工业机械臂,或多或少都在使用这个控制算法。简单而又可靠的特性使其适用于各种场合,而广泛的应用又使之衍生出了各种各样的改进算法。总而言之,PID可谓是控制领域最常用的一个组成部分之一。

    PID控制算法简介

    PID,这个名字由三个字母组成,P为比例,I为积分,D为微分。而这个算法顾名思义也就是包含三部分:比例(Proportional),积分(Integral)和微分(Derivative)。

    下面我将分别从控制理论和机器人学两个方面分别对PID算法的原理和组成进行探讨。

    自动控制原理中的PID

    我们从最基础的控制系统开始:

    开环控制系统

    这是一个开环的控制系统,优点是简单,但因为其没有反馈,所以无法保证精度,稳定性也较差,当系统有干扰时,输出有较大的不确定性。为了提高控制的精度和鲁棒性,一般会加入反馈,将反馈以一定的比例和输入量相减得到误差,将误差作为控制量,形成闭环控制系统,当K=1时,即为常见的单位负反馈

    闭环控制系统

    对于我们这里的电机控制而言,反馈量也就是编码器读取到的数据。

    而系统的重点就在于控制器(Controller)的设计,根据原理的不同也就产生了各种各样的控制器。

    而我们所要介绍的PID则是如下图所示的一个控制器:

    PID控制器

    上面所提到的三部分即为分别对误差E(t)E(t)比例积分微分运算,再加起来作为指令输入给被控制对象。表达式如下:
    U(t)=KpE(t)+Ki0tE(τ)dτ+KdE˙(t) U(t)=K_p\cdot E(t)+K_i\int^t_0 E(\tau)d\tau+K_d \cdot \dot{E}(t)

    一个例子

    我们用一个例子从本质上理解一下这三部分的作用,就以电机控制为例,比如我们要控制电机从0运动到1。

    这时,比例环节相当于每隔一段时间加KpErrK_p\cdot Err,也就是电机速度和误差是成正比的。而当kpk_p越大时,电机将更快的运动到目标位置。

    如果只有比例环节的话,当电机受到一个外作用扭矩时,电机最终是不会到1的,而是存在一个稳态误差。因为在到达这一点后,电机输入KpErrK_p\cdot Err所提供的扭矩已经无法克服外部扭矩作用,于是就停在了这一点。而积分环节的作用就是将误差积分,如果这个误差一直存在的话,积分会越来越大,最终也就表现在电机输入越来越大,也就可以克服这个扭矩继续运动至目标位置,消除稳态误差

    开过车的朋友们一定有这个经验,在红绿灯路口前如果速度较快的话,要提前点刹车,使车在线前停下来。微分环节的作用也在于此,其与误差的导数成正比,因为误差是一直在减小的,所以这一项一般是负的,相当于扣除一部分输入。当速度越快,也就是越快的接近目标时,扣除的部分越多,这一作用使得电机提前减速,起一个缓冲的作用,也避免了超调和振荡的出现。

    传递函数表示

    在经典控制理论中,习惯采用传递函数来描述控制系统,传递函数是指输出和输入的拉普拉斯变换之比。对上式求拉普拉斯变换,可得:
    U(s)=KpE(s)+Ki1sE(s)+KdsE(s)=Kp(1+1Tis+Tds)E(s) U(s)=K_pE(s)+K_i\frac{1}{s}E(s)+K_dsE(s)\\ =K_p(1+\frac{1}{T_is}+T_ds)E(s)
    式中涉及到的参数关系为Ki=KpTiK_i=\frac{K_p}{T_i}Kd=KpTdK_d=K_pT_d,而PID控制器性能的关键影响因素即为Kp,Ki,KdK_p,\,K_i,\,K_d这三个参数,这三个参数的整定也是设计PID控制器的重点部分。整定参数的方法暂时不表,我们下篇文章再做深入介绍。下面我们从另外一个角度去了解PID。

    机器人学中的PID

    这学期学习了机器人学,我们用了John J.Craig的**《机器人学导论》**作为教材,这本书中的最后几章对PID的介绍是从一个对我来说很“新鲜”的角度切入的,还挺有意思的。为了能彻底搞懂,我在这里做一些探讨。

    首先考虑一个“单关节”机械臂,简单起见,该关节为移动关节:

    这里我们假定关节处在一个势场中,其势能函数为V(x)=12kp(xxd)2V(x)=\frac{1}{2}k_p(x-x_d)^2xdx_d为目标位置,也就是关节处在目标位置时势能最低,而我们控制的目标也就是让关节的势能最低。这里利用了势能总是趋向于更低的地方这一原理设计了我们的控制算法。我们用弹性势能来类比我们所假定的势能,这样一来,就可以把左面的关节看作右面的系统。

    机器人PD控制

    先不考虑摩擦力及其他外力,由简单的受力分析很容易得到关节力表达式:
    f=mx¨=V(x)x=kp(xxd) f=m\ddot x =-\frac{\partial V(x)}{\partial x}=-k_p(x-x_d)
    上式即表达了一个简单的比例控制,接下来我们加入摩擦力:
    f=kp(xxd)kvx˙ f=-k_p(x-x_d)-k_v\dot x
    接下来是一个神奇的处理,要看仔细了。我们把控制器分为两部分,一部分基于模型(Model-Based),一部分基于伺服控制(Servo-Based)。基于模型的部分基于的就是机器人的模型;而伺服控制的部分比较贴近我们这里探讨的PID。

    之前我们已经知道f=mx¨f=m\ddot x,但实际上机器人正常工作时,因为各种因素影响还会引入一个非线性环节b(x,x˙)b(x,\dot x),因此:
    f=mx¨+b(x,x˙) f=m\ddot x+b(x,\dot x)
    这就是基于模型的部分,这一部分不做详细介绍,看不懂没关系。我们只关心伺服控制部分,所以接下来将Model-Based这部分剔出来:
    f=mx¨+b(x,x˙)=αf+β f=m\ddot x+b(x,\dot x)=\alpha f'+\beta
    形成类似下图的控制部分:

    Model-Based Part

    而显而易见的是,此时f=x¨f'=\ddot x,再代入上面受力分析的式子中可得:
    mf=mx¨=kp(xxd)kvx˙=m[kp(xxd)kvx˙]f=kp(xxd)kvx˙ mf'=m\ddot x=-k_p(x-x_d)-k_v\dot x=m[-k_p'(x-x_d)-k_v'\dot x]\\ \Rightarrow f'=-k_p'(x-x_d)-k_v'\dot x
    此时我们就得到了一个比例-微分(PD)控制的式子。观众朋友们或许也注意到了,这里的控制都是给定一个目标位置,控制关节运动到目标位置并停在这里(速度为0)。而对机器人做轨迹规划后得到的是轨迹上的目标位置,以及此时的目标速度和目标加速度。因此我们可以将关节力取为:
    f=x¨dkv(x˙x˙d)kp(xxd) f'=\ddot x_d-k_v'(\dot x-\dot x_d)-k_p'(x-x_d)
    由于f=x¨f'=\ddot x,移项可得:
    (x¨x¨d)+kv(x˙x˙d)+kp(xxd)=0e¨+kve˙+kpe=0 (\ddot x-\ddot x_d)+k_v'(\dot x-\dot x_d)+k_p'(x-x_d)=0\\ \Rightarrow \ddot e+k_v'\dot e+k_p'e=0
    也就是说我们最后的控制目的是要让位置,速度和加速度误差均为0。

    机器人PID控制

    但是这样的控制仍然存在一定的问题:比如关节受一个干扰力fdistf_{dist}时,系统到达稳态后会有一个稳态误差:
    Δx=fdistkp \Delta x=\frac{f_{dist}}{k_p}
    这个稳态误差可以用积分来表示:
    kpΔx=ki(xxd)dt k_p\Delta x=k_i\int (x-x_d)dt
    此时系统受力分析就变成了:
    f+fdist=mx¨+b(x,x˙) f+f_{dist}=m\ddot x+b(x,\dot x)
    将基于模型的控制规律代入可计算出f=x¨fdistmf'=\ddot x-\frac{f_{dist}}{m}

    于是我们在上面的ff'再加上这一个积分项得到如下比例-积分-微分(PID)控制规律:
    f=x¨dkv(x˙x˙d)kp(xxd)fdistm=x¨dkv(x˙x˙d)kp(xxd)ki(xxd)dt f'=\ddot x_d-k_v'(\dot x-\dot x_d)-k_p'(x-x_d)-\frac{f_{dist}}{m}\\ =\ddot x_d-k_v'(\dot x-\dot x_d)-k_p'(x-x_d)-k_i'\int(x-x_d)dt
    稍作移项我们就能发现这一控制规律的本质:
    e¨+kve˙+kpe+kiedt=fdistm ˙e¨+kve¨+kpe˙+kie=0 \ddot e+k_v'\dot e+k_p'e+k_i'\int e\,dt=\frac{f_{dist}}{m}\\ \xrightarrow{两边求导}\dot \ \ddot e+k_v'\ddot e+k_p'\dot e+k_i'e=0
    也就是说,我们PID控制的最终目的是使位置的误差,速度的误差,加速度的误差,甚至加加速度的误差都等于0。对于机器人的运动来说,当然是能越多的控制其运动过程中的参数,越能使其平稳的运动,也就越能达到良好的效果。

    最终我们的控制框图如下:

    PID控制框图

    Arduino上的PID算法

    原理讲了很多,或许读者已经晕晕乎乎,似懂非懂了,不过没有关系,我们不求甚解,直接从Arduino上实现PID控制入手,做一些实用的介绍。

    离散化

    在使用我们上面得到的式子前,还需要进行一步工作:离散化。毕竟我们不可能时时刻刻采样计算,输出连续的指令,这对于我们的控制系统来说是不现实的。正常的操作是以一定周期进行采样,计算,然后每隔一段时间给一个指令,输出的一个离散化的信号。

    我们假设系统的采样周期为T,以第K次采样为例。此时积分就可以表示为:i=1kE(i)\sum_{i=1}^k E(i),而微分则为:E(k)E(k1)E(k)-E(k-1),那上面的式子离散化后就为:
    U(k)=KpE(k)+Kii=1kE(i)+Kd[E(k)E(k1)] U(k)=K_pE(k)+K_i\sum^{k}_{i=1}E(i)+K_d\left[E(k)-E(k-1)\right]

    位置式PID

    直接采用上面的离散化公式,很容易写出代码:

    int PositionPID(int target_high, int target_low)
    {
        //bias为E(k),intergral_bias为积分项,last_bias为E(k-1),m为电机编码器读取到位置数据(反馈量)
      	static float bias, result, intergral_bias, last_bias;
      	if (m > target_high)
        	bias = target_high - m;
      	else if (m < target_low)
        	bias = target_low - m;
      	else
      	{
        	bias = 0;
        	intergral_bias = 0;
      	}
      	intergral_bias += bias;
      	result = Position_KP * bias + Position_KI * intergral_bias + Position_KD * (bias - last_bias);
      	last_bias = bias;
      	return result;
    }
    

    注意到上面的函数有两个target输入,这里是设定了一个目标阈,只要达到了target_lowtarget_high之间,就算达到了目标位置。这样操作增加了一定的稳定性,在一定程度上减小了振荡的可能性。

    另外要注意biasintergral_biaslast_bias变量类型是static float,这样会使其值保存到下一次计算,而不是随着该函数返回而清空。

    增量式PID

    除了位置式PID外,还有一种改进算法叫做增量式PID。所谓增量式,指的是我们每次不是计算U(k)U(k),而是ΔU\Delta U。其计算公式为:
    ΔU=U(k)U(k1)=Kp[E(k)E(k1)]+KiE(k)+Kd[E(k)2E(k1)+E(k2)] \Delta U=U(k)-U(k-1)\\ =K_p\left[E(k)-E(k-1)\right]+K_iE(k)+K_d\left[E(k)-2E(k-1)+E(k-2)\right]
    根据这个式子写出函数:

    float VelocityPID(float target)
    {
      	static float bias, result, last_bias, last_dif_bias;
      	bias = target - velocity;
        dif_bias = bias - last_bias;
      	result += Velocity_KP * dif_bias + Velocity_KI * bias + Velocity_KD * (dif_bias - last_dif_bias);	//注意这里是result+=
      	last_dif_bias = dif_bias;
      	last_bias = bias;
      	return result;
    }
    

    值得一提的是,增量式PID一般用于速度控制,也就是给定目标速度,得到电机输入。且一般只用PI两个参数,D为0。

    另外,增量式没有\sum运算,增量只由最近几次的采样值有关,因此偏差带来的影响较小,累计误差也小。

    PID库

    Arduino上有PID库,这个库贼好用👍,推荐大家用一用。可以使用Library Manager进行安装。具体的介绍在这里,例子很清楚,这里就不做详细介绍了。大神还对这个库的原理进行详细介绍,感兴趣的可以移步原文👈观看研究,(这里有翻译)

    展开全文
  • PID控制算法智能小车中的研究与应用PID控制算法智能小车中的研究与应用PID控制算法智能小车中的研究与应用
  • 51单片机智能小车PID速度控制算法源代码,该PID算法可以将智能小车的速度控制在设定的速度值上。需要结合测速模块使用。
  • 小车PID算法跑直线

    万次阅读 2014-08-03 19:47:37
    PID_setpoint(&PID_l, 160); PID_setpoint(&PID_r, 160); while(1) { printf("%d\r\n",(int)pwm_l); delay_ms(200); } } void CAR_GPIO_INIT() { GPIO_InitTypeDef GPIO_InitStructure; RCC_APB2PeriphClockCmd( ...
    #include<stm32f10x.h>
    #include"sys.h"
    
    
    extern unsigned char Rec_Dat_U1;
    extern unsigned char Rec_String_U1[200];
    extern int conut_flag_1;
    
    
    #define in_1(x)    x?GPIO_ResetBits(GPIOC , GPIO_Pin_0): GPIO_SetBits(GPIOC , GPIO_Pin_0)
    #define in_2(x)    x?GPIO_ResetBits(GPIOC , GPIO_Pin_1): GPIO_SetBits(GPIOC , GPIO_Pin_1)
    #define in_3(x)    x?GPIO_ResetBits(GPIOB , GPIO_Pin_10): GPIO_SetBits(GPIOB , GPIO_Pin_10)
    #define in_4(x)    x?GPIO_ResetBits(GPIOB , GPIO_Pin_11): GPIO_SetBits(GPIOB , GPIO_Pin_11)
    #define key_1   GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_12) 
    #define key_2   GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_13) 
    #define key_3   GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_14) 
    #define key_4   GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_15) 
    
    
    float  Kp_l =                0.82  ; //比例常数
    float  Ti_l =                0.09 ; //微分时间常数
    float  Td_l =                0.015 ; //采样周期
    
    
    float  Kp_r =                0.20  ; 
    float  Ti_r =                0.09 ;
    float  Td_r =                0.015 ;
    
    
    #define T                  0.02 
    #define Ki_l                Kp_l*(T/Ti_l)        // Kp Ki Kd 
    #define Kd_l                Kp_l*(Td_l/T)
    
    
    #define Ki_r                Kp_l*(T/Ti_l)        // Kp Ki Kd 
    #define Kd_r                Kp_l*(Td_l/T)
    
    
    #define left_b in_1(1); in_2(0);
    #define left_f in_1(0); in_2(1);
    #define right_f in_3(1); in_4(0);
    #define right_b in_1(0); in_2(1);
    
    
    extern float pwm_l;
    extern float pwm_r;
    
    
    void CAR_GPIO_INIT(void);
    
    
    int main()
    {
    
    
    // int speed=50;
    SYS_CONFIG();
    CAR_GPIO_INIT();
    pwm_init(TIM3_1);
    pwm_init(TIM3_2);
    TIM4_PWMINPUT_INIT(0xffff,31);  //1M速度采样率
    TIM5_PWMINPUT_INIT(0xffff,31);
    incPIDinit();
      PID_set(Kp_l,Ki_l,Kd_l,Kp_r,Ki_r,Kd_r);   
    //void PID_set(float pp_1,float ii_1,float dd_1,float pp_2,float ii_2,float dd_2)
      right_f
    left_f
    // set_pwm_percent(TIM3_1,50) ;
    // set_pwm_percent(TIM3_2,50) ;
    PID_setpoint(&PID_l, 160);
    PID_setpoint(&PID_r, 160);
    while(1)
    {
    printf("%d\r\n",(int)pwm_l);
    delay_ms(200);
    }
    }
    void CAR_GPIO_INIT() 
    {
    
    
      GPIO_InitTypeDef GPIO_InitStructure;
    
    RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOC|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOD, ENABLE); 
    
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
    GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_Out_PP; 
      GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
    GPIO_Init(GPIOB, &GPIO_InitStructure);           
    
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
    GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_Out_PP;  
      GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;  
    GPIO_Init(GPIOB, &GPIO_InitStructure);        
    
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
    GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_Out_PP;    
     GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz; 
    GPIO_Init(GPIOC, &GPIO_InitStructure);          
    
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
    GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_Out_PP;  
     GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz; 
    GPIO_Init(GPIOC, &GPIO_InitStructure);         
    
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
    GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_IPU;   
    GPIO_Init(GPIOD, &GPIO_InitStructure);          
    
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
    GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_IPU;  
    
    GPIO_Init(GPIOD, &GPIO_InitStructure);     
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
    GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_IPU;   
    GPIO_Init(GPIOD, &GPIO_InitStructure);          
    
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
    GPIO_InitStructure.GPIO_Mode =  GPIO_Mode_IPU;  
    GPIO_Init(GPIOD, &GPIO_InitStructure);   
    }

     

    展开全文
  • pid控制算法,pid算法介绍,从熟悉到精通pid,pid控制实际应用例程
  • 智能车采用PID控制算法,使用CCD线型摄像头作为黑色引导线的检测设备,经LM393比较后供单片机进行数据采集,图像识别,从而可以进行路径识别。电机驱动采用的是PC33886,使用直射型光电传感器来测量速度,并将相关...
  • 智能PID算法资料包

    2018-05-17 17:35:15
    最全的PID调节资料,里面包含了PID算法,调节技巧,源代码,以及上位机
  • PID常用口诀: 参数整定找最佳,从小到大顺序查,先是比例后积分,最后再把微分加,曲线振荡很频繁,比例度盘要放大,曲线漂浮绕大湾,比例度盘往小扳,曲线偏离回复慢,积分时间往下降,曲线波动周期长,积分时间再...
  • PID-小车类-PID算法控制小车直线行驶(制作步骤+程序+PID库).zip 里面包括了详细的制作步骤以及程序+PID库
  • 算法(一):智能小车速度控制(PID模糊控制)

    万次阅读 多人点赞 2020-03-22 10:42:36
    在控制领域,PID算法是应用最广泛的算法之一。小到电机的转速,大到机器人稳定性的控制。在实现智能小车速度的闭环控制时首选简单有效的PID控制算法。 二、基于PID 的速度控制 1.PID控制器 PID(Proportion I...

    一、前言

    本科的时候参加飞思卡尔比赛,在队友的神助攻下有幸拿了国奖,比赛中使用了模糊控制算法。这段时间正好看到了模糊控制算法,就把以前的代码翻出来,顺便总结一下PID和模糊控制算法,希望自己能养成记录知识点的好习惯。

    二、基于PID 的速度控制

    在控制领域,PID算法是应用最广泛的算法之一。小到电机的转速,大到机器人稳定性的控制。在实现智能小车速度的闭环控制时首选简单有效的PID控制算法。

    1.PID控制器

    PID(Proportion Integration Differentiation)控制器包括P比例、I积分和D微分三个控制单元,协同工作保证控制系统快速到达并稳定于目标值。PID控制算法的公式:
    u(t)=Kperr(t)+Kierr(t)dt+Kdderr(t)dtu(t)=K_perr(t)+K_i\int err(t)dt +K_d\frac{derr(t)}{dt}
    对其进行离散化:
    u(n)=Kperr(n)+Kik=0nerr(k)+Kd(err(n)err(n1))u(n)=K_perr(n)+K_i\sum_{k=0}^n err(k) +K_d(err(n)-err(n-1))

    1)KpK_p比例控制
    控制器的反应速度与KpK_p有关,该系数越大,控制系统反应更灵敏。但是KpK_p过大会引起较大的超调,并产生震荡,破坏系统的稳定性。因此在调整KpK_p参数时应从低到高增加,选择系统达到响应快并稳定的效果时的参数。

    2)KiK_i积分控制
    只要控制系统存在误差,该系数对系统的作用就会不断增强。只有误差err消失即系统稳定在目标值时,控制作用才不会变化。由此可见积分控制的调节会消除系统的静态误差。

    3)KdK_d微分控制
    为了抑制系统的超调和振荡,在控制系统中增加微分控制。通过对控制系统未来的预估,及时对控制量进行调整,提升系统的稳定性。

    2.PID速度控制

    刚开始对小车的速度控制采用位置式PID控制算法,即常用的PID控制算法。但是位置式PID算法使用过去误差的累加值,容易产生较大的累计误差。而且由于小车的目标速度时刻在变化,err值需要不断的累加,可能出现err_sum溢出的情况。因此对位置式加以变换,得到增量式PID控制算法:
    u=u(n)u(n1)=Kp(err(n)err(n1))+Kierr(n)+Kd(err(n)2err(n1)+err(n2))\triangle u=u(n)-u(n-1)=K_p(err(n)-err(n-1))+K_ierr(n) +K_d(err(n)-2err(n-1)+err(n-2))
    由此计算出:
    u(n)=u(n1)+Kp(err(n)err(n1))+Kierr(n)+Kd(err(n)2err(n1)+err(n2)) u(n) = u(n-1) + K_p(err(n)-err(n-1))+K_ierr(n) +K_d(err(n)-2err(n-1)+err(n-2))

    int speed_control(void)
    {
    	int i;
    	speed_set=get_speed_set();//设置车速                  
    	
    	//设置PID参数
    	kp_motor=33;
    	ki_motor=0.038;
    	kd_motor=0.04;
    
    	for(i=0;i<9;i++)
    		error[i]=error[i+1];
    	error[9]=speed_set-speed;	
    
    	de=kp_motor*(error[9]-error[8])+ki_motor*error[9]-kd_motor*(speed_save[9]-2*speed_save[8]+speed_save[7]);
    	pwm1 = pwm1_old+de;
      	
      	speed_set_old=speed_set;
      	pwm1_old=pwm1; 
      	return pwm1;//输出PWM波
    }
    

    二、基于模糊控制的速度控制

    拥有响应迅速地速度控制系统对于提升小车的速度是不够的,还需要根据赛道情况设置不同的车速以实现小车最快地通过不同的路况。这时,可以考虑模糊控制的思想根据路况及小车当前的状态对目标车速进行设置。

    1.变量的模糊化

    在模糊控制中,输入输出变量大小用语言形式进行描述。常选用的7个语言值为{负大,负中,负小,零,正小,正中,正大},即{NB,NM,NS,O,PS,PM,PB}。

    对小车当前行驶方向和赛道方向形成的偏差e及其变化率ec作为模糊控制器的输入,小车的目标车速为模糊控制器的输出。设偏差值的模糊量为E,偏差变化率模糊量为EC,U为目标车速。为了让速度切换更加细腻流畅,设置偏差e、偏差变化ec和控制量u的基本论域为[-6,6],并划分为13个等级,即{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}。

    E、EC和U均使用三角形隶属函数进行模糊化

    三角形隶属函数

    
    /**
    * 列坐标:NB,NM,NS,O,PS,PM,PB
    * 横坐标:-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6
    * @param 建立输入、输出隶属度函数,进行微调调整模糊量的范围
    */
    /***************************************误差隶属度函数***************************************/
    float Input1_Terms_Membership[7][13] =
    { 1,0.15,0,0,0,0,0,0,0,0,0,0,0,
    0,0.2,1,0.2,0,0,0,0,0,0,0,0,0,
    0,0,0,0.2,1,0.2,0,0,0,0,0,0,0,
    0,0,0,0,0,0.1,1,0.1,0,0,0,0,0,
    0,0,0,0,0,0,0,0.1,1,0.1,0,0,0,
    0,0,0,0,0,0,0,0,0,0.2,1,0.2,0,
    0,0,0,0,0,0,0,0,0,0,0,0.2,1
    };
    /***************************************误差变化率隶属度函数***************************************/
    float Input2_Terms_Membership[7][13] =
    { 1,0.15,0,0,0,0,0,0,0,0,0,0,0,
    0,0.2,1,0.2,0,0,0,0,0,0,0,0,0,
    0,0,0,0.2,1,0.2,0,0,0,0,0,0,0,
    0,0,0,0,0,0.1,1,0.1,0,0,0,0,0,
    0,0,0,0,0,0,0,0.1,1,0.1,0,0,0,
    0,0,0,0,0,0,0,0,0,0.2,1,0.2,0,
    0,0,0,0,0,0,0,0,0,0,0,0.2,1
    };
    /***************************************输出(速度)***************************************/
    float Output_Terms_Membership[7][13] =
    { 1,0.15,0,0,0,0,0,0,0,0,0,0,0,
    0,0.2,1,0.2,0,0,0,0,0,0,0,0,0,
    0,0,0,0.2,1,0.2,0,0,0,0,0,0,0,
    0,0,0,0,0,0.1,1,0.1,0,0,0,0,0,
    0,0,0,0,0,0,0,0.1,1,0.1,0,0,0,
    0,0,0,0,0,0,0,0,0,0.2,1,0.2,0,
    0,0,0,0,0,0,0,0,0,0,0,0.2,1
    };
    

    2.模糊查询表的计算

    对模糊量EC、E和U设置相关的模糊控制规则表
    模糊控制规则表

    /**
    * 纵轴为E(error),横轴为EC(error_delta),值为速度七档NB(0),NM(1),NS(2),Z(3),PS(4),PM(5),PB(6)速度由小变大再变小
    * 列坐标:E(NB,NM,NS,O,PS,PM,PB)
    * 横坐标:EC(NB,NM,NS,O,PS,PM,PB)
    * 值:U(1:NB:2,NM,3:NS,4:O,5:PS,6:PM,7:PB)
    * @param 模糊控制规则表,调整速度变化趋势
    */
    int Rule[7][7] =
    { 1,1,2,2,6,7,7,
     1,1,2,2,6,6,6,
     1,2,3,4,5,6,6,
     1,3,4,4,4,5,7,
     2,2,3,4,5,6,7,
     2,2,2,2,6,7,7,
     1,1,2,2,6,7,7
    };//调试参数
    

    规则库蕴含的模糊关系:

    R=(E×EC)×CR=(E\times EC)\times C

    其中,模糊运算×\times表示“取小”。

    计算出模糊规则蕴含的模糊关系R后,通过遍历E和EC所有的论域对模糊值进行选取并计算模糊输出值:

    U=(E×EC)RU^*=(E^* \times EC^*)\circ R

    其中,\circ表示模糊矩阵的合成,类似于普通矩阵的乘积运算,将乘积运算换成“取小”,将加法运算换成“取大”。

    在遍历过程中,对E和EC所有论域对应的模糊输出值一一采取加权平均法去模糊化,得到最终的模糊控制器查询表。

    float  R[169][13] = { 0 };
    float R1[13][13] = { 0 };
    float AdBd1[13][13] = { 0 };
    float R2[169] = { 0 };
    float AdBd2[169] = { 0 };
    float R3[169][13] = { 0 };
    float  Cd[13] = { 0 };
    float Fuzzy_Table[13][13] = { 0 };
    float SPEED[13] = { 200,220,230,240,250,270,300,270,250,240,230,220,200 };//调试参数
    int Max_Input1_value = 0, Max_Input2_value = 0;
    
    /**
    * @param 模糊化过程实现论域内不同值对应隶属度最大的语言值
    */
    int  E_MAX(int e)
    {
    	int i = 0, max = 0;
    	for (i = 0; i < 7; i++)
    		if (Input1_Terms_Membership[i][e] > Input1_Terms_Membership[max][e])
    			max = i;
    	return max;
    }
    
    int  EC_MAX(int ex)
    {
    	int i = 0, max = 0;
    	for (i = 0; i < 7; i++)
    		if (Input2_Terms_Membership[i][ex] > Input1_Terms_Membership[max][ex])
    			max = i;
    	return max;
    }
    
    void calculate()
    {
    	/***************************************计算所有规则模糊关系的并集Rule***************************************/
    	int i = 0, j = 0, k = 0;
    	int Input1_value_index = 0, Input2_value_index = 0;
    
    	//计算Rule(初始化),计算Rij,并对所有的R取并集,R=(EXEC)XU
    	for (Input1_Terms_Index = 0; Input1_Terms_Index < 7; Input1_Terms_Index++)
    		for (Input2_Terms_Index = 0; Input2_Terms_Index < 7; Input2_Terms_Index++)
    		{
    			// E和EC的语言值两两组合及其输出计算Rule
    			Output_Terms_Index = Rule[Input1_Terms_Index][Input2_Terms_Index] - 1;
    			k = 0;
    			for (i = 0; i < 13; i++)
    				for (j = 0; j < 13; j++)
    				{
    					// E和EC进行取小运算
    					if (Input1_Terms_Membership[Input1_Terms_Index][i] < Input2_Terms_Membership[Input2_Terms_Index][j])
    						R1[i][j] = Input1_Terms_Membership[Input1_Terms_Index][i];
    					else
    						R1[i][j] = Input2_Terms_Membership[Input2_Terms_Index][j];
    					// 转换R1矩阵为R2一维向量
    					R2[k] = R1[i][j];
    					k++;
    				}
    			///<A=Input1_Terms_Membership[Input1_Terms_Index],B=Input2_Terms_Membership[Input2_Terms_Index]
    			///<R1=AXB建立13x13的矩阵,R2=R1'把矩阵转成169x1的列向量
    			for (i = 0; i < 169; i++)
    				for (j = 0; j < 13; j++)
    				{
    					// R1(E, EC)与U进行取小运算
    					if (R2[i] < Output_Terms_Membership[Output_Terms_Index][j])
    						R3[i][j] = R2[i];
    					else
    						R3[i][j] = Output_Terms_Membership[Output_Terms_Index][j];
    					// R进行取大运算,为所有规则模糊关系的并集
    					if (R3[i][j] > R[i][j])
    						R[i][j] = R3[i][j];
    				}
    		}
    
    
    	/*************************对于每种可能的E、EC的精确取值模糊化后进行推理得到模糊输出Cd,Cd=(AdxBd)oR*************************/
    	for (Input1_value_index = 0; Input1_value_index < 13; Input1_value_index++) {
    		for (Input2_value_index = 0; Input2_value_index < 13; Input2_value_index++)
    		{
    			for (j = 0; j < 13; j++)
    				Cd[j] = 0;
    			int kd = 0;
    			float temp = 0;
    			Max_Input1_value = E_MAX(Input1_value_index);	///<找出误差隶属度最大的语言值
    			Max_Input2_value = EC_MAX(Input2_value_index);	///<找出误差变化率隶属度最大的语言值
    			for (i = 0; i < 13; i++)
    				for (j = 0; j < 13; j++)
    				{
    					// E(Ad)和EC(Bd)进行取小运算
    					if (Input1_Terms_Membership[Max_Input1_value][i] < Input2_Terms_Membership[Max_Input2_value][j])
    						AdBd1[i][j] = Input1_Terms_Membership[Max_Input1_value][i];
    					else
    						AdBd1[i][j] = Input2_Terms_Membership[Max_Input2_value][j];
    					AdBd2[kd] = AdBd1[i][j];
    					kd++;
    				}
    			for (i = 0; i < 169; i++)
    				for (j = 0; j < 13; j++)
    				{
    					// 模糊矩阵的合成,将乘积运算换成“取小”,将加法运算换成“取大”
    					if (AdBd2[i] < R[i][j])
    						temp = AdBd2[i];
    					else
    						temp = R[i][j];
    					if (temp > Cd[j])
    						Cd[j] = temp;
    				}
    
    
    			/*************************去模糊化(加权平均法),计算实际输出*************************/
    			float sum1 = 0, sum2 = 0;
    			float OUT;
    			for (i = 0; i < 13; i++)
    			{
    				sum1 = sum1 + Cd[i];
    				sum2 = sum2 + Cd[i] * SPEED[i];
    			}
    			OUT = (int)(sum2 / sum1 + 0.5);///<四舍五入
    			Fuzzy_Table[Input1_value_index][Input2_value_index] = OUT;
    			cout << OUT << ",";
    		}
    		cout << endl;
    	}
    }
    

    3.模糊查询表设置车速

    将模糊查询表复制进入代码程序,将实际的e和ec映射到论域中后,在模糊查询表中查询结果并设置目标车速。

    int_16 Fuzzy_Table[13][13]= 
    { 
    203,211,211,211,226,226,230,230,228,210,210,210,210,
    209,221,221,221,238,238,241,241,237,231,231,231,227,
    209,221,221,221,238,238,241,241,237,231,231,231,227,
    209,221,221,221,238,238,241,241,237,231,231,231,227,
    215,238,238,238,245,245,266,266,246,237,237,237,232,
    215,238,238,238,245,245,266,266,246,237,237,237,232,
    218,250,250,250,276,276,283,283,280,245,245,245,216,
    218,250,250,250,276,276,283,283,280,245,245,245,216,
    232,240,240,240,250,250,271,271,246,236,236,236,217,
    226,230,230,230,236,236,239,239,236,214,214,214,208,
    226,230,230,230,236,236,239,239,236,214,214,214,208,
    226,230,230,230,236,236,239,239,236,214,214,214,208,
    211,211,211,211,226,226,230,230,228,208,208,208,203
    }  ;
    
    int_16 get_speed_set(void) {
    	int_16 E = 0, EC = 0;
    	int_16 speed_target;
    	static int_16 re_pos = 0, ek = 0, eck = 0;
    	float ke = 400, kec = 10;
    	ek = 2500 - row;
    	eck = 2500 - row - re_pos;
    	re_pos = ek;
    
    	if (ek > 0) {
    
    		E = (int_32)(ek / ke + 0.5);
    	}
    	else {
    
    		E = (int_32)(ek / ke - 0.5);
    	}
    	//将E的论域转换到模糊控制器的论域
    	if (E > 6)
    		E = 6;
    	else if (E < -6)
    		E = -6;
    	if (eck > 0) {
    
    		EC = (int_16)(eck / kec + 0.5);
    	}
    	else {
    
    		EC = (int_16)(eck / kec - 0.5);
    	}//将EC的论域转换到模糊控制器的论域
    	if (EC > 6)
    		EC = 6;
    	else if (EC < -6)
    		EC = -6;
    
    	speed_target = (int_16)(Fuzzy_Table[E + 6][EC + 6]);
    	return speed_target ;
    }
    

    三、总结

    本文首先对PID控制器进行了简单的阐述,并应用于车速控制中。然后引入模糊控制对目标车速进行设置,实现速度的平稳过渡。PID算法和模糊控制算法在保证行驶平稳性的前提下,最大幅度地提升小车的行驶速度。

    参考资料
    [1]详细讲解PID控制
    [2]PID控制算法原理(抛弃公式,从本质上真正理解PID控制)
    [3]初识PID-搞懂PID概念
    [4]模糊控制——基本原理
    [5]基于单目视觉的智能车速度模糊控制系统
    [6]模糊算法在智能车控制中的应用

    展开全文
  • 基于arduino的循迹小车(含有PID算法

    万次阅读 多人点赞 2018-08-11 20:39:36
    循迹小车一般分为两方面:一方面是简单的闭环赛道只有直道和弯道,另一方面是毕设类型的包括一些元素:90度弯道、十字道路、S形弯道等。 1、CSDN下载: 含有PID:...

           循迹小车一般分为两方面:一方面是简单的闭环赛道只有直道和弯道,另一方面是毕设类型的包括一些元素:90度弯道、十字道路、S形弯道等。

     

    1、CSDN下载:

    含有PID:https://download.csdn.net/download/qq_38351824/11107048

    没有PID:https://download.csdn.net/download/qq_38351824/11107175

    2、可以关注点赞并在下方评论,我给你邮箱发过去。

    3、关注微信公众号下载:

         ① 关注微信公众号:Tech云  

         ②

     

           本篇博客试根据下图来进行书写的,如果大家有什么新的元素,也可以在下方评论,我进行更新。

    作者:sumjess

     

    注意本篇博客循迹模块使用了5个

    一、简单的闭环赛道

    随意画了一个

    (1)逻辑部分:

       

    所以程序的写法也很简单,就是检测到哪种情况对应着哪种反应。这一过程可以用switch也可以用if来实现这一过程。下文用if来演示。

    (2)各程序片段

    总的循环:

    void loop()
    {
         read_sensor_values();  //获取5个循迹模块的数值情况
         calc_pid();            //pid计算出转向的pwm值
         motor_control();       //电机转动
    }

    第一部分:检测部分程序片段

    void read_sensor_values()
    {
      sensor[0] = digitalRead(leftA_track_PIN);
      sensor[1] = digitalRead(leftB_track_PIN);
      sensor[2] = digitalRead(middle_track_PIN);
      sensor[3] = digitalRead(righA_track_PIN);
      sensor[4] = digitalRead(righB_track_PIN);
      
      if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1)) {
          error = 2;//          0 0 0 0 1
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0)) {
          error = 1;//          0 0 0 1 0
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
          error = 0;//          0 0 1 0 0
        } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
          error = -1;//         0 1 0 0 0
        } else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
          error = -2;//         1 0 0 0 0
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
          if (error == -2) {//  0 0 0 0 0
            error = -3;
          }else{
            error = 3;
          }
        }
      }

    解释:

    <1>  将读取到的五个循迹模块的数据存入数组sensor:

      sensor[0] = digitalRead(leftA_track_PIN);      左边第一个循迹模块
      sensor[1] = digitalRead(leftB_track_PIN);      左边第二个循迹模块
      sensor[2] = digitalRead(middle_track_PIN);   中间的循迹模块
      sensor[3] = digitalRead(righA_track_PIN);     右边第二个循迹模块
      sensor[4] = digitalRead(righB_track_PIN);     右边第一个循迹模块

    <2>  检测到哪种情况对应着哪种反应:

    if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1)) {
          error = 2;//          0 0 0 0 1
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0)) {
          error = 1;//          0 0 0 1 0
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
          error = 0;//          0 0 1 0 0
        } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
          error = -1;//         0 1 0 0 0
        } else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
          error = -2;//         1 0 0 0 0
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
          if (error == -2) {//  0 0 0 0 0
            error = -3;
          }else{
            error = 3;
          }
        }

    前面几个就不解释了完全按照前面excel写的,最后一个是所有传感器均没有检测到黑线的情况,下面具体解释一下:

    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {

     检测到为0000的情况
          if (error == -2) {// //如果上一次 error == -2

    意味着上一次是1000,也就是意味着这次可能车在左面第一个传感器和第二个传感器之间或者是在左边第一个传感器的左边,无论是上面的哪种情况,车都需要大左转
            error = -3;
          }else{

    否则就是相反的情况,都需要大右转
            error = 3;
          }
        }

    第二部分:计算PID(计算转向大小)

    void calc_pid()
    {
      P = error;
      I = I + error;
      D = error - previous_error;
     
      PID_value = (Kp * P) + (Ki * I) + (Kd * D);
     
      previous_error = error;
    }

    利用上一部分得到的error计算车的偏离情况,车偏离赛道的情况从而来调整下一次给出的PWM进而快速转正车身。

    这一部分的kp、ki、kd需要不断地调试,从而得出最佳解。

     

    第三部分:电机转向

    //速度设定范围(-255,255)
    void motorsWrite(int speedL, int speedR)
    {
      if (speedR > 0) {
        analogWrite(leftA_PIN, speedR);
        analogWrite(leftB_PIN, 0);
      } else {
        analogWrite(leftA_PIN, 0);
        analogWrite(leftB_PIN, -speedR);
      }
     
      if (speedL > 0) {
        analogWrite(righA_PIN, speedL);
        analogWrite(righB_PIN, 0);
      } else {
        analogWrite(righA_PIN, 0);
        analogWrite(righB_PIN, -speedL);
      }
    }
    //速度设定范围(-100,100)
    void motorsWritePct(int speedLpct, int speedRpct) {
      //speedLpct, speedRpct ranges from -100 to 100
      motorsWrite(speedLpct * 2.55, speedRpct * 2.55);
    }
    void motor_control()
    {
      int left_motor_speed = initial_motor_speed - PID_value;
      int right_motor_speed = initial_motor_speed + PID_value;
      
      if(left_motor_speed < -255){
        left_motor_speed = -255;
      }
      if(left_motor_speed > 255){
        left_motor_speed = 255;
      }
      motorsWrite(left_motor_speed,right_motor_speed);
    } 

    第一个函数是在检查更改正负值,来保证PWM都是正的,即轮子不会倒转。

    第二个函数是在利用一次函数,将输入范围变成0-100

    第三个函数是在控制范围,ardunio的PWM范围是在正负255,此函数是在做一个限制·

     

    二、在上面的基础上添加元素

     

    (1)逻辑部分:

    (2)各程序片段

     

    除了read_sensor_values() 各程序片段与之前都一样,read_sensor_values() 只是添加了一部分,情况如下:

    void read_sensor_values()
    {
      sensor[0] = digitalRead(leftA_track_PIN);
      sensor[1] = digitalRead(leftB_track_PIN);
      sensor[2] = digitalRead(middle_track_PIN);
      sensor[3] = digitalRead(righA_track_PIN);
      sensor[4] = digitalRead(righB_track_PIN);
      
          if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1)) {
          decide = 1;//十字路口 1 1 1 1 1   直行
        } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) {
          decide = 1;//十字路口 0 1 1 1 0   直行
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1)) {
          decide = 2;//90度路口 0 0 1 1 1    右转90度
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) {
          decide = 2;//90度路口 0 0 1 1 0    右转90度 
        } else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
          decide = 3;//90度路口 1 1 1 0 0    左转90度 
        } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
          decide = 3;//90度路口 0 1 1 0 0    左转90度 
        } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
          decide = 3;//向上锐角 0 1 1 0 0    向上锐角 
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1)) {
          error = 2;//          0 0 0 0 1
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0)) {
          error = 1;//          0 0 0 1 0
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
          error = 0;//          0 0 1 0 0
        } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
          error = -1;//         0 1 0 0 0
        } else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
          error = -2;//         1 0 0 0 0
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
          if (error == -2) {//  0 0 0 0 0
            error = -3;
          }else{
            error = 3;
          }
        }
     
    }

    各个条件都是按照前面excel写的

    附全套程序

    //电机使用:5-6-9-10
    //循迹模块使用:2-3-7-8-11
    
    //-------------------------------------------------------------------//
    //*******************************************************************//
    ///
    //小车部分/
    ///
    
    //           电机设置             //
    #define leftA_PIN 5
    #define leftB_PIN 6
    #define righA_PIN 9
    #define righB_PIN 10
    
    float Kp = 10, Ki = 0.5, Kd = 0;                    //pid弯道参数参数 
    float error = 0, P = 0, I = 0, D = 0, PID_value = 0;//pid直道参数 
    float decide = 0;                                   //元素判断
    float previous_error = 0, previous_I = 0;           //误差值 
    int sensor[5] = {0, 0, 0, 0, 0};                    //
    5个传感器数值的数组 
    static int initial_motor_speed = 60;                //初始速度 
     
    void read_sensor_values(void);                      //读取初值 
    void calc_pid(void);                                //计算pid 
    void motor_control(void);                           //电机控制 
    
    void motor_pinint( );     //引脚初始化
    void _stop();             //停车
    ///
    //*******************************************************************//
    //-------------------------------------------------------------------//
    
    //-------------------------------------------------------------------//
    //*******************************************************************//
    ///
    循迹部分///
    ///
    
    //             循迹模块设置               //
    #define leftA_track_PIN 3
    #define leftB_track_PIN 4
    #define middle_track_PIN 12
    #define righA_track_PIN 7
    #define righB_track_PIN 13
    
    
    //----------------------------------算法部分-----------------------------
    ---//
    void obstacle( );      //避障小车算法
                           //循迹小车算法/*  read_sensor_values(); calc_pid(
    ); motor_control();  */     
    //--------------------------------------------------------------------------//
    
    void setup()
    {
      Serial.begin(9600); //串口波特率115200(PC端使用)
      track_pinint( );     //循迹引脚初始化
      motor_pinint();        //电机引脚初始化
    
    }
    void loop()
    {
        read_sensor_values();         //循迹小车
        calc_pid();
        motor_control();
    }
    
    /*循迹模块引脚初始化*/
    void track_pinint( )
    { 
      pinMode (leftA_track_PIN, INPUT); //设置引脚为输入引脚
      pinMode (leftB_track_PIN, INPUT); //设置引脚为输入引脚
      pinMode (middle_track_PIN, INPUT);//设置引脚为输入引脚
      pinMode (righA_track_PIN, INPUT); //设置引脚为输入引脚
      pinMode (righB_track_PIN, INPUT); //设置引脚为输入引脚
    }
    
    /*电机引脚初始化*/
    void motor_pinint( )
    {
      pinMode (leftA_PIN, OUTPUT); //设置引脚为输出引脚
      pinMode (leftB_PIN, OUTPUT); //设置引脚为输出引脚
      pinMode (righA_PIN, OUTPUT); //设置引脚为输出引脚
      pinMode (righB_PIN, OUTPUT); //设置引脚为输出引脚
      }
    
    /**************************************************
    stop子函数—停止子函数
    函数功能:控制车停止
    **************************************************/
    void _stop()
    {
      analogWrite(leftA_PIN,0);      
      analogWrite(leftB_PIN,0);         //左轮静止不动
      analogWrite(righA_PIN,0);      
      analogWrite(righB_PIN,0);         //右轮静止不动
    }
    //速度设定范围(-255,255)
    void motorsWrite(int speedL, int speedR)
    {
      if (speedR > 0) {
        analogWrite(leftA_PIN, speedR);
        analogWrite(leftB_PIN, 0);
      } else {
        analogWrite(leftA_PIN, 0);
        analogWrite(leftB_PIN, -speedR);
      }
     
      if (speedL > 0) {
        analogWrite(righA_PIN, speedL);
        analogWrite(righB_PIN, 0);
      } else {
        analogWrite(righA_PIN, 0);
        analogWrite(righB_PIN, -speedL);
      }
    }
    //速度设定范围(-100,100)
    void motorsWritePct(int speedLpct, int speedRpct) {
      //speedLpct, speedRpct ranges from -100 to 100
      motorsWrite(speedLpct * 2.55, speedRpct * 2.55);
    }
    void motorsStop() 
    {
      analogWrite(leftA_PIN,0);      
      analogWrite(leftB_PIN,0);         //左轮静止不动
      analogWrite(righA_PIN,0);      
      analogWrite(righB_PIN,0);         //右轮静止不动
    }
     
    void read_sensor_values()
    {
      sensor[0] = digitalRead(leftA_track_PIN);
      sensor[1] = digitalRead(leftB_track_PIN);
      sensor[2] = digitalRead(middle_track_PIN);
      sensor[3] = digitalRead(righA_track_PIN);
      sensor[4] = digitalRead(righB_track_PIN);
      
        if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3
    ] == 1) && (sensor[4] == 1)) {
          decide = 1;//十字路口 1 1 1 1 1   直行
        } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (
    sensor[3] == 1) && (sensor[4] == 0)) {
          decide = 1;//十字路口 0 1 1 1 0   直行
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (
    sensor[3] == 1) && (sensor[4] == 1)) {
          decide = 2;//90度路口 0 0 1 1 1    右转90度
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (
    sensor[3] == 1) && (sensor[4] == 0)) {
          decide = 2;//90度路口 0 0 1 1 0    右转90度 
        } else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (
    sensor[3] == 0) && (sensor[4] == 0)) {
          decide = 3;//90度路口 1 1 1 0 0    左转90度 
        } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (
    sensor[3] == 0) && (sensor[4] == 0)) {
          decide = 3;//90度路口 0 1 1 0 0    左转90度 
        } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (
    sensor[3] == 0) && (sensor[4] == 0)) {
          decide = 3;//向上锐角 0 1 1 0 0    向上锐角 
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (
    sensor[3] == 0) && (sensor[4] == 1)) {
          error = 2;//          0 0 0 0 1
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (
    sensor[3] == 1) && (sensor[4] == 0)) {
          error = 1;//          0 0 0 1 0
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (
    sensor[3] == 0) && (sensor[4] == 0)) {
          error = 0;//          0 0 1 0 0
        } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (
    sensor[3] == 0) && (sensor[4] == 0)) {
          error = -1;//         0 1 0 0 0
        } else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (
    sensor[3] == 0) && (sensor[4] == 0)) {
          error = -2;//         1 0 0 0 0
        } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (
    sensor[3] == 0) && (sensor[4] == 0)) {
          if (error == -2) {//  0 0 0 0 0
            error = -3;
          }else{
            error = 3;
          }
        }
    }
    void calc_pid()
    {
      P = error;
      I = I + error;
      D = error - previous_error;
     
      PID_value = (Kp * P) + (Ki * I) + (Kd * D);
     
      previous_error = error;
    }
    void motor_control()
    {
      int left_motor_speed = initial_motor_speed - PID_value;
      int right_motor_speed = initial_motor_speed + PID_value;
      
      if(left_motor_speed < -255){
        left_motor_speed = -255;
      }
      if(left_motor_speed > 255){
        left_motor_speed = 255;
      }
      motorsWrite(left_motor_speed,right_motor_speed);
     
      Serial.print("move_A: ");
      Serial.print(left_motor_speed, OCT);
      Serial.print(" move_B: ");
      Serial.print(right_motor_speed, OCT);
      Serial.print(" error: ");
      Serial.print(error, OCT);
      Serial.print(" P: ");
      Serial.print(Kp, OCT);
      Serial.print(" PID_value: ");
      Serial.print(PID_value, OCT);
      Serial.println();
    } 
    
    

    附加:不含PID的代码

    #define leftA_PIN 5
    #define leftB_PIN 6
    #define righA_PIN 9
    #define righB_PIN 10
    
    #define leftA_track_PIN 3
    #define leftB_track_PIN 4
    #define middle_track_PIN 12
    #define righA_track_PIN 7
    #define righB_track_PIN 13
    
    void motor_pinint( );                               //引脚初始化
    void _stop();                                       //停车
    void forward();  
    void turnSRight();                                 //小右转
    void turnSLeft();                                  //小左转
    void turnRight();                                  //右转
    void turnLeft();                                   //左转
    void nine(); 
    void ten() ;
    int error = 0;
    int sensor[5] = {0, 0, 0, 0, 0};                    //5个传感器数值的数组 
    int read_sensor_values(void);                      //读取初值 
    
    void setup() {
      Serial.begin(9600); //串口波特率9600(PC端使用)
      track_pinint( );    //循迹引脚初始化
      motor_pinint();     //电机引脚初始化
    }
    
    void loop() {
      static int b=0;
      b=read_sensor_values();         //循迹小车
      switch (b){              //读取初值  
         case 0:  forward(); Serial.println(1); break; //直行
         case -1: turnSRight();Serial.println(2); break; //小左
         case -2: turnRight();Serial.println(3);break; //大左 
         case 1:  turnSLeft(); Serial.println(4); break; //小右
         case 2:  turnLeft();Serial.println(5); break; //大右    
         case 3:  _stop();Serial.println(6); break; //停
    //     case -3: turnLeft(); Serial.println(7);break; //大左
    //     case 4:  forward();  Serial.println(9);break; //直行   
         case 5:   nine() ;  Serial.println(9);break; //右转90度
         case 6:    ten();   Serial.println(9);break; 
         default: _stop();Serial.println(8);break; 
      }
    
    }
    
    /*循迹模块引脚初始化*/
    void track_pinint( )
    { 
      pinMode (leftA_track_PIN, INPUT); //设置引脚为输入引脚
      pinMode (leftB_track_PIN, INPUT); //设置引脚为输入引脚
      pinMode (middle_track_PIN, INPUT);//设置引脚为输入引脚
      pinMode (righA_track_PIN, INPUT); //设置引脚为输入引脚
      pinMode (righB_track_PIN, INPUT); //设置引脚为输入引脚
    }
    
    /*电机引脚初始化*/
    void motor_pinint( )
    {
      pinMode (leftA_PIN, OUTPUT); //设置引脚为输出引脚
      pinMode (leftB_PIN, OUTPUT); //设置引脚为输出引脚
      pinMode (righA_PIN, OUTPUT); //设置引脚为输出引脚
      pinMode (righB_PIN, OUTPUT); //设置引脚为输出引脚
      }
    
    /**************************************************
    stop子函数—停止子函数
    函数功能:控制车停止
    **************************************************/
    void _stop()
    {
       analogWrite(leftA_PIN,0);      
      analogWrite(leftB_PIN,0);         //左轮静止不动
      analogWrite(righA_PIN,0);      
      analogWrite(righB_PIN,0);         //右轮静止不动
    }
    int read_sensor_values()
    {
      sensor[0] = digitalRead(leftA_track_PIN);
      sensor[1] = digitalRead(leftB_track_PIN);
      sensor[2] = digitalRead(middle_track_PIN);
      sensor[3] = digitalRead(righA_track_PIN);
      sensor[4] = digitalRead(righB_track_PIN);
      //1为遇到黑线
    //   if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1)) {
    //      error = 2;//          00001
    //    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0)) {
    //      error = 1;//          00010
    //    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
    //      error = 0;//          00100
    //    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
    //      error = -1;//         01000
    //    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {
    //      error = -1;//         01100
    //    }  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) {
    //      error = 1;//          00110
    //    } else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
    //      error = -2;//         10000
    //    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {
    //      if (error == -2) {//  00000
    //        error = 4;//-3;
    //      }else{
    //        error = 4;//3;
    //      }
    //    }
    //    else error = -error;  
     //000-001-010-011-100-101-110-111
               if ((sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)) {
          error = 0;//          000  停
        } else if ((sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1)) {
          error = -2;//          001  右转
        } else if ((sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0)) {
          error = 0;//          010  直行
        } else if ((sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1)) {
          error = -1;//         011  右转
        } else if ((sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0)) {
          error = 2;//         100  左转
        }  else if ((sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0)) {
          error = 1;//         110  左转
        } else if ((sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1)) {
          error = 0;//          111  直行
        } /*else if ((sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 1)) {
          error = 1;//          101
        }*/
        if (sensor[4] == 1)  error=5;
        if (sensor[0] == 1)  error=6;
        for(int i=1;i<4;i++)
        Serial.print(sensor[i]);
        Serial.println( );
        return error;
    }
    /********************** ****************************
    forward子函数——前进子函数
    函数功能:控制车前进
    **************************************************/
     void forward()
    {
      analogWrite(leftA_PIN,60);      
      analogWrite(leftB_PIN,0);         //左轮前进60
      analogWrite(righA_PIN,60);      
      analogWrite(righB_PIN,0);         //右轮前进
    }
    /**************************************************
    turnLeft子函数——大左转子函数
    函数功能:控制车大左转
    **************************************************/
    void turnLeft() 
    {
      analogWrite(leftA_PIN,40);      
      analogWrite(leftB_PIN,0);         //左轮前进20
      analogWrite(righA_PIN,140);      
      analogWrite(righB_PIN,0);         //右轮前进70
    }
    /**************************************************
    turnRight子函数——小右转子函数
    函数功能:控制车大右转弯
    **************************************************/
    void turnRight()
    {
      analogWrite(leftA_PIN,140);      
      analogWrite(leftB_PIN,0);         //左轮前进
      analogWrite(righA_PIN,40);      
      analogWrite(righB_PIN,0);         //右轮前进
    }
    /**************************************************
    turnLeft子函数——大左转子函数
    函数功能:控制车小左转
    **************************************************/
    void turnSLeft()
    {
      analogWrite(leftA_PIN,60);      
      analogWrite(leftB_PIN,0);         //左轮前进20
      analogWrite(righA_PIN,110);      
      analogWrite(righB_PIN,0);         //右轮前进30
    }
    /**************************************************
    turnRight子函数——小右转子函数
    函数功能:控制车小右转弯
    **************************************************/
    void turnSRight()
    {
      analogWrite(leftA_PIN,110);      
      analogWrite(leftB_PIN,0);         //左轮前进
      analogWrite(righA_PIN,60);      
      analogWrite(righB_PIN,0);         //右轮前进
    }
    void nine() 
    {
      analogWrite(leftA_PIN,140);      
      analogWrite(leftB_PIN,0);         //左轮前进20
      analogWrite(righA_PIN,0);      
      analogWrite(righB_PIN,0);         //右轮前进70
    }
    void ten() 
    {
      analogWrite(leftA_PIN,0);      
      analogWrite(leftB_PIN,0);         //左轮前进20
      analogWrite(righA_PIN,140);      
      analogWrite(righB_PIN,0);         //右轮前进70
    }

     

    展开全文
  • 平衡小车PID算法

    万次阅读 多人点赞 2018-12-02 20:00:33
    一、直立控制(PD算法) int balance(float Angle,float Gyro)//角度,角速度 {  float Bias,kp=300,kd=1;  int balance;  Bias=Angle-ZHONGZHI; //求出平衡的速度中值  balance=kp*Bias+Gyro*kd; //计算平衡...
  • 视频链接:https://www.bilibili.com/video/BV1db4y1Q7qM/ 本设计是有AT89C51为主控芯片,主要实现了智能小车PID速度匀速前进 按键功能为设置目标速度值。
  • 跷跷板小车程序PID算法示例

    热门讨论 2011-07-30 10:11:35
    跷跷板程序,学习PId算法的经典例子,希望对大家有帮助;
  • 为实现小车的平稳运行,采用在AGV小车的前后两端各安装一个循迹传感器的方法,主控PLC通过查询循迹传感器的输出状态得出小车的偏移情况,并通过位置和速度的双闭环PID算法来调整AGV小车的左右驱动轮的速度差,从而...
  • 简介今天来分享一下我是如何用最短的时间进行智能小车PID调速的。在疫情期间比较无聊,在某宝买了一个智能小车底盘和一堆零件,基于Arduino Due和树莓派进行开发,Due负责底层控制,树莓派进行上层控制器开发,...
  • 根据小车的位置通过PID算法调节小车的速度,对主控中高级定时器进行设计,通过UART显示数据;观测小车运行过程中能否消除惯性带来的影响并立刻转向,及PID算法是否可以应用在喷绘机小车的控制系统中。
  • 基于PID算法的循迹小车

    万次阅读 多人点赞 2020-03-11 18:52:23
    这一期为创客们带来基于PID算法的循迹小车制作 1.标准赛道示意图: (该赛道包含了:左直角、右直角、十字路口等路况) 2.灰度传感器安装示意图: (可根据实际情况调节各传感器之间的间距) 3.硬件原理图: 4....
  • MSP432E401Y单片机智能小车PID调速代码

    千次阅读 2018-06-19 17:12:05
    * Description:PID处理函数 * 引脚: * * Author: Robin.J ***************************************************************************/ #include &lt;PID.h&gt; #include "ti/devices/msp43....
  • 智能PID

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 1,334
精华内容 533
关键字:

智能小车pid算法