一种无人驾驶协同探测方法与流程

本发明涉及无人驾驶技术领域，尤其是涉及一种无人驾驶协同探测方法。

背景技术：

无人驾驶技术主要是通过传感器获取车辆外部信息，从而在数字世界中构建一个可供分析、决策的场景模型，然而传感器的检测常常会受到各种干扰、阻碍或遮蔽，即存在探测盲区，使得探测盲区内的障碍物体无法被车辆探测到，这种现象在无人驾驶的运动环境中更为明显。

车辆在运动过程中的相对运动，由于相互之间距离较近，且存在车身的阻碍，导致车辆传感器的探测范围受到限制，容易产生探测盲区，一旦探测盲区内存在障碍物体，由于车辆无法提前获取探测盲区内的障碍物体信息，因此极容易引发安全事故。

综上所述，若能对探测盲区进行协同探测，使车辆根据探测信息生成对应场景，将有利于提升无人驾驶的安全性，减少因为车辆相对运动导致探测范围受限产生的不利影响，并防止由于探测范围受限造成的车辆决策失误。

技术实现要素：

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种无人驾驶协同探测方法。

本发明的目的可以通过以下技术方案来实现：一种无人驾驶协同探测方法，包括以下步骤：

s1、通过主车和遮蔽车各自的车载传感器，分别获取主车的姿态信息、速度信息以及遮蔽车的姿态信息、速度信息，以分别建立主车坐标系和遮蔽车坐标系，其中，主车坐标系的原点数据即为主车的位置信息，遮蔽车坐标系的原点数据即为遮蔽车的位置信息；

s2、通过遮蔽车的第一车载探测器获取遮蔽车周围的行驶环境数据，根据行驶环境数据，判断遮蔽车周围是否存在障碍物，若判断为是，则执行步骤s3，否则返回步骤s1；

s3、基于遮蔽车坐标系，通过遮蔽车的第二车载探测器获取障碍物的位置信息和速度信息；

s4、判断遮蔽车周围的障碍物是否位于主车的探测盲区内，若判断为是，则执行步骤s5，否则返回步骤s1；

s5、遮蔽车将遮蔽车自身的尺寸信息、位置信息、姿态信息、速度信息以及障碍物尺寸信息、位置信息和速度信息发送给主车，基于主车坐标系，由主车生成对应场景。

优选地，所述步骤s1中车载传感器为marg传感器，姿态信息包括侧倾偏角、俯仰偏角和车头指向偏角，主车坐标系具体为：主车坐标系原点位于主车的顶部中心位置，根据主车的车头指向偏角，确定主车坐标系的y轴正方向，主车所在平面顺时针垂直于y轴正方向为x轴正方向，由右手定则确定z轴正方向；

遮蔽车坐标系具体为：遮蔽车坐标系原点位于遮蔽车的顶部中心位置，根据遮蔽车的车头指向偏角，确定遮蔽车坐标系的y轴正方向，遮蔽车所在平面顺时针垂直于y轴正方向为x轴正方向，由右手定则确定z轴正方向；

主车速度信息是由主车的marg传感器获取的在主车坐标系下的主车绝对速度；

遮蔽车速度信息是由遮蔽车的marg传感器获取的在遮蔽车坐标系下的遮蔽车绝对速度。

优选地，所述步骤s2中第一车载探测器为图像探测器，用于获取车辆周围行驶环境的图像数据。

优选地，所述步骤s3中第二车载探测器包括激光雷达探测器和超声波探测器，所述获取障碍物的位置信息和速度信息的具体过程为：

s31、基于遮蔽车坐标系，遮蔽车的第二车载探测器以第一预设时间间隔分别探测障碍物的角度和距离数据，得到障碍物的第一组角度和距离数据，以及第二组角度和距离数据；

s32、根据障碍物的角度和距离数据，确定障碍物在遮蔽车坐标系下的位置信息，分别得到障碍物的第一位置信息和第二位置信息；

s33、将障碍物的第二位置信息减去障碍物的第一位置信息，得到障碍物的位置差信息，将障碍物的位置差信息除以第一预设时间间隔，得到障碍物在遮蔽车坐标系下的速度信息。

