一种基于视觉传感器的重型导引车定位方法及重型导引车与流程

本发明涉及重型agv(automatedguidedvehicle，自动导引运输车)技术领域，具体是一种基于视觉传感器的重型导引车定位方法及重型导引车。

背景技术：

随着智能化产业的发展，重载运输领域自动导引的需求越来越多。如船厂、变压器厂、钢厂等，尤其是工业制造自动化程度较高的行业，对大型构件自动化转运的需求更为迫切。在自动导引车行业，导引定位方法大多适用于小型agv。如常见的色带、二维码传感器，此类引导定位方法检测范围小，检测距离短，不适合具有升降功能的重型导引车。目前应用最广泛的自动化码头重载agv导引系统为磁钉导引方式，但这种引导方式检测距离较短，需要对路面改造，工艺复杂成本高，gps导引精度不高，不适合室内运输。

技术实现要素：

针对上述现有技术中存在的问题，本发明提供一种适用于具有升降功能及多模式转向功能重型导引车的基于视觉传感器的重型导引车定位方法，该重型导引车定位方法包括以下步骤：

通过分别安装于重型导引车中心线头尾的两个视觉传感器，实时扫描并解析重型导引车前后视野范围内的二维码地标的位置信息和预设信息；所述二维码地标的预设信息包括该二维码地标的编号，所述位置信息包括该二维码地标的像素位置信息、偏角信息以及最小单位像素信息；

其中，所述二维码地标的像素位置信息为该二维码地标的中心点位于正在扫描该二维码地标的当前视觉传感器所对应的视野坐标系中的x坐标和y坐标，用像素表示；所述偏角信息为该二维码地标相对于所述视野坐标系的偏角；所述最小单位像素信息为该二维码地标的最小单位在所述当前视觉传感器的视野范围内的像素值；

由视觉传感器所采集到的当前二维码地标的最小单位像素a、所述当前二维码的最小单位边长信息b以及该视觉传感器的像素分辨率c，根据该视觉传感器成像的线性关系，计算出该视觉传感器的视野大小d＝c*b/a；

然后根据所述当前二维码地标的像素位置信息，由该视觉传感器的像素分辨率及视野大小成像比例关系，计算出所述当前二维码地标的中心点在该视觉传感器所对应的视野坐标系中的坐标p；

由该视觉传感器的安装位置和所述当前二维码地标的中心点在该视觉传感器所对应的视野坐标系中的坐标p，根据坐标系平移旋转公式计算出所述当前二维码地标的中心点在车辆坐标系中的坐标^vp；并根据所述当前二维码地标的预设信息计算出所述当前二维码地标的中心点在大地坐标系中的坐标^gp；

由所述当前二维码地标的中心点对应的坐标^vp、坐标^gp以及所述车辆坐标系与所述大地坐标系的偏差，根据坐标旋转与平移变换方法，求得所述车辆坐标系与所述大地坐标系的位置偏差,即车辆实时位置与目标位置的偏差；

由前后视觉传感器各自所解析到的前后二维码地标的位置信息和预设信息分别计算出前后二维码地标的中心点在相应视野坐标系中的坐标和前后二维码地标的中心点在大地坐标系中的坐标，然后根据坐标变换方法得到前后视觉传感器各自对应的视野坐标系的原点在所述大地坐标系中的坐标；继而求得所述车辆坐标系与所述大地坐标系的姿态偏差，即车辆实时姿态与目标姿态的偏差；

车载控制器根据计算出的车辆实时位置与目标位置的偏差以及车辆实时姿态与目标姿态的偏差，调用闭环控制算法控制重型导引车自动作业。

进一步地，多个所述二维码地标以预设的固定间距呈网格状铺设于重型导引车所在工作区域的地面上，且多个二维码地标同向分布。

进一步地，所述视野坐标系以其所对应的视觉传感器的视野范围的左前点为坐标原点，以其所对应的视觉传感器的视野范围的顶边为x轴，左侧边为y轴；

所述车辆坐标系以车辆的几何中心为坐标原点，以车辆纵向中线为y轴，以车辆前进方向为y轴正向；所述大地坐标以位于二维码地标矩阵左下角的二维码地标的几何中心为坐标原点，以二维码地标矩阵的正向为y轴正向。

