一种基于UWB和视觉SLAM的AGV室内定位及导航的方法技术

本发明专利技术公开了一种基于UWB和视觉SLAM的AGV室内定位及导航的方法，包括在室内地图中选择若干个点布置UWB定位基站；通过视觉Slam扫描建立室内定位地图，且室内定位地图标有UWB定位基站标记；AGV搭载两个UWB定位标签MS1和MS2，MS1和MS2的直线距离为L；处理器根据UWB定位标签在t时刻的位置矢量，计算求得AGV在t时刻的航偏角yaw，建立位置测量系统运动方程与观测方程，并通过卡尔曼滤波算法求出AGV的最优位置估计；本发明专利技术提供了一种基于UWB和视觉SLAM的AGV室内定位及导航的方法，通过采用UWB定位和视觉Slam扫描建立室内定位地图，可以通过计算公式和算法准确的计算出AGV的最优位置估计，可以避免SLAM中的累积误差以及陀螺仪本身的累积误差，造成的实用化比较苛刻的情况。

【技术实现步骤摘要】
一种基于UWB和视觉SLAM的AGV室内定位及导航的方法
本专利技术涉及AGV室内导航和定位
，尤其涉及一种基于UWB和视觉SLAM的AGV室内定位及导航的方法。
技术介绍
随着科技的发展，AGV小车(AutomatedGuidedVehicle，即：自动导引运输车)正在越来越多被使用在物流搬运、分拣和智能仓库等方面，代替人工运输货物，提高了自动化程度。目前，AGV导航方法主要分为以下几种：(1)电磁导航，即在机器人工作区域铺设具有一定宽度的黑色磁条，机器人根据磁传感器读取磁条信号进行控制，从而实现机器人的导航。但是，由于铺设磁条等主干道一般都是单向行驶，该种导航方式效率低下，并且无法完成机器人多个任务同时调度和执行功能，因此，需要对机器人进行交通管制等额外操作。(2)Tag标签导航，在物流行业应用广泛，其用法是在机器人工作区域按一定间隔铺设二维Tag标签，由调度系统来实时调度和任务分派，该方法效率较高。但是，该种方式只能在铺设Tag标签的区域实现导航，在未布置Tag区域无法导航，且不适合环境复杂而且多变的情景。(3)SLAM(simultaneouslocalizationandmapping，即：即时定位与地图构建)导航，主要侧重探索未知区域，并且能够实时避障和导航，通过激光传感器或视觉传感器建立一套跟实际环境相符合的地图信息，并保存成地图数据，机器人在移动过程中需依赖存储后的地图信息，并依据实时位置所扫描的实际环境信息进行计算，最终规划出一种可以实时避障和导航的路线，并控制机器人按照计算好的路线驱动机器人行走，最终实现自动导航、实时定位以及实时避障的功能。SLAM相较其他两种方式具有很大优势的，但由于SLAM是相对定位并有累积误差难以消除的严重缺陷，即使有陀螺仪来辅助减少累积误差的方法，也因陀螺仪本身也会产生累积误差，造成实用化会比较苛刻的情况。
技术实现思路
本专利技术的目的是为了解决现有技术中的问题，而提出的一种基于UWB和视觉SLAM的AGV室内定位及导航的方法。为了实现上述目的，本专利技术采用了如下技术方案：一种基于UWB和视觉SLAM的AGV室内定位及导航的方法，包括如下步骤：步骤1：在室内地图中选择若干个点布置UWB定位基站；定义其中一个UWB定位基站为原点定位标记，定义原点定位标记的坐标轴为全局坐标轴，其余UWB定位基站标记在全局坐标轴上均具备相应的定位数据；步骤2：通过视觉Slam扫描建立室内定位地图，且室内定位地图标有UWB定位基站标记；步骤3：AGV搭载两个UWB定位标签MS1和MS2，MS1和MS2的直线距离为L；移动过程中，UWB定位标签发送由超宽频脉冲组成的脉冲数据包给UWB定位基站，UWB定位基站通过交换机并将该信号传给处理器，处理器按照TDOA方式进行解算处理，计算出带有噪声的移动定位标签的位置信息。步骤4：处理器根据UWB定位标签在t时刻的位置矢量，计算求得AGV在t时刻的航偏角yaw，建立位置测量系统运动方程与观测方程，并通过卡尔曼滤波算法求出AGV的最优位置估计。步骤5：AGV自身携带的双目摄像头在运动过程中，实时采集周围的环境特征点并构建环境地图，双目摄像机采集左右两张图片后，提取特征点作为环境特征并进行左右图片的立体匹配，通过三角测距法获得特征点相对于的位置坐标作为路标点的观测量。步骤6：将步骤3计算获得的位移及角度变化量作为系统的运动状态输入，之后将运动状态输入和观测量分别带入运动模型和观测模型，将观测模型的路标点与室内图系统特征地图库进行数据关联，通过状态估计算法再与运动状态量进行融合，最终完成AGV的同时定位与地图构建。优选的，在步骤1中每隔30-100米设置一个UWB定位基站，每个UWB定位基站通过菊花链首尾相连或星型连接的方式与交换机连接。优选的，在步骤2中通过视觉Slam扫描建立室内定位地图，每个定位标记的图像不同。优选的，步骤3的具体算法为：首先从所有接收到脉冲数据包的UWB定位基站中任选三个组成一组，以yK每个UWB定位基站坐标为圆心，以UWB定位基站到UWB标签的距离为半径画圆；然后根据交点组成的三角形，求其质心坐标(xk，yk)，k＝1,2,…i，l为组合得到的质心总数；接着根据距离越大定位误差越大的原则，对每个组合的质心坐标赋以权值，其中权值由UWB定位标签到对应质心距离的倒数确定；最后对每个组合的质心加权得到最终的UWB标签定位结果，具体计算公式如下：式中，(xi,yi)为质心坐标,li为与UWB标签到对应质心的距离。优选的，在步骤4中运动模型是进行AGV位置估计、运行轨迹计算和预测AGV航迹状态的基础；移动AGV的观测模型描述了AGV所观测到的环境信息与自身位置之间的关系；A、B为安装在AGV上的两个UWB移动定位节点，在欧式坐标系中，令mt，nt为A，B在时刻t的位置矢量，则mt＝(xa,ya)nt＝(xa,ya)则t时刻,AGV的偏航角yaw满足但由于外部环境的干扰，加上系统本身存在的误差，从UWB定位系统中读取的位置数据不可避免的会存在噪声污染，因此偏航角的计算公式相应需要加入噪声数据，但这样计算出来的偏航角由于混杂了噪声将失去实际意义，为此需要通过一定的方式滤除这些噪声；本系统通过卡尔曼滤波算法滤除噪声，建立位置测量系统运动方程与观测方程，并算法求出AGV的最优位置估计。优选的，在步骤5中双目立体视觉测距原理是基于双目视差原理，左右摄像机投影中心间的距离称为基线长度B；当两摄像机在同一时刻观看三维空间点P时，P在左摄像机和右摄像机采集图像上分别投影pleft(xleft,yleft)和pright(xright,yright)。假设两摄像机水平位置相同，P点在两平面内投影点y坐标相等，则由三角几何关系可得：设视差Disparity＝xleft-xright。因此，计算出P点在摄像机坐标系下的三维坐标是：图像上的点只要存在相应的匹配点，就可以进行三角测距计算，从而获取相机坐标系下三维坐标。本专利技术提供了一种基于UWB和视觉SLAM的AGV室内定位及导航的方法，通过采用UWB定位和视觉Slam扫描建立室内定位地图，可以通过计算公式和算法准确的计算出AGV的最优位置估计，可以避免SLAM中的累积误差以及陀螺仪本身的累积误差，造成的实用化比较苛刻的情况，使用方便，实用化便捷轻松。该装置中未涉及部分均与现有技术相同或可采用现有技术加以实现，本专利技术结构简单，操作方便。附图说明图1为本专利技术提出的一种基于UWB和视觉SLAM的AGV室内定位及导航的方法的基于UWB位姿测量系统体系结构图；图2为本专利技术提出的一种基于UWB和视觉SLAM的AGV室内定位及导航的方法的基于双目视觉和uwb的slam结构图；图3为本专利技术提出的一种基于UWB和视觉SLAM的AGV室内定位及导航的方法的双目测距流程图。具体实施方式下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本专利技术一部分实施例，而不是全部的实施例。在本专利技术的描述中，需要理解的是，术语“上”、“下”、“前”、“后”、“左”、“右”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本专利技术和简化本文档来自技高网...