优选地，所述步骤s32中障碍物在遮蔽车坐标系下的位置信息为：

其中，(xji，yji，zji)表示障碍物在遮蔽车坐标系下的坐标数据，dji表示障碍物在遮蔽车坐标系下与遮蔽车之间的距离，αji表示障碍物在遮蔽车坐标系下的仰角，θji表示障碍物在遮蔽车坐标系下的方向角；

所述步骤s33中障碍物在遮蔽车坐标系下的速度信息为：

其中，(δvjix，δvjiy，δvjiz)表示障碍物在遮蔽车坐标系下与遮蔽车之间的相对速度，表示障碍物在遮蔽车坐标系下的第一位置信息，表示障碍物在遮蔽车坐标系下的第二位置信息，t表示第一预设时间间隔。

优选地，所述步骤s4具体包括以下步骤：

s41、分别确定主车和障碍物在遮蔽车坐标系下的位置信息，以建立主车与障碍物之间的探测线方程；

s42、根据遮蔽车的尺寸信息，构建遮蔽车空间立方体模型，并确定遮蔽车空间立方体模型的约束条件；

s43、结合探测线方程以及遮蔽车空间立方体模型的约束条件，判断探测线与遮蔽车空间立方体之间是否存在交点，若存在交点，则确定障碍物位于主车的探测盲区内，若不存在交点，则确定障碍物没有位于主车的探测盲区内。

优选地，所述步骤s41中主车与障碍物之间的探测线方程为：

其中，(xl，yl，zl)表示在遮蔽车坐标系下探测线上的点的坐标数据，(x0i，y0i，z0i)表示主车在遮蔽车坐标系下的坐标数据，(xji，yji，zji)表示障碍物在遮蔽车坐标系下的坐标数据，k表示在遮蔽车坐标系下探测线上的点到主车的线段长度与探测线长度的比例系数；

所述步骤s42中遮蔽车空间立方体模型的约束条件包括：

正面约束：

后面约束：

右侧面约束：

左侧面约束：

顶面约束：

底面约束：

其中，(xs，ys，zs)表示遮蔽车空间立方体模型上的点的坐标数据，(li，wi，hi)表示遮蔽车长度、宽度和高度的尺寸信息；

所述步骤s43中判断探测线与遮蔽车空间立方体之间是否存在交点的具体过程为：依次将遮蔽车空间立方体模型的正面、后面、右侧面、左侧面、顶面和底面约束中点的坐标数据替换为探测线上点的坐标数据，若替换之后，所有约束条件中只要有一个约束条件依然成立，则判断探测线与遮蔽车空间立方体之间存在交点，即确定障碍物位于主车的探测盲区内，若替换之后，所有约束条件均不成立，则判断探测线与遮蔽车空间立方体之间不存在交点，即确定障碍物没有位于主车的探测盲区内。

优选地，所述步骤s5中主车生成对应场景的具体过程为：

s51、基于主车坐标系，获取遮蔽车与主车之间的空间关系信息，其中，遮蔽车与主车之间的空间关系信息包括遮蔽车在主车坐标系下的位置信息、遮蔽车与主车的相对速度以及遮蔽车的绝对速度；

s52、基于主车坐标系，获取障碍物与主车之间的空间关系信息，其中，障碍物与主车之间的空间关系信息包括障碍物在主车坐标系下的位置信息、障碍物与主车的相对速度以及障碍物的绝对速度；

s53、根据遮蔽车与主车之间的空间关系信息、障碍物与主车之间的空间关系信息，结合主车、遮蔽车以及障碍物各自的尺寸信息，生成对应的场景模型。

优选地，所述步骤s51具体包括以下步骤：

s511、基于主车坐标系，主车的第二车载探测器以第二预设时间间隔分别探测遮蔽车的角度和距离数据，得到遮蔽车的第一组角度和距离数据，以及第二组角度和距离数据；

s512、根据遮蔽车的角度和距离数据，确定遮蔽车在主车坐标系下的位置信息，分别得到遮蔽车的第一位置信息和第二位置信息；

s513、将遮蔽车的第二位置信息减去遮蔽车的第一位置信息，得到遮蔽车的位置差信息，将遮蔽车的位置差信息除以第二预设时间间隔，得到遮蔽车在主车坐标系下与主车的相对速度；

