两轮平衡电动车原理简单叙述

一、计算倾斜角度与角速率

csharp 复制代码
void Get_Tiltangle( void )				//¼Ƌ㽇˙Š¼°ǣб½Ƕȍ
{
	float buf;
	static uint32_t time;
    buf = total_ADXL_GYRO / total_looptime;
    total_ADXL_GYRO -= buf;
	total_ADXL_GYRO *=0.918;

	if( TIMER_MS>time+100 )
	{
		time = TIMER_MS;
		if( CarStus.limit_run )
		{
			if( CarStus.limit_angle <100 )
				CarStus.limit_angle++;
				
		}
		else
			CarStus.limit_angle=0;
	}
    // ADXL part
    total_ADXL_GYRO += (float)(ACCEL_X()-(config.s.ACCEL_X_M-CarStus.limit_angle));
    
    // Gyro part
    buf = Average_Gyro/total_looptime;
    Average_Gyro -= buf;
   	Average_Gyro += (float)GYRO_X();

    buf = Average_Gyro/(total_looptime);
	buf =  buf - (float)GYRO_X();
    buf *= 3;
    Angle_Rate = buf;

    total_ADXL_GYRO += Angle_Rate;
    Tilt_ANGLE = total_ADXL_GYRO/(total_looptime/10);
}

total_ADXL_GYRO *=0.918;为一个高通滤波,叠加角度数值与角速率值,除采样次数得到当前倾斜角度

Angle_Rate,利用滑动平均值滤波,叠加采样AD值得到乘以系数

二、计算平衡值

csharp 复制代码
Balance_moment =  Angle_Rate*(long)(config.s.Dangle) + (long)(config.s.Pangle)*(Tilt_ANGLE) + Protect;

倾斜角度计算使用P参数,角速率计算使用D参数,全车调试主要针对P参数与D参数调整

csharp 复制代码
Drivespeed = Drive_sum /240  + Balance_moment / 225 + slope_add;

加入slope_add校正,计算电机需要保持平衡的PWM值

三、计算两轮电机的轮速差值

csharp 复制代码
if( EncoderData )
		{
			EncoderData = 0;
			
			if (SpeedDiff>50)         // ±<<ƷϠ²0Ҕɏ£¬½øА±£>>¤
				SpeedDiff=50;
			else if (SpeedDiff<-50)
				SpeedDiff=-50;

			if( SpeedDiff>5 )         // ±<<ƷϠ²ԉϲŽøА¾Àƫ
			{
				if( DiffAdjust<10 )      // Ӄ׮´󲵵ĵ绺ֵÀ´¾Àƫ
					DiffAdjust+=(float)SpeedDiff*0.08;   // ¾Àƫµ绺ֵÿ5ms׮С0.8£¬׮´󸍊				else
					DiffAdjust = 10;
			}
			else if( SpeedDiff<-5 )
			{
				if( DiffAdjust>-10 )
					DiffAdjust+=(float)SpeedDiff*0.08;
				else
					DiffAdjust = -10;
			}
		}

0.08为调整系数

四、计算转向摇杆数值

