ROS小车（1）：小车测速和PID调速

ROS小车（1）：小车测速和PID调速

StarHui Lv3
  2025-09-14 14:50:51 2025-09-14 14:50:51 Created   2025-09-14 14:50:52 2025-09-14 14:50:52 Updated   6.4k Words  28 Mins  

前言

之前学ROS的时候，一直是看视频，学一下知识点，无法实操。后面老师那边买的有亚博的ROS小车，但是呢感觉已经做好了，就不知道该干什么了。之前断断续续想过做一个，现在从学校出来后，有条件做这个了。于是就开始从头，自己做一个ROS小车，从硬件（绘制原理图、PCB）到下位机（嵌入式软件）到上位机（SLAM等）。目前我也只是学过前两个部分，SLAM部分还没有接触，每天只能下班后搞这个，时间有限，可能更新有点慢，见谅。

好了，接下来进入正题

架构

做之前，一定要想明白这一套东西是如何工作的。

首先是上位机，负责实现定位、建图等任务 可以通过SLAM来实现；然后就是进行路径决策和规划，比如选取最短路径，利用dijkstra等算法来实现

然后是下位机，负责实现对小车底盘的控制，前进、后退、转向等操作。

最后是通过通信，架起上下位机的桥梁。上位机是大脑，下位机是执行机构，做出决策后，需要下位机来实现。可以通过串口通信实现

ROS小车架构

硬件准备

首先就是硬件准备，需要的东西有12V锂电池、带霍尔编码的减速电机、六角联轴器、电机驱动模块、轮子、37mm电机支架、底盘、STM32F103VET6、树莓派/jetson、激光雷达、摄像头、手柄、杜邦线、螺丝若干等等。

提一嘴，用杜邦线把这些模块连起来，太不美观了，后面会重新绘制PCB的！

目前不涉及到上位机，所以暂时不需要 树莓派/jetson 激光雷达这些，可以先不买，有点小贵哈哈哈。

驱动电机

测速、PID调速的前提是需要先把电机转起来，所以第一步我们要做的是，使用stm32使用tb6612电机驱动模块驱动电机，先不考虑编码问题，转起来再说。

MG513P3012V

简单说一下，这个电机是带霍尔编码器的，可以拿来测速。减速比是1：30，额定电压12V。后面测速的时候再详细介绍

四路电机驱动模块带稳压版本

电机驱动模块。我买的是轮趣科技的tb6612四路电机驱动模块带稳压版本的。

就是两个tb6612fng芯片（一个芯片可以驱动两个电机）+ 开关 + 稳压模块。

TB6612端口定义
TB6612真值表

以上两种图片来自买这个驱动模块提供的文档，就是看一下大致接线，通过真值表可以看到如何正转、反转、停止等操作。

接下来就看一下接线。

电机与电机驱动模块之间，我用的是6p的2.54端子线，但由于线序不一样，需要自己调整输出端口的线哈 或者调整代码哈。

接线说明

首先就是初始化四个电机的信号输入端IN1 IN2和STBY

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
//电机初始化
//E2 AIN1,E3 AIN2; E4 BIN1, E5 BIN2; E6 CIN1, D8 CIN2; D9 DIN1, D10 DIN2; D11 STBY
void Motor_Init(void)
{
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD | RCC_APB2Periph_GPIOE, ENABLE);
	
	//电机正负线
	GPIO_InitTypeDef GPIO_InitStructure;
	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_6;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOE, &GPIO_InitStructure);	
	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8| GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_11;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOD, &GPIO_InitStructure);	
	
	GPIO_WriteBit(GPIOD,GPIO_Pin_11,Bit_SET);//STBY高电平正常工作
}

接下来需要去初始定时器，使用pwm来驱动电机

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
void Timer_Init(void)
{
	//PWMA、PWMB、PWMC、PWMD
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOE | RCC_APB2Periph_TIM1 | RCC_APB2Periph_AFIO, ENABLE);
	
	GPIO_InitTypeDef GPIO_InitStructure;
	
	GPIO_PinRemapConfig(GPIO_FullRemap_TIM1,ENABLE);//TIM1映射

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_11 | GPIO_Pin_13 | GPIO_Pin_14;	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOE, &GPIO_InitStructure);	
	
	//pwm频率100k
	TIM_TimeBaseInitTypeDef TIM_TimeBase_InitStructure;
	TIM_TimeBase_InitStructure.TIM_Prescaler = 0;
	TIM_TimeBase_InitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBase_InitStructure.TIM_Period = 7199;
	TIM_TimeBase_InitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBase_InitStructure.TIM_RepetitionCounter =0; 
	TIM_TimeBaseInit(TIM1,&TIM_TimeBase_InitStructure);
	
	TIM_OCInitTypeDef TIM_OCInitStructure;
	TIM_OCStructInit(&TIM_OCInitStructure);
	
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCPolarity_High;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	
	TIM_OC1Init(TIM1,&TIM_OCInitStructure);
	TIM_OC2Init(TIM1,&TIM_OCInitStructure);
	TIM_OC3Init(TIM1,&TIM_OCInitStructure);
	TIM_OC4Init(TIM1,&TIM_OCInitStructure);
	
	TIM_Cmd(TIM1,ENABLE);
	TIM_CtrlPWMOutputs(TIM1,ENABLE)
}

