CN112033395B - 移动平台定位方法、装置、电子设备及存储介质 - Google Patents

Abstract

本申请提供一种移动平台定位方法、装置、电子设备及存储介质，涉及设备定位技术领域。所述方法包括：获取移动平台的第一测距值和第二测距值，第一测距值和第二测距值分别为后测距仪和前测距仪的测量值；基于第一测距值、第二测距值和总路径的路径参数，确定移动平台所处的当前路径；在当前路径为直行路径时，基于第一测距值、第二测距值、路径参数和后测距仪与前测距仪之间的测距仪距离确定移动平台的位置，或者，基于移动平台上一时刻的位置以及移动平台的移动速度确定移动平台的位置。上述方法通过融合双测距仪的测距值提高测距速率，解决了高精度测距仪速度低和无法进行弯道定位的问题，从而提高了移动平台的定位精度和适用性。

Description

移动平台定位方法、装置、电子设备及存储介质

技术领域

本申请涉及设备定位技术领域，具体而言，涉及一种移动平台定位方法、装置、电子设备及存储介质。

背景技术

在建筑机器人领域，特别是在外墙作业机器人领域，移动平台在导轨上移动，移动轨迹固定时，对精度要求高，要求毫米级定位精度要求。目前通常采用的位置检测方法有编码器测量、激光测距、超宽带测距等。

编码器测量是使用测量编码器距离归零点的距离，从而完成定位。编码器在短距离时的精度较高，可以达到毫米级，但是由于存在着累积误差，因此随着作业时间变长误差会越来越大。因而无法满足工业机器人长时间的作业。

激光测距通常有两种方法，一种是采用脉冲式激光测距仪，这种方法具有测量速率高的优点，但是精度较低，难以满足外墙场景下毫米级的精度要求；一种是采用相位式激光测距仪，这种方法具有测量精度高，达到毫米级精度，但是测量速率低的特点。这两种激光测距仪，要么精度不够，要么速率不够，均不能满足外墙场景下的作业要求。

超宽带测距特点与脉冲式激光测距仪特点类似，测量速率比较高，但是精度低。同时还有在狭窄空间内易受多径干扰的特点。因此不能满足外墙场景下毫米级的定位精度要求。

因此现有的移动平台定位技术仍然存在精确度不高、适用性较低的问题。

发明内容

有鉴于此，本申请实施例的目的在于提供一种移动平台定位方法、装置、电子设备及存储介质，以改善现有技术中存在的移动平台定位精确度和适用性较低的问题。

本申请实施例提供了一种移动平台定位方法，所述方法包括：获取移动平台的第一测距值和第二测距值，所述第一测距值为设置在所述移动平台的后端的后测距仪的测量值，所述第二测距值为设置在所述移动平台的前端的前测距仪的测量值；基于所述第一测距值、所述第二测距值和总路径的路径参数，确定所述移动平台所处的当前路径；在所述当前路径为直行路径时，基于所述第一测距值、所述第二测距值、所述路径参数和所述后测距仪与前测距仪之间的测距仪距离确定所述移动平台的位置，或者，基于所述移动平台上一时刻的位置以及所述移动平台的移动速度确定所述移动平台的位置，所述位置为所述移动平台与所述总路径的起点的距离。路径路径路径路径路径路径路径。

在上述实现方式中，融合双测距仪的测距值，同时配合标定的路径参数进行测距定位，提高了测距速率，解决了高精度测距仪速度低和无法进行弯道定位的问题，从而提高了移动平台的定位精度和适用性。

可选地，在所述获取移动平台的第一测距值和第二测距值之前，所述方法还包括：基于所述总路径的设计图或实体测量数据，沿指定方向将所述总路径划分为第一直行路径L₁至第M直行路径L_M，以及第一弯道路径S₁至第N弯道路径S_N，M为直行路径总数量，N为弯道路径总数量；基于所述总路径的设计图或实体测量数据标定路径参数，所述路径参数包括每条直行路径、每条弯道路径的长度和弯道路径半径。

在上述实现方式中，将总路径分段划分为直行路径和弯道路径，对每条直行路径和弯道路径进行路径序号以及长度等参数标定，从而能够在后续确定移动平台所处路径时直接基于路径序号使用相关参数，提高了定位效率。

可选地，所述总路径还包括弯道路径，所述路径参数包括各直行路径与各弯道路径的连接关系，所述基于所述第一测距值、所述第二测距值和总路径的路径参数，确定所述移动平台所处的当前路径，包括：基于所述第一测距值、所述第二测距值随所述移动平台运动时的变化值确定所述移动平台的运动方向；当所述移动平台从已知直行路径驶出时，基于所述运动方向，将当前路径更新为与所述已知直行路径相接的前一或后一弯道路径；当所述移动平台从已知弯道路径驶出时，基于所述运动方向，将所述当前路径更新为与所述已知弯道路径相接的前一或后一直行路径。

在上述实现方式中，通过第一测距值、第二测距值确定运动方向，并基于运动方向确定移动平台驶入或驶出弯道路径或直行路径，从而持续对所处路径的当前路径序号进行更新，迅速获取当前路径序号的路径参数，从而提高了定位效率。

可选地，判断所述移动平台驶出直行路径或驶出弯道路径的方法包括：在所述前测距仪与后测距仪中的一者无输出值时，确定所述移动平台驶出直行路径；在恢复至所述前测距仪与后测距仪均有输出值的状态时，确定所述移动平台驶出弯道路径。

