CN114637295A - 一种基于人工势场法与实例分割的机器人智能避障方法 - Google Patents

Abstract

一种基于人工势场法与实例分割算法融合的智能避障方法，包括：通过采集现实场景障碍物图片数据，使用Python脚本制作实例分割数据集；将数据集导入至Yolact算法进行训练，并制作ROS包；利用ZED2相机提取障碍物的深度信息，将实例分割的结果融合深度信息便可得到障碍物的世界坐标、类别CLASS以及MASK(遮罩)，将上述三个结果融合至改进的人工势场法中，定义目标点世界坐标，便可以实现在抢险救灾场景中有选择性的避障。本发明解决在抢险救灾场景中遇到障碍物耽误最佳抢险时间的问题，使之最快到达救援点。

Description

一种基于人工势场法与实例分割的机器人智能避障方法

技术领域

本发明涉及一种机器人智能避障方法，属于计算机视觉避障技术领域。

背景技术

利用机器人抢险救灾涉及多方面的技术，而如何快速准确的避开所有障碍物，找到通往救援点的最佳路径，是亟待解决的问题，现在主流的基于视觉的避障算法只会机械性的避开所有障碍物，比如人工势场法、A*算法等，这都极大的浪费了救援时间。现在，随着深度学习在图像识别、语音识别上的广泛应用，许多技术人员将深度学习模型广泛应用在机器人领域，在许多场景取得了不错的效果，但是就目前而言，还很少有人将深度学习方法运用到避障救援场景上来。

人工势场法是一种机器人路径规划算法。该算法将目标和障碍物分别看做对机器人有引力和斥力的物体，机器人沿引力与斥力的合力来进行运动，该算法结构简单，便于低层的实时控制，在实时避障和平滑的轨迹控制方面，得到了广泛应用。

实例分割是计算机视觉中的一个领域，相比于传统的目标检测，它是像素级分割，可以得到图像中所有物体的类别以及他们的像素级遮罩，基于这一特点，他在学术界与工业界得到了广泛的应用。

人工势场法应用于普通的避障场景中有着不错的效果，但是对于较为紧急的抢险救灾场景，人工势场法的滞后性较大，无法快速、准确的找到救援点。

发明内容

本发明要克服现有技术的上述缺点，提出一种基于人工势场法与实例分割的机器人智能避障方法。

本发明对Yolact模型的重训练，得到场景中的所有障碍物类别以及遮罩，通过双目视觉的方法得到障碍物的深度信息，在利用像素坐标系与世界坐标系的转换关系得到障碍物的世界坐标，最后利用人工势场法有选择性的避开危险障碍物，最实现最快速的到达救援点，节省救援时间。

技术方案：为了实现上述目的，本发明采用的技术方案为：

一种基于人工势场法与实例分割的机器人智能避障方法，通过ZED2相机采集障碍物图片数据，并将采集的数据集转换为COCO数据集，利用Yolact模型实时的对场景中的物体进行分割，得到其类别Class与遮罩MASK，利用双目相机ZED2实时获取各个障碍物的深度信息，深度信息矩阵与遮罩矩阵属于同纬矩阵，融合着两个矩阵，并利用像素坐标系与世界坐标系的转换关系便可以得到障碍物的世界坐标，此外，将人工势场法的势场方程与Class信息融合，便可以得到融合后势场，利用势场求出引力与斥力，并结合小车质量求出加速度，最后，求出速度，得到单位采样时间内小车需走的距离，最后，利用位置-速度转换方法得到下一采样时刻小车应行驶到的位置，至此，整个控制策略完成，具体包含以下步骤：

步骤1：采集实验所需的障碍物各个视角的RGB信息，并通过剪裁、旋转进行数据增强，对数据集进行预处理：

步骤1-1：采集室内障碍物样本图像数据并进行标注；

步骤1-2：对标注后的样本数据集进行特征增强，获得COCO数据集；