关于这个电机使用的PWM频率，我查了一些资料，仍然没有说明电机驱动的时候pwm的频率是多少，现在的100k是看轮趣提供的demo里面的，如果有哪位知道，可以跟我说一下哈

此时设置IN1 IN2 pwm的值，小车就可以动了

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
//设置电机pwm，参数为 左前轮pwm 右前轮pwm 左后轮pwm 右后轮pwm
void Motor_Run(int motor1_pwm, int motor2_pwm, int motor3_pwm, int motor4_pwm)
{
	if(motor1_pwm > 0)//前进,AIN1 1, AIN2 0
	{
		GPIO_WriteBit(GPIOE,GPIO_Pin_2,Bit_SET);
		GPIO_WriteBit(GPIOE,GPIO_Pin_3,Bit_RESET);
	}
	else//后退,AIN1 0, AIN2 1
	{
		GPIO_WriteBit(GPIOE,GPIO_Pin_2,Bit_RESET);
		GPIO_WriteBit(GPIOE,GPIO_Pin_3,Bit_SET);
		motor1_pwm = -motor1_pwm;
	}
	
	if(motor2_pwm > 0)//前进,BIN1 1, BIN2 0
	{
		GPIO_WriteBit(GPIOE,GPIO_Pin_4,Bit_SET);
		GPIO_WriteBit(GPIOE,GPIO_Pin_5,Bit_RESET);
	}
	else//后退,BIN1 0, BIN2 1
	{
		GPIO_WriteBit(GPIOE,GPIO_Pin_4,Bit_RESET);
		GPIO_WriteBit(GPIOE,GPIO_Pin_5,Bit_SET);
		motor2_pwm = -motor2_pwm;
	}
	
	if(motor3_pwm > 0)//前进,CIN1 1, CIN2 0
	{
		GPIO_WriteBit(GPIOE,GPIO_Pin_6,Bit_SET);
		GPIO_WriteBit(GPIOD,GPIO_Pin_8,Bit_RESET);
	}
	else//后退,CIN1 0, CIN2 1
	{
		GPIO_WriteBit(GPIOE,GPIO_Pin_6,Bit_RESET);
		GPIO_WriteBit(GPIOD,GPIO_Pin_8,Bit_SET);
		motor3_pwm = -motor3_pwm;
	}
	
	if(motor4_pwm > 0)//前进,DIN1 1, DIN2 0
	{
		GPIO_WriteBit(GPIOD,GPIO_Pin_9,Bit_SET);
		GPIO_WriteBit(GPIOD,GPIO_Pin_10,Bit_RESET);
	}
	else//后退,DIN1 0, DIN2 1
	{
		GPIO_WriteBit(GPIOD,GPIO_Pin_9,Bit_RESET);
		GPIO_WriteBit(GPIOD,GPIO_Pin_10,Bit_SET);
		motor4_pwm = -motor4_pwm;
	}
	
	//防止越界
	if(motor1_pwm > 7200)
		motor1_pwm = 7200;
	if(motor2_pwm > 7200)
		motor2_pwm = 7200;
	if(motor3_pwm > 7200)
		motor3_pwm = 7200;
	if(motor4_pwm > 7200)
		motor4_pwm = 7200;
	
	//pwm
	TIM_SetCompare1(TIM1,motor1_pwm);
	TIM_SetCompare2(TIM1,motor2_pwm);
	TIM_SetCompare3(TIM1,motor3_pwm);
	TIM_SetCompare4(TIM1,motor4_pwm);
}

现在在int main函数里面初始化一下，使用Motor_Run函数即可控制电机运动了，如下

1
2
3
4
5
6
7
8
9
10
11
int main(void)
{
	Motor_Init();
	Timer_Init();

    Motor_Run(2000, 2000, 2000, 2000);

	while (1)
	{
	}
}

注意

由于是四轮小车，左右侧轮子为轴对称关系，如果想要小车前进，必须让左侧轮子正转，右侧轮子反转，这样才能前进。

就是MotorA、MotorC的IN1 = 1 IN2 = 0正转，MotorB、MotorD的IN1 = 0，IN2 = 1反转，再给pwm值，这样就可以前进了

为了方便控制，电机驱动模块的内部把MotorB、MotorD的电机正负线调换了一下，这样四个轮子就方便控制了，IN1、IN2都可以同时给一样的值了。

MotorA、MotorB、MotorC、MotorD的IN1 = 1，IN2 = 0，这样小车就前进了。不再是之前那种不同侧的IN1 IN2的值不同。

电机驱动模块原理图

可以看到图中红框的地方，HDF-2.54_1*6的端子，MotorA和MotorC 的 1、6连接的网络与MotorB、MotorD不同。根据MG513电机的线序，电机线-、编码器电源、编码器A相、编码器B相、编码器地线、电机线+，由此得出，电机正负线被调换了，因此4个电机IN1 IN2给相同的值，小车可以前进或者后退。

电机测速

再次回顾一下MG513P3012V电机。30表示的是减速比。

