阅读说明：本技术 一种雷达定位方法、装置及存储介质 (Radar positioning method and device and storage medium ) 是由 王玥 贾全 郭承志 于 2020-07-28 设计创作，主要内容包括：本申请提供的一种雷达定位方法、装置及存储介质,涉及雷达定位技术领域。包括：获取全局地标坐标系中的反光柱信息、以及当前雷达坐标系下局部坐标系中的反光柱信息；通过反光柱匹配算法获取所述全局地标坐标系中的反光柱信息与所述雷达坐标系下局部坐标系中的反光柱信息之间的匹配关系；通过所述匹配关系以及加权最小二乘算法,计算获取雷达的位姿信息。利用多个匹配上的反光柱信息以及加权最小二乘算法进行雷达的位姿计算,解决了目前单个反光柱信息获取不准确时,对雷达定位精度影响较大的问题,提高了雷达位姿计算的准确性。(The application provides a radar positioning method, a radar positioning device and a storage medium, and relates to the technical field of radar positioning. The method comprises the following steps: acquiring reflective column information in a global landmark coordinate system and reflective column information in a local coordinate system under a current radar coordinate system; acquiring a matching relation between the reflective column information in the global landmark coordinate system and the reflective column information in a local coordinate system under the radar coordinate system through a reflective column matching algorithm; and calculating and acquiring the pose information of the radar through the matching relation and a weighted least square algorithm. The position and pose calculation of the radar is carried out by utilizing the information of the reflecting columns on the plurality of matches and the weighted least square algorithm, the problem that the positioning precision of the radar is greatly influenced when the information of a single reflecting column is not accurately obtained at present is solved, and the accuracy of the position and pose calculation of the radar is improved.)

一种雷达定位方法、装置及存储介质

技术领域

本申请涉及雷达定位技术领域，具体而言，涉及一种雷达定位方法、装置及存储介质。

背景技术

在现代制造业中，生产阶段自动化的重要性与日俱增，其中，自动导引小车(Automated Guided Vehicle，简称AGV)的导航定位是一个重要的研究方向。常见的工业AGV导航定位方式有磁导航、自然导航(视觉SLAM或激光SLAM)、地标导航(二维码或RFID标签等)、激光导航(反光柱)等。其中基于反光柱(或称有反)激光导航由于其定位精度高，维护成本低，安装成本相对较低，AGV的路径规划柔性高等优点在叉车AGV上获得了大量应用。

但是，现有的基于反光柱(或称有反)激光导航技术，其涉及的多个反光柱，如果其中存在有反光柱信息不准确的情况，会严重影响雷达的定位精度。

发明内容

为了解决现有技术中存在的问题，本申请提供了一种雷达定位方法、装置及存储介质。

为实现上述目的，本申请采用的技术方案为：

本申请第一方面提供一种雷达定位方法，包括：

获取全局地标坐标系中的反光柱信息、以及当前雷达坐标系下局部坐标系中的反光柱信息；

通过反光柱匹配算法获取所述全局地标坐标系中的反光柱信息与所述雷达坐标系下局部坐标系中的反光柱信息之间的匹配关系；

通过所述匹配关系以及加权最小二乘算法，计算获取雷达的位姿信息。

可选地，所述通过所述匹配关系以及加权最小二乘算法，计算获取雷达的位姿信息，包括：

根据所述匹配关系，获取匹配的每对反光柱的最小二乘解算权重；

将所述最小二乘解算权重进行归一化处理，得到归一化权重；

利用所述匹配关系以及所述归一化权重，计算获取所述雷达的位姿信息。

可选地，所述根据所述匹配关系，获取匹配的每对反光柱的最小二乘解算权重，包括：

通过所述匹配关系以及所述雷达坐标系下局部坐标系中的反光柱信息，计算每对反光柱与所述雷达之间的距离；

根据每对所述反光柱与所述雷达之间的距离，获取每对所述反光柱的最小二乘解算权重。

可选地，所述将所述最小二乘解算权重进行归一化处理，得到归一化权重，包括：

获取所有所述最小二乘解算权重之和；

将每对所述反光柱对应的所述最小二乘解算权重除以所述最小二乘解算权重之和，获取每对所述反光柱的归一化权重。

可选地，所述通过所述匹配关系以及加权最小二乘算法，计算获取雷达的位姿信息之后，还包括：

