一种位姿确定方法和装置制造方法及图纸

本发明专利技术公开了一种位姿确定方法和装置，涉及移动机器人技术领域。该方法包括：将参考点云划分至多个网格；计算网格中参考点云在正态分布下的均值和协方差矩阵；对协方差矩阵进行特征分解；基于当前位姿对目标点云进行变换，得到目标点云所在的网格；根据网格的特征值及特征值对应的特征向量，在目标点云所在的网格中确定有效网格；有效网格包括：面网格和/或线网格；根据均值、有效网格中的目标点云，计算目标函数的值，当满足预设的终止条件时，确定当前位姿为目标位姿，否则，基于目标函数的值更新当前位姿，并执行基于当前位姿对目标点云进行变换；其中，目标函数包括线目标函数和面目标函数。该实施方式能够提高定位结果的精确度。度。度。

【技术实现步骤摘要】
一种位姿确定方法和装置


[0001]本专利技术涉及移动机器人
，尤其涉及一种位姿确定方法和装置。

技术介绍

[0002]精确定位是保障自动驾驶车辆等移动机器人安全行驶的必要条件，其中，基于点云地图的激光雷达定位被广泛使用。该方法是将激光雷达实时扫描得到的目标点云与预先建立的参考点云(即点云地图)进行匹配，得到移动机器人的位姿。目前使用的点云匹配的方法包括NDT(Normal Distributions Transform，正态分布变换法)。
[0003]但是，对于线特征，该方法不仅考虑垂直于线的约束，还考虑平行于线的约束，类似地，对于面特征，该方法不仅考虑垂直于面的约束，还考虑平行于面的约束，而平行于线的约束和平行于面的约束将导致定位结果出现偏差。

技术实现思路

