文章目录
全向轮小车技术报告
最近做了个全向轮小车,把笔记改了一下向大家分享一下
工程文件和效果演示视频在HAL库三轮全向轮小车工程,使用stm32f411ceu6,ide为clion
工程环境:
IDE:Clion+stm32cubemx
串口助手:vofa+
主控板:STM32F411CEU6最小系统板+自行设计的扩展板
1:直流减速电机速度控制
所用硬件:
电机驱动:tb6612四路电机驱动模块
电机:光电编码器电机,360线,减速比:1:74.8
工程配置:
配置TIM2的CH2作为PWM波输出接口,PWM波频率设置为100kHz:
配置TIM1的编码器模式,并设置PB15和PB14为控制电机旋转方向的IO接口:
vofa+的调参界面和控件指令设置为下图所示,实现调参的代码可见附件中全向轮小车工程文件中的PID_Adjust.c和PID_Adjust.h(参考[调参神器]使用VOFA+上位机进行PID调参(附下位机代码)),使用USART1进行串口的收发,使用TIM9进行定时中断。
使用增量式PID进行速度控制,速度环PID函数如下:
/****************************************
* 作用:速度环PID计算
* 参数:PID参数结构体地址;目标值;反馈值
* 返回值:无
* ****************************************/
float Speed_PID_Realize(PID* pid,float target,float feedback)//一次PID计算
{
pid->err = target - feedback;
if(pid->err < 0.1 && pid->err > -0.1) pid->err = 0;//pid死区
pid->integral += pid->err;
if(pid->ki * pid->integral < -pid->maxIntegral) pid->integral = -pid->maxIntegral / pid->ki;//积分限幅
else if(pid->ki * pid->integral > pid->maxIntegral) pid->integral = pid->maxIntegral / pid->ki;
if(target == 0) pid->integral = 0; // 刹车时清空i
pid->output += (pid->kp * pid->err) + (pid->ki * pid->integral)
+ (pid->kd * (pid->err - pid->lastErr));//增量式PID
//输出限幅
if(target >= 0)//正转时
{
if(pid->output < 0) pid->output = 0;
else if(pid->output > pid->maxOutput) pid->output = pid->maxOutput;
}
else if(target < 0)//反转时
{
if(pid->output < -pid->maxOutput) pid->output = -pid->maxOutput;
else if(pid->output > 0) pid->output = 0;
}
pid->lastErr = pid->err;
if(target == 0) pid->output = 0; // 刹车时直接输出0
return pid->output;
}
定时器中断代码:
if (htim == (&htim9))
{
encoder_now=(short )(__HAL_TIM_GET_COUNTER(ENCODER1));//速度采样周期为1ms
__HAL_TIM_SET_COUNTER(ENCODER1,0);
time1++;
if (time1==10){//控制周期为10ms
time1=0;
printf("MOTOR:%.2f,%.2f,%d,%d\n", Target_Speed, motorA.speed,(short )(__HAL_TIM_GET_COUNTER(ENCODER1)),motorA.totalCount);//以FireWater协议的要求向vofa+发送数据,以打印数据波型
Speed_PID_Realize(&pid_speed,Target_Speed,motorA.speed);
MotorA_Run(pid_speed.output);
}
}
调试报告:
PID各项的初始值设置为0,先调节P项使电机的响应速度到达一个较理想的水平:
此时有较大的振荡和超调,因此调节D项抑制振荡和超调:
此时电机响应较快且没有振荡和超调。
发现加入I项会增大超调:
因此不使用I项。
2:直流减速电机位置控制
使用的硬件和工程配置与1相同。
实现思路:
将电机转动时的编码器产生的脉冲数映射为为角度值以实现对电机转动角度的控制,设定主控板复位时电机的位置为角度0点,使用串级PID进行对电机转过多少角度的控制。定时器中断代码如下:
if (htim == (&htim9))//1ms
{
encoder_now=(short )(__HAL_TIM_GET_COUNTER(ENCODER1));
__HAL_TIM_SET_COUNTER(ENCODER1,0);
motorA.speed=encoder_now/(4*74.8*360)*1000*60; //速度测量,rpm
motorA.totalCount+=encoder_now;//累计脉冲数
angle_now= transfer(motorA.totalCount);//将脉冲数映射为为角度值
time1++;
if (time1==10){//10ms进行一次PID计算
time1=0;
printf("MOTOR:%.2f,%.2f,%d,%d\n", angle_now, Target_Position,(short )(__HAL_TIM_GET_COUNTER(ENCODER1)),motorA.totalCount);//以FireWater协议的要求向vofa+发送数据,以打印数据波型
Target_Speed= Position_PID_Realize(&pid_position, Target_Position, angle_now);//外环,位置环
Speed_PID_Realize(&pid_speed,Target_Speed,motorA.speed);
MotorA_Run(pid_speed.output);
}
}
位置环函数,使用全量式PID:
/****************************************
* 作用:位置环PID计算
* 参数:PID参数结构体地址;目标值;反馈值
* 返回值:无
* ****************************************/
float Position_PID_Realize(PID* pid, float target, float feedback)//一次PID计算
{
if(pid->err < 0.1 && pid->err > -0.1) pid->err = 0;//pid死区
pid->err = target - feedback;
pid->integral += pid->err;
if(pid->ki * pid->integral < -pid->maxIntegral) pid->integral = -pid->maxIntegral / pid->ki;//积分限幅
else if(pid->ki * pid->integral > pid->maxIntegral) pid->integral = pid->maxIntegral / pid->ki;
pid->output = (pid->kp * pid->err) + (pid->ki * pid->integral) + (pid->kd * (pid->err - pid->lastErr));//全量式PID
//输出限幅
if(pid->output > pid->maxOutput) pid->output = pid->maxOutput;
if(pid->output < -pid->maxOutput) pid->output = -pid->maxOutput;
pid->lastErr = pid->err;
return pid->output;
}
调试报告:
PID各项的初始值设置为0,先调节P项使电机的响应速度到达一个较理想的水平:
此时电机的响应速度较为理想,但超调和振荡现象较为显著,因此调节D项消除超调和振荡:
此时已通过串级PID实现对直流减速电机位置的较好控制。
3:实现全向轮底盘解算,实现角度环的精准控制
所用硬件:
主控板,电机驱动:与1,2中使用的一样。
电机:3个光电编码器电机,360线,减速比:1:74.8
全向轮:3个直径70mm的塑料材质全向轮
电池:3S动力锂电池,容量为2200mAh
陀螺仪:IM600姿态传感器
调试工具:PS2手柄及其接收器,无线DAPLink
全向轮小车的结构:
全向轮小车的主体结构为以六角铜柱连接的两层铝合金材质平台和三个电机支架。
全向轮小车三视图(单位:mm)
小车组装完成的状态:
实现过程:
1.对小车旋转角度的控制
由于使用的电机和驱动与1中的相同,因此我直接使用1调好的速度环来控制小车的三个电机,调试后速度控制效果良好。我使用PID角度环来实现对小车旋转角度的精确控制。驱动motorB和motorC的定时器配置与1中类似。首先我使用IM600姿态传感器来实现对小车姿态角的获取,配置USART6来进行IM600与小车的通讯,main.c中实现通讯的代码如下,其中具体函数的代码可见附件中全向轮小车工程文件中的im948_CMD.h,im948_CMD.c和bsp_usart.h,bsp_usart.c
//主循环前的初始化代码
HAL_UART_Receive_IT(&huart6, &rx_byte, 1);//打开串口6的接收中断
Cmd_03();// 唤醒传感器
Cmd_12(5, 255, 0, 0, 3, 200, 2, 4, 9, 0x40);//设置IM600设备参数
Cmd_05();// 归零IM600Z轴姿态角数据,以小车复位时的姿态角为角度0点
Cmd_19();// 开启数据主动上报
//主循环中处理模块发过来的数据
U8 rxByte;
while (Uart.UartFifo.Cnt > 0)
{// 从fifo获取串口发来的数据
rxByte = Uart.UartFifo.RxBuf[Uart.UartFifo.Out];
if (++Uart.UartFifo.Out >= FifoSize)
{
Uart.UartFifo.Out = 0;
}
__disable_irq();
--Uart.UartFifo.Cnt;
__enable_irq();
Cmd_GetPkt(rxByte); // 移植 每收到1字节数据都填入该函数,当抓取到有效的数据包就会回调进入 //Cmd_RxUnpack(U8 *buf, U8 DLen) 函数处理
}
// 串口6的中断处理
if (huart == &huart6) {
if (FifoSize > Uart.UartFifo.Cnt)
{
Uart.UartFifo.RxBuf[Uart.UartFifo.In] = rx_byte;
if(++Uart.UartFifo.In >= FifoSize)
{
Uart.UartFifo.In = 0;
}
++Uart.UartFifo.Cnt;
}
IM600测量的Z轴姿态角范围为-180~180,实现对小车姿态角的获取后,就可用通过PID算法来对小车旋转角度进行控制,实现角度环的PID算法函数如下:
/****************************************
* 作用:角度环PID计算
* 参数:PID参数结构体地址;目标值;反馈值
* 返回值:无
* ****************************************/
float Angle_PID_Realize(PID* pid, float target, float feedback)//一次PID计算
{
if(target-feedback>180){
feedback+=360;
}else if(target-feedback<-180){
feedback-=360;
}
if(pid->err < 0.1 && pid->err > -0.1) pid->err = 0;//pid死区
pid->err = target - feedback;
pid->integral += pid->err;
if(pid->ki * pid->integral < -pid->maxIntegral) pid->integral = -pid->maxIntegral / pid->ki;//积分限幅
else if(pid->ki * pid->integral > pid->maxIntegral) pid->integral = pid->maxIntegral / pid->ki;
pid->output = (pid->kp * pid->err) + (pid->ki * pid->integral) + (pid->kd * (pid->err - pid->lastErr));//全量式PID
//输出限幅
if(pid->output > pid->maxOutput) pid->output = pid->maxOutput;
if(pid->output < -pid->maxOutput) pid->output = -pid->maxOutput;
pid->lastErr = pid->err;
return pid->output;
}
定时器代码:
uint8_t time1=0;
short encoder_now1=0;
short encoder_now2=0;
short encoder_now3=0;
void Angle_Speed_Car_Set(float Angle_Speed_Car);
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if (htim == (&htim9))//1ms
{
encoder_now1=(short )(__HAL_TIM_GET_COUNTER(ENCODER1));
encoder_now2=(short )(__HAL_TIM_GET_COUNTER(ENCODER2));
encoder_now3=(short )(__HAL_TIM_GET_COUNTER(ENCODER3));
__HAL_TIM_SET_COUNTER(ENCODER1,0);
__HAL_TIM_SET_COUNTER(ENCODER2,0);
__HAL_TIM_SET_COUNTER(ENCODER3,0);
motorA.speed=encoder_now1/(4*74.8*360)*1000*60; // //测量电机当前速度,rpm
motorB.speed=encoder_now2/(4*74.8*360)*1000*60; //rpm
motorC.speed=encoder_now3/(4*74.8*360)*1000*60; //rpm
time1++;
if (time1==10){//10ms进行一次PID计算
time1=0;
if (Target_Angle>180){
Target_Angle=-180;
}else if(Target_Angle<-180){
Target_Angle=180;
}
printf("MOTOR:%.2f,%.2f,%.2f,%.2f,%d,%d\n", pid_position.kp,pid_position.ki ,Target_Angle, angle_Car,rock_stick_RY,Key1);//以FireWater协议的要求向vofa+发送数据,以打印数据波型
Angle_PID_Realize(&pid_angle, Target_Angle, angle_Car);//角度环
Angle_Speed_Car_Set(pid_angle.output);//角度环输出
Speed_PID_Realize(&pid_speed,Target_Speed_A,motorA.speed);//速度环
Speed_PID_Realize(&pid_speed_B,Target_Speed_B,motorB.speed);
Speed_PID_Realize(&pid_speed_C,Target_Speed_C,motorC.speed);
MotorA_Run(pid_speed.output);//PWM波输出
MotorB_Run(pid_speed_B.output);
MotorC_Run(pid_speed_C.output);
}
}
}
void Angle_Speed_Car_Set(float Angle_Speed_Car){//根据角度环的输出设定三个电机的速度
Target_Speed_A=Angle_Speed_Car*0.117/(2*PI)*60;
Target_Speed_B=Angle_Speed_Car*0.117/(2*PI)*60;
Target_Speed_C=Angle_Speed_Car*0.117/(2*PI)*60;
}
调试过程:
角度环PID各项参数初始值设置为0,先调节P项是小车的响应速度达到一个较好的水平。(此处我使用与1,2中一样的调参通讯协议,使用原来速度环参数的发送控件来调试):
此时有一定的超调,因此调节D项抑制超调:
此时已经实现实现对小车旋转角度的精确控制。
2.全向轮底盘解算
三轮全向轮能够在任何方向平移,其结构简单,是一种全驱动系统,能够进行运动学分解。有三个输入速度( v 1 v_1 v1、 v 2 v_2 v2 、 v 3 v_3 v3),和三个输出速度( v x v_x vx, v y v_y vy, v θ v_θ vθ),三轮全向轮运动模型在世界坐标系的示意图如下:
由于三轮全向模型为全驱动模型,可进行运动分解,则对三个方向分解计算:
1.当小车仅在 X R X_R XR方向进行平移动时:
{
v
1
=
0
v
2
=
−
s
i
n
6
0
∘
×
v
x
v
3
=
s
i
n
6
0
∘
×
v
x
\begin{cases} v_1&= 0\\v_2&= -sin60^\circ\times v_x\\v_3&= sin60^\circ\times v_x\\\end{cases}
⎩
⎨
⎧v1v2v3=0=−sin60∘×vx=sin60∘×vx
2.当小车仅在
Y
R
Y_R
YR方向进行平移运动时:
{
v
1
=
v
y
v
2
=
−
c
o
s
6
0
∘
×
v
y
v
3
=
−
c
o
s
6
0
∘
×
v
y
\begin{cases} v_1&= v_y\\v_2&= -cos60^\circ\times v_y\\v_3&= -cos60^\circ\times v_y\\\end{cases}
⎩
⎨
⎧v1v2v3=vy=−cos60∘×vy=−cos60∘×vy
3.当小车仅进行旋转运动时:
{
v
1
=
v
θ
×
d
v
2
=
v
θ
×
d
v
3
=
v
θ
×
d
\begin{cases} v_1&= v_θ\times d\\v_2&= v_θ\times d\\v_3&= v_θ\times d\\\end{cases}
⎩
⎨
⎧v1v2v3=vθ×d=vθ×d=vθ×d
将上述运动分解进行合成可以得到如下表达式:
{
v
1
=
v
y
+
v
θ
×
d
v
2
=
−
s
i
n
6
0
∘
×
v
x
−
c
o
s
6
0
∘
×
v
y
+
v
θ
×
d
v
3
=
s
i
n
6
0
∘
×
v
x
−
c
o
s
6
0
∘
×
v
y
+
v
θ
×
d
\begin{cases} v_1&=v_y +v_θ\times d\\v_2&= -sin60^\circ\times v_x-cos60^\circ\times v_y+ v_θ\times d\\v_3&= sin60^\circ\times v_x-cos60^\circ\times v_y+v_θ\times d\\\end{cases}
⎩
⎨
⎧v1v2v3=vy+vθ×d=−sin60∘×vx−cos60∘×vy+vθ×d=sin60∘×vx−cos60∘×vy+vθ×d
代码实现:
#define X_PARAMETER (0.5f)
#define Y_PARAMETER (sqrt(3)/2.f)
#define L_PARAMETER (0.117f)
/**************************************************************************
函数功能:小车运动数学模型
入口参数:X Y 轴速度和角速度
返回 值:无
**************************************************************************/
void Kinematic_Analysis(float Vx,float Vy,float V_angle)
{
Target_Speed_C = Vx + L_PARAMETER*V_angle/(2*PI)*60;
Target_Speed_A = -X_PARAMETER*Vx + Y_PARAMETER*Vy + L_PARAMETER*V_angle/(2*PI)*60;
Target_Speed_B = -X_PARAMETER*Vx - Y_PARAMETER*Vy + L_PARAMETER*V_angle/(2*PI)*60;
}
当要小车向某个方向运动时,只需将该速度按 X R X_R XR和 Y R Y_R YR方向分解为 v x v_x vx, v y v_y vy后输入该函数,就可以使三个电机的速度设置为合适的值,输入的V_angle可以使小车以指定的速度旋转。
3.小车运动控制和基于角度环的航向角修正
我使用PS2手柄来实现对全向轮小车运动的遥控:
PS2手柄
PS2手柄接收器
将PS2手柄接收器连接到主控板上,配置PB0,PB1,PB2,PB10引脚分别为PS2手柄接收器的DAT,CMD,CS,CLK端口,实现PS2手柄与主控板通讯的代码可见于附件全向轮小车工程中的PS2.c和PS2.h中(参考博客基于STM32HAL库的PS2遥控手柄)。调试后发现该PS2手柄的摇杆存在严重的漂移现象,无法使用,因此我决定使用四个方向键来控制小车的前后左右的运动,并使用右侧四个按键来调节小车的速度,使用肩键L1,R1来切换小车原地旋转模式和运动模式。主循环中实现PS2手柄遥控小车运动和速度调节的代码如下:
PS2_ReadData(); //获取数据
Key1 = PS2_DataKey(); //获取手柄按键数据
if (Key1==11){//L1 小车原地旋转模式
flag=-1;//小车模式标志位
Cmd_05();//切换模式时清零IM600获取的Z轴姿态角
} else if(Key1==12){//R1 小车原地旋转模式和运动模式
Cmd_05();//切换模式时清零IM600获取的Z轴姿态角
Target_Angle=0;//设定角度环的目标值为0,从而通过角度环实现航向角修正,保证小车的运动方向稳定
flag=1;
}
switch (Key1) {
case 5:direction=FORWARD;break;//前进方向键
case 6:direction=RIGHT;break;//前进方向键
case 7:direction=BACK;break;//前进方向键
case 8:direction=LEFT;break;//前进方向键
case 4:Target_Speed=0;break;//start使小车急停
case 10:Target_Angle+=-90;break;//L2键使小车逆时针旋转90度
case 9:Target_Angle+=90;break;//R2键使小车逆时针旋转90度
case 13:Target_Speed = 20;break;//速度档位调节,rpm
case 14:Target_Speed = 40;break;
case 15:Target_Speed = 60;break;
case 16:Target_Speed = 80;break;
}
PS2_ClearData(); //清除手柄按键数据数据
HAL_Delay(100);
定时器中断的代码:
if (htim == (&htim9))//1ms
{
encoder_now1=(short )(__HAL_TIM_GET_COUNTER(ENCODER1));
encoder_now2=(short )(__HAL_TIM_GET_COUNTER(ENCODER2));
encoder_now3=(short )(__HAL_TIM_GET_COUNTER(ENCODER3));
__HAL_TIM_SET_COUNTER(ENCODER1,0);
__HAL_TIM_SET_COUNTER(ENCODER2,0);
__HAL_TIM_SET_COUNTER(ENCODER3,0);
motorA.speed=encoder_now1/(4*74.8*360)*1000*60; //测量电机当前速度,rpm
motorB.speed=encoder_now2/(4*74.8*360)*1000*60; //rpm
motorC.speed=encoder_now3/(4*74.8*360)*1000*60; //rpm
time1++;
if (time1==10){//10ms进行一次PID计算
time1=0;
if (Target_Angle>180){
Target_Angle=-180;
}else if(Target_Angle<-180){
Target_Angle=180;
}
printf("MOTOR:%.2f,%.2f,%.2f,%.2f,%d,%d\n", pid_position.kp,pid_position.ki ,Target_Angle, angle_Car,rock_stick_RY,Key1);//以FireWater协议的要求向vofa+发送数据,以打印数据波型
if(flag==-1) {//小车原地旋转模式
Angle_PID_Realize(&pid_angle, Target_Angle, angle_Car);//角度环
Angle_Speed_Car_Set(pid_angle.output);//根据角度环的输出设定三个电机的速度
} else if(flag==1){//小车运动模式
Angle_PID_Realize(&pid_angle, Target_Angle, angle_Car);//基于角度环的航向角修正
switch (direction) {//通过全向轮运动模型设定不同运动状态下小车三电机的速度
case FORWARD:Kinematic_Analysis(0, Target_Speed, pid_angle.output);break;
case LEFT:Kinematic_Analysis(-Target_Speed, 0, pid_angle.output);break;
case RIGHT:Kinematic_Analysis(Target_Speed, 0, pid_angle.output);break;
case BACK:Kinematic_Analysis(0, -Target_Speed, pid_angle.output);break;
}
}
Speed_PID_Realize(&pid_speed,Target_Speed_A,motorA.speed);//速度环
Speed_PID_Realize(&pid_speed_B,Target_Speed_B,motorB.speed);
Speed_PID_Realize(&pid_speed_C,Target_Speed_C,motorC.speed);
MotorA_Run(pid_speed.output);//PWM波输出
MotorB_Run(pid_speed_B.output);
MotorC_Run(pid_speed_C.output);
}
}
经过调试后可以实现小车运动控制和基于角度环的航向角修正。
if (htim == (&htim9))//1ms
{
encoder_now1=(short )(__HAL_TIM_GET_COUNTER(ENCODER1));
encoder_now2=(short )(__HAL_TIM_GET_COUNTER(ENCODER2));
encoder_now3=(short )(__HAL_TIM_GET_COUNTER(ENCODER3));
__HAL_TIM_SET_COUNTER(ENCODER1,0);
__HAL_TIM_SET_COUNTER(ENCODER2,0);
__HAL_TIM_SET_COUNTER(ENCODER3,0);
motorA.speed=encoder_now1/(4*74.8*360)*1000*60; //测量电机当前速度,rpm
motorB.speed=encoder_now2/(4*74.8*360)*1000*60; //rpm
motorC.speed=encoder_now3/(4*74.8*360)*1000*60; //rpm
time1++;
if (time1==10){//10ms进行一次PID计算
time1=0;
if (Target_Angle>180){
Target_Angle=-180;
}else if(Target_Angle<-180){
Target_Angle=180;
}
printf("MOTOR:%.2f,%.2f,%.2f,%.2f,%d,%d\n", pid_position.kp,pid_position.ki ,Target_Angle, angle_Car,rock_stick_RY,Key1);//以FireWater协议的要求向vofa+发送数据,以打印数据波型
if(flag==-1) {//小车原地旋转模式
Angle_PID_Realize(&pid_angle, Target_Angle, angle_Car);//角度环
Angle_Speed_Car_Set(pid_angle.output);//根据角度环的输出设定三个电机的速度
} else if(flag==1){//小车运动模式
Angle_PID_Realize(&pid_angle, Target_Angle, angle_Car);//基于角度环的航向角修正
switch (direction) {//通过全向轮运动模型设定不同运动状态下小车三电机的速度
case FORWARD:Kinematic_Analysis(0, Target_Speed, pid_angle.output);break;
case LEFT:Kinematic_Analysis(-Target_Speed, 0, pid_angle.output);break;
case RIGHT:Kinematic_Analysis(Target_Speed, 0, pid_angle.output);break;
case BACK:Kinematic_Analysis(0, -Target_Speed, pid_angle.output);break;
}
}
Speed_PID_Realize(&pid_speed,Target_Speed_A,motorA.speed);//速度环
Speed_PID_Realize(&pid_speed_B,Target_Speed_B,motorB.speed);
Speed_PID_Realize(&pid_speed_C,Target_Speed_C,motorC.speed);
MotorA_Run(pid_speed.output);//PWM波输出
MotorB_Run(pid_speed_B.output);
MotorC_Run(pid_speed_C.output);
}
}
经过调试后可以实现小车运动控制和基于角度环的航向角修正。