平衡小车调车保姆式教程
前言
（1）硬件选型注意点：电机转速、轮子大小
（2）车模硬件结构注意点：车模整体的重量要分布均匀，利于平衡
（3）硬件主要模块：陀螺仪、编码器电机、显示屏、驱动、主控、电源
（4）软件主要模块：陀螺仪、编码器、直立环、速度环、转向环
硬件选型
电机的选择
电机的关键参数：电压、转速
参数标的电压不能理解为电机最大供电电压，电机安全的工作状态主要是由于通过电机的电流决定的，只有电流才会损坏电机，电流小的话不会损坏电机，所以额定电流才是关键值。
重要的是转速的选择：转得越快，电机输出的力矩就越小，即电机产生的力量越小，电机转的越快，那劲肯定越小，电机转的越慢，电机的劲就越大。
平衡车需要电机的力矩大一点好。建议建议6V 150r/min的电机。
轮子的选择
平衡车需要摩擦力较大的轮子，同时车轮的半径要足够大，否则车体的重心太高或太低都不利于平衡。
软件模块
陀螺仪
这里我用钱来解决了麻烦的问题，重金入手了一个性能较好的六轴陀螺仪jy62。只需要使用单片机的一个串口，这个模块对数据的处理已经做的很成功了。
imu.c
//
// Created by fazhehy on 2023/7/17.
//
#include "platform/platform.h"
static uint8_t buffer[11];
imu_data_t acceleration, gyroscope, angle;
// uart1
void imu_init()
{
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
SysCtlPeripheralEnable(SYSCTL_PERIPH_UART1);
GPIOPinConfigure(GPIO_PB0_U1RX);
GPIOPinConfigure(GPIO_PB1_U1TX);
GPIOPinTypeUART(GPIO_PORTB_BASE, GPIO_PIN_0);
GPIOPinTypeUART(GPIO_PORTB_BASE, GPIO_PIN_1);
UARTConfigSetExpClk(UART1_BASE, SysCtlClockGet(), 115200, UART_CONFIG_WLEN_8|UART_CONFIG_STOP_ONE|UART_CONFIG_PAR_NONE);
UARTFIFOEnable(UART1_BASE);
UARTFIFOLevelSet(UART1_BASE, UART_FIFO_TX2_8, UART_FIFO_RX2_8);
UARTIntEnable(UART1_BASE, UART_INT_RX|UART_INT_RT);
void imu_handler(void);
UARTIntRegister(UART1_BASE, imu_handler);
IntPrioritySet(INT_UART1, USER_INT2);
IntEnable(INT_UART1);
IntMasterEnable();
UARTEnable(UART1_BASE);
}
static uint8_t get_verify_code()
{
uint32_t sum = 0;
for (uint8_t i = 0; i < 10; ++i) {
sum += buffer[i];
}
return sum&0xff;
}
int32_t k = 0;
void imu_analysis()
{
static float last_angle_z = 0, angle_z = 0;
switch (buffer[1]) {
case 0x51:
acceleration.x = (float )((int16_t)(buffer[2]|(buffer[3]<<8)))/32768*16;
acceleration.y = (float )((int16_t)(buffer[4]|(buffer[5]<<8)))/32768*16;
acceleration.z = (float )((int16_t)(buffer[6]|(buffer[7]<<8)))/32768*16;
break;
case 0x52:
gyroscope.x = (float )((int16_t)(buffer[2]|(buffer[3]<<8)))/32768*2000;
gyroscope.y = (float )((int16_t)(buffer[4]|(buffer[5]<<8)))/32768*2000;
gyroscope.z = (float )((int16_t)(buffer[6]|(buffer[7]<<8)))/32768*2000;
break;
case 0x53:
angle.x = (float )((int16_t)(buffer[2]|(buffer[3]<<8)))/32768*180;
angle.y = (float )((int16_t)(buffer[4]|(buffer[5]<<8)))/32768*180;
angle_z = (float )((int16_t)(buffer[6]|(buffer[7]<<8)))/32768*180;
break;
}
if (last_angle_z < -90 && angle_z > 90)
k --;
if (last_angle_z > 90 && angle_z < -90)
k ++;
last_angle_z = angle_z;
// printf("k:%ld\n", k);
angle.z = angle_z + (float )k*360;
}
#define ERROR 0
#define DOING 1
#define SUCCESS 2
static uint8_t n = 0, state = 0;
void imu_handler()
{
uint32_t data;
uint32_t status=UARTIntStatus(UART1_BASE, true);
UARTIntClear(UART1_BASE, status);
while(UARTCharsAvail(UART1_BASE)){
data = UARTCharGetNonBlocking(UART1_BASE)&0xff;
// data = UARTCharGet(UART1_BASE)&0xff;
// UARTCharPutNonBlocking(UART0_BASE, data);
// printf("data:%d\n", data);
if (state == ERROR){
n = 0;
}
buffer[n] = (uint8_t)data;
if (n == 0){
if (buffer[0] == 0x55){
state = DOING;
} else{
state = ERROR;
}
} else if (n == 10){
if (buffer[10] == get_verify_code()){
state = SUCCESS;
} else{
state = ERROR;
}
}
n += 1;
if (state == SUCCESS){
// printf("OK\n");
imu_analysis();
state = ERROR;
}
}
}
imu.h
//
// Created by fazhehy on 2023/7/17.
//
#ifndef IMU_H
#define IMU_H
typedef struct {
float x;
float y;
float z;
}imu_data_t;
void imu_init(void);
extern imu_data_t acceleration, gyroscope, angle;
#endif /* IMU_H */
代码分析
按函数分析
void imu_init()； //imu初始化
陀螺仪的初始化，初始化引脚，注册串口中断函数，打开中断。
void imu_init()
{
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
SysCtlPeripheralEnable(SYSCTL_PERIPH_UART1);
GPIOPinConfigure(GPIO_PB0_U1RX);
GPIOPinConfigure(GPIO_PB1_U1TX);
GPIOPinTypeUART(GPIO_PORTB_BASE, GPIO_PIN_0);
GPIOPinTypeUART(GPIO_PORTB_BASE, GPIO_PIN_1);
UARTConfigSetExpClk(UART1_BASE, SysCtlClockGet(), 115200, UART_CONFIG_WLEN_8|UART_CONFIG_STOP_ONE|UART_CONFIG_PAR_NONE);
UARTFIFOEnable(UART1_BASE);
UARTFIFOLevelSet(UART1_BASE, UART_FIFO_TX2_8, UART_FIFO_RX2_8);
UARTIntEnable(UART1_BASE, UART_INT_RX|UART_INT_RT);
void imu_handler(void);
UARTIntRegister(UART1_BASE, imu_handler);
IntPrioritySet(INT_UART1, USER_INT2);
IntEnable(INT_UART1);
IntMasterEnable();
UARTEnable(UART1_BASE);
}
static uint8_t get_verify_code()；//数据校准
10个为一组，对串口获取到的 buffer[i]数据进行求和。
static uint8_t get_verify_code()
{
uint32_t sum = 0;
for (uint8_t i = 0; i < 10; ++i) {
sum += buffer[i];
}
return sum&0xff;
}
void imu_analysis()； //数据分析
对陀螺仪获取到的数据进行处理，得到角速度和角度。
其中有一个对角度的巧妙处理，是的偏航角以z轴作为0度分界，解决陀螺仪自身得到角度的不连续性。
if (last_angle_z < -90 && angle_z > 90)
k --;
if (last_angle_z > 90 && angle_z < -90)
k ++;
last_angle_z = angle_z;
// printf("k:%ld\n", k);
angle.z = angle_z + (float )k*360;
int32_t k = 0;
void imu_analysis()
{
static float last_angle_z = 0, angle_z = 0;
switch (buffer[1]) {
case 0x51:
acceleration.x = (float )((int16_t)(buffer[2]|(buffer[3]<<8)))/32768*16;
acceleration.y = (float )((int16_t)(buffer[4]|(buffer[5]<<8)))/32768*16;
acceleration.z = (float )((int16_t)(buffer[6]|(buffer[7]<<8)))/32768*16;
break;
case 0x52:
gyroscope.x = (float )((int16_t)(buffer[2]|(buffer[3]<<8)))/32768*2000;
gyroscope.y = (float )((int16_t)(buffer[4]|(buffer[5]<<8)))/32768*2000;
gyroscope.z = (float )((int16_t)(buffer[6]|(buffer[7]<<8)))/32768*2000;
break;
case 0x53:
angle.x = (float )((int16_t)(buffer[2]|(buffer[3]<<8)))/32768*180;
angle.y = (float )((int16_t)(buffer[4]|(buffer[5]<<8)))/32768*180;
angle_z = (float )((int16_t)(buffer[6]|(buffer[7]<<8)))/32768*180;
break;
}
if (last_angle_z < -90 && angle_z > 90)
k --;
if (last_angle_z > 90 && angle_z < -90)
k ++;
last_angle_z = angle_z;
// printf("k:%ld\n", k);
angle.z = angle_z + (float )k*360;
}
void imu_handler()； //中断回调函数
处理中断工作。在这里面调用imu_analysis()函数。
#define ERROR 0
#define DOING 1
#define SUCCESS 2
static uint8_t n = 0, state = 0;
void imu_handler()
{
uint32_t data;
uint32_t status=UARTIntStatus(UART1_BASE, true);
UARTIntClear(UART1_BASE, status);
while(UARTCharsAvail(UART1_BASE)){
data = UARTCharGetNonBlocking(UART1_BASE)&0xff;
// data = UARTCharGet(UART1_BASE)&0xff;
// UARTCharPutNonBlocking(UART0_BASE, data);
// printf("data:%d\n", data);
if (state == ERROR){
n = 0;
}
buffer[n] = (uint8_t)data;
if (n == 0){
if (buffer[0] == 0x55){
state = DOING;
} else{
state = ERROR;
}
} else if (n == 10){
if (buffer[10] == get_verify_code()){
state = SUCCESS;
} else{
state = ERROR;
}
}
n += 1;
if (state == SUCCESS){
// printf("OK\n");
imu_analysis();
state = ERROR;
}
}
}
直立环
作用：使小车保持直立。
//****************平衡小车机械零点***************
float Car_zero = 1.0f;
//直立环
float zhili_Kp=95.5f*0.6f; //出现大幅度低频振荡 95.5f
float zhili_Kd=-10.0f*0.6f; //出现小幅度高频振荡 *0.6f
int zhili_out=0; //直立环输出
//***********平衡车控制******************************************
//函数功能：控制小车保持直立
//Angle：采集到的实际角度值 angle.x
//Gyro： 采集到的实际角速度值 gyroscope.x
int zhili(float Angle,float Gyro)
{
float err;
int pwm_zhili;
err=Car_zero-Angle; //期望值-实际值，这里期望小车平衡，因此期望值就是机械中值
pwm_zhili=zhili_Kp*err+Gyro*zhili_Kd;//计算平衡控制的电机PWM
return pwm_zhili;
}
速度环
作用：使小车平衡得更加稳定，平衡效果更好。
void speed_control_100hz(void)
{
//平衡车直立环上的速度环，为与直立环处理速度一致，把下面两行代码注释掉
// static uint16_t _cnt=0;
// _cnt++; if(_cnt<2) return; _cnt=0;//10ms控制一次 100hz是0.005s，两个100hz就是10ms
speed_feedback[0]=smartcar_imu.left_motor_speed_cmps;//获取左轮实际值
speed_error[0]=v_target_l-speed_feedback[0];
speed_error[0]=constrain_float(speed_error[0],-speed_err_max,speed_err_max);
speed_integral[0]+=speed_ki*speed_error[0];
speed_integral[0]=constrain_float(speed_integral[0],-speed_integral_max,speed_integral_max);
speed_output[0]=speed_integral[0]+speed_kp*speed_error[0];
speed_output[0]=constrain_float(speed_output[0],-speed_ctrl_output_max,speed_ctrl_output_max);
// UART_printf(UART0_BASE,"%d\n",(int)smartcar_imu.left_motor_speed_cmps);
speed_feedback[1]=smartcar_imu.right_motor_speed_cmps;//右轮
speed_error[1]=v_target_r-speed_feedback[1];//期望速度减去实际速度得到速度误差
speed_error[1]=constrain_float(speed_error[1],-speed_err_max,speed_err_max);//对速度误差做约束
speed_integral[1]+=speed_ki*speed_error[1]; //速度积分
speed_integral[1]=constrain_float(speed_integral[1],-speed_integral_max,speed_integral_max);//对得到的速度积分做约束
speed_output[1]=speed_integral[1]+speed_kp*speed_error[1];//pid得到输出
speed_output[1]=constrain_float(speed_output[1],-speed_ctrl_output_max,speed_ctrl_output_max);//对输出做约束
//UART_printf(UART0_BASE,"%d\n",(int)smartcar_imu.right_motor_speed_cmps);
}
转向环
作用：优化小车的平衡效果，小车不会左拐和右拐，只在正前方与正后方调整车身来平衡车体。
//转向环
float zhuan_Kp=0.0f; //期望小车转向，正反馈
float zhuan_Kd=-10.0f; //抑制小车转向，负反馈
//*************************************************************
//函数功能：控制小车转向
//Set_turn：目标旋转角速度
//Gyro_Z:陀螺仪Z轴的角速度
//不是一个严格的PD控制器，为小车的叠加控制
int zhuan(float Set_turn,float Gyro_Z)
{
int PWM_Out=0;
if(Set_turn==0)
{
PWM_Out=zhuan_Kd*Gyro_Z; //没有转向需求，Kd约束小车转向
}
if(Set_turn!=0)
{
PWM_Out=zhuan_Kp*Set_turn; //有转向需求，Kp为期望小车转向
}
return PWM_Out;
}
调参保姆级教程
1 机械中值的确定
（1）机械中值就是小车直立状态下的绕x轴的俯仰角pitch，对这个机械中值的要求不是特别高，因为在速度环的作用下，小车在一定范围内的机械中值下都能保持平衡（加入速度环后，机械中值的影响不是很大），没必要按网上那种两边倒下的值加起来取平均值的方法来取机械中值。
（2）我们除了要用到绕x轴方向的俯仰角外，还需要绕z轴方向的偏航角来使小车的平衡性能更好。
2 直立环调参
err=Car_zero-Angle; //期望值-实际值，这里期望小车平衡，因此期望值就是机械中值
pwm_zhili=zhili_Kp*err+Gyro*zhili_Kd;//计算平衡控制的电机PWM
2.1 直立环Kp极性的确定
直立的要求是小车往哪边倒，小车就往哪边加速前进，使小车得以平衡。
先给一个大的P值，例如P=100，在单独的直立环作用下，倾倒小车，观察小车的运动状态，正确Kp极性下，运动状态是小车往哪边倒，轮子就往哪边转，改变P值的正负使小车的运动状态达到要求。
2.2 直立环Kp大小的调节
（1）先把Kp设成一个小的值，若发现轮子不转，则是Kp的值过小。
（2）将Kp的值增大，小车向前倾斜，小车会向前运动；小车向后倾斜，小车会向后运动，但是直立不起来，是因为小车反应得太慢了，原因还是Kp过小。
（3）继续增大Kp，可以看到小车有接近平衡的趋势，但是还是平衡不起来，原因是Kp的值还是小了些。
（4）继续增大Kp，可以看到小车有明显的平衡趋势，平衡效果不错。
（5）根据工程经验，我们要继续增大Kp，直到松手让小车直立时出现低频振荡。
（6）小车出现低频振荡后，根据工程经验，在增大一点Kp后可以引入Kd分量。这里再增大一点Kp，小车出现明显低频振荡，效果符合，此时应快速关闭电源，不能让低频振荡持续太久。
2.3 直立环Kd极性的确定
Kd作用是辅助Kp的效果，使小车反应更迅速，达到好的直立效果。
先将直立环Kp置为0，将Kd给一个稍微大一点的值，例如;Kd=100;正确Kd极性下，运动状态是小车往哪边倒，轮子就往哪边转。
2.4 直立环Kd大小的调节
（1）恢复Kp的值，先将Kd设成一个较小的值，例如Kd=1；可以看到小车的平衡效果其实比较好，可以做到稳定平衡。但是按照工程实践，我们需要继续增大Kd寻找Kd的临界值，直到小车出现高频振荡。
（2）小车出现高频振荡时，需要迅速关闭电源，避免因为反馈电流过大导致电机驱动或者电池烧毁。
（3）小车出现小范围的高频振荡时，再增大Kd，可以看到小车出现明显的高频振荡。此时，可以停止调节直立环了。
按照工程实践，给直立环的Kp和Kd都乘上0.6（因为我们是按振荡的现象来调的，所以直这里乘以0.6可以让小车平衡）。
单独直立环效果
小车可以较长时间直立，但是需要手动去干预小车的运动状态，使他维持平衡。但是没有速度环的加持，小车不能够自己在大范围空间内保持长时间直立。
3 速度环调参
speed_feedback[0]=smartcar_imu.left_motor_speed_cmps;//获取左轮实际值
speed_error[0]=v_target_l-speed_feedback[0];
speed_error[0]=constrain_float(speed_error[0],-speed_err_max,speed_err_max);
speed_integral[0]+=speed_ki*speed_error[0];
speed_integral[0]=constrain_float(speed_integral[0],-speed_integral_max,speed_integral_max);
speed_output[0]=speed_integral[0]+speed_kp*speed_error[0];
speed_output[0]=constrain_float(speed_output[0],-speed_ctrl_output_max,speed_ctrl_output_max);
3.1 速度环Kp极性的确定
（1）关闭直立环
（2）给速度环Kp先设立一个比较大的值，给速度环一个目标值，正确的Kp极性下，轮子是正常快速转动的，否则，小车轮子不转。
3.2 速度环Ki极性的确定
根据工程实践，速度环的Ki = 1/200*Kp，所以后面我们速度环的Kp与Ki可以一起调。
给Kp一个较小的值，Ki一个更小的值，比Kp小一个数量级，更改Ki的极性观察轮子转动情况，正确的转动情况是，用手将轮子往一个方向转动，小车会按这个方向慢慢增大到最大转速，会出现小范围的振荡属于正常现象,错误极性下，轮子会按手转动的反方向转动，不会按原方向转动到最大。
3.3 速度环Kp、Kd大小的调节
速度环的Ki = 1/200*Kp，我们只需要调节Kp即可。
（1）打开直立环，在直立环基础上调速度环，以小车的平衡效果为调参的基准。
（2）先将Kp设成一个小的值，可以看到在速度环的加持下，小车的直立效果更好。
（3）但是反馈的效果依旧很慢，所以我们要继续增大速度环Kp。
（4）调节到合适的Kp，小车可以很好保持平衡，同时还具备一定抗击打能力，可以较长时间直立。
（5）但是小车在自己的平衡之中会出现旋转，同时在原地平衡时也会出现短期振荡现象。
直立环加速度环现象
（1）小车可以很好保持平衡，同时还具备一定抗击打能力，可以较长时间直立。
（2）原地平衡时也会出现短期振荡现象可以通过增大直立环的Kd和减小速度环的Kp来抵消这个情况，慢慢调整可以达到一个好的效果，这个比较麻烦，需要长时间的耐心。
（3）旋转的情况我们可以通过加入转向环来抑制。
4 转向环调参
转向环若是为了让小车保持一个方向上的原地静止，所以不需要Kp，仅加入Kd即可。
if(Set_turn==0)
{
PWM_Out=zhuan_Kd*Gyro_Z; //没有转向需求，Kd约束小车转向
}
if(Set_turn!=0)
{
PWM_Out=zhuan_Kp*Set_turn; //有转向需求，Kp为期望小车转向
}
4.1 转向环Kd极性的确定
将直立环和速度环关闭，将Kd设为100。正确Kp极性下小车的运动状态是，顺时针转动小车，轮子产生的作用可以抵抗小车顺时针转动，产生对抗旋转的扭矩。
4.2 转向环Kd大小的确定
（1）开启直立环和速度环
（2）从小到大调整Kd的值，合适的Kd可以使小车在平衡过程中不发生水平方向的转动
（3）若有平衡过程中小车有旋转趋势，说明Kd的值还是过小，则需要继续增大Kd的值。
（4）若小车出现明显的高频振荡，说明Kd过大了。
5 拿起与放下小车检测
拿起检测思路是当轮子以较大速度旋转一段时间后，关闭电机；
放下检测思路是当俯仰角过大时关闭电机。