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

基于目前的研究演进&＃xff0c;我们还是不能把python机器人编程——差速机器人小车的控制&＃xff0c;控制模型、轨迹跟踪&＃xff0c;轨迹规划、自动泊车&＃xff08;中&＃xff09;未完成部分给补上&＃xff0c;本篇先把一些基础的问题给先解决。那就是非常常见的agv无人控制方式——循迹自动驾驶。本篇是利用单目视觉去实现简单的预定轨迹&＃xff08;这里指轨迹没有交叉、突变等情况&＃xff09;&＃xff0c;小车自动跟随驾驶。这个实现后&＃xff0c;我们再接着开个中篇来处理复杂轨迹&＃xff08;比如&＃xff0c;十字交叉、断线等&＃xff09;的自主决策和驾驶问题。等处理完了这个&＃xff0c;再去研究没有轨迹引导时的一些小车自动形式实现方式&＃xff0c;最终能够把之前埋得坑给填平。

本次基于视觉实现的自动循迹行驶&＃xff0c;主要流程如下图&＃xff1a;

&＃xff08;1&＃xff09;图像的获取

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

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

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

&＃xff08;2&＃xff09;图像的预处理

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

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

&＃xff08;3&＃xff09;目标轨迹的提取

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

&＃xff08;4&＃xff09;根据已知曲线进行预测控制

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

轨迹图像细化

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

from skimage import morphology
def path_skeletonize(img):"""图像的细化Parameters----------img : TYPERGBReturns-------skeleton : TYPEBinary"""_,binary &＃61; cv2.threshold(img,200,255,cv2.THRESH_BINARY_INV)binary[binary&＃61;&＃61;255] &＃61; 1skeletonimg &＃61; morphology.skeletonize(binary)skeleton &＃61; skeletonimg.astype(np.uint8)*255return skeleton


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

for i in range(286,0,-1):u1&＃61;999u2&＃61;999temp&＃61;0count&＃61;0umax&＃61;Pathimg.shape[1]crossv&＃61;i#print("Pathimg shape:",Pathimg.shape)if Pathimg[i][0]>&＃61;1 or Pathimg[i][umax-1]>&＃61;1:iscross&＃61;1 breakfor j in range(umax):if count&＃61;&＃61;2: temp&＃61;temp/2aimPath[i]&＃61;tempbreakif Pathimg[i][umax-j-1]>&＃61;1 and u2&＃61;&＃61;999:temp&＃61;temp&＃43;umax-j count&＃61;count&＃43;1u2&＃61;10000if Pathimg[i][j]>&＃61;1 and u1&＃61;&＃61;999:temp&＃61;temp&＃43;j count&＃61;count&＃43;1u1&＃61;10000


小车轮距L的测量

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

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


预测轨迹变换

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

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

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

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

四、运行效果

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

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