在上述实现方式中，通过前测距仪和后测距仪在弯道路径时的输出特性判定移动平台驶出直行路径或弯道路径，提高了移动设备行驶路径确定的效率。

可选地，所述当前路径为直行路径时，所述方法还包括判断所述前测距仪和所述后测距仪的测量延迟是否满足测距条件；在所述前测距仪和所述后测距仪的测量延迟满足所述测距条件时，基于所述第一测距值、所述第二测距值、所述路径参数和所述测距仪距离确定述移动平台的位置，在所述前测距仪和所述后测距仪的测量延迟不满足所述测距条件时，基于所述移动平台上一时刻的位置以及所述移动平台的移动速度确定所述移动平台当前时刻的位置。

在上述实现方式中，基于前测距仪和后测距仪的测量延迟是否满足测距条件选用不同的测距方式进行移动平台的位置确定，避免由于测距仪误差导致的定位误差，从而提高了移动平台的定位准确性和适用性。

可选地，所述判断所述前测距仪和所述后测距仪的测量延迟是否满足测距条件，包括：确定所述第一测距值、所述第二测距值、所述路径参数和所述测距仪距离是否使测距仪测量延迟判断公式成立；在所述测距仪测量延迟判断公式成立时，确定所述前测距仪和所述后测距仪的测量延迟满足测距条件；在所述测距仪测量延迟判断公式不成立时，确定所述前测距仪和所述后测距仪的测量延迟不满足测距条件；所述测距仪测量延迟判断公式为：abs(l_A+l_B+H-(L_i+r))＜σ，其中，abs表示取绝对值，l_A为所述第一测距值，l_B为所述第二测距值，L_i为当前路径长度，σ为预设阈值。

在上述实现方式中，通过测距仪测量延迟判断公式进行测距仪是否满足定位需求的判定，其综合了前测距仪和后测距仪的数据进行融合判定，提高了判定准确度。

可选地，所述基于所述第一测距值、所述第二测距值、所述路径参数和所述后测距仪与前测距仪之间的测距仪距离确定所述移动平台的位置包括采用第一直行路径定位公式确定述移动平台的位置；所述第一直行路径定位公式为：

其中，P₁为所述移动平台与所述总路径的起点的距离，l_A为所述第一测距值，l_B为所述第二测距值，r为弯道路径半径，H为所述测距仪距离，L_i为当前路径长度，

为前序直行路径的总长度，

为前序弯道路径的总长度。

在上述实现方式中，在前测距仪和后测距仪的速率满足测距条件时采用第一直行路径定位公式对移动平台进行直行路径的定位，保证了测距仪满足定位需求，从而提高了定位准确率。

可选地，所述基于所述移动平台上一时刻的位置以及所述移动平台的移动速度确定所述移动平台当前时刻的位置包括采用第二直行路径定位公式确定所述移动平台在当前时刻的位置；所述第二直行路径定位公式为：

其中，P_i+1为移动平台当前时刻的位置，P_i为移动平台上一时刻的位置，

为移动平台的移动速度。

在上述实现方式中，在测距仪当前的测距误差不满足当前的定位需求时，根据历史定位数据计算移动平台的移动速度，基于移动速度和时间进行移动平台的位置计算以完成定位，避免采用误差较大的测距仪数据，从而提高了定位的适用性和准确性。

可选地，所述方法还包括：在所述当前路径为弯道路径，且所述第一测距值有效时，基于所述第一测距值和所述路径参数，采用第一弯道路径定位公式确定所述移动平台的位置；所述第一弯道路径定位公式为：

其中，P_i为所述移动平台在弯道路径的位置，l_A为所述第一测距值，

为前序直行路径的总长度，

为前序弯道路径的总长度；在所述当前路径为弯道路径时，且所述第二测距值有效时，基于所述第二测距值和所述路径参数，采用第二弯道路径定位公式确定所述移动平台的位置；所述第二弯道路径定位公式为：

其中，

为前序直行路径的总长度，

为当前路径与前序弯道路径的总长度，l_B为所述第二测距值，L_i为上一直行路径长度。

在上述实现方式中，基于测距仪和标定的路径参数对处于弯道路径中的移动平台进行定位，实现了直行路径和弯道路径的实时定位，提高了定位方法的适用性。

本申请实施例还提供了一种移动平台定位装置，所述装置包括：测距值获取模块，用于获取移动平台的第一测距值和第二测距值，所述第一测距值为设置在所述移动平台的后端的后测距仪的测量值，所述第二测距值为设置在所述移动平台的前端的前测距仪的测量值；路径确定模块，用于基于所述第一测距值、所述第二测距值和总路径的路径参数，确定所述移动平台所处的当前路径；定位模块，用于在所述当前路径为直行路径时，基于所述第一测距值、所述第二测距值、所述路径参数和所述后测距仪与前测距仪之间的测距仪距离确定所述移动平台的位置，或者，基于所述移动平台上一时刻的位置以及所述移动平台的移动速度确定所述移动平台的位置，所述位置为所述移动平台与所述总路径的起点的距离。

在上述实现方式中，融合双测距仪的测距值，同时配合标定的路径参数进行测距定位，提高了测距速率，解决了高精度测距仪速度低和无法进行弯道定位的问题，从而提高了移动平台的定位精度和适用性。

