基于UWB和激光雷达混合定位的行人跟随系统及方法技术方案

本发明专利技术提出了一种基于UWB和激光雷达混合定位的行人跟随系统方法，采用UWB对行人进行初步的定位与识别，然后利用激光雷达对行人进行精确的定位与识别，有效避障，满足复杂环境下行人的精确跟随以及定位；采用动态窗口法来进行避障，精准度更高；采用Kalman滤波对机器人的运动进行滤波处理，稳定性更强。

Pedestrian following system and method based on mixed positioning of UWB and laser radar

The present invention provides a method for pedestrian tracking system and laser radar UWB hybrid positioning based on UWB is used to locate and identify of pedestrians, and then use the laser radar positioning and precise identification of pedestrians, effective obstacle avoidance, meet the complex environment of the precise positioning and follow the pedestrian to the obstacle avoidance; dynamic window method, higher precision; the movement of the robot Kalman filter is used to filter more stability.

【技术实现步骤摘要】
基于UWB和激光雷达混合定位的行人跟随系统及方法
本专利技术涉及机器人跟随系统，尤其涉及一种基于UWB和激光雷达混合定位的行人跟随系统及方法。
技术介绍
机器人跟随技术是指机器人保持一定的距离和速度跟随行人，并辅助行人完成相关的社会生产活动。跟随机器人无论在服务机器人、仓储快消行业等都有广阔的应用前景。在仓储分拣系统中，跟随机器人跟随行人到达货架拣货区分拣订单货物，这不仅大大降低了人的劳动强度，而且还极大低提高了整个分拣任务的效率；在服务机器人领域，跟随技术可以帮助类似于迎宾机器人进行个性化服务。目前跟随技术主要采用超声波定位跟随、蓝牙定位跟随、超宽带射频信号(UWB)定位跟随等。超声波定位由于超声波存在衍射现象，导致定位不稳定，不能进行更精确的定位跟随任务；蓝牙定位由于依靠蓝牙信号进行三角定位，该方法定位误差较大达到米级，也无法进行精确的定位跟随任务；而超宽带射频信号(UWB)定位目前能达到10cm级，且成本适中目前已经成为机器人跟随领域的应用及研究热点。中国专利CN201610259595.1就提供了一种基于UWB的自主跟随机器人定位方法及系统，使用UWB无线定位技术实现自主跟随机器人定位，采用窄带脉冲进行通信，不需要载波，传输功率高，功耗小，抗干扰能力和穿透能力强，提高了定位的精度和准确性，从而实现特定目标的跟随。
技术实现思路
有鉴于此，本专利技术提出了一种基于UWB和激光雷达混合定位的行人跟随系统及方法，满足复杂环境下行人的精确跟随以及定位，采用UWB对行人进行初步的定位与识别，然后利用激光雷达对行人进行精确的定位与识别，最终使机器人能够无碰撞地精确地跟随行人，精度更高，稳定性更强。本专利技术的技术方案是这样实现的：本专利技术提供了一种基于UWB和激光雷达混合定位的行人跟随系统，其包括至少三个UWB基站、UWB标签、数据处理装置和机器人运动控制装置，还包括激光雷达，其中，UWB标签置于跟随目标上，接收从UWB基站发来的UWB信号；UWB基站发送UWB信号，机器人上固定至少三个位置成三角形的UWB基站；激光雷达，测量跟随目标与机器人之间的距离及角度，测量机器人周围环境信息；数据处理装置，通过UWB基站和UWB标签计算跟随目标与机器人之间的UWB距离及UWB角度，并根据激光雷达测量的跟随目标与机器人之间的距离及角度，对UWB距离及UWB角度进行校正，并生成一条参考路径；根据激光雷达测量的机器人周围环境信息，在参考路径的基础上生成一条无碰撞的最终路径；针对最终路径生成速度控制指令，并对生成的速度控制指令进行滤波处理，得到平滑的速度控制指令；机器人运动控制装置，将数据处理装置处理的速度控制指令转化为运动执行机构的执行指令。第二方面，本专利技术提供了一种基于UWB和激光雷达混合定位的行人跟随方法，包括以下步骤，S1，通过UWB基站向UWB标签发送UWB信号，计算跟随目标与机器人之间的UWB距离及UWB角度；S2，通过激光雷达测量跟随目标与机器人之间的距离及角度，对UWB距离及UWB角度进行校正，假设机器人周围无障碍物，生成一条最近的参考路径；S3，通过激光雷达测量机器人周围环境信息，创建局部地图，然后根据校正后的机器人相对于跟随目标的距离及角度，在参考路径的基础上生成一条无碰撞的最终路径；S4，针对最终路径生成速度控制指令，并对生成的速度控制指令进行滤波处理，得到平滑的速度控制指令；S5，机器人运动控制装置将平滑的速度控制指令转化为运动执行机构的执行指令。在以上技术方案的基础上，优选的，所述步骤S1中，根据三角测量原理测量机器人相对于UWB标签的UWB距离S1及UWB角度θ1，得到UWB标签相对于机器人的位置(S1,θ1)。进一步优选的，所述步骤S2中，通过激光雷达采集一帧激光雷达数据然后根据UWB角度θ1确定跟随目标在激光雷达测量范围内的大小(θ1-Δ/2,θ1+Δ/2)，其中，Δ为跟随目标的宽度对应的激光雷达的测量范围；然后，在(θ1-Δ/2,θ1+Δ/2)范围内搜索跟随目标相对于机器人的位置，加入测量到的n个激光雷达的点，在这n个点内首先去除在范围(S1-Δs,S1+Δs)内的干扰点，其中，其中S1为UWB距离，Δs为2倍人体腿部的最大宽度，得n1个点，然后求跟随目标到激光雷达的平均距离并将作为机器人与跟随目标校正后的距离，进一步计算机器人与跟随目标校正后的角度其中Δθ为激光雷达的分辨率，则最终跟随目标在机器人坐标下的位置为更进一步优选的，所述步骤S2中，参考路径为过机器人坐标系的原点指向点的直线，其控制线速度和角速度为其中K1为距离与线速度的比例关系，K2为机器人角速度与机器人正前方和跟随目标测量宽度的法线角度比例关系。在以上技术方案的基础上，优选的，所述步骤S3中，采用动态窗口法来进行机器人行进路径上的避障，从而得到最终路径，动态窗口法包括以下步骤，S3-1，将激光雷达搜索空间约束为机器人可控的控制指令空间；S3-2，使用目标函数对搜索空间内的命令进行评价，选择使目标函数最大化的指令作为最优指令。进一步优选的，所述步骤S3-1中，采用以下公式确定机器人速度范围，θt＝θt+ωΔtVm＝{v∈[vmin,vmax],ω∈[ωmin,ωmax]}其中，r为机器人做圆弧运动的半径；v为机器人线速度；ω为机器人旋转速度；机器人坐标为(x,y)，θt为机器人在t时刻的航向角，t为当前时刻；Vm为机器人起始速度空间，Vd为机器人可达速度空间，vc,ωc是机器人的当前速度，为机器人的最大减速度，为机器人的最大加速度，为机器人的最大角加速度，为机器人的最大角减速度，Va为机器人不撞击障碍物的可行速度空间，dist(v,ω)为速度(v,ω)对应轨迹距离障碍物最近的距离。在以上技术方案的基础上，优选的，所述步骤S3-2中，采用以下公式对每条轨迹进行评价，其中，H(v,ω)用来评价机器人在当前设定的采样速度下，达到模拟轨迹末端时的朝向和目标之间的角度差；G(v，ω)是轨迹评价函数，偏差越小，评价值越高，距离越大，评价值越高，速度越大，评价值越高；D(v,ω)代表机器人在当前轨迹上与最近障碍物之间的距离；V(v,ω)用来评价当前轨迹的速度大小，α,β,γ是加权系数；n为采样的所有轨迹，i为待评价的当前轨迹；h(i)为机器人在第i条轨迹末端的朝向与目标之间的角度差距，d(i)为机器人在第i条轨迹上与最近障碍物之间的距离，v(i)为机器人在第i条轨迹上的速度。在以上技术方案的基础上，优选的，所述步骤S4中，采用Kalman滤波对生成的速度控制指令进行滤波处理，预先建立的信号与噪声的状态空间模型，执行过程中，根据现时刻的观测值和前一时刻地估计值来更新对状态变量的估计，达到线性最优效果。本专利技术的基于UWB和激光雷达混合定位的行人跟随系统方法相对于现有技术具有以下有益效果：(1)采用UWB对行人进行初步的定位与识别，然后利用激光雷达对行人进行精确的定位与识别，有效避障，满足复杂环境下行人的精确跟随以及定位；(2)采用动态窗口法来进行避障，精准度更高；(3)采用Kalman滤波对机器人的运动进行滤波处理，稳定性更强。附图说明为了更清楚地说明本专利技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显本文档来自技高网...

