CN112130559A - 一种基于uwb与激光雷达的室内行人跟随与避障方法 - Google Patents
一种基于uwb与激光雷达的室内行人跟随与避障方法 Download PDFInfo
- Publication number
- CN112130559A CN112130559A CN202010847107.5A CN202010847107A CN112130559A CN 112130559 A CN112130559 A CN 112130559A CN 202010847107 A CN202010847107 A CN 202010847107A CN 112130559 A CN112130559 A CN 112130559A
- Authority
- CN
- China
- Prior art keywords
- pedestrian
- agv
- obstacle
- path
- laser radar
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 48
- 230000008859 change Effects 0.000 claims description 27
- 238000001914 filtration Methods 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 7
- 230000004927 fusion Effects 0.000 claims description 6
- 238000013139 quantization Methods 0.000 claims description 6
- 230000000007 visual effect Effects 0.000 claims description 6
- 230000002146 bilateral effect Effects 0.000 claims description 5
- 238000006073 displacement reaction Methods 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 4
- 238000010586 diagram Methods 0.000 description 9
- 230000004044 response Effects 0.000 description 6
- 230000000694 effects Effects 0.000 description 5
- 230000004888 barrier function Effects 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 3
- 230000010355 oscillation Effects 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 230000009977 dual effect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012935 Averaging Methods 0.000 description 1
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012847 principal component analysis method Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种基于UWB与激光雷达的室内行人跟随与避障方法，包括以下步骤：1)利用AGV车载UWB模块获取行人坐标信息，利用激光雷达获取障碍物信息，并实时发送至上位机；2)根据行人坐标信息搜索行人跟随路径，根据障碍物信息规划避障路径，获取AGV当前位姿信息(x,y,θ)，并分别发送至路径跟踪控制模块；3)路径跟踪控制模块计算AGV当前位姿与行人跟随路径之间的关系，并通过模糊PID算法输出线速度v和角速度w；4)将线速度v和角速度w发送至控制模块，实现AGV对行人的稳定跟踪，与现有技术相比，本发明具有提高准确性和稳定性等优点。
Description
技术领域
本发明涉及机器人跟随避障与自主导航领域，尤其是涉及一种基于UWB与激光雷达的室内行人跟随与避障方法。
背景技术
随着科技的进步，服务型机器人作为机器人技术集成的代表，其发展正在走向智能化和自主化。跟随型机器人作为服务型机器人的一种，能够在与目标没有物理接触的情况下自主完成连续的跟随动作，并与目标保持一定的距离，其在特殊人群(残障、老年人)辅助、物流搬运、军事运输等领域有着重要应用。
现有室内机器人跟随技术主要有以下几种：
一是基于摄像头识别的跟随方法：跟随机器人采用摄像头拍摄图像识别行人并进行自主跟随，但普通的单目相机无法获得深度信息，需要结合超声波、红外线等其他传感器进行距离判断，以保持适当跟随距离。此外，这种方法存在跟随对象容易丢失、易受陌生人干扰且受光线影响较大的问题，同时，如何在硬件资源有限的情况下，同时达到视觉跟随系统的准确性与实时性仍然是研究的难点。
二是基于超带宽UWB的方法：为了解决当前视觉跟随的准确性差、实时性低的难点，基于UWB的方法采用双边双向测距(SDS-TWR)的算法计算出标签到UWB基站之间的距离，再通过反余弦定理求出标签和基站之间形成的角度，并将角度和距离信息形成的融合信息通过串口发送给主控制器，通过主控制器对获取的融合信息进行处理，完成跟随目标对象的任务。这种方法可以实现跟随机器人对目标对象实时地、准确地跟随。但是，这种方法存在无法自主避障的问题，例如人群密集与墙壁转角处容易导致与障碍物碰撞。
三是基于RGB-D相机的方法：这种方法利用一种窗口自适应的目标跟踪方法，通过深度图像上的距离信息更新目标的跟踪窗口，采用主分量分析法联合卡尔曼滤波器更新目标的尺寸大小和方向。识别行人目标后，通过联合图像深度信息，获得移动机器人与目标之间的相对位置关系，建立移动机器人跟随模型。同样地，这种方法也存在受光线影响大、目标必须出现在视野内和无法主动避障的问题。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种提高准确性和稳定性的基于UWB与激光雷达的室内行人跟随与避障方法。
本发明的目的可以通过以下技术方案来实现：
一种基于UWB与激光雷达的室内行人跟随与避障方法，包括以下步骤：
1)利用AGV车载UWB模块获取行人坐标信息，利用激光雷达获取障碍物信息，并实时发送至上位机；
2)根据行人坐标信息搜索行人跟随路径，根据障碍物信息规划避障路径，获取AGV当前位姿信息(x,y,θ)，并分别发送至路径跟踪控制模块；
3)路径跟踪控制模块计算AGV当前位姿与行人跟随路径之间的关系，并通过模糊PID算法输出线速度v和角速度w；
4)将线速度v和角速度w发送至底层控制板，实现AGV对行人的稳定跟踪。
进一步地，所述的行人手持信标，所述的UWB模块通过双边双向测距算法获取信标的距离和角度，并将该距离投影于AGV自身坐标轴，得到行人坐标信息。
进一步地，所述的步骤2)具体包括：
21)将行人坐标信息作为目标点，利用A*算法搜索行人跟随路径；
22)利用动态避障算法实时预判碰撞，并规划避障路径；
23)利用IMU传感器和里程计提供的位姿信息进行扩展卡尔曼滤波融合，获取AGV当前的位姿信息(x,y,θ)；
24)将行人跟随路径、避障路径和位姿信息(x,y,θ)分别发送至路径跟踪控制模块。
更进一步地，所述的步骤21)中，初始化时利用激光雷达点云数据和Karto_Slam算法建立一张周围障碍物地图，采用A*算法搜索出地图中到达行人坐标的全局路径，并在AGV跟踪路径的过程中不断完善地图信息；
所述的步骤22)中，当预判到和障碍物发生碰撞时，首先原地转设定角度，判断前方90度视角内激光雷达点云分布状态，若激光雷达点云分布小于设定阈值，则向侧前方绕行，否则后退设定距离后重新规划避障路径。
更进一步地，所述的步骤23)具体包括：
231)使用扩展卡尔曼滤波融合IMU传感器与电机编码器的数据，得到AGV的里程计信息；
232)结合激光雷达点云数据，使用自适应蒙特卡洛定位算法，得到AGV在地图坐标系上的位姿信息。
更进一步地，所述的步骤21)中，当行人坐标信息丢失或者发生大于设定阈值的位移改变时，仍将上一时刻行人坐标信息作为目标点，所述的行人跟随路径的更新频率低于行人坐标信息的更新频率。
进一步地，所述的步骤3)具体包括：
31)路径跟踪控制模块计算AGV当前位姿与行人跟随路径之间的位置偏差、方向偏差、位置偏差变化率和方向偏差变化率；
32)利用偏差变化率进行模糊推理整定PID参数；
33)利用整定完成的PID参数对位置和方向偏差进行PID控制；
34)输出线速度v和角速度w。
更进一步地，所述的模糊推理整定PID参数中，所述的方向偏差eth、位置偏差变化率ded/dt和方向偏差变化率deth/dt分别采用{负大，负小，零，正小，正大}5个量化等级进行模糊化处理，所述的位置偏差ed采用{零，正小，正大}3个量化等级进行模糊化处理，整定后输出的PID参数取值范围分为{小，中，大}3个模糊子集。
更进一步地，所述的模糊推理整定PID参数具体包括：
当位置偏差变化率ded/dt和方向偏差变化率deth/dt为“正大”或“负大”时，增大比例系数Kp并减小积分系数Ki和微分系数Kd；当位置偏差变化率ded/dt和方向偏差变化率deth/dt为“正小”或“负小”时，增大积分系数Ki和微分系数Kd并减小比例系数Kp。
进一步地，所述的底层控制板根据AGV运动学模型解算为驱动电机的转速后，通过闭环控制实现AGV对行人跟踪。
与现有技术相比，本发明具有以下优点：
1)本发明结合了UWB和激光雷达，将UWB模块提供的行人相对坐标信息作为目标点，利用激光雷达提供障碍物地图进行跟随路径规划，同时具有对行人的稳定跟随功能和对障碍物的自主躲避功能，实现了跟随机器人在人群密集与墙壁转角处等复杂环境下对目标对象实时地、准确地跟随；
2)本发明利用A*算法规划全局路径，利用动态避障算法实现局部避障，采用扩展卡尔曼滤波融合IMU传感器和里程计信息对AGV位姿信息进行最优估计，最后通过模糊PID算法改善AGV对于连续急弯、急停等复杂路径的跟踪控制，提升了自主跟随机器人跟随目标的安全性、准确性和稳定性。
附图说明
图1为本发明方法的流程图；
图2为UWB模块定位原理图；
图3为两轮差速底盘；
图4为两轮差速底盘的运动学模型；
图5为路径跟踪控制示意图；
图6为模糊PID路径跟踪控制结构框图；
图7为双闭环路径跟踪控制器；
图8为底层控制框图；
图9为障碍图地图与任意路径跟踪效果图；
图10为避障路径规划与行人目标跟随效果图。
其中，1、万向轮，2、动力轮。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。
实施例
如图1所示，本发明提供一种基于UWB与激光雷达的室内行人跟随与避障方法，具体包括以下步骤：
S1、从AGV车载UWB模块获取手持信标行人的相对坐标，利用激光雷达获取周围障碍物信息，将行人坐标与障碍物信息实时发送给上位机。
具体为：被跟随行人需手持信标，UWB模块通过双边双向测距(SDS-TWR)算法获取行人手持信标的距离与角度，之后将该距离投影于AGV自身坐标轴得到行人的相对坐标，UWB模块定位的定位通过三个基站实现。行人的相对坐标即为路径规划的目标点，AGV接近目标点2.0米时速度随距离逐渐减慢，距离目标点0.8米时停车，停车时保持面向行人，当行人坐标突然丢失或者发生较大位移改变时，仍以上一时刻行人坐标为目标点，且路径规划的频率适当低于行人坐标的更新频率，以防止AGV运行状态频繁波动。
S2、以行人坐标为目标点，以障碍物地图为搜索空间，利用A*算法搜索代价最小的行人跟随路径，利用动态避障算法实时预判碰撞并规划避障路径，将跟随与避障路径发送给路径跟踪控制模块。
具体为：初始化时利用激光雷达点云数据和Karto_Slam算法建立一张周围障碍物地图信息，采用A*算法搜索出地图中到达行人坐标的全局路径，并在AGV跟踪路径的过程中不断完善地图信息。当有障碍物接近时，首先停车原地转动一定角度，判断前方90度视角内点云分布状态，若点云分布较窄(如小于车身半径)则尝试向侧前方绕行，否则后退一定距离后重新规划避障路径。
S3、利用IMU和里程计提供的位姿信息进行扩展卡尔曼滤波融合，估计出AGV当前的位姿(x,y,θ)，发送给路径跟踪控制模块。
具体为：使用扩展卡尔曼滤波融合IMU惯导与电机编码器数据，得到AGV的里程计信息；再结合激光雷达的数据，使用自适应蒙特卡洛定位算法，得到AGV在地图坐标系上准确的位姿。
S4、路径跟踪控制模块计算AGV当前位姿与行人跟随路径之间的位置、方向偏差以及偏差变化率，利用偏差变化率进行模糊推理整定PID参数，再利用整定的参数对位置和方向偏差进行PID控制，输出线速度v和角速度w。
具体为：路径跟踪控制采用模糊PID算法，模糊参数的自整定原则是：当偏差较大时(如急弯)，为了加快系统的响应速度，并防止超调和积分过饱和应取较大的Kp和较小的Ki、Kd；当偏差较小时(如直道)，为了使系统有较好的稳态性能，应适当增大Ki、Kd，同时为避免输出振荡应选取较小Kp。
S5、将速度指令发送给底层控制板，根据AGV运动学模型解算为驱动电机的转速后进行闭环控制，实现AGV对行人的稳定跟踪。
具体为：路径跟踪控制模块输出AGV的线速度和角速度指令，发送给下位机嵌入式控制板，首先利用两轮差速底盘模型将速度指令解算为两驱动轮转速，通过霍尔电机编码器反馈实际转速实现电机的闭环控制，从而驱动AGV跟踪上规划路径。
其中，步骤S1实施过程中，应注意基站安装方式：三个基站需位于同一平面且构成等边三角形，其中A基站应与AGV前进方向保持一致，基站辅设半径可通过串口调试助手配置(默认为10cm)，三个基站天线折叠的方向需相同。UWB模块通过USB串口与上位机通信和供电，需安装ros-serial软件包利用串口传输行人坐标等数据。基于UWB的定位方法采用双边双向测距(SDS-TWR)的算法，三个基站A、B、C分别发送Poll数据包到信标计算出电磁波飞行时间(TOF)，再乘以光速得到各基站到移动信标之间的距离，分别以该距离为半径、各个基站中点为圆心画圆，由于信标满足同时在三个基站的以各自距离为半径的圆上，因此三个圆的交点即为信标位置。定义基站A方向为0度角，平均各个基站的角度和距离信息得到信标的最终方位。将距离按照角度投影到AGV坐标轴即得到信标相对坐标。定位模块中使用卡尔曼滤波减少噪声干扰，Q值与R值可按照1:50选取。定位时认为行人在单位时间的位移在一定范围内，因此当行人坐标突然丢失或者发生较大位移改变时，仍以上一时刻行人坐标为目标点。图2为UWB模块定位原理图。
在步骤S2实施过程中，采用A*算法在地图上寻找一条从AGV到行人目标点不经过障碍物的代价最小的路线。A*算法在规划路径过程中搜索下一步的八个相邻节点(上、下、左、右和左上、左下、右上、右下)并对其估价，估价函数由代价函数与启发值函数求和得到，形式为f(n)＝g(n)+h(n)，其中g(n)为从初始点到当前点的实际代价值，h(n)为当前点到终点的估计值。这种估计值采用当前点到目标终点的曼哈顿距离来估计，即在X和Y方向上的距离之和。整个路径规划的频率适当低于行人坐标的更新频率，以防止AGV运行状态频繁波动。这里的动态避障算法未使用DWA动态窗口法，而是一种碰撞预判与尝试重规划的绕行避障方法，通过设置避障标志位避免AGV避障时反复震荡现象。
在步骤S3实施过程中，实际发现在IMU提供的位姿信息的协方差矩阵中，代表机器人航向角分量的协方差数量级为10-6，而里程计信息的协方差矩阵中机器人姿态分量的协方差数量级为10-3，两者之间的数量级相差很大。故在进行EKF融合时，会更“相信”IMU所提供的姿态角信息。此外，若机器人在转动过程中轮子发生打滑，用电机编码器推算出的姿态一直在旋转，而其实际姿态并未发生太大变化，通过EKF融合后的位姿信息则可以纠正此种情形下的错误估计，使姿态信息更加可信。通过EKF融合两种传感器数据得到里程计信息后，再结合激光雷达点云数据，利用自适应蒙特卡罗定位算法(AMCL)估计出AGV在地图中的位姿(x,y,θ)，最终用于路径跟踪控制，路径跟踪控制的示意图如图5所示。
在步骤S4实施过程中，采用两个模糊PID控制器分别对AGV的线速度和角速度进行控制，以跟踪上步骤S2中规划的跟随路径。如图6所示，首先以AGV当前位姿与规划的跟随路径之间的位置偏差、方向偏差以及偏差的变化率作为模糊推理系统的输入，确定线速度、角速度PID控制器的Kp，Ki，Kd参数。方向偏差eth、位置偏差变化率ded/dt和方向偏差变化率deth/dt均采用5个量化等级进行模糊化处理。模糊子集为{负大，负小，零，正小，正大}，即{NB，NS，ZO，PS，PB}；而位置偏差ed取机器人位置到当前两个跟踪点连成的线段的距离，恒为正值，因此使用3个量化等级{零，正小，正大}即可。最后单个PID的输出取值范围分为{小，中，大}，即{S，M，B}共3个模糊子集。模糊规则的确定，根据参数Kp，Ki，Kd对系统输出特性的影响，可以归纳出参数的自整定原则：
(1)当偏差较大时，为了加快系统的响应速度，并防止开始时偏差的瞬间变大可能引起的微分过饱和，应取较大的Kp和较小的Kd。另外为防止积分饱和，避免系统响应较大的超调，Ki要较小。
(2)当偏差和变化率为中等大小时，为了使系统响应的超调量减小和保证一定的响应速度，Kp应取得较小；这种情况下，Kd的取值对系统影响很大，应取小一些；Ki的取值应适当。
(3)当偏差较小时，为了使系统有较好的稳态性能，应增大Ki，Kd，同时为避免输出响应在设定值附近振荡，应适当减小Kp，原则是：偏差变化剧烈时，取大的值，反之取小。实际的模糊规则和隶属度函数需要根据实验测试结果进行修正，最终使得AGV能够根据路径曲折程度自适应地调整PID参数，更加稳定地跟踪规划路径。如图9为任意路径的跟踪控制效果示意图。
在步骤S5实施过程中，底层控制板接收上位机的速度指令，根据AGV运动学模型进行解算，AGV底盘包括两个万向轮1和两个动力轮2，将移动机器人的预期速度转换为左、右两个动力轮2的预期转速。AGV的两轮差速底盘结构及其运动学模型如图3和图4中所示，车体速度为V，车体自转速度为ω，左动力轮速度为Vl，右动力轮速度为Vr，转弯半径为R，两轮之间距离为D，两轮到车中心的距离为d，右轮到圆心距离为L。将机器人整体运动的预期前向速度V和转动角速度ω解算为左、右动力轮2独立速度Vl和Vr后分别控制，满足约束方程Vl＝ω×(L+D)＝ω×(R+d)＝V+ωd和Vr＝ω×L＝ω×(R-d)＝V-ωd。单个电机转速控制采用PID算法，通过电机编码器反馈实际转速，将对应占空比的PWM驱动波形发送给电机驱动板，控制动力电机转动，完成双电机转速的闭环控制。图7和图8是双闭环路径跟踪控制器和底层控制框图。
本发明提供的室内行人跟随与避障方法，与现有技术中的行人跟随方法相比，最大的创新点有两点：一是本发明结合了UWB和激光雷达，将UWB模块提供的行人相对坐标作为目标点，利用激光雷达提供的障碍物地图进行跟随路径规划，具有对行人跟随功能的同时，能够对动态障碍物进行自主躲避和绕行，实现了跟随机器人在人群密集与墙壁转角处等复杂环境下对目标对象安全地、准确地跟随。二是本发明通过模糊PID算法改善了AGV对于连续急弯、急停等复杂规划路径的跟踪控制效果，通过采用碰撞预判和尝试重规划的避障算法，避免了现有避障方法的重复震荡现象。这两点创新提升了自主跟随机器人跟随行人目标的安全性、准确性和稳定性。实际的避障路径规划与行人目标跟随效果如图10所示。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。
Claims (10)
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参数中，所述的方向偏差eth、位置偏差变化率ded/dt和方向偏差变化率deth/dt分别采用{负大，负小，零，正小，正大}5个量化等级进行模糊化处理，所述的位置偏差ed采用{零，正小，正大}3个量化等级进行模糊化处理，整定后输出的PID参数取值范围分为{小，中，大}3个模糊子集。
9.根据权利要求8所述的一种基于UWB与激光雷达的室内行人跟随与避障方法，其特征在于，所述的模糊推理整定PID参数具体包括：
当位置偏差变化率ded/dt和方向偏差变化率deth/dt为“正大”或“负大”时，增大比例系数Kp并减小积分系数Ki和微分系数Kd；当位置偏差变化率ded/dt和方向偏差变化率deth/dt为“正小”或“负小”时，增大积分系数Ki和微分系数Kd并减小比例系数Kp。
10.根据权利要求1所述的一种基于UWB与激光雷达的室内行人跟随与避障方法，其特征在于，所述的底层控制板根据AGV运动学模型解算为驱动电机的转速后，通过闭环控制实现AGV对行人跟踪。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010847107.5A CN112130559A (zh) | 2020-08-21 | 2020-08-21 | 一种基于uwb与激光雷达的室内行人跟随与避障方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010847107.5A CN112130559A (zh) | 2020-08-21 | 2020-08-21 | 一种基于uwb与激光雷达的室内行人跟随与避障方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112130559A true CN112130559A (zh) | 2020-12-25 |
Family
ID=73851057
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010847107.5A Pending CN112130559A (zh) | 2020-08-21 | 2020-08-21 | 一种基于uwb与激光雷达的室内行人跟随与避障方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112130559A (zh) |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112907625A (zh) * | 2021-02-05 | 2021-06-04 | 齐鲁工业大学 | 应用于四足仿生机器人的目标跟随方法及系统 |
| CN113034928A (zh) * | 2021-02-26 | 2021-06-25 | 山东交通学院 | 一种道路养护安全警示机器人的伴随系统 |
| CN113031002A (zh) * | 2021-02-25 | 2021-06-25 | 桂林航天工业学院 | 基于Kinect3和激光雷达的SLAM陪跑小车 |
| CN113059576A (zh) * | 2021-03-31 | 2021-07-02 | 上海应用技术大学 | 一种基于鱼群效应的医疗运输机器人及自适应巡航跟随方法 |
| CN113093722A (zh) * | 2021-02-20 | 2021-07-09 | 意欧斯物流科技(上海)有限公司 | 一种agv智能路径规划及模糊控制方法 |
| CN113110413A (zh) * | 2021-03-10 | 2021-07-13 | 成都永奉科技有限公司 | 跟随机器人以及跟随控制方法、跟随控制系统 |
| CN113252321A (zh) * | 2021-04-14 | 2021-08-13 | 南京云智控产业技术研究院有限公司 | 一种涉及动力轮打滑修正的多传感器融合载具定位方法 |
| CN113253717A (zh) * | 2021-03-17 | 2021-08-13 | 武汉科技大学 | 一种基于动态障碍物运动信息的室内移动机器人局部路径规划方法 |
| CN113359744A (zh) * | 2021-06-21 | 2021-09-07 | 暨南大学 | 一种基于安全强化学习及视觉传感器的机器人避障系统 |
| CN113625707A (zh) * | 2021-07-09 | 2021-11-09 | 江苏大学 | 一种多传感器融合的温室自动跟随平台及其控制方法 |
| CN113870316A (zh) * | 2021-10-19 | 2021-12-31 | 青岛德智汽车科技有限公司 | 一种无gps跟车场景下前车路径重建方法 |
| CN114217620A (zh) * | 2021-12-15 | 2022-03-22 | 常州信息职业技术学院 | 一种轮式机器人智能避障控制系统及方法 |
| CN114326732A (zh) * | 2021-12-28 | 2022-04-12 | 无锡笠泽智能科技有限公司 | 一种机器人自主跟随系统及自主跟随控制方法 |
| CN114415689A (zh) * | 2022-01-25 | 2022-04-29 | 无锡市金沙田科技有限公司 | 一种跟随车的决策方法及系统 |
| CN114995405A (zh) * | 2022-05-19 | 2022-09-02 | 同济大学 | 一种基于开放动态环境多目标协同理论的agv协作搬运方法 |
| CN115396807A (zh) * | 2022-06-09 | 2022-11-25 | 长春理工大学 | 一种基于uwb定位的多跟随车控制系统 |
| CN114265375B (zh) * | 2021-11-25 | 2024-06-04 | 云南昆船智能装备有限公司 | 一种agv对平板卡车货物存取的系统和方法 |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102358287A (zh) * | 2011-09-05 | 2012-02-22 | 北京航空航天大学 | 一种用于车辆自动驾驶机器人的轨迹跟踪控制方法 |
| CN105807775A (zh) * | 2016-05-17 | 2016-07-27 | 上海酷哇机器人有限公司 | 具有自主跟随避障功能的移动机器人 |
| TW201807523A (zh) * | 2016-08-22 | 2018-03-01 | 金寶電子工業股份有限公司 | 移動式機器人的即時導航方法 |
| CN107765220A (zh) * | 2017-09-20 | 2018-03-06 | 武汉木神机器人有限责任公司 | 基于uwb和激光雷达混合定位的行人跟随系统及方法 |
| CN108614563A (zh) * | 2018-06-12 | 2018-10-02 | 北京康力优蓝机器人科技有限公司 | 一种通过定位导航实现移动机器人目标跟随的方法 |
| CN108762264A (zh) * | 2018-05-22 | 2018-11-06 | 重庆邮电大学 | 基于人工势场与滚动窗口的机器人的动态避障方法 |
| CN109189105A (zh) * | 2018-10-18 | 2019-01-11 | 上海电力学院 | 一种巡检机器人云台控制方法 |
| CN109656250A (zh) * | 2018-12-26 | 2019-04-19 | 芜湖哈特机器人产业技术研究院有限公司 | 一种激光叉车的路径跟踪方法 |
| US20190146062A1 (en) * | 2017-11-15 | 2019-05-16 | Baidu Online Network Technology (Beijing) Co., Ltd | Laser point cloud positioning method and system |
| CN110228524A (zh) * | 2018-03-06 | 2019-09-13 | 重庆邮电大学 | 基于多层模糊控制的无人车辆自动转向控制方法 |
| CN110262495A (zh) * | 2019-06-26 | 2019-09-20 | 山东大学 | 可实现移动机器人自主导航与精确定位的控制系统及方法 |
| US20190339715A1 (en) * | 2018-05-04 | 2019-11-07 | Lg Electronics Inc. | Plurality of autonomous mobile robots and controlling method for the same |
-
2020
- 2020-08-21 CN CN202010847107.5A patent/CN112130559A/zh active Pending
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102358287A (zh) * | 2011-09-05 | 2012-02-22 | 北京航空航天大学 | 一种用于车辆自动驾驶机器人的轨迹跟踪控制方法 |
| CN105807775A (zh) * | 2016-05-17 | 2016-07-27 | 上海酷哇机器人有限公司 | 具有自主跟随避障功能的移动机器人 |
| TW201807523A (zh) * | 2016-08-22 | 2018-03-01 | 金寶電子工業股份有限公司 | 移動式機器人的即時導航方法 |
| CN107765220A (zh) * | 2017-09-20 | 2018-03-06 | 武汉木神机器人有限责任公司 | 基于uwb和激光雷达混合定位的行人跟随系统及方法 |
| US20190146062A1 (en) * | 2017-11-15 | 2019-05-16 | Baidu Online Network Technology (Beijing) Co., Ltd | Laser point cloud positioning method and system |
| CN110228524A (zh) * | 2018-03-06 | 2019-09-13 | 重庆邮电大学 | 基于多层模糊控制的无人车辆自动转向控制方法 |
| US20190339715A1 (en) * | 2018-05-04 | 2019-11-07 | Lg Electronics Inc. | Plurality of autonomous mobile robots and controlling method for the same |
| CN108762264A (zh) * | 2018-05-22 | 2018-11-06 | 重庆邮电大学 | 基于人工势场与滚动窗口的机器人的动态避障方法 |
| CN108614563A (zh) * | 2018-06-12 | 2018-10-02 | 北京康力优蓝机器人科技有限公司 | 一种通过定位导航实现移动机器人目标跟随的方法 |
| CN109189105A (zh) * | 2018-10-18 | 2019-01-11 | 上海电力学院 | 一种巡检机器人云台控制方法 |
| CN109656250A (zh) * | 2018-12-26 | 2019-04-19 | 芜湖哈特机器人产业技术研究院有限公司 | 一种激光叉车的路径跟踪方法 |
| CN110262495A (zh) * | 2019-06-26 | 2019-09-20 | 山东大学 | 可实现移动机器人自主导航与精确定位的控制系统及方法 |
Non-Patent Citations (2)
| Title |
|---|
| 董兵兵: "移动机器人主动环境感知及三维场景数字化", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
| 黎明安: "《动力学控制基础与应用》", 31 January 2013 * |
Cited By (26)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112907625A (zh) * | 2021-02-05 | 2021-06-04 | 齐鲁工业大学 | 应用于四足仿生机器人的目标跟随方法及系统 |
| CN112907625B (zh) * | 2021-02-05 | 2023-04-28 | 齐鲁工业大学 | 应用于四足仿生机器人的目标跟随方法及系统 |
| CN113093722A (zh) * | 2021-02-20 | 2021-07-09 | 意欧斯物流科技(上海)有限公司 | 一种agv智能路径规划及模糊控制方法 |
| CN113031002A (zh) * | 2021-02-25 | 2021-06-25 | 桂林航天工业学院 | 基于Kinect3和激光雷达的SLAM陪跑小车 |
| CN113031002B (zh) * | 2021-02-25 | 2023-10-24 | 桂林航天工业学院 | 基于Kinect3和激光雷达的SLAM陪跑小车 |
| CN113034928B (zh) * | 2021-02-26 | 2022-05-31 | 山东交通学院 | 一种道路养护安全警示机器人的伴随系统 |
| CN113034928A (zh) * | 2021-02-26 | 2021-06-25 | 山东交通学院 | 一种道路养护安全警示机器人的伴随系统 |
| CN113110413A (zh) * | 2021-03-10 | 2021-07-13 | 成都永奉科技有限公司 | 跟随机器人以及跟随控制方法、跟随控制系统 |
| CN113253717A (zh) * | 2021-03-17 | 2021-08-13 | 武汉科技大学 | 一种基于动态障碍物运动信息的室内移动机器人局部路径规划方法 |
| CN113253717B (zh) * | 2021-03-17 | 2022-10-11 | 武汉科技大学 | 一种基于动态障碍物的室内移动机器人局部路径规划方法 |
| CN113059576A (zh) * | 2021-03-31 | 2021-07-02 | 上海应用技术大学 | 一种基于鱼群效应的医疗运输机器人及自适应巡航跟随方法 |
| CN113252321A (zh) * | 2021-04-14 | 2021-08-13 | 南京云智控产业技术研究院有限公司 | 一种涉及动力轮打滑修正的多传感器融合载具定位方法 |
| CN113252321B (zh) * | 2021-04-14 | 2022-01-11 | 南京云智控产业技术研究院有限公司 | 一种涉及动力轮打滑修正的多传感器融合载具定位方法 |
| CN113359744A (zh) * | 2021-06-21 | 2021-09-07 | 暨南大学 | 一种基于安全强化学习及视觉传感器的机器人避障系统 |
| CN113359744B (zh) * | 2021-06-21 | 2022-03-01 | 暨南大学 | 一种基于安全强化学习及视觉传感器的机器人避障系统 |
| CN113625707A (zh) * | 2021-07-09 | 2021-11-09 | 江苏大学 | 一种多传感器融合的温室自动跟随平台及其控制方法 |
| CN113625707B (zh) * | 2021-07-09 | 2024-03-19 | 江苏大学 | 一种多传感器融合的温室自动跟随平台及其控制方法 |
| CN113870316B (zh) * | 2021-10-19 | 2023-08-15 | 青岛德智汽车科技有限公司 | 一种无gps跟车场景下前车路径重建方法 |
| CN113870316A (zh) * | 2021-10-19 | 2021-12-31 | 青岛德智汽车科技有限公司 | 一种无gps跟车场景下前车路径重建方法 |
| CN114265375B (zh) * | 2021-11-25 | 2024-06-04 | 云南昆船智能装备有限公司 | 一种agv对平板卡车货物存取的系统和方法 |
| CN114217620A (zh) * | 2021-12-15 | 2022-03-22 | 常州信息职业技术学院 | 一种轮式机器人智能避障控制系统及方法 |
| CN114326732A (zh) * | 2021-12-28 | 2022-04-12 | 无锡笠泽智能科技有限公司 | 一种机器人自主跟随系统及自主跟随控制方法 |
| CN114415689A (zh) * | 2022-01-25 | 2022-04-29 | 无锡市金沙田科技有限公司 | 一种跟随车的决策方法及系统 |
| CN114415689B (zh) * | 2022-01-25 | 2024-05-28 | 无锡市金沙田科技有限公司 | 一种跟随车的决策方法及系统 |
| CN114995405A (zh) * | 2022-05-19 | 2022-09-02 | 同济大学 | 一种基于开放动态环境多目标协同理论的agv协作搬运方法 |
| CN115396807A (zh) * | 2022-06-09 | 2022-11-25 | 长春理工大学 | 一种基于uwb定位的多跟随车控制系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112130559A (zh) | 一种基于uwb与激光雷达的室内行人跟随与避障方法 | |
| Morales et al. | Pure-pursuit reactive path tracking for nonholonomic mobile robots with a 2D laser scanner | |
| Goto et al. | Mobile robot navigation: The CMU system | |
| Wit | Vector pursuit path tracking for autonomous ground vehicles | |
| McGee et al. | Path planning and control for multiple point surveillance by an unmanned aircraft in wind | |
| JP2001255937A (ja) | 車両用自動走行制御装置 | |
| Hsu et al. | On the complete coverage path planning for mobile robots | |
| Rick et al. | Autonomous driving based on nonlinear model predictive control and multi-sensor fusion | |
| Ollero et al. | Control and perception components for autonomous vehicle guidance. Application to the ROMEO vehicles | |
| Mellinger et al. | Control and planning for vehicles with uncertainty in dynamics | |
| Hurni et al. | A pseudospectral optimal motion planner for autonomous unmanned vehicles | |
| Lundgren | Path tracking and obstacle avoidance for a miniature robot | |
| Ohkawa et al. | Development of autonomous mobile robot using articulated steering vehicle and lateral guiding method | |
| Bento et al. | Autonomous navigation control with magnetic markers guidance of a cybernetic car using fuzzy logic | |
| Sivarathri et al. | Kinematic modes of vision-based heterogeneous UAV-AGV system | |
| Ventura | Safe and flexible hybrid control architecture for the navigation in formation of a group of vehicles | |
| Ray et al. | GPS and sonar based area mapping and navigation by mobile robots | |
| Nejad et al. | Real time localization, path planning and motion control for autonomous parking in cluttered environment with narrow passages | |
| Chiang et al. | Multisensor-based outdoor tour guide robot NTU-I | |
| Kim | Underwater surface scan utilizing an unmanned underwater vehicle with sampled range information | |
| Doumbia et al. | A novel infrared navigational algorithm for autonomous robots | |
| Lee et al. | Sensor fusion design for navigation and control of an autonomous vehicle | |
| Nazari et al. | A vision-based intelligent path following control of a four-wheel differentially driven skid steer mobile robot | |
| Feng et al. | Research on path planning and control of driverless logistics train | |
| Mukherjee et al. | An enhanced experimental study of GPS based path tracking non-holonomic robot with SAWOA algorithm |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20201225 |