[发明专利]一种AGV视觉轨道的铺设及优化方法

权利要求书

1.一种AGV视觉轨道的铺设及优化方法，其特征在于：具体包含视觉轨道的铺设、视觉轨道的定位程和视觉轨道的优化两部分：

其中，视觉轨道的铺设具体包含如下步骤；

步骤1，启动搭载在小车上的摄像头并拍摄小车周边场景，初始化一个空的图G＝(P，E)，进而初始化一个图片数据{I}，记录初始位置为P₀＝(0，0，0)，并在此刻拍摄一张照片I₀；其中，P＝(x,y,theta)是一个点，表示一个拍摄照片的小车位置在坐标(x,y)，旋转角度为theta；E＝(Δx,Δy,Δtheta)是一条边，表示通过视觉里程计技术所得到的被连接的两个点对应的两张照片所计算出，小车瞬时的位移是(Δx,Δy)，旋转是(Δtheta)；

步骤2，操控小车行走预定的行走路线；

步骤3，设当前已拍摄k张照片，当摄像头拍摄第k+1张照片后：

步骤3.1，向增加一个照片I_k+1；

步骤3.2，通过视觉里程计技术，根据I_k和I_k+1照片，计算出小车的位移(Δx,Δy)和旋转(Δtheta)；

步骤3.3，向网络G中增加一个点P_k+1，为(P_k.x+cos(P_k.theta)Δx-sin(P_k.theta)Δy，P_k.y+sin(P_k.theta)△x+(P_k.theta)△y，P_k.theta+△theta)，然后将连接了Pk和Pk+1的边E＝(Δx,Δy,Δtheta)加入到网络P中；

步骤4，小车完成预定的行走路线，同时记当前共拍摄了N张照片。

2.根据权利要求1所述的一种AGV视觉轨道的铺设及优化方法，其特征在于：视觉轨道的定位具体包含如下步骤：

步骤a，摄像头拍摄第一张照片Y0并进行视觉轨道上初始位置检测，并得到初始位置坐标Q0

步骤b，对摄像头拍摄到的第k张照片Yk，通过视觉里程计计算Y_k-1和Y_k对应的小车位移和旋转为(Δx,Δy)，小车旋转为Δtheta。然后暂时设置Q_k为(Q_k-1.x+cos(Q_k-1.theta)Δx-sin(Q_k-1.theta)Δy,Q_k-1.y+sin(Q_k-1.theta)△x+(Q_k-1.theta)△y,mod(Q_k-1.theta+△theta,2pi)。

步骤c，启动一个临时线程，在P1-PN中寻找距离Qk最近的点Pr，然后通过视觉里程计计算Pr和Qk对应的小车位移和旋转。如果该位移小于阈值t，则坐标Qk最终设置为(P_r.x+cos(P_r.theta)Δx-sin(P_r.theta)Δy,P_r.y+sin(P_r.theta)△x+(P_r.theta)△y,mod(P_r.theta+△theta,2pi))。

3.根据权利要求1所述的一种AGV视觉轨道的铺设及优化方法，其特征在于：视觉轨道的优化具体包含如下步骤；

步骤5，对轨道数据库中所有的点P_x，如果P_y满足(P_x.x-P_y.x)^2+(P_x.y-P_y.y)^2<t，其中t是一个预设的匹配范围阈值，则使用视觉里程计技术，根据张I_x照片和I_y照片计算对应的小车位移(Δx,Δy)和旋转(Δtheta)，将连接了P_x和P_y的边E＝(Δx,Δy,Δtheta)加入到网络P中

步骤6，使用以下方法进行小车轨道优化：

步骤6.1>初始化集合K₁～K_N，其中K_n＝{Pn}，该K_n随后用于计算P_n的更新值

步骤6.2>对每条边E和其连接的两个点Pa，Pb：

集合Ka增加一个点(P_b.x+cos(P_b.theta)Δx-sin(P_b.theta)Δy,

P_b.y+sin(P_b.theta)△x+(P_b.theta)△y,mod(P_b.theta+△theta,2pi))

集合Kb增加一个点(P_a.x-cos(P_a.theta)Δx+sin(P_a.theta)Δy,P_a.y-sin(P_a.theta)△x-(P_a.theta)△y,mod(P_a.theta-△theta,2pi))

步骤6.3>更新Pn为Kn中所有点的平均值。

步骤6.4>重复该步骤，直到Pn的变化趋于0.

步骤7：使用B-spline拟合Pn并得到拟合后的平滑曲线W。曲线W、图G共同储存为视觉轨道。