一、前言

基于最近的测试，得到了一种粗略控制的算法，其控制效果适合单线路和急转弯的情况，本人认为可以稍微进行改进适用于十字路口等复杂轨迹的单目视觉循迹运行，本篇是对python机器人编程——差速机器人小车的控制，控制模型、轨迹跟踪，轨迹规划、自动泊车（中）未完成部分给补充，也提供了一种比较可靠和新的思路——我们把它暂且命名为《基于轨迹与路面重心偏离度误差的预测自动差速小车循迹控制》。

如上图所示，该算法可以在速度360m/h的情况下，比较粗略的自动转过一个大于90度的急转弯。虽然线不是非常贴切，但是我们认为这个方法实际比较可行。下面详细对该方法做个说明：

二、基于轨迹与路面重心偏离度误差的预测自动差速小车循迹控制策略

该方法中差速小车的控制还是用前面提到的预测控制方法，程序是一个以步长为dt的不断根据接受到的图像进行小车正反方向进行未来时刻轨迹预测的过程，并在边界条件下搜索出误差最靠近图像采集轨迹的小车左右轮子对应的速度即为当前步序的最优小车速度设定值。具体的算法流程伪代码如下：

while True：

 记录预测间隔累计时间con_Hz=con_Hz+1

 读取摄像头图像，并进行处理（具体方法见第三节）获得路径的重心图像坐标cu、cv

  if 当时间间隔到达预测控制的步长时con_Hz=300毫秒：

```
         con_Hz=0
```

         进入预测控制模块best_move(处理过的轨迹，当前的速度状态clv，重心坐标)：

                 定义最佳速度设定值bestv=clv

                 定义预测的微增速度dv=0.001

                 定义预测的边界范围vmax=10

                 for i in vmax：#速度增加的方向预测vmax个dv

                         左轮的速度sl_n=当前速度+i*dv

                         根据恒定速度计算右轮速度

                         计算当前速度下未来2.5秒后（可以调整）的轨迹线存在pv、pu的list中

                         将当前预测轨迹按照摄像头内参转化到图像坐标

                         计算预测轨迹的重心图像坐标（cpu、cpv）

                         计算重心距离dei=sqrt（（cpu-cu）**2+（cpv-cv)**2）

```
                         if dei<de:
```

                                 更新误差de=dei

                                 更新bestv=sl_n

                 for i in vmax：#速度减小的方向预测vmax个dv

                         左轮的速度sl_n=当前速度-i*dv

                         根据恒定速度计算右轮速度

                         计算当前速度下未来2.5秒后（可以调整）的轨迹线存在pv、pu的list中

                         将当前预测轨迹按照摄像头内参转化到图像坐标

                         计算预测轨迹的重心图像坐标（cpu、cpv）

                         计算重心距离dei=sqrt（（cpu-cu）**2+（cpv-cv)**2）

```
                         if dei<de:
```

                                 更新误差de=dei

                                 更新bestv=sl_n

             reurn  bestv#返回最优控制速度设定值

     将当前最优控制速度值下发至小车

```
     进入下一个循环。
```

以上是主要的算法描述。

根据这个算法，在仿真环境下，跑了很久，也没有大的偏离出轨道。如上图所示，中间的黑色图中：黄色为处理过的路径，蓝色的点为路径的重心点位置，红色的线在当前边界下最佳速度设定值时的预测未来行驶轨迹，当前轨迹是最靠近蓝色重心的，小车始终在这样的策略下运行。

三、轨迹图像的处理要点

对于获取的轨迹图像处理，主要流程如下：

获取摄像头当前的图像img
根据颜色选择算法留下黄色轨迹部分信息图像bimg
将bimg图像进行二值化，留下轨迹的像素为1，得到图binary
去除binary中小车部分的像素（图像的小下半部分）得到一个新的binary
计算当前轨迹的重心点cu、cv，供输入预测控制模块
图像的重心计算代码示例如下：

import cv2
import numpy as np
import matplotlib.pyplot as plt# Load the binary image
img = cv2.imread('binary_image.png', 0)# Calculate the moments of the image
M = cv2.moments(img)# Calculate the centroid
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])# Print the centroid coordinates
print(f'Centroid: ({cx}, {cy})')# Draw a circle at the centroid
img_with_centroid = cv2.circle(img.copy(), (cx, cy), 5, (255, 0, 0), -1)# Display the image with the centroid
plt.imshow(img_with_centroid, cmap='gray')
plt.show()

四、本篇部分核心控制策略python代码：

