非常详细的视频和文字教程，讲解常见的openmv教程包括 巡线、物体识别、圆环识别、阈值自动获取等。非常适合学习openmv、K210、K230等项目
视频合集链接在:
openmv教程合集 openmv入门到项目开发 openmv和STM32通信 openmv和opencv区别 openmv巡线 openmv数字识别教程LCD
下面的代码是自己写几个点的坐标，模拟后面识别到的物体，然后把前几个点给卡尔曼算法，卡尔曼算法就会预测计算出后面的点轨迹
注意为了方便演示，这里给系统增加了500ms延时时间
time. sleep_ms(500) #方便演示增加500ms延时时间
所以我们就想要大概调整下Ts时间
Ts = 1/2 #Ts = 1 帧率的倒数 这个要根据延时手动调节
import sensor, image, time
#*********[0]-增加卡尔曼计算需要数学计算的模块导入*********************************************
import sensor, image, time, math
from ulab import numpy as np
#教程作者:好家伙VCC
#欢迎交流群QQ: 771027961 作者邮箱: 1930299709@qq.com
#更多教程B站主页:[好家伙VCC的个人空间-好家伙VCC个人主页-哔哩哔哩视频](https://space.bilibili.com/434192043)
#淘宝主页链接:[首页-好家伙VCC-淘宝网](https://shop415231378.taobao.com)
#更多嵌入式手把手教程-尽在好家伙VCC
# 初始化摄像头模块
sensor.reset() # 重置摄像头，确保设备正常工作
sensor.set_pixformat(sensor.RGB565) # 设置摄像头的像素格式为RGB565，每个像素16位色深
sensor.set_framesize(sensor.QQVGA) # 设置摄像头的分辨率为QQVGA（160x120），适合快速处理
# *************************** 如果不需要镜像就注释掉以下代码 **************************
# 摄像头镜像和翻转设置，根据摄像头的安装方向调整
sensor.set_vflip(True) # 设置垂直翻转，适用于摄像头上下安装的情况
sensor.set_hmirror(True) # 设置水平翻转，适用于摄像头左右安装的情况
# *************************** 如果不需要镜像就注释掉以上代码 **************************
sensor.skip_frames(time = 2000) # 跳过前几帧的图像，确保图像稳定后再开始处理
sensor.set_auto_gain(False) # 必须关闭自动增益，防止影响颜色追踪
sensor.set_auto_whitebal(False) # 必须关闭自动白平衡，防止影响颜色追踪
# 创建一个时钟对象，用于计算和控制帧率
clock = time.clock()
#****************演示预测定义的变量 用来表示要识别到的点************
ball_positions = [#*演示预测定义的变量 这个是80个点
[20, 50, 5],
[22, 50, 5],
[24, 50, 5],
[26, 50, 5],
[28, 50, 5],
[30, 50, 5],
[32, 50, 5],
[34, 50, 5],
[36, 50, 5],
[38, 50, 5],
[40, 50, 5],
[42, 50, 5],
[44, 50, 5],
[46, 50, 5],
[48, 50, 5],
[50, 50, 5],
[52, 50, 5],
[54, 50, 5],
[56, 50, 5],
[58, 50, 5],
[60, 50, 5],
[62, 50, 5],
[64, 50, 5],
[66, 50, 5],
[68, 50, 5],
[70, 50, 5],
[72, 50, 5],
[74, 50, 5],
[76, 50, 5],
[78, 50, 5],
[80, 50, 5],
[82, 50, 5],
[84, 50, 5],
[86, 50, 5],
[88, 50, 5],
[90, 50, 5],
[92, 50, 5],
[94, 50, 5],
[96, 50, 5],
[98, 50, 5],
[100, 50, 5],
[102, 50, 5],
[104, 50, 5],
[106, 50, 5],
[108, 50, 5],
[110, 50, 5],
[112, 50, 5],
[114, 50, 5],
[116, 50, 5],
[118, 50, 5],
[120, 50, 5],
[122, 50, 5],
[124, 50, 5],
[126, 50, 5],
[128, 50, 5],
[130, 50, 5],
[132, 50, 5],
[134, 50, 5],
[136, 50, 5],
[138, 50, 5],
[140, 50, 5],
[142, 50, 5],
[144, 50, 5],
[146, 50, 5],
[148, 50, 5],
[150, 50, 5],
[152, 50, 5],
[154, 50, 5],
[156, 50, 5],
[158, 50, 5],
[160, 50, 5],
[162, 50, 5],
[164, 50, 5],
[166, 50, 5],
[168, 50, 5],
[170, 50, 5],
[172, 50, 5],
[174, 50, 5],
[176, 50, 5],
[178, 50, 5],
[180, 50, 5],
[182, 50, 5],
[184, 50, 5],
[186, 50, 5],
[188, 50, 5],
[190, 50, 5],
[192, 50, 5],
[194, 50, 5],
[196, 50, 5],
[198, 50, 5],
[200, 50, 5]
]
# 设置遍历的起始索引和最大索引
start_index = 0 # 开始从哪个位置进行识别
#*********************[1]-增加卡尔曼相关变量定义**有些参数需要根据情况调整 比如Ts*************************
Ts = 1/2 #Ts = 1 帧率的倒数 这个要根据延时手动调节
# 状态空间矩阵定义
# 状态转移矩阵 A，描述系统的状态变化
#A = np.array([[1,0,0,0,Ts,0],[0,1,0,0,0,Ts],[0,0,1,0,0,0],[0,0,0,1,0,0],[0,0,0,0,1,0],[0,0,0,0,0,1]])
# 改进状态转移矩阵A，将速度部分更明确地包含进来A 是状态转移矩阵，描述了系统状态（位置、速度等）的变化。该矩阵会把当前的状态转换到下一时刻的状态。
A = np.array([[1, 0, 0, 0, Ts, 0],
[0, 1, 0, 0, 0, Ts],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]])
# 观测矩阵 C，描述从状态到观测值的映射关系 C 是观测矩阵，它将状态向量（位置、速度）与观测量（图像中的矩形框信息）联系起来。这里假设观测量是位置和速度。
C = np.array([[1,0,0,0,0,0],[0,1,0,0,0,0],[0,0,1,0,0,0],[0,0,0,1,0,0],[0,0,0,0,1,0],[0,0,0,0,0,1]])
# 过程噪声协方差矩阵
# 过程噪声协方差矩阵 Q，用于描述过程的随机噪声
#Q_value = [1e-8 for _ in range(6)]
#Q = np.diag(Q_value) #创建对角矩阵
# 增大过程噪声矩阵Q_value，使得卡尔曼滤波更灵活地应对物体快速运动
#Q 是过程噪声协方差矩阵，用于描述系统过程中的不确定性。矩阵的对角元素表示每个状态变量的噪声水平。
Q_value = [1e-6 for _ in range(6)] # 调整过程噪声值Q_value = [1e-6 for _ in range(6)]
Q = np.diag(Q_value) # 更新过程噪声协方差矩阵
# 观测噪声协方差矩阵 R 是观测噪声协方差矩阵，表示观测过程中测量误差的大小。
R_value = [1e-6 for _ in range(6)]
R = np.diag(R_value)
# 定义观测量Z
x = 0 #左顶点x坐标
y = 0 #左顶点y坐标
last_frame_x = x #上一帧左顶点x坐标
last_frame_y = y #上一帧左顶点y坐标
w = 0 #矩形框宽度w
h = 0 #矩形框高度h
dx = 0 #左顶点x坐标移动速度
dy = 0 #左顶点y坐标移动速度
Z = np.array([x,y,w,h,dx,dy])
# 定义卡尔曼滤波函数变量
#x_hat = np.array([80,60,30,30,2,2]) # 初始估计的状态值（位置、速度等）
# 初始状态估计：根据实际应用情况调整位置和速度的初值
x_hat = np.array([80, 60, 30, 30, 2, 2]) # 根据你的应用需要调整这些值
x_hat_minus = np.array([0,0,0,0,0,0]) # 初始预测的状态值
p_value = [10 for _ in range(6)] # 状态误差的初始值 p 是状态误差的初始协方差矩阵。
p = np.diag(p_value)# 创建误差协方差矩阵 p
#*********************[2]-增加卡尔曼滤波函数**这个不需要动基本复制过去就可以用***********************************************************************
# 卡尔曼滤波函数
#预测阶段：利用状态转移矩阵和上一状态估计预测当前状态。
#校正阶段：通过卡尔曼增益对预测状态进行校正，使得估计值接近真实值。
#输入 Z：观测值（或测量值），通常是来自外部传感器（例如相机、雷达等）的数据。在这个代码中，Z 是一个包含目标的位置信息（如矩形框的四个角坐标）的向量，格式为 [x, y, w, h, dx, dy]，其中 x 和 y 是目标的中心位置，w 和 h 是目标的宽度和高度，dx 和 dy 是目标的速度。
#输出 x_hat：更新后的状态估计，包括位置（x, y）、宽度（w, h）、速度（dx, dy）。该值是通过卡尔曼滤波器的预测和校正步骤计算得到的最优估计。
def Kalman_Filter(Z):
global A,C,Q,R,x_hat,x_hat_minus,p
# 预测部分
x_hat_minus = np.dot(A,x_hat)
p_minus = np.dot(A, np.dot(p, A.T)) + Q
# 校正部分
S = np.dot(np.dot(C, p_minus), C.T) + R
# 选择一个小的正则化项
regularization_term = 1e-4
# 正则化 S 矩阵
S_regularized = S + regularization_term * np.eye(S.shape[0])
# 计算正则化后的 S 矩阵的逆
S_inv = np.linalg.inv(S_regularized)
# 计算卡尔曼增益 K
K = np.dot(np.dot(p_minus, C.T), S_inv)
x_hat = x_hat_minus + np.dot(K,(Z - np.dot(C,x_hat_minus)))
p = np.dot((np.eye(6) - np.dot(K,C)),p_minus)
return x_hat
#*********************[3]-定义用于存储给卡尔曼滤波使用的变量 用于保存一些历史数据**这个也不需要动***********************************************************************
last_frame_location = [0 for _ in range(4)] #用于存储上一帧的目标位置，这通常用于目标跟踪和计算目标移动等任务。一个长度为4的列表 last_frame_location，其中每个元素的初始值为 0
last_frame_rect = [0 for _ in range(4)] #存储上一帧检测到的矩形框坐标 成了一个长度为4的列表 last_frame_rect，并且每个元素的初始值为 0。
box = [0 for _ in range(4)] #生成一个包含四个子列表的列表,存储x1,y1,x2,y2
# 主循环，不断获取摄像头图像并进行处理
while(True):
clock.tick() # 计时当前帧的处理时间，计算帧率
# 获取当前图像并进行镜头畸变校正，纠正因镜头产生的畸变
img = sensor.snapshot().lens_corr(1.8) # 1.8是畸变系数，适当调整可以改善图像质量
start_index += 1 # 遍历下一个位置
if start_index >= 80: #如果值过大，就清零
start_index = 0 # 将 start_index 重置为 0
# 确定当前需要使用哪个位置的数据
if start_index < 25 or start_index > 40: #设置多少个点设置为可以被检测到
#*********************[4]-赋值要输入卡尔曼的数据*************
circle_center_x, circle_center_y, circle_radius = ball_positions[start_index] # 从ball_positions获取当前球的中心坐标和半径
#*********************[4]-赋值要输入卡尔曼的数据*************
img.draw_circle(circle_center_x, circle_center_y, circle_radius, color=(192, 255, 0)) #绘制圆为绿色
print("识别到的坐标 recognition : center_x = {}, center_y = {}, radius = {}".format(circle_center_x, circle_center_y, circle_radius))
#*********************[4]-检测出要识别物体后通过物体位置坐标等信息输入卡尔曼**根据自己的实际情况赋值rect_x、rect_y、rect_w、rect_h*********
#需要给赋值的是下面四个参数 rect_x、rect_y、rect_w、rect_h 这些参数是圈住识别物体的矩形框
# 计算矩形框的左上角坐标和宽高 给要赋值的四个变量赋值
rect_x = int(circle_center_x - circle_radius) # 矩形框左上角的 x 坐标
rect_y = int(circle_center_y - circle_radius) # 矩形框左上角的 y 坐标
rect_w = int(2 * circle_radius) # 矩形框的宽度，等于圆的直径
rect_h = int(2 * circle_radius) # 矩形框的高度，等于圆的直径
# 将矩形框的四个参数存储到 rect 数组中
rect = [rect_x, rect_y, rect_w, rect_h]
box = [rect[0], rect[1], rect[0] + rect[2], rect[1] + rect[3]] # 计算标记框的四个角坐标（左上角和右下角）
x, y, w, h = rect[0], rect[1], rect[2], rect[3]# 获取新的矩形框的坐标 识别成功的时候这个值是识别值的数据赋值
dx = (x - last_frame_x) / Ts# 计算x方向的速度，假设 Ts 为时间步长
dy = (y - last_frame_y) / Ts# 计算y方向的速度
Z = np.array([x, y, w, h, dx, dy]) # 构造测量向量 Z，包括位置和速度
x_hat = Kalman_Filter(Z)# 使用卡尔曼滤波器进行状态估计
last_frame_x, last_frame_y = x, y # 更新上一帧的x, y坐标
last_frame_rect = rect# 更新上一帧的矩形框 识别成功的时候
last_frame_location = box# 更新上一帧的位置 识别成功的时候
else:
#*********************[5]-检测检测不出物体时候 使用之前物体数据预测物体下面的位置等**不需要改动**********
#解释：根据卡尔曼滤波的预测值和目标的速度来更新目标的位置：
#x_hat[0] 和 x_hat[1] 是预测的位置，x_hat[4] 和 x_hat[5] 是目标的速度（速度分别对应 dx 和 dy）。
#Ts 是时间步长。更新后的 x, y 位置是通过加入速度（dx 和 dy）的影响来计算的。
#w 和 h 是目标的宽度和高度，这些信息没有改变，仍然使用 x_hat[2] 和 x_hat[3]。
x,y,w,h = (x_hat[0] + (x_hat[4] * Ts)),(x_hat[1] + (x_hat[5] * Ts)),x_hat[2],x_hat[3]
#解释：计算目标在 X 方向的速度（dx），通过当前帧的 x 位置与上一帧的 last_frame_x 位置之差来计算，除以时间步长 Ts
dx = (x - last_frame_x) / Ts
#解释：计算目标在 Y 方向的速度（dy），通过当前帧的 y 位置与上一帧的 last_frame_y 位置之差来计算，除以时间步长 Ts
dy = (y - last_frame_y) / Ts
#解释：将目标的位置、宽高、速度（dx 和 dy）组合成一个状态向量 Z，这个向量将作为卡尔曼滤波器的输入
Z = np.array([x, y, w, h, dx, dy])
#解释：将状态向量 Z 传递给卡尔曼滤波器 Kalman_Filter，以获得更新后的预测状态 x_hat
x_hat = Kalman_Filter(Z)
#更新上一帧的目标位置 last_frame_x 和 last_frame_y 为当前帧的目标位置 x 和 y，以便在下一帧计算速度时使用
last_frame_x, last_frame_y = x, y
#更新上一帧的目标矩形框 last_frame_rect 为当前帧的矩形框 x, y, w, h。
last_frame_rect = [x,y,w,h]
#更新上一帧的目标位置 last_frame_location 为当前帧的目标位置。这是一个矩形框的坐标，包括左上角 (x, y) 和右下角 (x + w, y + h)
last_frame_location = [x,y,(x + w),(y + h)]
#*********************[6]-无论是否可以检测到物体都 根据卡尔曼预测值绘制矩形框**这个不需要改动*********
predicted_rect = [ # 使用卡尔曼滤波的预测值绘制预测的矩形框
int(x_hat[0]),# 预测的矩形框左上角x坐标
int(x_hat[1]),# 预测的矩形框左上角y坐标
int(x_hat[2]), # 预测的矩形框宽度
int(x_hat[3]) # 预测的矩形框高度
]
# 绘制矩形框 这个就是一直绘制的卡尔曼预测的位置
img.draw_rectangle(predicted_rect, color=(100, 100, 100)) # 使用灰色绘制矩形框
#下面是把矩形转化会园绘制的，我们不需要
# 计算圆心坐标和半径
center_x = int(x_hat[0] + x_hat[2] / 2) # 圆心x坐标（矩形框左上角x + 宽度的一半）
center_y = int(x_hat[1] + x_hat[3] / 2) # 圆心y坐标（矩形框左上角y + 高度的一半）
radius = int(min(x_hat[2], x_hat[3]) / 2) # 半径取矩形框宽度和高度的最小值的一半
# 输出圆心坐标和半径信息
print("卡尔曼预测 Predicted Circle: center_x = {}, center_y = {}, radius = {}".format(center_x, center_y, radius))
# 打印当前帧率（每秒帧数），便于调试性能
print("FPS %f" % clock.fps()) # 输出当前的帧率
time. sleep_ms(500) #方便演示增加500ms延时时间
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