可选地，所述移动平台定位装置还包括：标定模块，用于基于所述总路径的设计图或实体测量数据，沿指定方向将所述总路径划分为第一直行路径L₁至第M直行路径L_M，以及第一弯道路径S₁至第N弯道路径S_N，M直行路径总数量，N为弯道总数量；基于所述总路径的设计图或实体测量数据标定路径参数，所述路径参数包括每条直行路径、每条弯道路径的长度和弯道路径半径。

在上述实现方式中，将总路径分段划分为直行路径和弯道，对每条直行路径和弯道进行路径序号以及长度等参数标定，从而能够在后续确定移动平台所处路径时直接基于路径序号使用相关参数，提高了定位效率。

可选地，所述总路径还包括弯道路径，所述路径参数包括各直行路径与各弯道路径的连接关系，所述路径确定模块具体用于：基于所述第一测距值、所述第二测距值随所述移动平台运动时的变化值确定所述移动平台的运动方向；当所述移动平台从已知直行路径驶出时，基于所述运动方向，将当前路径更新为与所述已知直行路径相接的前一或后一弯道路径；当所述移动平台从已知弯道路径驶出时，基于所述运动方向，将所述当前路径更新为与所述已知弯道路径相接的前一或后一直行路径。

在上述实现方式中，通过第一测距值、第二测距值确定运动方向，并基于运动方向确定移动平台驶入或驶出弯道路径或直行路径，从而持续对所处路径的当前路径序号进行更新，迅速获取当前路径序号的路径参数，从而提高了定位效率。

可选地，所述路径确定模块具体用于：在所述前测距仪与所述后测距仪中的一者无输出值时，确定所述移动平台驶出直行路径；在恢复至所述前测距仪与所述后测距仪均有输出值的状态时，确定所述移动平台驶出弯道路径。

在上述实现方式中，通过前测距仪和后测距仪的测距值确定移动平台驶出弯道路径或直行路径，提高了路径判定的准确率。

可选地，所述定位模块具体用于：判断所述前测距仪和所述后测距仪的测量延迟是否满足测距条件；在所述前测距仪和所述后测距仪的测量延迟满足所述测距条件时，基于所述第一测距值、所述第二测距值、所述路径参数和所述测距仪距离确定述移动平台的位置，在所述前测距仪和所述后测距仪的测量延迟不满足所述测距条件时，基于所述移动平台上一时刻的位置以及所述移动平台的移动速度确定所述移动平台当前时刻的位置。

在上述实现方式中，基于前测距仪和后测距仪的测量延迟是否满足测距条件选用不同的测距方式进行移动平台的位置确定，避免由于测距仪误差导致的定位误差，从而提高了移动平台的定位准确性和适用性。

可选地，所述定位模块具体用于：确定所述第一测距值、所述第二测距值、所述路径参数和所述测距仪距离是否使测距仪测量延迟判断公式成立；在所述测距仪测量延迟判断公式成立时，确定所述前测距仪和所述后测距仪的测量延迟满足测距条件；在所述测距仪测量延迟判断公式不成立时，确定所述前测距仪和所述后测距仪的测量延迟不满足测距条件；所述测距仪测量延迟判断公式为：abs(l_A+l_B+H-(L_i+r))＜σ，其中，abs表示取绝对值，l_A为所述第一测距值，l_B为所述第二测距值，L_i为当前路径长度，σ为预设阈值。

在上述实现方式中，通过测距仪测量延迟判断公式进行测距仪是否满足定位需求的判定，其综合了前测距仪和后测距仪的数据进行融合判定，提高了判定准确度。

可选地，所述定位模块具体用于：采用第一直行路径定位公式确定述移动平台的位置；所述第一直行路径定位公式为：

其中，P₁为所述移动平台与所述总路径的起点的距离，l_A为所述第一测距值，l_B为所述第二测距值，r为弯道路径半径，H为所述测距仪距离，L_i为当前路径长度，

为前序直行路径的总长度，

为前序弯道路径的总长度。

在上述实现方式中，在前测距仪和后测距仪的测量延迟满足测距条件时采用第一直行路径定位公式对移动平台进行直行路径的定位，保证了测距仪满足定位需求，从而提高了定位准确率。

可选地，所述定位模块具体用于：采用第二直行路径定位公式确定所述移动平台在当前时刻的位置；所述第二直行路径定位公式为：

其中，P_i+1为移动平台当前时刻的位置，P_i为移动平台上一时刻的位置，

为移动平台的移动速度。

在上述实现方式中，在测距仪当前的测距误差不满足当前的定位需求时，根据历史定位数据计算移动平台的移动速度，基于移动速度和时间进行移动平台的位置计算以完成定位，避免采用误差较大的测距仪数据，从而提高了定位的适用性和准确性。

可选地，所述定位模块具体用于：在所述当前路径为弯道路径，且所述第一测距值有效时，基于所述第一测距值和所述路径参数，采用第一弯道路径定位公式确定所述移动平台的位置；所述第一弯道路径定位公式为：

其中，P_i为所述移动平台在弯道路径的位置，l_A为所述第一测距值，

为前序直行路径的总长度，

为前序弯道路径的总长度；在所述当前路径为弯道路径时，且所述第二测距值有效时，基于所述第二测距值和所述路径参数，采用第二弯道路径定位公式确定所述移动平台的位置；所述第二弯道路径定位公式为：

其中，

为前序直行路径的总长度，