减速电机内部有两个齿轮，一个是转子连接轴上的齿轮，一个是输出主轴上的齿轮。转子转动N圈，主轴转动一圈，这就实现了降低转速。比如我们常见的1:30减速比的减速电机，就是转子转动30圈，主轴转动一圈。如此一来，就实现了降低转速，增加扭矩的目的。减速比就是小齿轮数比大齿轮数的比值。 这个减速电机上面有一个霍尔编码器，我们来了解一下。

编码器是把角位移或直线位移转换成电信号的一种装置。

霍尔编码器由码盘和霍尔元件组成。霍尔码盘与电机主轴同轴，码盘上等分的分布有多个磁极，电机转动时，霍尔元件会输出若干个脉冲信号，我们正是利用这些脉冲信号实现电机的测速和电机转向的判断。这个电机的线数为13，所以霍尔编码器的码盘旋转一圈，会产生13个脉冲。

我们正是通过检测霍尔编码器输出的脉冲信号来测速。通常会有三相输出，A、B和Z。A和B的输出是正交的。Z是用来标记旋转一周的起始位置，我们通常不使用。

如何判断电机转向？

我们通过A相出现脉冲时检测B相电平来判断电机旋转方向。

  • A检测到上升沿脉冲时，B为低电平，正转；
  • A检测到上升沿脉冲时，B为高电平，反转；

如何判断电机转速？

我们通过检测单位时间内产生的脉冲数来确定电机转速。为什么可以这么做？因为电机转动一圈产生的脉冲数是确定的。比如我们有一个减速比为1:30的减速电机，霍尔编码器的线数为13。那么霍尔码盘旋转一圈，产生13个脉冲，霍尔码盘旋转30圈，电机主轴旋转一圈。综上所述，电机主轴旋转一圈会产生13 * 30 = 390个脉冲。注意，这里是只检测A相的上升沿脉冲，电机旋转一圈有390个脉冲。

如果我们A相和B相都检测，而且不止检测上升沿脉冲，也检测下降沿脉冲，那么此时霍尔码盘旋转一圈会产生四倍于之前的脉冲数，也就是390*4。这就是所谓的四倍频。利用四倍频可以提高检测精度。

四个轮都使用双倍频计算转速

可以使用STM32的定时器的编码器模式来进行此操作，配置好后使用TIM_GetCounter函数即可获得计数值

关于encoder模式，详细可以看这个电机控制基础——定时器编码器模式使用与转速计算

首先需要4个通用/高级定时器，大家选好IO口即可。

下面是选择的IO口，以及接线