通过所述最小二乘残差法以及所述位姿信息，将所述雷达坐标系下局部坐标系中的反光柱信息转换至所述全局地标坐标系中，获取转换后的反光柱信息；

获取所述转换后的反光柱信息与所述全局地标坐标系中的反光柱信息之间的距离误差；

根据所述距离误差以及预设评估算法，获取定位质量评估信息。

本申请第二方面提供一种雷达定位装置，包括：获取单元、匹配单元以及计算单元；

所述获取单元，用于获取全局地标坐标系中的反光柱信息、以及当前雷达坐标系下局部坐标系中的反光柱信息；

所述匹配单元，用于通过反光柱匹配算法获取所述全局地标坐标系中的反光柱信息与所述雷达坐标系下局部坐标系中的反光柱信息之间的匹配关系；

所述计算单元，用于通过所述匹配关系以及加权最小二乘算法，计算获取雷达的位姿信息。

可选地，所述计算单元，用于根据所述匹配关系，获取匹配的每对反光柱的最小二乘解算权重；

将所述最小二乘解算权重进行归一化处理，得到归一化权重；

利用所述匹配关系以及所述归一化权重，计算获取所述雷达的位姿信息。

可选地，所述计算单元，用于通过所述匹配关系以及所述雷达坐标系下局部坐标系中的反光柱信息，计算每对反光柱与所述雷达之间的距离；

根据每对所述反光柱与所述雷达之间的距离，获取每对所述反光柱的最小二乘解算权重。

可选地，所述计算单元，用于获取所有所述最小二乘解算权重之和；

将每对所述反光柱对应的所述最小二乘解算权重除以所述最小二乘解算权重之和，获取每对所述反光柱的归一化权重。

可选地，所述装置还包括：转换单元；

所述转换单元，用于通过所述最小二乘残差法以及所述位姿信息，将所述雷达坐标系下局部坐标系中的反光柱信息转换至所述全局地标坐标系中，获取转换后的反光柱信息；

所述获取单元，用于获取所述转换后的反光柱信息与所述全局地标坐标系中的反光柱信息之间的距离误差；

所述计算单元，用于根据所述距离误差以及预设评估算法，获取定位质量评估信息。

本申请第三方面提供了一种雷达定位装置，包括：处理器、存储介质和总线，所述存储介质存储有所述处理器可执行的机器可读指令，当所述装置运行时，所述处理器与所述存储介质之间通过总线通信，所述处理器执行所述机器可读指令，以执行上述第一方面所述方法的步骤。

本申请第四方面提供了一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被处理器运行时执行如第一方面提供的方法。

本申请提供的一种雷达定位方法、装置及存储介质中，获取全局地标坐标系中的反光柱信息、以及当前雷达坐标系下局部坐标系中的反光柱信息；通过反光柱匹配算法获取所述全局地标坐标系中的反光柱信息与所述雷达坐标系下局部坐标系中的反光柱信息之间的匹配关系；通过所述匹配关系以及加权最小二乘算法，计算获取雷达的位姿信息。利用多个匹配上的反光柱信息以及加权最小二乘算法进行雷达的位姿计算，解决了目前单个反光柱信息获取不准确时，对雷达定位精度影响较大的问题，提高了雷达位姿计算的准确性。

附图说明

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

图1为本申请一实施例提供的雷达定位方法的流程示意图；

图2为本申请另一实施例提供的雷达定位方法的流程示意图；

图3为本申请另一实施例提供的雷达定位方法的流程示意图；

图4为本申请一实施例提供的雷达定位装置的结构示意图；

图5为本申请另一实施例提供的雷达定位装置的结构示意图；

图6为本申请一实施例提供的一种雷达定位装置的结构示意图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。

因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。

此外，若出现术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

需要说明的是，在不冲突的情况下，本申请的实施例中的特征可以相互结合。

需要介绍的是，有反激光雷达定位的过程可按顺序划分为几个阶段：反光柱检测、反光柱匹配/反光柱跟踪、雷达位姿解算等。在反光柱检测阶段，定位算法从当前激光雷达原始扫描数据中检测可能存在的反光柱并求解其中心坐标。获得了当前检测到的反光柱在雷达坐标系下的坐标后，需要在已知的反光柱全局坐标地图中找到对应的匹配，匹配的结果将用于雷达位姿解算。在这种基于反光柱雷达位姿解算技术中，其涉及的多个反光柱，如果其中存在有反光柱信息不准确的情况，会严重影响雷达的定位精度。