s514、结合遮蔽车与主车的相对速度、主车的绝对速度，计算得到主车坐标系下的遮蔽车绝对速度；

遮蔽车在主车坐标系下的位置信息为：

其中，(xi0，yi0，zi0)表示遮蔽车在主车坐标系下的坐标数据，di0表示遮蔽车在主车坐标系下与主车之间的距离，αi0表示遮蔽车在主车坐标系下的仰角，θi0表示遮蔽车在主车坐标系下的方向角；

遮蔽车在主车坐标系下与主车的相对速度为：

其中，(δvi0x，δvi0y，δvi0z)表示遮蔽车在主车坐标系下与主车之间的相对速度，表示遮蔽车在主车坐标系下的第一位置信息，表示遮蔽车在主车坐标系下的第二位置信息，t′表示第二预设时间间隔；

遮蔽车在主车坐标系下的绝对速度为：

其中，(vix，viy，viz)表示遮蔽车在主车坐标系下的绝对速度，(v0x，v0y，v0z)表示主车在主车坐标系下的绝对速度。

优选地，所述步骤s52中障碍物在主车坐标系下的位置信息为：

其中，(xj0，yj0，zj0)表示障碍物在主车坐标系下的坐标数据，rx表示主车坐标系在x轴方向的旋转矩阵，ry表示主车坐标系在y轴方向的旋转矩阵，rz表示主车坐标系在z轴方向的旋转矩阵，γi0表示遮蔽车在主车坐标系下的俯仰偏角，βi0表示遮蔽车在主车坐标系下的侧倾偏角，δi0表示遮蔽车在主车坐标系下的车头指向偏角；

障碍物在主车坐标系下与主车的相对速度为：

其中，(δvj0x，δvj0y，δvj0z)表示障碍物在主车坐标系下与主车之间的相对速度；

障碍物在主车坐标系下的绝对速度为：

其中，(vix，viy，viz)表示障碍物在主车坐标系下的绝对速度。

与现有技术相比，本发明采用主车与遮蔽车协同探测的方式，解决了车辆相对运动时遮蔽车对主车造成的探测盲区问题，能够使主车从遮蔽车提前获取探测盲区内障碍物的信息，充分感知周围环境，从而提高无人驾驶的安全性；

本发明通过建立主车与障碍物之前的探测线方程，结合遮蔽车空间立方体模型，能有效确定障碍物是否位于主车的探测盲区内，并通过遮蔽车与主车之间的通信，将主车探测盲区内的障碍物信息及时发送给主车；

本发明在不增加现有无人驾驶车辆传感器架构的基础上，利用车辆现有架构的车载传感器和车载探测器获取车辆之间、车辆与障碍物之间的空间关系，并通过建立车辆自身坐标系，能将其他车辆信息以及障碍物信息对应转换为自身坐标系下的数据，有利于车辆快速生成对应场景。

附图说明

图1为本发明的方法流程图；

图2为实施例中建立主车与障碍物之间探测线的示意图；

图3为实施例中主车生成的场景示意图；

图中标记说明：10、障碍物，20、第一遮蔽车，30、第二遮蔽车，40、主车，50、第一探测盲区，60、第二探测盲区。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

如图1所示，一种无人驾驶协同探测方法，包括以下步骤：

s1、通过主车和遮蔽车各自的车载传感器，分别获取主车的姿态信息、速度信息以及遮蔽车的姿态信息、速度信息，以分别建立主车坐标系和遮蔽车坐标系，其中，主车坐标系的原点数据即为主车的位置信息，遮蔽车坐标系的原点数据即为遮蔽车的位置信息；

s2、通过遮蔽车的第一车载探测器获取遮蔽车周围的行驶环境数据，根据行驶环境数据，判断遮蔽车周围是否存在障碍物，若判断为是，则执行步骤s3，否则返回步骤s1；

s3、基于遮蔽车坐标系，通过遮蔽车的第二车载探测器获取障碍物的位置信息和速度信息；

s4、判断遮蔽车周围的障碍物是否位于主车的探测盲区内，若判断为是，则执行步骤s5，否则返回步骤s1；