接线
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
//TIM1 PWM
//TIM2 TIM3 TIM4 TIM8 encoder
void Timer_Init(void)
{
	//PWMA、PWMB、PWMC、PWMD
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOE | RCC_APB2Periph_TIM1 | RCC_APB2Periph_AFIO, ENABLE);
	
	GPIO_InitTypeDef GPIO_InitStructure;
	
	GPIO_PinRemapConfig(GPIO_FullRemap_TIM1,ENABLE);//TIM1映射

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_11 | GPIO_Pin_13 | GPIO_Pin_14;	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOE, &GPIO_InitStructure);	
	
	//pwm频率100k
	TIM_TimeBaseInitTypeDef TIM_TimeBase_InitStructure;
	TIM_TimeBase_InitStructure.TIM_Prescaler = 0;
	TIM_TimeBase_InitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBase_InitStructure.TIM_Period = 7199;
	TIM_TimeBase_InitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBase_InitStructure.TIM_RepetitionCounter =0; 
	TIM_TimeBaseInit(TIM1,&TIM_TimeBase_InitStructure);
	
	TIM_OCInitTypeDef TIM_OCInitStructure;
	TIM_OCStructInit(&TIM_OCInitStructure);
	
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCPolarity_High;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	
	TIM_OC1Init(TIM1,&TIM_OCInitStructure);
	TIM_OC2Init(TIM1,&TIM_OCInitStructure);
	TIM_OC3Init(TIM1,&TIM_OCInitStructure);
	TIM_OC4Init(TIM1,&TIM_OCInitStructure);
	
	TIM_Cmd(TIM1,ENABLE);
	TIM_CtrlPWMOutputs(TIM1,ENABLE);
	
	//encoder
	//TIM2 高级/通用定时器只有CH1 CH2有编码器接口
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
	GPIO_Init(GPIOA, &GPIO_InitStructure);	
	
	TIM_TimeBase_InitStructure.TIM_Prescaler = 0;
	TIM_TimeBase_InitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBase_InitStructure.TIM_Period = 65535;
	TIM_TimeBase_InitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBase_InitStructure.TIM_RepetitionCounter =0; 
	TIM_TimeBaseInit(TIM2,&TIM_TimeBase_InitStructure);
	
	TIM_ICInitTypeDef TIM_ICInitStructure;
	TIM_ICStructInit(&TIM_ICInitStructure);
	
	TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
	TIM_ICInitStructure.TIM_ICFilter = 0x0F;
	TIM_ICInit(TIM2,&TIM_ICInitStructure);
	
	TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
	TIM_ICInitStructure.TIM_ICFilter = 0x0F;
	TIM_ICInit(TIM2,&TIM_ICInitStructure);
	
	TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
	
	TIM_Cmd(TIM2,ENABLE);
		
	//TIM3
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
	GPIO_Init(GPIOA, &GPIO_InitStructure);	
	
	TIM_TimeBase_InitStructure.TIM_Prescaler = 0;
	TIM_TimeBase_InitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBase_InitStructure.TIM_Period = 65535;
	TIM_TimeBase_InitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBase_InitStructure.TIM_RepetitionCounter =0; 
	TIM_TimeBaseInit(TIM3,&TIM_TimeBase_InitStructure);
	
	TIM_ICStructInit(&TIM_ICInitStructure);
	
	TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
	TIM_ICInitStructure.TIM_ICFilter = 0x0F;
	TIM_ICInit(TIM3,&TIM_ICInitStructure);
	
	TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
	TIM_ICInitStructure.TIM_ICFilter = 0x0F;
	TIM_ICInit(TIM3,&TIM_ICInitStructure);
	
	TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
	
	TIM_Cmd(TIM3,ENABLE);
	
	//TIM4
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
	GPIO_Init(GPIOB, &GPIO_InitStructure);	
	
	TIM_TimeBase_InitStructure.TIM_Prescaler = 0;
	TIM_TimeBase_InitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBase_InitStructure.TIM_Period = 65535;
	TIM_TimeBase_InitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBase_InitStructure.TIM_RepetitionCounter =0; 
	TIM_TimeBaseInit(TIM4,&TIM_TimeBase_InitStructure);
	
	TIM_ICStructInit(&TIM_ICInitStructure);
	
	TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
	TIM_ICInitStructure.TIM_ICFilter = 0x0F;
	TIM_ICInit(TIM4,&TIM_ICInitStructure);
	
	TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
	TIM_ICInitStructure.TIM_ICFilter = 0x0F;
	TIM_ICInit(TIM4,&TIM_ICInitStructure);
	
	TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
	
	TIM_Cmd(TIM4,ENABLE);
	
	//TIM8
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
	GPIO_Init(GPIOC, &GPIO_InitStructure);	
	
	TIM_TimeBase_InitStructure.TIM_Prescaler = 0;
	TIM_TimeBase_InitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBase_InitStructure.TIM_Period = 65535;
	TIM_TimeBase_InitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBase_InitStructure.TIM_RepetitionCounter =0; 
	TIM_TimeBaseInit(TIM8,&TIM_TimeBase_InitStructure);
	
	TIM_ICStructInit(&TIM_ICInitStructure);
	
	TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
	TIM_ICInitStructure.TIM_ICFilter = 0x0F;
	TIM_ICInit(TIM8,&TIM_ICInitStructure);
	
	TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
	TIM_ICInitStructure.TIM_ICFilter = 0x0F;
	TIM_ICInit(TIM8,&TIM_ICInitStructure);
	
	TIM_EncoderInterfaceConfig(TIM8, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
	
	TIM_Cmd(TIM8,ENABLE);
}

接下来使用TIM_GetCounter(TIMx)函数就可以获取脉冲数了。

为了方便测速，用过基本定时器，定个10m的定时中断，用于计算速度

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
//TIM6
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE);
	TIM_TimeBase_InitStructure.TIM_Prescaler = 72 - 1;
	TIM_TimeBase_InitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBase_InitStructure.TIM_Period = 10000 - 1;//1us * 10000 = 10ms
	TIM_TimeBase_InitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBase_InitStructure.TIM_RepetitionCounter =0; 
	TIM_TimeBaseInit(TIM6,&TIM_TimeBase_InitStructure);

	TIM_ITConfig(TIM6, TIM_IT_Update, ENABLE);
	TIM_ClearFlag(TIM6, TIM_FLAG_Update);
	
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);	
	
	NVIC_InitTypeDef NVIC_InitStructure;						//定义结构体变量
	NVIC_InitStructure.NVIC_IRQChannel = TIM6_IRQn;				//选择配置NVIC的TIM6线
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;				//指定NVIC线路使能
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;	//指定NVIC线路的抢占优先级为2
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;			//指定NVIC线路的响应优先级为1
	NVIC_Init(&NVIC_InitStructure);	
	
	TIM_Cmd(TIM6,ENABLE);

测速公式为：这款电机的转一圈的脉冲数是13，减速比是1：30，由于是4倍频，A、B相的上升沿、下降沿都产生脉冲了，所以小车转一圈的产生的脉冲数为 13 * 30 * 4

那么 编码器的值/ 4 / 13 / 30 就可以得到小车前进的圈数，此时再除以这段的时间间隔0.01s的话，最后的单位就是 rad/s。

看大家对单位的需求，这里我取的单位是 m / s 也就是在前面的基础上乘以轮胎的周长（2πR），也就能将rad/s转换为m/s了

