首先附上本项目的目的GitHub仓库

本项目使用STM32F103C8T6作为主控，Keil5开发，Mahony用算法解决姿态的平衡车。项目中给出了MPU6050、ICM20602、ICM42605三种主流IMU驱动。目前直立平衡只实现，设计设计增加了BlueTooth模块、手机遥控部分仍在开发中(新文件夹) (°ー°〃)

第一个前提知识是：

• Keil5的使用
• STM32有一定了解
• 使用电机驱动的原理

目前是第一版，画的很好，用的都是插件，线也很乱 (:3」∠)。以后会更新PCB(计划用贴片代替，体积会更小)

本项目使汽车平衡最重要的两个环是直立环和速度环，通过叠加获得电机的最终输出，这也是绝大多数在线汽车平衡控制方法。

1. 我们首先使用传统的速度负反馈算法来想象它。首先，我们给出了一个目标速度值，因为汽车在直立控制的作用下，汽车向前倾斜以获得加速度，车轮需要向后移动，这样汽车的速度就会下降。因为这是负反馈，在速度下降后，速度控制的偏差增加，汽车向前倾斜的角度增加，所以重复，汽车就会倒下。
2. 为了确保直立控制的优先级，我们将速度控制放在直立控制的前面，即速度控制调整的结果只是改变直立控制的目标值。因为根据经验，汽车的运行速度与汽车的倾角有关。例如，为了提高汽车的前进速度，有必要增加汽车的前倾角度。倾斜角度增加后，车轮需要在直立控制的作用下向前移动。因此，直立环Kp后乘值不是当前角度-机械中值，这是我们对速度的期望。理解这一点很重要！

a 1 = K p ? ( θ ? a 2 ) K d ? θ ′ ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ① 直 立 环 a_1=Kp*(\theta-a_2) Kd*\theta' \qquad········································①直立环 a 1​=Kp∗(θ−a2​)+Kd∗θ′⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅①直立环

a 2 = K p 1 ∗ E r r s p e e d + K i 1 ∗ ∑ E r r s p e e d   ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ② 速 度 环 a_2=Kp_1*Err_{speed}+Ki_1*\sum Err_{speed}\space \qquad·····························②速度环 a2​=Kp1​∗Errspeed​+Ki1​∗∑Errspeed​ ⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅⋅②速度环

合并一下就可以得到：

a = K p ∗ ( θ ) + K d ∗ θ ′ − K p [ k p 1 ∗ E r r s p e e d + k i 1 ∗ ∑ E r r s p e e d ] ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ③ 最 终 式 a=Kp*(\theta)+Kd*\theta'-Kp[kp_1*Err_{speed}+ki_1*\sum Err_{speed}] \qquad········③最终式 a=Kp∗(θ)+Kd∗θ′−Kp[kp1​∗Errspeed​+ki1​∗∑Errspeed​]⋅⋅⋅⋅⋅⋅⋅⋅③最终式

因此在代码实现上我们就可以实现两个环的直接相加或相减，在TIM3定时器中10ms一个周期进行控制。

      Balance_PID_Result = Position_PID_Cal(&Balance_PID, imu.Roll + 0.3f);
      if (Time_GAP_20ms) Velocity_PID_Result = Position_PID_Cal(&Velocity_PID, Velocity);

      PID_Result = Balance_PID_Result + Velocity_PID_Result;
      Set_Motor_Speed(PID_Result, PID_Result);

根据上边的分析我们只需要分别调整直立环的Kp，kd和速度环的Kp，Ki

1. 对于直立环的Kp，是调整最方便观察现象的，太小时小车没有足够的恢复力，太大时小车会在中值附近大幅震荡，调整到一个略微震荡的值即可
2. 直立环的Kd作用是减小低频振荡，但Kd太大小车又会造成高频振动，从小到大增大Kd，直到小车出现小幅高频振荡
3. 直立环调整结束后小车可以平衡，但受到扰动便会往一个方向疯跑，现在引入直立环
4. 网上查到的资料对于速度环Ki = Kp/200 ，但在我的实际调整中最终确定了Ki = Kp/100，这个看自己小车了，可以先按照Ki = Kp/200去调
5. 先将直立环Kp，Kd同时×0.8。调整速度环Kp，速度环Kp越大，小车便越不会出现向一个方向狂奔的情况（因为速度被速度环控住了），但会减弱直立环的控制效果，因此调整到一个车受到干扰会摇摇晃晃停下的一个状态。
6. 摇摇晃晃的原因就是因为速度环Kp太大，回调速度环Kp，并且增大直立环Kd（想想Kd的作用是什么呢？）

以上就是我调参的经验，可以参考，最好可以理解原理再去上手实践。

参考一篇文章：基于Manony滤波算法的姿态解算

static void Get_IMU_Values(float *values)
{ 
        
	int16_t gyro[3],acc[3];
	IMU_readGyro_Acc(&gyro[0],&acc[0]);
	for(int i=0;i<3;i++)
	{ 
        
        //gyro range +-2000; adc accuracy 2^16=65536; 65536/4000=16.4;
		values[i]=((float) gyro[i])/16.4f;	
		values[3+i]=(float) acc[i];
	}
}

然后编写函数实现计算姿态角的功能，使用四元数计算姿态角的公式在理论分析中推导：