【技术保护点】
1.一种基于UWB和视觉SLAM的AGV室内定位及导航的方法，其特征在于，包括如下步骤：步骤1：在室内地图中选择若干个点布置UWB定位基站；定义其中一个UWB定位基站为原点定位标记，定义原点定位标记的坐标轴为全局坐标轴，其余UWB定位基站标记在全局坐标轴上均具备相应的定位数据；步骤2：通过视觉Slam扫描建立室内定位地图，且室内定位地图标有UWB定位基站标记；步骤3：AGV搭载两个UWB定位标签MS1和MS2，MS1和MS2的直线距离为L；移动过程中，UWB定位标签发送由超宽频脉冲组成的脉冲数据包给UWB定位基站，UWB定位基站通过交换机并将该信号传给处理器，处理器按照TDOA方式进行解算处理，计算出带有噪声的移动定位标签的位置信息。步骤4：处理器根据UWB定位标签在t时刻的位置矢量，计算求得AGV在t时刻的航偏角yaw，建立位置测量系统运动方程与观测方程，并通过卡尔曼滤波算法求出AGV的最优位置估计。步骤5：AGV自身携带的双目摄像头在运动过程中，实时采集周围的环境特征点并构建环境地图，双目摄像机采集左右两张图片后，提取特征点作为环境特征并进行左右图片的立体匹配，通过三角测距法获得特征点相对于的位置坐标作为路标点的观测量。步骤6：将步骤3计算获得的位移及角度变化量作为系统的运动状态输入，之后将运动状态输入和观测量分别带入运动模型和观测模型，将观测模型的路标点与室内图系统特征地图库进行数据关联，通过状态估计算法再与运动状态量进行融合，最终完成AGV的同时定位与地图构建。...

