(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210912958.2 (22)申请日 2022.07.31 (71)申请人 浙江工业大 学 地址 310014 浙江省杭州市拱 墅区潮王路 18号 (72)发明人 朱威　洪力栋　谯先锋　郑雅羽　 何德峰　 (74)专利代理 机构 杭州赛科专利代理事务所 (普通合伙) 33230 专利代理师 宋飞燕 (51)Int.Cl. G06T 7/246(2017.01) G06V 10/25(2022.01) G06V 10/82(2022.01) G06V 40/10(2022.01)G06V 10/74(2022.01) G06V 10/26(2022.01) G06T 7/73(2017.01) G06T 7/62(2017.01) B25J 9/16(2006.01) (54)发明名称 一种基于SA C-PID的移动机器人目标跟随方 法 (57)摘要 本发明涉及一种基于SAC ‑PID的移动机器人 目标跟随方法， 获取机器人感知数据并预处理， 使用目标检测网络获取跟随对象候选区域， 跟随 对象目标匹配后， 采集强化学习的状态信息， 以 已训练好的SAC强化学习模型， 输出移动机器人 运动控制的目标角速度和目标线速度， 实现移动 机器人端到端的目标跟随决策输出， 利用PID控 制器对移动机器人进行平滑控制。 本发明的有益 效果在于， 建立跟踪目标的模型， 有效区分背景 和跟随目标， 遮挡情况下跟随方法可以继续应 用， 实现简单， 收敛速度快， 目标跟随的准确性 高， 能够应对复杂的应用场景， 具备较强的鲁棒 性， 实现跟踪过程的平 滑控制。 权利要求书3页 说明书11页 附图3页 CN 115457075 A 2022.12.09 CN 115457075 A 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进行相似度判别， 从中选择相似度最大的区域作 为当前帧的跟随对象OB Jt， 获得跟随对象OB Jt在RGB图像IRGB中的位置信息lt及其与移动机 器人之间的距离信息dt； 步骤4： 基于步骤1和步骤3采集强化学习的状态信息St； 步骤5： 将St输入已训练好的SA C强化学习模型， 输出移动机器人运动控制的目标角速度 ω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所述的一种基于SA C‑PID的移动机器 人目标跟随方法， 其特征在于： Qi ＝α×Qdistance+(1‑α )×Qhistogram， 其中， Qdistance表示候选区域ROIt,i与上一帧中跟随对象OBJt‑1在距离方面的相似度， Qhistogram表示候选区域ROIt,i与上一帧中跟随对象OBJt‑1在直方图方面的相似度， α表示 Qdistance的权重系数， 其取值范围为[0.2,0.5]； Qdistance和Qhistogram的数值范围均在0到1之 间。 5.根据权利要求4所述的一种基于SAC ‑PID的移动机器人目标跟随方法， 其特征在于： 在每个候选区域ROIt,i的中心位置确定一个m ×n的RGB像素区域， 然后通过RGB图IRGB与 深度图像ID的对应关系计算该m ×n像素区域 的深度平均值， 并将其作为该候选区域ROIt,i 与机器人之间的距离dROI,i， 并根据相似度 Qi， 从所有候选区域中选择相似度最大者的dROI,i 作为当前帧的机器人与跟随目标距离dt； dt‑1表示上一帧中跟随对象OBJt‑1与移动机器人的权　利　要　求　书 1/3 页 2 CN 115457075 A 2距离； drow,col表示深度图像中候选区域中心坐标附近位置对应的深度值。 6.根据权利要求 4所述的一种基于SAC ‑PID的移动机器人目标跟随方法， 其特 征在于： Hox(z)、 Hoy(c)分别是上一帧中跟随对象OBJt‑1在水平、 垂直方向上的投影直方图的取 值； Hx(z)、 Hy(c)分别是缩放后的当前帧中候选区域ROIt,i在X轴、 Y轴方向上的投影 直方图的 取值； z为水平 直方图分量索引， c为竖直直方图分量索引； w、 h 分别表示候选区域ROIt,i的宽 和高； Rz,c、 Gz,c、 Bz,c为z行c列位置的三 通道像素值。 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)， 其中， Qπ(s,a)为状态动作价值， α为熵正则化系数， π(a| s)为状态s下采取动作a的概 率； 动作选择公式为 ξ～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值， 其中， a'由策略网络根据s'得到， 从两个目标Q值网络中 选取较小的Qi(s′,a′)， r为当前 奖励， γ 为衰减因子 。 9.根据权利要求1所述的一种基于SAC ‑PID的移动机器人目标跟随方法， 其特征在于： 所述奖励函数r＝rcollision+rd_obstacle+rd_object+rlocation， rcollision为碰撞惩罚， rcollision＝‑10权　利　要　求　书 2/3 页 3 CN 115457075 A 3