CN115457075A - 一种基于sac-pid的移动机器人目标跟随方法 - Google Patents
一种基于sac-pid的移动机器人目标跟随方法 Download PDFInfo
- Publication number
- CN115457075A CN115457075A CN202210912958.2A CN202210912958A CN115457075A CN 115457075 A CN115457075 A CN 115457075A CN 202210912958 A CN202210912958 A CN 202210912958A CN 115457075 A CN115457075 A CN 115457075A
- Authority
- CN
- China
- Prior art keywords
- target
- mobile robot
- following
- distance
- value
- 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
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/60—Analysis of geometric attributes
- G06T7/62—Analysis of geometric attributes of area, perimeter, diameter or volume
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/20—Image preprocessing
- G06V10/25—Determination of region of interest [ROI] or a volume of interest [VOI]
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/20—Image preprocessing
- G06V10/26—Segmentation of patterns in the image field; Cutting or merging of image elements to establish the pattern region, e.g. clustering-based techniques; Detection of occlusion
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
- G06V10/761—Proximity, similarity or dissimilarity measures
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/82—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using neural networks
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V40/00—Recognition of biometric, human-related or animal-related patterns in image or video data
- G06V40/10—Human or animal bodies, e.g. vehicle occupants or pedestrians; Body parts, e.g. hands
- G06V40/103—Static body considered as a whole, e.g. static pedestrian or occupant recognition
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10024—Color image
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20081—Training; Learning
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20084—Artificial neural networks [ANN]
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V2201/00—Indexing scheme relating to image or video recognition or understanding
- G06V2201/07—Target detection
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Evolutionary Computation (AREA)
- Mechanical Engineering (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computing Systems (AREA)
- Databases & Information Systems (AREA)
- Robotics (AREA)
- General Health & Medical Sciences (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Geometry (AREA)
- Human Computer Interaction (AREA)
- Feedback Control In General (AREA)
Abstract
本发明涉及一种基于SAC‑PID的移动机器人目标跟随方法,获取机器人感知数据并预处理,使用目标检测网络获取跟随对象候选区域,跟随对象目标匹配后,采集强化学习的状态信息,以已训练好的SAC强化学习模型,输出移动机器人运动控制的目标角速度和目标线速度,实现移动机器人端到端的目标跟随决策输出,利用PID控制器对移动机器人进行平滑控制。本发明的有益效果在于,建立跟踪目标的模型,有效区分背景和跟随目标,遮挡情况下跟随方法可以继续应用,实现简单,收敛速度快,目标跟随的准确性高,能够应对复杂的应用场景,具备较强的鲁棒性,实现跟踪过程的平滑控制。
Description
技术领域
本发明涉及计算;推算;计数的技术领域,特别涉及一种智能机器人领域的、结合SAC与PID控制的基于SAC-PID的移动机器人目标跟随方法。
背景技术
随着移动机器人、人工智能等技术的快速发展,智能机器人正逐渐成为我们工作和生活的一部分。智能机器人与传统工业机器人相比,其对外界的感知能力更强,对定位系统、障碍检测和地图信息的要求更低,能够在复杂的环境中完成灵活精细的作业。人机融合也是智能机器人发展的重要趋势,在物流行业中,跟随型智能机器人相较于传统的人力搬运,具有搬运效率更高、搬运能力更强、人机协同能力更友好等优点。跟随任务要求机器人在确定跟随对象后,与跟随对象保持安全距离的情况下,无碰撞地完成跟随运动任务,对避障能力、目标检测能力都有一定的要求。
现有机器人跟随技术主要可以分为基于模板匹配的跟随方法、基于估计器的跟随方法和基于分类器的跟随方法。基于模板匹配的跟随方法通过选取第一帧跟随区域的目标信息作为匹配模板,在后续帧中,选择与模板的匹配结果误差最小的区域作为匹配结果实现目标跟随,具有实现简单、收敛速度快的优点,已在物体跟踪领域得到了广泛的运用。基于估计器的跟随方法是一种通过建立目标跟随模型来预测目标对象在下一帧中的位置,并更新目标当前状态的方法,通过不同场景设计不同类型的估计器,以此来提高目标跟随的准确性。基于分类器的目标跟随方法是通过线上学习的方式,并给定的类别和已知的训练数据,在线学习分类规则,然后对未知数据进行分类,能够应对复杂的应用场景,具备较强的鲁棒性。虽然这些方法具有各种优点,但仍然面临着许多难点,例如如何建立跟踪目标的模型、如何区分背景和跟随目标、如何解决遮挡情况下跟随方法失效、如何实现跟踪过程的平滑控制等问题。
申请号为201810980715.6的专利公开了一种基于单目视觉传感器的移动机器人目标对象跟随方法,该方法根据目标对象的目标区域得到跟随对象的图像特征矩阵,再利用跟踪算法和特征矩阵确定目标对象的中心点来实现目标跟随,但该方法不能较好地解决跟随过程中目标丢失的问题。申请号202010993247.3的专利公开了一种基于视觉的机器人跟随方法及跟随机器人,该方法通过将图像信息输入检测神经网络进行检测获取行人检测结果,再根据目标结果对机器人进行跟随控制,但该方法仅使用了单一传感器获取环境信息,容易受到环境因素的干扰,鲁棒性还有待提高。
发明内容
本发明解决了现有技术中存在的问题,提供了一种优化的基于SAC-PID的移动机器人目标跟随方法,采用深度学习目标检测网络检测候选跟随目标,使用深度强化学习算法SAC实现机器人的目标跟随决策,并通过PID实现平滑动作输出。
本发明所采用的技术方案是,一种基于SAC-PID的移动机器人目标跟随方法,所述方法包括以下步骤:
步骤1:以已标定的RGBD相机和激光雷达获取移动机器人目标跟随过程中的一帧实时RGBD数据IRGBD和N个点的雷达数据RN,将IRGBD拆分为RGB图像IRGB和深度图像ID,为后续目标检测和跟随对象目标匹配做准备,将RN进行k倍下采样处理,得到雷达数据RN/k,减少RN/k在后续步骤中强化学习状态空间的占例;
步骤2:使用人形数据集训练得到的目标检测网络对RGB图像IRGB中的人形目标进行检测,获取跟随对象候选区域ROIt,i;初始化时,跟随对象未确定,则将像素面积最大的候选区域ROIt,i指定为初始跟随对象OBJ0,作为后续目标匹配的基准,并返回步骤1,否则进行下一步;其中,t为当前帧的时间索引,i为候选区域索引;
此处目标检测网络主要跟随人形目标,以数据集训练得到,通过在数据集中额外增加一些本地相机拍摄的人形样本增加置信水平;
初始跟随目标未确定是指跟随算法在刚启动并获得第一帧图像之后确定跟随对象的过程,方便后续锁定跟随对象;基于此,由于一般跟随的目标是人,故对于一帧图像的检测结果中的几个人形检测ROI,取其中像素面积最大的框,作为摄像头中跟随的主要对象。
步骤3:对得到的所有候选区域ROIt,i进行相似度判别,从中选择相似度最大的区域作为当前帧的跟随对象OBJt,获得跟随对象OBJt在RGB图像IRGB中的位置信息lt及其与移动机器人之间的距离信息dt;解决跟随过程中疑似对象对机器人目标跟随的影响;
步骤4:基于步骤1和步骤3采集强化学习的状态信息St;将步骤1得到的激光雷达数据RN/k加入到状态空间,同时将步骤3中得到的目标跟随对象OBJt的位置信息Sl和距离信息Sd也加入到状态空间;
步骤5:将St输入已训练好的SAC强化学习模型,输出移动机器人运动控制的目标角速度ωtarget和目标线速度vtarget,实现移动机器人端到端的目标跟随决策输出;
步骤6:在迁移到现实世界的目标跟随过程中,机器人无法瞬间达到强化学习策略网络输出的目标角速度ωtarget和目标线速度vtarget,并且在迁移过程中状态观测会发生较大的变化,容易输出与前一帧差异较大的动作,动作间的割裂易导致机器人出现转向震荡,急停急启的不良现象,因此在SAC策略网络输出的分布上采样动作后,基于输出的目标跟随决策,利用PID控制器对移动机器人进行平滑控制。
优选地,所述步骤1中,k的取值范围为{2,3,4}。
优选地,所述步骤3包括以下步骤:
步骤3.1:将得到的所有候选区域ROIt,i都缩放到与上一帧中跟随对象OBJt-1相同大小,分别计算每个候选区域ROIt,i与上一帧中跟随对象OBJt-1的相似度Qi;Qi表示当前帧中第i个候选区域ROIt,i与上一帧中跟随对象OBJt-1的总体相似度;
步骤3.2:以Qi最大的候选区域ROIt,i作为当前帧的跟随对象OBJt,并确定其在RGB图像IRGB中的位置信息lt,并根据Qi最大的候选区域选定对应的dROI,i作为当前帧的跟随对象OBJt与机器人之间的距离dt。
优选地,Qi=α×Qdistance+(1-α)×Qhistogram,其中,Qdistance表示候选区域ROIt,i与上一帧中跟随对象OBJt-1在距离方面的相似度,Qhistogram表示候选区域ROIt,i与上一帧中跟随对象OBJt-1在直方图方面的相似度,α表示Qdistance的权重系数,其取值范围为[0.2,0.5];Qdistance和Qhistogram的数值范围均在0到1之间,Qdistance值越大说明待比较对象与机器人的距离越接近上一帧中跟随对象与机器人的距离,Qhistogram值越大说明相似度越高,当两张图像完全相同时,其值为1。
优选地,在每个候选区域ROIt,i的中心位置确定一个m×n的RGB像素区域,然后通过RGB图IRGB与深度图像ID的对应关系计算该m×n像素区域的深度平均值,并将其作为该候选区域ROIt,i与机器人之间的距离dROI,i,并将根据相似度Qi,从所有(有限个)候选区域中选择相似度最大者的dROI,i作为当前帧的机器人与跟随目标距离dt;dt-1表示上一帧中跟随对象OBJt-1与移动机器人的距离;drow,col表示深度图像中候选区域中心坐标附近位置对应的深度值。
优选地, Hox(z)、Hoy(c)分别是上一帧中跟随对象OBJt-1在水平、垂直方向上的投影直方图的取值,Qhistogram通过从水平、竖直两个方向计算当前候选区域ROIt,i与上一帧中的目标对象OBJt-1的投影直方图相似度计算得到;Hx(z)、Hy(c)分别是缩放后的当前帧中候选区域ROIt,i在X轴、Y轴方向上的投影直方图的取值;z为水平直方图分量索引,c为竖直直方图分量索引;w、h分别表示候选区域ROIt,i的宽和高;
Hx(z)、Hy(c)首先是将每一个候选区域ROIt,i中每个像素位置的R、G、B三通道像素值进行均值化处理,然后计算候选区域ROIt,i在水平X轴和竖直Y轴两个方向上的垂直投影直方图,为避免ROI区域大小对直方图相似度产生影响,在计算投影直方图时,对累加和也进行了均值化处理,Rz,c、Gz,c、Bz,c为z行c列位置的三通道像素值,Hox(z)、Hoy(c)的计算方法同Hx(z)、Hy(c)。
优选地,所述步骤4中,当前时刻的状态信息St={RN/k,Sl,Sd}St={RN/k,Sl,Sd},其中,Sl={lt-4,lt-3,lt-2,lt-1,lt},当Sl队列未满足时则等待,Sd即当前帧中跟随对象OBJt与移动机器人之间的距离dt;
此处将步骤1中得到的雷达距离数据RN/k作为状态空间的一部分,在每次雷达距离数据RN/k发生变化后,都需要更新状态空间中的该部分数值;
通过步骤3确定当前帧中的跟随对象OBJt的位置信息Sl和距离信息Sd,对于跟随对象OBJt的位置状态信息Sl是将前后五帧的RGB图像在步骤3得到的跟随对象位置坐标信息lt进行组合得到的。
优选地,所述SAC强化学习模型以并置的Q值网络和策略网络及设置的奖励函数进行训练;
策略网络中,将状态信息作为输入,通过全连接神经网络后输出动作分布的均值和方差,使用均值和方差构建动作的高斯分布,通过采样得到最终的动作信息;
策略网络的目标是在已知状态的前提下,策略网络采取的动作能够实现最大化期望奖励与期望熵之和,也就是策略网络的训练目标为获得最大化状态价值Qπ(s,a)为状态动作价值,α为熵正则化系数,π(a|s)为状态s下采取动作a的概率;α的取值可以由本领域技术人员基于需求自行设置。
为了加快策略网络的收敛,SAC算法采用重新参数化技巧,将策略网络的输出作为均值和方差实现输出动作的高斯分布,并在其中增加随机噪声,最后将采样到的动作进行扁平化操作,使动作在有效范围内;策略中,动作选择公式为ξ~N(0,I),其中a′(s,ξ)表示在带有噪声的高斯分布下随机采样得到的动作,ξ为服从均值为0、方差为I的高斯分布,μθ(s)表示策略网络输出的均值,σθ(s)表示策略网络输出的标准差。
Q值网络中,将状态信息和所述动作信息作为输入,将两者拼接后通过神经网络获得相应状态-动作对的Q值;
对于Q值网络的学习通过均方贝尔曼误差函数实现,损失函数为Lv(D)=E(s,a,r,s',done)~D(Qv(s,a)-y)2,v=1,2,其中,Lv(D)是第v个当前Q值网络的损失函数,D为经验池,(s,a,r,s’,done)为从经验池中采样得到的一条经验,该经验记录了从某一状态s,采取策略网络输出的动作a后获得了奖励r,并到达了下一状态s'的过程,done表示是否是终止状态的标志位(终止状态没有下一状态);y表示目标Q值,其中a'由策略网络根据s'得到,从两个目标Q值网络中选取较小的Qi(s′,a′),r为当前奖励,γ为衰减因子。通过目标Q值网络构建目标值供当前Q值网络更新,并通过当前Q值网络指导策略网络更新,实现SAC的训练过程。
由于本发明构建了两个当前Q值网络和两个目标Q值网络以及一个策略网络,构建两个Q值网络的目的是避免目标Q值过估计,这两个网络在构筑目标Q值时,选择了估计较低者,但用这个较低的yv来更新两个网络,而对于策略网络而言,则选取当前Q网络中较小者(v=1,2)进行更新;在目标Q值网络定义了两个的前提下,目标Q值通过目标Q网络输出和奖励r构成,Q(s,a)表示当前Q值网络根据状态s动作a给出的估计,Q’(s’,a’)为目标网络根据下一状态s’和策略网络根据状态s’的动作输出a’的估计;γ主要表示状态动作价值间的关联性,一般取0.8~0.99。
优选地,所述奖励函数r=rcollision+rd_obstacle+rd_object+rlocation,奖励函数的定义主要为了帮助智能体进行有效的训练,在移动机器人目标跟随方法中奖励函数的大小主要由四部分组成,分别为机器人是否与障碍物产生碰撞的惩罚rcollision,机器人与最近障碍物之间的距离奖励rd_obstacle,机器人与跟随目标之间的距离奖励rd_object,跟随对象在机器人视野中的位置奖励rlocation;
rcollision为碰撞惩罚,是指机器人在运行过程中与障碍物或是墙体产生碰撞后给予的一定惩罚,惩罚的大小与机器人到障碍物距离成反比,rcollision=-10×collision,以collision为碰撞标志位,取值为1或0,1代表碰撞,0代表未碰撞;
rd_obstacle为移动机器人与最近障碍物之间的距离奖励,是防止机器人在跟随过程中与障碍物或墙体靠得太近,奖励值的大小与障碍物的距离成反比,以dobstacle为移动机器人与最近的障碍物之间的距离,d1为离障碍物的安全距离,应当根据机器人尺寸做出调整,单位为米,
rd_object为移动机器人与跟随对象之间的距离奖励,是为了使机器人能够与跟随对象保持有效距离范围,在设定范围内可获得正向奖励,而超出这个范围将会受到惩罚,本发明实现的目标跟踪距离是d2~d3,以dt为移动机器人与跟随对象之间的距离,dmax是机器人与跟随对象之间的最大距离,若是大于该距离,则认定此次跟随失败,
rlocation为跟随对象在移动机器人视野中的奖励,是为了保证机器人一直在跟随对象后面,若存在偏差能够及时进行调整,
其中,ω是移动机器人当前的角速度,ωmax是移动机器人能够达到的最大角速度,w为相机的横向分辨率大小,x是跟随对象的中心坐标。
优选地,所述步骤6中,离散化的PID公式为,其中T为调节周期,比例项P被表示为Kp,积分项I被表示为微分项D被表示为Ti和Td分别为积分时间和微分时间,k为执行次数,ek为当前帧的实际值和设定值的误差,为累计误差,将累计误差求和,消除静差;在机器人跟随任务中,PID控制器的控制目标是平滑当前帧与前一帧的动作输出,避免震荡,因此误差值ek设为当前速度和强化学习策略网络给出动作的差值,即借助比例项快速调节、积分项消除累计速度误差、微分项增加系统稳定性,实现平滑到达策略网络输出动作设定值的目的;实际速度和目标速度的误差经由离散化的PID公式的输出记作vPID与ωPID,作为机器人执行机构的输入。
本发明涉及一种基于SAC-PID的移动机器人目标跟随方法,获取机器人感知数据并预处理,使用目标检测网络获取跟随对象候选区域,跟随对象目标匹配后,采集强化学习的状态信息,以已训练好的SAC强化学习模型,输出移动机器人运动控制的目标角速度和目标线速度,实现移动机器人端到端的目标跟随决策输出,利用PID控制器对移动机器人进行平滑控制。
本发明的有益效果在于,建立跟踪目标的模型,有效区分背景和跟随目标,遮挡情况下跟随方法可以继续应用,实现简单,收敛速度快,目标跟随的准确性高,能够应对复杂的应用场景,具备较强的鲁棒性,实现跟踪过程的平滑控制。
附图说明
图1为本发明的流程框图;
图2为本发明中跟随对象目标匹配流程图;
图3为本发明中SAC决策神经网络及末端PID控制器结构的示意图。
具体实施方式
下面结合实施例对本发明做进一步的详细描述,但本发明的保护范围并不限于此。
本发明方法的实施基于机器人操作系统ROS操作平台,机器人的转向结构是两轮差速,所使用的传感器主要包括Intel RealSense D455深度相机、Delta-3A激光雷达和光电编码器。双轮差速机器人上搭载的计算单元为英伟达公司的AI边缘计算平台JestonXavier NX,操作系统为Ubuntu18.04+ROS Melodic。
如图1所示,一种结合SAC和PID控制的移动机器人目标跟随方法,包括以下步骤:
(1)获取机器人感知数据并预处理;
(2)使用目标检测网络获取跟随对象候选区域;
(3)跟随对象目标匹配;
(4)强化学习状态信息采集;
(5)机器人目标跟随决策;
(6)基于PID的动作控制。
步骤(1)具体包括:
本发明采用已标定好的RGBD相机和激光雷达获取移动机器人目标跟随过程中的状态信息,使用RGBD相机采集一帧实时RGBD数据IRGBD,并将其拆分为RGB图像IRGB和深度图像ID,为后续目标检测和跟随对象目标匹配做准备;使用激光雷达扫描机器人周围一周得到N个点的雷达距离数据RN,并将N个点的雷达数据RN进行k倍下采样处理得到RN/k,k的取值范围为{2,3,4},以减少RN/k在后续步骤中强化学习状态空间的占例。
步骤(2)具体包括:
使用已训练好的深度学习目标检测网络模型对RGB图像IRGB进行人形目标检测,得到跟随对象候选区域ROIt,i,其中t为当前帧的时间索引,i为候选区域索引;初始化时,跟随对象未确定,则直接将像素面积最大的人形检测候选区域ROIt,i指定为初始跟随对象OBJ0,作为后续目标匹配的基准,并回到步骤(1);若初始跟随对象已确定,则跳到下一步。
为了得到跟随对象在RGB图像IRGB中的准确位置信息,本发明使用YOLOv5s作为目标检测网络,并采用旷视发布的基准数据集和数据增强方法对检测数据集进行扩充,使检测网络能够得到更好的检测效果。YOLOv5s检测网络训练具体步骤如下:
(2-1)为了适应机器人跟随运行环境的复杂性和跟随对象的广泛性,选择旷视发布的基准数据集CrowdHuman作为本发明目标检测数据集,该数据集包含大量数据,并且标注内容十分丰富。同时为了增加数据集的种类与样式,使用Mosaic数据增强技术对其进行数据增广,通过选取数据集中的任意四张图片进行翻转、缩放、色域变换、剪切操作,然后再将其进行拼接组合成一张图片的方式增强数据的表达能力。
(2-2)使用CrowdHuman基准数据集和Mosaic数据增强方法处理后的图像对YOLOv5s检测网络进行训练,调整网络参数,训练模型至平均精度mAP大于95%时完成训练。然后使用步骤(1)中得到的RGB三通道图像进行目标检测,如果当前RGB图像中没有检测到目标对象,则直接跳至步骤(4),否则进行步骤(3)。
步骤(3)具体包括:
为了解决跟随过程中疑似对象对机器人的影响,利用上一帧的跟随对象OBJt-1对步骤(2)检测得到的候选区域ROIt,i进行相似度判别,判别过程如图2所示,从中选择相似度最大的区域作为当前帧的跟随对象OBJt,具体判别方法如下:
(3-1)使用步骤(2)检测得到的候选区域ROIt,i计算其域与上一帧中的目标跟随对象OBJt-1的相似度。首先将候选区域ROIt,i缩放到与上一帧中跟随对象OBJt-1相同大小,再计算每一个候选区域ROIt,i与上一帧中跟随对象OBJt-1的相似度,具体计算公式如下:
Qi=α×Qdistance+(1-α)×Qhistogram (1)
其中,Qi表示当前帧中第i个候选区域ROIt,i与上一帧中跟随对象OBJt-1的总体相似度;Qdistance表示该候选区域ROIt,i与上一帧中跟随对象OBJt-1在距离方面的相似度;Qhistogram表示该候选区域ROIt,i与上一帧中跟随对象OBJt-1在直方图方面的相似度;α表示Qdistance在整体相似度对比中占据的比例,其取值范围为α∈[0.2,0.5]。
所述距离相似度Qdistance的计算首先是在每个候选区域ROIt,i的中心位置确定一个m×m的像素区域,在本实施过程中取3×3,然后通过RGB图IRGB与深度图像ID的对应关系计算该3×3像素区域的深度平均值,并将其作为该候选区域ROIt,i与机器人之间的距离,最后再计算与上一帧中对象的距离相似度,具体计算公式如下:
其中,dt-1表示上一帧中跟随对象OBJt-1与机器人的距离;drow,col表示深度图像中候选区域中心坐标附近位置对应的深度值,中心附近m×m的像素区域可大致表征该候选区域与机器人的距离,深度均值记为dROI,i;Qdistance的数值范围为0到1之间,其值越大说明待比较对象与机器人的距离越接近上一帧中跟随对象与机器人的距离。
所述直方图相似度的计算首先是将每一个候选区域ROIt,i的三通道像素值进行均值化处理,然后从X轴和Y轴两个方向计算当前候选区域ROIt,i与上一帧中的目标对象OBJt-1的直方图相似度,具体计算公式如下:
其中,Qhistogram的数值范围为[0,1],其值越大说明相似度越高,当两张图像完全相同时,值为1;z为水平投影直方图分量索引,c为竖直投影直方图分量索引;w、h分别表示该候选区域ROIt,i的宽和高;Hx(z)、Hy(c)分别是缩放后的当前帧中候选区域ROIt,i在X轴、Y轴方向上的投影直方图的取值,其计算分别按式(4)和(5),Hox(z)、Hoy(c)的计算方法与Hx(z)、Hy(c)相同:
(3-2)通过上述步骤计算得到各个候选区域的相似度Qi,选择相似度最高的候选区域为当前帧的跟随对象OBJt,并确定其在RGB图像IRGB中的位置信息lt,然后将相似度最大区域的dROI,i作为当前帧中跟随对象OBJt与机器人之间的距离dt;所述位置信息lt包括在前后五帧RGB图像IRGB中的检测得到的跟随对象在该图像中的位置坐标信息。
步骤(4)具体包括:
SAC决策算法的输入状态St主要是由激光雷达数据RN/k、目标跟随对象OBJt的位置信息lt和距离信息dt组成,具体表示如式(6)所示。
St={RN/k,Sl,Sd} (6)
其中,St表示当前时刻的状态信息;Sl表示当前时刻跟随对象OBJt的位置状态信息;Sd表示当前时刻机器人与跟随对象OBJt的距离状态信息。
具体状态信息的采集步骤如下:
(4-1)对于激光雷达数据RN/k是在步骤(1)中得到,并将其作为状态SAC算法状态空间的一部分,该部分状态信息在每次机器人与环境交互之后对该部分信息进行更新;
(4-2)对于跟随对象OBJt的位置状态信息Sl是将前后五帧的RGB图像在步骤(3)得到的跟随对象位置坐标信息lt进行组合得到,具体表示如式(7)所示,当缓存队列未满时,则等待缓存队列满再开始,若检测对象丢失,则沿用上一帧位置信息。
Sl={lt-4,lt-3,lt-2,lt-1,lt} (7)
(4-3)对于跟随对象OBJt的距离状态信息Sd是通过获取步骤(3)中跟随对象OBJt所对应的相似度最高候选区域的dROI,i作为当前帧跟随目标与机器人的距离dt,若检测对象丢失,则沿用上一帧位置信息。
步骤(5)具体包括:
本发明通过将步骤(4)中的智能体状态信息St输入到已训练好的SAC强化学习模型,输出机器人运动控制的目标角速度ω和目标线速度v,实现机器人端到端的目标动作设定值输出。所述SAC强化学习模型的具体训练步骤如下:
通过步骤(4)得到移动机器人目标跟随状态信息St,并根据状态St的特征和目标跟随控制过程设计SAC强化学习模型的神经网络和奖励函数,最后再对SAC强化学习模型进行训练得到机器人目标跟随控制模型。
(5-1)SAC算法的神经网络结构如图3所示,该网络结构主要分为Q值网络和策略网络。策略网络的输入是机器人传感器采集到的状态信息,经过三层全连接网络后输出动作高斯分布的均值和方差,然后使用其构建动作的高斯分布,最后的动作输出是对该高斯分布进行采样并通过Tanh函数使输出的动作在指定的范围内。Q值网络的输入由传感器采集的状态信息和策略网络输出的动作构成,两者分别输入到全连接层,然后再进行维度拼接,最后通过四层全连接后输出Q值。
(5-2)奖励函数的定义主要为了帮助智能体进行有效的训练,在移动机器人目标跟随方法中奖励函数的大小主要由四部分组成,分别为机器人是否与障碍物产生碰撞的惩罚rcollision,机器人与最近障碍物之间的距离奖励rd_obstacle,机器人与跟随目标之间的距离奖励rd_object,跟随对象在机器人视野中的位置奖励rlocation,具体计算公式如下:
r=rcollision+rd_obstacle+rd_object+rlocation (8)
碰撞惩罚rcollision指是机器人在运行过程中与障碍物或是墙体产生碰撞后给予的一定惩罚,惩罚的大小与机器人到障碍物距离成反比,具体计算公式如下:
rcollision=-10×collision (9)
其中,collision为碰撞标志位,取值为1或0,1代表碰撞,0代表未碰撞。
机器人与最近障碍物之间的距离奖励rd_obstacle,是防止机器人在跟随过程中与障碍物或墙体靠得太近,奖励值的大小与障碍物的距离成反比,具体计算公式如下:
其中,dobstacle为机器人最近的障碍物之间的距离,d1为设定的与障碍物的安全距离,在本实施过程中取0.5米。
机器人与跟随对象之间的距离奖励rd_object,设定目标跟随距离在d2~d3之间,根据需求选定距离,保持跟随目标在此距离范围内,智能体可获得正向奖励,而超出这个范围将会受到惩罚,机器人最终表现为会尽量保持在这个范围内,跟踪过程中距离太远不利于目标检测,故一般取d2为1.5、d3为2.5,奖励具体计算公式为:
其中,dt是机器人与跟随对象之间的距离;dmax是机器人与跟随对象之间的最大距离,一般为7.5米,若是大于该距离,则认定此次跟随失败。
跟随对象在机器人视野中的奖励rlocation,是为了保证机器人一直在跟随对象后面,若存在偏差能够及时进行调整,具体计算公式如下:
其中,ω是机器人当前的角速度;ωmax是机器人能够达到的最大角速度;w为相机的横向分辨率大小;x是跟随对象的中心坐标。
(5-3)根据步骤(4)得到的状态信息和步骤(5-1)和(5-2)设计的神经网络和奖励函数对SAC策略算法进行训练,并且在计算状态动作函数中增加了熵正则化,通过增加熵的方式来加强策略的随机性,进而提升策略后续学习的速度,并且可以避免策略陷入局部最优的情况。在SAC算法中需要同时学习一个策略网络和两个当前Q值网络,对于Q网络的学习是通过均方贝尔曼误差函数来实现,具体损失函数如下:
Lv(D)=E(s,a,r,s',done)~D(Qv(s,a)-y)2,v=1,2 (13)
其中,Lv(D)是第v个Q值网络的损失函数;D为经验池;y是目标Q值,(s,a,r,s',d)为从经验池中采样得到的一条经验,该经验记录了从某一状态s,采取动作a后获得了奖励r,并到达了下一状态s'的过程,done表示是否为终止状态的标志位。具体计算公式如下:
从两个目标Q值网络中选取较小的Qv'(s′,a′);r是当前奖励;γ为衰减因子,通过目标Q值网络构建目标值供当前Q值网络学习更新。
策略网络的目标是在已知状态的前提下,策略网络采取的动作能够实现最大化期望奖励与期望熵之和,也就是最大化状态价值V,通过当前Q值网络指导策略网络更新。状态价值V的计算公式为:
其中,Qπ(s,a)为状态动作价值,α为熵正则化系数,π(a|s)为状态s下采取动作a的概率。
同时,为了加快策略网络的收敛,SAC采用重新参数化技巧,将策略网络的输出作为均值和方差实现输出动作的高斯分布,并在其中增加随机噪声,最后将采样到的动作进行扁平化操作,使动作在有效范围内。所以,动作选择公式为:
其中a′(s,ξ)表示在带有噪声的高斯分布下随机采样得到的动作,ξ为服从均值为0、方差为I的高斯分布,μθ(s)表示策略网络输出的均值,σθ(s)表示策略网络输出的标准差。
步骤(6)具体包括:
(6)基于PID的动作控制
为确保机器人的执行机构能够稳定实现跟踪任务,在动作执行的输出端添加一个PID控制器。将雷达相机获取机器人所处场景的观测信息输入策略网络即可得到目标角速度ωtarget和目标线速度vtarget。借助光电编码器获得两轮转动量,结合两轮差速模型易得到当前角速度ωactual与当前线速度vactual。获取了设定值与实际值,即可利用PID来进行平滑控制。离散化的PID公式为:
Claims (10)
1.一种基于SAC-PID的移动机器人目标跟随方法,其特征在于:所述方法包括以下步骤:
步骤1:以已标定的RGBD相机和激光雷达获取移动机器人目标跟随过程中的一帧实时RGBD数据IRGBD和N个点的雷达数据RN,将IRGBD拆分为RGB图像IRGB和深度图像ID,将RN进行k倍下采样处理,得到雷达数据RN/k;
步骤2:使用人形数据集训练得到的目标检测网络对RGB图像IRGB中的人形目标进行检测,获取跟随对象候选区域ROIt,i;初始化时,跟随对象未确定,则将像素面积最大的候选区域ROIt,i指定为初始跟随对象OBJ0,作为后续目标匹配的基准,并返回步骤1,否则进行下一步;其中,t为当前帧的时间索引,i为候选区域索引;
步骤3:对得到的所有候选区域ROIt,i进行相似度判别,从中选择相似度最大的区域作为当前帧的跟随对象OBJt,获得跟随对象OBJt在RGB图像IRGB中的位置信息lt及其与移动机器人之间的距离信息dt;
步骤4:基于步骤1和步骤3采集强化学习的状态信息St;
步骤5:将St输入已训练好的SAC强化学习模型,输出移动机器人运动控制的目标角速度ωtarget和目标线速度vtarget,实现移动机器人端到端的目标跟随决策输出;
步骤6:基于输出的目标跟随决策,利用PID控制器对移动机器人进行平滑控制。
2.根据权利要求1所述的一种基于SAC-PID的移动机器人目标跟随方法,其特征在于:所述步骤1中,k的取值范围为{2,3,4}。
3.根据权利要求1所述的一种基于SAC-PID的移动机器人目标跟随方法,其特征在于:所述步骤3包括以下步骤:
步骤3.1:将得到的所有候选区域ROIt,i都缩放到与上一帧中跟随对象OBJt-1相同大小,分别计算每个候选区域ROIt,i与上一帧中跟随对象OBJt-1的相似度Qi;
步骤3.2:以Qi最大的候选区域ROIt,i作为当前帧的跟随对象OBJt,并确定其在RGB图像IRGB中的位置信息lt,并根据Qi最大的候选区域选定对应的dROI,i作为当前帧的跟随对象OBJt与机器人之间的距离dt。
4.根据权利要求3所述的一种基于SAC-PID的移动机器人目标跟随方法,其特征在于:Qi=α×Qdistance+(1-α)×Qhistogram,
其中,Qdistance表示候选区域ROIt,i与上一帧中跟随对象OBJt-1在距离方面的相似度,Qhistogram表示候选区域ROIt,i与上一帧中跟随对象OBJt-1在直方图方面的相似度,α表示Qdistance的权重系数,其取值范围为[0.2,0.5];Qdistance和Qhistogram的数值范围均在0到1之间。
7.根据权利要求1所述的一种基于SAC-PID的移动机器人目标跟随方法,其特征在于:所述步骤4中,当前时刻的状态信息St={RN/k,Sl,Sd},其中,Sl={lt-4,lt-3,lt-2,lt-1,lt},当Sl队列未满足时则等待,令当前帧中跟随对象OBJt与移动机器人之间的距离dt为Sd。
8.根据权利要求1所述的一种基于SAC-PID的移动机器人目标跟随方法,其特征在于:所述SAC强化学习模型以并置的Q值网络和策略网络及设置的奖励函数进行训练;
策略网络中,将状态信息作为输入,通过全连接神经网络后输出动作分布的均值和方差,使用均值和方差构建动作的高斯分布,通过采样得到最终的动作信息;
策略网络的训练目标为获得最大化状态价值Vπ(s),
动作选择公式为ξ~N(0,I),其中,a′(s,ξ)表示在带有噪声的高斯分布下随机采样得到的动作,ξ为服从均值为0、方差为I的高斯分布,μθ(s)表示策略网络输出的均值,σθ(s)表示策略网络输出的标准差;
Q值网络中,将状态信息和所述动作信息作为输入,将两者拼接后通过神经网络获得相应状态-动作对的Q值;
对于Q值网络的学习通过均方贝尔曼误差函数实现,损失函数为Lv(D)=E(s,a,r,s',done)~D(Qv(s,a)-y)2,v=1,2,其中,Lv(D)是第v个当前Q值网络的损失函数,D为经验池,(s,a,r,s’,done)为从经验池中采样得到的一条经验,记录了从某一状态s,采取策略网络输出的动作a后获得了奖励r,并到达了下一状态s'的过程,done表示是否是终止状态的标志位;yi是目标Q值,
9.根据权利要求1所述的一种基于SAC-PID的移动机器人目标跟随方法,其特征在于:所述奖励函数r=rcollision+rd_obstacle+rd_object+rlocation,rcollision为碰撞惩罚,rcollision=-10×collision,以collision为碰撞标志位,1代表碰撞,0代表未碰撞;
rd_obstacle为移动机器人与最近障碍物之间的距离奖励,以dobstacle为移动机器人与最近的障碍物之间的距离,单位为米,
rd_object为移动机器人与跟随对象之间的距离奖励,以dobject为移动机器人与跟随对象之间的距离,dmax是机器人与跟随对象之间的最大距离,
rlocation为跟随对象在移动机器人视野中的奖励,
其中,ω是移动机器人当前的角速度,ωmax是移动机器人能够达到的最大角速度,w为相机的横向分辨率大小,x是跟随对象的中心坐标。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210912958.2A CN115457075A (zh) | 2022-07-31 | 2022-07-31 | 一种基于sac-pid的移动机器人目标跟随方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210912958.2A CN115457075A (zh) | 2022-07-31 | 2022-07-31 | 一种基于sac-pid的移动机器人目标跟随方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115457075A true CN115457075A (zh) | 2022-12-09 |
Family
ID=84295904
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210912958.2A Pending CN115457075A (zh) | 2022-07-31 | 2022-07-31 | 一种基于sac-pid的移动机器人目标跟随方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115457075A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117226810A (zh) * | 2023-11-13 | 2023-12-15 | 华侨大学 | 一种绳索载重并联机器人及其避障方法、装置、存储介质 |
-
2022
- 2022-07-31 CN CN202210912958.2A patent/CN115457075A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117226810A (zh) * | 2023-11-13 | 2023-12-15 | 华侨大学 | 一种绳索载重并联机器人及其避障方法、装置、存储介质 |
CN117226810B (zh) * | 2023-11-13 | 2024-02-02 | 华侨大学 | 一种绳索载重并联机器人及其避障方法、装置、存储介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109800689B (zh) | 一种基于时空特征融合学习的目标跟踪方法 | |
CN114384920A (zh) | 一种基于局部栅格地图实时构建的动态避障方法 | |
Jia et al. | Sensor fusion-based visual target tracking for autonomous vehicles with the out-of-sequence measurements solution | |
CN106780631A (zh) | 一种基于深度学习的机器人闭环检测方法 | |
Taylor et al. | Fusion of multimodal visual cues for model-based object tracking | |
Engel et al. | Deep object tracking on dynamic occupancy grid maps using rnns | |
CN113903011A (zh) | 一种适用于室内停车场的语义地图构建及定位方法 | |
CN115205339A (zh) | 基于改进yolox的移动机器人目标跟随方法 | |
Chen | Monocular vision-based obstacle detection and avoidance for a multicopter | |
CN111258311A (zh) | 一种基于智能视觉的井下移动机器人避障方法 | |
CN114120075A (zh) | 一种融合单目相机和激光雷达的三维目标检测方法 | |
CN114708293A (zh) | 基于深度学习点线特征和imu紧耦合的机器人运动估计方法 | |
CN114964236A (zh) | 一种针对地下停车场环境的建图与车辆定位系统及方法 | |
CN115457075A (zh) | 一种基于sac-pid的移动机器人目标跟随方法 | |
CN113593035A (zh) | 一种运动控制决策生成方法、装置、电子设备及存储介质 | |
CN117519168A (zh) | 一种基于视觉图像和激光雷达的机器人导航避障方法 | |
CN115565130A (zh) | 一种无人值守系统及其基于光流的监控方法 | |
CN108469729B (zh) | 一种基于rgb-d信息的人体目标识别与跟随方法 | |
JP2007280387A (ja) | 物体移動の検出方法及び検出装置 | |
US11467592B2 (en) | Route determination method | |
CN113031002A (zh) | 基于Kinect3和激光雷达的SLAM陪跑小车 | |
Goyat et al. | Tracking of vehicle trajectory by combining a camera and a laser rangefinder | |
CN116777956A (zh) | 基于多尺度航迹管理的运动目标筛选方法 | |
CN113673462B (zh) | 一种基于车道线的物流agv定位方法 | |
CN108534797A (zh) | 一种实时高精度视觉里程计方法 |
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 |