s5、遮蔽车将遮蔽车自身的尺寸信息、位置信息、姿态信息、速度信息以及障碍物尺寸信息、位置信息和速度信息发送给主车，基于主车坐标系，由主车生成对应场景。

其中，车载传感器为marg传感器，用于获取车辆自身的姿态信息，姿态信息包括姿态信息包括侧倾偏角、俯仰偏角和车头指向偏角，通过车头指向偏角确定车辆坐标系xayaza的y轴方向，以车辆所在平面顺时针垂直于y轴正方向为x轴正方向，由右手定则确定z轴正方向，车辆坐标系xayaza分为主车坐标系x0y0z0(下标0表示主车)和遮蔽车坐标系xiyizi(下标i表示遮蔽车)；

第一车载探测器为图像探测器，用于获取车辆周围的行驶环境图像数据，以确定车辆周围是否存在障碍物j，第二车载探测器包括激光雷达探测器和超声波探测器，用于获取障碍物j的位置信息和速度信息；

上述各传感器和探测器根据自身特点，在不同探测精度和距离上进行信息检测，为统一基准，需要将各传感器和探测器测量数据均转换为在车辆自身坐标系xayaza下的数据，再进行计算；

探测盲区是由于遮蔽车与主车间相对运动造成遮蔽车遮挡主车传感器和探测器，导致主车无法探测盲区内障碍物所致，其中，遮蔽车通过以下方法判断主车对障碍物的观测是否被自身遮挡：

在遮蔽车坐标系xiyizi下，根据探测结果确定主车位置(x0i，y0i，z0i)，各传感器和探测器因安装在车身位置不同导致探测视野有所细微差异，在此，忽略探测视野细微差异，默认各传感器和探测器的探测视角均从主车坐标系原点处出发，即车辆顶部中间位置；

遮蔽车在遮蔽车坐标系xiyizi下，判断主车观测点(x0i，y0i，z0i)和障碍物坐标(xji，yji，zji)连线段与遮蔽车所包围空间的交点，如果交点个数为0个，则认为障碍物没有被遮蔽车遮挡，如果交点个数大于0，则认为障碍物被遮蔽车遮挡，该障碍物位于主车的探测盲区内。

本发明方法的具体应用过程为：

步骤101，无人驾驶车辆通过车载传感器，探测主车与遮蔽车相互空间位置、遮蔽车与障碍物相互空间位置；

步骤102，若遮蔽车周围不存在障碍物，则遮蔽车发送安全信息给主车，并返回步骤101，否则执行步骤103；

步骤103，如图2所示，建立主车40与障碍物10之间的探测线，其中，第一遮蔽车20被抽象成空间立方体模型，该空间立方体模型的长宽高数据由第一遮蔽车20自身的尺寸信息获得，通过判断探测线是否与遮蔽车的空间立方体产生交点，以判断障碍物10是否位于主车40的探测盲区内，若判断结果为“是”，执行步骤104，否则，返回步骤101；

步骤104，探测盲区对应遮蔽车将障碍物检测概要信息发送给对应探测盲区的主车；

步骤105，主车融合车载传感器探测信息和其他车辆探测信息进行场景建模。

无人驾驶车辆通过车车通信装置，进行信息发送和接收，信息内容包括车辆空间尺寸信息、姿态信息，并将对应主车探测盲区内障碍物概要信息发送至相应车辆，概要信息包括障碍物尺寸、运动速度、障碍物在遮蔽车坐标系xiyizi下空间位置。

图3所示为实施例中主车生成的场景示意图，在该场景下，第一遮蔽车20、第二遮蔽车30和主车40分别通过各自的车载传感器和探测器对周围场景进行探测，以主车40为例，由于第一遮蔽车20和第二遮蔽车30的存在，分别导致了第一探测盲区50和第二探测盲区60，由于探测盲区内信息无法被主车探测，因此第一探测盲区50内的障碍物10无法被主车40探测到，障碍物10对主车40存在潜在安全威胁。

本实施例中，主车和遮蔽车的空间位置关系包括：在主车坐标系下的主车绝对速度v0(v0x，v0y，v0z)，遮蔽车和主车的相对速度δvi0(δvi0x，δvi0y，δvi0z)(i＝1，2...)，遮蔽车绝对速度vi(vix，viy，viz)(i＝1，2...)；