【技术特征摘要】
1.一种基于UWB和视觉SLAM的AGV室内定位及导航的方法，其特征在于，包括如下步骤：步骤1：在室内地图中选择若干个点布置UWB定位基站；定义其中一个UWB定位基站为原点定位标记，定义原点定位标记的坐标轴为全局坐标轴，其余UWB定位基站标记在全局坐标轴上均具备相应的定位数据；步骤2：通过视觉Slam扫描建立室内定位地图，且室内定位地图标有UWB定位基站标记；步骤3：AGV搭载两个UWB定位标签MS1和MS2，MS1和MS2的直线距离为L；移动过程中，UWB定位标签发送由超宽频脉冲组成的脉冲数据包给UWB定位基站，UWB定位基站通过交换机并将该信号传给处理器，处理器按照TDOA方式进行解算处理，计算出带有噪声的移动定位标签的位置信息。步骤4：处理器根据UWB定位标签在t时刻的位置矢量，计算求得AGV在t时刻的航偏角yaw，建立位置测量系统运动方程与观测方程，并通过卡尔曼滤波算法求出AGV的最优位置估计。步骤5：AGV自身携带的双目摄像头在运动过程中，实时采集周围的环境特征点并构建环境地图，双目摄像机采集左右两张图片后，提取特征点作为环境特征并进行左右图片的立体匹配，通过三角测距法获得特征点相对于的位置坐标作为路标点的观测量。步骤6：将步骤3计算获得的位移及角度变化量作为系统的运动状态输入，之后将运动状态输入和观测量分别带入运动模型和观测模型，将观测模型的路标点与室内图系统特征地图库进行数据关联，通过状态估计算法再与运动状态量进行融合，最终完成AGV的同时定位与地图构建。2.根据权利要求1所述的一种基于UWB和视觉SLAM的AGV室内定位及导航的方法，其特征在于，在步骤1中每隔30-100米设置一个UWB定位基站，每个UWB定位基站通过菊花链首尾相连或星型连接的方式与交换机连接。3.根据权利要求1所述的一种基于UWB和视觉SLAM的AGV室内定位及导航的方法，其特征在于，在步骤2中通过视觉Slam扫描建立室内定位地图，每个定位标记的图像不同。4.根据权利要求1所述的一种基于UWB和视觉SLAM的AGV室内定位及导航的方法，其特征在于，步骤3的具体算法为：首先从所有接收到脉冲数据包的UWB定位基站中...

【专利技术属性】
技术研发人员：江俊，
申请(专利权)人：浙江智澜科技有限公司，
类型：发明
国别省市：浙江,33