基于canmv的k230的寻迹串级pid小车---stm32架构（HAL）_k230寻迹

canmv--k230

1.整体思路

在canmv平台上面，本质代码逻辑就是在灰度图下用色块查找。

2.具体实现讲解

第一步简单，直接配置好就行。由于我是带了lcd触摸屏，具体尺寸你改下就行

其中sensor.set_pixformat(Sensor.GRAYSCALE)为改为灰度图像处理

import time, os, sys, mathfrom machine import UARTfrom machine import FPIOAfrom media.sensor import * #导入sensor模块，使用摄像头相关接口from media.display import * #导入display模块，使用display相关接口from media.media import * #导入media模块，使用meida相关接口#串口设置fpioa = FPIOA()fpioa.set_function(11,FPIOA.UART2_TXD)fpioa.set_function(12,FPIOA.UART2_RXD)uart=UART(UART.UART2,115200) #设置串口号2和波特率#固定代码lcd_width=640lcd_height=480sensor=Sensor(width=1280,height=960)sensor.reset()#framesize数字越大，越显示的近，帧率越低sensor.set_framesize(sensor.VGA)sensor.set_pixformat(Sensor.GRAYSCALE)#在mipi上显示那么frmesize需要匹配display显示Display.init(Display.ST7701, width=lcd_width, height=lcd_height, to_ide=True)MediaManager.init() #初始化media资源管理器sensor.run()clock=time.clock()

第二步，首先需要划分ROI区域，最后一个小数为权重值，前者是告诉采集的当前区域最大面积，后者是是四个区域的中心点连成一条线后经权重比换算过来的线中心，该线中心是后面换算距离的重要之处。

#黑线阈值GRAYSCALE_THRESHOLD = [(0, 64)]#采样图像为VGA 640*480，列表把roi把图像分成4个矩形，越靠近的摄像头视野（通常为图像下方）的矩形权重越大。ROIS = [ # [ROI, weight] (0,420,640,60,0.7), (0,280,640,60,0.5), # 可以根据不同机器人情况进行调整。 (0,140,640,60,0.3), (0, 0,640,60,0.1), ]weight_sum = 0for r in ROIS: weight_sum += r[4] # r[4] 为矩形权重值.

第三步，利用img.find_blobs函数寻找黑色色块，阈值为：GRAYSCALE_THRESHOLD = [(0, 64)]

然后因为设置好的roi元组，那么k230会自动划分4块区域：

接着计算出中心点，经权重值换算后得到线中心。权重值可以不总和为1，注意越靠近屏幕下方的权重值越大，这样换算过来的角度越大那么单片机给电机的反应越大

假设摄像头当前画面的像素是分辨率：160（宽）X120（高），左上角坐标为（0,0），然后当前出现直线坐标为（80,120）至（160,0）偏右的直线。上中下三个部分的权重分别为0.1、0.3、0.7（底部图像靠近机器人，权重大，权重总和可以不是1），我们来计算一下其中心值，得到（98,60），即X’=（800.7+1200.3+160*0.1）/（0.7+0.3+0.1）=98

于是进行偏离角度换算：

那么直线偏离坐标可以认为是（98,60），图中绿色“+”位置。那么利用反正切函数可以求出偏离角度：a = atan((98-80)/60)=16.7°，机器人相当于实线的位置往左偏了，所以加一个负号，即 -16.7°；偏离角度就是这么计算出来的。得到偏离角度后就可以自己编程去调整小车或者机器人的运动状态，直到0°为没有偏离。

while True: clock.tick() img=sensor.snapshot() #权重值之和 centroid_sum = 0 for r in ROIS: #找到具体色块,找到当前roi矩形范围内的具体符合黑线的色块，其中合并相邻且重叠的色块 blobs=img.find_blobs(GRAYSCALE_THRESHOLD,roi=r[0:4],merge=True) if blobs: #检测到符合条件的色块时，比较色块最多的地方，定义为主路径，作用为过滤阴影 largest_blobs=max(blobs,key=lambda b: b.pixels())#lambda 参数:返回值 #开始绘画矩形，和中心点十字箭头 img.draw_rectangle(largest_blobs.rect()) img.draw_cross(largest_blobs.cx(), largest_blobs.cy()) centroid_sum+=largest_blobs.cx()*r[4] center_pos = (centroid_sum / weight_sum) # 确定直线的中心. #角度转换 angle=0 angle = -math.atan((center_pos-320)/240) #采用图像为VGA时候使用 angle = math.degrees(angle)#最终为具体角度 #lcd显示角度值，左半屏为正值 img.draw_string_advanced(2,2,20, str(\'%.1f\' % angle), color=(255,255,255)) #串口传递角度给单片机 angle_int = int(angle * 100) # 放大100倍转为整数 uart.write(angle_int.to_bytes(2, \'little\', True)) # 小端2字节 print(\"Turn Angle: %f\" % angle) Display.show_image(img) #显示图片 print(clock.fps()) #打印FPS

小端二字节就是将原来例如42.33度的角度先转换为整数（x100），再将int转byte

