devbox
小丑西瓜9
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

全国大学生智能汽车竞赛_智能车实际期望速度利用偏差速度因子

作者：小丑西瓜9 | 2024-04-09 21:56:56

赞

踩

智能车实际期望速度利用偏差速度因子

智能汽车控制算法

` 提示：本博客主要讲解全国大学生“恩智浦”智能汽车竞赛专用
本博客主要讲解直立智能汽车上的控制方面的软件代码的实现。

文章目录

前言

该智能小车主要是一辆电磁直立智能小车，实现了高速、自动巡线操作。该电磁直立智能小车有机械部分、硬件部分以及软件部分组成。我主要负责软件部分，对硬件部分也有一定的了解。硬件部分主要由降压电路、滤波电路、主控电路以及电机驱动电路4个模块组成。软件部分主要由电感数据处理模块、小车直立模块、小车速度模块，小车方向模块以及总输出5个模块组成。

1、电感数据处理模块

将电感采集的数据依次运用滤波算法、差比和算法等相关技术，得到直立智能小车在方向上的偏差值。

  • 滤波算法代码
	for(i=0;i<5;i++)    //将每个电感值保存在数组中
		{
			 ad_value[0][i] =ADC_Get1(ADC,ADC_CHANNEL_AD14);    //PTF4   LH  ADC4
			 ad_value[1][i] =ADC_Get1(ADC,ADC_CHANNEL_AD13);    //PTF7   RH  ADC3
			 ad_value[2][i] =ADC_Get1(ADC,ADC_CHANNEL_AD12);    //PTF5   ZH  ADC2
			 ad_value[3][i] =ADC_Get1(ADC,ADC_CHANNEL_AD7);     //PTF7   LB  ADC1
			 ad_value[4][i] =ADC_Get1(ADC,ADC_CHANNEL_AD6);     //PTF7   RB  ADC0
		}
		 for(k=0;k<6;k++)  //对每个电感进行快速排序
		{ 
				quickSort(ad_value,0,4);
		}
		 for(i=0;i<6;i++)  //去掉每个电感中的最大值与最小值，然后在将其他值进行平均
		{
				sum[i]=ad_value[i][1]+ad_value[i][2]+ad_value[i][3];
				AD[i]=(int)(sum[i]/3);
		}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 电感中的差比和运算
     Delta02=g_A0-g_A2;  //其中g_A0、g_A2是两个电感值
	 Add02=g_A0+g_A2;
	 if(Delta02==0)       Delta02=1;  //进行限幅
	 if(Add02==0)       Add02=1;
   offset_w1_02=Absolute(10.0*(Delta02/Add02));  //这就是差比和运算，作为智能小车的方向偏差

  • 1
  • 2
  • 3
  • 4
  • 5

随便还是说下本数据处理代码不只是这些，如果涉及环岛和其它元素，大家可以加上八字电感与横电感进行配合处理，一起使用差比和运算。对于坡道大家可以试下竖电感，其具体算法就先不说了

2、PID算法介绍

比例-积分-微分(Proportion-Integral-Differential，简称PID)控制器在自动控制领域拥有悠久历史。它根据给定值与实际输出值构成控制偏差，将偏差的比例§、积分(I)和微分(D)通过线性组合构成控制量，对被控对象进行控制。

PID算法的三个部分作用分别是：
比例控制：就是对偏差进行控制，偏差一旦产生，控制器立即就发生作用即调节控制输出，使被控量朝着减小偏差的方向变化。偏差减小的速度取决于比例系数Kp，Kp越大偏差减小的越快，但是很容易引起振荡，尤其是在迟滞环节比较大的情况下，Kp减小，发生振荡的可能性减小但是调节速度变慢。但单纯的比例控制不能消除静差，这里就需要积分控制。

积分控制：实质上就是对偏差累积进行控制，直至偏差为零。积分控制作用始终施加指向给定值的作用力，有利于消除静差，其效果不仅与偏差大小有关，而且还与偏差持续的时间有关。简单来说就是把偏差积累起来一起运算。

微分控制：它能敏感出误差的变化趋势，可在误差信号出现之前就起到修正误差的作用，有利于提高输出响应的快速性，减小被控量的超调和增加系统的稳定性。但微分作用很容易放大高频噪声，降低系统的信噪比，从而使系统抑制干扰的能力下降。因此，在实际应用中，应慎用微分控制。

在这里插入图片描述
数字PID控制算法通常分为位置式PID控制算法和增量式PID控制算法.
位置式PID算法:
u ( k ) = K p { e ( k ) + T T i ∑ 0 k e ( j ) + T d T [ e ( k ) − e ( k − 1 ) ] } u(k)=K p\left\{e(k)+\frac{T}{T i} \sum_{0}^{\mathrm{k}} e(j)+\frac{T d}{T}[e(k)-e(k-1)]\right\} u(k)=Kp{e(k)+TiT​∑0k​e(j)+TTd​[e(k)−e(k−1)]}
其源代码如下所示：

	int Wanted_val,	// 当前误差
	int return_val;   //实际输出	
	Error = Wanted_val- return_val;	// 计算当前误差
	i_Error += KI * Error;	// 误差积分
    i_Error = i_Error >max?max:i_Error;//积分限幅
	i_Error = i_Error <min?min:i_Error;
	Realize = KP * Error        //比例P
            + i_Error   //积分I
			+ KD * (Error - Last_Error);  //微分D
	Last_Error=Error;
	return Realize ;

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11

增量式PID算法:
u = u ( k ) − u ( k − 1 ) = K P [ e ( k ) − e ( k − 1 ) ] + K I e ( k ) + K D [ e ( k ) − 2 e ( k − 1 ) + e ( k − 2 ) ]

u=u(k)−u(k−1)=KP[e(k)−e(k−1)]+KIe(k)+KD[e(k)−2e(k−1)+e(k−2)]
u​=u(k)−u(k−1)=KP​[e(k)−e(k−1)]+KI​e(k)+KD​[e(k)−2e(k−1)+e(k−2)]​
其源代码如下所示：

	int Wanted_val,	// 当前误差
	int return_val;   //实际输出	
	Error = Wanted_val- return_val;	// 计算当前误差
	Realize = KP * (Error-Last_Error)       //比例P
            + KI*Error;//积分I
			+ KD * (Error - 2*Last_Error+pre_Error);  //微分D
	pre_Error=Last_Error;
	Last_Error=Error;
	return Realize ;

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

总结：
其实这两中算法各有各的优点与缺点：位置式PID可以利用积分累加的形式，消除静态误差和是数据很稳定，不会发生很大的抖动和跳跃；但是由于积分累加与前面的误差都有关联会导致前面出现不正当的操作，引入到积分累加中，出现错误，还有就是由于积分累加会导致积分饱和，需要进行相关的控制积分饱和的问题出现，例如：限幅值，积分分离，抗积分饱和等手段进行抑制积分饱和现象。增量式PID是这次的位置式PID与上次的位置式PID进行相减，这样就减掉了前面的误差累积，就没有了相应的积分累积现象；它的缺点就是由于没有积分累加，会有相应的静态误差。这就是各有各的优势，各取所需。

3、小车直立模块

直立模块是直立智能小车的精髓所在，无论是四旋翼还是四轮智能车都可以适当的借鉴。下面将从如何得到以下几个方面进行讲解：

  • 俯仰角度的计算
    我们使用了加速度和角速度进行互补滤波融合，实现角度的计算。我们使用的是模拟陀螺仪（通过使用ADC测量引脚的电压得到加速度和角速度值），不是数字陀螺仪（通过通信协议进行得到）。

  • 加速度和角速度的知识
    通过ADC进行采集相应接口的电压，得到加速度值，该加速度是竖直方向上的加速度，与重力加速度有联系的。当此时采集的加速度和重力加速度重合时，加速度值最大，是4096。当小车偏移是则得到的加速度值减小，加速度的值是持续不变的，和角速度值不一样，角速度是小车动才有相应的值，因此加速度值可以通过相应的转换成为角度值。那为什么要与加速度进行融合呢？这是由于加速度产生的是高频信号，波动与抖动性很大，直接作为角度值则会使小车有很大的抖动。角速度产生的是低频信号，进行积分运算可以得到角度，得到的角度很稳定，不会像加速度一样出现抖动，但是由于积分的作用会随着时间的推移测量的角度不精准，会出现漂逸现象，导致计算出的角度不准确。因此，要进行互补滤波，利用双方的优势，得到角度值。利用加速度的测量精准的优势，利用角速度测量平滑不抖动的优势，用加速度来弥补角速度测量的角度的偏差。
    在这里插入图片描述

  • 下面将用代码讲解下互补滤波融合

	 acc_z = ADC_Get1(ADC,ADC_CHANNEL_AD3);  //通过ADC进行采集相应接口的电压，得到加速度值
	
	 error_angle = ( Acc_Offset- acc_z) /1000.0; //其中Acc_Offset刚开电采集的零漂值，也就是加速的起始值。来计算出小车相对于临界点偏移了多少度
	 
	 error_angle = LIMIT (error_angle,-1,1);//这是限幅
	 error_angle = asin(error_angle)*(-57.3); //这是将弧度值转化为角度值
	
	 gyro_x = ADC_Get1(ADC,ADC_CHANNEL_AD4);   //采集角速度值						 
	 Gyro_x = ( X_Gyro_Offset- gyro_x)*(0.00123);  //它也有相应的零漂值,其中的0.00123是时间，就是 θ=g*t ,就是此时t时间内偏移的角度值
	       
	 Angle_error = error_angle - Angle;  //这是来计算出现在用加速度得到的角度值与上次融合的实际角度的差值。防止出现漂逸现象
	 Angle_F += 0.15*(Angle_error - Angle_F);  //这是进行加权滤波行为，将这次的差值与上次的差值进行加权滤波，这样可以是得到的值更加的平滑。不会出现抖动
	 Angle = 0.0075 * Angle_F+ (Angle+ Gyro_x );//在上次的角度值加上上面计算出来的Angle_F差值，再加上上面利用角速度得到的角度值，进行融合，通过得到角度值。其中Angle_F是为了出现漂移现象，Gyro_x为了防止加速度产生的角度值出现抖动，它可以抑制抖动现象。

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 直立环控制算法
    直立环是整个部分最重要的环节，主要是利用了上面得到的俯仰角和相应的角速度值实现双闭环PID算法。其中角速度作为内环，角度作为外环。这是因为内环中的角速度的灵敏度高于外环角度，这样当小车直立的时候，上下偏移的时候，小车中的角速度将最先感应到，就会及时向相反的方向进行抑制。一般设置内环与外环是有讲究的，一般内环的控制频率比外环的控制频率高，其内环灵敏度最高。以下是直立环中的串行PID控制算法!
    在这里插入图片描述
    其中角速度环相当于是一个阻尼力，没有这个角速度环就没有这个阻力，小车会出现来回轻微的上下摆动和抖动现象。

4、小车速度模块

由于小车直立环调的很好了之后，就要开始调节小车的速度环，小车的速度环我们选用位置式PID，单环PID，这是有以下原因，由于小车比赛的时候，起步速度要快，这样就是利用位置式pid中积分累积，来实现快速的偏差累积，以达到期望的速度。
由于积分累积的作用，我们采用了抗积分饱和和限制幅值，来抑制积分漂移。
代码如下所示：

	Delta_Speed=Wanted_Speed - Return_speed;  //是期望值与返回值的误差
	/********************抗积分饱和***********************/
	//主要是用于在位置实PID中积分漂移现象
  if(Delta_Speed_Integral>=speed_max)  
    {  
      if((Delta_Speed<0)&&Absolute(Delta_Speed)<300)         
        { 
          Delta_Speed_Integral+= Delta_Speed;             
        }
		}				
   else if(Delta_Speed_Integral<=speed_min)
   {  
		  if((Delta_Speed>0)&&(Absolute(Delta_Speed))<300)
			  {
				  Delta_Speed_Integral+=Delta_Speed;
			  }
	 }
	 else
	 {
		      Delta_Speed_Integral+=Delta_Speed;
	 }

if(Delta_Speed_Integral>speed_max)   //幅值限制
			Delta_Speed_Integral=speed_max;
    if(Delta_Speed_Integral<speed_min)
			Delta_Speed_Integral=speed_min;
		
		Speed_POut = Delta_Speed_P;
		Speed_IOut = indexd*I_Speed*Delta_Speed_Integral;
		Speed_DOut = D_Speed * (Delta_Speed - Delta_Speed_old);
		
		Delta_Speed_old = Delta_Speed;    
		
		Speed_Out_Vaule_Old = Speed_Out_Vaule_New ;
		Speed_Out_Vaule_New =(0.2*Wanted_Speed)+Speed_POut+Speed_IOut+Speed_DOut;     
		Speed_Out_Vaule_New=(int)(0.8*Speed_Out_Vaule_New+0.15*Speed_Out_Vaule_New_1+0.03*Speed_Out_Vaule_New_2+0.02*Speed_Out_Vaule_New_3);  //主要是最后的滤波功能，加权滤波
		Speed_Out_Vaule_New_3=Speed_Out_Vaule_New_2;
		Speed_Out_Vaule_New_2=Speed_Out_Vaule_New_1;

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38

这个可以实现小车的快速启动，达到竞速的效果。其实直立环和速度环是有关联的，大家可以好好思考下。

5、小车方向模块

智能小车方向控制模块主要运用了角速度环、差速度环、偏差控制环实现三闭环控制系统，实现方向上的闭环。将偏差控制环中输出作为差速度环的期望输入，其差速度环中的输出作为角度环的输入，最后直接输出。
其代码如下所示：

void Direction_Control()
{
	 GYRO_Y=ADC_Get1(ADC,ADC_CHANNEL_AD5);
	 gyro_y=GYRO_Y-Y_Gyro_Offset;
	/**************外环***************************/
	 sum1=30*(0-final_offset); 
	 Position_true_old=Position_true; 
     Position_true=sum1;
	/************转向PID控制************************/
     want_y_ang=P_p*Position_true+D_p*(Position_true-Position_true_old);
	/*******************差速环**********************************/

	 differ_err_old=differ_err;
	 differ_err=(want_y_ang-differ_speed);
//	 fuzzy_differ();					//差速环 模糊控制&&Jolt_Flag==0
	 differ_out=(differ_P*differ_err+differ_D*(differ_err-differ_err_old));
	/*******************陀螺仪内环******************************/
	 Delta_y_ang_old=Delta_y_ang; 
     Delta_y_ang=differ_out-gyro_y;  

     Car_Direction_out_old=Car_Direction_out;
     Car_Direction_out=(kp_Dir*Delta_y_ang+kd_Dir*(Delta_y_ang-Delta_y_ang_old));
	 Direction_out_filter[2]=Direction_out_filter[1];
     Direction_out_filter[1]=Direction_out_filter[0];
     Direction_out_filter[0]=Car_Direction_out; 
     Car_Direction_out=0.95*Direction_out_filter[0]+0.03*Direction_out_filter[1]+0.02*Direction_out_filter[2];

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26

6、总输出

将上面的直立环的输出、速度环的输出、方向环的输出直接并行连接作为总输出，其这部分可以在进行一个电机闭环系统。

在这里插入图片描述

7、模糊PID控制算法

模糊PID我们主要是应用在小车的方向上的控制，主要输入是方向上的偏差和偏差率。主要是通过偏差和偏差率的变化来输出kp的值，来实现kp的动态变化。主要有kp规则表：主要是用来将输入的偏差和偏差率的下标值，输出kp模糊集合的下标值，然后在对应到Kp中模糊集合中的值，再和通过偏差隶属度和偏差率隶属度来输出Kp中模糊集合中的值的隶属度，然后进行加权平均。

#define FMAX 100      //该值是偏差与偏差率隶属函数的幅值，该值的大小不影响最终输出，有助于将float型转换为int型。
 /* 误差特征点，通过让智能车跑的过程中，让24l01无线模块将小车的最大最小的偏差值传回来进行特征点划分*/
int e_cval[4]={0, 500, 2500, 3000};   //其实该数组有7个数
/*误差变化率特征点，和误差特征点一样*/
int ec_cval[4]={0, 800, 1500, 1800};
/* kp 特征点 */
int kp_cval[7]={0,10,30,60,100,150,160};		//间隔10*n
int kp_rules[7][7]=    //这是kp规则表，其实就是通过偏差和偏差率得到的模糊集合下标值，从kp规则表中得到其kp在模糊集合中的下标值
//ec -3 -2 -1  0  1  2  3
//   NL NM  NS ZO PS PM PL      //  e
{   { 6, 5, 4, 3, 2, 1, 0,},    // -3 NL
    { 5, 4, 3, 2, 1, 0, 1,},    // -2 NM
    { 4, 3, 2, 1, 0, 1, 2,},    // -1 NS
    { 3, 2, 1, 0, 1, 2, 3,},    //  0 ZO
    { 2, 1, 0, 1, 2, 3, 4,},    //  1 PS
    { 1, 0, 1, 2, 3, 4, 5,},    //  2 PM
    { 0, 1, 2, 3, 4, 5, 6,}     //  3 PL
};
/**
*@Descrition:模糊推理运算
*@Args:
*     e: 误差
*     ec: 误差变化率
*     rules : 规则表  kp
*     u_cval : 输出变量论域特征点  kp
*@Return: 
*     u: 输出变量精确值
*/
int fuzzy_control(int e,int ec,int rules[7][7],int u_cval[]) 
{ 
	int  u; /*输出精确值*/
	int 	e_n;/*误差所属模糊集合的下标*/
	int ec_n; /*误差变化率所属模糊集合的下标*/
	int u_n[4]; 
	unsigned int edm[2]; /* 误差的隶属度*/
	unsigned int ecdm[2];/* 误差变化率的隶属度*/
	unsigned int udm[4];/*输出变量的隶属度*/
	long temp1,temp2; 

/*******************我是分界线SlB*********************************/
	/*求误差的所属模糊集合求出相应的隶属度*/
	if(e>-e_cval[3] && e<e_cval[3])
	{ 
		if(e<=-e_cval[2])  
		{
			e_n=-2;   //这是偏差在模糊集合中的下标，相当于三角函数的x轴的值
			edm[0]=FMAX*((float)(-e_cval[2]-e)/(e_cval[3]-e_cval[2]));//这是隶属度，也是三角函数的幅值，相当于三角函数的y值
		} 
		else if(e<=-e_cval[1])  
		{
			e_n=-1;
			edm[0]=FMAX*((float)(-e_cval[1]-e)/(e_cval[2]-e_cval[1]));
		} 
		else if(e<=e_cval[0])
		{
			e_n=0;
			edm[0]=FMAX*((float)(-e_cval[0]-e)/(e_cval[1]-e_cval[0]));
		} 
		else if(e<=e_cval[1])  
		{
			e_n=1;
			edm[0]=FMAX*((float)(e_cval[1]-e)/(e_cval[1]-e_cval[0]));
		} 
		else if(e<=e_cval[2]) 
		{
			e_n=2;
			edm[0]=FMAX*((float)(e_cval[2]-e)/(e_cval[2]-e_cval[1]));
		} 
		else if(e<=e_cval[3]) 
		{
			e_n=3;
			edm[0]=FMAX*((float)(e_cval[3]-e)/(e_cval[3]-e_cval[2]));
		}  
	} 
	else if(e<=-e_cval[3])  
	{
		e_n=-2; 
		edm[0]=FMAX;
	} 
	else if(e>=e_cval[3])  
	{
		e_n=3;
		edm[0]=FMAX;
	} 
	edm[1]=FMAX-edm[0];    //误差还可能归属于其他模糊集合里面的数，也就是相邻的模糊集合的数

/****************************我是分界线*******************************/
/*求误差变化率所属模糊集合及相应的隶属度*/
	if(ec>-ec_cval[3] && ec<ec_cval[3])
	{ 
		if(ec<=-ec_cval[2])  
		{
			ec_n=-2;  //偏差率在模糊集合中的小标，也是三角函数的x轴
			ecdm[0]=FMAX*((float)(-ec_cval[2]-ec)/(ec_cval[3]-ec_cval[2])); //隶属度，相当于三角函数幅值。也是其Y值
		} 
		else if(ec<=-ec_cval[1])  
		{
			ec_n=-1;
			ecdm[0]=FMAX*((float)(-ec_cval[1]-ec)/(ec_cval[2]-ec_cval[1]));
		} 
		else if(ec<=ec_cval[0])  
		{
			ec_n=0; 
			ecdm[0]=FMAX*((float)(-ec_cval[0]-ec)/(ec_cval[1]-ec_cval[0]));
		} 
		else if(ec<=ec_cval[1])  
		{
			ec_n=1;
			ecdm[0]=FMAX*((float)(ec_cval[1]-ec)/(ec_cval[1]-ec_cval[0]));
		} 
		else if(ec<=ec_cval[2])  
		{
			ec_n=2;
			ecdm[0]=FMAX*((float)(ec_cval[2]-ec)/(ec_cval[2]-ec_cval[1]));
		} 
		else if(ec<=ec_cval[3])  
		{
			ec_n=3; 
			ecdm[0]=FMAX*((float)(ec_cval[3]-ec)/(ec_cval[3]-ec_cval[2]));	
		}  
	} 
	else if(ec<=-ec_cval[3])  
	{
		ec_n=-2; 
		ecdm[0]=FMAX;
	} 
	else if(ec>=ec_cval[3]) 
	{
		ec_n=3;
		ecdm[0]=FMAX;
	} 
	ecdm[1]=FMAX-ecdm[0];    //误差变化率还可能归属于其他模糊集合里面的数
	
/* 根据所给规则表给出输出变量所属模糊集的下标*/
/*知道偏差和偏差率模糊集合的下标值，在kp规则表中找出周围相邻点的kp模糊集合的下标值*/
	u_n[0]=rules[e_n-1+3][ec_n-1+3]; 
	u_n[1]=rules[e_n+3][ec_n-1+3]; 
	u_n[2]=rules[e_n-1+3][ec_n+3];
	u_n[3]=rules[e_n+3][ec_n+3];


/*根据误差和误差变化率的隶属度求出输出变量对应的隶属度*/
//采用最小原则表示规则前件的隶属度（确信度）
	udm[0]=fuzzy_and(edm[0],ecdm[0]); //那返回最小值，与上面的u_n数组对应
	udm[1]=fuzzy_and(edm[1],ecdm[0]);
	udm[2]=fuzzy_and(edm[0],ecdm[1]);
	udm[3]=fuzzy_and(edm[1],ecdm[1]);

/*找出输出属于相同模糊集合下标志，求出隶属度最大的模糊子集，并将隶属度小的置为0*/
	//这样做有助于避免重复性  避免在输出属于相同模糊集合的下标值时，有不同的隶属度，这样做
	//使其更加简洁实用。
	if(u_n[0]==u_n[1])
	{
		fuzzy_max(&udm[0],&udm[1]); //将最小的隶属度置零
	} 
	if(u_n[0]==u_n[2])
	{
		fuzzy_max(&udm[0],&udm[2]);
	} 
	if(u_n[0]==u_n[3])
	{
		fuzzy_max(&udm[0],&udm[3]);
	} 
	if(u_n[1]==u_n[2])
	{
		fuzzy_max(&udm[1],&udm[2]);
	} 
	if(u_n[1]==u_n[3])
	{
		fuzzy_max(&udm[1],&udm[3]);
	} 
	if(u_n[2]==u_n[3])
	{
		fuzzy_max(&udm[2],&udm[3]);
	} 


/* 求出输出变量所属模糊集合的特征点*/
	if(u_n[0]>=0)  这是在kp模糊集合中找出其kP的特征点
		u_n[0]=u_cval[u_n[0]];
	else 
		u_n[0]=-u_cval[-u_n[0]]; 
	if(u_n[1]>=0)
		u_n[1]=u_cval[u_n[1]];
	else 
		u_n[1]=-u_cval[-u_n[1]]; 

	if(u_n[2]>=0)
		u_n[2]=u_cval[u_n[2]];
	else
		u_n[2]=-u_cval[-u_n[2]]; 

	if(u_n[3]>=0)
		u_n[3]=u_cval[u_n[3]];
	else 
		u_n[3]=-u_cval[-u_n[3]]; //通过上面的操作u_n从kp模糊集合中的下标值变成了其里面可能的特征值大小
/* fuck, 终于到算出输出值了，bady  come */
/* “中心-平均”法求输出变量的实际值*/   //重心法与其“中心-平均”法实质是一样的 
//这相当于加权平均的效果其中udm数组变成了加权平均的系数数组
	temp1=udm[0]*u_n[0]+udm[1]*u_n[1]+udm[2]*u_n[2]+udm[3]*u_n[3]; 
	temp2=udm[0]+udm[1]+udm[2]+udm[3]; 
	u=temp1/temp2;
	return u;  //此时就是模糊生成的Kp值
} 

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201
  • 202
  • 203
  • 204

8、后续计划

后续计划就是讲解下四旋翼无人机中的控制算法实现。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小丑西瓜9/article/detail/395095
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号