目录

• 一、前言

• 二、视觉自动循迹的算法流程

• （1）图像的获取

• （2）图像的预处理

• （3）目标轨迹的提取

• （4）根据已知曲线进行预测控制

• 三、核心模块及要点

• 轨迹图像细化

• 小车轮距L的测量

• 预测轨迹变换

• 四、运行效果

• 五、总结

一、前言

本篇是利用单目视觉去实现简单的预定轨迹（这里指轨迹没有交叉、突变等情况），小车自动跟随驾驶。这个实现后，我们再接着开个中篇来处理复杂轨迹（比如，十字交叉、断线等）的自主决策和驾驶问题。等处理完了这个，再去研究没有轨迹引导时的一些小车自动形式实现方式，最终能够把之前埋得坑给填平。

二、视觉自动循迹的算法流程

本次基于视觉实现的自动循迹行驶，主要流程如下图：

（1）图像的获取

这里简化了摄像头的摆放位置，我们采用的是垂直地面的摄像头摆放，这样省略了摄像头的旋转变换，如下图：

在实际小车的摄像头布置中，往往是斜向布置，需要先进行必要的坐标变换，我们这里简化了，摄像头与物理世界的位置关系如下图所示：

摄像头成像的图片尺寸分辨率为512X512像素，对应的现实物理平面尺寸为1X1米。
通过该摄像头，我们获取了小车的前半部分和车头前面的部分轨迹图像。

（2）图像的预处理

接下来，对图像进行必要的处理，处理流程如上图所示，可以通过颜色查找的方法过滤掉非轨迹的图像元素：

从而提取我们想要的目标轨迹，接着进行图像的二值化，然后获得了一个轨迹图像。

（3）目标轨迹的提取

这里得到的轨迹往往是比较宽的，需要将轨迹进行细化，可以用细化算法进行处理（细化算法下节描述），或者是通过自上而下，左右像素得扫描，对横向得像素进行归并（可以取平均值），获得一个只有一个像素宽度的细细的轨迹线，然后通过获取的轨迹线，变换到与我们预测轨迹同一坐标系下，用于后续得轨迹对照。

（4）根据已知曲线进行预测控制

得到一个曲线后，我们就可以利用预测控制的思想，通过改变小车的两轮的差速进行轨迹的预测，并与已知曲线进行比较，得到误差最小的一个轨迹，对应的轮速就是本次最优的一个控制量。这部分我们已经在之前的一篇CSDN博文里——
python机器人编程——差速机器人小车的控制，控制模型、轨迹跟踪，轨迹规划、自动泊车（中）详细进行了说明，这里不再赘述。

三、核心模块及要点

轨迹图像细化

除了应用预测控制思想，这里所涉及的一个核心的模块是对轨迹的细化处理。细化处理的原理网上有很多，读者自行了解。我们直接可以使用一个其它牛人的库就可以了，代码如下：

from skimage import morphology
def path_skeletonize(img):
    
    """
    图像的细化
    Parameters
    ----------
    img : TYPE
        RGB
    Returns
    -------
    skeleton : TYPE
        Binary
    """

    _,binary = cv2.threshold(img,200,255,cv2.THRESH_BINARY_INV)
    binary[binary==255] = 1
    skeletonimg = morphology.skeletonize(binary)
    skeleton = skeletonimg.astype(np.uint8)*255
    return skeleton

*注意：*用此算法有时候会出现毛刺现象，其鲁棒性不太好。后来经过多次试验，发现采用逐个像素扫描，同行归并得方式比较可靠：

    for i in range(286,0,-1):
        u1=999
        u2=999
        temp=0
        count=0
        umax=Pathimg.shape[1]
        crossv=i
        #print("Pathimg shape:",Pathimg.shape)
        if Pathimg[i][0]>=1  or Pathimg[i][umax-1]>=1:
            iscross=1            
            break
        for j in range(umax):
            if count==2:  
                temp=temp/2
                aimPath[i]=temp
                break
            if Pathimg[i][umax-j-1]>=1 and u2==999:
                temp=temp+umax-j 
                count=count+1
                u2=10000
            if Pathimg[i][j]>=1 and u1==999:
                temp=temp+j 
                count=count+1
                u1=10000

