目录
- 一、前言
- 二、视觉自动循迹的算法流程
- （1）图像的获取
- （2）图像的预处理
- （3）目标轨迹的提取
- （4）根据已知曲线进行预测控制
- 三、核心模块及要点
- 轨迹图像细化
- 小车轮距L的测量
- 预测轨迹变换
- 四、运行效果
- 五、总结
一、前言
基于目前的研究演进，我们还是不能把python机器人编程——差速机器人小车的控制，控制模型、轨迹跟踪，轨迹规划、自动泊车（中）未完成部分给补上，本篇先把一些基础的问题给先解决。那就是非常常见的agv无人控制方式——循迹自动驾驶。本篇是利用单目视觉去实现简单的预定轨迹（这里指轨迹没有交叉、突变等情况），小车自动跟随驾驶。这个实现后，我们再接着开个中篇来处理复杂轨迹（比如，十字交叉、断线等）的自主决策和驾驶问题。等处理完了这个，再去研究没有轨迹引导时的一些小车自动形式实现方式，最终能够把之前埋得坑给填平。
二、视觉自动循迹的算法流程
本次基于视觉实现的自动循迹行驶，主要流程如下图：
（1）图像的获取
这里简化了摄像头的摆放位置，我们采用的是垂直地面的摄像头摆放，这样省略了摄像头的旋转变换，如下图：
在实际小车的摄像头布置中，往往是斜向布置，需要先进行必要的坐标变换，我们这里简化了，摄像头与物理世界的位置关系如下图所示：
摄像头成像的图片尺寸分辨率为512X512像素，对应的现实物理平面尺寸为1X1米。
通过该摄像头，我们获取了小车的前半部分和车头前面的部分轨迹图像。
（2）图像的预处理
接下来，对图像进行必要的处理，处理流程如上图所示，可以通过颜色查找的方法过滤掉非轨迹的图像元素：
从而提取我们想要的目标轨迹，接着进行图像的二值化，然后获得了一个轨迹图像。
（3）目标轨迹的提取
这里得到的轨迹往往是比较宽的，需要将轨迹进行细化，可以用细化算法进行处理（细化算法下节描述），或者是通过自上而下，左右像素得扫描，对横向得像素进行归并（可以取平均值），获得一个只有一个像素宽度的细细的轨迹线，然后通过获取的轨迹线，变换到与我们预测轨迹同一坐标系下，用于后续得轨迹对照。
（4）根据已知曲线进行预测控制
得到一个曲线后，我们就可以利用预测控制的思想，通过改变小车的两轮的差速进行轨迹的预测，并与已知曲线进行比较，得到误差最小的一个轨迹，对应的轮速就是本次最优的一个控制量。这部分我们已经在之前的一篇博文里——
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资源池中。
注：本篇纯属个人不成熟解决方案，存在多处不足，仅供参考。
