一种多无人车协同探测系统

1.本发明涉及集群控制领域，尤其涉及一种多无人车协同探测系统。


背景技术：

2.对智能集群系统的研究在近二十年蓬勃发展，在理论方面进行了广泛的研究，并建立数字仿真为主、半实物仿真为辅的验证体系。其中集群验证体系可以根据有无控制中心节点，区分为集中式系统和分布式系统，由于集中式系统的集体系架构简单，前期的集群算法验证大多围绕集中式系统开展，这类系统由全局定位系统作为感知手段(如gps、顶视相机),控制中心完成智能体的协同规划，个体只作为任务的执行者。
3.研究集群问题必然需要解决定位和导航的需求，而基于集中式的方法能辐射的实验范围有限，基于全局定位的方法(gps)在室内又受到信号影响效果差。


技术实现要素：

4.为了解决上述技术问题，本发明的目的是提供一种多无人车协同探测系统，对无人车进行集群控制，完成环境的探测。
5.本发明所采用的第一技术方案是：一种多无人车协同探测系统，包括以下步骤：
6.智能小车模块，用于搭载模块并模拟车辆运动。
7.基于单目相机相对测量模块，基于已知世界坐标的起始位置的单目相机，在运动过程中通过对贴有标记的目标持续进行测距和测角，计算目标的距离信息和智能小车的位置；
8.基于单线雷达相对测量模块，用于通过扫描二维点云，点云聚类和特征分析对智能小车进行识别定位和目标运动估计；
9.分布式集群规划控制模块，用于通过标定场地全局坐标系和小车初始位置，基于测量信息、智能小车信息和虚拟势场动态规划算法进行集群规划控制。
10.进一步，所述智能小车模块包括：
11.以stm32f103r8t6为控制核心的移动平台，用于进行车速和转向控制；
12.以nvidia jetson nano为处理核心的环境感知单元，对智能小车自身状态感知、局部环境感知和目标识别与跟踪；
13.惯导单元，同于获取智能小车自身的运动信息。
14.进一步，所述基于单目相机相对测量模块的测距步骤具体包括：
15.基于单目摄像机采集前方目标二维码的图像；
16.在区域对物体进行检测，通过矩形框二维码识别并显示，得到矩形框信息；
17.结合矩形框信息，找到该矩形框顶边的两个像平面坐标；
18.基于几何关系推导法，根据顶边的两个像平面坐标推导目标平面坐标；
19.基于目标平面坐标，通过欧氏距离公式计算出目标与智能小车的距离信息。
20.进一步，所述基于单目相机相对测量模块的测角步骤具体包括：
21.获取单目摄像机主距；
22.结合矩形框信息，找到该矩形框角点的像平面坐标；
23.根据矩形框角点的像平面坐标计算目标中心点像素坐标；
24.将目标中心点像素坐标转图像坐标
25.将目标中心点图像坐标转相机坐标；
26.基于相机坐标，通过极坐标转换计算目标的方位角和高度角。
27.进一步，所述目标的方位角和高度角计算公式如下：
[0028][0029]
上式中，θ表示方位角，表示高度角，f表示单目摄像机主距，(x，y，z)表示相机坐标。
[0030]
进一步，所述基于单线雷达相对测量模块的工作步骤具体包括：
[0031]
基于单线雷达扫描二维点云，得到点云数据；
[0032]
将点云数据排成一维向量并按照点云的序号依次对点云数据进行聚类，得到点云簇；
[0033]
对点云簇进行线段和角点提取，得到二维特征；
[0034]
基于二维特征与预设模板库内的小车外形特征匹配，计算其他小车的中心位置坐标和姿态；
[0035]
根据其他小车的中心位置坐标和姿态进行运动估计。
[0036]
进一步，还包括协同定位步骤：
[0037]
判断到存在具备定位信息的智能小车，采用绝对定位方法完成智能小车集群之间的协同定位；
[0038]
将具备定位信息的智能小车定义为锚点；
[0039]
确定锚点某个时刻的自身运动状态并测量其他智能小车的运动状态，得到锚点自身运动状态和测量状态；
[0040]
根据锚点自身运动状态和测量状态，结合卡尔曼滤波方法，更新对应智能小车的定位，得到完成定位的智能小车；
[0041]
将完成定位的智能小车定义为另一锚点并循环协同定位步骤，直至所有智能小车均完成定位。
[0042]
进一步，所述分布式集群规划控制模块的工作步骤具体包括：
[0043]
根据场地边界定义一个初始的绝对坐标系，将若干台小车放置在场地中并记录其初始坐标；
[0044]
根据智能小车自身的运动信息对智能小车位置进行更新；
[0045]
根据基于单目相机相对测量模块和基于单线雷达相对测量模块计算得到环境源产生的距离信息；
[0046]
将环境源产生的距离信息通过虚拟势场法计算成势场函数，叠加势场函数得到智能小车该时刻的期望速度；
[0047]
根据期望速度，通过pid控制器对智能小车进行控制，完成智能小车捕捉目标和动态避障的任务。
[0048]
本发明方法及系统的有益效果是：本发明通过机载的传感器进行相对测量和信息融合，实现智能小车基于自身与友邻/环境的相对位置，确定自己及环境的位置关系，并在此基础之上，对集群内的智能小车进行规划控制，完成对未知环境的探测和动态避障。
附图说明
[0049]
图1是本发明一种多无人车协同探测系统的工作示意图；
[0050]
图2是本发明具体实施例基于单目相机相对测量模块的工作步骤示意图；
[0051]
图3是本发明具体实施例摄影机成像过程示意图；
[0052]
图4是本发明具体实施例聚类的算法流程图.
具体实施方式
[0053]
下面结合附图和具体实施例对本发明做进一步的详细说明。对于以下实施例中的步骤编号，其仅为了便于阐述说明而设置，对步骤之间的顺序不做任何限定，实施例中的各步骤的执行顺序均可根据本领域技术人员的理解来进行适应性调整。
[0054]
参照图1，本发明提供了一种多无人车协同探测系统，包括：
[0055]
智能小车模块，用于搭载模块并模拟车辆运动。
[0056]
基于单目相机相对测量模块，基于已知世界坐标的起始位置的单目相机，在运动过程中通过对贴有标记的目标持续进行测距和测角，计算目标的距离信息和智能小车的位置；
[0057]
具体地，基于二维标靶的单目相机相对测量是基于已知世界坐标的起始位置单目相机，在运动过程中通过对贴有二维标记的目标持续进行测距和测角，可实现在实验过程中的自身定位和目标追踪。
[0058]
在相对测量中，相机测量获得的距离、方位角和高度角r，a，e均为球坐标量，但我们的相对需要在相机坐标系统中进行，所以其中必然涉及相机坐标系和测量(球)坐标系统的转换。距离测量和角度测量(方位角和高度角)分别进行，其中，单目相机在进行距离测量时需要已知靶标的实际尺寸，进而计算相机的焦距信息，完成对目标距离的测量。角度测量是基于已知的内方位元素相机主距f，根据跟踪目标在成像平面上的投影位置，使用投影原理，计算出高度角和方位角。图2是相机相对测量的流程图。
[0059]
基于单线雷达相对测量模块，用于通过扫描二维点云，点云聚类和特征分析对智能小车进行识别定位和目标运动估计；
[0060]
具体地，基于单线激光雷达的相对测量是通过雷达扫描二维点云，再根据点云聚类和特征分析实现对贴有白色外壳的智能自主小车进行目标识别定位，并估计目标运动，可实现在实验过程中的自身定位和相邻智能自主小车的定位姿估计。
[0061]
分布式集群规划控制模块，用于通过标定场地全局坐标系和小车初始位置，基于测量信息、智能小车信息和虚拟势场动态规划算法进行集群规划控制。
[0062]
进一步作为本方法的优选实施例，所述智能小车模块包括：
[0063]
以stm32f103r8t6为控制核心的移动平台，用于进行车速和转向控制；
[0064]
以nvidia jetson nano为处理核心的环境感知单元，对智能小车自身状态感知、局部环境感知和目标识别与跟踪；
[0065]
惯导单元，同于获取智能小车自身的运动信息。
[0066]
具体地，设计一款基于jetson nano环境感知处理器和stm32底层控制处理器的智能自主小车，该小车具备自主运动控制的功能，并针对任务需求增加环境感知拓展模块，如可见光相机、单线激光雷达、imu模块等，并配备千兆网卡，使小车能实时更新自己运动轨迹、识别贴有合作标识的目标、感知未知环境的障碍和友邻信息、通过局域网实现通信。
[0067]
对于每个智能自主小车样机，采用对小车模型进行改装来实现智能集群的功能。小车采用前轮转向、后轮驱动的动力系统结构，同时，小车的前轮与舵机相连，舵机通过一个传动结构来控制两个前轮的转向。小车的动力由一个车载的有刷直流电机提供，电机由一块全桥型电机驱动器进行驱动，能够实现前进和倒车的动作；该电机通过齿轮连接了一个旋转编码器，能够准确测量驱动电机的实时转速，进而获得车辆运动的径向速度，实现径向闭环控制。当小车运动时，由舵机转向引起的车身朝向改变信息通过车载的9轴惯性测量单元(imu)进行测量，实现侧向闭环控制。
[0068]
进一步作为本方法的优选实施例，所述基于单目相机相对测量模块的测距步骤具体包括：
[0069]
s1、基于单目摄像机采集前方目标二维码的图像；
[0070]
s2、在区域对物体进行检测，通过矩形框二维码识别并显示，得到矩形框信息；
[0071]
s3、结合矩形框信息，找到该矩形框顶边的两个像平面坐标(u1，v1)和(u2，v2)；
[0072]
s4、基于几何关系推导法，根据顶边的两个像平面坐标(u1，v1)和(u2，v2)推导目标平面坐标(x1，y1)和(x2，y2)；
[0073]
s5、基于目标平面坐标，通过欧氏距离公式计算出目标与智能小车的距离信息d。
[0074]
具体地，单目视觉测距是利用一个摄像机获得的图片得出深度信息，按照测量的原理主要分为基于已知运动和已知物体的测量方法。在这里我们使用已知物体的测量方法。已知物体的测量方法是指在已知物体信息的条件下利用摄像机获得的目标图片得到深度信息。
[0075]
使用相似三角形来计算相机到一个已知的物体或者目标的距离。假设有一个宽度为w的目标或者物体，然后将这个目标放在距离相机为d的位置，用相机对物体进行拍照并且测量物体的像素宽度p，这样就可以得出相机焦距f的计算公式：
[0076]
f＝(p*d)/w
[0077]
当将相机移动靠近或者离远物体或者目标时，用相似三角形来计算出物体离相机的距离d：
[0078]
d＝(w*f)/p
[0079]
得出目标到摄像机的距离d，其中p是指像素距离，w是二维码标记的宽度，f是摄像机焦距。
[0080]
进一步作为本方法的优选实施例，所述基于单目相机相对测量模块的测角步骤具体包括：
[0081]
获取单目摄像机主距f；
[0082]
结合矩形框信息，找到该矩形框角点的像平面坐标，分别记为(u1，v1)(u2，v2)(u3，
v3)(u4，v4)；
[0083]
根据矩形框角点的像平面坐标计算目标中心点像素坐标(u5，v5)；
[0084]
将目标中心点像素(u5，v5)坐标转图像坐标(x，y)；
[0085]
将目标中心点图像坐标(x，y)转相机坐标(x，y，z)；
[0086]
基于相机坐标，通过极坐标转换计算目标的方位角和高度角θ。
[0087]
具体地，在角度测量中，我们可以利用成像原理，参照图3，目标在像平面的成像点为p(u，v)，根据已知的摄影机主距f，以及图上每个像素对应的实际距离dx，就可以计算出相机在水平和垂直方向上的两个视线角θ。
[0088]
进一步作为本方法优选实施例，所述目标的方位角和高度角计算公式如下：
[0089][0090]
上式中，θ表示方位角，表示高度角，f表示单目摄像机主距，(x，y，z)表示相机坐标。
[0091]
进一步作为本方法优选实施例，述基于单线雷达相对测量模块的工作步骤具体包括：
[0092]
基于单线雷达扫描二维点云，得到点云数据；
[0093]
将点云数据排成一维向量并按照点云的序号依次对点云数据进行聚类，得到点云簇；
[0094]
具体地，聚类过程参照图4，聚类即将相同或类似的数据聚成一簇，此处的聚类算法是用来区分来自不同目标的点云，进而感知周围的环境。单线激光雷达获得的点云数据是有序的，因此我们将点云排成一维向量，按照点云的序号依次对点云数据进行聚类，通过相邻点之间的距离判断是否为同一物体的反射信息。
[0095]
对点云簇进行线段和角点提取，得到二维特征；
[0096]
具体地，在聚类得到的若干点云簇中，想要从中识别出目标的分类，需要提取点云的二维特征。此处我们通过提取线段和角点来识别目标。线段特征可以通过直线拟合的残差来判断，角点特征可以先识别线段特征，后通过两线段夹角判断。线段以点云簇中始末两点在拟合直线上的垂足为端点计算。
[0097]
基于二维特征与预设模板库内的小车外形特征匹配，计算其他小车的中心位置坐标和姿态。
[0098]
具体地，在无人车集群的协同任务中，需要识别出检测到的物体是其他无人车还是障碍物。此时我们利用检测出的二维特征与已知的小车外形特征相匹配，已知小车的外包围为矩形框，长度为l，宽度为w。通过直线特征匹配长宽，通过角点特征匹配直角，计算小车中心位置坐标和姿态。
[0099]
计算无人车的位姿需要根据检测到的几何形状进行分类，再计算出无人车的中心位置和朝向。
[0100]
检测到直线目标时，拟合得到的直线方为y＝kx+b，线段中点坐标为pm＝[xm,ym]。因为小车中心位置相对于雷达处在小车边框的远端，所以需要先判断小车的象限。当小车
处于一、四象限时，方向向量为：
[0101][0102]
当小车处于二、三象限时，方向向量为：
[0103][0104]
当检测到的线段为小车的长边时，小车的位置为
[0105]
p＝pm+uw(a)
[0106]
小车的朝向为arctank或者arctank+π。
[0107]
当检测到的线段为小车的宽边时，小车的位置为
[0108]
p＝pm+ul(b)
[0109]
小车的朝向为arctank+0.5π或者arctank+1.5π。
[0110]
检测到角点目标时，拟合两条直线分别为y＝k1x+b1，y＝k2x+b2。两端点的坐标分别为p1和pn，中心位置坐标可以选用任一条线段计算，同(a)(b)，或者用两条线段的端点所连成线段的中点表示：
[0111][0112]
同理朝向也可以选用任一线段计算。
[0113]
根据其他小车的中心位置坐标和姿态进行运动估计，进行多目标轨迹维护：
[0114]
在无人集群协同任务中，车载雷达会同时检测到多个目标。为了能够稳定地维护集群成员的轨迹，了解同伴的运动状态，需要对观测量与维护状态进行匹配。多目标估计维护可以分为匹配过程和卡尔曼滤波过程，其关键步骤为：卡尔曼预测，匈牙利算法将预测后的轨迹与当前的观测相匹配，卡尔曼状态更新。对于未匹配的轨迹，可以在时间域内保留；对于未匹配的观测，可以跟据集群的具体情况建立新的轨迹或者删除。在此系统内，维护的状态估计为x＝[x,y,ψ,v,ω]，为目标的位置，朝向，速度与角速度。观测值为d＝[x,y,ψ1,ψ2]，为目标的位置与朝向。因为由直线提取出的小车朝向有两个方向，因此在初始化轨迹时由连续两帧的观测和预测值最接近的方向确定。
[0115]
匈牙利算法是解决最大分配的经典算法，它寻找总损失最小的观测与预测的匹配对。此处的损失定义为cost＝α*dis_cost+β*ori_cost，其中dis_cost为观测与预测的位置的距离，ori_cost为观测与预测的朝向的差值。此处由于观测具有相反的两个方向，我们选择ori_cos最小的那个作为观测的朝向。α和β为系数。
[0116]
进一步作为本方法优选实施例，还包括协同定位步骤：
[0117]
具体地，无人系统的自主协同需要足够高的定位精度，目前的室外定位往往依赖gps，室内依靠动态视觉捕捉系统。但仍然有一些任务场景受到限制，使得部分乃至全部无人车无法收到定位辅助系统的定位信息。此时，协同定位技术就成为了最好的选择。在一组无人机中，每个个体都可以通过测量其他队友的位置来得到反馈，进而联合估计自己和队友的位置。协同定位的最明显优势在于任何一个个体获得的精确的绝对定位信息都可以通过集体内部的相对测量传递到每一架无人机，而且仅依赖于无人机自身搭载的测量载荷而
非外部设备，降低了成本又提高了应对恶劣环境的鲁棒性。同时，协同定位也可以作为一个增强补偿系统，在对定位精度要求极高的任务中，依靠高精度的测量仪器来提高粗糙的绝对定位精度。在此无人车集群中，即便只有少数个体能够接受到绝对位置信息，他们仍可以通过激光雷达的识别与定位，将测量到的位置分享给无法定位的队友，实现整个集群的定位。在全部都无法接收定位信息的情况下，无人车可以通过结合自身的惯导信息和雷达的相对测量信息，实现集群内部之间的相对定位。
[0118]
判断到存在具备定位信息的智能小车，采用绝对定位方法完成智能小车集群之间的协同定位；
[0119]
将具备定位信息的智能小车定义为锚点；
[0120]
确定锚点某个时刻的自身运动状态并测量其他智能小车的运动状态，得到锚点自身运动状态和测量状态；
[0121]
根据锚点自身运动状态和测量状态，结合卡尔曼滤波方法，更新对应智能小车的定位，得到完成定位的智能小车；
[0122]
将完成定位的智能小车定义为另一锚点并循环协同定位步骤，直至所有智能小车均完成定位。
[0123]
具体地，上述过程是在稳定的定位过程中实现的，因为需要每台车量可以对自己的运动有状态预测。因此，如果在初始状态未知的情况下，需要锚点车辆广播自己的运动，其他车辆在静止条件下识别出锚点车辆后，才能开始上述的协同定位过程。
[0124]
另外，当所有车辆均不具备绝对定位信息时，需要依靠车载惯导和雷达的结合实现集群间相对定位。因为不具备绝对的位置坐标，每台车需要在本地坐标系下更新其他车辆的状态估计。在k时刻，获取a车运动状态，并以此时的位置为原点，朝向为x轴正方向，逆时针旋转90度为y轴正方向，建立坐标系xoyk。根据a车测量得到b车的状态。在k+1时刻，根据a车测量得到b车的状态。根据惯导的信息记录，此时的本地坐标系xoy
k+1
相对于xoyk的旋转角为φ，平移向量为[δx，δy]。调整k+1时刻的b车在xoyk坐标系下的测量状态。当观测到多个车辆时，需要从其他车辆获得其自身的状态预测，应用多目标轨迹维护方法进行匹配。因为不同的车辆的状态预测均在自己本地坐标系下，需要通过测量与预测的差异计算出本地坐标系的旋转与平移，将坐标系统一。同样，这个过程也是在稳定定位的过程中实现的，如果在初始状态未知的情况下，需要一台车辆先确定坐标系再将自身运动状态广播出去。其他车辆在静止条件下，通过观测该车的状态广播和对其的测量确定坐标系，然后才可以开始运动。
[0125]
进一步作为本方法优选实施例，所述分布式集群规划控制模块的工作步骤具体包括：
[0126]
根据智能小车自身的运动信息对智能小车位置进行更新；
[0127]
根据基于单目相机相对测量模块和基于单线雷达相对测量模块计算得到环境源产生的距离信息；
[0128]
将环境源产生的距离信息通过虚拟势场法计算成势场函数，叠加势场函数得到智能小车该时刻的期望速度；
[0129]
根据期望速度，通过pid控制器对智能小车进行控制，完成智能小车捕捉目标和动态避障的任务。
[0130]
具体地，分布式集群规划控制方法是基于虚拟势场的一种规划方式，其原理是将实验场地内的所有环境要素定义为不同的势场源(如障碍物、目标、边界、友邻等)，势场大小基于环境要素与自身的相对距离计算，通过叠加不同的势场源，可以确定个体的移动方向，针对移动方向与车体朝向的偏差设计pid控制器，可实现小车的航向控制或航点控制。
[0131]
另外，在此过程中，基于惯导的位置更新在大多数情况下并不可靠，会产生数据漂移的情况，我们提出通过雷达对环境信息的变化进行配准，与惯导产生的运动信息进行数据融合，以及在初始场地内布置已知位置的合作标识，通过视觉相对测量合作标识的位置，可间断修正小车的位置，确保在移动过程中小车始终知道自己的正确位置，总体集群规划控制流程如图。
[0132]
以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做作出种种的等同变形或替换，这些等同的变形或替换均包含在本技术权利要求所限定的范围内。