步骤1-3：构建Yolact模型；

步骤1-4：利用COCO数据集对所述的Yolact模型进行训练，得到障碍物样本的目标参数与目标权重，并将所述的目标参数和目标权重作为训练好的Yolact模型的参数和权重；

步骤1-5：使用训练好的Yolact模型对经标准化处理的障碍物样本图像进行检测，输出对应障碍物样本的类别Class与MASK矩阵M_w×h，(w，h)代表RGB图片的输入维度，定义Class_safe为安全类别，Class_danger为危险类别；

步骤2：通过ZED2相机获取障碍物的深度信息矩阵D_w×h，并通过相机的旋转矩阵R_3×3与平移矩阵T_3×1获取障碍物相对于相机的3D位姿信息，并建立小车世界坐标系base_link与相机的世界坐标系zed2_link之间的转换关系矩阵H_3×1：

步骤2-1：根据等距采样算法，获取障碍物样本各个区域像素位置的深度值z以及对应的像素坐标(x，y)；

步骤2-3：通过相机标定获取其内参矩阵Q，其Q可以由如下公式表示：

其中，f_x、f_y分别代表相机坐标系中x，y坐标轴方向上的焦距，u₀、v₀分别代表像素坐标系中u轴、v轴的像素中心；

步骤2-4：通过障碍物的像素坐标(u，v)以及其对应的深度值z将其转换为相机的世界坐标(X_ω，Y_ω，Z_ω)，其转换公式如下：

其中R，T分别为旋转矩阵与平移矩阵；

步骤2-5：基于步骤2给出的小车世界坐标系base_link与相机世界坐标系zed2_link的转换关系矩阵H_3×1可以得到，障碍物在小车世界坐标系的坐标由如下公式给出：

其中，

X_offset，Y_offset，Z_offset分别代表空间中相机与小车之间的相对位移。

步骤3：确定目标点的坐标，基于人工势场法建立模型，基于步骤1与步骤2，我们可以得到障碍物相对于小车之间的世界坐标以及其对应的类别(X_ω，Y_ω，Z_ω，Class)，在本文中，我们不考虑Z_ω坐标，我们根据救援点方向确定一个目标点坐标(X_g，Y_g)，以此来建立势场模型：

步骤3-1：建立人工势场，将小车周围环境中的运动抽象成在引力场中的运动，目标对小车产生引力，障碍物对小车产生斥力，根据叠加原理计算出合力方向，即为小车运动方向；

步骤3-2：定义目标点处的引力公式：

其中，α是引力增益，d(q，q_goal)是小车当前位置与目标点q_goal：(X_g，Y_g)之间的距离；

步骤3-3：实例分割算法Yolact将会实时计算障碍物类别Class，当障碍物类别属于步骤1-5中的Class_danger时，该类别障碍物处存在斥力，当障碍物类别属于Class_safe时，忽略斥力，因此斥力公式分配如下：

其中，d(q，q_obs)是小车当前点与障碍物之间的距离，β是斥力增益，Q是障碍物作用的距离阈值，当前点与障碍物距离大于此阈值时，障碍物对小车不会产生斥力；

步骤3-4：根据步骤3-2、3-3便可计算出无人机在任意位置处的引力与斥力，其中引力由如下公式给出：

斥力由如下公式给出：

则小车当前的合力大小为：

F(q)＝F_att(q)+F_rep(q) (8)

步骤3-5：根据里程计确定小车的实时转向角θ_time；

步骤3-6：根据3-4给出的F_att(q)、F_rep(q)便可以得到合力F_com以及相对于世界坐标系的夹角θ_expect，根据小车质量M以及加速度公式得到其期望速度v_control，根据采样时间t得到期望坐标(x_expect，y_expect)，其中v_expect、x_expect、y_expect由如下公式给出：

x_expect＝x₀+v_expect·t·cos θ_expect (10)