主车和遮蔽车所在坐标系原点距离di0(i＝1，2...)，遮蔽车坐标系原点的方向角θi0(i＝1，2...)，遮蔽车坐标系原点与主车坐标系原点连线和x0y0平面的仰角αi0(i＝1，2...)，主车尺寸大小(l0，w0，h0)，遮蔽车尺寸大小(li，wi，hi)(i＝1，2...)；

遮蔽车辆i(i＝1，2...)在主车坐标系x0y0z0下空间位置，通过遮蔽车辆i坐标系xiyizi原点所在位置(xi0，yi0，zi0)进行描述，公式如下：

遮蔽车和主车的相对速度为δvi0(δvi0x，δvi0y，δvi0z)(i＝1，2...)：

其中，表示遮蔽车在主车坐标系下的第一位置信息，表示遮蔽车在主车坐标系下的第二位置信息，即有(δxi0t，δyi0t，δzi0t)为前后两次主车探测得到的遮蔽车在主车坐标系的x0，y0，z0方向上的位移，t′表示主车上第二车载探测器的探测时间间隔；

遮蔽车在主车坐标系下的绝对速度为vi(vix，viy，viz)(i＝1，2...)：

式中，(vix，viy，viz)表示遮蔽车在主车坐标系下的绝对速度，(v0x，v0v，v0z)表示主车在主车坐标系下的绝对速度。

障碍物运动信息包括：在遮蔽车坐标系下的障碍物j和遮蔽车i的相对速度δvji(δvjix，δvjiy，δvjiz)(j＝1，2...)，障碍物j在遮蔽车坐标系xiyizi的空间位置(xji，yji，zji)(j＝1，2...)，障碍物j与遮蔽车坐标系原点距离dji(j＝1，2...)、方向角θji(j＝1，2...)、仰角αji(j＝1，2...)，障碍物j的尺寸大小(lj，wj，hj)(j＝1，2...)。

遮蔽车辆i获取障碍物j相对于遮蔽车辆的空间位置(xji，yji，zji)(j＝1，2...)，根据以下公式计算得到：

障碍物j和遮蔽车i相对速度δvji(δvjix，δvjiy，δvjiz)(j＝1，2...)：

式中，(δvjix，δvjiy，δvjiz)表示障碍物在遮蔽车坐标系下与遮蔽车之间的相对速度，表示障碍物在遮蔽车坐标系下的第一位置信息，表示障碍物在遮蔽车坐标系下的第二位置信息，即有(δxjit，δyjit，δzjit)为前后两次遮蔽车探测得到的障碍物j在遮蔽车坐标系xi，yi，zi方向上的位移，t表示遮蔽车上第二车载探测器的探测时间间隔。

遮蔽车坐标系xiyizi相对主车坐标系x0y0z0中侧倾偏角βi0、俯仰偏角γi0、车头指向偏角δi0，主车观测到障碍物j在x0y0z0坐标系下的位置(xj0，yj0，zj0)通过以下公式计算得到：

式中，rx表示主车坐标系在x轴方向的旋转矩阵，ry表示主车坐标系在y轴方向的旋转矩阵，rz表示主车坐标系在z轴方向的旋转矩阵，

主车通过以下公式(10)计算得到障碍物j在主车坐标系下相对于主车的相对速度δvj0(δvj0x，δvj0y，δvj0z)：

此外，主车通过以下公式(11)计算得到障碍物j在主车坐标系下的绝对速度vj(vjx，vjy，vjz)：

遮蔽车i观测到障碍物j在遮蔽车坐标系下的绝对速度v′j(v′jx，v′jy，v′jz)：

综上所述，本发明能有效解决车辆相对运动车身遮挡所造成的探测盲区问题，提升无人驾驶车辆的安全性能，构建全息式数字场景，防止因为感知范围受限造成的车辆决策失误具，从而有效减小事故发生风险。

本发明方法简明，在不改变现有无人驾驶硬件架构的基础上，通过算法改进，使无人驾驶车辆充分感知周围环境，进行场景重建，提升无人驾驶车辆行驶安全性。