车辆定位的方法、装置、控制器、智能车和系统与流程
本申请要求在2020年01月14日提交中国专利局、申请号为202010038272.6、申请名称为“智能车定位的方法、装置、系统和智能车”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
本申请涉及智能车(intelligentdriving)领域，尤其涉及一种车辆定位的方法、装置、控制器、智能车和系统。
背景技术：
全球定位系统(globalpositioningsystem，gps)具有性能好、精度高、应用广的特点，是迄今较常用的导航定位系统。目前车辆定位领域gps的应用已极为广泛。将gps应用在车辆中，可以达到导航定位、安全控制、智能交通等目标，具备良好的发展趋势。
gps技术广泛应用于车辆的定位中，比如可以应用到支持无人驾驶(unmanneddriving)、辅助驾驶(driverassistance/adas)、智能驾驶(intelligentdriving)、网联驾驶(connecteddriving)、智能网联驾驶(intelligentnetworkdriving)或汽车共享(carsharing)的智能车。在实际应用中，可能会由于硬件和/或通信的原因，导致gps信号弱甚至无gps信号的情况发生，这种情况下若还依据gps来进行车辆的定位，则会带来较为严重的安全问题。
技术实现要素：
本申请提供一种车辆定位方法、装置和可读存储介质，用于在gps信号弱或者无gps信号的情况下，解决车辆定位的问题。
第一方面，本申请实施例提供一种车辆定位方法，该方法中，控制器获取第一车辆与被求助物的第一相对位姿以及被求助物的全局位姿，根据第一相对位姿和全局位姿，计算第一车辆的全局位姿。其中，第一相对位姿用于指示以第一车辆为基准，被求助物相对于第一车辆的位置和姿态；第一相对位姿是在第一坐标系中确定的被求助物的位姿；全局位姿是在第二坐标系确定的被求助物的全局位姿。如此，在第一车辆的gps信号弱或者无gps信号的情况下，可以基于被求助物的全局位姿，以及第一车辆和被求助物之间的第一相对位姿来确定出第一车辆的全局位姿。
在一种可能地实施方式中，被求助物为第二车辆，第一相对位姿用于指示以第一车辆为基准，第二车辆相对于第一车辆的位置和姿态，姿态用于指示以第一车辆的车头朝向为基准，第二车辆的车头的朝向。如此，在第一车辆的gps信号弱或者无gps信号的情况下，可以向其他车辆求助，且可以基于其他车辆的信息，确定出第一车辆的车头朝向，从而在智能车领域可以辅助其实现自动驾驶。
在另一种可能地实施方式中，获取第一车辆与被求助物的第一相对位姿，包括：获取第一车辆的激光雷达进行扫描第一车辆周围物体所得到的在第一坐标系的第一点云数据；获取第二车辆的激光雷达进行扫描第二车辆周围物体所得到的在第二坐标系的第二点云数据；其中，第一车辆的激光雷达和第二车辆的激光雷达具有重合扫描区域，且针对重合扫描区域中的障碍物，第一点云数据中包括障碍物对应的点云数据，第二点云数据中包括障碍物对应的点云数据；根据障碍物在第一点云数据中对应的点云数据，以及障碍物在第二点云数据中对应的点云数据，计算第一相对位姿。由于部署在两辆车的激光雷达具有重合扫描区域，因此，可以根据部署在两辆车的激光雷达扫描出的点云数据进行点云匹配，从而确定出较为准确的两辆车之间的相对位姿。
在另一种可能地实施方式中，根据第一点云数据和第二点云数据，确定出第一相对位姿，包括：根据障碍物在第一点云数据中对应的点云数据，以及障碍物在第二点云数据中对应的点云数据，计算第一相对位姿，包括：将第一坐标系中第一点云数据转换至预设的第一车辆的第一基准坐标系，获得第三点云数据；其中，第一基准坐标系是将第一坐标系的原点平移至预设的第一基准点后得到的坐标系；将第二坐标系中第二点云数据转换至预设的第二车辆的第二基准坐标系，获得第四点云数据；其中，第二基准坐标系是将第二坐标系的原点平移至预设的第二基准点后得到的坐标系；对障碍物在第三点云数据中对应的点云数据，以及障碍物在第四点云数据中对应的点云数据进行点云匹配，得到第一相对位姿。如此，可以将第一点云数据和第二点云数据均转换至各自车辆的基准坐标系下，各个车辆的基准坐标系的原点相对于各自的车辆的位置都是对应的，例如各个车辆的基准坐标系的原点均位于各自的车辆的后轴中心，从而可以较为准确的确定出两辆车之间的相对位姿。
在另一种可能地实施方式中，对障碍物在第三点云数据中对应的点云数据，以及障碍物在第四点云数据中对应的点云数据进行点云匹配，得到第一相对位姿，包括：根据障碍物在第三点云数据中对应的点云数据，以及障碍物在第四点云数据中对应的点云数据，执行n次迭代，得到第n次迭代输出的第三变换矩阵；n为正整数；根据第n次迭代输出的第三变换矩阵，确定第一相对位姿。其中，针对n迭代中的第i次迭代，i为不大于n的正整数：针对障碍物的m个点中的每个点执行：根据第i-1次迭代输出的第三变换矩阵，对该点在第三点云数据中对应的点云数据执行一次变换，得到该点对应的变换后点云数据；计算该点对应的变换后点云数据和该点在第四点云数据中对应的点云数据的差值，得到该点对应的残差；m为正整数；根据障碍物的m个点中每个点对应的残差，计算残差总和；若残差总和不小于预设的残差阈值，则以预设变换量更新第i-1次迭代输出的第三变换矩阵，并将更新后的第i-1次迭代输出的第三变换矩阵作为：第i次迭代输出的第三变换矩阵，并执行下一次迭代；若残差总和小于残差阈值，则结束迭代，将第i-1次迭代输出的第三变换矩阵作为：第n次迭代输出的第三变换矩阵。如此，可以通过n次迭代的方式求解出较为准确的第三变换矩阵，从而确定出更加准确的第一相对位姿。
在另一种可能地实施方式中，当i为1时，第i-1次迭代输出的第三变换矩阵通过如下内容确定：根据前一时刻第一车辆的全局位姿，以及基于imu计算出的：以前一时刻第一车辆的全局位姿为基准，第一车辆在当前时刻相对于前一时刻的位姿，计算第一车辆当前时刻的预估全局位姿；获取第二车辆基于全球定位系统gps和imu得到的当前时刻的全局位姿；根据第一车辆当前时刻的预估全局位姿和第二车辆当前时刻的全局位姿，确定出第二相对位姿，第二相对位姿用于指示以第一车辆为基准，被求助物相对于第一车辆的位置和姿态；第二相对位姿是在第一坐标系中确定的被求助物的位姿；将用于表达第二相对位姿的矩阵作为：第i-1次迭代输出的第三变换矩阵。如此，可以为n次迭代提供一个第三矩阵初始值，由于第三矩阵的初始值是基于第一车辆当前时刻的预估全局位姿，以及第二车辆当前时刻的全局位姿确定的，因此，该第三矩阵初始值是较为接近第n次迭代输出的第三变换矩阵，进而，可以减少迭代次数，缩短寻找到最优解的时间。
在另一种可能地实施方式中，前一时刻第一车辆的全局位姿可以基于以下内容得到：系统记录的最近一次基于gps的第一车辆的全局位姿，以及基于imu计算出的：以第一时刻第一车辆的全局位姿为基准，第一车辆在当前时刻相对于第一时刻的位姿；第一时刻为系统记录的最近一次基于gps的第一车辆的全局位姿的时刻。
在另一种可能地实施方式中，前一时刻第一车辆的全局位姿可以基于以下内容得到：最近一次计算出的第一车辆的全局位姿，以及基于imu计算出的：以第二时刻第一车辆的全局位姿为基准，第一车辆在当前时刻相对于第二时刻的位姿；第二时刻为最近一次计算出的第一车辆的全局位姿的时刻。如此，可以提高方案的灵活性。
为了提高方案的灵活性，本申请中可以有多种确定第一相对位姿的方案，在另一种可能地实施方式中，将第n次迭代输出的第三变换矩阵作为第一相对位姿的数学表达形式。在另一种可能地实施方式中，对第i-1次迭代输出的第三变换矩阵和第n次迭代输出的第三变换矩阵进行加权融合，得到第四变换矩阵，将第四变换矩阵作为第一相对位姿的数学表达形式。通过该可能地实施方案可以得到的第一相对位姿可以更加准确，进而可以计算出更加准确的全局位姿。
在另一种可能地实施方式中，所述根据所述第一相对位姿和所述相对位姿初始值，计算第二相对位姿，包括：对所述第一相对位姿和所述相对位姿初始值进行加权融合，得到所述第二相对位姿。
在另一种可能地实施方式中，获取第一车辆与被求助物的第一相对位姿之前，还包括：在第一车辆的基于gps的全局位姿误差大于误差阈值；或者；第一车辆在第一车辆的gps定位装置出现故障。
当控制器部署在第一车辆上时，获取被求助物的全局位姿，包括：向被求助物发送第一请求；第一请求中携带第一车辆的标识；接收被求助物发送的被求助物的全局位姿。
在另一种可能地实施方式中，根据第一相对位姿和被求助物的全局位姿，确定出第一车辆的全局位姿之后，还包括：获取多个被求助物中每个被求助物对应的第一车辆的全局位姿；对多个被求助物对应的多个第一车辆的全局位姿进行加权融合，得到第一车辆的目标全局位姿。
第二方面，提供了一种车辆定位的装置，装置包括用于执行第一方面或第一方面任一种可能实现方式中的车辆定位方法的各个模块。
第三方面，提供了一种用于车辆定位的控制器，该控制器包括处理器和存储器。存储器中用于存储计算机执行指令，控制器运行时，处理器执行存储器中的计算机执行指令以利用控制器中的硬件资源执行第一方面或第一方面任一种可能实现方式中方法的操作步骤。
第四方面，本申请提供一种智能车，该智能车包括如上述第三方面或第三方面任一种可能的实现方式中的控制器。
第五方面，本申请还提供一种车辆定位的系统，该系统包括第一车辆和云端的计算平台，上述云端计算平台用于执行上述第三方面或第三方面任一种可能实现方式中的控制器的功能。
第六方面，本申请还提供一种车辆定位的系统，该系统包括第一车辆和被求助物，被求助物用于执行上述第三方面或第三方面任一种可能实现方式中的控制器的功能。
第七方面，本申请提供一种计算机可读存储介质，计算机可读存储介质中存储有指令，当其在计算机上运行时，使得计算机执行上述各方面的方法。
第八方面，本申请提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述各方面的方法。
本申请在上述各方面提供的实现方式的基础上，还可以进行进一步组合以提供更多实现方式。
附图说明
图1为本申请提供的一种应用场景的示意图；
图2为本申请提供的一种控制器部署在第一车辆的情况下的车辆定位方法的流程示意图；
图3为本申请提供的一种以图1所示的车辆a和车辆b为例的坐标系转换示意图；
图4为本申请提供的一种计算第三变换矩阵的方法流程示意图；
图5为本申请提供的一种预估车辆的全局位姿的计算过程的示意图；
图6为本申请提供的一种车辆定位装置的结构示意图；
图7为本申请提供的另一种车辆定位装置的结构示意图。
具体实施方式
为了便于理解，下面先对本申请实施例涉及到的专有概念和名词进行解释。
(1)全局位姿
全局位姿也可以称为绝对位姿，是指在一个基准坐标系中物体的位置和姿态。其中，基准坐标系可以为多维坐标系，多维坐标系包括具有多个维度的坐标系，如二维坐标系、三维坐标系等多维坐标系，具体可以为大地坐标系、通用横墨卡托(universaltransversemercartor，utm)网格系统(gridsystem)等。
物体的位置可以由坐标系中的坐标轴的坐标值来表示。同一个物体在不同的坐标系下，坐标值可能不同。本申请实施例中车辆的位置可以由该车辆所在坐标系中各个坐标轴的坐标值表示，为了便于计算，也可以利用一个基准坐标系中的坐标轴的坐标值来表示。其中，当多个物体分别利用多个坐标系标识位置时，可以以一个坐标系为准，确定所有物体所在的位置，进而进一步确定其他物体在该基准坐标系中的位置。作为基准的坐标系也称为基准坐标系，可以为任意一个物体所在的坐标系，也可以是一个第三方公共的坐标系，本申请实施例对此不做限定。
物体的姿态对于车辆而言可以理解为该车辆车头的朝向，或者，理解为车身的任意位置的朝向。具体可以由车辆对应的矢量与多维坐标系中的水平坐标轴之间的夹角来确定。矢量是一种既有大小又有方向的量，通过车辆的姿态可以确定出车头方向，也可以确定出车辆的行进方向。
(2)相对位姿
相对位姿是指两个物体中一个物体相对于另一个物体的位姿。比如，以物体o1为基准，物体o2相对于物体o1的位姿；或者，以物体o2为基准，物体o1相对于物体o2的位姿。本申请实施例中提到的第一车辆(也可以称为故障车辆)与被求助物的第一相对位姿，可以是指以第一车辆为基准，被求助物相对于第一车辆的位姿；或者，以被求助物为基准，第一车辆相对于被求助物的位置。
(3)点云数据
点云数据是指在一个三维坐标系统中的一组向量的集合。这些向量通常以x、y、z三维坐标的形式表示，而且一般主要用来代表一个物体的外表面形状。不仅如此，除(x，y，z)代表的几何位置信息之外，点云数据还可以表示一个点的rgb颜色，灰度值，深度，分割结果等。
本申请实施例中的点云数据可以由激光雷达扫描获取，激光雷达也可以称为激光探测及测距系统(lightdetectionandranging，lidar)。
(4)变换矩阵
变换矩阵是数学线性代数中的一个概念。在线性代数中，线性变换能够用变换矩阵表示。最为常用的几何变换都是线性变换，这包括旋转、平移、缩放、切变、反射以及正投。本申请实施例中以线性变换包括旋转和平移为例进行示意，即变换矩阵可以表示的线性变换包括旋转和平移。
下面结合附图进一步介绍本申请实施例提供的车辆定位方法。图1为本申请实施例提供的一种应用场景的示意图。如图1所示的车道行驶方向的指示信息，包括4个车道，其中左侧两个车道的行驶方向一致，右侧两个车道的车辆行驶方向一致。任意一个车辆可以是智能车或非智能车，本申请实施例对比不做限定。每个车辆上设置有传感器，该传感器用于探测车辆周边的障碍物，其中，传感器包括激光雷达、毫米波雷达和相机。另外，每辆车可以设置一种或多种传感器，每种传感器的个数可以为一个或多个。传感器可以安装在车辆的顶部、具体可以设置在车辆顶部的中间位置，本申请实施例对每个车辆中传感器安装位置和数量并不做限定。本申请实施例中车辆可以基于车辆与外界无线通信技术(例如，vehicletoeverything(v2x))与其它物体进行通信。例如，可以基于车辆间无线通信技术(例如，vehicletovehicle(v2v))实现车辆与车辆之间的通信。车辆与其它物体之间进行通信可以基于无线高保真(例如，wirelessfidelity(wi-fi))、第五代(5thgeneration，5g)移动通信技术等进行通信。例如，可以基于5g实现车辆与智能设备(如支持定位功能的智能手机或移动设备)之间的通信。
可选地，图1所示的应用场景中还可以包括云端的计算平台，该计算平台可以由云端的服务器或虚拟机实现。
本申请实施例中确定第一车辆的全局位姿的控制器可以位于第一车辆中，具体可以由该车辆中定位系统的控制器、智能驾驶的控制器或其他任何具有计算能力的设备实施，该控制器也可以位于被求助物中，此时，该控制器同样也可以是被求助物中任何具有计算能力的设备或器件，在确定第一车辆的全局位姿后将该第一车辆的全局位姿发送给第一车辆。另外，该控制器还可以位于云端的计算平台(这种情况也可以描述为控制器的功能由云端虚拟机或服务器实施)，此时，在确定第一车辆的全局位姿后，将第一车辆的全局位姿发送给第一车辆。
接下来，以图1所示的场景中传感器为激光雷达，假设第一车辆为车辆a在路上行驶过程中gps出现了异常为例，进一步介绍车辆a如何借助其他物体来进行定位。车辆a和其它物体上均配置有通信模块，车辆a可以通过通信模块搜索到预设区域内其它物体发出的无线信号。车辆a的预设区域如图1中虚线框圆圈所示，以车辆a为中心，以预设距离为半径的区域。该预设距离可以是小于或等于车辆a所发出的无线信号的覆盖范围的半径的一个值。
本申请实施例中用于帮助车辆a进行定位的被求助物可以是车辆，比如车辆b、车辆c和车辆d中的一辆或多辆，还可以是其它具有定位能力或可获知定位信息的物体，比如是道路两侧的一些安装有通信模块的基础设施，比如安装有通信模块的电线杆、移动基站、行人携带的移动电子设备(如带有定位功能的智能手机)和摄像头等等。图1中示例性示出的基础设施为以车辆a为中心的预设区域内的摄像头h。如图1所示，图1中还示出了车辆e和车辆f，由于车辆e和车辆f在以车辆a为中心的预设区域之外，因此车辆a不会借助车辆e和车辆f来进行定位。
在一种可选地实施方式中，车辆a在预设区域中搜索到其它物体后，可以向其发送第一请求，用于请求其它物体协助车辆a进行定位。其中，车辆a搜到的其它物体中包括上述内容提到的被求助物。
在另外一种可选地实施方式中，车辆a在预设区域中搜索到其它物体后，可以先与其建立互信，也就是车辆a和搜索到的其他物体首先进行安全认证，鉴权成功后再向其发送第一请求。其中，鉴权成功的其它物体中包括上述内容提到的被求助物。其中，一种可能地建立互信的过程如下：车辆a搜索到预设区域中的其它物体后，针对每个搜索到的物体，车辆a可以通过通信模块向该物体发送第一信令，第一信令用于请求与该物体建立之间建立通信链路，第一信令中可以携带用于指示车辆a的标识，该标识包括能够全球唯一指示该车辆的信息，例如，发动机号、车辆的序列号。该物体的通信模块接收到第一信令之后，可以对车辆a进行校验，具体身份校验过程可以由该物体根据预置的互信列表确认，也可以是该物体向第三方系统请求验证车辆a的身份，在校验成功后，向车辆a发送第二信令，第二信令用于指示该物体同意与车辆a建立互信。
图2为本申请实施例提供的一种以第一车辆为故障车辆且控制器部署在第一车辆的情况为例进一步解释本申请实施例所提供的车辆定位方法的流程的示意图，如图2所示，该方法包括：
步骤310，第一车辆在确定第一车辆的基于gps的全局位姿误差大于误差阈值，或者，在确定第一车辆的gps定位装置出现故障的情况下，向被求助物发送第一请求。
其中，第一请求用于指示第一车辆需要请求协助定位。第一请求中可以包括第一车辆的标识。其中，第一车辆的标识包括能够全球唯一指示第一车辆的信息，例如，第一车辆的发动机号、第一车辆的序列号。
相对应地，被求助物接收第一请求。
步骤311，控制器获取第一车辆与被求助物的第一相对位姿。第一相对位姿用于指示以第一车辆为基准，被求助物相对于第一车辆的位置和姿态；第一相对位姿是在第一坐标系中确定的被求助物的位姿。第一坐标系可以为第一车辆的传感器所在坐标系，例如，第一坐标系可以为以第一车辆的传感器所在中心位置为原点所建立的坐标系。
步骤312，被求助物向控制器发送被求助物的全局位姿。
全局位姿是在第二坐标系确定的被求助物的全局位姿。第二坐标系可以是以第二车辆的传感器所在坐标系，例如，第二坐标系可以为以第二车辆的传感器的中心位置为原点所建立的坐标系。
相对应地，控制器接收被求助物发送的被求助物的全局位姿。
步骤313，控制器根据第一相对位姿和被求助物的全局位姿，计算出第一车辆的全局位姿。
其中，第一车辆的全局位姿也是在上述第二坐标系下的第一车辆的全局位姿。第二坐标系可以是二维坐标系、三维坐标系等多维坐标系，例如上述提到的大地坐标系、utm网格系统等。
通过上述步骤311至步骤313提供的方案可以看出，车辆在行驶过程中，若发生了gps信号弱或者无gps信号的情况，控制器可以基于被求助物的全局位姿，以及被求助物和第一车辆的第一相对位姿计算出第一车辆的全局位姿，从而可以解决gps信号弱或者无gps信号的情况下的车辆定位问题。若应用在无人驾驶领域，则可以解决无人驾驶应用场景下gps信号弱或者无gps信号的情况下的车辆定位问题。
在一种可能地实施方式中，第一车辆可以向一个或多个被求助物请求协助。被求助物可以是车辆，这种情况下，也可以称该车辆为被求助车。第一车辆将一个车辆作为被求助车后，向该被求助车发送请求，并以该被求助车为准确定第一车辆的第一相对位姿，并基于该第一相对位姿和该被求助车的全局位姿确定第一车辆的全局位姿。可选地，第一车辆还可以选择多个被求助物请求协助。比如，第一车辆可以向k个被求助物请求协助，k为正整数，当k为大于1的正整数时，针对k个被求助物中的每个被求助物，控制器可以根据该被求助物的全局位姿，以及第一车辆与该被求助物的第一相对位姿，计算该被求助物对应的第一车辆的全局位姿。最终可以得到k个被求助物对应的k个第一车辆的全局位姿。进一步，可以根据k个被求助物对应的k个第一车辆的全局位姿，计算出该第一车辆的全局位姿，由多个被求助物对应的第一车辆的全局位姿确定出的一个第一车辆的全局位姿称为该第一车辆的目标全局位姿。
根据k个第一车辆的全局位姿计算第一车辆的全局位姿的方案可以有多种，具体实施中可以采用以下方式中任意一种：
方式一，将k个第一车辆的全局位姿中的一个作为第一车辆的全局位姿。
控制器可以从k个第一车辆的k个全局位姿中选择任意一个作为第一车辆的全局位姿。也可以根据k个全局位姿中每个全局位姿的可信度，选择k个全局位姿中可信度最高的一个全局位姿作为第一车辆的全局位姿。
一个全局位姿的可信度可以与以下参数项中的一项或多项相关：
参数项1：该全局位姿对应的被求助物发送的无线信号的强度；其中，该全局位姿对应的被求助物发送的无线信号的强度越强，则该参数项的参数值越大；反之，则该参数项的参数值越小；
参数项2：该全局位姿对应的被求助物与第一车辆之间的间距；其中，该全局位姿对应的被求助物与第一车辆之间的间距越小，则该参数项的参数值越大；反之，则该参数项的参数值越小；
参数项3：该全局位姿对应的被求助物的身份(智能电线杆、智能摄像头、车辆等等)；其中，可以根据被求助物的身份为预设被求助的身份与该参数项的参数值的对应关系，例如，被求助物的身份为车辆时，该参数项的参数值为80分，被求助物的身份为智能电线杆时，该参数项的参数值为60分，被求助物的身份为智能摄像头时，该参数项的参数值为50分。一种可能地实施方式中，可以人为的根据被求助物的身份，硬件能力等为其设置对应的参数值；
参数项4：该全局位姿计算过程中使用到的传感器类型(激光雷达传感器、毫米波雷达、相机等)；其中，可以为预设传感器类型与该参数项的参数值的对应关系，例如，传感器类型为激光雷达传感器时，该参数项的参数值为80分，传感器类型为毫米波雷达时，该参数项的参数值为60分，传感器类型为相机时，该参数项的参数值为50分。
当一个全局位姿的可信度与多个参数项相关时，可以预设每个参数项对应的权重，并对该多个参数项的参数值进行加权相加，从而得到该全局位姿的可信度。
方式二，根据k个第一车辆的全局位姿计算第一车辆的全局位姿。
控制器分别获取k个被求助物对应的k个第一车辆的k个全局位姿，然后，对k个全局位姿进行加权融合，从而得到第一车辆的目标全局位姿。对k个全局位姿进行加权融合的过程具体可以包括：为k个全局位姿确定各自对应的权重，之后对各个全局位姿进行加权相加。
一个全局位姿的权重与该全局位姿的可信度相关，比如该全局位姿的可信度越高，则该全局位姿的权重可以越大；该全局位姿的可信度越低，则该全局位姿的权重可以越小。值得说明的是，方式二中所使用的加权融合的过程可以采用卡尔曼滤波融合，卡尔曼滤波融合的具体过程可以采用传统方法或该方法的改进算法进行处理，本申请实施例对此并不做限定。
上述方式一提供的方案基于一个全局位姿计算第一车辆的目标全局位姿，该计算过程较为简单，计算速度较快。上述方式二提供的方案基于k个全局位姿计算第一车辆的目标全局位姿，可以提高第一车辆的目标全局位姿的精度。
接下来，以被求助物为第二车辆，且第一车辆和第二车辆分别安装激光雷达为例进一步介绍步骤311中确定第一相对位姿的具体过程。激光雷达所探测的障碍物信息以点云数据表示，控制器获取第一车辆的激光雷达扫描得到的第一点云数据和第二车辆的激光雷达扫描得到的第二点云数据。第一点云数据的坐标系通常是以采集该第一点云数据的设备所在位置为原点建立的多维坐标系，即以第一车辆的激光雷达所在位置为原点建立的多维坐标系。类似地，第二坐标系通常是以采集该第二点云数据的设备所在位置为原点建立的多维坐标系，即以第二车辆的激光雷达所在位置为原点建立的多维坐标系。
针对第一点云数据和第二点云数据中的每个点云数据，本申请实施例中可以对激光雷达扫描出的点云数据进行筛选，例如可以从点云数据中筛选出曲率大于曲率阈值的点云数据(该曲率大于曲率阈值的点云数据也可以理解为激光雷达扫描到的物体的表面上的点对应的点云数据)。由于第一车辆的激光雷达和第二车辆的激光雷达具有重合扫描区域，如此，第一点云数据和第二点云数据中均包括该重合扫描区域中的一个或多个障碍物对应的点云数据，(对第一点云数据和第二点云数据进行筛选，筛选后的第一点云数据和第二点云数据中均包括该重合扫描区域中的一个或多个障碍物的表面上的点对应的点云数据)。也可以理解为：该重合扫描区域中的一个障碍物的点在第一坐标系下对应一组向量的集合，且在第二坐标系下也对应一组向量的集合，进一步，可以根据该障碍物在该两个不同的坐标系下的两组向量的集合，确定出第一车辆和第二车辆之间的第一相对位姿。其中，第一相对位姿用于指示以第一车辆为基准，被求助物相对于第一车辆的位置和姿态；第一相对位姿是在第一坐标系中确定的被求助物的位姿。
下述内容将详细介绍确定第一相对位姿的过程。
方式一，分别选择第一车辆和第二坐标系，再将第一点云数据和第二点云数据在其对应的基准坐标系中进行坐标转换。
由于两辆车的激光雷达在一辆车的安装位置可能是不同的，比如第一车辆的激光雷达装在靠近车头的位置，第二车辆的激光雷达装在靠近车尾的位置，且由于激光雷达扫描到的点云数据的通常以激光雷达的位置为原点建立多维坐标系，因此，第一坐标系原点靠近第一车辆的车头，第二坐标系原点靠近第二车辆的车尾。这种情况下，若直接以第一坐标系为基准，对第二点云数据与第一点云数据进行点云匹配，则得出的点云匹配的结果反映的是：以第一车辆的车头为基准，第二车辆的车尾相对于第一车辆的车头的相对位姿。显然，这种相对位姿并不能精确的表达第一车辆和第二车辆之间的相对位姿。更加精确的第一车辆和第二车辆之间的相对位姿例如可以是：以第一车辆的车头为基准，第二车辆的车头相对于第一车辆的车头的相对位姿；或者，以第一车辆的车尾为基准，第二车辆的车尾相对于第一车辆的车尾的相对位姿。
为了更加准确的确定第一车辆和第二车辆之间的第一相对位姿，可以为第一车辆和第二车辆分别选择两个各自的基准坐标系，比如，在第一车辆上选择第一基准坐标系，在第二车辆上选择第二基准坐标系。其中，第一基准坐标系是将第一坐标系的原点平移至预设的第一基准点后得到的坐标系，即第一基准坐标系的原点为第一基准点。第二基准坐标系是将第二坐标系的原点平移至预设的第二基准点后得到的坐标系，即第二基准坐标系的原点为第二基准点。第一基准坐标系和第二基准坐标系的各个坐标轴对应平行(例如，第一基准坐标系和第二基准坐标系均为x/y/z三维坐标系，则第一基准坐标系的x轴与第二基准坐标系的x轴平行，第一基准坐标系的y轴与第二基准坐标系的y轴平行，第一基准坐标系的z轴与第二基准坐标系的z轴平行)，然后，将第一点云数据转换至第一基准坐标系下，将第二点云数据转换至第二基准坐标系下，再对转换至两个基准坐标系下的点云数据进行点云匹配，以得到点云匹配结果。
其中，第一基准点在第一车辆的位置与第二基准点在第二车辆的位置是车辆的同一个部位。具体来说，第一基准点和第二基准点可以是两辆车的车身上的点，比如第一基准点可以是指第一车辆的后轴中心，第二基准点可以是指第二车辆的后轴中心；再比如，第一基准点可以是指第一车辆的车头中轴中心，第二基准点可以是指第二车辆的车头中轴中心；再比如，第一基准点可以是指第一车辆的车顶中心，第二基准点可以是指第二车辆的车顶中心。
以第一基准点是指第一车辆的后轴中心，第二基准点是指第二车辆的后轴中心为例进行说明。首先，将第一车辆的激光雷达扫描得到的第一点云数据进行坐标系转换，使其转换至第一坐标系下。坐标系转换的过程可以是将一个坐标系的原点向基准坐标系的原点进行平移。例如，将第一坐标系原点由第一车辆的激光雷达所在位置移动至第一车辆的后轴中心，为了便于描述，将坐标系原点移动至第一车辆的后轴中心的第一点云数据称为第三点云数据。与第一车辆的点云数据进行坐标转换过程类似，第二车辆的激光雷达扫描得到的第二点云数据也需要进行坐标系转换，使其转换至第二坐标系下，将第二坐标系原点由第二车辆的激光雷达所在位置移动至第二车辆的后轴中心，为了便于描述，将坐标系原定移动至第二车辆的后轴中心的第二点云数据称为第四点云数据。
在平移不同坐标系的原点后，还需要对不同坐标系中各个坐标值进行转换，具体可以通过点坐标乘以与变换矩阵获得。例如，一个点是在坐标系s1下表示的(即采用坐标系s1下的坐标轴的坐标值表示)，若需要将该点进行坐标系转换，转换至坐标系s2下(即将该点在坐标系s2下表示)，则可以将该点乘以变换矩阵。举个例子，坐标系s1的原点在坐标系s1下的坐标轴为(0,0,0，)，在坐标系s2下为(x1，y1，z1)，(x1，y1，z1)即为坐标系s1的原点乘以变换矩阵后得到的值。
图3示例性示出了图1中的车辆a和车辆b的坐标系转换示意图，如图3所示，以车辆a为第一车辆，车辆b为第二车辆为例，车辆a扫描到的点云数据称为第一点云数据，第一点云数据的坐标轴中心位于车辆a的激光雷达上，第一点云数据所属的坐标系称为第一坐标系或激光雷达坐标系，第一点云数据进行坐标系转换的过程具体为：将其坐标系原点移动至车辆a的后轴中心，将以车辆后轴中心为圆心的坐标系(即第三点云数据所属的坐标系)称为第一坐标系。则该过程涉及到激光雷达坐标系与第一坐标系之间的转换(坐标系转换参见前述内容)。也可以说，针对第一车辆的激光雷达和第二车辆的激光雷达的重合扫描区域的一个障碍物的表面的一个点，将该点在第一点云数据对应的一组向量(该组向量可以称为该点在第一点云数据中对应的点云数据)乘以预设的第一变换矩阵，得到该点在第三点云数据对应的一组向量(该组向量可以称为该点在第三点云数据中对应的点云数据)。由于车辆a的激光雷达在安装时已经固定，车辆后轴中心也已确定，因此该第一变换矩阵也可以确定。
类似地，车辆b扫描到的点云数据称为第二点云数据，第二点云数据的坐标轴中心位于车辆b的激光雷达上，也就是说，第二点云数据是在第二车辆的第二坐标系中所得的数据。将第二点云数据进行坐标系转换，将其坐标系原点移动至车辆b的后轴中心。具体转换过程与前述内容类似，需要说明的是，将第二点云数据与第四点云数据之间进行坐标系转换过程中使用到的变换矩阵称为第二变换矩阵，第二变换矩阵是以车辆b的后轴中心为基准，车辆b的激光雷达的中心相对于车辆a的后轴中心需要进行的线性变换对应的变换矩阵。由于车辆a的激光雷达的安装位置与车辆b的安装位置可以不同，因此第一变换矩阵和第二变换矩阵也可以不同。
进一步，以第一基准坐标系为基准，根据第三点云数据和第四点云数据，计算出第二基准坐标系与第一基准坐标系之间的第三变换矩阵。如何根据两个点云数据计算变换矩阵，后续内容将详细说明。
作为一种可能的实现方式，除了分别设置两个基准坐标系转换第一点云数据和第二点云数据外，还可以仅选择一个基准坐标系(例如可以是上述内容中提到的大地坐标系、utm网格系统、第二坐标系等)，分别将第一点云数据和第二点云数据分别转换至该基准坐标系中，然后再对转换后的点云数据进行点云匹配，进而确定点云匹配结果所获得的第三变换矩阵。具体转换过程与上述第一车辆和第二车辆分别选择基准坐标系时获得第三点云数据和第四点云数据的方法类似，为了简洁，在此不再赘述。
方式二，选择第一坐标系作为第一点云数据和第二点云数据坐标转换的基准坐标系。
在该方式二中，需要有前提条件，即第一车辆的激光雷达在第一车辆的安装位置，与第二车辆的激光雷达在第二车辆的安装位置对应。例如，第一车辆的激光雷达安装在第一车辆的车顶中间位置，第二车辆的激光雷达安装在第二车辆的车顶中间位置。再例如，第一车辆的激光雷达安装在第一车辆的车顶上靠近车头位置，第二车辆的激光雷达安装在第二车辆的车顶上靠近车头位置。再例如，第一车辆的激光雷达安装在第一车辆的车顶上靠近车尾位置，第二车辆的激光雷达安装在第二车辆的车顶上靠近车尾位置。
一种可能地实施方式中，可以计算第一车辆的激光雷达的安装位置与第一车辆的车头的距离，以及第二车辆的激光雷达的安装位置与第二车辆的车头的距离，该两个距离小于距离阈值，则可以确定：第一车辆的激光雷达在第一车辆的安装位置，与第二车辆的激光雷达在第二车辆的安装位置对应。
进而，在方式二中，可以以第一坐标系为基准，根据第一点云数据和第二点云数据，计算出第二坐标系与第一坐标系之间的第三变换矩阵。
下面进一步介绍如何根据两个点云数据计算第三变换矩阵的方法，该方法中以根据第三点云数据和第四点云数据计算第三变换矩阵为例进行说明。对第三点云数据和第四点云数据进行点云匹配的过程也可以理解为：以第三点云数据所属的第一基准坐标系为基准，求解将第四点云数据从所属的第二基准坐标系下转换至第一基准坐标系所进行的线性变换对应的第三变换矩阵。也就是说，点云匹配过程就是求解第三变换矩阵的过程。
对第三点云数据和第四点云数据的进行点云匹配的关系可以采用下述公式(1)来表达：
在公式(1)中，第一车辆编号为v1，第二车辆的编号为v2，
可以通过迭代最近邻(iterativeclosestpoint，icp)算法求解上述公式(1)中的第三变换矩阵。icp算法是通过n次迭代求解第三变换矩阵的过程，n为正整数。针对每次迭代过程，该次迭代过程采用的第三变换矩阵是将上次迭代所输出的第三变换矩阵乘以预设变换量所得到。针对第一车辆的激光雷达和第二车辆的激光雷达的重合扫描区域内的障碍物的m个点中的每个点(其中，障碍物的m个点可以是第一车辆的激光雷达和第二车辆的激光雷达均扫描到的该障碍物的表面的m个点，m为正整数)，以该点为基准，将该点在第四点云数据中对应的点云数据乘以第三变换矩阵，得到该点对应的变换后点云数据，将变换后点云数据称为第五点云数据，则针对该点在第五点云数据中对应的点云数据，以及该点在第四点云数据中对应的点云数据，计算两者之间的残差。计算两个点云数据间的残差(也可以理解为误差)，由于两个点云数据对应两组向量，因此计算两个点云数据之间的残差可以理解为求解该两组向量之间的残差，求解向量之间的残差也可以理解为求解两组向量之间的欧几里得度量(euclideanmetric)(也称欧氏距离)。
针对障碍物中的m个点对应的m个残差，可以进一步计算m个点的残差总和，若该残差总和小于预设的残差阈值，则说明该次迭代结果满足要求，可以将该次迭代过程中使用的第三变换矩阵作为最后求解的第三变换矩阵。若该残差总和不满足要求，则需要以预设的变换量调整该第三变换矩阵，将该调整后的第三变换矩阵作为下一次迭代过程的第三变换矩阵。
用icp算法来进行点云匹配的过程可以用下述公式(2)来表示：
在公式(2)中，第一车辆编号为v1，第二车辆的编号为v2，
图4为本申请实施例提供的一种通过icp算法求解公式(2)中的第三变换矩阵的流程示意图，如图4所示，包括：
步骤301，开始。
步骤302，初始化第三变换矩阵δt，δt为第三变换矩阵，对应线性变换为旋转和平移，也可以将第三变换矩阵表示为一个旋转矩阵r和平移矩阵t。该步骤302也可以描述为初始化旋转矩阵r和平移矩阵t。
在步骤302中，一种可能地实施方式中，初始化第三变换矩阵具体可以是指：将下属内容中提到的第三变换矩阵的初始值赋值给δt。
该第三变换矩阵的初始值，也可以理解为下述内容中进行第一次迭代所使用的第三变换矩阵，即下述内容中提到的n次迭代中，当i为1时，第i-1次迭代输出的第三变换矩阵。
另一种可能地实施方式中，初始化第三变换矩阵具体可以是指：给δt赋值，一种可能地示例中，旋转矩阵r和平移矩阵t有如下取值：
下面开始进行n次迭代，n次迭代中以第1次迭代为起始开始迭代。步骤303至步骤307是第i次迭代的过程，i为不大于n的正整数：
步骤303，针对障碍物的m个点中的每个点执行：根据第i-1次迭代输出的第三变换矩阵，对该点在第三点云数据中对应的点云数据执行一次变换，得到该点对应的变换后点云数据。m为正整数。
针对障碍物的m个点中的每个点xi，计算(rxi+t)。
(rxi+t)也可以写为(xi*δt)。在步骤303中，将该点在第三点云数据中对应的点云数据进行旋转和平移后得到该点对应的变换后点云数据。
步骤304，根据m个点中的每个点对应的变换后点云数据，以及每个点在第四点云数据中对应的点云数据，得到m个点中每个点对应的残差。
在步骤304中，可以通过如下方式确定出该点在第四点云数据中对应的点云数据：
针对第四点云数据的一个点云数据，若该点云数据与该点对应的变换后点云数据满足以下条件，则确定该点云数据为：该点在第四点云数据中对应的点云数据；其中，该条件包括：该点对应的变换后点云数据与该点云数据之间对应的距离小于预设的距离阈值。其中，该两个点云数据之间对应的距离可以是欧几里得度量，可以是指在多维(二维或二维以上)空间中两个点之间的真实距离。在二维和三维空间中的欧氏距离就是两点之间的实际距离。
针对m个点中的每个点，计算该点对应的变化后点云数据和该点在第四点云数据中对应的点云数据之间对应的距离，可以作为该点对应的残差。
步骤305，根据障碍物的m个点中每个点对应的残差，计算残差总和。
步骤306，判断残差总和是否小于残差阈值，若不是则执行步骤307，若是，则执行步骤308。
在步骤306中，残差阈值可以是预设的值，比如可以是10-10。
步骤307，在确定迭代次数不大于迭代次数阈值的情况下，以预设变换量更新第i-1次迭代输出的第三变换矩阵，并将更新后的第i-1次迭代输出的第三变换矩阵作为：第i次迭代输出的第三变换矩阵，并执行下一次迭代，以重复执行步骤303，从而进入下次迭代。
在步骤307中，设置有迭代次数阈值，若迭代次数大于迭代次数阈值，则结束该进程，不再进行迭代。迭代次数阈值可以是预设的一个值，以防止迭代次数过多的问题。
在步骤307中，预设的变换量可以包括预设的旋转步长和预设的平移步长，可以通过高斯牛顿法来计算。
步骤308，若残差总和小于残差阈值，则结束迭代，将第i-1次迭代输出的第三变换矩阵作为：第n次迭代输出的第三变换矩阵。
一种可能地实施方式中，将第n次迭代输出的第三变换矩阵作为第一相对位姿的数学表达形式。也就是说，第n次迭代输出的第三变换矩阵可以作为本申请实施例中的第一相对位姿的一种数学表达方式。本申请实施例中第一相对位姿是指以第一车辆为基准，第二车辆相对于第一车辆的位置和姿态，该位置和姿态可以通过第n次迭代输出的第三变换矩阵来表示。第一相对位姿用变换矩阵来表达(例如第n次迭代输出的第三变换矩阵，或下述内容提到的第四变换矩阵)仅仅是一种可能地实现方式，具体实施例中，也可以用其它数学表达式来表达，例如，四元素、角轴、欧拉角等。
上述icp算法的迭代过程使用到的第三点云数据和第四点云数据是由第一点云数据和第二点云数据进行坐标系转换得到的，因此第一点云数据和第二点云数据需要保证是同一个时刻由两个激光雷达采集到的数据，为了保证此要求，可以设置以第一车辆的无线时钟源为基准，调整第二车辆的时钟，以使第一车辆和第二车辆的时钟同步，第一点云数据和第二点云数据均携带有各自的时间戳，该时间戳可以有多种表示方法，该时间戳可以是当前时间与预设时间之间的增量值，该预设时间比如可以是指定的2020年12月31日0点0分0秒。
在具体实施中，当第一车辆和第二车辆距离较远时，上述icp算法可能迭代次数会过多，而且很容易陷入局部极值的情况。为了应对这个问题，本申请实施例中可以为icp算法迭代提供一个第三变换矩阵的初始值，也可以说是为点云匹配提供一个初始值。该第三变换矩阵的初始值，也可以说是上述内容中当i为1时，第i-1次迭代输出的第三变换矩阵，即第三变换矩阵的初始值也可以称为第0次迭代输出的第三变换矩阵。上述迭代过程中是从第1次迭代起始的。
下面介绍一种确定第三变换矩阵的初始值的方案。
本申请实施例中提供的第三变换矩阵的初始值可以是基于第一车辆的预估全局位姿以及第二车辆的全局位姿确定的，第二车辆的gps没有损坏，因此第二车辆的全局位姿是较准确的，而第一车辆的gps出了故障，本申请实施例基于其它方案预估出第一车辆的预估全局位姿(下述内容将介绍如何计算第一车辆的预估全局位姿)，该值是预估的，因此准确度没有第二车辆的全局位姿高，根据第一车辆当前时刻的预估全局位姿和第二车辆当前时刻的全局位姿，确定出第二相对位姿，第二相对位姿用于指示以第一车辆为基准，被求助物相对于第一车辆的位置和姿态；第二相对位姿是在第一坐标系中确定的被求助物的位姿。将用于表达第二相对位姿的矩阵作为：第i-1次迭代输出的第三变换矩阵。也可以说：将用于表达第二相对位姿的矩阵作为：第三变换矩阵的初始值。计算出的第三变换矩阵的初始值虽然准确度不是很高，但是较为接近第n次迭代输出的第三矩阵(第n次迭代输出的第三矩阵可以称为最优解)，如此，基于第三变换矩阵的初始值进行icp算法的迭代，就好比在距离最优解较近的地方寻找该最优解，明显可以缩小寻找最优解的搜索范围，迭代次数可以明显减少，从而可以大大的提高算法收敛的速度和精度。
需要注意的是，本申请实施例提到的最优解，最优解是数学规划的基本概念之一。指在数学规划问题中，使目标函数取最小值(对极大化问题取最大值)的可行解。使目标函数取最小值的可行解称为极小解，使其取最大值的可行解称为极大解。极小解或极大解均称为最优解。相应地，目标函数的最小值或最大值称为最优值。有时，也将最优解和最优值一起称为相应数学规划问题的最优解。这里的目标函数是指上述公式(2)，求解公式(2)的过程可以理解为求解第三矩阵的最优解。
第一车辆的当前时刻的预估全局位姿可以根据前一时刻第一车辆的全局位姿，以及基于imu计算出的：以前一时刻第一车辆的全局位姿为基准，第一车辆在当前时刻相对于前一时刻的位姿，计算得到。下面进行详细介绍。
方式a1：前一时刻第一车辆的全局位姿可以是系统记录的最近一次有效的基于gps的全局位姿。
在一种可能地实施方式中，在gps失效(比如基于gps的全局位姿误差大于误差阈值，或者，gps定位装置出现故障)的情况下，系统记录有最近的一次有效的基于gps的全局位姿(第一车辆的最近的一次有效的基于gps的全局位姿也可以称为第一车辆的前一帧的基于gps的全局位姿)。在方式a1中，将系统记录的最近的一次有效的基于gps的全局位姿的时刻称为第一时刻，第一车辆当前时刻的预估全局位姿基于以下内容得到：第一车辆前一帧的基于gps的全局位姿，以及第一车辆在当前时刻的位姿和第一车辆在第一时刻的位姿之间的相对位姿。
在计算一辆车在一个时间段内发生的相对位姿的方法有多种，比如基于轮式里程计、视觉里程计、惯性测量单元等进行计算得到。通常可以采用数据预积分的方法计算一个时间段内发生的相对位姿。为了介绍方便，下面以imu方法进行示例。
在方式a1中可以基于imu计算第一时刻至当前时刻这段时长第一车辆的位姿变化量(也可以称为第一车辆在当前时刻的位姿和第一车辆在第一时刻的位姿之间的相对位姿)。结合第一时刻的第一车辆的基于gps的全局位姿，以及第一时刻至当前时刻基于imu计算出的相对位姿，可以得到第一车辆的当前时刻的全局位姿。
在方式a1中，第一车辆当前时刻的预估全局位姿可以通过下述公式(3)来确定：
在公式(3)中，第一车辆编号为v1，第二车辆的编号为v2，
公式(3)也可以理解为：基于imu计算第一车辆的当前时刻与第一时刻第一车辆的全局位姿之间的相对位姿
在方式a1中，第二车辆基于gps和imu得到的当前时刻的全局位姿。因为第二车辆的gps未失效，但是由于基于gps获知的第二车辆的全局位姿本身会有一定的延迟，因此在基于gps对第二车辆进行定位时，可以将基于gps得到的信息和基于其它技术(例如imu)得到的信息融合，将融合后的结果作为第二车辆的当前时刻的全局位姿。这种将基于gps得到的信息和基于其它技术(例如imu)得到的信息进行融合以实现定位的方式可以称为组合定位。具有来说，先确定收到的最新的第二车辆的基于gps的全局位姿的时刻，之后基于imu计算接收到最新的第二车辆的基于gps的全局位姿的时刻至当前时刻这段时间内第二车辆可能行驶的距离，结合二者确定出第二车辆的当前时刻的全局位姿。
在方式a1中，可以通过下述公式(4)计算第一车辆和第二车辆的第三变换矩阵的初始值：
在公式(4)中，第一车辆编号为v1，第二车辆的编号为v2，
图5示例性示出了图1中的车辆a的当前时刻的预估全局位姿的计算示意图，如图5所示，以车辆a为第一车辆进行示例，车辆a的当前时刻的预估全局位姿可以根据车辆a的第一时刻的全局位姿和车辆a在第一时刻至当前时刻这段时长内的相对位姿来确定。如图5所示，车辆a在第一时刻的全局位姿可以是上述公式(3)中的
方式a2，前一时刻第一车辆的全局位姿可以是最近一次采用上述图3提供的方法得到的第一车辆的全局位姿。
在方式a2中，一种可能地实施方式中，在本次计算第一车辆的全局位姿之前，已经至少根据上述步骤310至步骤313提供的方案确定出了一次第一车辆的全局位姿。在方式a2中，最近一次计算出的第一车辆的全局位姿的时刻为第二时刻，则一种可选地实施方式中，第一车辆当前时刻的预估全局位姿基于以下内容得到：以最近一次计算出的第一车辆的全局位姿(即第一车辆在第二时刻的全局位姿)为基准，第一车辆在当前时刻相对于第二时刻的位姿变换量，也可以说是第一车辆在第二时刻和当前时刻之间的相对位姿。
在方式a2中，第二车辆得到当前时刻的全局位姿的方式与方式a1中相同，不再赘述。且基于第一车辆当前时刻的预估全局位姿和第二车辆当前时刻的全局位姿计算第一车辆和第二车辆的第三变换矩阵的初始值可以采用上述公式(4)，不再赘述。
图5示例性示出了图1中的车辆a的当前时刻的预估全局位姿的计算示意图，如图5所示，以车辆a为第一车辆进行示例，车辆a的当前时刻的预估全局位姿可以根据车辆a的第二时刻的全局位姿和车辆a在第二时刻至当前时刻这段时长内的相对位姿来确定。如图5所示，车辆a在第二时刻的全局位姿可以是根据上述步骤311至步骤313提供的方案确定出的一次第一车辆的全局位姿。车辆a在第二时刻至当前时刻这段时长内的相对位姿可以是基于以下内容得到：前一时刻计算出的第一车辆的全局位姿和imu得到。
需要说明的是，上述方式a1和方式a2的选用时机，上述方式a1和方式a2可以灵活选用，本申请实施例提供一种可能地实施方案，可以在第一车辆的gps失效后第一次采用上述图2提供的方案计算第一车辆的全局位姿时采用上述方式a1计算第一车辆和第二车辆的第三变换矩阵的初始值。在第二次以及以后采用上述图2提供的方案计算第一车辆的全局位姿时采用上述方式a2计算第一车辆和第二车辆的第三变换矩阵的初始值。
一种可能地实施方式中，将第n次迭代输出的第三变换矩阵作为第一相对位姿的数学表达形式。另一种可能地实施方式中，对第三变换矩阵的初始值和第n次迭代输出的第三变换矩阵进行加权融合，得到第四变换矩阵，将第四变换矩阵作为第一相对位姿的数学表达形式。加权融合可以采用卡尔曼滤波融合。
在对第n次迭代输出的第三变换矩阵和第三变换矩阵的初始值进行卡尔曼滤波融合的过程中，为第n次迭代输出的第三变换矩阵和第三变换矩阵的初始值确定各自对应的权重，之后对第n次迭代输出的第三变换矩阵和第三变换矩阵的初始值进行加权相加，从而得到第四变换矩阵。将第四变换矩阵作为第一相对位姿的数学表达形式。第n次迭代输出的第三变换矩阵的权重与其可信度相关，第三变换矩阵的初始值的权重与其可信度相关。由于第三变换矩阵的初始值是根据第一车辆的预估的全局位姿确定的，因此可信度较低，而第n次迭代输出的第三变换矩阵的可信度较高，基于此，可以设定第一相对位姿作为卡尔曼滤波融合的观测模型(观测模型的可信度较高，)，第三变换矩阵的初始值作为卡尔曼滤波融合的预测模型(预测模型的可信度较低)。
第一车辆的全局位姿可以通过下述公式(5)来确定：
在公式(5)中，第一车辆编号为v1，第二车辆的编号为v2，
上述方案介绍了被求助物为第二车辆，且通过第一车辆和第二车辆上安装的激光雷达来辅助确定第一车辆和第二车辆的第一相对位姿的方案。下面介绍几种其它用于确定第一车辆和被求助物之间的第一相对位姿的方案。
作为一种可能的实施例，除了将实施车辆定位的控制器部署在第一车辆中，还可以部署在被求助物上，这种情况下，相对于上述图2所示的方案的不同之处在于，控制器可以直接获取被求助物的全局位姿，不需要通过上述步骤312来获取被求助物的全局位姿。另一方面，当控制器确定出第一车辆的全局位姿之后，还需要向第一车辆发送该第一车辆的全局位姿。这种实施方式中，由于服务器部署在被求助物上，因此可以减少第一车辆的计算量，即减轻故障车辆的计算负担。
作为另一种可能的实施例，实施车辆定位的控制器还可以部署在云端，这种情况下，相对于上述图2所示的方案的不同之处在于，被求助物可以在接收到上述步骤310中的第一请求之后，可以将被求助物的全局位姿发送至云端的控制器。另一种可能地实施方式中，第一车辆可以向云端的控制器发送第一请求，并由部署于云端的控制器向被求助物发送第一请求，之后被求助物将被求助物的全局位姿可以发送至云端的控制器。在该实施方式中，当控制器计算出第一车辆的全局位姿之后，需要向第一车辆发送该第一车辆的全局位姿。这种实施方式中，由于将服务器部署在云端，因此可以减少第一车辆或第二车辆的计算量，减轻第一车辆或第二车辆的负担。
在另一种可能地实施方式中，除了图2的方法中以被求助物为可以是第二车辆且第二车辆上部署激光雷达以外，第一车辆和第二车辆上也可以安装有相机，相机传感器可以是全景相机传感器。第一车辆可以通过相机传感器连续拍摄两帧第一图像，每个图像为二维坐标系(例如x/y坐标系)下的图像，图像中的特征点对应x轴和y轴的坐标。结合两帧第一图像可以确定出该第一图像中的特征点的深度信息，即z轴坐标。也就是说，第一车辆可以通过相机传感器获取障碍物的特征点的x轴、y轴和z轴的坐标，该特征点的x轴、y轴和z轴的坐标构成一组向量，这种情况下可以称获取了该点对应的点云数据。可以看出，通过第一车辆的相机传感器也可以获取第一图像对应的点云数据。类似地，通过第二车辆的相机传感器获取第二图像对应的点云数据。第一车辆的相机传感器和第二车辆的相机传感器具有重合的拍摄区域，进而可以根据该重合的拍摄区域的障碍物的特征点在第一图像的点云数据中对应的点云数据，以及该特征点在第二图像的点云数据中对应的点云数据进行点云匹配，从而确定出第一车辆和第二车辆的第一相对位姿。结合两个点云数据进行点云匹配从而确定第一相对位姿的方案可以参见上述内容，在此不再赘述。进一步，根据第一相对位姿以及第二车辆的全局位姿确定出第一车辆的全局位姿。
在另一种可能地实施方式中，被求助物可以是配置有通信模块的基础设施，例如上述内容提到的配置有通信模块的路灯，或者还可以是路边的基站，还可以是配置有通信模块的摄像头。
当被求助物为配置有通信模块的路灯或者为基站等基础设置，以路灯为例进行介绍，第一车辆可以接收路灯发送的信号，可以根据信号的强度判断出路灯和第一车辆之间的相对位置。路灯可以将自身的全局位置发送给车辆，之后第一车辆可以基于相对位置和路灯的位置，确定出第一车辆自身的位置。另一方面，第一车辆可以估计出自身的姿态，例如：根据前一时刻所述第一车辆的姿态，以及基于imu计算出的：以前一时刻第一车辆的姿态为基准，所述第一车辆在当前时刻相对于所述前一时刻的姿态，计算所述第一车辆当前时刻的姿态。
当被求助物为配置有通信模块的摄像头，摄像头可以拍摄两帧包括有第一车辆的第三图像，每个图像为二维坐标系(例如x/y坐标系)下的图像，图像中第一车辆的特征点对应x轴和y轴的坐标。结合两帧第三图像可以确定出该第三图像中第一车辆的特征点的深度信息，即z轴坐标。第一车辆的特征点的x轴、y轴和z轴的坐标可以作为第一车辆的全局姿态信息的一种表示方式。
上文中结合图1至图5，详细描述了根据本申请实施例所提供的车辆定位的方法，下面将结合图6至图7，描述根据本申请实施例所提供的车辆定位的控制器和系统。
图6为本申请实施例提供的一种车辆定位装置的结构的示意图，如图所示，装置1501包括获取单元1502和计算单元1503。
获取单元1502，用于获取第一车辆与被求助物的第一相对位姿，获取被求助物的全局位姿；第一相对位姿用于指示以第一车辆为基准，被求助物相对于第一车辆的位置和姿态；第一相对位姿是在第一坐标系中确定的被求助物的位姿；全局位姿是在第二坐标系确定的被求助物的全局位姿；
计算单元1503，用于根据第一相对位姿和全局位姿，计算第一车辆的全局位姿。如此，在第一车辆的gps信号弱或者无gps信号的情况下，可以基于被求助物的全局位姿，以及第一车辆和被求助物之间的第一相对位姿来确定出第一车辆的全局位姿。
应理解的是，本申请实施例的装置1501可以通过专用集成电路(application-specificintegratedcircuit，asic)实现，或可编程逻辑器件(programmablelogicdevice，pld)实现，上述pld可以是复杂程序逻辑器件(complexprogrammablelogicaldevice，cpld)，现场可编程门阵列(field-programmablegatearray，fpga)，通用阵列逻辑(genericarraylogic，gal)或其任意组合。也可以通过软件实现图2至图5所示的车辆定位方法时，装置1501及其各个模块也可以为软件模块。
在一种可能地实施方式中，被求助物为第二车辆，第一相对位姿用于指示以第一车辆为基准，第二车辆相对于第一车辆的位置和姿态，姿态用于指示以第一车辆的车头朝向为基准，第二车辆的车头的朝向。如此，在第一车辆的gps信号弱或者无gps信号的情况下，可以向其他车辆求助，且可以基于其他车辆的信息，确定出第一车辆的车头朝向，从而在智能车领域可以辅助其实现自动驾驶。
在一种可能地实施方式中，获取单元1502，还用于：获取第一车辆的激光雷达进行扫描第一车辆周围物体所得到的在第一坐标系的第一点云数据；获取第二车辆的激光雷达进行扫描第二车辆周围物体所得到的在第二坐标系的第二点云数据；其中，第一车辆的激光雷达和第二车辆的激光雷达具有重合扫描区域，且针对重合扫描区域中的障碍物，第一点云数据中包括障碍物对应的点云数据，第二点云数据中包括障碍物对应的点云数据；根据障碍物在第一点云数据中对应的点云数据，以及障碍物在第二点云数据中对应的点云数据，计算第一相对位姿。如此，可以根据激光雷达扫描出的点云数据进行点云匹配，从而确定出较为准确的两辆车之间的相对位姿。
在一种可能地实施方式中，获取单元1502，还用于：将第一坐标系中第一点云数据转换至预设的第一车辆的第一基准坐标系，获得第三点云数据；其中，第一基准坐标系是将第一坐标系的原点平移至预设的第一基准点后得到的坐标系；将第二坐标系中第二点云数据转换至预设的第二车辆的第二基准坐标系，获得第四点云数据；其中，第二基准坐标系是将第二坐标系的原点平移至预设的第二基准点后得到的坐标系；对障碍物在第三点云数据中对应的点云数据，以及障碍物在第四点云数据中对应的点云数据进行点云匹配，得到第一相对位姿。如此，可以将第一点云数据和第二点云数据均转换至各自车辆的基准坐标系下，各个车辆的基准坐标系的原点相对于各自的车辆的位置都是对应的，例如各个车辆的基准坐标系的原点均位于各自的车辆的后轴中心，从而可以较为准确的确定出两辆车之间的相对位姿。
在一种可能地实施方式中，获取单元1502，还用于：根据障碍物在第三点云数据中对应的点云数据，以及障碍物在第四点云数据中对应的点云数据，执行n次迭代，得到第n次迭代输出的第三变换矩阵；n为正整数；根据第n次迭代输出的第三变换矩阵，确定第一相对位姿；其中，针对n迭代中的第i次迭代，i为不大于n的正整数：针对障碍物的m个点中的每个点执行：根据第i-1次迭代输出的第三变换矩阵，对该点在第三点云数据中对应的点云数据执行一次变换，得到该点对应的变换后点云数据；计算该点对应的变换后点云数据和该点在第四点云数据中对应的点云数据的差值，得到该点对应的残差；m为正整数；根据障碍物的m个点中每个点对应的残差，计算残差总和；若残差总和不小于预设的残差阈值，则以预设变换量更新第i-1次迭代输出的第三变换矩阵，并将更新后的第i-1次迭代输出的第三变换矩阵作为：第i次迭代输出的第三变换矩阵，并执行下一次迭代；若残差总和小于残差阈值，则结束迭代，将第i-1次迭代输出的第三变换矩阵作为：第n次迭代输出的第三变换矩阵。
在一种可能地实施方式中，获取单元1502，还用于：当i为1时，通过如下内容确定第i-1次迭代输出的第三变换矩阵：根据前一时刻第一车辆的全局位姿，以及基于imu计算出的：以前一时刻第一车辆的全局位姿为基准，第一车辆在当前时刻相对于前一时刻的位姿，计算第一车辆当前时刻的预估全局位姿；获取第二车辆基于全球定位系统gps和imu得到的当前时刻的全局位姿；根据第一车辆当前时刻的预估全局位姿和第二车辆当前时刻的全局位姿，确定出第二相对位姿，第二相对位姿用于指示以第一车辆为基准，被求助物相对于第一车辆的位置和姿态；第二相对位姿是在第一坐标系中确定的被求助物的位姿；将用于表达第二相对位姿的矩阵作为：第i-1次迭代输出的第三变换矩阵。
在一种可能地实施方式中，获取单元1502，还用于：第i-1次迭代输出的第三变换矩阵，还用于：将第n次迭代输出的第三变换矩阵作为第一相对位姿的数学表达形式。在另一种可能地实施方式中，获取单元1502，还用于：对第i-1次迭代输出的第三变换矩阵和第n次迭代输出的第三变换矩阵进行加权融合，得到第四变换矩阵，将第四变换矩阵作为第一相对位姿的数学表达形式。
在一种可能地实施方式中，装置1501还包括确定单元1504，用于：确定第一车辆的基于gps的全局位姿误差大于误差阈值；或者；第一车辆在第一车辆的gps定位装置出现故障。
在一种可能地实施方式中，获取单元1502，还用于：获取多个被求助物中每个被求助物对应的第一车辆的全局位姿；计算单元1503，还用于：对多个被求助物对应的多个第一车辆的全局位姿进行加权融合，得到第一车辆的目标全局位姿。
装置1501还包括发送单元1505和接收单元1506，发送单元1505用于：向被求助物发送第一请求；第一请求中携带第一车辆的标识。接收单元1506，用于：接收被求助物发送的被求助物的全局位姿。
根据本申请实施例的装置1501可对应于执行本申请实施例中描述的方法，并且装置1501中的各个单元的上述和其它操作和/或功能分别为了实现图2至图5中的各个方法的相应流程，为了简洁，在此不再赘述。
图7为本申请实施例提供的一种控制器结构的示意图，如图所示，控制器1301包括处理器1302、存储器1304和通信接口1303。其中，处理器1302、存储器1304和通信接口1303可以通过总线1305进行通信，也可以通过无线传输等其他手段实现通信。该存储器1304用于存储指令，该处理器1302用于执行该存储器1304存储的指令。该存储器1304存储程序代码，且处理器1302可以调用存储器1304中存储的程序代码执行以下操作：
处理器1302，用于获取第一车辆与被求助物的第一相对位姿，获取被求助物的全局位姿；第一相对位姿用于指示以第一车辆为基准，被求助物相对于第一车辆的位置和姿态；第一相对位姿是在第一坐标系中确定的被求助物的位姿；全局位姿是在第二坐标系确定的被求助物的全局位姿；处理器1302，用于根据第一相对位姿和全局位姿，计算第一车辆的全局位姿。如此，在第一车辆的gps信号弱或者无gps信号的情况下，可以基于被求助物的全局位姿，以及第一车辆和被求助物之间的第一相对位姿来确定出第一车辆的全局位姿。
应理解，在本申请实施例中，该处理器1302可以是中央处理单元(centralprocessingunit，cpu)，该处理器1302还可以是其他通用处理器、数字信号处理器(digitalsignalprocessing，dsp)、专用集成电路(asic)、现场可编程门阵列(fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者是任何常规的处理器等。
该存储器1304可以包括只读存储器和随机存取存储器，并向处理器1302提供指令和数据。存储器1304还可以包括非易失性随机存取存储器。例如，存储器1304还可以存储设备类型的信息。
该存储器1304可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(read-onlymemory，rom)、可编程只读存储器(programmablerom，prom)、可擦除可编程只读存储器(erasableprom，eprom)、电可擦除可编程只读存储器(electricallyeprom，eeprom)或闪存。易失性存储器可以是随机存取存储器(randomaccessmemory，ram)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的ram可用，例如静态随机存取存储器(staticram，sram)、动态随机存取存储器(dram)、同步动态随机存取存储器(synchronousdram，sdram)、双倍数据速率同步动态随机存取存储器(doubledatadatesdram，ddrsdram)、增强型同步动态随机存取存储器(enhancedsdram，esdram)、同步连接动态随机存取存储器(synchlinkdram，sldram)和直接内存总线随机存取存储器(directrambusram，drram)。
该总线1305除包括数据总线之外，还可以包括电源总线、控制总线和状态信号总线等。但是为了清楚说明起见，在图中将各种总线都标为总线1305。
应理解，根据本申请实施例的车辆定位的控制器1301可对应于本申请实施例中的车辆定位的装置的装置1501，并可以对应于执行根据本申请实施例的方法中的相应主体，并且控制器1301中的各个模块的上述和其它操作和/或功能分别为了实现图2至图5中的各个方法的相应流程，为了简洁，在此不再赘述。
本申请还提供一种车辆，该车辆上包括如图7所示的控制器1301。该车辆可以对应图2所示方法中第一车辆，用于实现如图2至图5中各个方法的操作步骤，为了简洁，在此不再赘述。
本申请还提供一种车辆，该车辆包括如图7所示的控制器1301。此时，该车辆可以为被求助车辆，用于利用图2至图5所示的各个方法的操作步骤实现辅助gps故障车辆定位的过程。
本申请还提供一种系统，该系统包括第一车辆和云端的计算平台，该云端的计算平台用于实现上述方法中由云端的计算平台辅助gps故障的第一车辆进行定位的过程。
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行计算机指令时，全部或部分地产生按照本申请实施例的流程或功能。计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(digitalsubscriberline，dsl))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。可用介质可以是磁性介质(例如，软盘、硬盘、磁带)、光介质(例如，高密度数字视频光盘(digitalvideodisc，dvd))、或者半导体介质(例如，固态硬盘(solidstatedisc，ssd))等。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以权利要求的保护范围为准。
- 还没有人留言评论。精彩留言会获得点赞！