进一步地，所述由该视觉传感器的安装位置和所述当前二维码地标的中心点在该视觉传感器所对应的视野坐标系中的坐标p，根据坐标系平移旋转公式计算出所述当前二维码地标的中心点在车辆坐标系中的坐标^vp，具体为：

由该视觉传感器的安装位置得到该视觉传感器所对应的视野坐标系相对于所述车辆坐标系的旋转角度为0；

由该视觉传感器的视野大小、该视觉传感器的安装支架尺寸、重型导引车尺寸求得该视觉传感器所对应的视野坐标系相对于车辆坐标系的平移矩阵，

由二维码地标的中心点在该视觉传感器所对应的视野坐标系中的坐标p、该视野坐标系相对于车辆坐标系的旋转矩阵和平移矩阵，根据坐标系平移旋转公式计算出该二维码地标的中心点在车辆坐标系中的坐标^vp。

进一步地，所述由当前二维码地标的中心点对应的坐标^vp、坐标^gp以及车辆坐标系与大地坐标系的偏差，根据坐标旋转与平移变换方法，求得车辆坐标系与大地坐标系的位置偏差,即车辆实时位置与目标位置的偏差，具体为：

将当前二维码地标的偏角信息作为大地坐标系相对于车辆坐标系的旋转角θ，根据所述旋转角θ求得所述大地坐标系相对于所述车辆坐标系的旋转矩阵；

根据大地坐标系相对于车辆坐标系的旋转矩阵、当前二维码地标的中心点在车辆坐标系中的坐标、当前二维码地标的中心点在大地坐标系中的坐标，求得大地坐标与车辆坐标系的平移矩阵，即车辆实时位置与目标位置的偏差。

相应地，针对上述现有技术中存在的问题，本发明还提供一种采用上述重型导引车定位方法的重型导引车，该重型导引车包括重型车本体和自动导引系统；其中，所述重型车本体包括多个轮组，每一所述轮组上分别安装有液压悬挂、角度传感器、升降电磁阀以及转向电磁阀；且位于所述重型车本体的四个角上的液压悬挂分别安装有高度传感器；

此外，所述重型车本体还包括安装于该重型车本体的动力单元上的电池包和电池管理系统；其中，所述电池管理系统与所述电池包电连接，且所述电池包设置有自动充电装置；所述多个轮组中的其中一部分轮组为电驱动轮组；且每一所述电驱动轮组分别与驱动电机控制器电连接，所述驱动电机控制器与所述电池包电连接；

所述自动导引系统包括通信转换模块、车载控制器以及用于扫描并解析二维码地标的位置信息和预设信息的两套视觉传感器；所述两套视觉传感器分别通过安装支架安装于所述重型车本体的中心线头尾，其检测面与地面平行；且所述视觉传感器带有自动变焦镜头，其检测距离能够根据车辆升降情况自动对焦，自适应检测算法满足车辆自动升降作业需求；同时所述视觉传感器还带有微控制器并集成算法，能够实时扫描并解析二维码地标的位置信息和预设信息；而所述通信转换模块和车载控制器则均安装于所述重型车本体的控制箱中；

所述视觉传感器的电源线与所述电池包电连接，所述视觉传感器的通信线通过所述通信转换模块与所述车载控制器电连接；所述车载控制器的输入端口与各角度传感器、各高度传感器分别电连接，所述车载控制器的输出端口与各转向电磁阀、各升降电磁阀分别电连接；同时所述车载控制器与所述驱动电机控制器和所述电池管理系统分别电连接。

进一步地，所述电驱动轮组包括两个通过插装桥连接的橡胶轮，所述插装桥上安装有驱动电机，所述驱动电机与所述驱动电机控制器电连接；且所述插装桥内置有与两个所述橡胶轮相连的减速机和差速器。

进一步地，所述重型车本体设置有十二个轮组，其中四个为电驱动轮组。