csharp 复制代码
if( turn<config.s.RCOKER_M-ROCKER_DEADBAND )	//ҡ¸˗󗪍
		{
			if( LeftFlag == 0 )
			{
				#if MODEL_BW
				Steeringsignal = -12;     //BW -12, ST -8
				#elif MODEL_ST
				Steeringsignal = -8;     //BW -12, ST -8
				#else
				Steeringsignal = -8;     //BW -12, ST -8
				#endif
				if( fabs(Drivespeed)<3 && fabs(CarStus.Speed)>360)
					Steeringsignal = 0;
			}
			if( RightFlag )
			{
				RightFlag = 0;
				Steeringsignal = 0;
			}
			#if FIRMWARE15
			if (TurnSpeed > (700/(fabs(CarStus.Speed)/500+5))*(-1) )
			#elif FIRMWARE10
			if (TurnSpeed > (420/(fabs(CarStus.Speed)/300+5))*(-1) )
			#else
			if (TurnSpeed > (700/(fabs(CarStus.Speed)/500+5))*(-1) )
			#endif
			{
				#if MODEL_BW
				Steeringsignal -= 0.25;    //BW 0.15, ST 0.1
				#elif MODEL_ST
				Steeringsignal -= 0.2;    //BW 0.15, ST 0.1
				#else
				Steeringsignal -= 0.2;    //BW 0.15, ST 0.1
				#endif
				if( fabs(Drivespeed)<1.1 )
					Steeringsignal -= 0.04;
			}
			/*#if FIRMWARE15
			else if (TurnSpeed < (800/(fabs(CarStus.Speed)/500+5))*(-1) )
			#elif FIRMWARE10
			else if (TurnSpeed < (500/(fabs(CarStus.Speed)/300+5))*(-1) )
			#else
			else if (TurnSpeed < (800/(fabs(CarStus.Speed)/500+5))*(-1) )
			#endif
			{
				#if MODEL_BW
				Steeringsignal += 0.25;  
				#elif MODEL_ST
				Steeringsignal += 0.2; 
				#else
				Steeringsignal += 0.2; 
				#endif
				if( fabs(Drivespeed)<1.1 )
					Steeringsignal += 0.04;
			}*/
			LeftFlag = 1;
		}
        else if( turn>config.s.RCOKER_M+ROCKER_DEADBAND )	//Ӓת
		{
			if( RightFlag == 0 )
			{
				#if MODEL_BW
				Steeringsignal = 12;       //BW 12, ST 8
				#elif MODEL_ST
				Steeringsignal = 8;       //BW 12, ST 8
				#else
				Steeringsignal = 8;       //BW 12, ST 8
				#endif
				if( fabs(Drivespeed)<5 && fabs(CarStus.Speed)>360)
					Steeringsignal = 0;
			}
			if( LeftFlag )
			{
				LeftFlag = 0;
				Steeringsignal = 0;
			}
			#if FIRMWARE15
			if (TurnSpeed < (700/(fabs(CarStus.Speed)/500+5)) )
			#elif FIRMWARE10
			if (TurnSpeed < (420/(fabs(CarStus.Speed)/300+5)) )
			#else 
			if (TurnSpeed < (700/(fabs(CarStus.Speed)/500+5)) )
			#endif
			{
				#if MODEL_BW
				Steeringsignal += 0.25; 
				#elif MODEL_ST
				Steeringsignal += 0.2; 
				#else
				Steeringsignal += 0.2;
				#endif
				if( fabs(Drivespeed)<1.1 )
					Steeringsignal += 0.04;
			}
/*			#if FIRMWARE15
			else if (TurnSpeed > (800/(fabs(CarStus.Speed)/500+5)) )
			#elif FIRMWARE10
			else if (TurnSpeed > (500/(fabs(CarStus.Speed)/300+5)) )
			#else 
			else if (TurnSpeed > (800/(fabs(CarStus.Speed)/500+5)) )
			#endif
			{
				#if MODEL_BW
				Steeringsignal -= 0.25;
				#elif MODEL_ST
				Steeringsignal -= 0.2;
				#else
				Steeringsignal -= 0.2;
				#endif
				if( fabs(Drivespeed)<1.1 )
					Steeringsignal -= 0.04;
			}*/
			RightFlag = 1;
		}
        else								//>>ָ´µ½֐µ㍊		{
            if (Steeringsignal > 0)
			{
				Steeringsignal -= 1;
				if( Steeringsignal<0.5 )
				{
					Steeringsignal = 0;
					RightFlag = 0;
				}
			}
			else if (Steeringsignal < 0)
			{
				Steeringsignal += 1;
				if( Steeringsignal>-0.5 )
				{
					Steeringsignal = 0;
					LeftFlag = 0;
				}
			}
		}
		//Ϟֆתϲµķù¶ȍ
		if( Steeringsignal>70 )
			Steeringsignal = 70;
		else if( Steeringsignal<-70 )
			Steeringsignal = -70;

Steeringsignal 通过编码器不同速度条件下计算的转向PWM值

五、计算实际输出电机PWM数值

csharp 复制代码
Drive_A = Drivespeed + Steeringsignal - DiffAdjust;
Drive_B = Drivespeed - Steeringsignal + DiffAdjust;

Drivespeed 为陀螺仪与加计计算出来电机驱动值

Steeringsignal 为摇杆计算出来电机驱动值

DiffAdjust 为两电机编码器差值补偿值

Drive_A 与 Drive_B 输出到电机实际PWM值

相关推荐
youcans_18 天前
【动手学电机驱动】 STM32-FOC(2)STM32 导入和创建项目
stm32·单片机·嵌入式硬件·mcu·电机
youcans_2 个月前
【永磁同步电机(PMSM)】 6. 矢量空间算法(SVPWM)
算法·matlab·电机·控制·pmsm
youcans_2 个月前
【永磁同步电机(PMSM)】 3. 基于Matlab 的仿真与控制
matlab·电机·pmsm·仿真模型
不脱发的程序猿3 个月前
MATLAB实现PID参数自动整定
matlab·pid
教练、我想打篮球3 个月前
53 mysql pid 文件的创建
mysql·pid·start·stop
蔚蓝慕3 个月前
Arduino 控制理论(3)- 如何在 Arduino 中调节 PID 控制器
pid
不知道是谁24 个月前
控制程序确定执行步长,也就是输出信号更新频率
算法·控制·pid·现代控制理论·自动控制原理
Mr.Cssust4 个月前
【研发日记】Matlab/Simulink技能解锁(十)——PID调参技巧
仿真·pid·嵌入式软件·matlab/simulink·嵌入式处理器·ecu控制器·基于模型开发
烦恼归林4 个月前
电机控制杂谈——以袁雷老师滑模控制模型为例子,如何搭建准确的电机仿真模型
matlab·电机·电力电子·电机控制·永磁同步电机
东方.既白4 个月前
PID控制
pid