devbox
繁依Fanyi0
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

【开源】智能视觉组麦轮+OpenMV循赛道_麦轮车自由度计算公式

作者：繁依Fanyi0 | 2024-04-24 00:42:45

赞

踩

麦轮车自由度计算公式

目录

前言

一、找路部分

1.总钻风

2.OpenMV

二、循路部分

1.麦轮的运动解算

2.运动信息的计算

3.驱动

三、效果视频

四、飘移

1.三个自由度间的互相影响

2.画面裁切

前言

写这篇文章的起因是参加智能车寒假校赛时，由于我们组开始着手比较晚，实验室的总钻风摄像头不够用了，所以只能使用OpenMV进行视像头视觉循迹。而当我在网上搜索OpenMV循智能车赛道时，发现竟然什么也没有（可能有简单循黑线的），所以我打算分享一下自己的代码。同时我发现麦轮循迹的资料也比较少，所以我打算把二者结合做个小开源。

一、找路部分

提前声明，本篇扫线用的是传统的大津法。

1.总钻风

这些摄像头传感器模块的工作原理都差不多，基本都是把捕获到的图片以各个像素点存储在一个一维数组里。

我们本次暂时用的主控是沁恒的CH32V307，沁恒芯片使用DVP去控制图像传输，DVP内部调用了DMA，总钻风获取的灰度图通过DMA传输，在每个场中断时初始化DMA传输，在传输结束后触发DMA中断，标志着一场图像传输结束。在程序中设定两个缓冲区，一边MCU读取摄像头数据，一边DMA将要传输的数据放入另一个缓冲区，提高摄像头的帧率。

如果想了解总钻风拍摄后如何找中线的，网上一大把。我这里主要说OpenMV。

2.OpenMV

OpenMV与主控之间通过串口通信，如果把整幅图像都传递给单片机显然有亿点慢。所以我让OpenMV捕获到图片后自己找中线，计算后只把小车需要的两个量（自旋量、平移量）传递给单片机。OpenMV获取一张图片计算一张图片，相比上述两个缓冲区的模式，速率有点慢。

