基于平面运动的自主移动平台的标定方法及其系统和设备与流程

1.本发明涉及自主移动平台技术领域，更具体地涉及基于平面运动的自主移动平台的标定方法及其系统和设备。


背景技术：

2.目前，随着人工智能技术的发展，越来越多的公司及高校团队专注于自主移动平台的研究。自主移动平台通常是指搭载了诸多传感器的车辆，可以实现自主感知和移动，再通过所搭载的任务模块完成相应的工作，如巡检车或扫地机器人等等。该自主移动平台因具有降低人工成本、减少人员接触等优势，也受到越来越多的研发团队的关注。
3.而由于自主移动平台通常需要处理多种复杂任务，并且按照任务需求，该自主移动平台通常需要联合使用雷达和定位器，因此在投入使用前就需要对该自主移动平台上的雷达和定位器进行联合标定。可以理解的是，该自主移动平台上搭载的雷达通常指的是激光探测及测距系统(英文lighet detection andranging；简称lidar)，是通过发射激光束以探测目标的位置、速度等信息。该自主移动平台上搭载的定位器通常指的是北斗导航系统定位器或gps定位器等等各种类型的导航定位系统，用于获取该自主移动平台的位姿、朝向以及速度等。
4.然而，虽然当前学术界对雷达和定位器标定方法的研究多集中于手眼标定或手眼标定的改进方案，但是这些标定方法都要求自主移动平台(如机器人或智能车)在每个坐标轴(如x轴、y轴以及z轴)都存在较大的旋转量，这对于一些要求自主移动平台只做平面运动(如只在水平地面上移动的扫地机器人等)的项目并不现实，无法应用。例如，现有的两步法是先求解旋转矩阵，并对该旋转矩阵正交化后再求平移矩阵，但其缺点在于无法应用到只进行平面运动的机器人或智能车；现有的数学法则是通过数学工具求解手眼关系，其相对于两步法虽然存在更高的精度，但在噪声明显的情况下极其不稳定；现有的运动限制法是通过限制机器人或智能车的运动来求解手眼标定，但在现实场景中实现起来非常困难，需要提供特定的运动轨迹和极高的运动精度；现有的非线性方法虽然是先求出四元数的初始值，再通过非线性优化来求解最终解，但此方法实现复杂、实用性较弱，并且需要其他方法先算出初始值。


技术实现要素：