y_expect＝y₀+v_expect·t·sinθ_expect (11)

其中，(x₀，y₀)代表小车的初始坐标；

步骤4：建立相对位姿转换为速度模型：

步骤4-1：给定线速度v与角速度ω的控制系数(k_v，k_ω)以及速度初值(v₀，ω₀)，根据步骤3所给出的θ_expect、x_expect、y_expect以及小车当前的位姿信息θ_time、x_time、y_time建立速度转换模型，则其误差矩阵E_robot、平移矩阵T_robot可由如下公式给出：

则速度控制矩阵C_robot＝T_robot·E_robot；

步骤4-2：计算小车实时的速度，我们定义步骤4-1中的速度控制矩阵为：

则小车目标线速度v_goal、目标角速度ω_goal可由如下公式给出：

v_goal＝k_v·c₁+v₀·cos c₃ (15)

ω_goal＝k_ω·sin c₃+v₀·c₂+ω₀ (16)

优选的：步骤1所采集的各个视角的RGB图像信息是有限的，因此在现实场景中依旧会存在类别无法识别的问题。

优选的：步骤1-5所述的Class_safe与Class_danger在算法层面上是人为定义的，具有一定的随机性，考虑现实场景，Class_safe应尽可能安全。

优选的：步骤2所建立的小车世界坐标系base_link与相机的世界坐标系zed2-link之间的转换关系矩阵H_3×1的数值随相机与小车之间相对位姿的改变而改变。

优选的：步骤2-4所确定的旋转矩阵R与与平移矩阵T组成的

是相机外参，会随着障碍物与相机位姿的变化而变化。

优选的：步骤3-5中根据里程计确定实时转向角θ_time在长时间工作下会出现累计误差。

优选的：步骤3-6中小车的初始坐标(x₀，y₀)会随着小车的重启而归零。

优选的：步骤4-1中给定的线速度v与角速度ω的控制系数(k_v，k_ω)应按照小车速度上限来确定。

优选的：为保证速度控制的连续性与稳定性，步骤4-1中给定的线速度初值v₀应尽可能的小，角速度初值ω₀应设置为0。

本发明相比现有技术，具有以下有益效果：

1、实例分割技术应用于避障算法可能是在本发明中首次提出；

2、相比简单的避障算法，本文提出了基于人工势场法与实例分割的机器人智能避障方法，提高了在救援场景中的工作效率；

3、本发明提出了当小车遇到不同障碍物时的不同路径规划方法。

附图说明

图1为基于人工势场法与实例分割的机器人智能避障方法的流程图

图2为像素坐标系与世界坐标系的转换关系图；

图3为人工势场算法示意图；

图4为智能避障方法框架图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面结合附图和具体实施例，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例是本发明一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

一种基于人工势场法与实例分割的机器人智能避障方法，如图1所示，包括RGB信息与深度信息的输入、数据集制作与预处理、深度信息与实例分割结果的融合、障碍物世界坐标的获取、融合改人工势场法开始实验，最终到达目标点。

具体步骤如下：

步骤1：采集实验所需的障碍物各个视角的RGB信息，并通过剪裁、旋转进行数据增强，对数据集进行预处理：

步骤1-1：采集室内障碍物样本图像数据并进行标注；

步骤1-2：对标注后的样本数据集进行特征增强，获得COCO数据集；

步骤1-3：构建Yolact模型；

步骤1-4：利用COCO数据集对所述的Yolact模型进行训练，得到障碍物样本的目标参数与目标权重，并将所述的目标参数和目标权重作为训练好的Yolact模型的参数和权重；

步骤1-5：使用训练好的Yolact模型对经标准化处理的障碍物样本图像进行检测，输出对应障碍物样本的类别Class与MASK矩阵M_w×h，(w，h)代表RGB图片的输入维度，定义Class_safe为安全类别，Class_danger为危险类别；