最终完整的公式为 speed = encoder_value / 13 / 30 / 4 / 0.01 * 2πR

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
//10ms定时中断，计算速度
void TIM6_IRQHandler(void)
{
	if (TIM_GetITStatus(TIM6, TIM_IT_Update) == SET)
	{	
		//读取编码器值
		Motor1_Encoder_Value = TIM_GetCounter(TIM2);
		Motor2_Encoder_Value = TIM_GetCounter(TIM3);
		Motor3_Encoder_Value = TIM_GetCounter(TIM4);
		Motor4_Encoder_Value = TIM_GetCounter(TIM8);
		
//		Serial_Printf("Motor1_Encoder_Value = %d\r\n",Motor1_Encoder_Value);
//		Serial_Printf("Motor2_Encoder_Value = %d\r\n",Motor2_Encoder_Value);
//		Serial_Printf("Motor3_Encoder_Value = %d\r\n",Motor3_Encoder_Value);
//		Serial_Printf("Motor4_Encoder_Value = %d\r\n",Motor4_Encoder_Value);
		
		//计数器清零
		TIM_SetCounter(TIM2,0);
		TIM_SetCounter(TIM3,0);
		TIM_SetCounter(TIM4,0);
		TIM_SetCounter(TIM8,0);
		
		/*
		4倍频，减速比1：30，线数13，所以主轴转一圈为 13 * 30 * 4, 除以1560得到圈数
		圈数乘以轮胎周长（PI * D） = 行进的距离，除以时间
		最后的单位就是 mm/s
		*/
		Motor1_Speed = (float)Motor1_Encoder_Value * 3.1415926 * 65 / 1560 / 0.01;
		Motor2_Speed = (float)Motor2_Encoder_Value * 3.1415926 * 65 / 1560 / 0.01;
		Motor3_Speed = (float)Motor3_Encoder_Value * 3.1415926 * 65 / 1560 / 0.01;
		Motor4_Speed = (float)Motor4_Encoder_Value * 3.1415926 * 65 / 1560 / 0.01;
		
		Serial_Printf("Motor1_Speed = %.2f m/s\r\n",Motor1_Speed);
		Serial_Printf("Motor2_Speed = %.2f m/s\r\n",Motor2_Speed);
		Serial_Printf("Motor3_Speed = %.2f m/s\r\n",Motor3_Speed);
		Serial_Printf("Motor4_Speed = %.2f m/s\r\n",Motor4_Speed);

		TIM_ClearITPendingBit(TIM6, TIM_IT_Update);
	}
}

其中的Serial_Printf()函数是copy的江科大的STM32入门视频的串口发送，大家可以替换为自己的串口发送函数即可。

注意

只有高级/通用定时器的通道1、通道2才可以使用编码器模式。基本定时器是不可以的，这也是我选择使用STM32F103VET6的原因

PID调速

理论

PID这个词，大家可能比较陌生，但是它的应用在我们的生活中无处不在。比如空调的温度控制、无人机的精准悬停、汽车的定速巡航等等。

就拿汽车的定速巡航来说，假设我想把汽车的速度维持在60km/h小时。应该怎么做呢？

可能大多数人第一反应就是写一个判断语句，当速度大于60时，关闭发动机；小于的时候，发动机一直在运转。但是这样真的合理吗？答案是不行的。

PS：这样的控制方式是开环的（open-loop）意思就是不将控制的结果反馈回来影响当前控制的系统。开环就相当于单向操作，我们给控制器一个值，控制器就按这个值操作控制。也就是只控制输出，不计后果的控制。打个比方，开环相当于开水龙头，你拧到什么位置，水龙头就出多少大小的水，没有反馈信号。

当汽车速度大于60的时候，我们如果关闭发动机，会对汽车速度产生影响（速度变小），达不到定速。那么应该怎么办呢？我们需要一个闭环控制系统。

PS：拿这个例子说明一下，我们给汽车的输入（下达的指令，定速60）不再是一个恒定的输入，而是会随着汽车的输出（汽车的速度）而进行调整。比如速度快了降一点，慢了加点速。并不是与开环那样，指令下达后，一步到位，就不管了。

闭环控制系统是控制系统的一种类型。具体内容是指： 把控制系统输出量的一部分或全部，通过一定方法和装置反送回系统的输入端，然后将反馈信息与原输入信息进行比较，再将比较的结果施加于系统进行控制，避免系统偏离预定目标。闭环控制系统利用的是负反馈。 即是由信号正向通路和反馈通路构成闭合回路的自动控制系统，又称反馈控制系统。 而今天的主角——PID就是闭环控制系统的一种，应用最为广泛的一种。

PID（Proportional-Integral-Derivative）是一种经典的反馈控制算法，常用于工业控制系统中。它通过对系统当前状态和目标状态之间的差异进行计算，调整控制量，使系统输出尽可能接近目标值。

PID控制器由三个部分组成： - 比例（Proportional）：根据当前误差的大小，以比例系数来调整控制量。比例控制作用于减小误差，但不能消除稳态误差。 - 积分（Integral）：累积历史误差的大小，并乘以积分系数，进行控制量的调整。积分控制作用于消除稳态误差，但可能引入超调和振荡。 - 微分（Derivative）：根据误差变化的速率，乘以微分系数，进行控制量的调整。微分控制作用于抑制系统的超调和振荡，提高系统的动态响应。

\[ \mu(t)=K_{P} e(t)+K_{I} \int_{0}^{t} e(t) d t+K_{D} \frac{d e(t)}{d t} \]

  • e(t)为pid控制的输入
  • μ(t)为pid的输出和被控对象的输入
  • Kp控制器的比例系数
  • Ki控制器的积分时间，也称积分系数
  • Kd控制器的微分时间，也称微分系数

