基于视觉与IMU或里程计的AGV组合导航定位方法技术

本发明专利技术公开了一种基于视觉与IMU或里程计的AGV组合导航定位方法，属于自动控制技术领域，包括建立坐标系、设置配置文件、AGV初始化以及组合定位四个步骤。控制器处理二维码信息得到AGV的初始位姿；当视觉相机采集到有效地面信息后，控制器通过卡尔曼滤波算法融合视觉与IMU或里程计数据纠正定位误差，完成组合导航定位。本发明专利技术的有益效果是：摆脱了传统方法对专用参照物的依赖，只需要已有常见地面参照物，大大降低了现场施工成本、难度与工作量；不依赖无线射频定位信号（如UWB，WIFI，Bluetooth等），适用于舞台剧场等复杂电磁环境的应用场合；视觉相机拍摄地面，不怕人员或障碍物的遮挡（如激光反射板、激光雷达等），适用于动态非结构环境的应用场合。

AGV Integrated Navigation and Location Method Based on Vision and IMU or Odometer

【技术实现步骤摘要】
基于视觉与IMU或里程计的AGV组合导航定位方法
本专利技术属于自动控制
，具体涉及一种基于视觉相机和IMU或者视觉相机和里程计的AGV组合导航定位方法。
技术介绍
AGV，即AutomatedGuidedVehicle，通过预设的程序自动将物品从一个位置移动至另一位置，是一种自动化、信息化和智能化设备。其目前的导航方式有磁条、RFID信标、二维码标识、激光等。对于应用于舞台剧场等需要驱动舞台按预设的轨迹移动、旋转的AGV而言，上述的导航方式至少存在以下技术问题：舞台的移动方式多种多样、运动范围较大，在地面上铺设磁条、RFID信标或者二维码标识，将大大提升现场施工的成本、难度和工作量；如果采用无线射频的方式，由于舞台剧场中音响设备、无线话筒以及观众随身电子设备使得电磁环境十分复杂、干扰严重；如果采用激光导航的方式，由于舞台剧场等特殊的环境(舞台灯光的照射、演出人员的走动、设备的移动等)导致激光导航用的反射板难以安装导致上述的已知的导航方式无法实现或者需要较高的成本才能实现。
技术实现思路
本专利技术要解决的技术问题是提供一种基于视觉与IMU或里程计的AGV组合导航定位方法，通过视觉相机拍摄地面上的规则参照物得到的地面信息，配合AGV系统中IMU或里程计的信息，经过控制器对信息处理后实现组合定位，实现AGV的高精度定位，以适用于舞台剧场等环境。为解决上述技术问题，本专利技术采用的技术方案是：一种基于视觉与IMU或里程计的AGV组合导航定位方法，基于具有规则参照物的地面，包括以下步骤：步骤A、建立坐标系：以地面任意一点为原点建立地面平面直角坐标系，并在地面上粘贴具有位姿信息的二维码标识；步骤B、设置配置文件：将规则参照物的尺寸信息及二维码位姿信息输入至AGV的控制器配置文件，AGV控制器根据该配置文件自动生成网格地图；步骤C、AGV初始化：AGV上电后手动操控其经过二维码标识，视觉相机扫描二维码标识并通过控制器计算得到AGV在地面平面直角坐标系中的绝对位置完成AGV初始化；步骤D、组合导航定位：AGV在初始化完成后的运动过程中，视觉相机采集参照物信息，控制器将采集到的参照物信息利用平面几何关系求解此时AGV位姿数据、并将得到的AGV位姿数据与IMU或里程计数据通过卡尔曼滤波算法进行融合，完成组合导航定位。本专利技术的有益效果是：摆脱了传统方法对专用参照物(如磁条、RFID信标、二维码阵列等)的依赖，只需要已有常见地面参照物(如地板、瓷砖等)，大大降低了现场施工成本、难度与工作量；不依赖无线射频定位信号(如UWB，WIFI，Bluetooth等)，适用于舞台剧场等复杂电磁环境的应用场合；视觉相机在车体内部向下拍摄地面，不怕人员或障碍物的遮挡(如激光反射板、激光雷达等)，适用于动态非结构环境的应用场合。下面结合附图对本专利技术进行详细说明。附图说明图1是以采用木地板的地面建立的坐标系示意图；图2是以采用瓷砖的地面建立的坐标系示意图；图3是根据参照物自动生成的网格地图示意图；图4是IMU数据计算得到的相机视野与视觉相机采集到的真实视野的示意图。具体实施方式本专利技术提供了一种基于视觉与IMU或里程计的AGV组合导航定位方法，基于具有规则参照物的地面。具有规则参照物的地面是指规则地铺设于地面的参照物如木地板、瓷砖贴纸等任意可拼接材质或可构成直线网格的喷涂性、粘贴性的标记，如附图1和2所示。AGV系统上设有舞台，用于根据配置文件按照指定的轨迹移动舞台。在AGV系统的底部设有用于检测地面规则参照物的视觉相机以及相应的补光灯。视觉相机拍摄地面信息以得到AGV在地面坐标系下的位姿(位置与姿态)。在AGV系统上还设有IMU或里程计，用于采集AGV系统在移动过程中的移动数据。配套的控制器与视觉相机、IMU或里程计相连，以获取数据并处理得到的数据后控制AGV移动。本专利技术的方法包括以下步骤：步骤A、建立坐标系：以所要应用场景中的地面的任意一点为原点建立地面平面直角坐标系{W}，然后将坐标系划分为整备区和工作区两个部分。在整备区粘贴一系列具有位姿信息的二维码。上述的二维码自身信息及其对应的地面平面直角坐标系{W}下的位姿信息[xq,yq,αq]均保存在AGV中控制器的配置文件。步骤B、设置配置文件：将规则参照物的尺寸信息输入至AGV的控制器的配置文件。参见附图3，将地面参照物(如地板、瓷砖等)的长宽[lf,wf]以及长度方向(人为指定)与地面平面直角坐标系{W}的X轴方向夹角αf保存到控制器的配置文件；AGV控制器根据该配置文件自动生成网格地图，即对应于地面网格线的直线族：{M}＝{LX}∪{LY}。步骤C、AGV初始化：AGV上电后手动操控其经过二维码标识，视觉相机扫描二维码标识并通过控制器计算得到AGV在地面平面直角坐标系中的绝对位置信息完成AGV初始化。具体地，AGV在整备区内上电后，由操作员手动操控AGV经过粘贴有二维码的区域，视觉相机扫描到二维码后控制器通过配置文件自动计算AGV在地面平面直角坐标系{W}中的绝对位置信息[xa,ya,αa]，完成AGV的初始化。步骤D、组合导航定位：参见附图4，AGV在初始化完成后根据配置文件中的移动路径运动过程中，视觉相机采集参照物信息，控制器将采集到的参照物信息利用平面几何关系求解此时AGV位姿数据、并将得到的AGV位姿数据与IMU或里程计数据通过卡尔曼滤波算法进行融合，完成组合导航定位。AGV初始化完成后的一段短暂运动过程中，在视觉相机的采集不到有效地图信息的短时间隔内，由IMU或里程计负责此时AGV的导航定位。AGV初始化完成后的运动过程中，当视觉相机采集到有效地图信息(如地板或瓷砖的缝隙、网格线等)后，控制器利用平面几何关系求解此时AGV可能的位姿数据，并将其与IMU或里程计数据通过卡尔曼滤波算法进行融合，完成组合导航定位。具体地，控制器将采集到的参照物信息利用平面几何关系求解AGV位姿数据包括以下步骤：S1：读取视觉相机采集到的地面图像。S2：经过灰度化、去噪、增强等处理后检测图像中的直线，如果检测到直线存在，通过相机内参单应矩阵H求出图像中所有直线在视觉相机坐标系{C}下的方程{Li}＝{H·si,1+t·(H·si,1-H·si,2)|t∈R}；其中si,1和si,2是检测出来的第i条直线的两个端点，t为实数形参。如果没有检测到直线存在，则说明此时是AGV处于初始化完成后的短暂运动过程，由IMU或里程计负责此时AGV的导航定位。S3：由IMU或里程计数据确定网格地图中网格线的直线族{M}＝{LX}∪{LY}中位于视觉相机视野内且距离其镜头原点最近的两条直线{Lx}和{Ly}。具体求解过程如下：网格线的直线族{LX}为平行线，由以下参数确定：平行线中任意一条直线上的一点s；平行线的方向向量v；平行线之间的距离w。设定与方向向量v垂直的向量为V，则距离点s为w的整数倍且与点s连线垂直于v的点s+n·w·V一定位于平行线族{LX}中的某条直线上。平面上任意点p与平行线族{LX}的距离可表示为：d＝(p-s)·V/|p-s|，其中等号右侧为向量计算。如此，距离的平方d2是关于整数n的二次曲线即开口向上的抛物线，由抛物线只存在一个顶点的性质可得到d2的最小值以及其对应的整数n，进而得到点s本文档来自技高网...