步骤2：通过ZED2相机获取障碍物的深度信息矩阵D_w×h，建立像素坐标系与世界坐标系的转换关系，如图2所示，通过相机的旋转矩阵R_3×3与平移矩阵T_3×1获取障碍物相对于相机的3D位姿信息，并建立小车世界坐标系base_link与相机的世界坐标系zed2_link之间的转换关系矩阵H_3×1：

步骤2-1：根据等距采样算法，获取障碍物样本各个区域像素位置的深度值z以及对应的像素坐标(x，y)；

步骤2-3：通过相机标定获取其内参矩阵Q，其Q可以由如下公式表示：

其中，f_x、f_y分别代表相机坐标系中x，y坐标轴方向上的焦距，u₀、v₀分别代表像素坐标系中u轴、v轴的像素中心；

步骤2-4：通过障碍物的像素坐标(u，v)以及其对应的深度值z将其转换为相机的世界坐标(X_ω，Y_ω，Z_ω)，其转换公式如下：

其中R，T分别为旋转矩阵与平移矩阵；

步骤2-5：基于步骤2给出的小车世界坐标系base_link与相机世界坐标系zed2_link的转换关系矩阵H_3×1可以得到，障碍物在小车世界坐标系的坐标由如下公式给出：

其中，

X_offset，Y_offset，Z_offset分别代表空间中相机与小车之间的相对位移。

步骤3：确定目标点的坐标，基于人工势场法建立模型，如图3所示，基于步骤1与步骤2，我们可以得到障碍物相对于小车之间的世界坐标以及其对应的类别(X_ω，Y_ω，Z_ω，Class)，在本文中，我们不考虑Z_ω坐标，我们根据救援点方向确定一个目标点坐标(X_g，Y_g)，以此来建立势场模型：

步骤3-1：建立人工势场，将小车周围环境中的运动抽象成在引力场中的运动，目标对小车产生引力，障碍物对小车产生斥力，根据叠加原理计算出合力方向，即为小车运动方向；

步骤3-2：定义目标点处的引力公式：

其中，α是引力增益，d(q，q_goal)是小车当前位置与目标点q_goal：(X_g，Y_g)之间的距离；

步骤3-3：实例分割算法Yolact将会实时计算障碍物类别Class，当障碍物类别属于步骤1-5中的Class_danger时，该类别障碍物处存在斥力，当障碍物类别属于Class_safe时，忽略斥力，基于此，建立智能避障模型，如图4所示，在此模型下，其斥力公式分配如下：

其中，d(q，q_obs)是小车当前点与障碍物之间的距离，β是斥力增益，Q是障碍物作用的距离阈值，当前点与障碍物距离大于此阈值时，障碍物对小车不会产生斥力；

步骤3-4：根据步骤3-2、3-3便可计算出无人机在任意位置处的引力与斥力，其中引力由如下公式给出：

斥力由如下公式给出：

则小车当前的合力大小为：

F(q)＝F_att(q)+F_rep(q) (8)

步骤3-5：根据里程计确定小车的实时转向角θ_time；

步骤3-6：根据步骤3-4给出的F_att(q)、F_rep(q)便可以得到合力F_com以及相对于世界坐标系的夹角θ_expect，根据小车质量M以及加速度公式得到其期望速度v_control，根据采样时间t得到期望坐标(x_expect，y_expect)，其中v_expect、x_expect、y_expect由如下公式给出：

x_expect＝x₀+v_expect·t·cosθ_expect (10)

y_expect＝y₀+v_expect·t·sinθ_expect (11)

其中，(x₀，y₀)代表小车的初始坐标；

步骤4：建立相对位姿转换为速度模型：

步骤4-1：给定线速度v与角速度ω的控制系数(k_v，k_ω)以及速度初值(v₀，ω₀)，根据步骤3所给出的θ_expect、x_expect、y_expect以及小车当前的位姿信息θ_time、x_time、y_time建立速度转换模型，则其误差矩阵E_robot、平移矩阵T_robot可由如下公式给出：