大家可以看一下下面的链接，帮助理解各个参数的作用 故事+动图，让PID只是通俗易懂

实现

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
#include "stm32f10x.h"                  // Device header
#include "pid.h"

//四个轮子pid
tpid Motor1PID;
tpid Motor2PID;
tpid Motor3PID;
tpid Motor4PID;

//初始化
void PID_Init(void)
{
	Motor1PID.target_value = 0;
	Motor1PID.actual_value = 0;
	Motor1PID.now_error = 0;
	Motor1PID.last_error = 0;
	Motor1PID.sum_error = 0;
	Motor1PID.Kp = 0;
	Motor1PID.Ki = 0;
	Motor1PID.Kd = 0;
	
	Motor2PID.target_value = 0;
	Motor2PID.actual_value = 0;
	Motor2PID.now_error = 0;
	Motor2PID.last_error = 0;
	Motor2PID.sum_error = 0;
	Motor2PID.Kp = 0;
	Motor2PID.Ki = 0;
	Motor2PID.Kd = 0;
	
	Motor3PID.target_value = 0;
	Motor3PID.actual_value = 0;
	Motor3PID.now_error = 0;
	Motor3PID.last_error = 0;
	Motor3PID.sum_error = 0;
	Motor3PID.Kp = 0;
	Motor3PID.Ki = 1;
	Motor3PID.Kd = 0;
	
	Motor4PID.target_value = 0;
	Motor4PID.actual_value = 0;
	Motor4PID.now_error = 0;
	Motor4PID.last_error = 0;
	Motor4PID.sum_error = 0;
	Motor4PID.Kp = 0;
	Motor4PID.Ki = 0;
	Motor4PID.Kd = 0;
}

//比例P调节
float P_realize(tpid* pid, float target_value, float actual_value)
{
	pid ->target_value = target_value;
	pid ->actual_value = actual_value;//存储真实值
	pid ->now_error = pid ->target_value - pid ->actual_value; //计算误差
	pid ->actual_value = pid ->now_error * pid ->Kp; //比例调节，当前误差 * Kp
	return pid ->actual_value;
}

//PI调节
float PI_realize(tpid* pid, float target_value, float actual_value)
{
	pid ->target_value = target_value;
	pid ->actual_value = actual_value;//存储真实值
	pid ->now_error = pid ->target_value - pid ->actual_value; //计算误差
	pid ->sum_error += pid ->now_error;
	pid ->actual_value = pid ->now_error * pid ->Kp + pid ->sum_error * pid ->Ki; //比例、积分调节
	return pid ->actual_value;
}

//PID
float PID_realize(tpid* pid, float target_value, float actual_value)
{
	pid ->target_value = target_value;
	pid ->actual_value = actual_value;//存储真实值
	pid ->now_error = pid ->target_value - pid ->actual_value; //计算误差
	pid ->sum_error += pid ->now_error;
	pid ->actual_value = pid ->now_error * pid ->Kp + pid ->sum_error * pid ->Ki + pid ->last_error * pid ->Kd; //比例、积分、微分调节
	pid ->last_error = pid ->now_error;
	return pid ->actual_value ;
}

其中初始化的时候，需要大家按照自己的情况来去调整PID的值

