基于stm32和openmv的视觉云台追踪小车源码+详细文档说明（高分项目）

# 基于Openmv的视觉跟踪小车 ## 一、实验原理及实验内容 ### 1.物体识别 本次实验目的是使得小车可以跟踪目标，故首先确定跟踪目标，由于小车整体框架从零开始搭建，并没有太多的金钱可以选择昂贵的摄像头，故本次实验的目标识别选择较为简单的方式以减少硬件压力。本次实验首先识别纯色物体，是完成对纯色物体识别之后更进一步选择跟踪AprilTag。 AprilTag是一个视觉基准系统，可用于各种任务，包括AR，机器人和相机校准。这个tag可以直接用打印机打印出来，而AprilTag检测程序可以计算相对于相机的精确3D位置，方向和id。 AprilTag内容主要包含三个步骤： 第一步是如何根据梯度检测出图像中的各种边缘。 第二步即如何在边缘图像中找出需要的四边形图案并进行筛选，AprilTag尽可能的对检测出的边缘检测，首先剔除非直线边缘，在直线边缘进行邻接边缘查找，最终若形成闭环则为检测到一个四边形。对四边形进行解码确定Apriltag标签。 第三步确定四边形的中心点作为要跟踪的三维左边点。 Openmv对以上步骤进行了函数封装，可以用img.find_apriltags()函数定位Apriltag标签，并且可以通过该函数的返回值的方法确定三维坐标和三维角度：可以用获取x轴坐标tag.x_translation(), tag.y_translation()、tag.z_translation()是y、z轴坐标 。 ### 2.云台追踪 openmv中搜索目标函数的返回值包括了目标物体中心的x、y坐标，原点是在图片的最左下角，就是说如果我们按照直接得到的坐标都是正的，但是我们要求云台追踪目标就是让目标始终出现在视野最中间，都是正的值我们无法判断图片到底是往哪边偏。为了解决这样的问题，我们只需要对得到的坐标进行简单的处理,openmv获得图片宽高都可以用函数获得，故已知图片宽width，高度height，目标中心点坐标x,y。按照相对比例来判断目标点在相机内的相对位置： $$ y1=y/height-0.5 $$ $$ x1=x/width-0.5 $$ 这样x1,y1就是我们最新获得的值，其取值范围均为[-0.5,0.5]。 为了实现云台始终追随目标，我们还需要将得到的坐标值转换为舵机旋转的角度，本实验云台为二自由度云台，如图1.1。下面的舵机控制偏航角与相机x轴相关，上面的舵机负责控制俯仰角与相机y轴相关，偏航角舵机的机械转角范围为[0,180],其中，当角度为0时，舵机朝向右侧，角度为180度时，舵机朝向左侧。俯仰角的机械转角范围为[90,180]，其中，当角度为90度时，平台成水平，当角度为180度时，平台垂直水平面。 <img src="readme.imgs/1-16497681807631.jpg" style="zoom: 25%;" /> ​ **图1.1** 算法上的实现，算法上的实现可以使用pd控制，pd控制较为稳定更适合舵机。我们已知（x1,y1）为当前目标的坐标，目的是将其移动到镜头中央，那么目标点为（0,0）,我们获得了x轴的偏差以及y轴的偏差error_x,error_y上一次x,y偏差为error_x_last,error_y_last。假设此时舵机角度为yaw_now,pitch_now,那么有简化版增量式pd算法为： $$ output_x=Kp*（error_x ）+Kd*(error_x-error_(x_last )) $$ $$ output_y=Kp_y*(error_y )+Kd*(error_y-error_(y_last )) $$ 则当前获得偏航角 $$ yaw_now+=output_x $$ 获得当前俯仰角 $$ pitch_now+=output_y $$ 让两个舵机执行对应的角度，实现控制，过程中需要不断调节Kp，Kd参数，Kp调节参数方式：逐渐增大Kp，使舵机出现抖动，再逐渐降低Kp值，使系统逐渐趋于稳定，Kd的调参方式与此相似。 ### 3.与目标直线距离的几何化处理 选择识别的目标为AprilTag，在openmv中，有一个函数名为`find_AprilTag()`,具体使用可查看：[image — 机器视觉 — MicroPython 1.9.2 文档 (singtown.com)](https://docs.singtown.com/micropython/zh/latest/openmvcam/library/omv.image.html#image.Image.image.find_apriltags)此函数其返回对象的具体内容：可查看[image — 机器视觉 — MicroPython 1.9.2 文档 (singtown.com)](https://docs.singtown.com/micropython/zh/latest/openmvcam/library/omv.image.html#image.apriltag)。 我们只需要用到其返回的x,y,z坐标（相对于镜头中央），这三个需要的变量为索引12、13、14对应的值，其单位未知，但是与实际距离成一定的关系，这一比例可以用**K**来表示，对于不同大小的AprilTag其对应的K是不同的。 $$ diatance=K*(\sqrt(x^2+y^2+z^2)) $$ 那么怎么测量属于我们自己的K呢，很简单。 #### K值的计算 OpenMV采用的是单目摄像头，想要实现测距，就需要选参照物，利用参照物的大小比例来计算距离。  Lm为物体距摄像头的距离，L’为焦距，Bx为物体在图像中的直径像素，Ax为图像的直径像素，Rm为物体实际大小的半径，Hm为摄像头可拍摄的半径。  求常数k的方法是：先让物体距离摄像头10cm，打印出摄像头里直径的像素值，然后将像素值与距离相乘，就得到了k的值。 由于打印的AprilTag 标签大小不定，所以img.find_apriltags()函数返回参与单位也不定，确定单位方法与确定常数K的方法一致。 目前，我们已经得到了一个距离物体的距离distance，对于追踪小车我们要发送给小车可以是直接的距离吗？当然不是，在第一步中我们设置openmv跟随物体，跟随时物体要处在镜头的正中央，舵机进行了角度的旋转，车呈现的状态，如图1.2  ​ 图1.2 所以说这个时候目标距离已知，我们舵机跟随的角度yaw_now,pitch_now已知，如何求出在实际空间中与物体之间的x、y距离？首先我们先不管x，y坐标距离究竟为何，设置默认坐标轴的方向，以车体为坐标轴，设置向右为x正方向，向前为y轴正方向，图1.2中已标出。 对小车距离进行分解，如图1.3给出了距离分解示意，原理还是很简单的，我们可看出首先应该将diatance分解到水平平面上，相当于空间在水平面上作出投影，得到水平距离distance_horizontal $$ distance_horizontal=distance*cos(pitch) $$  通过水平距离我们就可以求得x,y方向坐标，坐标为 $$ （distance_horizontal*sin(yaw),distance_horizontal*cos(yaw)) $$ 要注意的是，这里的yaw不是yaw_now,pitch也不是pitch_now，yaw_now,pitch_now均为舵机目前的旋转角，那么我们根据其机械几何特性（搞不懂请看：2.云台追踪）： $$ pitch=180-pitch_now $$ $$ yaw=yaw_now-90 $$ 更新x,y坐标表示： $$ (x,y)=（distance*cos(pitch)*sin(yaw), distance*cos(pitch)*cos(yaw))=(-distance*cos(pitch_now)*cos(yaw_now),-distance*cos(pitch_now)*sin(yaw_now)) $$ 这样我们openmv的任务就基本搞定，得到的x,y即为物体空间坐标表示，下面需要将消息通过串口发送出去。 ### 4.串口消息的发送与接收 #### （1）连线： ##### 对于stm32 选择stm32的USART2与openmv的USART3相连接，其中stm32的USART2:  在上图中可以看出PA2为信号发送端口，PA3为接收端口。 ##### 对于openmv： 图中的openmv为ov7725，我们购买的摄像头为ov5640,其中usart3对应的是p4、p5引脚，p4引脚对应的是发送端口，p5端口对应的是接收端口。 ##### 二者的连接： stm32的发送端口连接open