目录前言预备知识1、什么是RPM2、简单了解PID算法①、PID控制小车控制系统的应用②、什么是PID算法3、简单了解腐蚀膨胀算法寻迹部分K210代码部分讲解K210完整代码OpenMV4代码部分讲解OpenMV4完整代码前言本人所用为01 studio的pyAi-K210和pyAI-OpenMV4本人在学习完K210后学习openMV时发现很多实验K210和openMV可以一起学习所以写下此篇分享本人基本都是个人笔记非常希望有大佬提出错误和改进之处但大佬轻喷哈预备知识1、什么是RPMRPM是转速(Revolutions Per Minute)的缩写,表示每分钟转动次数。它是测量旋转设备如马达、轴、齿轮等旋转频率的单位。比如一个转速为3600 RPM的马达,意味着它的轴每分钟转动3600圈,每秒转动60圈(3600/60)。RPM常用于指定和控制马达、风扇、轴、齿轮等旋转设备的转动速度。例如:- 电动工具(如电钻)的转速- 风扇的转速,控制其风量- 机器人减速器的传动比计算- 3D打印机轴的转速计算两者间的RPM转换公式为:1. RPM 转 rad/s: RPM * π / 302. rad/s 转 RPM: rad/s * 30 / π其中π≈3.14。所以举个例子:- 一个转速3600RPM的马达轴,其对应的弧度每秒为:3600 * 3.14 / 30 188.4 rad/s- 一个转速300rad/s的轴,其RPM为:300 * 30 / 3.14 2850RPM所以RPM是一个很重要的工业单位,理解转速单位对于机械、机电控制很有帮助。2、简单了解PID算法①、PID控制小车控制系统的应用PID控制器在小车中的应用非常广泛,主要用于控制车辆的速度、方向等。例如:1. 速度控制我们可以使用PID控制器来控制小车的速度。设置一个目标速度,然后通过获取小车的编码器读数计算实际速度,并作为PID控制器的反馈。PID控制器输出一个PWM信号来控制电机速度,从而达到速度控制的目的。2. 航向控制我们可以使用PID控制器控制小车的航向。设置一个目标航向角,通过电子罗盘获取小车的实际航向角作为反馈。PID控制器输出PWM信号控制左右电机转速不同,产生差速转向,来控制小车航向,达到航向控制。3. 跟踪控制我们可以采用PID控制器实现小车的路径跟踪。设置一个预定路径,通过获取小车实时位姿,计算路径误差作为反馈。PID控制器输出控制信号,控制小车轮速和转向,使小车能及时修正路径,追踪预定轨迹。4. 姿态控制对于有倾角传感器(陀螺仪)的小车,我们可以使用PID控制器实现小车的姿态控制。设置一个目标倾角,从陀螺仪获取实际倾角作为反馈,PID控制器输出一个控制信号驱动伺服电机,调整小车的倾角,达到姿态控制的目的。等等。总之,PID控制器通过获取反馈信号,计算出控制量来驱动执行机构(电机、舵机等),从而实现对小车各项状态量(速度、航向、倾角等)的控制,这在小车控制系统中有很广泛的应用。②、什么是PID算法PID算法是一种较古老而简单实用的控制回路反馈机制。PID的英文全称为比例-积分-微分(Proportional-Integral-Derivative)。PID控制器会根据误差的比例、积分和微分三个因素产生控制量,从而影响控制对象,使其输出能够达到预期的参考输入。PID算法用数学公式表示为:u(t) Kp*e(t) Ki*∫e(t)dt Kd*de(t)/dt其中:- u(t) - 控制量(输出)- e(t) - 误差,设置值与反馈值之间的差值- Kp - 比例增益,对误差的影响- Ki - 积分增益,对误差积分的影响- Kd - 微分增益,对误差的变化率的影响- ∫e(t)dt - 误差的积分- de(t)/dt - 误差的微分,即误差的变化率PID控制器的目的是控制控制对象(被控系统),使其输出y(t)尽可能跟踪输入的参考值r(t)。其原理是:- 比例项Kp根据误差e(t)的大小调节控制量u(t),使e(t)趋 toward 0。但仅依赖这一项,无法完全消除稳态误差。- 积分项Ki根据误差e(t)的积分∫e(t)dt来消除稳态误差,但是该项会引入超调量,甚至引起系统失稳。- 微分项Kd根据误差e(t)的变化率de(t)/dt来补偿比例项和积分项的不足,加快系统的相应速度和稳定性。通过三项的结合,可以获得一定的控制效果。PID控制简单易用,应用非常广泛,用于工业控制、运动控制和各种自动化系统中。3、简单了解腐蚀膨胀算法膨胀腐蚀算法Dilation-Compression Algorithm是一种基于形态学操作的图像处理算法用于压缩二值图像中的连通区域。该算法的基本思想是将连通区域先进行膨胀操作然后再进行腐蚀操作。通过这样的迭代过程可以将连通区域中的像素逐渐膨胀然后再收缩回原来的形状从而实现连通区域的压缩。具体的算法步骤如下对输入的二值图像进行膨胀操作将连通区域进行扩张。对膨胀后的图像进行腐蚀操作将连通区域进行收缩。迭代以上两个步骤直到连通区域不再发生变化为止。膨胀腐蚀算法常用于图像处理和形态学图像分析中可以对图像进行去噪、连接分割、轮廓提取等操作。它适用于处理具有连通性的图像区域能够有效压缩图像数据并保持区域的形状特征。需要注意的是膨胀腐蚀算法的效果取决于膨胀和腐蚀操作的次数和结构元素的选择。根据具体应用场景可能需要调整算法的参数和操作步骤以达到期望的图像处理效果。需要注意的是在OpenMV4中有集成好的算法--image.close(size[, threshold[, maskNone]])处理图形内部的噪点先膨胀后腐蚀-- image.open(size[, threshold[, maskNone]])处理图形外部背景的噪点先腐蚀后膨胀寻迹部分使用OpenMV4进行巡线通过光敏传感器进行自适应调整阈值将偏转角度传给K210控制编码电机占空比进行调整姿态。K210代码部分讲解# todo 在K210中需要自己先定义IO #导入模块 from fpioa_manager import fm from Maix import GPIO import utime,time,lcd from machine import Timer,PWM,UART #初始化屏幕 lcd.init() lcd.clear(lcd.WHITE) #定义电机IO fm.register(9, fm.fpioa.GPIO0) fm.register(26, fm.fpioa.GPIO1) fm.register(27, fm.fpioa.GPIO2) fm.register(8, fm.fpioa.GPIO3) GPIO(GPIO.GPIO0,GPIO.OUT,value0) GPIO(GPIO.GPIO1,GPIO.OUT,value1) GPIO(GPIO.GPIO2,GPIO.OUT,value1) GPIO(GPIO.GPIO3,GPIO.OUT,value0) #定义电机A/B编码读取IO fm.register(18, fm.fpioa.GPIOHS0) fm.register(19, fm.fpioa.GPIOHS1) fm.register(10, fm.fpioa.GPIOHS2) fm.register(11, fm.fpioa.GPIOHS3) INR_A GPIO(GPIO.GPIOHS0,GPIO.IN) INR_B GPIO(GPIO.GPIOHS1,GPIO.IN) INL_A GPIO(GPIO.GPIOHS2,GPIO.IN) INL_B GPIO(GPIO.GPIOHS3,GPIO.IN) 定义串口IO fm.register(6, fm.fpioa.UART1_RX, forceTrue) fm.register(7, fm.fpioa.UART1_TX, forceTrue) #初始串口 uartUART(UART.UART1, 9600, read_buf_len4096)# todo 创建所需要的PID类 #此处我就用了 P和D 防止溢出即可 class PID: def __init__(self, kp0.0, kd0.0): self.kp kp self.kd kd reset() def reset(self): self.p_error 0.0 self.d_error 0.0 def compute(self, error,last_eror0): self.p_error error self.d_error error - last_eror return self.kp*self.p_error self.kd*self.d_error pid_L PID(kp0.3,kd0.01) #左电机参数 pid_R PID(kp0.3,kd0.01) #右电机参数#给出后续需要的参数默认值 last_error_L 0 last_error_R 0 pos_cnt_L0 pos_cnt_R0 encoder_a_last 0 encoder_b_last 0 output_L 30 output_R 30 _001 0 _002 0 读取#右编码电机的信号 def clk_callback_R(INR_A): global pos_cnt_R,encoder_a_last if INR_A.value() ! encoder_a_last: if INR_B.value() ! encoder_a_last: pos_cnt_R 1 else: pos_cnt_R -1 encoder_a_last INR_A.value() INR_A.irq(clk_callback_R, GPIO.IRQ_FALLING) #读取左编码电机的信号 def clk_callback_L(INL_A): global encoder_b_last,pos_cnt_L if INL_A.value() ! encoder_b_last: if INL_B.value() ! encoder_b_last: pos_cnt_L 1 else: pos_cnt_L -1 encoder_b_last INL_A.value() INL_A.irq(clk_callback_L, GPIO.IRQ_FALLING) #设置PWM定时器 tim_01 Timer(Timer.TIMER0,Timer.CHANNEL0,modeTimer.MODE_PWM) tim_02 Timer(Timer.TIMER0,Timer.CHANNEL1,modeTimer.MODE_PWM) #驱动电机函数及PWM def IN_run(output_L,output_R): PWM(tim_01, freq50, dutyoutput_L ,pin15) PWM(tim_02, freq50, dutyoutput_R, pin17) # 设置目标速度 target_speed_L 100 target_speed_R 100#主要运行步骤-可以用main函数写然后调用: while True: JiaoDu_error0 #初始化角度误差值 text uart.read() # 读取串口数据 if text: # 如果读取到了数据 JiaoDu_error text.decode(utf-8) #print(JiaoDu_error) #这里进行 try.....except Exception as e: 处理因为我们是直接串口读取未进行串口协议有时候会一次传输多组数据导致报错这个给出异常处理增加代码鲁棒性 try: JD_out int(float(JiaoDu_error)) #串口读取的是str类型要进行处理 #print(JD_out) if (JD_out ! 0): if (JD_out 0): if (JD_out 25): if (JD_out 50): target_speed_R 100 target_speed_L 10 elif (JD_out 25): target_speed_R 100 target_speed_L 50 elif (JD_out 0): if (JD_out -25): if (JD_out -50): target_speed_R 10 target_speed_L 100 elif (JD_out -25): target_speed_R 50 target_speed_L 100 else: target_speed_R 100 target_speed_L 100 except Exception as e: _001 1 lcd.draw_string(0,80,_001:%d %(_001),lcd.BLACK, lcd.WHITE) curr_speed_L0 #左电机速度 curr_speed_R0 #右电机速度 #读取编码器计数器计算速度 for i in range(10): time.sleep_ms(20) curr_speed_L pos_cnt_L/0.02 curr_speed_R pos_cnt_R/0.02 pos_cnt_L0 pos_cnt_R0 curr_speed_R curr_speed_R curr_speed_L curr_speed_L # PID计算PWM-duty #print(curr_speed/10) error_L target_speed_L - (curr_speed_L//20) error_R target_speed_R - (curr_speed_R//20) output_L abs(pid_L.compute(error_L,last_error_L)) output_R abs(pid_R.compute(error_R,last_error_R)) #print(output_L,***,output_R) last_error_L error_L last_error_R error_R time.sleep(0.01) IN_run(output_L,output_R) print(output_L,output_R) try: lcd.draw_string(0,0,URAT: %s %(JiaoDu_error) ,lcd.BLACK, lcd.WHITE) lcd.draw_string(0,20, output_L:%d/n %(output_L) ,lcd.BLACK, lcd.WHITE) lcd.draw_string(0,40,output_R:%d/n %(output_R),lcd.BLACK, lcd.WHITE) except Exception as e: _002 1 lcd.draw_string(0,60,_002:%d %(_002),lcd.BLACK, lcd.WHITE)K210完整代码from fpioa_manager import fm from Maix import GPIO import utime,time,lcd from machine import Timer,PWM,UART lcd.init() lcd.clear(lcd.WHITE) last_error_L 0 last_error_R 0 pos_cnt_L0 pos_cnt_R0 encoder_a_last 0 encoder_b_last 0 output_L 30 output_R 30 _001 0 _002 0 fm.register(9, fm.fpioa.GPIO0) fm.register(26, fm.fpioa.GPIO1) fm.register(27, fm.fpioa.GPIO2) fm.register(8, fm.fpioa.GPIO3) fm.register(18, fm.fpioa.GPIOHS0) fm.register(19, fm.fpioa.GPIOHS1) fm.register(10, fm.fpioa.GPIOHS2) fm.register(11, fm.fpioa.GPIOHS3) fm.register(6, fm.fpioa.UART1_RX, forceTrue) fm.register(7, fm.fpioa.UART1_TX, forceTrue) GPIO(GPIO.GPIO0,GPIO.OUT,value0) GPIO(GPIO.GPIO1,GPIO.OUT,value1) GPIO(GPIO.GPIO2,GPIO.OUT,value1) GPIO(GPIO.GPIO3,GPIO.OUT,value0) INR_A GPIO(GPIO.GPIOHS0,GPIO.IN) INR_B GPIO(GPIO.GPIOHS1,GPIO.IN) INL_A GPIO(GPIO.GPIOHS2,GPIO.IN) INL_B GPIO(GPIO.GPIOHS3,GPIO.IN) uartUART(UART.UART1, 9600, read_buf_len4096) #创建PID类 class PID: def __init__(self, kp0.0, kd0.0): self.kp kp self.kd kd def reset(self): self.p_error 0.0 self.d_error 0.0 def compute(self, error,last_eror0): self.p_error error self.d_error error - last_eror return self.kp*self.p_error self.kd*self.d_error pid_L PID(kp0.3,kd0.01) pid_R PID(kp0.3,kd0.01) #创建定时器 tim_01 Timer(Timer.TIMER0,Timer.CHANNEL0,modeTimer.MODE_PWM) tim_02 Timer(Timer.TIMER0,Timer.CHANNEL1,modeTimer.MODE_PWM) def clk_callback_R(INR_A): global pos_cnt_R,encoder_a_last if INR_A.value() ! encoder_a_last: if INR_B.value() ! encoder_a_last: pos_cnt_R 1 else: pos_cnt_R -1 encoder_a_last INR_A.value() INR_A.irq(clk_callback_R, GPIO.IRQ_FALLING) def clk_callback_L(INL_A): global encoder_b_last,pos_cnt_L if INL_A.value() ! encoder_b_last: if INL_B.value() ! encoder_b_last: pos_cnt_L 1 else: pos_cnt_L -1 encoder_b_last INL_A.value() INL_A.irq(clk_callback_L, GPIO.IRQ_FALLING) def IN_run(output_L,output_R): PWM(tim_01, freq50, dutyoutput_L ,pin15) PWM(tim_02, freq50, dutyoutput_R, pin17) # 设置目标速度 target_speed_L 100 target_speed_R 100 while True: JiaoDu_error0 text uart.read() # 读取数据 if text: # 如果读取到了数据 JiaoDu_error text.decode(utf-8) #print(JiaoDu_error) try: JD_out int(float(JiaoDu_error)) #print(JD_out) if (JD_out ! 0): if (JD_out 0): if (JD_out 25): if (JD_out 50): target_speed_R 100 target_speed_L 10 elif (JD_out 25): target_speed_R 100 target_speed_L 50 elif (JD_out 0): if (JD_out -25): if (JD_out -50): target_speed_R 10 target_speed_L 100 elif (JD_out -25): target_speed_R 50 target_speed_L 100 else: target_speed_R 100 target_speed_L 100 except Exception as e: _001 1 lcd.draw_string(0,80,_001:%d %(_001),lcd.BLACK, lcd.WHITE) curr_speed_L0 curr_speed_R0 #读取编码器计数器计算速度 for i in range(10): time.sleep_ms(20) curr_speed_L pos_cnt_L/0.02 curr_speed_R pos_cnt_R/0.02 pos_cnt_L0 pos_cnt_R0 curr_speed_R curr_speed_R curr_speed_L curr_speed_L # PID计算PWM #print(curr_speed/10) error_L target_speed_L - (curr_speed_L//20) error_R target_speed_R - (curr_speed_R//20) output_L abs(pid_L.compute(error_L,last_error_L)) output_R abs(pid_R.compute(error_R,last_error_R)) #print(output_L,***,output_R) last_error_L error_L last_error_R error_R time.sleep(0.01) IN_run(output_L,output_R) print(output_L,output_R) try: lcd.draw_string(0,0,URAT: %s %(JiaoDu_error) ,lcd.BLACK, lcd.WHITE) lcd.draw_string(0,20, output_L:%d/n %(output_L) ,lcd.BLACK, lcd.WHITE) lcd.draw_string(0,40,output_R:%d/n %(output_R),lcd.BLACK, lcd.WHITE) except Exception as e: _002 1 lcd.draw_string(0,60,_002:%d %(_002),lcd.BLACK, lcd.WHITE)OpenMV4代码部分讲解#导入模块 import sensor, image, time, math,pyb from pyb import UART #光敏传感器adc初始化 p6 pyb.Pin(P6, pyb.Pin.IN) adc pyb.ADC(p6) #串口初始化 uart UART(3,9600,timeout_char1000) text_1 0 # 追踪黑线。使用 [(128, 255)] 追踪白线.这里加光敏传感器进行自适应threshold。 #GRAYSCALE_THRESHOLD (0, 70) threshold_min0 threshold_max100 ROIS [ # [ROI, weight] (0, 100, 160, 20, 0.8), # 可以根据不同机器人情况进行调整。 (0, 50, 160, 20, 0.6), (0, 20, 160, 20, 0.4) ] # 计算以上 3 个矩形的权值【weight】的和和不需要一定为 1. weight_sum 0 for r in ROIS: weight_sum r[4] # r[4] 为矩形权重值. # 摄像头配置 sensor.reset() # 初始化. sensor.set_pixformat(sensor.GRAYSCALE) # 使用灰度图像. sensor.set_framesize(sensor.QQVGA) # 使用 QQVGA 分辨率160*120. sensor.skip_frames(time 2000) # 等待摄像头稳定. sensor.set_auto_gain(False) # 必须关闭自动增益 sensor.set_auto_exposure(True) sensor.set_auto_whitebal(True) # 必须关闭白平衡 sensor.set_vflip(0) #摄像头翻转 clock time.clock() # Tracks FPS.#主要运行部分 while(True): intensity_1 0 clock.tick() # Track elapsed milliseconds between snapshots(). #光敏传感器信号处理 for i in range(300): intensity adc.read() intensity_1 intensity intensity_1 intensity_1//300 #print(intensity_1) #根据光敏传感器进行调整threshold if 10intensity_17: threshold_min0 threshold_max60 elif 3intensity_17: threshold_min0 threshold_max100 else: threshold_min80 threshold_max150 #print((threshold_min,threshold_max)) img sensor.snapshot() # 实时拍照. #进行膨胀腐蚀算法处理其中参数要根据实际情况进行调整 imgimg.close(3,100).open(3,130) imgimg.mean(3) imgimg.close(3,50).open(3,100) # 锐化 kernel_sharp (-1, 4, -1, 4, 4, 4, -1, 4, -1) imgimg.morph(1,kernel_sharp).close(3,70).open(3,90) #二值化 imgimg.binary([(threshold_min,threshold_max)],invert1).close(3,126).open(3,200) centroid_sum 0 for r in ROIS: blobs img.find_blobs([(threshold_min,threshold_max)], roir[0:4], mergeTrue,pixels_threshold200, area_threshold200) # r[0:4] 是上面定义的 roi 元组. if blobs: # Find the blob with the most pixels. largest_blob max(blobs, keylambda b: b.pixels()) # Draw a rect around the blob. img.draw_rectangle(largest_blob.rect()) img.draw_cross(largest_blob.cx(),largest_blob.cy()) # r[4] 是每个 roi 的权重值. centroid_sum largest_blob.cx() * r[4] center_pos (centroid_sum / weight_sum) # 确定直线的中心. # 将直线中心位置转换成角度便于机器人处理. deflection_angle 0 # 使用反正切函数计算直线中心偏离角度。可以自行画图理解 #权重 X 坐标落在图像左半部分记作正偏落在右边部分记为负偏 #所以计算结果加负号。 deflection_angle -math.atan((center_pos-80)/60) # 将偏离值转换成偏离角度. deflection_angle math.degrees(deflection_angle) #通过串口输出偏转角度给K210 if abs(deflection_angle)100: text_1 deflection_angle print(text_1) text str(text_1) uart.write(text.encode()) text_1,sum 0,0 elseprint(数据出现错误)OpenMV4完整代码import sensor, image, time, math,pyb from pyb import UART p6 pyb.Pin(P6, pyb.Pin.IN) adc pyb.ADC(p6) uart UART(3,9600,timeout_char1000) text_1 0 # 追踪黑线。使用 [(128, 255)] 追踪白线. #GRAYSCALE_THRESHOLD (0, 70) threshold_min0 threshold_max100 ROIS [ # [ROI, weight] (0, 100, 160, 20, 0.8), # 可以根据不同机器人情况进行调整。 (0, 50, 160, 20, 0.6), (0, 20, 160, 20, 0.4) ] # 计算以上 3 个矩形的权值【weight】的和和不需要一定为 1. weight_sum 0 for r in ROIS: weight_sum r[4] # r[4] 为矩形权重值. # 摄像头配置 sensor.reset() # 初始化. sensor.set_pixformat(sensor.GRAYSCALE) # 使用灰度图像. sensor.set_framesize(sensor.QQVGA) # 使用 QQVGA 分辨率160*120. sensor.skip_frames(time 2000) # 等待摄像头稳定. sensor.set_auto_gain(False) # 必须关闭自动增益 sensor.set_auto_exposure(True) sensor.set_auto_whitebal(True) # 必须关闭白平衡 sensor.set_vflip(0) clock time.clock() # Tracks FPS. while(True): intensity_1 0 clock.tick() # Track elapsed milliseconds between snapshots(). for i in range(300): intensity adc.read() intensity_1 intensity intensity_1 intensity_1//300 #print(intensity_1) if 10intensity_17: threshold_min0 threshold_max60 elif 3intensity_17: threshold_min0 threshold_max100 else: threshold_min80 threshold_max150 #print((threshold_min,threshold_max)) img sensor.snapshot() # 实时拍照. img img.laplacian(2, sharpenTrue) imgimg.close(3,100).open(3,130) imgimg.mean(3) imgimg.close(3,50).open(3,100) # 锐化 kernel_sharp (-1, 4, -1, 4, 4, 4, -1, 4, -1) imgimg.morph(1,kernel_sharp).close(3,70).open(3,90) #二值化 imgimg.binary([(threshold_min,threshold_max)],invert1).close(3,126).open(3,200) centroid_sum 0 for r in ROIS: blobs img.find_blobs([(threshold_min,threshold_max)], roir[0:4], mergeTrue,pixels_threshold200, area_threshold200) # r[0:4] 是上面定义的 roi 元组. if blobs: # Find the blob with the most pixels. largest_blob max(blobs, keylambda b: b.pixels()) # Draw a rect around the blob. img.draw_rectangle(largest_blob.rect()) img.draw_cross(largest_blob.cx(),largest_blob.cy()) # r[4] 是每个 roi 的权重值. centroid_sum largest_blob.cx() * r[4] center_pos (centroid_sum / weight_sum) # 确定直线的中心. # 将直线中心位置转换成角度便于机器人处理. deflection_angle 0 # 使用反正切函数计算直线中心偏离角度。可以自行画图理解 #权重 X 坐标落在图像左半部分记作正偏落在右边部分记为负偏 #所以计算结果加负号。 deflection_angle -math.atan((center_pos-80)/60) # 将偏离值转换成偏离角度. deflection_angle math.degrees(deflection_angle) if abs(deflection_angle)100: text_1 deflection_angle print(text_1) text str(text_1) uart.write(text.encode()) text_1,sum 0,0以上代码本人测试可用但是学习仅供参考需根据自己实际情况进行修改哦