为当前路径与前序弯道路径的总长度，l_B为所述第二测距值，L_i为上一直行路径长度。

在上述实现方式中，基于测距仪和标定的路径参数对处于弯道路径中的移动平台进行定位，实现了直行路径和弯道路径的实时定位，提高了定位方法的适用性。

本申请实施例还提供了一种电子设备，所述电子设备包括存储器和处理器，所述存储器中存储有程序指令，所述处理器读取并运行所述程序指令时，执行上述任一实现方式中的步骤。

本申请实施例还提供了一种可读取存储介质，所述可读取存储介质中存储有计算机程序指令，所述计算机程序指令被一处理器读取并运行时，执行上述任一实现方式中的步骤。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对本申请实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本申请实施例提供的一种移动平台定位方法的流程示意图。

图2为本申请实施例提供的一种移动平台的结构示意图。

图3为本申请实施例提供的一种路径标定示意图。

图4为本申请实施例提供的一种定位步骤的流程示意图。

图5为本申请实施例提供的一种移动平台定位装置的模块示意图。

图标：20-移动平台定位装置；21-测距值获取模块；22-路径确定模块；23-定位模块。

具体实施方式

下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行描述。

经本申请人研究发现，在建筑领域或其他领域中移动平台依赖编码尺测量存在累积误差，造成随着路径长度增加，误差变大。比如通常一圈外墙路径长达上百米，则会造成最少30cm的误差。编码器由于累积误差，无法做移动平台运动控制；而激光测距仪因为测量速率低，也不能直接应用到外墙移动平台的运动控制中。激光测距仪一般分为两种，一种是脉冲式激光测距仪，具有测量频率到的特点，但是精度较差，一般都是10cm左右；一种是相位式激光测距仪，具有毫米级精度，但是测量频率低，只有5-30Hz。而外墙移动平台运动控制需要毫米级测量精度，同时还要求最少1kHz的测量频率。两种激光测距仪均不能满足外墙移动平台的运动控制要求。因此现有技术在进行移动平台的定位时，均存在准确性和适用性较差的问题。

为了解决上述问题，本申请实施例提供了一种移动平台定位方法，请参考图1，图1为本申请实施例提供的一种移动平台定位方法的流程示意图，该方法的具体步骤可以如下：

步骤S12：获取移动平台的第一测距值和第二测距值。

首先对移动平台进行说明，请参考图2，图2为本申请实施例提供的一种移动平台的结构示意图。移动平台分为通过缆绳、滑竿等活动连接结构连接的后车厢和前车厢A，前车厢A的前端设置有前测距仪A，后车厢B的后端设置有后测距仪B，后测距仪获取的数据为第一测距值l_A，前测距仪获取的数据为第二测距值l_B。

应当理解的是，本实施例中的对移动平台进行定位的具体算法需要用到预先标定的路径参数，因此，本实施例在实际进行计算步骤之前应当对移动平台运行的路径的参数进行标定，该标定的具体步骤可以如下：

步骤S111：基于总路径的设计图或实体测量数据，沿指定方向将总路径划分为第一直行路径L₁至第M直行路径L_M，以及第一弯道路径S₁至第N弯道路径S_N，M为直行路径总数量，N为弯道总数量。

进一步地，还可以对移动平台中的前测距仪和后测距仪的距离标定为H。

可选地，由于移动平台通常在轨道中行驶，本实施例中的直行路径和弯道路径可以为轨道，在其他实施例中，路径还可以是移动平台可以行驶的任意形式的行驶地形。

可选地，本实施例中的前测距仪和后测距仪可以是激光测距仪、超声波测距仪等任意能够满足测距需求的测距仪。

步骤S112：基于总路径的设计图或实体测量数据标定路径参数，路径参数包括每条直行路径、每条弯道路径的长度和弯道路径半径。

请参考图3，图3为本申请实施例提供的一种路径标定示意图，其中，每个直行路径的长度均已标注即第i直行路径的长度为L_i，每个弯道路径的长度均已标注即第i弯道路径的长度为S_i，其中总路径的起点用三角形表示。

进一步地，本实施例中在每段直行路径的起点和终点设置有测距标靶，该测距标靶设置在同一弯道相连的两条直行路径的延长线交点处，该测距标靶与上述两条直行路径的距离在本实施例中可以算作弯道路径半径r。应当理解的是，图3中的相邻的两条直行路径为相互垂直，每条弯道路径的半径相同，在其他实施例中路径可以进行灵活调整。

步骤S14：基于第一测距值、第二测距值和总路径的路径参数，确定移动平台所处的当前路径。

具体地，步骤S14可以包括如下子步骤：

步骤S141：基于第一测距值、第二测距值随移动平台运动时的变化值确定移动平台的运动方向。

移动平台处于某一直行路径或弯道路径时，由于第一测距仪和第二测距仪分别设置在移动平台的后端和前端，其后端对应逆时针方向，前端对应顺时针方向，则在第一测距值变小和/或第二测距值变大时表示移动平台沿路径进行逆时针方向运动，在第一测距值变大和/或第二测距值变小时表示移动平台沿路径进行顺时针方向运动。

应当理解的是，除了上述运动方向确定方式，在其他实施例中还可以是通过加速度计等对移动平台的运动方向进行判定。

可选地，本实施例可以是但不限于是将顺指针运动方向记为Direction＝1，逆时针运动方向记为Direction＝-1。