进一步地，所述通信转换模块为rs232串口转can通信模块，用于将两个视觉传感器输出的rs232串口信号转换为一路can总线信号；所述车载控制器通过can总线分别与所述通信转换模块、所述驱动电机控制器以及所述电池管理系统电连接。

进一步地，所述二维码地标采用激光打印机在钢板上打印而成，并通过双面胶固定在地面上。

本发明通过分别安装于重型导引车中心线头尾的两套视觉传感器，在一定检测距离内自动对焦识别二维码地标，实时扫描并解析二维码地标的位置信息和预设信息。并通过二维码地标的位置信息计算出车辆在大地坐标系中的实时位置，通过两套视觉传感器的位置信息计算出车辆在大地坐标系中的实时姿态；进而根据车辆实际位姿与目标位姿的偏差，结合上位机路径规划指令控制车辆自动作业。其中所用的视觉传感器的检测距离能够根据车辆升降情况自动对焦，可以适应室内环境和室外环境应用需求；且具有应用灵活，检测范围大，读取速度快的特点，适用于大型车辆高速及低速运行需求。

此外，本发明的重型导引车配置有负载敏感液压转向系统，应用全轮独立转向技术。每个轮组分别配置有液压悬挂系统，在路面不平整时具有很好的补偿能力，对液压悬挂进行分组，可以实现整体升降和单点升降功能。配置锂电池及自动充电装置，采用分布式电驱动方式。单车运输吨位可达几十吨，多台车协同运输可达上千吨。

附图说明

图1为本发明中的二维码地标的布置示意图；

图2为本发明中的视野坐标系、车辆坐标系以及大地坐标系的示意图；

图3为本发明中的自动导引系统的系统框图；

图4为本发明中的重型导引车的系统布置图；

图5为本发明中的重型导引车的另一系统布置图；

图6为本发明中的液压悬挂及电驱动轮组的结构示意图。

附图标记说明：

1：二维码地标，2：安装支架，3：视觉传感器，4：通信转换模块，

5：车载控制器，6：电池管理系统，7：驱动电机控制器，8：电池包，

9：电驱动轮组，10：液压悬挂，11：高度传感器，12：角度传感器，

13：自动充电装置，9-1：插装桥，9-2：驱动电机，9-3：橡胶轮，

10-1：回转支承，10-2：升降油缸，10-3：涡轮蜗杆转向机构。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

第一实施例

本实施例提供一种基于视觉传感器的重型导引车定位方法，该重型导引车定位方法包括以下步骤：

s101，通过分别安装于重型导引车中心线头尾的两个视觉传感器，实时扫描并解析重型导引车前后视野范围内的二维码地标的位置信息和预设信息；

需要说明的是，多个二维码地标是以预设的固定间距呈网格状铺设于重型导引车所在工作区域的地面上，且多个二维码地标同向分布，如图1所示；具体地，本实施中所采用的二维码地标的大小为5mm*5mm，铺设时各二维码地标的横向间距和纵向间距均为500mm。

多个二维码地标按照从下至上，从左至右的顺序依次进行编号。如从下到上第n排，从左到右第m排的二维码地标的编号为anm。其中二维码地标的预设信息为该二维码地标的编号，如anm号的二维码地标的预设信息为nm。位置信息包括二维码地标的像素位置信息、偏角信息以及最小单位像素信息；

更进一步地，二维码地标的像素位置信息为该二维码地标的中心点位于正在扫描该二维码地标的当前视觉传感器所对应的视野坐标系中的x坐标和y坐标，用像素表示；偏角信息为该二维码地标相对于视野坐标系的偏角；最小单位像素信息为该二维码地标的最小单位在当前视觉传感器的视野范围内的像素值；其中的视野坐标系是以视觉传感器的视野范围的左前点为坐标原点，以视野范围的顶边为x轴，左侧边为y轴的坐标系。其中前部视觉传感器的视野坐标系为of,后部视觉传感器的视野坐标系为or，如图2所示。

