一种基于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

如图1-3所示，一种基于uwb和sla的avg室内定位及导航的方法的具体操作步骤如下：

首先，布置uwb定位基站的位置；首先配置交换机网络参数，按照每隔30-100米左右的原则在天花板上布置uwb定位基站，将每个uwb定位基站通过菊花链首尾相连或星型连接的方式用屏蔽网线与交换机连接，一方面可实现定位区域信号全覆盖，另一方面可减少uwb定位基站的部署数量；室内环境的改变势必会影响信号衰减程度，故uwb定位基站放置间距需要根据实际情况有所调整。当室内面积较小时，uwb定位基站相互间距设置在30米左右，该距离不宜过小，否则易造成干扰，影响定位精度；当室内面积较大时，则可以将接收机间距放大，最大可达到100左右。

接着通过视觉slam扫描建立室内定位地图；室内定位地图标有uwb定位基站标记。

uwb定位基站的位置固定且已知，两个定位标签ms1，ms2安装于agv小车的底盘上，且两个标签间的距离固定，称为基线距离1。如图2所示，三台定位基站用于接收定位点发出的uwb信号，并将该信号传给处理器，处理器按照tdoa方式进行解算处理，最后输出是带有噪声的移动标签的位置信息。

计算得到定位标签ms1和ms2的位置坐标，在欧式坐标系中，令mt，nt为ms1，ms2在时刻t的位置矢量，则mt＝(xa,ya)，nt＝(xa,ya)，则t时刻agv机器人的偏航角yaw满足但由于外部环境的干扰，加上系统本身存在的误差，从uwb定位系统中读取的位置数据不可避免的会存在噪声污染，令wt，vt为系统噪声，且满足零均值的高斯分布，则因此偏航角的计算公式mt＝(xa,ya)+wt，nt＝(xb,yb)+vt相应需要加入噪声数据，但这样计算出来的偏航角由于混杂了噪声将失去实际意义，为此需要通过一定的方式滤除这些噪声。本系统通过卡尔曼滤波算法滤除噪声，建立位姿测量系统运动方程与观测方程，并算法求出agv机器人的最优位姿估计。

agv小车自身携带的双目摄像头在小车运动过程中，实时采集周围的环境特征点并构建环境地图；双目立体视觉测距传感器，基线长度为14厘米，两摄像头的usb线接入上位机，同时采集图像，设定采集图片分辨率为640*480,记录采集时间并存储图像。bouguet算法对原双目摄像机左右图像进行立体匹配，使左右图像光轴平行，得到理想的平行配置双目立体视觉系统。对极线校正后的图像进行剪切，去除左右两摄像机不重合的视角区域，选择新的图像中心及边界，使得左右图像同时观察到的面积最大化，提高之后的特征点匹配及深度计算的计算速度和精度。采用sift方法进行特征点提取，提取完左右两幅图像的特征点及其128维描述符后，即可完成两幅图像的特征点匹配工作。采集的图像在上位机中进行立体匹配，路标点检测和匹配工作，最终根据三角测距原理，获取观测到的路标点在摄像机坐标系下的位置信息。

最后，将计算获得的小车位移及角度变化量作为系统的运动状态输入，将运动状态输入和观测量分别带入运动模型和观测模型，将观测模型的路标点与室内图系统特征地图库进行数据关联，通过状态估计算法再与运动状态量进行融合，最终完成机器人的同时定位与地图构建。

需要说明的是，本发明公开的一种基于uwb和视觉slam的agv室内定位及导航的方法，通过采用uwb定位和视觉slam扫描建立室内定位地图，可以通过计算公式和算法准确的计算出agv的最优位置估计，可以避免slam中的累积误差以及陀螺仪本身的累积误差，造成的实用化比较苛刻的情况，使用方便，实用化便捷轻松。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。