步骤S142：当移动平台从已知直行路径驶出时，基于运动方向，将当前路径更新为与已知直行路径相接的前一或后一弯道路径；当移动平台从已知弯道路径驶出时，基于运动方向，将当前路径更新为与已知弯道路径相接的前一或后一直行路径。

具体地，当移动平台从L_i驶入弯道路径时，基于运动方向，将当前路径更新为S_i-1或S_i+1，移动平台从S_i驶出弯道路径时，基于运动方向，将当前路径更新为L_i-1或L_i+1。

可选地，本实施例中判断移动平台驶出直行路径或驶出弯道路径的方式可以包括：在前测距仪与后测距仪中的一者无输出值时，确定移动平台驶出直行路径；在恢复至前测距仪与后测距仪均有输出值的状态时，确定移动平台驶出弯道路径。这种输出值有无的状态变化是因为，移动平台的总长度大于弯道长度且移动平台包括了可相对转动的前车厢与后车厢，前测距仪与后测距分别安装在前车厢与后车厢，因此两者中总有一者能够打到激光靶标上而具有输出值。

可选地，本本实施例可以通过设置在移动平台上的加速度传感器或陀螺仪或惯性测量单元(InertialMeasurement Unit，IMU)基于移动平台转弯时会产生向心力的原理确定移动设备驶入或驶出弯道路径，则可以根据标定的路径参数确定移动平台的当前路径序号。