[0004]有鉴于此，本专利技术实施例提供一种位姿确定方法和装置，能够提高定位结果的精确度。
[0005]第一方面，本专利技术实施例提供了一种位姿确定方法，包括：
[0006]将参考点云划分至多个网格；
[0007]计算所述网格中参考点云在正态分布下的均值和协方差矩阵；
[0008]对所述协方差矩阵进行特征分解，得到所述网格的特征值；
[0009]基于当前位姿对目标点云进行变换，得到所述目标点云所在的网格；
[0010]根据所述网格的特征值，在所述目标点云所在的网格中确定有效网格；其中，所述有效网格包括：分布特征为面的面网格和/或分布特征为线的线网格；
[0011]根据所述均值、所述有效网格中的目标点云，计算目标函数的值，当满足预设的终止条件时，确定所述当前位姿为目标位姿，否则，基于所述目标函数的值更新所述当前位姿，并执行所述基于当前位姿对目标点云进行变换；
[0012]其中，所述目标函数包括线目标函数和/或面目标函数，当所述有效网格为线网格时，所述线目标函数的值由垂直于直线的特征向量及其对应的特征值、所述均值、所述有效网格中的目标点云计算得到；当所述有效网格为面网格时，所述面目标函数的值由垂直于面的特征向量及其对应的特征值、所述均值、所述有效网格中的目标点云计算得到。
[0013]第二方面，本专利技术实施例提供了一种位姿确定装置，包括：
[0014]划分模块，配置为将参考点云划分至多个网格；计算所述网格中参考点云在正态分布下的均值；根据所述参考点云在正态分布下的均值和协方差矩阵；
[0015]特征分解模块，配置为对所述协方差矩阵进行特征分解，得到所述网格的特征值；
[0016]变换模块，配置为基于当前位姿对目标点云进行变换，得到所述目标点云所在的网格；
[0017]确定模块，配置为根据所述网格的特征值，在所述目标点云所在的网格中确定有
效网格；其中，所述有效网格包括：分布特征为面的面网格和/或分布特征为线的线网格；根据所述均值、所述有效网格中的目标点云，计算目标函数的值，当满足预设的终止条件时，确定所述当前位姿为目标位姿，否则，基于所述目标函数的值更新所述当前位姿，并触发变换模块；
[0018]其中，所述目标函数包括线目标函数和/或面目标函数，当所述有效网格为线网格时，所述线目标函数的值由垂直于直线的特征向量及其对应的特征值、所述均值、所述有效网格中的目标点云计算得到；当所述有效网格为面网格时，所述面目标函数的值由垂直于面的特征向量及其对应的特征值、所述有效网格中的目标点云计算得到。
[0019]第三方面，本专利技术实施例提供了一种电子设备，包括：
[0020]一个或多个处理器；
[0021]存储装置，用于存储一个或多个程序，
[0022]当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上述任一实施例所述的方法。
[0023]第四方面，本专利技术实施例提供了一种计算机可读介质，其上存储有计算机程序，所述程序被处理器执行时实现如上述任一实施例所述的方法。
[0024]上述专利技术中的一个实施例具有如下优点或有益效果：通过协方差矩阵的特征分解，在目标点云所在的网格中筛选出线网格和面网格，对于线网格，仅使用垂直于直线的特征向量及其对应的特征值计算线目标函数的值，即仅考虑垂直于线的约束，不考虑平行于线的约束；对于面网格，仅使用垂直于面的特征向量及其对应的特征值计算面目标函数的值，即仅考虑垂直于面的约束，不考虑平行于面的约束。本专利技术实施例能够降低平行于线或面的约束所引起的误差，提高位姿的精确度。
[0025]上述的非惯用的可选方式所具有的进一步效果将在下文中结合具体实施方式加以说明。
附图说明
[0026]附图用于更好地理解本专利技术，不构成对本专利技术的不当限定。其中：
[0027]图1是本专利技术的一个实施例提供的一种球形分布点云的示意图；
[0028]图2是本专利技术的一个实施例提供的一种平面分布点云的示意图；
[0029]图3是本专利技术的一个实施例提供的一种直线分布点云的示意图；
[0030]图4是本专利技术的一个实施例提供的一种直线分布点云在优化过程中的约束示意图；
[0031]图5是本专利技术的一个实施例提供的一种位姿确定方法的流程图；
[0032]图6是本专利技术的另一个实施例提供的一种位姿确定方法的流程图；
[0033]图7是本专利技术的一个实施例提供的一种位姿确定装置的示意图；
[0034]图8是适于用来实现本专利技术实施例的终端设备或服务器的计算机系统的结构示意图。
具体实施方式
[0035]以下结合附图对本专利技术的示范性实施例做出说明，其中包括本专利技术实施例的各种
细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本专利技术的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。
[0036]NDT算法将参考点云划分至不同的网格中，基于网格中的参考点云，计算使score(P
vehicle
)取得最大值的P
vehicle
。
[0037][0038]其中，score(P
vehicle
)用于表征当前位姿的得分，用于表征第k个有效网格的目标点云中第i个点的坐标，μ
k
用于表征第k个有效网格中参考点云在正态分布下的均值，∑
k
用于表征第k个有效网格的参考点云在正态分布下的协方差矩阵，M
k
用于表征第k个有效网格的目标点云中点的数量。
[0039]网格中的参考点云可以存在不同的分布形式，如球状分布、平面分布和直线分布，如图1
‑
3所示。
[0040]以线特征为例，在优化过程中，每一个线外的点都会受到向线上的约束，如图4所示，这个约束可以分解为垂直于线的约束和平行于线的约束，垂直于线的约束是优化过程需要的，能够使实时扫描得到的直线与点云地图中的直线重合，而平行于线的本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种位姿确定方法，其特征在于，包括：将参考点云划分至多个网格；计算所述网格中参考点云在正态分布下的均值和协方差矩阵；对所述协方差矩阵进行特征分解，得到所述网格的特征值；基于当前位姿对目标点云进行变换，得到所述目标点云所在的网格；根据所述网格的特征值，在所述目标点云所在的网格中确定有效网格；其中，所述有效网格包括：分布特征为面的面网格和/或分布特征为线的线网格；根据所述均值、所述有效网格中的目标点云，计算目标函数的值，当满足预设的终止条件时，确定所述当前位姿为目标位姿，否则，基于所述目标函数的值更新所述当前位姿，并执行所述基于当前位姿对目标点云进行变换；其中，所述目标函数包括线目标函数和/或面目标函数，当所述有效网格为线网格时，所述线目标函数的值由垂直于直线的特征向量及其对应的特征值、所述均值、所述有效网格中的目标点云计算得到；当所述有效网格为面网格时，所述面目标函数的值由垂直于面的特征向量及其对应的特征值、所述均值、所述有效网格中的目标点云计算得到。2.如权利要求1所述的方法，其特征在于，根据所述网格的特征值，在所述目标点云所在的网格中确定有效网格，包括：根据所述网格的特征值，计算所述目标点云所在的各个网格的线特征值和面特征值；根据所述线特征值和所述面特征值，在所述目标点云所在的多个网格中确定所述线网格和所述面网格；在所述目标点云所在的线网格和面网格中，确定所述有效网格。3.如权利要求2所述的方法，其特征在于，对所述协方差矩阵进行特征分解，得到所述网格的特征值，包括：对所述协方差矩阵进行特征分解，得到所述网格的特征值和所述特征值对应的特征向量；在所述目标点云所在的线网格和面网格中，确定所述有效网格，包括：根据所述面网格的特征值及所述特征值对应的特征向量，将所述目标点云所在的面网格划分为地面网格和非地面网格；在所述目标点云所在的线网格、地面网格和非地面网格中，确定所述有效网格。4.如权利要求3所述的方法，其特征在于，对所述协方差矩阵进行特征分解，得到所述网格的特征值和所述特征值对应的特征向量，包括：基于第一公式，对所述协方差矩阵进行特征分解；所述第一公式包括：其中，Σ用于表征所述协方差矩阵，λ1、λ2和λ3分别用于表征所述特征值，q1、q2和q3分别用于表征与所述特征值对应的特征向量，q1、q2和q3两两正交；根据所述网格的特征值，计算所述目标点云所在的各个网格的线特征值和面特征值，
包括：根据第二公式，计算所述目标点云所在的各个网格的线特征值；根据第三公式，计算所述目标点云所在的各个网格的面特征值；所述第二公式包括：所述第三公式包括：其中，c用于表征所述线特征值，p用于表征所述面特征值，λ1、λ2和λ3分别用于表征所述特征值，λ1≤λ2≤λ3。5.如权利要求4所述的方法，其特征在于，根据所述线特征值和所述面特征值，在所述目标点云所在的多个网格中确定所述线网格和所述面网格，包括：当所述线特征值比所述面特征值大时，所述目标点云所在的网格为所述线网格，否则，所述目标点云所在的网格为所述面网格。6.如权利要求4所述的方法，其特征在于，根据所述面网格的特征值...

【专利技术属性】
技术研发人员：韩锐，郭晗，
申请(专利权)人：新石器慧通北京科技有限公司，
类型：发明
国别省市：