取自这篇文章https://blog.csdn.net/weixin_46283997/article/details/123209469

然后将得到的偏离角度用串口传到stm32单片机，我们开始单片机的处理

基于STM32F103单片机——stm32cubeide/stm32cubemx

这是一个三轮小车，带霍尔编码器的轮子分在左右两侧，第三个轮为万向轮，做支撑作用

配置如下

计时器tim1作为直流电机的pwm

时钟改为72MHZ后，预分频器为7200-1，那么时钟频率为1/10000，自动重装载值100-1，也就是将pwm分为0-99，简单的百分制，值越大占空比频率越大

计时器tim2与tim3设置为编码器模式

注意你要看清你直流电机的编码器（一般是霍尔编码器）是不是双边沿触发的（对速度反馈有影响）。

电机转一圈会产生减速比*编码器线数*程序倍频数的脉冲。其中倍频数如下解释：

计时器tim4作为正儿八经的计时器

在频率为1/10000秒内，每达到100就重置，就是1/10000*100=1/100，0.01s的计数频率。

开始写代码

int main(void){ /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ HAL_Init(); /* USER CODE BEGIN Init */ /* USER CODE END Init */ /* Configure the system clock */ SystemClock_Config(); /* USER CODE BEGIN SysInit */ /* USER CODE END SysInit */ /* Initialize all configured peripherals */ MX_GPIO_Init(); MX_DMA_Init(); MX_TIM1_Init(); MX_TIM2_Init(); MX_TIM3_Init(); MX_TIM4_Init(); MX_USART2_UART_Init(); MX_USART3_UART_Init(); /* USER CODE BEGIN 2 */ HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL); // 电机A编码器 HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); // 电机B编码器 HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); // 电机A AIN1 HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); // 电机A AIN2 HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); // 电机B BIN1 HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); // 电机B BIN2 HAL_TIM_Base_Start_IT(&htim4); //开启串口的上位机传输 HAL_UARTEx_ReceiveToIdle_DMA(&huart3, a, sizeof(a)); /* USER CODE END 2 */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ while (1) { /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ } /* USER CODE END 3 */}

串口用来利用vofa+的上位机进行pid调试作用

接下来是各回调函数：

/* USER CODE BEGIN Header *//** ****************************************************************************** * @file  : main.c * @brief : Main program body ****************************************************************************** * @attention * * Copyright (c) 2025 STMicroelectronics. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** *//* USER CODE END Header *//* Includes ------------------------------------------------------------------*/#include \"main.h\"#include \"dma.h\"#include \"tim.h\"#include \"usart.h\"#include \"gpio.h\"/* Private includes ----------------------------------------------------------*//* USER CODE BEGIN Includes */#include\"control.h\"#include\"go.h\"/* USER CODE END Includes *//* Private typedef -----------------------------------------------------------*//* USER CODE BEGIN PTD */float speed_kp=4,speed_ki=0.1,speed_kd=0.8;float turn_kp=0.3,turn_kd=0;/* USER CODE END PTD *//* Private define ------------------------------------------------------------*//* USER CODE BEGIN PD *//* USER CODE END PD *//* Private macro -------------------------------------------------------------*//* USER CODE BEGIN PM *//* USER CODE END PM *//* Private variables ---------------------------------------------------------*//* USER CODE BEGIN PV */char a[100];//上位机用数组uint8_t rx_buf[2]; // 2字节缓冲区float angle = 0.0f;/* USER CODE END PV *//* Private function prototypes -----------------------------------------------*/void SystemClock_Config(void);/* USER CODE BEGIN PFP *//* USER CODE END PFP *//* Private user code ---------------------------------------------------------*//* USER CODE BEGIN 0 */void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size){if(huart==&huart3){ // 1. 查找结束符\'#\'的位置 char *end_ptr = memchr(a, \'#\', Size); if (end_ptr != NULL) { // 2. 在\'#\'位置添加字符串终止符 *end_ptr = \'\\0\'; // 3. 解析参数（格式：up_kp,up_kd#） float new_kp = 0, new_kd = 0; int parsed = sscanf(a, \"%f,%f\", &new_kp, &new_kd); // 4. 校验并更新参数 if (parsed == 2) { // 确保解析到两个参数 turn_kp = new_kp; turn_kd = new_kd; } }}HAL_UARTEx_ReceiveToIdle_DMA(&huart3, a, sizeof(a));}void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){if(htim==&htim4){get_now_speed();HAL_UART_Receive_DMA(&huart2, rx_buf, 2); // 解析角度值（小端模式） int16_t angle_int = (rx_buf[1] <0)go_forward_a(pwm_a);else if(pwm_a0)go_forward_b(pwm_b);else if(pwm_b<0)go_backward_b(-pwm_b);char message[80];int len = sprintf(message, \"%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f\\n\",speed_A,speed_B,pwm_a,pwm_b,angle,left_v,right_v);HAL_UART_Transmit_DMA(&huart3, (uint8_t*)message, len);}}/* USER CODE END 0 */