两套视觉传感器通过安装支架分别安装于车辆中心线前后，各视觉传感器的检测面与地面平行。当车辆降到最低时，视觉传感器的检测距离为1000m，车辆上升到最高时，视觉传感器的检测距离为1600mm。视觉传感器的视觉范围随着检测距离的增大而增大，其中最小视野范围为600mm*800mm。因此当二维码地标的铺设间距为500mm*500mm时，能够保证视觉传感器每个时刻均能检测到至少一个二维码地标，从而到达实时控制的目的。其中前部视觉传感器检测到的二维码地标的中心点为p1，后部视觉传感器检测到的二维码地标的中心点定义为p2。

s102，根据视觉传感器所采集到的二维码地标的位置信息计算出该二维码地标的中心点在该视觉传感器所对应的视野坐标系中的坐标；

需要说明的是，随着车辆的升降，视觉传感器的视野大小会发生变化，进而二维码地标的最小单位像素值随之变化。因此结合视觉传感器所固有的像素信息、二维码地标的最小单位边长信息就可以计算出视觉传感器的视野大小。然后根据该视觉传感器所采集到的二维码地标的像素位置信息就可以计算该二维码地标的中心点在该视觉传感器所对应的视野坐标系中的坐标。

具体计算过程为：由视觉传感器所采集到的当前二维码地标的最小单位像素a、所述当前二维码的最小单位边长信息b以及该视觉传感器的像素分辨率c，根据该视觉传感器成像的线性关系，计算出该视觉传感器的视野大小d＝c*b/a；然后根据所述当前二维码地标的像素位置信息p’，由该视觉传感器的像素分辨率c及视野大小d成像比例关系，计算出所述当前二维码地标的中心点在该视觉传感器所对应的视野坐标系中的坐标p；其中前部视觉传感器所检测到的二维码地标的中心点在其视野坐标系中的坐标记为^fp1，后部视觉传感器所检测到的二维码地标的中心点在其视野坐标系中的坐标记为^rp2。

s103，由该视觉传感器的安装位置和所述当前二维码地标的中心点在该视觉传感器所对应的视野坐标系中的坐标p，根据坐标系平移旋转公式计算出所述当前二维码地标的中心点在车辆坐标系中的坐标^vp；

需要说明的是，上述车辆坐标系ov是以车辆的几何中心为坐标原点，以车辆纵向中线为y轴，以车辆前进方向为y轴正向的坐标系，如图2所示。根据坐标系平移旋转公式，可知：其中为b坐标系相对于a坐标系的旋转矩阵，为b坐标系相对于a的平移矩阵，^ap为p点在a坐标中的位姿，^bp为p点在b坐标中的位姿。据此，上述计算二维码地标的中心点在车辆坐标系中的坐标^vp的过程具体为：

由该视觉传感器的安装位置得到该视觉传感器所对应的视野坐标系相对于所述车辆坐标系的旋转角度为0；由该视觉传感器的视野大小d、该视觉传感器的安装支架尺寸、重型导引车尺寸求得该视觉传感器所对应的视野坐标系相对于车辆坐标系的平移矩阵，由二维码地标的中心点在该视觉传感器所对应的视野坐标系中的坐标p、该视野坐标系相对于车辆坐标系的旋转矩阵和平移矩阵，根据坐标系平移旋转公式计算出该二维码地标的中心点在车辆坐标系中的坐标^vp。

s104，根据二维码地标的预设信息计算出该二维码地标的中心点在大地坐标系中的坐标^gp；

需要说明的是，上述大地坐标og是以位于二维码地标矩阵左下角的二维码地标，也即a11号二维码地标的几何中心为坐标原点，以二维码地标矩阵的正向为y轴正向的坐标系，如图2所示。由于二维码地标是以固定间距同向网格状铺设于地面，按照顺序对二维码地标进行编号，并将二维码地标的编号作为预设信息输入至二维码地标，因此可根据二码地标的编号计算得到该二维码地标在大地坐标系中的坐标。

s105，由二维码地标的中心点对应的坐标^vp、坐标^gp以及所述车辆坐标系与所述大地坐标系的偏差，根据坐标旋转与平移变换方法，求得所述车辆坐标系与所述大地坐标系的位置偏差,即车辆实时位置与目标位置的偏差；