def best_move(sl,Pathimg,vc=0.1,dt=0.5,barycenter=(256,256)):#控制增量dvl=0.001#要预测的控制量     sl_n=sl#最优的控制量best_sl=slmod="G"#方差de=500dei=e+100Gbest_sl=slfor i in range(10):#逐次减小控制量sl_n=sl_n-i*dvl#待入预测函数获取预测序列        xx,yy,tt=predicate(0,0,math.pi/2,sl_n,step=5,dt=dt,vc=vc,dspeed=True)#中心轨迹转化为车头轨迹        pv=[]pu=[]#将小车坐标转化到图像坐标for i in range(len(xx)):puv1=realxy2uv(xx[i],yy[i],Pathimg,xymax=0.5,zero="self_define",dzero=(256,286))pu.append(puv1[0])pv.append(puv1[1])cpu=np.average(pu)cpv=np.average(pv) dei= np.sqrt((barycenter[0]-cpu)**2+(barycenter[1]-cpv)**2)   if dei<de:de=deiGbest_sl=sl_n   sl_n=sl  for i in range(10):#逐次增大控制量sl_n=sl_n+i*dvlxx,yy,tt=predicate(0,0,math.pi/2,sl_n,step=5,dt=dt,vc=vc,dspeed=True)    #将小车坐标转化到图像坐标pv=[]pu=[]for i in range(len(xx)):puv1=realxy2uv(xx[i],yy[i],Pathimg,xymax=0.5,zero="self_define",dzero=(256,286))pu.append(puv1[0])pv.append(puv1[1])cpu=np.average(pu)cpv=np.average(pv)     dei= np.sqrt((barycenter[0]-cpu)**2+(barycenter[1]-cpv)**2)if dei<de:de=deiGbest_sl=sl_n   print("Gbest_sll:",best_sl)#返回最优控制量if mod=="G":return Gbest_slelse:return best_sl,np

五、结论

后续还有对复杂轨迹的处理需要改进。如需要本篇的仿真环境和源代码，可以加入公众号或者私信留言。

换了个其它的路径，可以看出这个办法鲁棒性还是不错的，路面有破损只要是趋向于均匀的，自动驾驶也有好的表现：

python机器人编程——差速AGV机器、基于视觉和预测控制的循迹、自动行驶（下篇）相关推荐

python机器人编程——差速AGV机器、基于视觉和预测控制的循迹、自动行驶（上篇）
目录一.前言二.视觉自动循迹的算法流程 (1)图像的获取 (2)图像的预处理 (3)目标轨迹的提取 (4)根据已知曲线进行预测控制三.核心模块及要点轨迹图像细化小车轮距L的测量预测轨迹变换 ...
python机器人编程——差速机器人小车的控制，控制模型、轨迹跟踪，轨迹规划、自动泊车（上）
目录一.前言二.差速小车机器人的运动分析三.正向运动控制模型推导 3.1问题描述 3.2符号定义 3.3算式推导 Step1 寻找数量关系,求出圆周半径,角度变化 Step2 获取相对坐标 St ...
python机器人编程——在VREP环境中，UARM与摄像头联动，实现基于视觉识别的自动抓取，垃圾自动分类（上）
目录前言在VREP中加入视觉传感,用python侧获取图像在VREP中布置垃圾分类场景 Step1:用三维软件造模型 Step2:导入到vrep Step3:关联shap.调整位置及贴图测试一 ...
python机器人编程——垃圾自动分类，在VREP环境中，UARM与摄像头联动，实现基于视觉识别的自动抓取（下）
目录前言非绝对垂直摄像头图像的坐标变换物体的识别和坐标确定 (1)物体的识别 (2)坐标的确定机械臂执行动作逻辑实现 python源码前言本篇我们在<python机器人编程--VRE ...
python机器人编程——四轴UARM机械臂的运动控制（逆解）原理及python实现（下）
目录 1 概要 2 uarm机械臂运动控制程序的python实现 2.1机械结构的python表达 2.2 逆解算法的python表达 2.2.1 俯视图根据目标点(x,y)计算J1,L 2.2.2 ...
【C51开发应用】基于C51单片机开发的循迹灭火机器人
基于C51单片机开发的循迹灭火机器人一.作品摘要二.系统设计步骤三.方案设计 3.1 小车主体设计 3.1.1 购买玩具小车进行改装 3.1.2 自己设计小车主体结构 3.2 电 ...
基于MSP432控制的红外循迹爬坡小车设计报告
基于MSP432控制的红外循迹爬坡小车设计报告竞赛选题:坡道行驶电动小车(C题) 摘要本小车基于TI的MSP432平台,设计了可以沿指定路线在坡道上循迹行驶的四轮小车.小车采用L298N驱动芯片控 ...
基于arduino的5路循迹小车（1）
基于arduino的5路循迹小车(1) 初步小车运动 1.硬件选用 1.开发板使用arduino 2560 2.电机驱动板使用大功率电机驱动板(PWM由两个端子控制) 3.电源12V锂电池 4.降压 ...
基于STC8G2K64S4单片机控制小车循迹（直流电机和步进电机）
基于STC8G2K64S4单片机控制小车循迹 #前言本文从硬件和软件两方面介绍了使用STC8G2K64S4单片机控制舵机的方法,并使用步进电机和直流电机来举例说明之前说讲了如何用STC8G2K64 ...

python机器人编程——差速AGV机器、基于视觉和预测控制的循迹、自动行驶（下篇）

目录