小车轮距L的测量

如果是实物模型，我们只要用尺子量一下两个轮子的距离就可以了。仿真模型可以也可以通过计算获取，也可以通过一个轮子不动，只转另一个轮子，然后获取三个采样点，通过三个点计算旋转的半径，代码如下：

def solve_ccenterby_3P(p1,p2,p3):
    """
    已知圆上三点坐标，求圆心和半径
    """
    try:
        if len(p1)==2:            
                x1=p1[0]
                y1=p1[1]
                x2=p2[0]
                y2=p2[1]
                x3=p3[0]
                y3=p3[1]
                A=x1*(y2-y3)-y1*(x2-x3)+x2*y3-x3*y2
                B=(x1**2+y1**2)*(y3-y2)+(x2**2+y2**2)*(y1-y3)+(x3**2+y3**2)*(y2-y1)
                C=(x1**2+y1**2)*(x2-x3)+(x2**2+y2**2)*(x3-x1)+(x3**2+y3**2)*(x1-x2)
                D=(x1**2+y1**2)*(x3*y2-x2*y3)+(x2**2+y2**2)*(x1*y3-x3*y1)+(x3**2+y3**2)*(x2*y1-x1*y2)
                cx=-B/A/2
                cy=-C/A/2
                r=math.sqrt((B**2+C**2-4*A*D)/(4*A*A))            
                return (cx,cy),r
        else:
            print("type erro or lenth must be 2 .ex:(x1,y1)")
    except Exception as e:
        print("erro",e)
        return None,None

预测轨迹变换

除此之外，在预测轨迹生成的时候，优于我们布置的摄像是布置在小车的前正上方的，小车中心到车头的那部分轨迹其实是看不到的，

因此，我们获取的轨迹其实是车头之前的轨迹，我们之前那篇博文的轨迹是小车的中心出发的，因此我们需要根据车头与小车中心的物理关系，将预测的轨迹转化为车头的轨迹，然后才可以比较目标轨迹和预测轨迹的均方差（当然这可能不是最好的办法，读者知道更好的方法请留言告知）。

在已知小车中心和朝向角的情况下，可以通过旋转平移变换，求得车头的坐标，代码如下：

def Center2Head(xt, yt, theta,HeadL=0.2):
    """
    已知中心点坐标（xt,yt），及转向角theta，求车头坐标
        车头
         O        --------
         |
         |          车头距离中心：0.2M
    |----O----|   --------
      小车中心
    """
    #平移矩阵
    T=np.array([[1,0,xt],
                [0,1,yt],
                [0,0,1]])
    #旋转矩阵
    Tangle=math.pi/2-theta
    
    R=np.array([[math.cos(Tangle),math.sin(Tangle),0],
                [-math.sin(Tangle),math.cos(Tangle),0],
                [0,0,1]])
    
    HXY=np.array([[0],
                   [HeadL],
                   [1]])
    
    Hxy=np.dot(T, np.dot(R,HXY)) 
    
    hx=Hxy[0][0]
    hy=Hxy[1][0]               
    
    
    return hx, hy

四、运行效果

以上的方法运行后，能够跟随轨迹进行自动调整方向，总体效果还行，需要对一些参数进行再细致的调整，如步长、预测的时间dt等，还有需要优化的地方。

五、总结

以上就是简单的轨迹自动跟随的python实现，目前的方法还不能应对轨迹交叉，急转弯、岔路等情况，需要再进一步进行研究完善，还有就是可能存在的一些灯光干扰等问题，有待进一步解决。
关于性能上的问题，由于python在计算的处理速度上面相比与c++等语言会慢些，尤其是像本篇涉及的一些图像处理，预测的轨迹运算等任务，实际情况，如果都放在一个线程上面，实时性会比较差，因此需要进行一些优化处理，可以将状态的获取，图像的处理，轨迹寻优这几个运算分别开一个线程进行计算，或者利用一些加速库进行加速，这样对于现实系统应用会有很大的帮助。笔者也将继续对本篇的程序进行优化。等优化完后，上传到CSDN资源池中。
注：本篇纯属个人不成熟解决方案，存在多处不足，仅供参考。