需要说明的是，由于视觉传感器通过安装支架安装于车辆前后中心线，且视觉传感器视野相对于车辆姿态没有偏差，因此二维码地标相对于视野坐标系的偏角就等同于二维码地标相对于车辆坐标系的偏差。设大地坐标系正方向为二维码地标正方向，由此可知视觉传感器实时反馈的偏差信息即为车辆坐标系与大地坐标系的偏差。因此上述求得车辆实时位置与目标位置的偏差的过程具体为：

将当前二维码地标的偏角信息作为大地坐标系相对于车辆坐标系的旋转角θ，根据所述旋转角θ求得所述大地坐标系相对于所述车辆坐标系的旋转矩阵根据旋转矩阵当前二维码地标的中心点在车辆坐标系中的坐标^vp、当前二维码地标的中心点在大地坐标系中的坐标^gp，求得大地坐标与车辆坐标系的平移矩阵即车辆实时位置与目标位置的偏差。

s106，由前后视觉传感器各自所解析到的前后二维码地标的位置信息和预设信息分别计算出前后二维码地标的中心点在相应视野坐标系中的坐标和前后二维码地标的中心点在大地坐标系中的坐标，然后根据坐标变换方法得到前后视觉传感器各自对应的视野坐标系的原点在大地坐标系中的坐标；继而求得车辆坐标系与大地坐标系的姿态偏差，即车辆实时姿态与目标姿态偏差；

需要说明的是，由于重型车体型较长，用视觉传感器直接读取的偏差信息进行车辆轨迹误差校正控制精度不高。因此本实施例通过前后视觉传感器的位置信息计算车辆在大地坐标系中的偏差信息以提高精度。

其中，由前后传感器读取的二维码地标的位置信息可以计算出前后二维码地标的中心点在视野坐标系中的坐标^fp1、^rp2，由前后传感器读取的二维码地标的预设信息可以得到前后二维码地标的中心点在大地坐标系中的坐标^gp1、^gp2，然后依据坐标变换方法就可以得到前后传感器视野坐标系原点在大地坐标中坐标由于前后传感器视野坐标系原点连线与车辆纵向中心线平行，从而可得车辆坐标系与大地坐标系姿态偏差，即车辆实时姿态与目标姿态的偏角。

s107，车载控制器根据计算出的车辆实时位置与目标位置的偏差以及车辆实时姿态与目标姿态的偏差，调用闭环控制算法控制重型导引车自动作业。

需要说明的是，上述控制重型导引车自动作业具体为：视觉传感器将读取的二维码地标的最小单位像素信息、像素位置信息、偏差信息、预设信息通过串口发送给通信转换模块，通信转换模块将信息转换为can总线信息发送给车载控制器，车载控制器根据上述定位计算方法运算得到车辆实时位姿与目标位姿的偏差，调用闭环控制算法分别控制车辆转向电磁阀，升降电磁阀，驱动电机控制器动作，实现重型车自动作业功能。

同时本发明的重型导引车在自动导引定位时，可以通过无线通信系统接收上位机调度信息同时控制多台车辆协同工作，也可以在车载控制器中预设作业任务，完成单机自动导引运输。

第二实施例

请参阅图3至图6，本实施例提供一种重型导引车，该重型导引车包括重型车本体和自动导引系统；其中，所述重型车本体包括同步控制液压升降系统、电子多模式负载敏感液压转向系统和十二个轮组，其中每一轮组上分别安装有液压悬挂10、角度传感器12、升降电磁阀以及转向电磁阀；每个液压悬挂10均包含升降油缸10-2、回转支承10-1、涡轮蜗杆转向机构10-3。通过车载控制器5可控制液压悬挂10的涡轮蜗杆转向机构10-3自由转向，实现整车不同转向模式。多个液压悬挂10可通过液压管路及开关阀进行分组，可通过升降电磁阀控制升降油缸10-2伸缩，实现车辆总体升降和单点升降功能。且位于重型车本体的四个角上的液压悬挂10分别安装有高度传感器11；可通过车载控制器5实现升降闭环同步控制。