则速度控制矩阵C_robot＝T_robot·E_robot；

步骤4-2：计算小车实时的速度，我们定义步骤4-1中的速度控制矩阵为：

则小车目标线速度v_goal、目标角速度ω_goal可由如下公式给出：

v_goal＝k_v·c₁+v₀·cos c₃(15)

ω_goal＝k_ω·sin c₃+v₀·c₂+ω₀ (16)

本发明比传统的视觉避障方法更为复杂，但是避障的速度与精度更好，对救援等场景具有较大的实用性。

以上所述是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (9)

1.一种基于人工势场法与实例分割的机器人智能避障方法，其特征在于，包括以下步骤：

步骤1：采集实验所需的障碍物各个视角的RGB信息，并通过剪裁、旋转进行数据增强，对数据集进行预处理：

步骤1-1：采集室内障碍物样本图像数据并进行标注；

步骤1-2：对标注后的样本数据集进行特征增强，获得COCO数据集；

步骤1-3：构建Yolact模型；

步骤1-4：利用COCO数据集对所述的Yolact模型进行训练，得到障碍物样本的目标参数与目标权重，并将所述的目标参数和目标权重作为训练好的Yolact模型的参数和权重；

步骤1-5：使用训练好的Yolact模型对经标准化处理的障碍物样本图像进行检测，输出对应障碍物样本的类别Class与MASK矩阵M_w×h，(w，h)代表RGB图片的输入维度，定义Class_safe为安全类别，Class_danger为危险类别；

步骤2：通过ZED2相机获取障碍物的深度信息矩阵D_w×h，并通过相机的旋转矩阵R_3×3与平移矩阵T_3×1获取障碍物相对于相机的3D位姿信息，并建立小车世界坐标系base_link与相机的世界坐标系zed2_link之间的转换关系矩阵H_3×1：

步骤2-1：根据等距采样算法，获取障碍物样本各个区域像素位置的深度值z以及对应的像素坐标(x，y)；

步骤2-3：通过相机标定获取其内参矩阵Q，其Q可以由如下公式表示：

其中，f_x、f_y分别代表相机坐标系中x，y坐标轴方向上的焦距，u₀、v₀分别代表像素坐标系中u轴、v轴的像素中心；

步骤2-4：通过障碍物的像素坐标(u，v)以及其对应的深度值z将其转换为相机的世界坐标(X_ω，Y_ω，Z_ω)，其转换公式如下：

其中R，T分别为旋转矩阵与平移矩阵；

步骤2-5：基于步骤2给出的小车世界坐标系base_link与相机世界坐标系zed2_link的转换关系矩阵H_3×1可以得到，障碍物在小车世界坐标系的坐标由如下公式给出：

其中，

X_offset，Y_offset，Z_offset分别代表空间中相机与小车之间的相对位移。

步骤3：确定目标点的坐标，基于人工势场法建立模型，基于步骤1与步骤2，得到障碍物相对于小车之间的世界坐标以及其对应的类别(X_ω，Y_ω，Z_ω，Class)，不考虑Z_ω坐标，根据救援点方向确定一个目标点坐标(X_g，Y_g)，以此来建立势场模型：

步骤3-1：建立人工势场，将小车周围环境中的运动抽象成在引力场中的运动，目标对小车产生引力，障碍物对小车产生斥力，根据叠加原理计算出合力方向，即为小车运动方向；

步骤3-2：定义目标点处的引力公式：

其中，α是引力增益，d(q，q_goal)是小车当前位置与目标点q_goal：(X_g，Y_g)之间的距离；

步骤3-3：实例分割算法Yolact将会实时计算障碍物类别Class，当障碍物类别属于步骤1-5中的Class_danger时，该类别障碍物处存在斥力，当障碍物类别属于Class_safe时，忽略斥力，因此斥力公式分配如下：