基于此，本申请实施例提出一种雷达定位方法，用于解决上述技术问题。图1为本申请实施例提供的雷达定位方法的流程示意图，如图1所示，该方法包括：

S101、获取全局地标坐标系中的反光柱信息、以及当前雷达坐标系下局部坐标系中的反光柱信息。

需要说明的是，在本申请实施例中，全局地标坐标系的建立可以以正北、正西等任一方位中的任一点为原点建立坐标系，本申请实施例对此不做限制。雷达坐标系是以雷达为原点建立的坐标系。

其中，反光柱信息可以是反光柱的位置信息，例如为反光柱的坐标信息。

在具体实施中，在基于反光柱的激光导航技术对雷达进行定位时，先需要检测出反光柱的位置，具体地，先通过在激光雷达上安装的激光扫描设备发射激光束，并获取水平面360°角度范围的各个扫描点的扫描数据，这里，扫描数据包括各个扫描点到发射激光束的激光扫描设备之间的距离，各个扫描点的反光强度、各个扫描点到激光扫描设备的光束和激光扫描设备发射的水平光束之间的角度，即，每个扫描点对应扫描数据包括距离、反光强度和角度。其中，激光扫描设备可以为激光雷达，也可以为激光扫描仪，也可以为其他具有激光扫描效果的设备。

具体地，在本申请实施例中，通过激光扫描设备获取反光柱在雷达坐标系下反光柱的坐标信息并定义其标号信息。

示例性地，假设全局地标地图已精确测量得到，地图中存有q个反光柱信息，则在全局地标坐标系下，将获取的反光柱信息可以记为：

通过激光扫描设备获取的p个反光柱在雷达坐标系下反光柱的坐标信息，可以记为：

其中，x和y分别表示反光柱的横、纵坐标信息，i表示反光柱的标号信息，p和q分别表示在全局地标坐标系，以及雷达坐标系下反光柱的个数信息，G和L分别对应全局地标坐标系下以及雷达坐标系下的坐标信息。

S102、通过反光柱匹配算法获取全局地标坐标系中的反光柱信息与雷达坐标系下局部坐标系中的反光柱信息之间的匹配关系。

在本申请实施例中，全局地标坐标系中包括了各个已知的反光柱的全局坐标信息以及标号信息，进而能够在全局地标坐标系中通过与获取的雷达坐标系下反光柱的坐标信息进行匹配，获取全局地标坐标系中的反光柱信息与雷达坐标系下局部坐标系中的反光柱信息之间的匹配关系。

为了避免由于反光柱信息(坐标信息以及标号信息)误检测，导致未对检测到的所有真实反光柱找到正确匹配，最后导致有反激光雷达定位精度下降。本申请实施例中，采用的匹配过程如下：为反光柱检测集合中的每个反光柱预设误检权重值，将反光柱检测集合中的所有反光柱按照各自对应的误检权重值的大小进行排序，并从排序后的反光柱检测集合中选取误检权重值最低的预设数量的反光柱得到待匹配集合，在预设全局地图中查找待匹配集合中的每个反光柱对应的目标匹配反光柱以生成目标匹配集合，当待匹配集合中存在任一反光柱无法获取到对应的目标匹配反光柱时，则通过更新待匹配集合中各个反光柱各自对应的误检权重值以使待匹配集合中每个反光柱均在预设全局地图中获取对应的目标匹配反光柱并更新目标匹配集合。

示例性地，在本申请实施例中，通过上述反光柱匹配过程找到当前扫描获得的反光柱与全局地标坐标系下反光柱之间的匹配关系，设通过上述匹配方法，匹配上的反光柱对标号记为集合S，S中元素数为m(其中：m≤p)，每个元素为一对标号(i,j)，i∈[1,q],j∈[1,p]。

其中，i表示全局地标坐标系下反光柱的标号，相应地，j表示雷达坐标系下局部坐标系中的反光柱标号。

可以理解的是，在本申请实施例中，通过记录匹配上的反光柱的标号信息而不是坐标信息，不仅可以在一定程度上减少数据的存储量，同时还可以加速反光柱搜索匹配的过程。

S103、通过匹配关系以及加权最小二乘算法，计算获取雷达的位姿信息。