此外，所述重型车本体还包括安装于该重型车本体的动力单元上的电池包8和电池管理系统6；其中，所述电池管理系统6与所述电池包8电连接，且所述电池包8设置有自动充电装置13；上述十二个轮组中的四个轮组为电驱动轮组9；电驱动轮组9包括两个通过插装桥9-1连接的橡胶轮9-3，所述插装桥9-1上安装有驱动电机9-2，所述驱动电机9-2与驱动电机控制器7电连接；且所述插装桥9-1内置有与两个所述橡胶轮9-3相连的减速机和差速器。所述驱动电机控制器7与所述电池包8电连接。通过驱动电机控制器7可对每个电驱动轮组9的转速及扭矩进行闭环控制。

所述自动导引系统包括通信转换模块4、车载控制器5以及用于扫描并解析二维码地标1的位置信息和预设信息的两套视觉传感器3；两套视觉传感器3分别通过安装支架2安装于所述重型车本体的中心线头尾，所述通信转换模块4和所述车载控制器5均安装于所述重型车本体的控制箱中；其中的安装支架2为可调支架，其焊接在车架上，与车辆装载面平齐，保证不影响车辆装卸货物。并伸出车头与车尾，保证视觉传感器3检测面不被遮挡。

此外，本实施例中在可调支架上设置有加强筋，保证视觉传感器3安装稳定。同时可调支架用于安装视觉传感器3的小支架可以微调视觉传感器3水平方向的安装角度，保证视觉传感器3安装角度与车辆姿态偏差为零。

所述视觉传感器3的电源线与所述电池包8电连接，所述视觉传感器3的通信线通过所述通信转换模块4与所述车载控制器5电连接；所述车载控制器5的输入端口与各角度传感器12、各高度传感器11分别电连接，所述车载控制器5的输出端口与各转向电磁阀、各升降电磁阀分别电连接；同时所述车载控制器5与所述驱动电机控制器7和电池管理系统6分别电连接。

进一步地，上述视觉传感器3采用康耐视公司dm360系列产品，带有小型控制器并集成算法，可快速扫描可视范围内的二维码地标信息。带有液态自动变焦镜头，其检测距离能够根据车辆升降情况自动对焦，自适应检测算法满足车辆自动升降作业需求；检测距离大于1000mm，可适应800mm以上的距离变化检测需求。检测范围大于500mm*500mm。且该视觉传感器3自带光源，在光线不足时仍能快速检测，可满足室内外导引需求。

上述通信转换模块4为rs232串口转can通信模块，用于将两个视觉传感器3输出的rs232串口信号转换为一路can总线信号传输给上述车载控制器5；该车载控制器5选用易福门公司cr0234可编程控制器，防护性高，自带i/o，其通过can总线分别与通信转换模块4、驱动电机控制器7以及电池管理系统6电连接。该车载控制器5的自动导引程序包括信号处理模块、逻辑控制模块、安全保护模块、避障模块、通信模块、监测诊断模块等。

上述二维码地标1采用激光打印机在薄钢板上打印，保证强度。且薄钢板进行表面处理，改变其反光特性，使视觉传感器3能够可靠快速地读取。二维码地标1制作完成后用双面胶固定在地面上，从而在保证稳定性的基础上不损坏原有路面，具有灵活的应用性。

本发明设计了一种适用于具有升降功能及多模式转向功能重型导引车的定位方法及重型导引车。可适用于重型车室内室外自动导引需求，其定位精度达到毫米级，检测距离可以自动调节，适用于车辆自动作业需求，检测范围大，满足了大型车辆运动控制需求。检测速度快，识别速度达到42帧/秒，可满足车辆高速运行需求。检测信息丰富，导引控制算法能够灵活扩展。

采用两套视觉传感器融合算法，提高了重型车姿态检测精度，使重型车轨迹跟踪控制成为可能。相比磁钉复杂的基础建设要求，本发明的特殊工艺的二维码地标在兼具稳定性的同时，可满足灵活扩展的应用需求。同时本发明的重型导引车配置液压悬挂，具有良好的补偿能力。配置液压升降系统，具备自动升降作业功能。配置分布式电驱动轮组，结构紧凑控制精度高。

需要说明的是，本发明实施例中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。

以上仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域技术人员来说，本发明可有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。