其中，d(q，q_obs)是小车当前点与障碍物之间的距离，β是斥力增益，Q是障碍物作用的距离阈值，当前点与障碍物距离大于此阈值时，障碍物对小车不会产生斥力；

步骤3-4：根据步骤3-2、3-3便可计算出无人机在任意位置处的引力与斥力，其中引力由如下公式给出：

斥力由如下公式给出：

则小车当前的合力大小为：

F(q)＝F_att(q)+F_rep(q) (8)

步骤3-5：根据里程计确定小车的实时转向角θ_time；

步骤3-6：根据3-4给出的F_att(q)、F_rep(q)便可以得到合力F_com以及相对于世界坐标系的夹角θ_expect，根据小车质量M以及加速度公式得到其期望速度v_control，根据采样时间t得到期望坐标(x_expect，y_expect)，其中v_expect、x_expect、y_expect由如下公式给出：

x_expect＝x₀+v_expect·t·cosθ_expect (10)

y_expect＝y₀+v_expect·t·sinθ_expect (11)

其中，(x₀，y₀)代表小车的初始坐标；

步骤4：建立相对位姿转换为速度模型：

步骤4-1：给定线速度v与角速度ω的控制系数(k_v，k_ω)以及速度初值(v₀，ω₀)，根据步骤3所给出的θ_expect、x_expect、y_expect以及小车当前的位姿信息θ_time、x_time、y_time建立速度转换模型，则其误差矩阵E_robot、平移矩阵T_robot可由如下公式给出：

则速度控制矩阵C_robot＝T_robot·E_robot；

步骤4-2：计算小车实时的速度，我们定义步骤4-1中的速度控制矩阵为：

则小车目标线速度v_goal、目标角速度ω_goal可由如下公式给出：

v_goal＝k_v·c₁+v₀·cos c₃ (15)

ω_goal＝k_ω·sin c₃+v₀·c₂+ω₀ (16)

根据上述步骤来设计算法便可以实现权利要求1所述功能。

2.根据权利要求1所述的基于人工势场法与实例分割的机器人智能避障方法，其特征在于：步骤1所采集的各个视角的RGB图像信息是有限的，因此在现实场景中依旧会存在类别无法识别的问题。

3.根据权利要求1所述的基于人工势场法与实例分割的机器人智能避障方法，其特征在于：步骤1-5所述的Class_safe与Class_danger在算法层面上是人为定义的，具有一定的随机性，考虑现实场景，Class_safe应尽可能安全。

4.根据权利要求1所述的基于人工势场法与实例分割的机器人智能避障方法，其特征在于：步骤2所建立的小车世界坐标系base_link与相机的世界坐标系zed2_link之间的转换关系矩阵H_3×1的数值随相机与小车之间相对位姿的改变而改变。

5.根据权利要求1所述的基于人工势场法与实例分割的机器人智能避障方法，其特征在于：步骤2-4所确定的旋转矩阵R与与平移矩阵T组成的

是相机外参，会随着障碍物与相机位姿的变化而变化。

6.根据权利要求1所述的基于人工势场法与实例分割的机器人智能避障方法，其特征在于：步骤3-5中根据里程计确定实时转向角θ_time在长时间工作下会出现累计误差。

7.根据权利要求1所述的基于人工势场法与实例分割的机器人智能避障方法，其特征在于：步骤3-6中小车的初始坐标(x₀，y₀)会随着小车的重启而归零。

8.根据权利要求1所述的基于人工势场法与实例分割的机器人智能避障方法，其特征在于：步骤4-1中给定的线速度v与角速度ω的控制系数(k_v，k_ω)应按照小车速度上限来确定。

9.根据权利要求1所述的基于人工势场法与实例分割的机器人智能避障方法，其特征在于：为保证速度控制的连续性与稳定性，步骤4-1中给定的线速度初值v₀应尽可能的小，角速度初值ω₀应设置为0。

Images