例如路径序号包括直行路径序号和弯道序号，移动平台沿路径顺时针运动，移动平台从总路径起点出发后记录当前直行路径序号为Straight＝1(对应路径为L₁，该当前直行路径序号在弯道序号变化后作为前序直行路径序号，在移动平台上的加速度传感器或陀螺仪基于移动平台转弯时会产生向心力的原理确定移动设备驶入弯道路径后将当前弯道路径序号更新为Curve＝1，该当前弯道路径序号在直行路径序号变化后作为上一弯道路径序号，同理确定移动平台驶出弯道路径时更新移动平台的上一直行路径序号加一以更新当前直行路径序号为Straight＝2，移动平台再次驶入弯道路径后将上一弯道路径序号加一以更新当前弯道路径序号为Curve＝2，以此类推。

可选地，本实施例中的路径序号数值与路径标定的分段下标数值相同，则可以根据路径序号确定当前路径，例如，Straight＝2时表示移动平台所处的当前路径为第二直行路径L₂，Curve＝2时表示移动平台所处的当前路径为第二弯道路径S₂。同时，确定当前路径时还根据标定的路径参数确定当前路径的长度。

步骤S16：在当前路径为直行路径时，基于第一测距值、第二测距值、路径参数和后测距仪与前测距仪之间的测距仪距离确定移动平台的位置，或者，基于移动平台上一时刻的位置以及移动平台的移动速度确定移动平台的位置。

上述移动平台的位置为移动平台与总路径的起点的距离。

在激光测距仪或超声波测距仪等测距仪的测距频率较低，可能会造成定位延时，因此在测距仪的测距频率满足定位误差需求和不满足定位误差需求时采用不同的定位方式进行移动平台定位。具体地，在前测距仪和后测距仪的测量延迟满足测距条件时，才基于第一测距值、第二测距值、路径参数和测距仪距离确定述移动平台的位置；在前测距仪和后测距仪的测量延迟不满足测距条件时，则基于移动平台上一时刻的位置以及移动平台的移动速度确定移动平台当前时刻的位置。

请参考图4，图4为本申请实施例提供的一种定位步骤的流程示意图，步骤S16的具体步骤可以包括：

步骤S161：判断前测距仪和后测距仪的测量延迟是否满足测距条件。

具体地，步骤S161可以包括如下子步骤：

步骤S1611：确定第一测距值、第二测距值、路径参数和测距仪距离是否使测距仪测量延迟判断公式成立。

上述测距仪测量延迟判断公式可以为abs(l_A+l_B+H-(L_i+r))＜σ，其中，abs表示取绝对值，σ为预设阈值。abs(l_A+l_B+H-(L_i+r))用于表征测距仪的测量延迟高低，当其大于预设阈值时说明延迟大，测量信息不可信。

步骤S1612：在测距仪测量延迟判断公式成立时，确定前测距仪和后测距仪的测量延迟满足测距条件。

步骤S1613：在测距仪测量延迟判断公式不成立时，确定前测距仪和后测距仪的测量延迟不满足测距条件。

步骤S162：在前测距仪和后测距仪的测量延迟满足测距条件时，基于第一测距值、第二测距值、路径参数和测距仪距离，采用第一直行路径定位公式确定述移动平台的位置。

移动平台位于直行路径时，后测距仪的测量值l_A与前测距仪的测量值l_B的和是定值，即l_A+l_B+H＝L_i+r，则在t时刻，移动平台的位置P₁由第一直行路径定位公式得出，该第一直行路径定位公式可以为

其中，P₁为移动平台与总路径的起点的距离，l_A为第一测距值，l_B为第二测距值，r为弯道路径半径，H为测距仪距离，L_i为当前路径长度，

为前序直行路径的总长度，

为前序弯道路径的总长度。上述前序直行路径为移动平台已行驶过的所有直行路径，比如当前直行路径为L₄，则前序直行路径的总长度为L₁+L₂+L₃。

上述前序弯道路径为移动平台已行驶过的所有弯道路径。

可选地，在前测距仪和后测距仪的测量延迟不满足测距条件时，可以采用第二直行路径定位公式，基于移动平台的位置差值确定某一特定时刻移动平台的位置，其具体步骤包括：

步骤S163：在前测距仪和后测距仪的测量延迟不满足测距条件时，基于t_i时刻移动平台的位置P_i、t_i-1时刻移动平台的位置P_i-1，采用第二直行路径定位公式确定移动平台在t_i+1时刻的位置。

具体地，记t_i时刻为第一时刻，移动平台的第一位置为P_i，上一时刻为第二时刻即t_i-1时刻，移动平台的第二位置为P_i-1，则移动平台速度V为

则可以根据t_i和t_i-1时刻的移动平台位置，根据速度与时间的乘积为路程的原理确定第三时刻即t_i+1时刻，移动平台的第三位置即当前时刻的位置P_i+1为：

另一方面，移动平台处于弯道中时，因为移动平台整车长度大于弯道弧长，前测距仪和后测距仪中总有一个能打到测距标靶上，获得有效测距值，因此本实施例还可以通过如下步骤对弯道中的移动平台进行定位。

步骤S181：在当前路径为弯道，且第一测距值有效时，基于第一测距值和路径参数，采用第一弯道定位公式确定移动平台的位置。

移动平台处于第i弯道的前半弯时，前测距仪对已经过的测距标靶采集的测距数据有效，上述第一弯道定位公式为：

其中，P_i为移动平台在弯道的位置。

其中，

为前序直行路径的总长度，

为前序弯道路径的总长度。

步骤S182：在当前路径为弯道时，且第二测距值有效时，基于第二测距值和路径参数，采用第二弯道定位公式确定移动平台的位置。

移动平台处于第i弯道的后半弯时，后测距仪对未经过的邻近测距标靶采集的测距数据有效，上述第二弯道定位公式为：

其中，

为前序直行路径的总长度，

为当前路径与前序弯道路径的总长度，L_i为上一直行路径长度。

可选地，本实施例在判定前测距仪或后测距仪的测距数据是否为有效数值时，可以根据第一测距值和第二测距值的大小判定。

具体地，由于移动平台处于弯道中时，与该弯道处设置的测距标靶的距离最大为弯道半径r，则第一测距值和第二测距值中大于r的认定为无效数值，小于或等于r的可以被认定为有效数值。

为了配合本实施例提供的上述移动平台定位方法，本申请实施例提供了一种移动平台定位装置20。

请参考图5，图5为本申请实施例提供的一种移动平台定位装置的模块示意图。

移动平台定位装置20包括：

测距值获取模块21，用于获取移动平台的第一测距值和第二测距值，第一测距值为设置在移动平台的后端的后测距仪的测量值，第二测距值为设置在移动平台的前端的前测距仪的测量值；

路径确定模块22，用于基于第一测距值、第二测距值和总路径的路径参数，确定移动平台所处的当前路径；

定位模块23，用于在当前路径为直行路径时，基于第一测距值、第二测距值、路径参数和后测距仪与前测距仪之间的测距仪距离确定移动平台的位置，或者，基于移动平台上一时刻的位置以及移动平台的移动速度确定移动平台的位置，位置为移动平台与总路径的起点的距离。

可选地，移动平台定位装置20还包括：标定模块，用于基于总路径的设计图或实体测量数据，沿指定方向将总路径划分为第一直行路径L₁至第M直行路径L_M，以及第一弯道路径S₁至第N弯道路径S_N，M直行路径总数量，N为弯道总数量；基于总路径的设计图或实体测量数据标定路径参数，路径参数包括每条直行路径、每条弯道路径的长度和弯道路径半径。

可选地，路径确定模块22具体用于：基于第一测距值、第二测距值随移动平台运动时的变化值确定移动平台的运动方向；当移动平台从已知直行路径驶出时，基于运动方向，将当前路径更新为与已知直行路径相接的前一或后一弯道路径；当移动平台从已知弯道路径驶出时，基于运动方向，将当前路径更新为与已知弯道路径相接的前一或后一直行路径。

可选地，路径确定模块22具体用于：在前测距仪与后测距仪中的一者无输出值时，确定移动平台驶出直行路径；在恢复至前测距仪与后测距仪均有输出值的状态时，确定移动平台驶出弯道路径。

可选地，定位模块23具体用于：判断前测距仪和后测距仪的测量延迟是否满足测距条件；在前测距仪和后测距仪的测量延迟满足测距条件时，基于第一测距值、第二测距值、路径参数和测距仪距离确定述移动平台的位置，在前测距仪和后测距仪的测量延迟不满足测距条件时，基于移动平台上一时刻的位置以及移动平台的移动速度确定移动平台当前时刻的位置。

可选地，定位模块23具体用于：确定第一测距值、第二测距值、路径参数和测距仪距离是否使测距仪测量延迟判断公式成立；在测距仪测量延迟判断公式成立时，确定前测距仪和后测距仪的测量延迟满足测距条件；在测距仪测量延迟判断公式不成立时，确定前测距仪和后测距仪的测量延迟不满足测距条件；测距仪测量延迟判断公式为：abs(l_A+l_B+H-(L_i+r))＜σ，其中，abs表示取绝对值，l_A为第一测距值，l_B为第二测距值，L_i为当前路径长度，σ为预设阈值。

可选地，定位模块具23具体用于：采用第一直行路径定位公式确定述移动平台的位置；第一直行路径定位公式为：

其中，P₁为移动平台与总路径的起点的距离，l_A为第一测距值，l_B为第二测距值，r为弯道路径半径，H为测距仪距离，L_i为当前路径长度，

为前序直行路径的总长度，

为前序弯道路径的总长度。

可选地，定位模块23具体用于：采用第二直行路径定位公式确定移动平台在当前时刻的位置；第二直行路径定位公式为：

其中，P_i+1为移动平台当前时刻的位置，P_i为移动平台上一时刻的位置，

为移动平台的移动速度。

可选地，定位模块23具体用于：在当前路径为弯道路径，且第一测距值有效时，基于第一测距值和路径参数，采用第一弯道路径定位公式确定移动平台的位置；第一弯道路径定位公式为：

其中，P_i为移动平台在弯道路径的位置，l_A为第一测距值，

为前序直行路径的总长度，

为前序弯道路径的总长度；在当前路径为弯道路径时，且第二测距值有效时，基于第二测距值和路径参数，采用第二弯道路径定位公式确定移动平台的位置；第二弯道路径定位公式为：

其中，

为前序直行路径的总长度，

为当前路径与前序弯道路径的总长度，l_B为第二测距值，L_i为上一直行路径长度。

本申请实施例还提供了一种电子设备，该电子设备包括存储器和处理器，所述存储器中存储有程序指令，所述处理器读取并运行所述程序指令时，执行本实施例提供的移动平台定位方法中任一项所述方法中的步骤。

应当理解是，该电子设备可以是个人电脑(Personal Computer，PC)、平板电脑、智能手机、个人数字助理(Personal Digital Assistant，PDA)等具有逻辑计算功能的电子设备。

本申请实施例还提供了一种可读取存储介质，所述可读取存储介质中存储有计算机程序指令，所述计算机程序指令被一处理器读取并运行时，执行移动平台定位方法中的步骤。

综上所述，本申请实施例提供了一种移动平台定位方法、装置、电子设备及存储介质，所述方法包括：获取移动平台的第一测距值和第二测距值，所述第一测距值为设置在所述移动平台的后端的后测距仪的测量值，所述第二测距值为设置在所述移动平台的前端的前测距仪的测量值；基于所述第一测距值、所述第二测距值和总路径的路径参数，确定所述移动平台所处的当前路径；在所述当前路径为直行路径时，基于所述第一测距值、所述第二测距值、所述路径参数和所述后测距仪与前测距仪之间的测距仪距离确定所述移动平台的位置，或者，基于移动平台上一时刻的位置以及移动平台的移动速度确定移动平台的位置，所述位置为所述移动平台与所述总路径的起点的距离。

在上述实现方式中，融合双测距仪的测距值，同时配合标定的路径参数进行测距定位，提高了测距速率，解决了高精度测距仪速度低和无法进行弯道定位的问题，从而提高了移动平台的定位精度和适用性。

在本申请所提供的几个实施例中，应该理解到，所揭露的设备，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的框图显示了根据本申请的多个实施例的设备的可能实现的体系架构、功能和操作。在这点上，框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图中的每个方框、以及框图的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。因此本实施例还提供了一种可读取存储介质中存储有计算机程序指令，所述计算机程序指令被一处理器读取并运行时，执行区块数据存储方法中任一项所述方法中的步骤。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，RanDom Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅为本申请的实施例而已，并不用于限制本申请的保护范围，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括......”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

Claims (11)

1.一种移动平台定位方法，其特征在于，所述方法包括：

获取移动平台的第一测距值和第二测距值，所述第一测距值为设置在所述移动平台的后端的后测距仪的测量值，所述第二测距值为设置在所述移动平台的前端的前测距仪的测量值；

基于所述第一测距值、所述第二测距值和总路径的路径参数，确定所述移动平台所处的当前路径；

在所述当前路径为直行路径时，基于所述第一测距值、所述第二测距值、所述路径参数和所述后测距仪与前测距仪之间的测距仪距离确定所述移动平台的位置，或者，基于所述移动平台上一时刻的位置以及所述移动平台的移动速度确定所述移动平台的位置，所述位置为所述移动平台与所述总路径的起点的距离；

所述总路径还包括弯道路径，所述路径参数包括各直行路径与各弯道路径的连接关系，所述基于所述第一测距值、所述第二测距值和总路径的路径参数，确定所述移动平台所处的当前路径，包括：

基于所述第一测距值、所述第二测距值随所述移动平台运动时的变化值确定所述移动平台的运动方向；

当所述移动平台从已知直行路径驶出时，基于所述运动方向，将当前路径更新为与所述已知直行路径相接的前一或后一弯道路径；

当所述移动平台从已知弯道路径驶出时，基于所述运动方向，将所述当前路径更新为与所述已知弯道路径相接的前一或后一直行路径。

2.根据权利要求1所述的方法，其特征在于，在所述获取移动平台的第一测距值和第二测距值之前，所述方法还包括：

基于所述总路径的设计图或实体测量数据，沿指定方向将所述总路径划分为第一直行路径L₁至第M直行路径L_M，以及第一弯道路径S₁至第N弯道路径S_N，M为直行路径总数量，N为弯道路径总数量；

基于所述总路径的设计图或实体测量数据标定路径参数，所述路径参数包括每条直行路径、每条弯道路径的长度和弯道路径半径。

3.根据权利要求2所述的方法，其特征在于，判断所述移动平台驶出直行路径或驶出弯道路径的方法包括：

在所述前测距仪与所述后测距仪中的一者无输出值时，确定所述移动平台驶出直行路径；

在恢复至所述前测距仪与所述后测距仪均有输出值的状态时，确定所述移动平台驶出弯道路径。

4.根据权利要求1所述的方法，其特征在于，所述当前路径为直行路径时，所述方法还包括判断所述前测距仪和所述后测距仪的测量延迟是否满足测距条件；

在所述前测距仪和所述后测距仪的测量延迟满足所述测距条件时，基于所述第一测距值、所述第二测距值、所述路径参数和所述测距仪距离确定述移动平台的位置；

在所述前测距仪和所述后测距仪的测量延迟不满足所述测距条件时，基于所述移动平台上一时刻的位置以及所述移动平台的移动速度确定所述移动平台当前时刻的位置。

5.根据权利要求4所述的方法，其特征在于，所述判断所述前测距仪和所述后测距仪的测量延迟是否满足测距条件，包括：

确定所述第一测距值、所述第二测距值、所述路径参数和所述测距仪距离是否使测距仪测量延迟判断公式成立；

在所述测距仪测量延迟判断公式成立时，确定所述前测距仪和所述后测距仪的测量延迟满足测距条件；

在所述测距仪测量延迟判断公式不成立时，确定所述前测距仪和所述后测距仪的测量延迟不满足测距条件；

所述测距仪测量延迟判断公式为：abs(l_A+l_B+H-(L_i+r))＜σ，其中，abs表示取绝对值，l_A为所述第一测距值，l_B为所述第二测距值，L_i为当前路径长度，σ为预设阈值。

6.根据权利要求1所述的方法，其特征在于，所述基于所述第一测距值、所述第二测距值、所述路径参数和所述后测距仪与前测距仪之间的测距仪距离确定所述移动平台的位置包括采用第一直行路径定位公式确定述移动平台的位置；

所述第一直行路径定位公式为：

其中，P₁为所述移动平台与所述总路径的起点的距离，l_A为所述第一测距值，l_B为所述第二测距值，r为弯道路径半径，H为所述测距仪距离，L_i为当前路径长度，

为前序直行路径的总长度，

为前序弯道路径的总长度。

7.根据权利要求1所述的方法，其特征在于，所述基于所述移动平台上一时刻的位置以及所述移动平台的移动速度确定所述移动平台当前时刻的位置包括采用第二直行路径定位公式确定所述移动平台在当前时刻的位置；

所述第二直行路径定位公式为：

其中，P_i+1为移动平台当前时刻的位置，P_i为移动平台上一时刻的位置，

为移动平台的移动速度。

8.根据权利要求1所述的方法，其特征在于，所述方法还包括：

在所述当前路径为弯道路径，且所述第一测距值有效时，基于所述第一测距值和所述路径参数，采用第一弯道路径定位公式确定所述移动平台的位置；

所述第一弯道路径定位公式为：

其中，P_i为所述移动平台在弯道路径的位置，l_A为所述第一测距值，

为前序直行路径的总长度，

为前序弯道路径的总长度；

在所述当前路径为弯道路径时，且所述第二测距值有效时，基于所述第二测距值和所述路径参数，采用第二弯道路径定位公式确定所述移动平台的位置；

所述第二弯道路径定位公式为：

其中，

为前序直行路径的总长度，

为当前路径与前序弯道路径的总长度，l_B为所述第二测距值，L_i为上一直行路径长度。

9.一种移动平台定位装置，其特征在于，所述装置包括：

测距值获取模块，用于获取移动平台的第一测距值和第二测距值，所述第一测距值为设置在所述移动平台的后端的后测距仪的测量值，所述第二测距值为设置在所述移动平台的前端的前测距仪的测量值；

路径确定模块，用于基于所述第一测距值、所述第二测距值和总路径的路径参数，确定所述移动平台所处的当前路径；

定位模块，用于在所述当前路径为直行路径时，基于所述第一测距值、所述第二测距值、所述路径参数和所述后测距仪与前测距仪之间的测距仪距离确定所述移动平台的位置，或者，基于所述移动平台上一时刻的位置以及所述移动平台的移动速度确定所述移动平台当前时刻的位置，所述位置为所述移动平台与所述总路径的起点的距离；

所述总路径还包括弯道路径，所述路径参数包括各直行路径与各弯道路径的连接关系，所述路径确定模块具体用于：基于所述第一测距值、所述第二测距值随所述移动平台运动时的变化值确定所述移动平台的运动方向；当所述移动平台从已知直行路径驶出时，基于所述运动方向，将当前路径更新为与所述已知直行路径相接的前一或后一弯道路径；当所述移动平台从已知弯道路径驶出时，基于所述运动方向，将所述当前路径更新为与所述已知弯道路径相接的前一或后一直行路径。

10.一种电子设备，其特征在于，所述电子设备包括存储器和处理器，所述存储器中存储有程序指令，所述处理器运行所述程序指令时，执行权利要求1-8中任一项所述方法中的步骤。

11.一种存储介质，其特征在于，所述存储介质中存储有计算机程序指令，所述计算机程序指令被一处理器运行时，执行权利要求1-8中任一项所述方法中的步骤。

Images