本发明涉及机器人跟随系统，尤其涉及一种基于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中，参考路径为过机器人坐标系的原点指向
在以上技术方案的基础上，优选的，所述步骤s3中，采用动态窗口法来进行机器人行进路径上的避障，从而得到最终路径，动态窗口法包括以下步骤，
s3-1，将激光雷达搜索空间约束为机器人可控的控制指令空间；
s3-2，使用目标函数对搜索空间内的命令进行评价，选择使目标函数最大化的指令作为最优指令。
进一步优选的，所述步骤s3-1中，采用以下公式确定机器人速度范围，
θt＝θt+ωδt
vm＝{v∈[vmin,vmax],ω∈[ωmin,ωmax]}
其中，r为机器人做圆弧运动的半径；v为机器人线速度；ω为机器人旋转速度；
机器人坐标为(x,y)，θt为机器人在t时刻的航向角，t为当前时刻；
vm为机器人起始速度空间，vd为机器人可达速度空间，vc,ωc是机器人的当前速度，
在以上技术方案的基础上，优选的，所述步骤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滤波对机器人的运动进行滤波处理，稳定性更强。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明基于uwb和激光雷达混合定位的行人跟随系统的框图。
具体实施方式
下面将结合本发明实施方式中的附图，对本发明实施方式中的技术方案进行清楚、完整地描述，显然，所描述的实施方式仅仅是本发明一部分实施方式，而不是全部的实施方式。基于本发明中的实施方式，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式，都属于本发明保护的范围。
如图1所示，本发明的uwb和激光雷达混合定位的行人跟随系统，其包括激光雷达1、至少三个uwb基站2、uwb标签3、数据处理装置4和机器人运动控制装置5。
uwb标签3置于跟随目标上，接收从uwb基站2发来的uwb信号；
uwb基站2发送uwb信号，机器人上固定至少三个位置成三角形的uwb基站2；
激光雷达1，测量跟随目标与机器人之间的距离及角度，测量机器人周围环境信息；
数据处理装置4，通过uwb基站2和uwb标签3计算跟随目标与机器人之间的uwb距离及uwb角度，并根据激光雷达1测量的跟随目标与机器人之间的距离及角度，对uwb距离及uwb角度进行校正，并生成一条参考路径；根据激光雷达1测量的机器人周围环境信息，在参考路径的基础上生成一条无碰撞的最终路径；针对最终路径生成速度控制指令，并对生成的速度控制指令进行滤波处理，得到平滑的速度控制指令；
机器人运动控制装置5，将数据处理装置4处理的速度控制指令转化为运动执行机构的执行指令。
本发明的基于uwb和激光雷达混合定位的行人跟随方法，包括以下步骤，
s1，通过uwb基站2向uwb标签3发送uwb信号，计算跟随目标与机器人之间的uwb距离及uwb角度。
具体的，根据三角测量原理测量机器人相对于uwb标签3的uwb距离s1及uwb角度θ1，得到uwb标签3相对于机器人的位置(s1,θ1)。
三角测量原理：双向飞行时间法，uwb基站2在其时间戳上的ta1发射请求性质的脉冲信号，被uwb标签3在ta2时刻接收，uwb标签3在tb1时刻发射一个响应性质的信号，被uwb基站2在自己的时间戳tb2时刻接收。依次可以计算出脉冲信号在uwb基站2和uwb标签3之间的飞行时间，从而确定飞行距离s。s＝c×[(ta2-ta1)-(tb2-tb1)](c为光速)，然后通过在机器人上多个不同位置布置uwb基站2就可以通过三角测量原理测量出uwb标签3相对于机器人的位置(s1,θ1)。
s2，通过激光雷达1测量跟随目标与机器人之间的距离及角度，对uwb距离及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的平均距离
参考路径为过机器人坐标系的原点指向
s3，通过激光雷达1测量机器人周围环境信息，创建局部地图，然后根据校正后的机器人相对于跟随目标的距离及角度，在参考路径的基础上生成一条无碰撞的最终路径。
机器人在跟踪目标的，可能会出现与目标不相关的的障碍物，会影响机器人跟踪目标，本发明采用动态窗口法(dwa)来进行避障，动态窗口法是直接在控制指令空间内搜索使目标函数取最大值的最佳控制指令的自主避障算法。该方法可概括为两步：一是将激光雷达1搜索空间约束为机器人可控的控制指令空间；二是使用目标函数对搜索空间内的命令进行评价，选择使目标函数最大化的指令作为最优指令。具体原理如下：
假设机器人不是全向运动机器人，只能进行前进何旋转，一对(v,ω)就代表一个圆弧轨迹，它做圆弧运动的半径为，
其中，r为机器人做圆弧运动的半径；v为机器人线速度；ω为机器人旋转速度。
当旋转速度ω不等于0时，机器人坐标为，
θtt＝θt+ωδt
其中，机器人坐标为(x,y)，θt为机器人在t时刻的航向角，t为当前时刻；
根据速度就可以推算出轨迹，然后评价这些轨迹。
移动机器人受自身的运动学约束，即最大速度和最小速度的的限制：
vm＝{v∈[vmin,vmax],ω∈[ωmin,ωmax]}
其中，vm为机器人起始速度空间。
由于机器人的电机力矩有限，存在最大的加减速限制，因此移动机器人在一个计算周期内，存在一个动态窗口，在该窗口内的速度是机器人能够实际达到的速度：
vd为机器人可达速度空间，vc,ωc是机器人的当前速度，
还考虑机器人能在碰到障碍物之前能停下来，因此在最大减速度条件下，速度有一个范围：
其中，va为机器人不撞击障碍物的可行速度空间，dist(v,ω)为速度(v,ω)对应轨迹距离障碍物最近的距离。
在采样的速度组中，有若干组轨迹是可行的，因此采用评价函数的方式为每条轨迹进行评价。评价函数为：
h(v,ω)是用来评价机器人在当前设定的采样速度下，达到模拟轨迹末端时的朝向和目标之间的角度差；
g(v，ω)是轨迹评价函数，偏差越小，评价值越高，距离越大，评价值越高，速度越大，评价值越高；
d(v,ω)代表机器人在当前轨迹上与最近障碍物之间的距离，如果在这条轨迹上没有障碍物，那就将其设定为一个常数；
v(v,ω)用来评价当前轨迹的速度大小，α,β,γ是加权系数；
为了避免出现某一项在评价函数中太占优势，对评价函数的每一项进行归一化处理：
其中，n为采样的所有轨迹，i为待评价的当前轨迹；
h(i)为机器人在第i条轨迹末端的朝向与目标之间的角度差距，d(i)为机器人在第i条轨迹上与最近障碍物之间的距离，v(i)为机器人在第i条轨迹上的速度。
每一个计算周期内，通过激光雷达1获取机器人周围的障碍物信息和目标信息，得到可行的轨迹，并通过计算每条轨迹对应的评价函数的值，使评价函数取得最大值相应的轨迹所对应的(v,ω)下一周期速度和角速度的给定值。
s4，针对最终路径生成速度控制指令，并对生成的速度控制指令进行滤波处理，得到平滑的速度控制指令。
机器人在跟踪目标人运动时候，速度会受噪声影响出现抖动现象，同时还有测量误差，这里使用kalman滤波对机器人运动进行滤波。kalman滤波采用的是递归算法，预先建立的信号与噪声的状态空间模型，执行过程中，根据现时刻的观测值和前一时刻地估计值来更新对状态变量的估计，达到线性最优效果。该算法简洁明了，并且只用到了前一时刻的数据，所以卡尔曼滤波适合与计算机实时运算。滤波原理如下：
系统的运动状态方程
x(k)＝ax(k-1)+wk
其中，wk是过程噪声量，此种噪声为高斯白噪声，符合wk～(0,q(k))。
x(k)＝[lk,vl.k,θk,vθ.k]t状态变量分别为跟随目标k时刻的长度、线速度、角度、角速度。
状态转移矩阵：
系统的观测方程为：
z(k)＝hx(k)+υk
其中，υk是观测的噪声向量，此种噪声为高斯白噪声符合υk～(0,r(k))。
观测矩阵:
假如我们已知系统k-1时刻的系统的状态x(k-1|k-1)和其对应的协方差p(k-1|k-1)，可以预测下一时刻的系统的状态x(k|k-1)和其对应的协方差p(k|k-1)
预测方程为
x(k|k-1)＝ax(k-1|k-1)
p(k|k-1)＝ap(k-1|k-1)at+q(1)
然后我们可以得到k时刻的观测值z(k)，并由预测值x(k|k-1)和其对应的协方差p(k|k-1)得到x(k|k)和其对应的协方差p(k|k)。其中x(k|k)是对系统真实值的估计值。
令卡尔曼增益
kg(k)＝p(k|k-1)ht(hp(k|k-1)ht+r)-1(2)
那么可以更新x(k|k)，p(k|k)，更新方程如下：
x(k|k)＝c+kg(k)[z(k)-hx(k|k-1)]
p(k|k)＝[i-kg(k)h]·p(k|k-1)(3)
在每个的k时刻我们通过使用人体的测量位置z(k)和(2)(3)更新卡尔曼方程，并得到人体位置的估计值x(k|k)，然后根据式(1)得到预测值x(k+1|k)，并以此点作为预测值。如此反复循环达到滤波效果。
s5，机器人运动控制装置5将平滑的速度控制指令转化为运动执行机构的执行指令。
以上所述仅为本发明的较佳实施方式而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。