5.本发明的一优势在于提供基于平面运动的自主移动平台的标定方法及其系统和设备，其能够在自主移动平台只做平面运动的情况下，就能够完成对所述自主移动平台上搭载的雷达和定位器进行标定。
6.本发明的另一优势在于提供基于平面运动的自主移动平台的标定方法及其系统和设备，其中，在本发明的一实施例中，所述基于平面运动的自主移动平台的标定方法不再需要所述自主移动平台在标定过程中具有固定的轨迹和苛刻的位姿，有助于简化标定过程和降低对自主移动平台的限制。
7.本发明的另一优势在于提供基于平面运动的自主移动平台的标定方法及其系统和设备，其中，在本发明的一实施例中，所述基于平面运动的自主移动平台的标定方法能够先标定出两个维度的旋转角度，以将运动过程修正为真正的平面运动，使得所述自动移动平台不需要固定的轨迹和苛刻的位姿，只需要做简单的平面运动就能够进行标定。
8.本发明的另一优势在于提供基于平面运动的自主移动平台的标定方法及其系统和设备，其中，在本发明的一实施例中，所述基于平面运动的自主移动平台的标定方法并不需要借助标定板，并且对外界环境无苛刻要求，有助于适用于各种复杂的场景。
9.本发明的另一优势在于提供基于平面运动的自主移动平台的标定方法及其系统和设备，其中，在本发明的一实施例中，所述基于平面运动的自主移动平台的标定方法能够借助点云配准方案和定位差方案来计算位姿变化，使得其不需要借助标定板，以便适合任何复杂的标定环境。
10.本发明的另一优势在于提供基于平面运动的自主移动平台的标定方法及其系统和设备，其中，在本发明的一实施例中，所述基于平面运动的自主移动平台的标定方法无需赋予待标定参数的初始值，有助于简化标定过程、降低标定难度。
11.本发明的另一优势在于提供基于平面运动的自主移动平台的标定方法及其系统和设备，其中，在本发明的一实施例中，所述基于平面运动的自主移动平台的标定方法能够合理地减少噪声的干扰，有助于使标定方法具备更强的抗噪能力。
12.本发明的另一目的在于提供基于平面运动的自主移动平台的标定方法及其系统和设备，其中，为了达到上述目的，在本发明中不需要采用复杂的结构或算法。因此，本发明成功和有效地提供一解决方案，不只提供一简单的基于平面运动的自主移动平台的标定方法及其系统和设备，同时还增加了所述基于平面运动的自主移动平台的标定方法及其系统和设备的实用性和可靠性。
13.为了实现上述至少一发明目的或其他目的和优点，本发明提供了基于平面运动的自主移动平台的标定方法，包括步骤：
14.s100：获取经由做平面运动的该自主移动平台上搭载的雷达和定位器同时采集的雷达数据和定位器数据；
15.s200：根据该雷达数据和该定位器数据，获得在各个时间段内的雷达变换矩阵和定位器变换矩阵，其中该雷达变换矩阵包括该雷达在各个时间段的旋转变换关系和平移变换关系，并且该定位器变换矩阵包括该定位器在各个时间段的旋转变换关系和平移变换关系；
16.s300：根据在同一时间段的该雷达的旋转变换关系和该定位器的旋转变换关系，标定出从定位器坐标系变换到雷达坐标系的过程中两个维度的旋转角度；以及
17.s400：根据所标定出的该旋转角度和在同一时间段内的该雷达变换矩阵和该定位器变换矩阵，标定出该雷达和该定位器之间相对位姿的剩余参数。
18.根据本技术的一实施例，在所述步骤s100中：搭载有该雷达和该定位器的该自主移动平台在水平平面上做s型的平面运动。
19.根据本技术的一实施例，所述步骤s200包括步骤：
20.通过根据该定位器数据而获得每个时刻从定位器坐标系到世界坐标系的变换关系，计算出该定位器在每个时间段内的该定位器变换矩阵；和
21.通过将该定位器变换矩阵作为初始值，并利用正态分布变换法和迭代最近点法来获取该雷达数据中点云匹配的结果，计算出该雷达在每个时间段内的该雷达变换矩阵。
22.根据本技术的一实施例，所述步骤s300包括步骤：
23.设定该定位器坐标系到该雷达坐标系的变换是依次绕z轴、y轴以及z轴旋转；和
24.根据在同一时间段内该雷达的旋转变换关系和该定位器的旋转变换关系，通过yz角度标定模型计算出从该定位器坐标系变换到该雷达坐标系的过程中先后绕z轴和y轴旋转的旋转向量，以获得分别绕z轴和y轴旋转的角度。
25.根据本技术的一实施例，所述yz角度标定模型被实施为：
[0026][0027]
其中：
[0028][0029]
其中在上式中s表示正弦sin，c表示余弦cos；并且存在两个约束条件如下：
[0030]qyz1qyz4
＝q
yz2qyz3
,||q
yz
||＝1；
[0031]
其中q
yz
表示从该定位坐标系到该雷达坐标系先后绕z轴和y轴旋转的旋转向量，即四元数q
yz
＝[q
yz1
，q
yz2
，q
yz3
，q
yz4
]，对应地绕y轴旋转的角度为β和绕z轴旋转的角度为γ；表示该雷达从时刻i到时刻i+1的时间段内的旋转向量；表示该定位器从时刻i到时刻i+1的时间段内的旋转向量；表示该定位器从时刻i到时刻i+1的时间段内旋转的角度；表示该雷达从时刻i到时刻i+1的时间段内旋转的角度；表示使该yz角度标定模型最小的旋转向量。
[0032]
根据本技术的一实施例，所述步骤s300包括步骤：
[0033]
设定该定位器坐标系到该雷达坐标系的变换是依次绕x轴、y轴以及z轴旋转；和
[0034]
根据在同一时间段内该雷达的旋转变换关系和该定位器的旋转变换关系，通过yx角度标定模型计算出从该定位器坐标系变换到该雷达坐标系的过程中先后绕x轴和y轴旋转的旋转向量，以获得分别绕x轴和y轴旋转的角度。
[0035]
根据本技术的一实施例，所述yx角度标定模型被实施为：
[0036]
[0037]
其中：
[0038][0039]
其中在上式中s表示正弦sin，c表示余弦cos；并且存在两个约束条件如下：
[0040]qyx1qyx4
＝q
yx2qyx3
,||q
yx
||＝1；
[0041]
其中q
yx
表示从该定位坐标系到该雷达坐标系先后绕x轴和y轴旋转的旋转向量，即四元数q
yx
＝[q
yx1
，q
yx2
，q
yx3
，q
yx4
]，对应地绕y轴旋转的角度为β和绕x轴旋转的角度为γ；表示该雷达从时刻i到时刻i+1的时间段内的旋转向量；表示该定位器从时刻i到时刻i+1的时间段内的旋转向量；表示该定位器从时刻i到时刻i+1的时间段内旋转的角度；表示该雷达从时刻i到时刻i+1的时间段内旋转的角度；表示使该yx角度标定模型最小的旋转向量。
[0042]
根据本技术的一实施例，在所述步骤s400中：
[0043]
根据在同一时间段内该平移变换关系和该定位器的平移变换关系，通过平移标定模型计算出从该定位器坐标系变换到该雷达坐标系的过程中剩余的相对位姿参数。
[0044]
根据本技术的一实施例，所述平移标定模型被实施为：
[0045][0046][0047][0048]
其中：表示该定位器从时刻i到时刻i+1的时间段内的旋转角度；tm表示该定位器从时刻i到时刻i+1的时间段内的平移向量的前两维；α表示从该定位器坐标系到该雷达坐标系变换中最后绕z轴旋转的角度；t
x
表示从该定位器坐标系到该雷达坐标系变换中沿x轴的平移量；ty表示从该定位器坐标系到该雷达坐标系变换中沿y轴的平移量。
[0049]
根据本技术的一实施例，所述的基于平面运动的自主移动平台的标定方法，进一步包括步骤：
[0050]
s500：通过非线性最小二乘法，优化所标定出的所有参数，以得到标定参数的最优解。
[0051]
根据本技术的另一方面，本技术进一步提供了基于平面运动的自主移动平台的标定系统，用于标定该自主移动平台上搭载的雷达和定位器，其中所述基于平面运动的自主移动平台的标定系统包括相互可通信地连接的：
[0052]
一数据获取模块，用于获取经由做平面运动的该自主移动平台上搭载的雷达和定位器同时采集的雷达数据和定位器数据；
[0053]
一矩阵获得模块，用于根据该雷达数据和该定位器数据，获得在各个时间段内的雷达变换矩阵和定位器变换矩阵，其中该雷达变换矩阵包括该雷达在各个时间段的旋转变换关系和平移变换关系，并且该定位器变换矩阵包括该定位器在各个时间段的旋转变换关系和平移变换关系；
[0054]
一旋转标定模块，用于根据在同一时间段的该雷达的旋转变换关系和该定位器的旋转变换关系，标定出从定位器坐标系变换到雷达坐标系的过程中两个维度的旋转角度；以及
[0055]
一平移标定模块，用于根据所标定出的该旋转角度和在同一时间段内的该雷达变换矩阵和该定位器变换矩阵，标定出该雷达和该定位器之间相对位姿的剩余参数。
[0056]
根据本技术的一实施例，所述矩阵获得模块包括相互可通信地连接的一定位器矩阵计算模块和一雷达矩阵计算模块，其中所述定位器矩阵计算模块用于通过根据该定位器数据而获得每个时刻从定位器坐标系到世界坐标系的变换关系，计算出该定位器在每个时间段内的该定位器变换矩阵；其中所述雷达矩阵计算模块用于通过将该定位器变换矩阵作为初始值，并利用正态分布变换法和迭代最近点法来获取该雷达数据中点云匹配的结果，计算出该雷达在每个时间段内的该雷达变换矩阵。
[0057]
根据本技术的一实施例，所述旋转标定模块包括相互可通信地连接的一旋转设定模块和一旋转计算模块，其中所述旋转设定模块用于设定该定位器坐标系到该雷达坐标系的变换是依次绕z轴、y轴以及z轴旋转；其中所述旋转计算模块用于根据在同一时间段内该雷达的旋转变换关系和该定位器的旋转变换关系，通过yz角度标定模型计算出从该定位器坐标系变换到该雷达坐标系的过程中先后绕z轴和y轴旋转的旋转向量，以获得分别绕z轴和y轴旋转的角度。
[0058]
根据本技术的一实施例，所述旋转标定模块包括相互可通信地连接的一旋转设定模块和一旋转计算模块，其中所述旋转设定模块用于设定该定位器坐标系到该雷达坐标系的变换是依次绕x轴、y轴以及z轴旋转；其中所述旋转计算模块用于根据在同一时间段内该雷达的旋转变换关系和该定位器的旋转变换关系，通过yx角度标定模型计算出从该定位器坐标系变换到该雷达坐标系的过程中先后绕x轴和y轴旋转的旋转向量，以获得分别绕x轴和y轴旋转的角度。
[0059]
根据本技术的一实施例，所述的基于平面运动的自主移动平台的标定系统，进一步包括一参数优化模块，用于通过非线性最小二乘法，优化所标定出的所有参数，以得到标定参数的最优解。
[0060]
根据本技术的另一方面，本技术进一步提供了电子设备，包括：
[0061]
一处理器，用于执行程序指令；和
[0062]
一存储器，其中所述存储机被配置用于保存可由所述处理器执行以实现基于平面运动的自主移动平台的标定方法的程序指令，其中所述基于平面运动的自主移动平台的标
定方法包括步骤：
[0063]
s100：获取经由做平面运动的该自主移动平台上搭载的雷达和定位器同时采集的雷达数据和定位器数据；
[0064]
s200：根据该雷达数据和该定位器数据，获得在各个时间段内的雷达变换矩阵和定位器变换矩阵，其中该雷达变换矩阵包括该雷达在各个时间段的旋转变换关系和平移变换关系，并且该定位器变换矩阵包括该定位器在各个时间段的旋转变换关系和平移变换关系；
[0065]
s300：根据在同一时间段的该雷达的旋转变换关系和该定位器的旋转变换关系，标定出从定位器坐标系变换到雷达坐标系的过程中两个维度的旋转角度；以及
[0066]
s400：根据所标定出的该旋转角度和在同一时间段内的该雷达变换矩阵和该定位器变换矩阵，标定出该雷达和该定位器之间相对位姿的剩余参数。
[0067]
通过对随后的描述和附图的理解，本发明进一步的目的和优势将得以充分体现。
[0068]
本发明的这些和其它目的、特点和优势，通过下述的详细说明，附图和权利要求得以充分体现。
附图说明
[0069]
图1是根据本发明的一实施例的基于平面运动的自主移动平台的标定方法的流程示意图。
[0070]
图2示出了根据本发明的上述实施例的所述基于平面运动的自主移动平台的标定方法中矩阵获得步骤的流程示意图。
[0071]
图3示出了根据本技术上述实施例的所述矩阵获得步骤中定位器和雷达之间的变换关系示意图。
[0072]
图4示出了根据本发明的上述实施例的所述基于平面运动的自主移动平台的标定方法中旋转标定步骤的第一示例。
[0073]
图5示出了根据本发明的上述实施例的所述基于平面运动的自主移动平台的标定方法中旋转标定步骤的第二示例。
[0074]
图6是根据本发明的一实施例的基于平面运动的自主移动平台的标定系统的框图示意图。
[0075]
图7示出了根据本发明的一实施例的一电子设备的框图示意图。
具体实施方式
[0076]
以下描述用于揭露本发明以使本领域技术人员能够实现本发明。以下描述中的优选实施例只作为举例，本领域技术人员可以想到其他显而易见的变型。在以下描述中界定的本发明的基本原理可以应用于其他实施方案、变形方案、改进方案、等同方案以及没有背离本发明的精神和范围的其他技术方案。
[0077]
在本发明中，权利要求和说明书中术语“一”应理解为“一个或多个”，即在一个实施例，一个元件的数量可以为一个，而在另外的实施例中，该元件的数量可以为多个。除非在本发明的揭露中明确示意该元件的数量只有一个，否则术语“一”并不能理解为唯一或单一，术语“一”不能理解为对数量的限制。
[0078]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
[0079]
目前，虽然当前学术界对雷达和定位器标定方法的研究多集中于手眼标定或手眼标定的改进方案，但是这些标定方法都要求自主移动平台(如机器人或智能车)在每个坐标轴(如x轴、y轴以及z轴)都存在较大的旋转量，这对于一些要求自主移动平台只做平面运动(如只在水平地面上移动的扫地机器人等)的项目并不现实，无法应用。因此，为了解决上述问题，本技术提出了一种基于平面运动的自主移动平台的标定方法及其系统和设备，其能够通过分析所述自主移动平台上搭载的传感器(如雷达和定位器等等)在各个时间段内的运动关系，分两步标定传感器之间的相对位姿参数。
[0080]
具体地，首先，分析运动过程中所述雷达和所述定位器之间的关系：将运动过程按时间分为多段，分别计算出各个时间段内雷达的运动情况和定位器的运动情况，以列出旋转变换关系；根据多组数据，用最小二乘法计算出，在从雷达坐标系变换到定位器坐标系的过程中，分别绕任意两个坐标轴旋转的两个角度；进而用这两个角度来修正整个运动过程，使整个运动过程变为理想状态下的平面运动，从而可以将整个标定任务转化到二维空间中进行。其次，分析经过角度修正后的所述雷达和所述定位器在二维空间里的运动关系；根据各个时间段内所述雷达和所述定位器的位姿变换，并考虑自身带有的误差，列出变换方程；通过最小二乘法计算出需要标定的剩余参数。最后，为了确保标定的准确性，还可以再对三维空间中的运动关系做一次整体优化，以得到各个参数的最优解。
[0081]
值得注意的是，与现有的标定方法不同的是，本技术的所述基于平面运动的自主移动平台的标定方法首先考虑了噪声的影响，降低了计算过程中仪器误差导致的发散性，提升了标定结果的准确度；其次，两个最小二乘法问题都是通过解析和推导获得的，无需为待标定的参数赋予初始值；最后，本技术的标定方法不需要任何标板，而是利用未知特征来解析雷达的运动，适用的场景更加广泛。
[0082]
示意性方法
[0083]
参考说明书附图之图1至图5所示，根据本发明的一实施例的一基于平面运动的自主移动平台的标定方法被阐明。具体地，如图1所示，所述基于平面运动的自主移动平台的标定方法，可以包括步骤：
[0084]
s100：获取经由做平面运动的所述自主移动平台上搭载的雷达和定位器同时采集的雷达数据和定位器数据；
[0085]
s200：根据所述雷达数据和所述定位器数据，获得在各个时间段内的雷达变换矩阵和定位器变换矩阵，其中所述雷达变换矩阵包括所述雷达在各个时间段的旋转变换关系和平移变换关系，并且所述定位器变换矩阵包括所述定位器在各个时间段的旋转变换关系和平移变换关系；
[0086]
s300：根据在同一时间段的所述雷达的旋转变换关系和所述定位器的旋转变换关
系，标定出从定位器坐标系变换到雷达坐标系的过程中两个维度的旋转角度；以及
[0087]
s400：根据所标定出的所述旋转角度和在同一时间段内的所述雷达变换矩阵和所述定位器变换矩阵，标定出所述雷达和所述定位器之间相对位姿的剩余参数。
[0088]
值得注意的是，由于本技术的所述基于平面运动的自主移动平台的标定方法先标定出了两个维度的旋转角度，以便将所述自主移动平台的运动修正为真正的平面运动，使得所述自主移动平台不再需要固定的轨迹和苛刻的位姿，因此本技术的本技术的所述基于平面运动的自主移动平台的标定方法能够对做简单的平面运动的自主移动平台进行标定，并且在标定过程中不需要任何的参照物与标定板，适用任何复杂的场景。
[0089]
更具体地，在所述基于平面运动的自主移动平台的标定方法的所述步骤s100中：搭载有所述雷达和所述定位器的所述自主移动平台优选地在水平平面上做平面运动，使得所述雷达和所述定位器在运动的过程中同时采集相应的所述雷达数据和所述定位器数据。可以理解的是，在本技术的其他示例中，搭载有所述雷达和所述定位器的所述自主移动平台也可以在其他平面上进行运动。
[0090]
更优选地，搭载有所述雷达和所述定位器的所述自主移动平台优选地在水平平面上做s型的平面运动，以便在确保能够采集到标定所需数据的同时，简化所述自主移动平台的运动过程。
[0091]
值得注意的是，本技术的所述雷达可以但不限于被实施为激光探测及测距系统，用于探测目标的位置、速度等信息；换言之，本技术的所述雷达数据可以包括探测目标的点云数据。本技术的所述定位器可以但不限于被实施为北斗导航系统定位器或gps定位器，用于获取所述自主移动平台的位姿、朝向以及速度等；换言之，本技术的所述定位器数据可以包括所述自主移动平台的定位和朝向信息。
[0092]
此外，在启动所述自主移动平台在水平平面上做s型的平面运动并采集到所述雷达数据和所述定位器数据之后，需要获取在各个时间段内所述雷达和所述定位器的运动情况，即在各个时间段内的所述雷达变换矩阵和所述定位器变换矩阵。
[0093]
根据本技术的上述实施例，如图2所示，所述基于平面运动的自主移动平台的标定方法的所述步骤s200，可以包括步骤：
[0094]
s210：通过根据所述定位器数据而获得每个时刻从定位器坐标系到世界坐标系的变换关系，计算出所述定位器在每个时间段内的所述定位器变换矩阵；和
[0095]
s220：通过将所述定位器变换矩阵作为初始值，并利用正态分布变换法和迭代最近点法来获取所述雷达数据中点云匹配的结果，计算出所述雷达在每个时间段内的所述雷达变换矩阵。
[0096]
示例性地，在所述步骤s210中，由于所述定位器数据中包含着所述自主移动平台的定位和朝向信息，因此本技术能够得到每个时刻从定位器坐标系到世界坐标系的变换关系，记时刻i的变换关系为时刻i+1的变换关系为进而，通过时刻变换模型计算出所述定位器从时刻i到时刻i+1的变换关系即在时间段i至i+1内的所述定位器变换矩阵
[0097]
优选地，本技术的所述时刻变换模型可以但不限于被实施为：
[0098][0099]
在所述步骤s220中，所述雷达的变换矩阵可以通过匹配两时刻的点云数据获得，以先后通过正态分布变换法(简称ndt)和迭代最近点法(简称icp)来获取点云匹配的结果。而由于ndt和icp都需要较好的初始值，且所述雷达和所述定位器都是被固定在所述自主移动平台上的，使得所述雷达和所述定位器在同一时间段内所做的运动变换相差较小，因此本技术可以将所述定位器变换矩阵作为初始值，以通过所述正态分布变换法和所述迭代最近点法来计算出所述雷达从时刻i到时刻i+1的变换关系即在时间段i至i+1内的所述雷达变换矩阵
[0100]
值得注意的是，本技术的所述雷达变换矩阵可以被分解为所述雷达的旋转变换关系(包括旋转轴向量、旋转矩阵、旋转轴以及旋转角度)和平移变换关系，例如，用来表示所述雷达从时刻i到时刻i+1的旋转矩阵，来表示所述雷达从时刻i到时刻i+1的旋转向量(四元数)，表示所述雷达从时刻i到i+1时刻绕旋转轴[k
x
，ky，kz]旋转的角度，表示所述雷达从时刻i到i+1时刻的平移向量。同理地，本技术的所述定位器可以被分解为所述定位器的旋转变换关系(包括旋转轴向量、旋转矩阵、旋转轴以及旋转角度)和平移变换关系，例如，用来表示所述定位器从时刻i到时刻i+1的旋转矩阵，来表示所述定位器从时刻i到时刻i+1的旋转向量(四元数)，表示所述定位器从时刻i到i+1时刻绕旋转轴[k
x
，ky，kz]旋转的角度，表示所述定位器从时刻i到i+1时刻的平移向量。
[0101]
而为了求得所述雷达和所述定位器之间的变换关系，本技术可以用r
l,g
表示所述定位器到所述雷达的旋转矩阵，q
l，g
表示所述定位器到所述雷达的旋转向量(如四元数)，t
l，g
表示所述定位器到所述雷达的平移向量。
[0102]
示例性地，根据如图3所示的变换关系可知：从时刻i的定位器坐标系变换到时刻i+1的雷达坐标系存在两种方法。第一种方法是所述定位器先从时刻i运动到时刻i+1，再在时刻i+1由所述定位器坐标系变换到所述雷达坐标系；第二种方法是先在时刻i由所述定位器坐标变换到所述雷达坐标系，再使所述雷达从时刻i运动到时刻i+1。特别地，由于上述两种方法的变换在实质上是相等的，因此本技术能够得到对应的旋转变换关系和平移变换关系如下式(1)和(2)：
[0103][0104][0105]
其中，上式(2)中i3表示三阶单位矩阵。
[0106]
然而，事实上上述变换关系只是理想的情况，在现实中所述雷达和所述定位器通常都会受到噪声的影响，并且噪声主要体现在传感器获取的数据中，即会造成平移误差和旋转误差，进而使得整个运动过程中不再是理性状态下的平面运动。为了解决上述问题，本技术先假设平移矩阵存在的噪声申请先假设平移矩阵存在的噪声其中和δq均代表噪声；同样地，旋
转矩阵中也有相应的噪声再假设其中u表示所述雷达第一次位移的长度，服从正态分布。综上，在考虑到噪声的影响时，本技术所采用的旋转变换关系和平移变换关系被调整为下式(3)和(4)：
[0107][0108][0109]
基于此，在本技术的第一示例中，如图4所示，所述基于平面运动的自主移动平台的标定方法的所述步骤s300，可以包括步骤：
[0110]
s310：设定所述定位器坐标系到所述雷达坐标系的变换是依次绕z轴、y轴以及z轴旋转；和
[0111]
s320：根据在同一时间段内所述雷达的旋转变换关系和所述定位器的旋转变换关系，通过yz角度标定模型计算出从所述定位器坐标系变换到所述雷达坐标系的过程中先后绕z轴和y轴旋转的旋转向量，以获得分别绕z轴和y轴旋转的角度。
[0112]
优选地，本技术的所述yz角度标定模型可以但不限于被实施为：
[0113][0114]
其中：
[0115][0116]
其中在上式中s表示正弦sin，c表示余弦cos；并且存在两个约束条件如下：
[0117]qyz1qyz4
＝q
yz2qyz3
,||q
yz
||＝1。
[0118]
其中q
yz
表示从所述定位坐标系到所述雷达坐标系先后绕z轴和y轴旋转的旋转向量，即四元数q
yz
＝[q
yz1
，q
yz2
，q
yz3
，q
yz4
]，对应地绕y轴旋转的角度为β和绕z轴旋转的角度为γ；表示所述雷达从时刻i到时刻i+1的时间段内的旋转向量；表示所述定位器从时刻i到时刻i+1的时间段内的旋转向量；表示所述定位器从时刻i到时刻i+1的时间段内旋转的角度；表示所述雷达从时刻i到时刻i+1的时间段内旋转的角度；表示使所述yz角度标定模型最小的旋转向量。
[0119]
示例性地，针对有现实干扰的运动情况，本技术能够通过将旋转向量q
yz
、以及旋转角度和输入所述yz角度标定模型，并通过最小二乘法求得使所述yz角度
标定模型最小的旋转向量从而得到从所述定位器坐标系到所述雷达坐标系变换时绕y轴旋转的角度为β和绕z轴旋转的角度为γ。
[0120]
值得注意的是，所述yz角度标定模型可以但不限于根据上述公式(3)和(4)通过合理地简化、推导而来。具体地，根据trawny的结论，当q1、q2是四元数时，存在关系式：
[0121]
其中，
[0122]
此时，上述公式(3)可以变为下式(6)：
[0123][0124]
可以理解的是，要标定所述雷达与所述定位器之间的转角，就需要使从时刻i的所述定位器坐标系变换到时刻i+1的所述雷达坐标系的角度误差最小，即最小化下式(7)：
[0125][0126]
然而，由于直接最小化上式(7)的计算太过复杂，因此本技术对上式(7)进行了化简。具体地，先假设q
l，g
是分别绕z、y、z轴旋转的旋转向量，对应的旋转角度分别为α、β、γ，则
[0127]
式中的四元数分别为：qz(α)＝[0，0，s(α/2)，c(α/2)]
t
；qz(β)＝[0，s(β/2)，0，c(β/2)]
t
；qz(γ)＝[0，0，s(γ/2)，c(γ/2)]
t
。
[0128]
那么，上式(7)中的ηi可以改写成下式(8)：
[0129][0130]
由于上式(8)中的和qz(α)都是绕z轴旋转的，因此两者可以调换位置，以得到下式(9)：
[0131][0132]
接着，联合上式(5)和(9)得下式(10)：
[0133][0134]
其中：
[0135]
之后，记q
yz
＝[q
yz1
，q
yz2
，q
yz3
，q
yz4
]，为正交矩阵，可以从上式(10)中取出，再将上式(5)带入上式(10)得到下式(11)：
[0136][0137]
最后，本技术进一步将上式(11)改写一下形式，以得到所述yz角度标定模型。
[0138]
值得注意的是，根据本技术的上述实施例，在所述基于平面运动的自主移动平台的标定方法的所述步骤s400中：
[0139]
根据在同一时间段内所述雷达的平移变换关系和所述定位器的平移变换关系，通过平移标定模型计算出从所述定位器坐标系变换到所述雷达坐标系的过程中剩余的相对位姿参数。
[0140]
优选地，本技术的所述平移标定模型可以但不限于被实施为：
[0141][0142][0143][0144]
其中：表示所述定位器从时刻i到时刻i+1的时间段内的旋转角度；tm表示所述定位器从时刻i到时刻i+1的时间段内的平移向量的前两维；α表示从所述定位器坐标系到所述雷达坐标系变换中最后绕z轴旋转的角度；t
x
表示从所述定位器坐标系到所述雷达坐标系变换中沿x轴的平移量；ty表示从所述定位器坐标系到所述雷达坐标系变换中沿y轴的平移量。
[0145]
示例性地，在通过所述步骤s300获得使所述yz角度标定模型最小的旋转向量之后，本技术通过将从时刻i的所述定位器坐标系变换到时刻i+1的所述雷达坐标系的平移误差最小，即最小化上述公式(4)，以获得所述雷达和所述定位器之间相对位姿的剩余参数，如α，t
x
以及ty等等。
[0146]
具体地，先将旋转向量代入上式(4)，以得到下式(12)：
[0147][0148]
其中，表示所述定位器从时刻i到时刻i+1的时间段内的旋转角度；表示经过角度修正后的所述雷达从时刻i到时刻i+1的时间段内的平移向量，记作经过角度修正后的所述雷达从时刻i到时刻i+1的时间段内的平移向量，记作表示所述定位器从时刻i到时刻i+1的时间段内的平移向量。
[0149]
可以理解的是，在没有噪声影响的情况下，上式(12)第三行中的所有项都应该为
零，但实际上并不为零，特别是这是由所述雷达的噪声所引起的。由于本技术的所述自主移动平台只做平面运动，在垂直方向(如z轴方向)上的位移变化将不会被考虑，而的第三个维度代表着z轴上的平移量，因此本技术不考虑z轴的平移。特别地，因为平面运动是无法标定出z轴的平移量，所以去除z轴平移的干扰能让标定结果更加准确。于是本技术可以删除上式(12)中的第三行，得到下式(13)：
[0150][0151]
进而整理上式(13)可以得到下式(14)：
[0152][0153]
则上式(14)可以转化成所述平移标定模型，以通过所述平移标定模型就能够解出参数α，t
x
，ty，u，以得到相应的标定结果：所述定位器坐标系到所述雷达坐标系变换中最后绕z轴旋转的角度α，沿x轴的平移量t
x
以及沿y轴的平移量ty。
[0154]
值得一提的是，为了确保标定结果的准确性，本技术可以对三维空间中的运动关系做一次整体优化，以求得各个标定参数(包括所述定位器坐标系到所述雷达坐标系变换中的旋转角度α、β、γ和平移量t
x
、ty)的最优解。
[0155]
具体地，根据本技术的上述实施例，如图1所示，所述基于平面运动的自主移动平台的标定方法可以进一步包括步骤：
[0156]
s500：通过非线性最小二乘法，优化所标定出的所有参数，以得到标定参数的最优解。
[0157]
示例性地，在标定出所述定位器坐标系到所述雷达坐标系变换中的旋转角度α、β、γ和平移量t
x
、ty之后，通过对做非线性最小二乘法来优化上述标定参数，以得到各个参数的最优解。
[0158]
综上所述，本技术的所述角度标定模型和所述平移标定模型在建立时将各个传感器(所述雷达和所述定位器)和环境带来的噪声都考虑在内，大大地减少了噪声的影响。例如，在所述雷达和所述定位器各存在8cm左右的噪声时，本技术的所述基于平面运动的自主移动平台的标定方法的标定结果中平移偏移量小于1.5cm，角度偏移量小于0.8
°
，完全能够满足实际的工业需求。
[0159]
值得注意的是，在本技术的第二示例中，如图5所示，所述基于平面运动的自主移动平台的标定方法的所述步骤s300，可以包括步骤：
[0160]
s310’：设定所述定位器坐标系到所述雷达坐标系的变换是依次绕x轴、y轴以及z轴旋转；和
[0161]
s320’：根据在同一时间段的所述雷达的旋转变换关系和所述定位器的旋转变换关系，通过yx角度标定模型计算出从所述定位器坐标系变换到所述雷达坐标系的过程中先后绕x轴和y轴旋转的旋转向量，以获得分别绕x轴和y轴旋转的角度。
[0162]
优选地，本技术的所述yx角度标定模型可以但不限于被实施为：
[0163]
[0164]
其中：
[0165][0166]
其中在上式中s表示正弦sin，c表示余弦cos；并且存在两个约束条件如下：
[0167]qyx1qyx4
＝q
yx2qyx3
,||q
yx
||＝1。
[0168]
其中q
yx
表示从所述定位坐标系到所述雷达坐标系先后绕x轴和y轴旋转的旋转向量，即四元数q
yx
＝[q
yx1
，q
yx2
，q
yx3
，q
yx4
]，对应地绕y轴旋转的角度为β和绕x轴旋转的角度为γ；表示所述雷达从时刻i到时刻i+1的时间段内的旋转向量；表示所述定位器从时刻i到时刻i+1的时间段内的旋转向量；表示所述定位器从时刻i到时刻i+1的时间段内旋转的角度；表示所述雷达从时刻i到时刻i+1的时间段内旋转的角度；表示使所述yx角度标定模型最小的旋转向量。
[0169]
示例性地，针对有现实干扰的运动情况，本技术能够通过将旋转向量q
yx
、以及旋转角度和输入所述yx角度标定模型，并通过最小二乘法求得使所述yx角度标定模型最小的旋转向量从而得到从所述定位器坐标系到所述雷达坐标系变换时绕y轴旋转的角度为β和绕x轴旋转的角度为γ。
[0170]
值得注意的是，根据本技术的上述第二示例，在通过所述步骤s300获得使所述yx角度标定模型最小的旋转向量之后，本技术通过将从时刻i的所述定位器坐标系变换到时刻i+1的所述雷达坐标系的平移误差最小，即最小化上述公式(4)，以获得所述雷达和所述定位器之间相对位姿的剩余参数，如α，t
x
以及ty等等。
[0171]
具体地，先将旋转向量代入上式(4)，以得到下式(12’)：
[0172][0173]
其中，表示所述定位器从时刻i到时刻i+1的时间段内的旋转角度；表示
经过角度修正后的所述雷达从时刻i到时刻i+1的时间段内的平移向量，记作经过角度修正后的所述雷达从时刻i到时刻i+1的时间段内的平移向量，记作表示所述定位器从时刻i到时刻i+1的时间段内的平移向量。
[0174]
可以理解的是，在没有噪声影响的情况下，上式(12’)第三行中的所有项都应该为零，但实际上并不为零，特别是这是由所述雷达的噪声所引起的。由于本技术的所述自主移动平台只做平面运动，在垂直方向(如z轴方向)上的位移变化将不会被考虑，而的第三个维度代表着z轴上的平移量，因此本技术不考虑z轴的平移。特别地，因为平面运动是无法标定出z轴的平移量，所以去除z轴平移的干扰能让标定结果更加准确。于是本技术可以删除上式(12’)中的第三行，得到下式(13’)：
[0175][0176]
进而整理上式(13’)可以得到下式(14’)：
[0177][0178]
则上式(14’)仍可以转化成所述平移标定模型：
[0179][0180][0181][0182]
其中：表示所述定位器从时刻i到时刻i+1的时间段内的旋转角度；tm表示所述定位器从时刻i到时刻i+1的时间段内的平移向量的前两维；α表示从所述定位器坐标系到所述雷达坐标系变换中最后绕z轴旋转的角度；t
x
表示从所述定位器坐标系到所述雷达坐标系变换中沿x轴的平移量；ty表示从所述定位器坐标系到所述雷达坐标系变换中沿y轴的平移量。
[0183]
进而，通过所述平移标定模型就能够解出参数α，t
x
，ty，u，以得到相应的标定结果：所述定位器坐标系到所述雷达坐标系变换中最后绕z轴旋转的角度α，沿x轴的平移量t
x
以及沿y轴的平移量ty。
[0184]
示意性系统
[0185]
参考说明书附图之图6所示，根据本发明的一实施例的基于平面运动的自主移动平台的标定系统被阐明，用于标定该自主移动平台上搭载的雷达和定位器。具体地，如图6所示，所述基于平面运动的自主移动平台的标定系统1可以包括相互可通信地连接的一数据获取模块10、一矩阵获得模块20、一旋转标定模块30以及一平移标定模块40。所述数据获取模块10用于获取经由做平面运动的该自主移动平台上搭载的雷达和定位器同时采集的雷达数据和定位器数据。所述矩阵获得模块20用于根据该雷达数据和该定位器数据，获得在各个时间段内的雷达变换矩阵和定位器变换矩阵，其中该雷达变换矩阵包括该雷达在各
个时间段的旋转变换关系和平移变换关系，并且该定位器变换矩阵包括该定位器在各个时间段的旋转变换关系和平移变换关系。所述旋转标定模块30用于根据在同一时间段的该雷达的旋转变换关系和该定位器的旋转变换关系，标定出从定位器坐标系变换到雷达坐标系的过程中两个维度的旋转角度。所述平移标定模块40用于根据所标定出的该旋转角度和在同一时间段内的该雷达变换矩阵和该定位器变换矩阵，标定出该雷达和该定位器之间相对位姿的剩余参数。
[0186]
值得注意的是，根据本技术的上述实施例，如图6所示，所述矩阵获得模块20可以包括相互可通信地连接的一定位器矩阵计算模块21和一雷达矩阵计算模块22，其中所述定位器矩阵计算模块21用于通过根据该定位器数据而获得每个时刻从定位器坐标系到世界坐标系的变换关系，计算出该定位器在每个时间段内的该定位器变换矩阵；其中所述雷达矩阵计算模块22用于通过将该定位器变换矩阵作为初始值，并利用正态分布变换法和迭代最近点法来获取该雷达数据中点云匹配的结果，计算出该雷达在每个时间段内的该雷达变换矩阵。
[0187]
在本技术的一示例中，如图6所示，所述旋转标定模块30可以包括相互可通信地连接的一旋转设定模块31和一旋转计算模块32，其中所述旋转设定模块31用于设定该定位器坐标系到该雷达坐标系的变换是依次绕z轴、y轴以及z轴旋转；其中所述旋转计算模块32用于根据在同一时间段内该雷达的旋转变换关系和该定位器的旋转变换关系，通过yz角度标定模型计算出从该定位器坐标系变换到该雷达坐标系的过程中先后绕z轴和y轴旋转的旋转向量，以获得分别绕z轴和y轴旋转的角度。
[0188]
当然，在本技术的另一示例中，所述旋转标定模块30也可以包括相互可通信地连接的一旋转设定模块31和一旋转计算模块32，其中所述旋转设定模块31用于设定该定位器坐标系到该雷达坐标系的变换是依次绕x轴、y轴以及z轴旋转；其中所述旋转计算模块32用于根据在同一时间段内该雷达的旋转变换关系和该定位器的旋转变换关系，通过yx角度标定模型计算出从该定位器坐标系变换到该雷达坐标系的过程中先后绕x轴和y轴旋转的旋转向量，以获得分别绕x轴和y轴旋转的角度。
[0189]
值得一提的是，根据本技术的上述实施例，如图6所示，所述的基于平面运动的自主移动平台的标定系统1可以进一步包括一参数优化模块50，用于通过非线性最小二乘法，优化所标定出的所有参数，以得到标定参数的最优解。
[0190]
示意性电子设备
[0191]
下面，参考图7来描述根据本发明实施例的电子设备(图7示出了根据本发明实施例的电子设备的框图)。如图7所示，电子设备60包括一个或多个处理器61和存储器62。
[0192]
所述处理器61可以是中央处理单元(cpu)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制电子设备60中的其他组件以执行期望的功能。
[0193]
所述存储器62可以包括一个或多个计算程序产品，所述计算程序产品可以包括各种形式的计算可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(ram)和/或高速缓冲存储器(cache)等。所述非易失性存储器例如可以包括只读存储器(rom)、硬盘、闪存等。在所述计算可读存储介质上可以存储一个或多个计算程序指令，所述处理器61可以运行所述程序指令，以实现上文所述的本发明的各个实施例的方法以及/或者其他期望的功能。
[0194]
在一个示例中，如图7所示，电子设备60还可以包括：输入装置63和输出装置64，这些组件通过总线系统和/或其他形式的连接机构(未示出)互连。
[0195]
例如，该输入装置63可以是例如用于采集图像数据或视频数据的摄像模组等等。
[0196]
该输出装置64可以向外部输出各种信息，包括分类结果等。该输出设备64可以包括例如显示器、扬声器、打印机、以及通信网络及其所连接的远程输出设备等等。
[0197]
当然，为了简化，图7中仅示出了该电子设备60中与本发明有关的组件中的一些，省略了诸如总线、输入/输出接口等等的组件。除此之外，根据具体应用情况，电子设备60还可以包括任何其他适当的组件。
[0198]
示意性计算程序产品
[0199]
除了上述方法和设备以外，本发明的实施例还可以是计算程序产品，其包括计算程序指令，所述计算程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本发明各种实施例的方法中的步骤。
[0200]
所述计算程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本发明实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如java、c++等，还包括常规的过程式程序设计语言，诸如c语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。
[0201]
此外，本发明的实施例还可以是计算可读存储介质，其上存储有计算程序指令，所述计算程序指令在被处理器运行时使得所述处理器执行本说明书上述方法中的步骤。
[0202]
所述计算可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(ram)、只读存储器(rom)、可擦式可编程只读存储器(eprom或闪存)、光纤、便携式紧凑盘只读存储器(cd-rom)、光存储器件、磁存储器件、或者上述的任意合适的组合。
[0203]
以上结合具体实施例描述了本发明的基本原理，但是，需要指出的是，在本发明中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本发明的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本发明为必须采用上述具体的细节来实现。
[0204]
本发明中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“诸如但不限于”，且可与其互换使用。
[0205]
还需要指出的是，在本发明的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本发明的等效方案。
[0206]
提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本
发明。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本发明的范围。因此，本发明不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。
[0207]
本领域的技术人员应理解，上述描述及附图中所示的本发明的实施例只作为举例而并不限制本发明。本发明的目的已经完整并有效地实现。本发明的功能及结构原理已在实施例中展示和说明，在没有背离所述原理下，本发明的实施方式可以有任何变形或修改。