前言
从五月份拿到赛题到8月参加华北赛区省赛，对于一个小白来说一切都是从头开始，从一开始想方案到实物制作都是从头开始，但在实验室团队每个人的努力下，最后取得好的成绩。在此，我代表团队将代码以及设计思路向未来加入实验室的学弟，学妹以及看到这篇文章的朋友进行分享。希望看到这篇文章的各位有所收获。
车辆搭建
由于时间比较紧迫所以我们选择结构较为简单的二驱三轮小车作为平台，项目证明使用该平台是当时最好的选择，但车辆也出现了重心靠前结构不够稳定的问题，但总体是稳定的。
车辆底盘
车辆底盘我们使用轮趣的普通三轮小车底盘,电机使用的是MG513光电编码器电机，电机驱动使用TB6612电机驱动，电机驱动拥有双向12v电压输入输出以及3.3v 5v电压输出可用于给openmv，stm32输出5v电压，也可直接接受pwm信号直接输出给电机非常方便。
底层控制
底层控制使用stm32f103zet6作为控制芯片，使用版型为正点原子的，项目几乎占用了zet6的所有通用定时器，因此项目使用该芯片是合适的。
识别及路径规划控制
该部分使用openmv4plus，openmv是一个可编程的摄像头，通过MicroPython语言编程。介绍openmv的文章有很多这里不再赘述，具体可通过openmv官网查看。
-
OpenMV中国官网：
http://openmv.cc
基于openmv的强大我们将藏宝图和宝藏识别部分以及路径规划部分集成至一个openmv4plus中。
循迹控制
循迹部分使用openmv4(openmv3也行，但openmv4plus反而没有前两者跑出来的帧数高，我们猜测可能是openmv4plus使用外挂大容量闪存，数据读入需要占用时间导致帧数下降)，摄像头开源库函数丰富，可快速上手。实际上我上手就用了2天，python也是现学现用，所以是很简单的。但openmv所用算法需要一定时间了解学习其原理才能更好搭配使用。
设计思路
设计思路如下：首先先通过设计在stm32中的交互系统通过oled屏幕显示的方式进行人机交互进行选队，执行藏宝图识别，执行车辆行走的步骤。由于藏宝图识别以及路径规划都在openmv4plus中，因此stm32通过串口发送将识图指令以及队伍选择发送至openmv4plus中，然后就可以开始对藏宝图进行识别，藏宝图识别完成后openmv再通过串口将识别宝藏的坐标点发送到stm32，stm32收到后控制oled屏幕显示出来供人工确定。完成识图后openmvplus会执行路径规划算法进行对起点到第一目标点的路径规划，路径规划完成之后按下按键实现一键启动，stm32端开始执行车辆行驶部分，通过串口单向接受循迹openmv巡线程序的数据进行巡线，当openmv识别到路口时会将信号发送到stm32，stm332再通过openmv4plus传回的路径规划数组控制车辆在路口执行转向。当车辆到达宝藏坐标点后将向openmv4plus发送指令执行对宝藏的识别，识别完成之后将识别结果以及对下一点的路径规划发送至stm32，32端在接受到数据后根据数据及执行宝藏推倒或车辆掉头控制，重复上述过程再结合部分策略算法即可实现迷宫中宝藏的寻找，直到走到出口完成寻宝。
核心代码以及细节讲解
代码由团队共同编写，从0到1实属不易。这次项目的复杂程度以及对代码能力的考验，我都有深刻体会。
stm32部分
首先是对电机的控制，使用pwm即可实现对电机的调速，控制io口高低电平的控制即可实现转动方向的改变，实现车辆的停止，前进，转向，掉头等工作，这是实现电机控制的基础也是核心。
要实现电机调速，pwm是核心。
如何配置和使用pwm有很多文章进行详细讲解，这里不再赘述。这里给出一种可以集成控制车辆运动函数的方式。
// Mode: 1:正转; 2:反转; 3:停止;
void Set_Motor_Mode(u8 Lmode, u8 Rmode)
{
switch (Lmode)
{
case 1:
GPIO_ResetBits(GPIOA, GPIO_Pin_4);
GPIO_SetBits(GPIOA, GPIO_Pin_5);
break;
case 2:
GPIO_SetBits(GPIOA, GPIO_Pin_4);
GPIO_ResetBits(GPIOA, GPIO_Pin_5);
break;
case 3:
GPIO_SetBits(GPIOA, GPIO_Pin_4);
GPIO_SetBits(GPIOA, GPIO_Pin_5);
break;
default:
break;
}
switch (Rmode)
{
case 1:
GPIO_SetBits(GPIOA, GPIO_Pin_6);
GPIO_ResetBits(GPIOA, GPIO_Pin_7);
break;
case 2:
GPIO_ResetBits(GPIOA, GPIO_Pin_6);
GPIO_SetBits(GPIOA, GPIO_Pin_7);
break;
case 3:
GPIO_SetBits(GPIOA, GPIO_Pin_6);
GPIO_SetBits(GPIOA, GPIO_Pin_7);
break;
default:
break;
}
}
void Set_Motor_PWM(u16 Lval, u16 Rval)
{
TIM_SetCompare3(TIM3, Lval);
TIM_SetCompare4(TIM3, Rval);
}
但要想实现电机以恒定速度运行光靠pwm是无法实现的，必须引入pid算法才能实现电机以恒定速度稳定运行。本项目我们使用速度环转向环再加上巡线pid三环串级pid实现对车辆的稳定控制。
pid速度环以及巡线pid代码：
#include "pid.h"
float currentError, currentOutput, Vp, Vi, Vd;
// 给结构体类型变量赋初值，直接在这里改，而不是在主函数里在写一遍
void PID_Struct_Init_L(tPid *pid)
{
pid->Kp = 0.1;
pid->Ki = 0.098;
pid->Kd = 0.00;
pid->setValue = 0.0;
pid->baseValue = 0.0;
pid->preError = 0.0;
pid->prepreError = 0.0;
pid->preOutput = 0.0;
}
void PID_Struct_Init_R(tPid *pid)
{
pid->Kp = 0.11;
pid->Ki = 0.114;
pid->Kd = 0.00;
pid->setValue = 0.0;
pid->baseValue = 0.0;
pid->preError = 0.0;
pid->prepreError = 0.0;
pid->preOutput = 0.0;
}
void PID_Struct_Init(tPid *pid)
{
pid->Kp = 0.00;
pid->Ki = 0.10;
pid->Kd = 0.00;
pid->setValue = 0.0;
pid->baseValue = 0.0;
pid->preError = 0.0;
pid->prepreError = 0.0;
pid->preOutput = 0.0;
}
void PID_Struct_Part_Init(tPid *pid)
{
pid->setValue = 0.0;
pid->baseValue = 0.0;
pid->preError = 0.0;
pid->prepreError = 0.0;
pid->preOutput = 0.0;
}
float PID_cL(tPid *pid, float currentValue)
{
currentError = pid->setValue - currentValue;
Vp = pid->Kp * (currentError - pid->preError);
Vi = pid->Ki * currentError;
Vd = pid->Kd * (currentError - (pid->preOutput) * 2 + pid->prepreError);
currentOutput = pid->preOutput + Vp + Vi + Vd; // 计算输出值
if (currentOutput > 2200)
{
currentOutput = 2200;
}
else if (currentOutput < 0)
{
currentOutput = 0;
}
pid->prepreError = pid->preError; // 更新记录
pid->preError = currentError;
pid->preOutput = currentOutput;
return currentOutput;
}
float PID_cR(tPid *pid, float currentValue)
{
currentError = pid->setValue - currentValue;
Vp = pid->Kp * (currentError - pid->preError);
Vi = pid->Ki * currentError;
Vd = pid->Kd * (currentError - (pid->preOutput) * 2 + pid->prepreError);
currentOutput = pid->preOutput + Vp + Vi + Vd; // 计算输出值
if (currentOutput > 2440)
{
currentOutput = 2440;
}
else if (currentOutput < 0)
{
currentOutput = 0;
}
pid->prepreError = pid->preError; // 更新记录
pid->preError = currentError;
pid->preOutput = currentOutput;
return currentOutput;
}
float PID(tPid *pid, float currentValue)
{
currentError = pid->setValue - currentValue;
Vp = pid->Kp * (currentError - pid->preError);
Vi = pid->Ki * currentError;
Vd = pid->Kd * (currentError - (pid->preOutput) * 2 + pid->prepreError);
currentOutput = pid->preOutput + Vp + Vi + Vd; // 计算输出值
if (currentOutput < 0)
{
currentOutput = 0;
}
pid->prepreError = pid->preError; // 更新记录
pid->preError = currentError;
pid->preOutput = currentOutput;
return currentOutput;
}
位置环车辆转向代码：
#include "MPU6050.h"
#include "element.h"
#include "USART_func.h"
#include "motor.h"
#include "delay.h"
#include "CarCore.h"
int16_t MPU6050_Z = 0;
int16_t turn_angle = 0;
int16_t target_angle = 0;
int16_t angle_err = 0;
int16_t angle_err_buff = 0;
u16 MPU6050_ad_times = 0;
float low_speed = 0;
float angle_k = 182.044;
void MPU6050_Reset(void)
{
MPU6050_Z = 0; // 之前的角度数据清零
target_angle = 0; // 之前的目标角度清零
angle_err_buff = angle_err;
angle_err = 0;
angle_SpeedIncL = 0; // 将之前的矫正清零
angle_SpeedIncR = 0; // 因为复位后的第一次主循环可能不会处理mpu数据,那它就不能进行矫正的设置,增益速度将是基于复位前的速度,是错误的矫正速度
usart3_send_buf(); // 发送复位指令
delay_ms(3); // 等待以确保复位指令发送完成;
usart3_send_buf(); // 再次发送,避免复位失败
delay_ms(3);
usart3_buff_status = 0; // 允许中断覆盖前一次数据;阻止主循环处理上一次缓存的数据;保证下一次要被处理的缓存中的数据是复位后的mpu的数据
}
void MPU6050_T90_L(void)
{
MPU6050_Reset(); // 角度归零
CarStop_L(); // 使小车的左轮停下，开始转弯
SetCarSpeed(low_speed, 9000);
Set_Motor_Mode(1, 1);
turn_angle = 87 * angle_k + angle_err_buff; // 设置目标角度
target_angle = 89 * angle_k + angle_err_buff; // 角度环pid目标值
car_turn_status = 1; // 标志小车正在转弯
en_down_turning_speed = 1; // 允许转弯降速一次
}
void MPU6050_T90_R(void)
{
MPU6050_Reset(); // 角度归零
CarStop_R(); // 使小车的右轮停下，开始转弯
SetCarSpeed(9000, low_speed); // 6000=8000
Set_Motor_Mode(1, 1);
turn_angle = 87 * angle_k - angle_err_buff; // 设置目标角度
target_angle = 89 * angle_k - angle_err_buff;
car_turn_status = 1; // 标志小车正在转弯
en_down_turning_speed = 1; // 允许转弯降速一次
}
void MPU6050_T180(void)
{
MPU6050_Reset(); // 角度归零
SetCarSpeed(7500, 7500); // 原来是6000
Set_Motor_Mode(1, 2); // 车身左旋转;车头右旋转
// Set_Motor_Mode(2, 1); // 右旋转
turn_angle = 175 * angle_k; // 原来是174
target_angle = 177 * angle_k; // 原来是180
car_turn_status = 1;
en_down_turning_speed = 1; // 允许转弯降速一次
}
void MPU6050_T180_R(void)
{
MPU6050_Reset(); // 角度归零
SetCarSpeed(7500, 7500); // 原来是6000
// Set_Motor_Mode(1, 2); // 左旋转
Set_Motor_Mode(2, 1); // 右旋转
turn_angle = 175 * angle_k; // 原来是174
target_angle = 177 * angle_k; // 原来是180
car_turn_status = 1;
en_down_turning_speed = 1; // 允许转弯降速一次
}
转向部分使用感为科技jy61陀螺仪传感器，利用串口dma通讯发送复位指令以及接受jy61传回的z轴方位角实现转向，角度环pid得到角度数据后实现精准直行。
串口dma配置代码：
#include "DMA_Config.h"
#include "element.h"
void Config_DMA(void)
{
// 定义结构体===============================================================================
DMA_InitTypeDef dmaInit;
// 打开时钟===============================================================================
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE);
// DMA1 通道4
// USART1 发送===============================================================================
// 发送数据给MV4
DMA_StructInit(&dmaInit);
dmaInit.DMA_PeripheralBaseAddr = (uint32_t)(&(USART1->DR));
dmaInit.DMA_MemoryBaseAddr = (uint32_t)txBuffer_1;
dmaInit.DMA_DIR = DMA_DIR_PeripheralDST; // 数据传输方向:从内存到外设
dmaInit.DMA_BufferSize = txSize_1;
dmaInit.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
dmaInit.DMA_MemoryInc = DMA_MemoryInc_Enable;
dmaInit.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
dmaInit.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
dmaInit.DMA_Mode = DMA_Mode_Normal;
dmaInit.DMA_Priority = DMA_Priority_High;
dmaInit.DMA_M2M = DMA_M2M_Disable;
DMA_Init(DMA1_Channel4, &dmaInit);
// 使能DMA1通道4的传输完成中断
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE);
// 初始化关闭DMA1通道4 (开始传输时打开,传输完成后在中断中关闭);为方便后续代码检测dma的传输状态;
DMA_Cmd(DMA1_Channel4, DISABLE);
// DMA1 通道5
// USART1 接收===============================================================================
// 接收MV4的数据
DMA_StructInit(&dmaInit);
dmaInit.DMA_PeripheralBaseAddr = (uint32_t)(&(USART1->DR)); // 外设地址
dmaInit.DMA_MemoryBaseAddr = (uint32_t)rxBuffer_1; // 内存地址
dmaInit.DMA_DIR = DMA_DIR_PeripheralSRC; // 数据传输方向:从外设到内存
dmaInit.DMA_BufferSize = rxSize_1; // 缓冲区大小
dmaInit.DMA_PeripheralInc = DMA_PeripheralInc_Disable; // 关闭外设地址自增
dmaInit.DMA_MemoryInc = DMA_MemoryInc_Enable; // 打开内存地址自增
dmaInit.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; // 传输单位:字节 8位
dmaInit.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; // 传输单位: 字节 8位
dmaInit.DMA_Mode = DMA_Mode_Normal; // 普通模式
dmaInit.DMA_Priority = DMA_Priority_High; // 高优先级
dmaInit.DMA_M2M = DMA_M2M_Disable; // 关闭内存到内存
DMA_Init(DMA1_Channel5, &dmaInit);
// 使能DMA1通道5的传输完成中断
DMA_ITConfig(DMA1_Channel5, DMA_IT_TC, ENABLE);
// 初始化打开DMA1通道5
DMA_Cmd(DMA1_Channel5, ENABLE);
// DMA1 通道6
// USART2 接收===============================================================================
// 接收OpenMV3的数据
DMA_StructInit(&dmaInit);
dmaInit.DMA_PeripheralBaseAddr = (uint32_t)(&(USART2->DR)); // 外设地址
dmaInit.DMA_MemoryBaseAddr = (uint32_t)&rxBuffer_2; // 内存地址
dmaInit.DMA_DIR = DMA_DIR_PeripheralSRC; // 数据传输方向:从外设到内存
dmaInit.DMA_BufferSize = rxSize_2; // 缓冲区大小
dmaInit.DMA_PeripheralInc = DMA_PeripheralInc_Disable; // 关闭外设地址自增
dmaInit.DMA_MemoryInc = DMA_MemoryInc_Enable; // 打开内存地址自增
dmaInit.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; // 传输单位:字节 8位
dmaInit.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; // 传输单位: 字节 8位
dmaInit.DMA_Mode = DMA_Mode_Normal; // 普通模式
dmaInit.DMA_Priority = DMA_Priority_VeryHigh; // 高优先级
dmaInit.DMA_M2M = DMA_M2M_Disable; // 关闭内存到内存
DMA_Init(DMA1_Channel6, &dmaInit);
// 使能DMA1通道6的传输完成中断
DMA_ITConfig(DMA1_Channel6, DMA_IT_TC, ENABLE);
// 初始化打开DMA1通道6
DMA_Cmd(DMA1_Channel6, ENABLE);
// DMA1 通道2
// USART3 发送===============================================================================
// MPU6050指令发送
DMA_StructInit(&dmaInit);
dmaInit.DMA_PeripheralBaseAddr = (uint32_t)(&(USART3->DR));
dmaInit.DMA_MemoryBaseAddr = (uint32_t)txBuffer_3;
dmaInit.DMA_DIR = DMA_DIR_PeripheralDST; // 数据传输方向:从内存到外设
dmaInit.DMA_BufferSize = txSize_3;
dmaInit.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
dmaInit.DMA_MemoryInc = DMA_MemoryInc_Enable;
dmaInit.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
dmaInit.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
dmaInit.DMA_Mode = DMA_Mode_Normal;
dmaInit.DMA_Priority = DMA_Priority_High;
dmaInit.DMA_M2M = DMA_M2M_Disable;
DMA_Init(DMA1_Channel2, &dmaInit);
// 使能DMA1通道2的传输完成中断
DMA_ITConfig(DMA1_Channel2, DMA_IT_TC, ENABLE);
// 初始化关闭DMA1通道2
DMA_Cmd(DMA1_Channel2, DISABLE);
// DMA 通道3
// USART3 接收===============================================================================
// 接收MPU6050的数据
DMA_StructInit(&dmaInit);
dmaInit.DMA_PeripheralBaseAddr = (uint32_t)(&(USART3->DR)); // 外设地址
dmaInit.DMA_MemoryBaseAddr = (uint32_t)rxBuffer_3; // 内存地址
dmaInit.DMA_DIR = DMA_DIR_PeripheralSRC; // 数据传输方向:从外设到内存
dmaInit.DMA_BufferSize = rxSize_3; // 缓冲区大小
dmaInit.DMA_PeripheralInc = DMA_PeripheralInc_Disable; // 关闭外设地址自增
dmaInit.DMA_MemoryInc = DMA_MemoryInc_Enable; // 打开内存地址自增
dmaInit.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; // 传输单位:字节 8位
dmaInit.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; // 传输单位: 字节 8位
dmaInit.DMA_Mode = DMA_Mode_Normal; // 普通模式
dmaInit.DMA_Priority = DMA_Priority_High; // 高优先级
dmaInit.DMA_M2M = DMA_M2M_Disable; // 关闭内存到内存
DMA_Init(DMA1_Channel3, &dmaInit);
// 使能DMA1通道2的传输完成中断
DMA_ITConfig(DMA1_Channel3, DMA_IT_TC, ENABLE);
// 初始化打开DMA1通道2
DMA_Cmd(DMA1_Channel3, ENABLE);
// DMA2 通道5
// UART4 发送===============================================================================
// 发送数据给ESP32-S
DMA_StructInit(&dmaInit);
dmaInit.DMA_PeripheralBaseAddr = (uint32_t)(&(UART4->DR));
dmaInit.DMA_MemoryBaseAddr = (uint32_t)txBuffer_4;
dmaInit.DMA_DIR = DMA_DIR_PeripheralDST; // 数据传输方向:从内存到外设
dmaInit.DMA_BufferSize = txSize_4;
dmaInit.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
dmaInit.DMA_MemoryInc = DMA_MemoryInc_Enable;
dmaInit.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
dmaInit.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
dmaInit.DMA_Mode = DMA_Mode_Normal;
dmaInit.DMA_Priority = DMA_Priority_High;
dmaInit.DMA_M2M = DMA_M2M_Disable;
DMA_Init(DMA2_Channel5, &dmaInit);
// 使能DMA1通道4的传输完成中断
DMA_ITConfig(DMA2_Channel5, DMA_IT_TC, ENABLE);
// 初始化关闭DMA1通道4 (开始传输时打开,传输完成后在中断中关闭);为方便后续代码检测dma的传输状态;
DMA_Cmd(DMA2_Channel5, DISABLE);
}
stm32发送复位指令代码以及接受数据代码：
/ USART3接收处理函数===============================================================================
// 处理MPU6050的数据
void usart3_buffprocess(void)
{
u8 i, sum = 0, k = 0; // 715原来k=10;关闭角度环,因为误差累积的原因,当角度环的效果明显时,将会使误差持续累积
int16_t angle_err_adjust_speed = 0;
if (usart3_buff[1] == 0x53)
{
for (i = 0; i < rxSize_3 - 1; i++)
{
sum += usart3_buff[i];
}
if (sum == usart3_buff[rxSize_3 - 1])
{
// MPU6050_Z = (*(int16_t *)(&usart3_buff[6])) * 180 / 32768; // 只需要z轴的数据
MPU6050_Z = (*(int16_t *)(&usart3_buff[6])); // 用回原始数据
if (MPU6050_Z < 0)
{
// 只输出正数;使左转和右转没正负区别;(导致了后面的一个问题，转弯前的偏差记录，左转后的左右偏差，与右转后的左右偏差是不一样的，使得z角为正和负时要分开算)
angle_err_adjust_speed = -target_angle - MPU6050_Z; // 其实这个和angle_err可以共用一个变量,但那样会导致它们产生关联,怕出现不能预料的问题
angle_err = -target_angle - MPU6050_Z;
MPU6050_Z = -MPU6050_Z;
}
else
{
angle_err_adjust_speed = target_angle - MPU6050_Z;
angle_err = target_angle - MPU6050_Z;
}
}
angle_SpeedIncL = 0;
angle_SpeedIncR = 0;
// 角度环以关闭; 陀螺仪只用于转弯
if (!car_turn_status)
{
if (angle_err_adjust_speed < 0) // 左偏
{
angle_SpeedIncL = -angle_err_adjust_speed * k;
angle_SpeedIncR = 0;
}
else // 右偏
{
angle_SpeedIncL = 0;
angle_SpeedIncR = angle_err_adjust_speed * k;
}
}
// LED1 = !LED1;
}
usart3_buff_status = 0; // 标志位清零
}
// USART3 发送函数===============================================================================
// 发送MPU6050 Z轴角度归零指令
void usart3_send_buf(void)
{
if ((DMA1_Channel2->CCR & DMA_CCR2_EN) == 0) // 通道没在发送数据
{
DMA_SetCurrDataCounter(DMA1_Channel2, txSize_3);
DMA_Cmd(DMA1_Channel2, ENABLE); // 打开DMA1通道2,开始发送
}
}本项目最复杂的部分就是串口数据传输处理,使用dma可有效减少cpu占用提高数据传输速度，如何使用dma这里不再赘述可查找代码相关文章进行学习。串口部分还有与openmv4以及openmv4plus的通信部分，与上述jy61通信方式大同小异，都是使用dma配置直接实现通信，在根据需求编写数据处理函数实现外设与主控板的同步运行。
循迹部分的openmv4
本项目使用视觉循迹方式进行巡线，使用openmv4通过色块识别框选出循迹线，再使用灰度二值化将循迹线与周围环境进行分层，借此得到相对清晰的循迹线区块，再使用线性回归的方式将循迹线回归为一条随循迹线移动的直线，再将直线的像素坐标与图像中中心线坐标进行比较即可得到直线与中心线的相对距离，openmv的线性回归函数还可得出直线与图像底线所形成的夹角，通过得到这两个数据放入pid函数中，pid目标值是循迹线与底线所成夹角为90度，循迹线坐标与中心线重合这样的结果才能使得车辆一直沿着循迹线直线行驶。
循迹线性回归代码：
blobs_over = img.find_blobs([(255,255)], x_stride = 4, y_stride = 3, pixels_threshold=60, merge=True, roi=roi_over)
if(blobs_over): # 划定区域有色块
line = img.get_regression([(255,255)], roi = roi_line)
if (line):
rho_err = abs(line.rho())-img.width()/2 #直线偏移距离
if line.theta()>90: #直线角度
theta_err = line.theta()-180
else:
theta_err = line.theta()
if (line.magnitude()>3):
en_blobs_over = 1
# img.draw_line(line.line(), color = 127) # 画出回归线
rho_output = rho_pid.get_pid(rho_err,1)
theta_output = theta_pid.get_pid(theta_err,1)
sj = rho_output+theta_output
if theta_err<1: # 可以用于矫正陀螺仪
send_data_1_2(sj)
else:
send_data_1_1(sj)
else: # 线性回归效果差
data = ustruct.pack("<bbbHb",
0x24,
0xEE,
0x00,
int(0),
0x63)
uart.write(data)
# print('0xEE')
else:
# 线性回归失败,可能是视野太杂乱了
data = ustruct.pack("<bbbHb",
0x24,
0x78,
0x00,
int(0),
0x63)
uart.write(data)
# print('0x78')
# 调试串口打印
#print("FPS %f" % (clock.fps()))
# print('线性回归效果:{}'.format(line.magnitude() if (line) else 0)) # 加了判断，没line也不会程序崩溃
# print('直线偏移：{}，pid：{}'.format(rho_err,rho_output))
# print('角度：{}，pid：{}'.format(theta_err,theta_output))
# print('sj：{}'.format(sj))
# print('')
elif (en_blobs_over == 1): # 划定区域没有色块 同时 前一次检测是直线循迹,即前一次检测没有路口
# 这个区域没检测到东西没检测到说明到终点了，应该停下
data = ustruct.pack("<bbbHb",
0x24,
0xAE,
0x00,
int(0),
0x63)
uart.write(data)
# 调试串口打印
#print("FPS %f" % (clock.fps()))
# print('0xAE')
else:
# 前一次是路口,现在识别到了终点,未定义的情况;
data = ustruct.pack("<bbbHb",
0x24,
0x1A,
0x00,
int(0),
0x63)
uart.write(data)同时openmv4也负责路口检测，路口检测其实原理很简单，其本质就是对色块进行检测。在摄像头拍摄返回图像内划定部分感兴趣区(roi)，感兴趣区的作用在于特定的函数只会在特定的roi中进行图像处理，在本项目中我们就划定了4个不同的roi，两个位于图像左右两侧用于对路口的检测，在图像顶部有一个长条roi用于宝藏点前停车，中间划定roi用于对循迹线进行线性回归避免图像的其他部分对线性回归进行干扰。
路口检测以及openmv端串口发送代码：
blobs_r = img.find_blobs([(255,255)], x_stride = 2, y_stride = 3, pixels_threshold=80, merge=True, roi=roi_r)
blobs_l = img.find_blobs([(255,255)], x_stride = 2, y_stride = 3, pixels_threshold=80, merge=True, roi=roi_l)
if(blobs_r or blobs_l):
en_blobs_over = 0
data = ustruct.pack("<bbbHb",
0x24,
0xAC,
0x00,
int(0),
0x63)
uart.write(data)
# 调试串口打印
#print("FPS %f" % (clock.fps()))
# print('0xAC')识别部分的openmv
这一部分主要分为三块，宝藏图识别，宝藏识别，路径规划。可以说这一部分包含了项目的全部核心，基于openmv4plus的强大芯片得以实现。
宝藏图识别
宝藏图识别的思路如下：根据参赛规则藏宝图是下图这样的，在藏宝图四个角拥有4个用于透视矫正的4个矫正点用于藏宝图透视矫正。
但由于openmv库并未提供透视矫正函数且openmv也无法运行opencv库要对透视矫正函数进行复现难度太大且透视矫正算法涉及太多运算，就算手搓出透视矫正算法openmv也不一定带的动，所以我们直接放弃了外围4点的透视矫正。在识别图像时人工将宝藏图摆正进行识别，在后面的比赛其他队伍的识图也证明透视矫正效果并不好。因此我们选择直接对藏宝图核心区进行识别。我们使用寻找色块算法实现藏宝图核心区自动框选，效果如下图所示
效果还是很不错的，再自动框选之后将藏宝图核心区截出，随后使用圆形识别函数在图像中查找出8个宝藏点，返回个宝藏点的像素坐标，再利用坐标转换函数将宝藏点的像素坐标转换为地图坐标。得到8个点坐标后由规则可知他们是一一中心对称的，根据这一特性对返回坐标进行验证，建立循环直到得到正确宝藏坐标，完成藏宝图识别。
藏宝图识别代码：
def ditushibie():
i = 1
m = 0
n = 0
t = (100, 255)
roih = 0
roiw = 0
while (n != 8):
i = 1
m = 0
n = 0
while (i == 1):
clock.tick()
img = sensor.snapshot().to_grayscale().laplacian(1, sharpen=True)
blobs = img.find_blobs([(100, 0)], area_threshold=30000, pixels_threshold=2300, merge=True)
for b in blobs:
img.draw_rectangle(b.rect())
roi = b.rect()
roih = b.h()
roiw = b.w()
juxinglist = b.corners()
# print(b.corners())
# print(b.pixels())
# print(roi)
test = img.copy(roi=b.rect(), copy_to_fb=True).binary([t])
test.save("img3.jpg")
if (b[0] != 0):
i = 2
break
if (i == 2):
# print("start!")
circle_list = []
map_list = []
for c in test.find_circles(threshold=4000, x_margin=10, y_margin=10, r_margin=10, r_min=4, r_max=5,
r_step=1):
# print(c)
pyb.LED(1).on()
circle_list.append((c.x(), c.y()))
a = int(c.x() / 17.5)
b = int((180 - c.y()) / 16 + 0.5)
if (a > 10):
a = 10
if (b > 10):
b = 10
map_list.append((a, b))
m += 1
if (roih < 200 and m >= 7 and roiw < 230):
pyb.LED(3).on()
else:
pyb.LED(3).off()
if (m == 8):
# print(circle_list)
for i in range(8):
a = map_list[i][0]
b = map_list[i][1]
for j in range(8):
c = (a + map_list[j][0]) / 2.0
d = (b + map_list[j][1]) / 2.0
if ((c == 5.5) & (d == 5.5)):
n += 1
if (n == 8):
print(map_list)
pyb.LED(1).off()
pyb.LED(3).off()
pyb.LED(2).on()
pyb.delay(200)
pyb.LED(2).off()
return map_list
print(m)
print("End!")
宝藏识别
根据国赛规则宝藏分为四种，蓝方真宝藏，蓝方伪宝藏，红方真宝藏，红方伪宝藏，如下图：
因此通过识别到返回图像中蓝色，红色，黄色，绿色其中的两种颜色通过判断他们的组合就可以知道宝藏的类型，例如：识别得到蓝色，绿色那么这个宝藏就一定是蓝方伪宝藏。那么实现方法也很简单，使用以上述四钟颜色阈值设定的色块查找函数，通过他们的返回值组合就可知道是什么宝藏。这里代码很容易实现，但难点在于阈值的采集。一旦环境光发生变化就会影响识别结果，这个问题赛前很长一段时间一直困扰我们，但深究阈值采集方式之后你会发现他是使用LAB色彩空间建立的阈值。而什么是LAB呢？
同RGB颜色空间相比，Lab是一种不常用的色彩空间。它是在1931年国际照明委员会（CIE）制定的颜色度量国际标准的基础上建立起来的。1976年，经修改后被正式命名为CIELab。它是一种设备无关的颜色系统，也是一种基于生理特征的颜色系统。这也就意味着，它是用数字化的方法来描述人的视觉感应。Lab颜色空间中的L分量用于表示像素的亮度，取值范围是[0,100],表示从纯黑到纯白；a表示从红色到绿色的范围，取值范围是[127,-128]；b表示从黄色到蓝色的范围，取值范围是[127,-128]。
所以其通过调整a和b之后就可以非常精准的查找到目标颜色！说这个也是想告诉朋友们，再使用一项新技术时最好去了解一下他的原理，这样能更好的使用他。
宝藏识别代码：
def baozhangshibie():
# global cn # 用于测试
# cn += 1 # 用于测试
color_list = [0, 0, 0, 0]
clock.tick()
img = sensor.snapshot()
# 识别红色
blobs = img.find_blobs([thresholds[0]], pixels_threshold=900)
if blobs:
color_list[0] = 1
# 识别绿色
blobs = img.find_blobs([thresholds[1]], pixels_threshold=200)
if blobs:
color_list[1] = 1
# 识别黄色
blobs = img.find_blobs([thresholds[2]], pixels_threshold=240)
if blobs:
color_list[2] = 1
# 识别蓝色
blobs = img.find_blobs([thresholds[3]], pixels_threshold=900)
if blobs:
color_list[3] = 1
if color_list == [1, 1, 0, 0]: # red
return 0
elif color_list == [0, 0, 1, 1]: # blue
return 1
elif color_list == [1, 0, 1, 0]: # red_feak
return 2
elif color_list == [0, 1, 0, 1]: # blue_feak
return 3
路径规划
我们使用的是Dijkstra算法。这个算法有很多文章讲解，算法大佬讲的肯定比我好，所以这里就不详细讲了。我们使用该算法进行点对点的规划，而不是全局一次规划，这样可以实现动态规划。如果你仔细看过规则就会发现根据规则中的宝藏摆放是不用个点全部跑一遍的，因此使用动态规划可以节省很多跑图时间。
结语
这次比赛真的学到很多，果然代码还是多练习和接手项目才能进步，也感谢实验室所有的小伙伴。代码非常多，核心代码都已摆出来了。各位大佬也都能看懂，欢迎大家评论交流。谢谢各位！
(ps:实验室新来的学弟学妹想复现学习的来找学长要代码捏)
4224
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