整体逻辑很清晰，通过计时器每次启动后进入回调函数内，将串口得到的角度值解析出来，再传入转向环，转向环传出单向转向速度转为差速比后再给速度环，从而规划速度以达到稳定的转向+稳定的速度 

PID——一个抽象的变化

pid的核心是输入一值再经过比例-积分-微分的变化后，输出一个“模糊值”（模糊值是需要调试参数让它变为“精准值”）。pid的输入实际上是上一次输出的结果，再变化后输出符合需求的结果，不断反馈不断纠正的这一过程就是闭环，不同作用的闭环操作就简称为“环”，正如所说的速度环、转向环。

上述代码是将得到的角度值，例如32.52度传入转向环，其中目标角度就是0度，那么差值就是angle=32.52，于是经过模糊变换后输出一个名为“单向转向速度”的模糊值，这个值还不能马上输入进速度环里（左右两轮的速度环显然需要具体输入速度），它传到一个差速比的函数，获得左右轮具体想要达到的速度值：就像两轮由基本速度v_base=30cm/s，左轮加上一个值，右轮减去一个值，形成差速比以达到对输入角度值的对应转弯效果。接下来的速度环就是不断反馈上一次结果的过程再输出要达到的具体速度，从而实现稳定速度减小误差效果。

整个过程是抽象的，模糊的，把角度转为差速比，将速度化为pwm占空比，占空比指导速度形成，编码器返回实际速度，这一系列的模糊与抽象变化，即使我们不知道转向环具体输出的是什么，不知道输出的pwm占空比的值又是多少，但就是能搞定你想要的结果。

虽然pid算法相当简单，但是所有的难点都在调参上面，对kp、ki、kd的调整，需要细细调试！

#include\"go.h\"#include\"main.h\"float delta_t=0.01;//控制周期float wheel_circumference = 20.42; // cmfloat speed_A=0,speed_B=0;int output_max = 100;float left_v=0,right_v=0;//差速比左右轮速度float v_base=30;int32_t prev_count_A=0,prev_count_B=0;//转向环(pd)float pid_turn(float target,float need){static float last_error=0;float error=need-target;float output=turn_kp*error+turn_kd*(error-last_error);last_error=error;return output;//输出的是单向转向速度}//速度环(pid)float pid_speed(float current_speed,float need_speed){static float integral = 0,last_error=0;float error=need_speed-current_speed;integral+=error;if(integral>output_max)integral=output_max;else if(integral100)left_v=100; if(left_v100)right_v=100; if(right_v<-100)right_v=-100;}void get_now_speed(){ // 1. 读取当前编码器值 int32_t current_count_A = __HAL_TIM_GET_COUNTER(&htim2); int32_t current_count_B = __HAL_TIM_GET_COUNTER(&htim3); // 2. 计算脉冲差值（处理溢出） int16_t delta_A = (int16_t)(current_count_A - prev_count_A); int16_t delta_B = (int16_t)(current_count_B - prev_count_B); // 3. 更新前值 prev_count_A = current_count_A; prev_count_B = current_count_B; // 4. 计算线速度（cm/s） speed_A = (delta_A * wheel_circumference) / (2496 * delta_t); speed_B = (delta_B * wheel_circumference) / (2496 * delta_t);}

以下是左右轮控制函数

#include\"main.h\"#include\"tim.h\"#include\"control.h\"void go_backward_a(uint16_t pwm){ //左轮--a //后退为an1 HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_2); //PINA8-1--AN1/PINA9-0--AN2 HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pwm); }void go_backward_b(uint16_t pwm){ //右轮--b //后退为bn1 HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4); //PINA10-1--BN1/PINA11-0--BN2 HAL_GPIO_WritePin(GPIOA, GPIO_PIN_11, GPIO_PIN_RESET); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, pwm);} void go_forward_a(uint16_t pwm){ //左轮--a //前进为an2 HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1); //PINA8-0--AN1/PINA9-1--AN2 HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, pwm); } void go_forward_b(uint16_t pwm){ //右轮--b //前进为bn2 HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3); //PINA10-0--BN1/PINA11-1--BN2 HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_RESET); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, pwm); } void go_stop(){ //左轮 HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3); // 停止BIN1的PWM HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4); // 停止BIN2的PWM HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET); // BIN1=1 HAL_GPIO_WritePin(GPIOA, GPIO_PIN_11, GPIO_PIN_SET); // BIN2=1 //右轮 HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1); // 停止AIN1的PWM HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_2); // 停止AIN2的PWM HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_SET); // AIN1=1 HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_SET); // AIN2=1 }

注意事项：

pid是个相当简单的算法，写出来不难，主要是调试，怎么调试网上有很多资料了，这里要说的是串级pid。串级pid是要明确每一pid闭环是要输入什么和输出什么，由外而内，环环相扣。上述例子较为简单，通过输入的角度，告诉转向环应该怎么输出每一个轮子的速度来形成差速比，使得达到转弯效果。速度环的作用就是稳定速度，否则就会导致电机误差直接形成客观上的差速比，导致小车无法正确直线或者曲线前进。

结果展示

小车寻迹