一种自动驾驶车辆对接拖车自动托挂钩的方法
技术领域
本发明涉及一种自动对接方法,尤其是涉及一种自动驾驶车辆对接拖车自动托挂钩的方法。
背景技术
拖车或托板用于承载货物,其前端设置有挂接装置,目前动力车辆挂接拖车和拖板的操作主要由人工手动进行。在涉及到自动驾驶车辆的时候,没有司机或者操作人员进行自动驾驶车辆与拖车或拖板的人工挂接,如果单独设立一个挂接的人员,则又会增加人力成本,丧失自动驾驶的意义。
为了解决上述问题,当前的现有方案是采用固定位置挂接的方法,即当需要自动驾驶车辆进行自动挂接拖车和拖板时,即将拖车或拖板停放到标定好的固定位置,自动驾驶车辆根据预设的位置信息来挂接。现有技术缺点:使用现有固定位置挂接的方案,当拖车停靠的位置有偏差、错位、倾斜或精度不够时,就会发生挂接失败的情况。而人工驾驶的车辆很难一次性停靠到精准的位置。自动驾驶车辆也存在精度不够的情况。如果多次调整定位或者人工干预调整的话,则会增加人力成本,降低工作效率。失去了自动驾驶的意义。
发明内容
本发明提供了一种自动驾驶车辆对接拖车自动托挂钩的方法,解决了自动驾驶车辆自动挂接拖车或拖板的问题,解决了现有的自动驾驶车辆、人工驾驶车辆,交接拖车和拖板的难题。其技术方案如下所述:
一种自动驾驶车辆对接拖车自动托挂钩的方法,在自动驾驶车辆上安装有激光雷达,在拖车前端的左右两侧分别设置有靶点,不同的拖车类型,两个靶点间的距离不同;自动托挂钩的对接包括下列步骤:
S1:所述自动驾驶车辆或路侧设备,通过传感器获取到拖车的数据;
S2:使用激光靶点时,所述自动驾驶车辆的工控机获取到点云数据后,根据反射率对点云数据进行点云滤波得到靶点点云数据;获取到靶点数据后进入S6;
S3:使用视觉靶点时,所述自动驾驶车辆的工控机获取到图像后,根据颜色对图像进行滤波,得到靶点数据。获取到靶点数据后进入S6进行计算;
S4:使用无线电靶点时,所述自动驾驶车辆使用UWB接收器测量靶点的距离,获取到距离后进入S6计算;
S5:使用深度学习方案时,所述自动驾驶车辆先对激光雷达和相机进行标定。使用预先训练的模型通过视觉识别拖板,并与激光雷达点云进行融合,获取到拖板前挡板的点云数据。获取到点云数据后进入S6进行计算;
S6:通过两个靶点对应的两片点云坐标,计算两个靶点的距离,根据工控机存储器预设的靶点信息确定拖车的型号识别,以及确定拖板车的车辆类型;
S7:根据符合分布模式的点云数据,获取挂钩位置;
S8:根据牵引点在雷达坐标系中的坐标,求出激光雷达在雷达坐标系中的位姿;
S9:工控机根据位姿规划路径驱动自动驾驶车辆向原点倒车,完成自动挂接;
S10:在板车挂接点和车辆挂接点挂接时会有一定误差,在车辆挂接点安装限位引导装置,使用左右的八字型限位引导器以及向下的舌型引导器来容差并引导挂点,在挂点处设置限位传感器,当挂点到位时触发传感器,插销落下完成自动挂接。
进一步的,步骤S1中,扫描获取点云数据包括以下步骤:
S11:所述自动驾驶车辆的工控机获取激光雷达的扫描数据;
S12:工控机循环读取扫描数据对应的每个点;
S13:工控机读取每个点的点反射率;
S14:工控机在其存储器查找反射率阈值;
S15:根据反射率阈值,判断点反射率是否符合,工控机将符合的点记录下来,不符合则跳转到步骤S12;
S16:工控机将符合的点的数据保存,并发布点云数据。
步骤S1中,所述靶点的激光反射率明显区分于周边环境。
进一步的,步骤S2中,点云滤波包括下列步骤:
S21:工控机遍历雷达点云;
S22:对点云的每个点,判断该点是否为靶点;不是则返回步骤S21;
S23:该点是靶点时,则判断该点是否为点云的首点;是首点则将该点存储到新点集,并返回步骤S21;
S24:该点不是点云的首点时,判断该点前面的点是否为靶点;不是则将该点存储到新点集,并返回步骤S21;
S25:该点前面的点是靶点时,判断该点与前面的点的距离是否超出设定的阈值;是的话说明该点也是靶点,将该点存储到新点集,并返回步骤S21;
S26:如果两点间的距离没有超出阈值,则将该点存储到当前点集;
S27:当前点集合并存储到新点集的各点,进而实现对过滤后的点云数据进行聚类。
进一步的,步骤S3中视觉滤波的步骤如下:
S31:将图像转为HSV或HSL格式;
S32:根据颜色空间确定靶点的位置;
S33:根据预知的靶点距离和靶点间的像素值,计算出靶点各像素点的坐标;
S34:进入S6进行计算。
进一步的,步骤S4中,UWB识别的步骤如下:
S41:通过UWB接受器接受发射器数据;
S42:根据飞行时间计算距离;
S43:计算出靶点三维坐标;
S44:进入S6进行计算。
进一步的,步骤S5中,视觉激光雷达融合识别的步骤如下:
S51:获取视觉图像;
S52:获取激光点云;
S53:融合图像和点云数据,形成统一数据;
S54:通过深度学习识别出图像中的拖板车的前挡板;
S55:找到融合数据中前挡板的点云数据;
S56:挡板点云数据与靶点点云数据是一致的,计入S6进行计算。
进一步的,步骤S6中,通过靶点距离判断车辆类型的过程如下所述:
S61:遍历聚合后的点集;
S62:对每个点集求(x,y)坐标的平均值,即为中心点坐标;
S63:计算中心点的分布模式;
S64:匹配已知车辆的分布模式,判断车辆类型;
S65:保留符合分布模式的点,过滤不符合的点。
进一步的,步骤S7中,靶点的中心点坐标,需要先根据点间距聚合为左侧点群和右侧点群分别求出Pl和Pr的平均坐标,再求出中心点的坐标Xc=(Xl+Xr)/2;Yc=(Yl+Yr)/2;其中Pl为左侧靶点,为Pr右侧靶点;(Xc,Yc)为中心点坐标,(Xl,Yl)为左侧靶点的坐标,(Xr,Yr)为右侧靶点坐标。
进一步的,步骤S8中,输出位姿的数据包括挂钩点的坐标、拖车斜率以及挂钩点距离。
进一步的,步骤S9中,自动挂接包括以下步骤:
S91:拖头车到达自动脱挂钩开始位置,开始接受拖板位姿;
S92:根据接收到的位姿判断是否找到拖板车,如果找到执行步骤S63,如果找不到则向调度***报告错误,结束本次挂接;
S93:根据拖板位姿规划路径;
S94:判断路径是否可达,如果可达则按照规划路径挂接,本结束本次操作;如果不可达则执行步骤S85;
S95:沿斜率切向向前摆正车体;
S96:规划挂接路径并挂接。
进一步的,步骤S10中,自动挂接包括以下步骤:
S101:挂点接触限位装置;
S102:挂点在限位装置引导下向插销行进;
S103:挂点到达插销处,出发到位传感器;
S104:所述自动驾驶车辆收到到位信号后,插销擒纵装置松开插销,插销落下,销住挂点。
本发明可以极大的提高自动驾驶车辆对接拖车,实现挂接拖车的运行效率,减少了人工干预,使作业流程更流畅。并可以完成一些原来的方案无法完成的任务,使自动驾驶的应用范围更广泛。
附图说明
图1是激光雷达与靶点的安装示意图;
图2是所述自动驾驶车辆对接拖车自动托挂钩的方法的流程示意图;
图3是所述数据筛选的处理示意图;
图4是所述激光靶点聚合的处理示意图;
图5是视觉靶点的数据处理流程图;
图6是UWB靶点的数据处理流程图;
图7是视觉融合激光深度学习处理流程图;
图8是所述过滤杂点的处理示意图;
图9是所述计算输出的处理示意图;
图10是所述拖头车行进的处理示意图;
图11是限位引导装置的结构示意图。
具体实施方式
如图1所示,本发明采用激光雷达加激光靶点的方式进行拖车识别。为便于表述,作为提供动力的自动驾驶车辆,采用能够自动驾驶的拖头车1,所述拖头车1的后端中部设置有牵引点2,在拖头车1上设置有激光雷达3,通过激光雷达3来进行识别。
需要和拖头车1进行对接的拖车4的前端中部,设置有与牵引点2对应的拖车挂点5,也可称为挂钩,所述拖车4的前端两侧分别设置有左靶点6、右靶点7。实施例中,以拖车挂点5为中心,在拖车4的拖车挂点5两侧各一米的位置,对称安装两个靶点,所述靶点尺寸为50mm*250mm,保证激光雷达3可以打到靶点上,进而实现靶点识别。现有拖车车体的高度为500mm,靶点安装在拖车4下侧,以保证不影响货物装卸。
其中,靶点识别通过反光率结合靶点安装模式来实现,所述反光率的理解是,不同材质的物体对激光雷达的反光率是不同的,根据这一原理本发明使用具有特定反光率的材质制作激光靶点,在雷达数据中根据特定反光率来筛选数据找到靶点。所述靶点的安装是依照数学模式,可以在靶点信息中筛选出预置靶点,去除干扰点。
这样,激光雷达3数据传送到拖头车1的工控机,在工控机中进行拖车4的识别和位置计算。工控机中的车辆控制单元,接受拖车4的类型和位姿,根据拖车4的位姿来规划挂接路径,控制拖头车1进行挂接。
如图2所示,本发明提供的自动驾驶车辆对接拖车自动托挂钩的方法,包括以下步骤:
S1:所述拖头车1通过激光雷达3扫描靶点,其中,靶点的激光反射率明显区分于周边环境,从而获取到靶点反射率的点云数据;
结合图3所示,扫描靶点是对扫描到的每个点都进行判断,将符合反射率阈值的点进行记录,进而形成靶点的点云数据。左靶点6、右靶点7两个靶点,会形成两片点云数据。
其中,点云数据的筛选包括以下步骤:
S11:所述拖头车1的工控机获取激光雷达3的扫描数据;
S12:工控机循环读取扫描数据对应的每个点;
S13:工控机读取每个点的点反射率;
S14:工控机在其存储器查找反射率阈值;
S15:根据反射率阈值,判断点反射率是否符合,工控机将符合的点记录下来,不符合则跳转到步骤S12;
S16:工控机将符合的点的数据保存,并发布点云数据。
激光雷达扫描是现有技术,不再赘述,以下是某实施例中激光雷达3扫描时涉及的输入数据:
点云输入监听激光雷达发表的话题,话题格式为标准PointCloud2格式:
Header header#文件头
uint32 height#点云的高度
uint32 width
PointField[]fields
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[]data
bool is_dense
S2:所述工控机获取到激光靶点的点云数据后,根据反射率对点云数据进行滤波处理,优化点云数据,由于靶点的反射率明显区别于环境,通过滤波即可确定点云坐标。
如图4所示,点云滤波包括下列步骤:
S21:工控机遍历雷达点云;
S22:对点云的每个点,判断该点是否为靶点;不是则返回步骤S21;
S23:该点是靶点时,则判断该点是否为点云的首点;是首点则将该点存储到新点集,并返回步骤S21;
S24:该点不是点云的首点时,判断该点前面的点是否为靶点;不是则将该点存储到新点集,并返回步骤S21;
S25:该点前面的点是靶点时,判断该点与前面的点的距离是否超出设定的阈值;是的话说明该点也是靶点,将该点存储到新点集,并返回步骤S21;
S26:如果两点间的距离没有超出阈值,则将该点存储到当前点集;
S27:当前点集合并存储到新点集的各点,进而实现对过滤后的点云数据进行聚类。
该方法中,点云坐标的滤波并不需要复杂的建模,或者进行深度学习。
S3:当采用视觉靶点时,根据颜色对图像数据进行滤波处理,优化图像数据,由于靶点的颜色明显区别于环境,通过滤波即可确定点云坐标。
如图5所示,图像处理包括以下步骤:
S31:将图像转为HSV或HSL格式;
S32:根据颜色空间确定靶点的位置;
S33:根据预知的靶点距离和靶点间的像素值,计算出靶点各像素点的坐标;
S34:进入S6进行计算。
S4:使用无线电靶点时,使用UWB接收器测量靶点的距离,获取到距离后进入S6计算。
如图6所示,UWB处理流程如下:
S41:通过UWB接受器接受发射器数据;
S42:根据飞行时间计算距离;
S43:计算出靶点三维坐标;
S44:进入S6进行计算。
S5:当采用视觉和激光融合算法时,先对相机和激光雷达进行标定,将图像和点云数据融合,利用视觉深度学习识别算法比较成熟,激光测量数据非常精准的特点进行测量。
如图7所示,融合处理流程如下:
S41:获取视觉图像;
S42:获取激光点云;
S43:融合图像和点云数据,形成统一数据;
S44:通过深度学习识别出图像中的拖板车的前挡板;
S45:找到融合数据中前挡板的激光点云数据;
S46:挡板点云数据与靶点点云数据是一致的,计入S6进行计算。
S6:通过上述处理,获取到两个靶点对应的两片点云坐标,通过两个靶点的距离,根据工控机存储器预设的靶点信息,来实现拖车的型号识别,以及确定拖板车的车辆类型,并进一步过滤靶点,实现点云聚合。
结合图8所示,包括下列步骤:
S61:遍历聚合后的点集;
S62:对每个点集求(x,y)坐标的平均值,即为中心点坐标;
S63:计算中心点的分布模式
S64:匹配已知车辆的分布模式,判断车辆类型
S65:保留符合分布模式的点,过滤不符合的点。
S7:根据符合分布模式的点云数据,获取挂钩位置,进而得到拖车位姿。
所述挂钩位置根据存储器中的预设数据进行计算,计算方法采用最小二乘法。
最小二乘法确定位姿时:
在具体的测量中,在静止状态下通过多帧数据复合,本发明可以得到多个测量点。根据最小二乘法公式:
拟合直线公式为:y=k*x+b;其中x为直线上点的x坐标、y为直线上点的y坐标、k为直线的斜率、b为直线的截距。
其中k为直线的斜率,x为获得的点的x坐标,y为获得的点的y坐标,
为所有点x乘y的值的平均值,
为所有点的x值的平均值乘所有点的y值的平均值,
为所有点x值平方的平均值,
为所有点的x值的平均值的平方。
计算出斜率k后,根据
和前面算出的斜率k,使用待定系数法求出截距b。
对于中心点坐标,需要先根据点间距聚合为左侧点群和右侧点群分别求出Pl和Pr的平均坐标,再求出中心点的坐标Xc=(Xl+Xr)/2;Yc=(Yl+Yr)/2。其中Pl为左侧靶点,为Pr右侧靶点。(Xc,Yc)为中心点坐标,(Xl,Yl)为左侧靶点的坐标,(Xr,Yr)为右侧靶点坐标。
S8:然后,根据坐标变换公式,根据牵引点在雷达坐标系中的坐标,求出激光雷达在雷达坐标系中的位姿。
输出位姿的数据如下表所示:
数据序号 |
数据类型 |
数据内容 |
备注 |
0 |
Header |
消息头 |
|
1 |
float32 |
挂钩点x坐标 |
|
2 |
float32 |
挂钩点y坐标 |
|
3 |
float32 |
拖车斜率 |
|
4 |
float32 |
挂钩点距离 |
|
此时,将挂点作为雷达坐标系中的原点,将拖头车的激光雷达作为该坐标系中的某坐标点,然后进行下面的路径规划。
S9:工控机根据位姿规划路径驱动拖头车向原点(即挂点)倒车,如图10所示,其包括以下步骤:
S91:拖头车到达自动脱挂钩开始位置,开始接受拖板位姿;
S92:根据接收到的位姿判断是否找到拖板车,如果找到执行步骤S63,如果找不到则向调度***报告错误,结束本次挂接;
S93:根据拖板位姿规划路径;
S94:判断路径是否可达,如果可达则按照规划路径挂接,本结束本次操作;如果不可达则执行步骤S65;
S95:沿斜率切向向前摆正车体;
S96:规划挂接路径并挂接。
S10:图11为限位引导装置的结构图,在板车挂接点和车辆挂接点挂接时会有一定误差,在车辆挂接点安装限位引导装置,使用左右的八字型限位引导器以及向下的舌型引导器来容差并引导挂点,在挂点处设置限位传感器,当挂点到位时触发传感器,插销落下完成自动挂接。
本发明实现了无人工干预的自动识别、测量拖车的停靠位姿,可以真正的自动调整挂接路径、自动挂接,提高流程效率。