接下来看一下应该怎么用

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
//10ms定时中断，计算速度
void TIM6_IRQHandler(void)
{
	if (TIM_GetITStatus(TIM6, TIM_IT_Update) == SET)
	{	
		//读取编码器值
		Motor1_Encoder_Value = TIM_GetCounter(TIM2);
		Motor2_Encoder_Value = TIM_GetCounter(TIM3);
		Motor3_Encoder_Value = TIM_GetCounter(TIM4);
		Motor4_Encoder_Value = TIM_GetCounter(TIM8);
		
//		Serial_Printf("Motor1_Encoder_Value = %d\r\n",Motor1_Encoder_Value);
//		Serial_Printf("Motor2_Encoder_Value = %d\r\n",Motor2_Encoder_Value);
//		Serial_Printf("Motor3_Encoder_Value = %d\r\n",Motor3_Encoder_Value);
//		Serial_Printf("Motor4_Encoder_Value = %d\r\n",Motor4_Encoder_Value);
		
		//计数器清零
		TIM_SetCounter(TIM2,0);
		TIM_SetCounter(TIM3,0);
		TIM_SetCounter(TIM4,0);
		TIM_SetCounter(TIM8,0);
		
		/*
		4倍频，减速比1：30，线数13，所以主轴转一圈为 13 * 30 * 4, 除以1560得到圈数
		圈数乘以轮胎周长（PI * D） = 行进的距离，除以时间
		最后的单位就是 mm/s
		*/
		Motor1_Speed = (float)Motor1_Encoder_Value * 3.1415926 * 65 / 1560 / 0.01;
		Motor2_Speed = (float)Motor2_Encoder_Value * 3.1415926 * 65 / 1560 / 0.01;
		Motor3_Speed = (float)Motor3_Encoder_Value * 3.1415926 * 65 / 1560 / 0.01;
		Motor4_Speed = (float)Motor4_Encoder_Value * 3.1415926 * 65 / 1560 / 0.01;
		
//		Serial_Printf("Motor1_Speed = %.2f m/s\r\n",Motor1_Speed);
//		Serial_Printf("Motor2_Speed = %.2f m/s\r\n",Motor2_Speed);
//		Serial_Printf("Motor3_Speed = %.2f m/s\r\n",Motor3_Speed);
//		Serial_Printf("Motor4_Speed = %.2f m/s\r\n",Motor4_Speed);
		
		//因为2、4轮是反转，所以加了负号
		int16_t Motor1_Write_PWM = (int)(PID_realize(&Motor1PID, Motor1_Target_Speed, Motor1_Speed));
		int16_t Motor2_Write_PWM = (int)(PID_realize(&Motor2PID, -Motor2_Target_Speed, Motor2_Speed));
		int16_t Motor3_Write_PWM = (int)(PID_realize(&Motor3PID, Motor3_Target_Speed, Motor3_Speed));
		int16_t Motor4_Write_PWM = (int)(PID_realize(&Motor4PID, -Motor4_Target_Speed, Motor4_Speed));
//		Serial_Printf("Motor1_Write_PWM = %d\r\n",Motor1_Write_PWM);
//		Serial_Printf("Motor2_Write_PWM = %d\r\n",Motor2_Write_PWM);
//		Serial_Printf("Motor3_Write_PWM = %d\r\n",Motor3_Write_PWM);
//		Serial_Printf("Motor4_Write_PWM = %d\r\n",Motor4_Write_PWM);
		Motor_Run(Motor1_Write_PWM,-Motor2_Write_PWM,Motor3_Write_PWM,-Motor4_Write_PWM);

		TIM_ClearITPendingBit(TIM6, TIM_IT_Update);
	}
}

其实就是通过PID_realize函数来计算反馈后的pwm值，通过不断的调整，最后将当前速度慢慢逼近速度

调参

比较重要的就是调参了。

调参小技巧，先将I、D为0，调P比例系数。接下来调D，再调I

如何判断P调好了呢？

就是在首先会达到目标值，然后在目标值小浮动。此时尽可能增大P，让其离目标值最近。如果出现在目标值上下震荡，P就大了，需要调小

我个人对这三个参数的理解是，P就是调节比例，将当前值的比例缩放到逼近目标值去。I就是调节当前值与目标值的误差，将误差尽可能缩减到最小。I就是调节稳定下后的振荡幅度，尽可能将振荡降低。

为了方便调节，我通过串口将四个轮的速度发出来了，在电脑上使用pyserial库将其速度曲线绘制出来。

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
#include "stm32f10x.h"                  // Device header
#include "pid_parameter_tune.h"
#include <string.h>
#include <stdint.h>
#include "usart.h"

#define FRAME_HEAD1  0xAA
#define FRAME_HEAD2  0x55
#define FRAME_TAIL1  0x0D
#define FRAME_TAIL2  0x0A

// CRC16 (MODBUS) 算法
uint16_t CRC16_Modbus(uint8_t *data, uint16_t len)
{
    uint16_t crc = 0xFFFF;
    for (int pos = 0; pos < len; pos++) {
        crc ^= (uint16_t)data[pos];
        for (int i = 8; i != 0; i--) {
            if ((crc & 0x0001) != 0) {
                crc >>= 1;
                crc ^= 0xA001;
            } else
                crc >>= 1;
        }
    }
    return crc;
}

// 发送电机测速数据
void Serial_SendMotorData(float s1, float s2, float s3, float s4,
                          float t1, float t2, float t3, float t4)
{
    uint8_t buffer[64]; // 足够大
    uint8_t index = 0;

    // === 帧头 ===
    buffer[index++] = FRAME_HEAD1;
    buffer[index++] = FRAME_HEAD2;

    // === 数据长度（8*float=32字节） ===
    buffer[index++] = 32;

    // === 数据区：float 转字节 (小端) ===
    memcpy(&buffer[index], &s1, 4); index += 4;
    memcpy(&buffer[index], &s2, 4); index += 4;
    memcpy(&buffer[index], &s3, 4); index += 4;
    memcpy(&buffer[index], &s4, 4); index += 4;
    memcpy(&buffer[index], &t1, 4); index += 4;
    memcpy(&buffer[index], &t2, 4); index += 4;
    memcpy(&buffer[index], &t3, 4); index += 4;
    memcpy(&buffer[index], &t4, 4); index += 4;

    // === CRC16 ===
    uint16_t crc = CRC16_Modbus(buffer, index);
    buffer[index++] = crc & 0xFF;
    buffer[index++] = (crc >> 8) & 0xFF;

    // === 帧尾 ===
    buffer[index++] = FRAME_TAIL1;
    buffer[index++] = FRAME_TAIL2;

    // === 发送 ===
    Serial_SendArray(buffer, index);
}

这里做的就是，将四个轮的float数据通过mcmcpy将其转换为uint8_t后，加了帧头帧尾通过串口发送出去

memcpy函数用法

在TIM6_IRQHandler函数里面使用