OpenMV从捕获图片到传递参量的全部代码如下：

  1. import sensor, image, time    #导入所需的库
  2. from pyb import millis, UART
  3.  
  4. ###声明摄像头
  5. sensor.reset()  #摄像头重启
  6. sensor.set_pixformat(sensor.GRAYSCALE) #8位灰度模式
  7. sensor.set_framesize(sensor.QQVGA)   #160*120分辨率
  8. #sensor.set_windowing(0,60,160,60)
  9. sensor.set_auto_whitebal(True)  #关闭自动白平衡
  10. sensor.set_auto_gain(False) #关闭自动亮度
  11. clock = time.clock()    #声明clock，用于主循环中统计帧速。
  12. uart = UART(3, 115200) #开启串口
  13. uart.init(115200, bits=8, parity=None, stop=1)  #8位数据位，无校验位，1位停止位
  14.  
  15. ###变量
  16. midline=[0]*120 #存储赛道中线的数组
  17. leftline=[0]*120 #存储赛道左边线的数组
  18. rightline=[0]*120 #存储赛道右边线的数组
  19. Row=119 #画面120行
  20. Col=160 #画面160列
  21. #例：若要调用img[2][3],则需调用img[2*Col+3]
  22. white=255 #白色为255
  23. black=0 #黑色为0
  24. translation=0 #平移量
  25. spin=0 #自旋量
  26.  
  27. ###找赛道中线函数
  28. def Way_Finding():
  29.     if img[Col*Row+(Col//2)] == black:  #判断车身中点是否在赛道上，决定从哪里开始爬线
  30.         if img[Col*Row+5] == white:
  31.             midline[Row] = 5
  32.         elif img[Col*Row+(Col-5)] == white:
  33.             midline[Row] = Col-5
  34.         else:
  35.             midline[Row] = Col//2
  36.     else:
  37.         midline[Row] = Col//2
  38.     for i in range(Row, 50, -1):  #画面横向120行，这里只往上爬到50行，车速不快，爬多了也没用
  39.         for j in range(midline[i], -1, -1):
  40.             if img[int(Col*i+j)]==black or j==0:  #找到黑色边界或最后什么也没找到 跳出
  41.                 leftline[i] = j
  42.                 break
  43.         for j in range(midline[i], Col):
  44.             if img[int(Col*i+j)]==black or j==Col:  #找到黑色边界或最后什么也没找到 跳出
  45.                 rightline[i] = j
  46.                 break
  47.         midline[i] = (leftline[i] + rightline[i]) / 2  #左右边线相加除以二得到中线
  48.         if img[Col*i+int(midline[i])]==0 or i==60:  #若img的中线爬到黑色则退出循环
  49.             break
  50.  
  51. ###主循环
  52. while True:
  53.     img = sensor.snapshot() #获取图像
  54.     histogram = img.get_histogram() #获取图像的直方图
  55.     Thresholds = histogram.get_threshold() #获取直方图的阈值
  56.     img.binary([(Thresholds.value(), 255)]) #二值化图像，将低于阈值的像素设为黑色（0），高于阈值的像素设为白色（255）
  57.     Way_Finding() #调用找中线函数
  58.     translation=midline[Row]-Col/2 #计算平移量
  59.     for k in range(Row, 65, -1): #计算自旋量
  60.         img.draw_line((int(midline[k]), int(k), int(midline[k-1]), int(k-1)), color=(0,0,0)) #在屏幕上标识出中线
  61.         spin=spin+midline[k]
  62.     spin=spin/(Row-k)-Col/2
  63.     moving=bytearray([130,130,int(translation),int(spin)]) #打包数据，包头为两个130，无包尾
  64.     uart.write(moving) #发送给单片机
  65.     print(midline[Row],midline[60],translation,spin) #在串行终端中打印一下以便观察
  66.     spin=0 #清零自旋量

先初始化摄像头和串口，然后声明一些变量。

在主循环中，先获取一张图像，再调用OpenMV自带的Otsu算法二值化图像，得到赛道基本样貌，效果如下：

接着调用我们自己定义的Way_Finding()函数找出赛道中线。在Way_Finding()中，先判断屏幕最下面一行的中心像素点是否为白色，如若不是，则向左或向右随便找一个是的作为midline[Row]的值开始爬线。画面横向有120行，我们只遍历60行，剩下的60行太靠前了会造成干扰。最后通过判断黑色像素跳变点一行一行地找出左右边线和中线（判断一个跳变点就基本够用，我看画面中的赛道没有噪点）。

二、循路部分

1.麦轮的运动解算

关于麦轮的运动解算，网上的资料还是蛮多的，但基本都是从辊子的受力分析开始一顿推算，容易把新手搞懵圈。这里我推荐去看b站up主程欢欢的视频，个人感觉他讲得很清晰。我在本篇中也借用他的图片简单阐述一下。

假设四个轮子的标号如下：

我们通过几个已知运动合成最终的全向运动。蓝色表示车轮旋转方向，绿色表示车轮对地面施力方向。

上面是三个自由度各自的分解运动，把它们合成就会得到小车总体的运动，即

小车的运动=前后+左右+旋转

那么我们再把这个公式代入四个轮各自的运动，规定向前、向右、顺时针自转为正方向，就会得到四个轮各自的运动公式：

左前轮（轮1）=  前后 + 左右 + 自转

右前轮（轮2）=  前后 - 左右 - 自转

左后轮（轮3）=  前后 - 左右 + 自转

右后轮（轮4）=  前后 + 左右 - 自转

综上，为控制车整体的运动，我们需要三个量：前进量、平移量、自旋量。下面我们开始获取这三个量。

2.运动信息的计算

（1）前进量

小车在直道行驶时的平移量和自旋量为0，匀速直线运动的速度作为前进量，在主控中自己定义，为车的基准速度，大小与编码器读取的转速值范围有关，因车而异，各不相同，自己根据自己调的PID给一个合适的值即可，不过这个基准速度的大小会影响后面平移量和自旋量乘的比例系数。

（2）自旋量

自旋量是决定小车转弯的重要因素，计算方法如下：

  1.  for k in range(Row, 65, -1): #计算自旋量
  2.      spin=spin+midline[k]
  3. spin=spin/(Row-k)-Col/2

可见，先求midline中线数组里的元素（每一行的中点）的平均值，再把该值与画面中线（160/2）做差，得到的就是小车的自旋量spin，像这样：

（3）平移量

有的时候，车身与赛道会出现平行偏差，像这样：

这种情况通过自旋量也能调整过来，无非多经过几个s型的摇摆，但显然这种水多了加面、面多了加水的处理方式效率低下，故我们引出平移量：

平移量的计算很简单，只需要把画面最后一行的midline[Row]值与画面中线值做差。

translation=midline[Row]-Col/2 #计算平移量

综上，我们便得到了小车运动所需的三个量，把它们通过串口传递给单片机。

  1. moving=bytearray([130,130,int(translation),int(spin)]) #打包数据，包头为两个130，无包尾
  2. uart.write(moving) #发送给单片机

3.驱动

（1）PID

首先，你要封装好自己的PID。定义个结构体，声明四个轮子的变量：

  1. typedef struct
  2. {
  3.     float Kp,Ki,Kd;
  4.     float error[3]; //error[0]代表本次误差，error[1]代表上次误差，error[2]代表上上次误差
  5.     float target;
  6.     int actual;
  7.     float p,i,d;
  8.     float output,delta_output;
  9. }PID;
  10. PID PID1,PID2,PID3,PID4;

开个10ms的定时中断：

  1. void TIM5_IRQHandler(void)
  2. {
  3.     if(TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET)
  4.     {
  5.        TIM_ClearITPendingBit(TIM5, TIM_IT_Update );
  6.        PID_Interrupt(); //中断回调函数
  7.     }
  8. }

中断处理函数：

  1. void PID_Interrupt(void)
  2. {
  3.     PID1.actual=-encoder_get_count(TIM4_ENCOEDER);
  4.     encoder_clean_count(TIM4_ENCOEDER);
  5.     PID2.actual=-encoder_get_count(TIM9_ENCOEDER);
  6.     encoder_clean_count(TIM9_ENCOEDER);
  7.     PID3.actual=encoder_get_count(TIM8_ENCOEDER);
  8.     encoder_clean_count(TIM8_ENCOEDER);
  9.     PID4.actual=-encoder_get_count(TIM3_ENCOEDER);
  10.     encoder_clean_count(TIM3_ENCOEDER);
  11.     Set_All(PID_Set(&PID1),PID_Set(&PID2),PID_Set(&PID3),PID_Set(&PID4));
  12. }

其中的PID_Set()计算函数：

  1. float PID_Set(PID* pid)   //增量式
  2. {
  3.     pid->error[0]=pid->target-pid->actual;
  4.     pid->p=pid->Kp*(pid->error[0]-pid->error[1]);
  5.     pid->i=pid->Ki*pid->error[0];
  6.     pid->d=pid->Kd*(pid->error[0]-2*pid->error[1]+pid->error[2]);
  7.     pid->delta_output=pid->p+pid->i+pid->d;
  8.     pid->output +=pid->delta_output;
  9.     pid->output=pid->output<Output_Max?pid->output:Output_Max;
  10.     pid->output=pid->output>-Output_Max?pid->output:-Output_Max;
  11.     pid->error[2]=pid->error[1];
  12.     pid->error[1]=pid->error[0];
  13.     return pid->output;
  14. }

调参的话调成这样就差不多：

常规的PID封装准备工作完成，想要更改速度的时候直接更改PIDx.target就行了。

（2）接收中断

然后开启串口接收中断，在中断里接收OpenMV发过来的数据并进行处理：

  1. void USART3_IRQHandler(void)
  2. {
  3.     if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)
  4.     {
  5.         static uint8_t rebuf[4]={0},i=0;
  6.         uart_query_byte(UART_3, &rebuf[i++]);
  7.                 if(rebuf[0]!=130) //帧头
  8.                     i=0;
  9.                 if((i==2)&&(rebuf[1]!=130)) //判断帧头
  10.                     i=0;
  11.                 if(i>=4) //代表一帧数据完毕
  12.                 {
  13.                     memcpy(moving,rebuf,i); //把rebuf数组里的内容拷贝到moving数组
  14.                     i = 0;
  15.                     OpenMV_Track(); //中断回调函数
  16.                 }
  17.         USART_ClearITPendingBit(USART3, USART_IT_RXNE);
  18.     }
  19. }

其中的OpenMV_Track()函数：

  1. void OpenMV_Track(void)
  2. {
  3.     translation=0.9*(unsigned short int)(moving[2]);
  4.     spin=1.4*(unsigned short int)(moving[3]);
  5.     straight=200;
  6.     if(fabs(spin)<5) spin=0;
  7.     McNamm_Set(straight,translation,spin);
  8.     if((unsigned short int)(moving[2])>=160||(unsigned short int) (moving[2])<=40)McNamm_Set(0,0,0); //偏离赛道时停车
  9. }

其中的McNamm_Set()函数：

  1. void McNamm_Set(float straight,float translation,float spin)
  2. {
  3.     PID1.target=straight+translation+spin;
  4.     PID2.target=straight-translation-spin;
  5.     PID3.target=straight-translation+spin;
  6.     PID4.target=straight+translation-spin;
  7.     if(PID1.target>700||PID1.target<-700) PID1.target=0;
  8.     if(PID2.target>700||PID2.target<-700) PID2.target=0;
  9.     if(PID3.target>700||PID3.target<-700) PID3.target=0;
  10.     if(PID4.target>700||PID4.target<-700) PID4.target=0;
  11. }

target=straight+translation+spin

straight前进量是一个固定值，不设定为可变值的原因是translation和spin是OpenMV传过来的直接量，需要乘以一个比例系数再作用于target，这个比例系数需要上赛道调试，不同的straight对应的比例系数不同，但并非成简单的线性关系，所以每更改一次straight都需要手动调出一个合适的比例系数。

另外为了防止走直道时左右摇摆不稳定，设立一条判断语句，若自旋量spin小于某值则spin=0，只让translation作用。

三、效果视频

智能视觉循迹

四、飘移

既然麦轮有能够平移的特点，那不用来飘移岂不是白瞎了！！！

想要达到飘移的效果，大致需要做两点改动。

1.三个自由度间的互相影响

（1）translation

在前面我们说translation是画面最后一行的midline[Row]值与画面中线值的差，所以试想当小车遇见弯道时，translation的作用会使小车偏向弯道内侧。举个例子，当遇到右转的弯道时，摄像头看到的画面中最后一行的midline[Row]值在画面中线的右侧，所以translation是正值，会作用使小车向右平移，而如果向右平移的话车就会贴近弯道内侧行驶，所以没有飘移的效果。

相反的，若想达使车身过弯横漂，那么应该使translation的符号取反，相应的作用也是与从前相反。另外，此时的translation还应该受spin的影响，spin越大，translation越大，调试出一个合适的比例系数。

（2）straight

straight不再是一个固定的值，而是受spin的影响。当遇到弯路时，spin会增大，straight需随之减小，以平移代替前进，凸显飘移的效果。

综上，修改后OpenMV_Track()函数如下：

  1. void OpenMV_Track(void)
  2. {
  3.     translation=3.1*((unsigned short int)(moving[2])-100); //平移量
  4.     spin=2.6*((unsigned short int)(moving[3])-100); //自旋量
  5.     straight=300-1.4*fabs(spin)-0.7*fabs(translation); //前进量=基准量-1.4倍自旋量-0.7倍平移量
  6.     if(straight<0) straight=0; //防止前进量被减成负数
  7.     if(spin>20) translation=-1.5*fabs(translation)-0.9*fabs(spin); //右转时往左飘
  8.     if(spin<-20) translation=1.5*fabs(translation)+0.9*fabs(spin); //左转时往右飘
  9.     McNamm_Set(straight,translation,spin);
  10.     if((unsigned short int)(moving[2])>=160||(unsigned short int) (moving[2])<=40)McNamm_Set(0,0,0); //偏离赛道时停车
  11. }

2.画面裁切

经实践发现，改版飘移后的小车过弯时具有滞后性，反应有些迟钝。那么提升预判的方法便是提高画面处理的起始高度。

在前文中提到spin自旋量是这样得到的：

  1. for k in range(Row, 65, -1):
  2.     spin=spin+midline[k]
  3. spin=spin/(Row-k)-Col/2

现在我们或许可以改成这样：

  1. for k in range(Row-20, 55, -1):
  2.     spin=spin+midline[k]
  3. spin=spin/(Row-20-k)-80

二者的区别就是处理的画面区域不一样。第一段代码是使用第65行到第120行的img，第二段代码是使用第55行到第100行的img。

好，做完以上两点更改你就能用第一人称飘移青春了。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/繁依Fanyi0/article/detail/476790
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号