智能送药小车设计详解
1、赛题回顾
2、系统方案
- 根据题目要求，基本需要无线通讯模块，直流电机，摄像头以及陀螺仪等设备，用每次开始时摄像头数字识别到的内容决定去往的病房。摄像头用于阈值巡线和数字识别，根据采集的数据来控制小车沿着标识的红线前进，去往指定药房；陀螺仪用于采集角度信息，控制小车的转向；光电编码器用于计算行进的距离，落在指定的区域。通过单片机，利用PID算法进行控制，使小车完成送药过程，同时通过LED给出提示。通过无线通信模块实现双机通信。
2.1、摄像头寻线算法的分析
黑色硬质小球，摄像头采集图像通过硬件二值化：
- （1）处理二值化图像，黑为0，白为255.
- （2）将0，255化的图像装进一个二维数组。
- （3）进行行扫描。
- （4）进行列扫描。
- （5）如果为255，则分别将x，y坐标输出到两个一维数组。
- （6）该一维数组第一个和最后一个数组相加除以2，即为（x，y）坐标。
2.2、摄像头数字识别算法分析
- 增量式PID控制将当前时刻的控制量和上一时刻的控制量做差，以差值为新的控制量，是一种递推式的算法。增量式PID控制主要是通过求出增量，将原先的积分环节的累积作用进行了替换，避免积分环节占用大量计算性能和存储空间。
增量式PID控制的主要优点为：
- ①算式中不需要累加。控制增量Δu(k)的确定仅与最近3次的采样值有关，容易通过加权处理获得比较好的控制效果；
- ②计算机每次只输出控制增量，即对应执行机构位置的变化量，故机器发生故障时影响范围小、不会严重影响生产过程；
- ③手动—自动切换时冲击小。当控制从手动向自动切换时，可以作到无扰动切换。
3、电路设计
3.1、A4950电机驱动模块
- 本项目中电机驱动模块，采用A4950双路电机驱动模块，相比于TB6122和L298电机驱动，其负载电流大，正负相转换快速稳定，驱动电压，驱动频率范围大。
- 通过stm32f401主控芯片的定时器TIM3的通道1，2，3，4输出四路1KHZ PWM波，左轮的有效驱动占空比等于通道1与、通道2的占空比之差，右轮的有效占空比等于通道3、通道4的占空比之差；当两通道占空比相等时为制动模式；两通道一个占空比为1，另一个占空比大于1时为驱动模式。
- 通过PID算法对PWM波的占空比进行实时调制，实现对本项目中对小车高速巡线时的速度，转向，差速控制。
3.2、OpenmvH7识别电路
- OpenmvH7是一款基于stm32H7高性能系列的一个micropython开源视觉项目，通过其功能丰富的视觉算法和灵活方便的接口，能够实现本项目中红线识别和小车路径规划。
视觉巡线识别：
- 对有效像素点进行线性回归算法，得到红线的线性拟合参数:倾角和几何中心，得到小车当前偏移红线的方向角误差和偏心位移误差，并能通过openmv的USART3将误差信息传输到主控stm32f401。通过巡线环-位移环-速度环三串级PID，实现对小车巡线前进的实时控制。
OPENMV数字识别：
- OPENMV先识别出黑色数字部分矩形，再对该区域进行特征点匹配，选取匹配程度最高的那个数字作为目标数字
4、程序设计
4.1、STM32与OPENMV通信
- 本项目中采用openmv进行巡线误差采集和实时数字检测，并通过串口将采集信息发送到stm32主控。
void uart2_init(u32 bound){
//GPIO端口设置
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOA时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2,ENABLE);//使能USART1时钟
//串口1对应引脚复用映射
GPIO_PinAFConfig(GPIOA,GPIO_PinSource2,GPIO_AF_USART2); //GPIOA9复用为USART1
GPIO_PinAFConfig(GPIOA,GPIO_PinSource3,GPIO_AF_USART2); //GPIOA10复用为USART1
//USART1端口配置
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3; //GPIOA9与GPIOA10
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //速度50MHz
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
GPIO_Init(GPIOA,&GPIO_InitStructure); //初始化PA9，PA10
//USART1 初始化设置
USART_InitStructure.USART_BaudRate = bound;//波特率设置
USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位
USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
USART_InitStructure.USART_Mode = USART_Mode_Rx ; //收发模式
USART_Init(USART2, &USART_InitStructure); //初始化串口1
USART_Cmd(USART2, ENABLE); //使能串口1
// USART_ClearFlag(USART2, USART_FLAG_TC);
//Usart1 NVIC 配置
NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;//串口1中断通道
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0;//抢占优先级3
NVIC_InitStructure.NVIC_IRQChannelSubPriority =0; //子优先级3
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
NVIC_Init(&NVIC_InitStructure); //根据指定的参数初始化VIC寄存器、
USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);//开启相关中断
}
void USART2_IRQHandler(void)
{
u8 Res;
LED_COUNT++;
if(LED_COUNT>10)
LED=~LED,LED_COUNT=0;
if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET)
{
Res = USART_ReceiveData(USART2);
if(Res=='&')
uart2_res_flag=1;
if(uart2_res_flag==1)
USART_RX_BUF2[USART2_RX_STA++] = Res;
if(uart2_res_flag==1&&Res=='*')
{
sbz_parse((char*)USART_RX_BUF2,u2,USART2_RX_STA,3);
uart2_res_flag=0;
USART2_RX_STA=0;
u21=u2[0]; //开始解析数据
LEN2=u21%10;
u21=(u21-LEN2)/10;
LEN1=u21%10;
u21=(u21-LEN1)/10;
NNUM=u21%10;
u21=(u21-NNUM)/10;
UNUM2=u21%10;
u21=(u21-UNUM2)/10;
UNUM1=u21%10;
}
}
}
4.2、药物检测
- 当药物覆盖超声测距模块接口，超声模块ECHO无法接收，导致测距值为0，实现药物的有和无判断
void HW_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_9;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOB, &GPIO_InitStruct);
}
void PUT_TEST_HW(void)
{
int count1=0;
int n=3;
if(PUT_TEST_FLAG==1)
{
for (int i=0;i<n;i++)
{
if(ECHO==1) count1++;
// printfloat(ECHO);
delay_ms(100);
}
if(PUT_TARGET==1)
{
if(count1==n) PUT_FLAG=1;
}
if(PUT_TARGET==0)
{
if(count1==0) PUT_FLAG=0;
}
}
else if(PUT_TEST_FLAG==0)
{
PUT_FLAG=2;
}
}
4.3、PID算法
4.3.1、小车巡线控制程序设计
- 小车巡线控制程序采用巡线环、位移环、速度环三串级PID。Stm32主控使用openmv所采集的偏心误差和方向误差代入巡线环，将输出传递给速度坏，使小车两轮产生差速，小车行进方向始终对准红线方向。
4.3.2、小车转向控制程序设计
- 小车转向控制程序采用角度环、位移环、速度环三串级PID。小车转向的角度应该和小车两轮位移差，轮距成正比。通过测量小车轮距为9.9cm，将角度环输出代入位移环、速度环，即可控制小车的精确转向角度。
int go(float x2,float x4,float v2,float v4)
{
pid_go_x2.kp=0.7;
pid_go_x2.max=v2;
pid_go_x2.min=-v2;
pid_go_x2.target=x2;
pid_go_x2.cx=encoder.x2;
pid_go_x4.kp=0.7; ;
pid_go_x4.max=v4;
pid_go_x4.min=-v4;
pid_go_x4.target=x4;
pid_go_x4.cx=encoder.x4;
pid_go_x2.watch_dog=x2>0?x2*8:(-x2)*8;
pid_go_x4.watch_dog=x2>0?x2*8:(-x2)*8;
pid_init3(&pid_go_x2);
pid_init3(&pid_go_x4);
mov(pid_go_x2.output,pid_go_x4.output);
// printfloat(pid_go_x2.e[0]);
return pid_go_x2.finish & pid_go_x4.finish;
}
5、OPENMV巡线识别
def get_redline(img):
ROI_H=4
W=128
H=160
POINT=[0,0]
count=0
length=[0,0]
for i in [int(H/ROI_H/1.3),int(H/ROI_H/1.125)]:
FIRST=0
LAST=0
for j in [8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26]:
ROI=(int(j*ROI_H),int(i*ROI_H),ROI_H,ROI_H)
statistics=img.get_statistics(roi=ROI)
COLOR_THRESHOLD=[statistics.l_mean() ,statistics.a_mean() ,statistics.b_mean()]
img.draw_rectangle((int(j*ROI_H),int(i*ROI_H),ROI_H,ROI_H),color=(0,0,255))
if PIXEL_TEST(COLOR_THRESHOLD,RED_THRESHOLD)==1:
img.draw_rectangle((int(j*ROI_H),int(i*ROI_H),ROI_H,ROI_H),color=(255,0,0))
if FIRST==0:
FIRST=j-17
LAST=j-17
POINT[count]=(LAST+FIRST)/2
count+=1
ROI_H=4
for i in [0]:
for j in [11,12,13,16,17,18,21,22,23]:
ROI=(int(j*ROI_H),int(i*ROI_H),ROI_H,ROI_H)
statistics=img.get_statistics(roi=ROI)
COLOR_THRESHOLD=[statistics.l_mean() ,statistics.a_mean() ,statistics.b_mean()]
img.draw_rectangle((int(j*ROI_H),int(i*ROI_H),ROI_H,ROI_H),color=(0,0,255))
if PIXEL_TEST(COLOR_THRESHOLD,RED_THRESHOLD)==1:
img.draw_rectangle((int(j*ROI_H),int(i*ROI_H),ROI_H,ROI_H),color=(255,0,0))
length[0]+=1
ROI_H=10
for i in [int(H/ROI_H)-1]:
for j in [2,3,5,6,7,8,10,11]:
ROI=(int(j*ROI_H),int(i*ROI_H),ROI_H,ROI_H)
statistics=img.get_statistics(roi=ROI)
COLOR_THRESHOLD=[statistics.l_mean() ,statistics.a_mean() ,statistics.b_mean()]
img.draw_rectangle((int(j*ROI_H),int(i*ROI_H),ROI_H,ROI_H),color=(0,0,255))
if PIXEL_TEST(COLOR_THRESHOLD,RED_THRESHOLD)==1:
img.draw_rectangle((int(j*ROI_H),int(i*ROI_H),ROI_H,ROI_H),color=(255,0,0))
length[1]+=1
return [(POINT[0]-POINT[1]),(POINT[0]+POINT[1])/2,length[0],length[1]]
6、OPENMV数字识别
def Get_Num(d):
data=[test1(d),test2(d),test3(d),test4(d),test5(d),test6(d),test7(d),test8(d)]
num=1
target=max(data)
for i in range(len(data)):
if data[i]==target:
return i+1
扫一扫
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