其中α为绕x轴旋转角即roll，β为绕y轴旋转角即pitch，γ为绕z轴旋转角即yaw。a,b,c,d即q0,q1,q2,q3.

void IMU_Update(void)
{ 
        
	static float q[4];
	float Values[6];	
	Get_IMU_Values(Values);	
	
	//change angle to radian,the calculate the imu with Mahony
	MahonyAHRSupdateIMU(Values[0] * PI/180, Values[1] * PI/180, Values[2] * PI/180,
 	                    Values[3], Values[4], Values[5]);		
	//save Quaternion
	q[0] = q0;
	q[1] = q1;
	q[2] = q2;
	q[3] = q3;
    
	imu.ax = Values[3];
	imu.ay = Values[4];
	imu.az = Values[5];
    
	imu.Pitch_v = Values[0];
	imu.Roll_v = Values[1];
	imu.Yaw_v = Values[2];

	//calculate the imu angle with quaternion
	imu.Roll = (atan2(2.0f*(q[0]*q[1] + q[2]*q[3]),1 - 2.0f*(q[1]*q[1] + q[2]*q[2])))* 180/PI;	
	imu.Pitch = -safe_asin(2.0f*(q[0]*q[2] - q[1]*q[3]))* 180/PI;
	imu.Yaw = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3 * q3 + 1)* 180/PI;
}

代码中MahonyAHRSupdateIMU()函数实现的就是四元数的更新算法。

逻辑上，首先用加速度计校准陀螺仪，方式是通过计算当前四元数姿态下的重力分量，与加速度计的重力分量作叉积，得到误差。 对误差作P(比例)和I(积分)运算后加到陀螺仪角速度上。最终由角速度计算新的四元数。

代码中的 sampleFreq 即执行姿态解算的频率，这里用定时器，以500HZ的频率调用get_angle();

void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) { 
        
	float recipNorm;	
	float halfvx, halfvy, halfvz;	//1/2 重力分量
	float halfex, halfey, halfez;	//1/2 重力误差
	float qa, qb, qc;
	//对加速度数据归一化
	recipNorm = invSqrt(ax * ax + ay * ay + az * az);
	ax *= recipNorm;
	ay *= recipNorm;
	az *= recipNorm;

	// 由四元数计算重力分量
	halfvx = q1 * q3 - q0 * q2;
	halfvy = q0 * q1 + q2 * q3;
	halfvz = q0 * q0 - 0.5f + q3 * q3;

	// 将四元数重力分量 与 加速度计重力分量 作叉积 得到误差
	halfex = (ay * halfvz - az * halfvy);
	halfey = (az * halfvx - ax * halfvz);
	halfez = (ax * halfvy - ay * halfvx);

	//对误差作积分
	integralFBx += twoKi * halfex * (1.0f / sampleFreq);	
	integralFBy += twoKi * halfey * (1.0f / sampleFreq);
	integralFBz += twoKi * halfez * (1.0f / sampleFreq);
	//反馈到角速度
	gx += integralFBx;	     gy += integralFBy;       gz += integralFBz;

	// 对误差作比例运算并反馈
	gx += twoKp * halfex;    gy += twoKp * halfey;    gz += twoKp * halfez;

	// 计算1/2 dt
	gx *= (0.5f * (1.0f / sampleFreq));		
	gy *= (0.5f * (1.0f / sampleFreq));
	gz *= (0.5f * (1.0f / sampleFreq));
	qa = q0;     qb = q1;      qc = q2;
	// 更新四元数
	q0 += (-qb * gx - qc * gy - q3 * gz);
	q1 += (qa * gx + qc * gz - q3 * gy);
	q2 += (qa * gy - qb * gz + q3 * gx);
	q3 += (qa * gz + qb * gy - qc * gx);

	// 四元数归一化
	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= recipNorm;     q1 *= recipNorm;     q2 *= recipNorm;     q3 *= recipNorm;
}

由于加速度计对水平方向的旋转无能为力，故用此程序得到的yaw角数据会一直漂移，无法得到校准；通常的解决方法是增加一个磁场传感器，来获得一个准确的水平方向角来校准陀螺仪的漂移。MPU6050支持扩展一个IIC接口到磁场传感器，可通过配置MPU6050的IIC MASTER 来读取磁场传感器的数据。

在Mahony中提供了包含磁场数据的融合函数：

> void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, 
>                       float mx, float my, float mz);

首先看main函数：

int main(void)
{ 
        
	All_HardWare_init();
	while (1)
	{ 
        
		Protect_Check();
		LED_show_working();
		LCD_show_Brief_info();
	}
}

代码都封装在了函数里，因此主控的main函数非常简单。All_HardWare_init();包含了所有硬件、片内资源的初始化。因为Mahony每次上电融合解算姿态时需要几秒的自我校准，因此先打开定时器。

	TIM3_Int_Init(99, 7199); // 72M ÷7200 ÷100 = 10 ms

几秒过后再初始化PID控制器

	PID_init();              //直立环，速度环PID控制器初始化

定时器3中断服务函数在control.c文件中，包含姿态解算和PID控制。

void TIM3_IRQHandler(void)
{ 
        
  if (TIM_GetITStatus(TIM3, TIM_IT_Update) == SET) 
  { 
           TIM_ClearITPendingBit(TIM3, TIM_IT_Update);