在本申请实施例中，利用不同坐标系中匹配成对的反光柱以及最小二程算法，计算获取雷达的位姿信息，可以提升计算结果的准确度。

本申请实施例提供的雷达定位方法，获取全局地标坐标系中的反光柱信息、以及当前雷达坐标系下局部坐标系中的反光柱信息；通过反光柱匹配算法获取所述全局地标坐标系中的反光柱信息与所述雷达坐标系下局部坐标系中的反光柱信息之间的匹配关系；通过所述匹配关系以及加权最小二乘算法，计算获取雷达的位姿信息。利用全局地标坐标系以及雷达坐标系下匹配得到的反光柱信息以及加权最小二乘算法进行雷达的位姿计算，综合考虑多个反光柱的匹配结果，解决了目前单个反光柱信息获取不准确时，对雷达定位精度影响较大的问题，提高了雷达位姿计算的准确性。

在上述实施例中，可以为所匹配上的集合S中的每个反光柱分配最小二乘法求解时的权重，进而再计算位姿信息。

需要说明的是，在本申请实施例中对于权重设定可以采用多种方式，具体以体现不同的反光柱在雷达定位中的作用大小不同即可。

图2为本申请另一实施例提供的雷达定位方法的流程示意图，如图2所示，在本申请的一些实现方式中，步骤S103具体可以包括：

S1031、根据匹配关系，获取匹配的每对反光柱的最小二乘解算权重。

在本申请实施例中，权重的设定与距离相关，而由于距离雷达越远的反光柱在检测时出现误差的可能性越大。因此在本申请实施例中，权重分配原则采用：距离雷达越远的反光柱分配的权重越小，相应地，距离雷达越近的反光柱分配的权重越大。对于权重具体设定规则，本申请实施例不做限制。

S1032、将最小二乘解算权重进行归一化处理，得到归一化权重。

为了归纳统一权重数据的统计分布性，在本申请实施例中，将每对反光柱对应的最小二乘解算权重进行归一化处理。归一化处理，即是将原始数据压缩于(0,1)范围内。

因此采用归一化处理公式，将匹配上的每对反光柱的权重信息，进行归一化处理，得到(0,1)之间的归一化权重信息。

可以理解的是，将权重数据进行归一化处理不仅可以便于数据统一管理，还在一定程度上节约了数据存储量。在本申请实施例中，归一化处理主要是用于获取正确的、高精度的雷达位姿解算结果。

S1033、利用匹配关系以及归一化权重，计算获取雷达的位姿信息。

在本申请实施例中，设得到的归一化权重W为：

设雷达当前位姿为：(X,Y,ψ)，其中X,Y分别表示当前获取的雷达信息中，雷达在二维平面的横坐标以及纵坐标，ψ表示雷达在二维平面的航向角信息。

对于当前检测到的第j号反光柱，且该反光柱与全局地标坐标系中的反光柱能够匹配上，其在雷达坐标系下的齐次坐标为：

其匹配的反光柱全局地图中第i号反光柱在全局地标坐标系下的齐次坐标为：

将全局地标坐标系转换为雷达坐标后的转换矩阵为：

则有如下关系：

或可写为：

记待求解参数向量为：

将S中的全部匹配上的反光柱对坐标转换关系向量化列出，则有：

将该向量化关系对应记为：

AΘ＝B

同时在AΘ＝B两边加入权重向量，则可以表示为：

WAΘ＝WB

利用加权最小二乘获得参数向量的估计：

可得：

通过参数向量求解雷达的位姿信息如下：

其中，分别表示最终计算获取的雷达的横坐标、纵坐标，以及航向角信息。

可选地，在本申请实施例中，根据所述匹配关系，获取匹配的每对反光柱的最小二乘解算权重，即步骤S1031，具体包括：通过匹配关系以及雷达坐标系下局部坐标系中的反光柱信息，计算每对反光柱与雷达之间的距离；根据每对反光柱与雷达之间的距离，获取每对反光柱的最小二乘解算权重。

需要说明的是，权重的设定只要是与距离相关的单调递减函数即可，在具体实施方式中，不同的权重设定方式体现了对不同反光柱检测结果的信任程度。在本申请实施例中，采用反光柱与雷达距离平方的倒数分配权重。