【技术保护点】
一种基于UWB和激光雷达混合定位的行人跟随系统，其包括至少三个UWB基站(2)、UWB标签(3)、数据处理装置(4)和机器人运动控制装置(5)，其特征在于：还包括激光雷达(1)，其中，UWB标签(3)置于跟随目标上，接收从UWB基站(2)发来的UWB信号；UWB基站(2)发送UWB信号，机器人上固定至少三个位置成三角形的UWB基站(2)；激光雷达(1)，测量跟随目标与机器人之间的距离及角度，测量机器人周围环境信息；数据处理装置(4)，通过UWB基站(2)和UWB标签(3)计算跟随目标与机器人之间的UWB距离及UWB角度，并根据激光雷达(1)测量的跟随目标与机器人之间的距离及角度，对UWB距离及UWB角度进行校正，并生成一条参考路径；根据激光雷达(1)测量的机器人周围环境信息，在参考路径的基础上生成一条无碰撞的最终路径；针对最终路径生成速度控制指令，并对生成的速度控制指令进行滤波处理，得到平滑的速度控制指令；机器人运动控制装置(5)，将数据处理装置(4)处理的速度控制指令转化为运动执行机构的执行指令。

【技术特征摘要】
1.一种基于UWB和激光雷达混合定位的行人跟随系统，其包括至少三个UWB基站(2)、UWB标签(3)、数据处理装置(4)和机器人运动控制装置(5)，其特征在于：还包括激光雷达(1)，其中，UWB标签(3)置于跟随目标上，接收从UWB基站(2)发来的UWB信号；UWB基站(2)发送UWB信号，机器人上固定至少三个位置成三角形的UWB基站(2)；激光雷达(1)，测量跟随目标与机器人之间的距离及角度，测量机器人周围环境信息；数据处理装置(4)，通过UWB基站(2)和UWB标签(3)计算跟随目标与机器人之间的UWB距离及UWB角度，并根据激光雷达(1)测量的跟随目标与机器人之间的距离及角度，对UWB距离及UWB角度进行校正，并生成一条参考路径；根据激光雷达(1)测量的机器人周围环境信息，在参考路径的基础上生成一条无碰撞的最终路径；针对最终路径生成速度控制指令，并对生成的速度控制指令进行滤波处理，得到平滑的速度控制指令；机器人运动控制装置(5)，将数据处理装置(4)处理的速度控制指令转化为运动执行机构的执行指令。2.一种基于UWB和激光雷达混合定位的行人跟随方法，其特征在于：包括以下步骤，S1，通过UWB基站(2)向UWB标签(3)发送UWB信号，计算跟随目标与机器人之间的UWB距离及UWB角度；S2，通过激光雷达(1)测量跟随目标与机器人之间的距离及角度，对UWB距离及UWB角度进行校正，假设机器人周围无障碍物，生成一条最近的参考路径；S3，通过激光雷达(1)测量机器人周围环境信息，创建局部地图，然后根据校正后的机器人相对于跟随目标的距离及角度，在参考路径的基础上生成一条无碰撞的最终路径；S4，针对最终路径生成速度控制指令，并对生成的速度控制指令进行滤波处理，得到平滑的速度控制指令；S5，机器人运动控制装置(5)将平滑的速度控制指令转化为运动执行机构的执行指令。3.如权利要求2所述的基于UWB和激光雷达混合定位的行人跟随方法，其特征在于：所述步骤S1中，根据三角测量原理测量机器人相对于UWB标签(3)的UWB距离S1及UWB角度θ1，得到UWB标签(3)相对于机器人的位置(S1,θ1)。4.如权利要求3所述的基于UWB和激光雷达混合定位的行人跟随方法，其特征在于：所述步骤S2中，通过激光雷达(1)采集一帧激光雷达数据然后根据UWB角度θ1确定跟随目标在激光雷达(1)测量范围内的大小(θ1-Δ/2,θ1+Δ/2)，其中，Δ为跟随目标的宽度对应的激光雷达(1)的测量范围；然后，在(θ1-Δ/2,θ1+Δ/2)范围内搜索跟随目标相对于机器人的位置，加入测量到的n个激光雷达(1)的点，在这n个点内首先去除在范围(S1-Δs,S1+Δs)内的干扰点，其中，其中S1为UWB距离，Δs为2倍人体腿部最大宽度，得n1个点，然后求跟随目标到激光雷达(1)的平均距离并将作为机器人与跟随目标校正后的距离，进一步计算机器人与跟随目标校正后的角度其中Δθ为激光雷达(1)的分辨率，则最终跟随目标在机器人坐标下的位置为5.如权利要求4所述的基于UWB和激光雷达混合定位的行人跟随方法，其特征在于：所述步骤S2中，参考路径为过机器人坐标系的原点指向点的直线，其控制线速度和角速度为其中K1为距...

【专利技术属性】
技术研发人员：辛亚运，苟利军，
申请(专利权)人：武汉木神机器人有限责任公司，
类型：发明
国别省市：湖北,42