【技术保护点】
1.一种基于视觉与IMU或里程计的AGV组合导航定位方法，基于具有规则参照物的地面，其特征在于，包括以下步骤：步骤A、建立坐标系：以地面任意一点为原点建立地面平面直角坐标系，并在地面上粘贴具有位姿信息的二维码标识；步骤B、设置配置文件：将规则参照物的尺寸信息及二维码位姿信息输入至AGV的控制器配置文件，AGV控制器根据该配置文件自动生成网格地图；步骤C、AGV初始化：AGV上电后手动操控其经过二维码标识，视觉相机扫描二维码标识并通过控制器计算得到AGV在地面平面直角坐标系中的绝对位置完成AGV初始化；步骤D、组合导航定位：AGV在初始化完成后的运动过程中，视觉相机采集参照物信息，控制器将采集到的参照物信息利用平面几何关系求解此时AGV位姿数据、并将得到的AGV位姿数据与IMU或里程计数据通过卡尔曼滤波算法进行融合，完成组合导航定位。

【技术特征摘要】
1.一种基于视觉与IMU或里程计的AGV组合导航定位方法，基于具有规则参照物的地面，其特征在于，包括以下步骤：步骤A、建立坐标系：以地面任意一点为原点建立地面平面直角坐标系，并在地面上粘贴具有位姿信息的二维码标识；步骤B、设置配置文件：将规则参照物的尺寸信息及二维码位姿信息输入至AGV的控制器配置文件，AGV控制器根据该配置文件自动生成网格地图；步骤C、AGV初始化：AGV上电后手动操控其经过二维码标识，视觉相机扫描二维码标识并通过控制器计算得到AGV在地面平面直角坐标系中的绝对位置完成AGV初始化；步骤D、组合导航定位：AGV在初始化完成后的运动过程中，视觉相机采集参照物信息，控制器将采集到的参照物信息利用平面几何关系求解此时AGV位姿数据、并将得到的AGV位姿数据与IMU或里程计数据通过卡尔曼滤波算法进行融合，完成组合导航定位。2.根据权利要求1所述的基于视觉与IMU或里程计的AGV组合导航定位方法，其特征在于，在步骤D中控制器将采集到的参照物信息利用平面几何关系求解此时AGV位姿数据包括以下步骤：S1：读取视觉相机采集到的地面图像；S2：经过灰度化、去噪、增强等处理后检测图像中的直线，如果检测到直线存在，则通过相机内参单应矩阵H求出图像中所有直线在视觉相机坐标系{C}下的方程{Li}；S3：由IMU或里程计数据确定网格地图中网格线的直线族{M}＝{LX}∪{LY}中位于视觉相机视野内且距离其镜头原点最近的两条直线{Lx}和{Ly}；S4：求解所有直线方程{Li}在视觉相机镜头坐标系{C}下的斜率θi与截距di以及直线{Lx}和{Ly}在视觉相机镜头坐标系{C}下的斜率θx、θy与截距dx、dy；S5：按照设定的阈值θt和dt将所有直线分成两组并排除错误直线，得到两组分别对应于{Lx}和{Ly}的检测直线{Lxi|i＝1,2,…,m}和{Lyi|i＝1,2,…,n}，其中，Li∈{Lxi}，if|θi-θx|...

【专利技术属性】
技术研发人员：杜惠斌，刘静怡，李中胜，张荣山，李财盛，李江楠，
申请(专利权)人：石家庄辰宙智能装备有限公司，
类型：发明
国别省市：河北,13