示例性地，对于第k对匹配上的反光柱对S_k＝(i,j)，以及S_k中反光柱在雷达坐标系下局部坐标系中的坐标该反光柱距雷达距离因此，为该反光柱分配权重

可选地，将最小二乘解算权重进行归一化处理，得到归一化权重，包括：

获取所有最小二乘解算权重之和；将每对反光柱对应的最小二乘解算权重除以最小二乘解算权重之和，获取每对反光柱的归一化权重。

在本申请实施方式中，对集合S中的每对反光柱初步分配的权重(w₁,w₂,…,w_k,…,w_m)进行归一化处理，得到归一化权重，并将最终得到的所有归一化权重存入权重行向量W。

图3为本申请另一实施例提供的雷达定位方法的流程示意图，如图3所示，在一些实现方式中，步骤S103之后，还包括：

S104、通过最小二乘残差法以及位姿信息，将雷达坐标系下局部坐标系中的反光柱信息转换至全局地标坐标系中，获取转换后的反光柱信息。

在本申请实施例中，将定位质量定义为加权最小二乘的残差。参照步骤S1033中全局地标坐标系到雷达坐标的转换过程，将集合S中的全部匹配反光柱对坐标转换关系向量化列出，则有：

将上述等式变形，并定义加权最小二乘残差，即转换后的反光柱信息为：

S105、获取转换后的反光柱信息与全局地标坐标系中的反光柱信息之间的距离误差。

在本申请实施例中，距离误差E的计算过程如下：

S106、根据距离误差以及预设评估算法，获取定位质量评估信息。

在本申请实施例中，定位质量评估信息(距离误差加权平均)为：

Q＝WE

需要说明的是，定位质量评估信息为雷达位姿解算算法定位精度的相对评估指标，并非是当前定位绝对精度的评估，而是通过对用于定位的反光柱位置的评估来反映定位效果，该标量值可清晰明确的体现不同定位结果之间的相对精度(质量)，用于指导算法优化等工作。

本申请实施例提供一种雷达定位装置，用于执行前述一种雷达定位方法。图4为本申请一实施例提供的一种雷达定位装置的结构示意图，如图4所示，该雷达定位包括：获取单元501、匹配单元502以及计算单元503。

获取单元501，用于获取全局地标坐标系中的反光柱信息、以及当前雷达坐标系下局部坐标系中的反光柱信息；

匹配单元502，用于通过反光柱匹配算法获取全局地标坐标系中的反光柱信息与雷达坐标系下局部坐标系中的反光柱信息之间的匹配关系；

计算单元503，用于通过匹配关系以及加权最小二乘算法，计算获取雷达的位姿信息。

可选地，计算单元503，用于根据匹配关系，获取匹配的每对反光柱的最小二乘解算权重；

将最小二乘解算权重进行归一化处理，得到归一化权重；

利用匹配关系以及归一化权重，计算获取雷达的位姿信息。

可选地，计算单元503，用于通过匹配关系以及雷达坐标系下局部坐标系中的反光柱信息，计算每对反光柱与雷达之间的距离；

根据每对反光柱与雷达之间的距离，获取每对反光柱的最小二乘解算权重。

可选地，计算单元503，用于获取所有最小二乘解算权重之和；

将每对反光柱对应的最小二乘解算权重除以最小二乘解算权重之和，获取每对反光柱的归一化权重。

图5为本申请另一实施例提供的雷达定位装置的结构示意图，如图5所示，装置还包括：转换单元504；转换单元504，用于通过最小二乘残差法以及位姿信息，将雷达坐标系下局部坐标系中的反光柱信息转换至全局地标坐标系中，获取转换后的反光柱信息；

获取单元501，用于获取转换后的反光柱信息与全局地标坐标系中的反光柱信息之间的距离误差；

计算单元503，用于根据距离误差以及预设评估算法，获取定位质量评估信息。

图6为本申请实施例提供的一种雷达定位装置的结构示意图，包括：处理器610、存储介质620和总线630，存储介质620存储有处理器610可执行的机器可读指令，当电子设备运行时，处理器610与存储介质620之间通过总线630通信，处理器610执行机器可读指令，以执行上述方法实施例的步骤。具体实现方式和技术效果类似，这里不再赘述。

本申请实施例提供了一种存储介质，该存储介质上存储有计算机程序，所述计算机程序被处理器运行时执行上述方法。

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

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。