[发明专利]基于UWB和激光雷达混合定位的行人跟随系统及方法

权利要求书

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)处理的速度控制指令转化为运动执行机构的执行指令；

根据三角测量原理测量机器人相对于UWB标签(3)的UWB距离S1及UWB角度θ1，得到UWB标签(3)相对于机器人的位置(S1,θ1)；

通过激光雷达(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)的分辨率，则最终跟随目标在机器人坐标下的位置为

2.一种基于UWB和激光雷达混合定位的行人跟随方法，其特征在于：包括以下步骤，

S1，通过UWB基站(2)向UWB标签(3)发送UWB信号，计算跟随目标与机器人之间的UWB距离及UWB角度；

S2，通过激光雷达(1)测量跟随目标与机器人之间的距离及角度，对UWB距离及UWB角度进行校正，假设机器人周围无障碍物，生成一条最近的参考路径；

S3，通过激光雷达(1)测量机器人周围环境信息，创建局部地图，然后根据校正后的机器人相对于跟随目标的距离及角度，在参考路径的基础上生成一条无碰撞的最终路径；

S4，针对最终路径生成速度控制指令，并对生成的速度控制指令进行滤波处理，得到平滑的速度控制指令；

S5，机器人运动控制装置(5)将平滑的速度控制指令转化为运动执行机构的执行指令；

所述步骤S1中，根据三角测量原理测量机器人相对于UWB标签(3)的UWB距离S1及UWB角度θ1，得到UWB标签(3)相对于机器人的位置(S1,θ1)；所述步骤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)的分辨率，则最终跟随目标在机器人坐标下的位置为

3.如权利要求2所述的基于UWB和激光雷达混合定位的行人跟随方法，其特征在于：所述步骤S2中，参考路径为过机器人坐标系的原点指向点的直线，其控制线速度和角速度为其中K1为距离与线速度的比例关系，K2为机器人角速度与机器人正前方和跟随目标测量宽度的法线角度比例关系。