(19)中华人民共和国国家知识产权局
(12)发明专利申请
(10)申请公布号 (43)申请公布日 (21)申请号 202010847107.5
(22)申请日 2020.08.21
(71)申请人 同济大学
地址 200092 上海市杨浦区四平路1239号
(72)发明人 刘成菊 袁家遥 陈启军 石文博
杜家源
(74)专利代理机构 上海科盛知识产权代理有限
公司 31225
代理人 杨宏泰
(51)Int.Cl.
G05D 1/02(2020.01)
(54)发明名称
(57)摘要
本发明涉及一种基于UWB与激光雷达的室内
行人跟随与避障方法，包括以下步骤：1)利用AGV
获取障碍物信息，并实时发送至上位机；2)根据
息规划避障路径，获取AGV当前位姿信息(x ,y ,
θ)，并分别发送至路径跟踪控制模块；3)路径跟
踪控制模块计算AGV当前位姿与行人跟随路径之
间的关系，并通过模糊PID算法输出线速度v和角
速度w；4)将线速度v和角速度w发送至控制模块，
实现AGV对行人的稳定跟踪，与现有技术相比，本
发明具有提高准确性和稳定性等优点。权利要求书2页 说明书6页 附图5页CN 112130559 A 2020.12.25
C N 112130559
A
1.一种基于UWB与激光雷达的室内行人跟随与避障方法，其特征在于，包括以下步骤：
1)利用AGV车载UWB模块获取行人坐标信息，利用激光雷达获取障碍物信息，并实时发送至上位机；
2)根据行人坐标信息搜索行人跟随路径，根据障碍物信息规划避障路径，获取AGV当前位姿信息(x，y，θ)，并分别发送至路径跟踪控制模块；
3)路径跟踪控制模块计算AGV当前位姿与行人跟随路径之间的关系，并通过模糊PID算法输出线速度v和角速度w；
4)将线速度v和角速度w发送至底层控制板，实现AGV对行人的稳定跟踪。
2.根据权利要求1所述的一种基于UWB与激光雷达的室内行人跟随与避障方法，其特征在于，所述的行人手持信标，所述的UWB模块通过双边双向测距算法获取信标的距离和角度，并将该距离投影于AGV自身坐标轴，得到行人坐标信息。
3.根据权利要求1所述的一种基于UWB与激光雷达的室内行人跟随与避障方法，其特征在于，所述的步骤2)具体包括：
21)将行人坐标信息作为目标点，利用A*算法搜索行人跟随路径；
22)利用动态避障算法实时预判碰撞，并规划避障路径；
23)利用IMU传感器和里程计提供的位姿信息进行扩展卡尔曼滤波融合，获取AGV当前的位姿信息(x，y，θ)；
24)将行人跟随路径、避障路径和位姿信息(x，y，θ)分别发送至路径跟踪控制模块。
4.根据权利要求3所述的一种基于UWB与激光雷达的室内行人跟随与避障方法，其特征在于，所述的步骤21)中，初始化时利用激光雷达点云数据和Karto_Slam算法建立一张周围障碍物地图，采用A*算法搜索出地图中到达行人坐标的全局路径，并在AGV跟踪路径的过程中不断完善地图信息；
所述的步骤22)中，当预判到和障碍物发生碰撞时，首先原地转设定角度，判断前方90度视角内激光雷达点云分布状态，若激光雷达点云分布小于设定阈值，则向侧前方绕行，否则后退设定距离后重新规划避障路径。
5.根据权利要求3或4所述的一种基于UWB与激光雷达的室内行人跟随与避障方法，其特征在于，所述的步骤23)具体包括：
231)使用扩展卡尔曼滤波融合IMU传感器与电机编码器的数据，得到AGV的里程计信息；
232)结合激光雷达点云数据，使用自适应蒙特卡洛定位算法，得到AGV在地图坐标系上的位姿信息。
6.根据权利要求3或4所述的一种基于UWB与激光雷达的室内行人跟随与避障方法，其特征在于，所述的步骤21)中，当行人坐标信息丢失或者发生大于设定阈值的位移改变时，仍将上一时刻行人坐标信息作为目标点，所述的行人跟随路径的更新频率低于行人坐标信息的更新频率。
7.根据权利要求1所述的一种基于UWB与激光雷达的室内行人跟随与避障方法，其特征在于，所述的步骤3)具体包括：
31)路径跟踪控制模块计算AGV当前位姿与行人跟随路径之间的位置偏差、方向偏差、位置偏差变化率和方向偏差变化率；
32)利用偏差变化率进行模糊推理整定PID参数；
33)利用整定完成的PID参数对位置和方向偏差进行PID控制；
34)输出线速度v和角速度w。
8.根据权利要求7所述的一种基于UWB与激光雷达的室内行人跟随与避障方法，其特征在于，所述的模糊推理整定PID参数中，所述的方向偏差e th、位置偏差变化率de d/dt和方向偏差变化率de th/dt分别采用{负大，负小，零，正小，正大}5个量化等级进行模糊化处理，所述的位置偏差e d采用{零，正小，正大}3个量化等级进行模糊化处理，整定后输出的PID参数取值范围分为{小，中，大}3个模糊子集。
9.根据权利要求8所述的一种基于UWB与激光雷达的室内行人跟随与避障方法，其特征在于，所述的模糊推理整定PID参数具体包括：
当位置偏差变化率de d/dt和方向偏差变化率de th/dt为“正大”或“负大”时，增大比例系数K p并减小积分系数K i和微分系数K d；当位置偏差变化率de d/dt和方向偏差变化率de th/dt 为“正小”或“负小”时，增大积分系数K i和微分系数K d并减小比例系数K p。
10.根据权利要求1所述的一种基于UWB与激光雷达的室内行人跟随与避障方法，其特征在于，所述的底层控制板根据AGV运动学模型解算为驱动电机的转速后，通过闭环控制实现AGV对行人跟踪。
一种基于UWB与激光雷达的室内行人跟随与避障方法
技术领域
[0001]本发明涉及机器人跟随避障与自主导航领域，尤其是涉及一种基于UWB与激光雷达的室内行人跟随与避障方法。
背景技术
[0002]随着科技的进步，服务型机器人作为机器人技术集成的代表，其发展正在走向智能化和自主化。跟随型机器人作为服务型机器人的一种，能够在与目标没有物理接触的情况下自主完成连续的跟随动作，并与目标保持一定的距离，其在特殊人(残障、老年人)辅助、物流搬运、军事运输等领域有着重要应用。
[0003]现有室内机器人跟随技术主要有以下几种：
[0004]一是基于摄像头识别的跟随方法：跟随机器人采用摄像头拍摄图像识别行人并进行自主跟随，但普通的单目相机无法获得深度信息，需要结合超声波、红外线等其他传感器进行距离判断，以保持适当跟随距离。此外，这种方法存在跟随对象容易丢失、易受陌生人干扰且受光线影响较大的问题，同时，如何在硬件资源有限的情况下，同时达到视觉跟随系统的准确性与实时性仍然是研究的难点。
[0005]二是基于超带宽UWB的方法：为了解决当前视觉跟随的准确性差、实时性低的难点，基于UWB的方法采用双边双向测距(SDS-TWR)的算法计算出标签到UWB之间的距离，再通过反余弦定理求出标签和之间形成的角度，并将角度和距离信息形成的融合信息通过串口发送给主控制器，通过主控制器对获取的融合信息进行处理，完成跟随目标对象的任务。这种方法可以实现跟随机器人对目标对象实时地、准确地跟随。但是，这种方法存在无法自主避障的问题，例如人密集与墙壁转角处容易导致与障碍物碰撞。
[0006]三是基于RGB-D相机的方法：这种方法利用一种窗口自适应的目标跟踪方法，通过深度图像上的距离信息更新目标的跟踪窗口，采用主分量分析法联合卡尔曼滤波器更新目标的尺寸大小和方向。识别行人目标后，通过联合图像深度信息，获得移动机器人与目标之间的相对位置关系，建立移动机器人跟随模型。同样地，这种方法也存在受光线影响大、目标必须出现在视野内和无法主动避障的问题。
发明内容
[0007]本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种提高准确性和稳定性的基于UWB与激光雷达的室内行人跟随与避障方法。
[0008]本发明的目的可以通过以下技术方案来实现：
[0009]一种基于UWB与激光雷达的室内行人跟随与避障方法，包括以下步骤：
[0010]1)利用AGV车载UWB模块获取行人坐标信息，利用激光雷达获取障碍物信息，并实时发送至上位机；
[0011]2)根据行人坐标信息搜索行人跟随路径，根据障碍物信息规划避障路径，获取AGV 当前位姿信息(x,y,θ)，并分别发送至路径跟踪控制模块；
[0012]3)路径跟踪控制模块计算AGV当前位姿与行人跟随路径之间的关系，并通过模糊PID算法输出线速度v和角速度w；
[0013]4)将线速度v和角速度w发送至底层控制板，实现AGV对行人的稳定跟踪。[0014]进一步地，所述的行人手持信标，所述的UWB模块通过双边双向测距算法获取信标的距离和角度，并将该距离投影于AGV自身坐标轴，得到行人坐标信息。
[0015]进一步地，所述的步骤2)具体包括：
[0016]21)将行人坐标信息作为目标点，利用A*算法搜索行人跟随路径；
[0017]22)利用动态避障算法实时预判碰撞，并规划避障路径；
[0018]23)利用IMU传感器和里程计提供的位姿信息进行扩展卡尔曼滤波融合，获取AGV 当前的位姿信息(x,y,θ)；
[0019]24)将行人跟随路径、避障路径和位姿信息(x,y,θ)分别发送至路径跟踪控制模块。
[0020]更进一步地，所述的步骤21)中，初始化时利用激光雷达点云数据和Karto_Slam算法建立一张周围障碍物地图，采用A*算法搜索出地图中到达行人坐标的全局路径，并在AGV 跟踪路径的过程中不断完善地图信息；
[0021]所述的步骤22)中，当预判到和障碍物发生碰撞时，首先原地转设定角度，判断前方90度视角内激光雷达点云分布状态，若激光雷达点云分布小于设定阈值，则向侧前方绕行，否则后退设定距离后重新规划避障路径。
[0022]更进一步地，所述的步骤23)具体包括：
[0023]231)使用扩展卡尔曼滤波融合IMU传感器与电机编码器的数据，得到AGV的里程计信息；
[0024]232)结合激光雷达点云数据，使用自适应蒙特卡洛定位算法，得到AGV在地图坐标系上的位姿信息。
[0025]更进一步地，所述的步骤21)中，当行人坐标信息丢失或者发生大于设定阈值的位移改变时，仍将上一时刻行人坐标信息作为目标点，所述的行人跟随路径的更新频率低于行人坐标信息的更新频率。
[0026]进一步地，所述的步骤3)具体包括：
[0027]31)路径跟踪控制模块计算AGV当前位姿与行人跟随路径之间的位置偏差、方向偏差、位置偏差变化率和方向偏差变化率；
[0028]32)利用偏差变化率进行模糊推理整定PID参数；
[0029]33)利用整定完成的PID参数对位置和方向偏差进行PID控制；
[0030]34)输出线速度v和角速度w。
[0031]更进一步地，所述的模糊推理整定PID参数中，所述的方向偏差e th、位置偏差变化率de d/dt和方
向偏差变化率de th/dt分别采用{负大，负小，零，正小，正大}5个量化等级进行模糊化处理，所述的位置偏差e d采用{零，正小，正大}3个量化等级进行模糊化处理，整定后输出的PID参数取值范围分为{小，中，大}3个模糊子集。
[0032]更进一步地，所述的模糊推理整定PID参数具体包括：
[0033]当位置偏差变化率de d/dt和方向偏差变化率de th/dt为“正大”或“负大”时，增大比例系数K p并减小积分系数K i和微分系数K d；当位置偏差变化率de d/dt和方向偏差变化率
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议