1
2
Serial_SendMotorData(Motor1_Speed, Motor2_Speed, Motor3_Speed, Motor4_Speed,
                  Motor1_Target_Speed, Motor2_Target_Speed, Motor3_Target_Speed, Motor4_Target_Speed);

上位机代码

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
import serial
import struct
import matplotlib.pyplot as plt

# === 串口配置 ===
ser = serial.Serial("COM14", 115200, timeout=0.5)  # 改成你的端口号

# === 协议常量 ===
FRAME_HEAD = b'\xAA\x55'
FRAME_TAIL = b'\x0D\x0A'
DATA_LEN   = 32   # 8个float = 32字节
FRAME_LEN  = 39   # 总帧长 = 2+1+32+2+2

# === CRC16-MODBUS 算法 ===
def crc16_modbus(data: bytes) -> int:
    crc = 0xFFFF
    for pos in data:
        crc ^= pos
        for _ in range(8):
            if crc & 0x0001:
                crc >>= 1
                crc ^= 0xA001
            else:
                crc >>= 1
    return crc & 0xFFFF

# === 数据解析函数 ===
def parse_packet(packet: bytes):
    if len(packet) != FRAME_LEN:
        return None
    if not (packet.startswith(FRAME_HEAD) and packet.endswith(FRAME_TAIL)):
        return None

    length = packet[2]
    if length != DATA_LEN:
        return None

    data = packet[3:3+DATA_LEN]
    crc_recv = packet[3+DATA_LEN] | (packet[3+DATA_LEN+1] << 8)
    crc_calc = crc16_modbus(packet[:3+DATA_LEN])
    if crc_recv != crc_calc:
        print("⚠ CRC 校验失败")
        return None

    # 解包 8 个 float (小端序)
    values = struct.unpack('<ffffffff', data)
    speeds = values[0:4]   # Motor1..4 实际速度
    targets = values[4:8]  # Motor1..4 目标速度
    return speeds, targets

# === 数据缓存 ===
motor_speed = [[] for _ in range(4)]
motor_target = [[] for _ in range(4)]

plt.ion()
fig, ax = plt.subplots()

# === 接收循环 ===
buffer = bytearray()
while True:
    bytes_waiting = ser.in_waiting
    if bytes_waiting:
        buffer.extend(ser.read(bytes_waiting))

        while True:
            start = buffer.find(FRAME_HEAD)
            end = buffer.find(FRAME_TAIL, start+1)

            if start != -1 and end != -1 and (end - start + 2) == FRAME_LEN:
                packet = buffer[start:end+2]
                buffer = buffer[end+2:]  # 移除已处理数据

                result = parse_packet(packet)
                if result:
                    speeds, targets = result
                    for i in range(4):
                        motor_speed[i].append(speeds[i])
                        motor_target[i].append(targets[i])

                    # === 绘图 ===
                    ax.clear()
                    colors = ['r', 'g', 'b', 'm']
                    for i in range(4):
                        ax.plot(motor_speed[i], color=colors[i], label=f"M{i+1}_Speed")
                        ax.plot(motor_target[i], color=colors[i], linestyle="--", label=f"M{i+1}_Target")
                    ax.legend()
                    ax.set_xlabel("Sample")
                    ax.set_ylabel("Speed (m/s)")
                    ax.set_title("Motor Speed vs Target")
                    plt.pause(0.01)
            else:
                break

接下来看一下调参过程吧

此时为 P = 10、I = 0、D = 0

P = 10、I = 0、D = 0

可以看到并没有达到目标值，小了，需要往上调节 此时为 P = 30、I = 0、D = 0

P = 30、I = 0、D = 0

可以看到此时一直在目标值上下震荡，说明P值大了，需要往下调 此时为 P = 26.5、I = 0、D = 0

P = 26.5、I = 0、D = 0

这个就差不多了，达到目标值后，然后下来，趋于平稳

然后我只是用了PI没用D，所以就调了I I就是调节静态误差，也就是调节速度曲线与目标速度的差距，可以将速度曲线往上移或者下移。

此时为 P = 26.5、I = 1、D = 0

P = 26.5、I = 1、D = 0

可以看到，已经很好的拟合目标速度了，但是还是有振荡。

此时需要调节D，D就是调这个振荡的，可以将其缩小。但是不知道啥原因，我的调不调没太大区别，这个振荡无法缩小，就设置为0了，也就是只用了PI。

关于PID算法的一些调参经验总结

最后

这就是今天的测速+PID调速的内容了，大家需要源码的话可去Github上 STM32下位机下载。

  • Title: ROS小车（1）：小车测速和PID调速
  • Author: StarHui
  • Created at : 2025-09-14 14:50:51
  • Updated at : 2025-09-14 14:50:52
  • Link: https://renyuhui0415.github.io/post/ROS_Car(1)_Speed_Measurement_and_PID_Speed_Regulation.html
  • License: This work is licensed under CC BY-NC-SA 4.0.
Comments
On this page
ROS小车（1）：小车测速和PID调速
  1. 前言
  2. 架构
  3. 硬件准备
  4. 驱动电机
  5. 电机测速
  6. PID调速
    1. 理论
    2. 实现
    3. 调参
  7. 最后