根据匹配线段对计算其所属坐标系间位姿修正值的方法技术

本发明专利技术涉及一种根据匹配线段对计算其所属坐标系间位姿修正值的方法，包括以下步骤：(1)旋转待匹配线段集，使得旋转后的待匹配线段集和原本的被匹配线段集之间的匹配线段对的夹角最小；(2)计算角度修正值；(3)计算位置修正值；(4)结合角度修正值和位置修正值，得到最终的位姿修正值。本发明专利技术提供一种能够得到复合解的线段

【技术实现步骤摘要】
根据匹配线段对计算其所属坐标系间位姿修正值的方法


[0001]本专利技术属于图像处理
，涉及一种计算位姿修正值的方法，具体涉及一种根据匹配线段对计算其所属坐标系间位姿修正值的方法。

技术介绍

[0002]关于利用匹配线段对来计算得到位姿修正值这个过程的介绍有很多，其中的方法根据所得位姿修正值的特点不同而大体可以归为两类：第一类得到的位姿修正值是解析解，这类方法利用全部匹配线段对的信息构造一个矩阵，然后根据矩阵运算得到唯一的修正值，这个修正值充分利用了全部的数据；第二类得到的是非解析解(non closed
‑
form solution)，这类方法把每个匹配线段对都转化为对位姿修正值的一个约束，然后根据全部约束求解位姿修正值，由于不同约束可能直接存在矛盾，最终得到的修正值并不能完全保证同时满足每个约束。
[0003]目前，已知的方法都把匹配线段对视为匹配直线对而没有利用线段信息，如PL
‑
ICP、ICL等往往做的是直线
‑
直线匹配，这样一来每条直线上利用的信息只有其方向，这就意味着放弃了线段端点，没有充分利用已知信息，造成了信息的浪费，如公布号为CN 110310331 A的专利技术专利具体公开了一种基于直线特征和点云特征结合的位姿估计方法，其利用的就只有方向，而放弃了线段端点。

技术实现思路

[0004]本专利技术所要解决的技术问题是克服现有技术的不足，提供一种根据匹配线段对计算其所属坐标系间位姿修正值的方法，一种能够得到复合解的线段
‑
线段匹配方法，在计算旋转角度时本方法没有矩阵操作，计算量比常见方法小得多。
[0005]为解决以上技术问题，本专利技术采取的一种技术方案是：
[0006]一种根据匹配线段对计算其所属坐标系间位姿修正值的方法，匹配线段对包括被匹配线段和待匹配线段，所有被匹配线段构成被匹配线段集，记为所有待匹配线段构成待匹配线段集，记为被匹配线段集所在坐标系为被匹配坐标系，待匹配线段集所在坐标系为待匹配坐标系，其中，具有匹配关系的待匹配线段和被匹配线段具有相同的第二下角标，即和相匹配，被匹配坐标系为经过优化的位姿所对应的坐标系；
[0007]该方法包括以下步骤：
[0008](1)旋转待匹配线段集，使得旋转后的待匹配线段集和原本的被匹配线段集之间的匹配线段对的夹角最小；
[0009](2)计算角度修正值；
[0010](3)计算位置修正值；
[0011](4)结合角度修正值和位置修正值，得到最终的位姿修正值；
[0012]步骤(1)对待匹配线段集的旋转操作通过一个二维旋转矩阵R(θ)来实现，二维旋
转矩阵作用在一个待匹配线段矢量之后，这个待匹配线段变成：它和被匹配线段之间的点积是：
[0013]对不同的匹配线段对，直接求和：
[0014][0015]步骤(2)的具体计算过程为：利用三角公式将步骤(1)中得到的公式转化为以下形式：两参数F
max
和满足：
[0016][0017]其中，F
cos
和F
sin
是为了叙述方便而引入的辅助量；通过得到角度修正值的解析解为
[0018]优选地，步骤(3)包括以下步骤：
[0019](3.1)按照角度修正值来旋转全部待匹配线段，然后计算每个匹配线段对之间的垂直距离；
[0020](3.2)遍历处理每个匹配线段对，对于每个匹配线段对：如果旋转后的待匹配线段和被匹配线段大体平行，根据直线间距计算出线段之间的距离，对最终的位置修正值的约束为在这个垂向上对应的长度，即给出一个约束；如果旋转后的待匹配线段和被匹配线段之间的夹角还是很大，即二者不平行，则从被匹配线段的中点向待匹配线段引垂线，按照背离垂足方向的垂线矢量的x和y分量分别构建对最终的位置修正值在x和y方向上的两个约束。
[0021](3.3)全部匹配线段对计算处理完毕后，整理全部约束，利用最小二乘方法求解超定方程组，得到一个非解析解的位置修正值；
[0022](3.4)通过一次坐标变换，把步骤(3.3)得到的位于待匹配坐标系下的位置修正值转换到全局地图坐标系下，得到真正的位置修正值。
[0023]由于以上技术方案的采用，本专利技术与现有技术相比具有如下优点：
[0024]1、本专利技术利用了匹配线段对中的线段属性，而不是和其他算法一样舍弃了线段、只用其直线属性，充分利用已知信息，在计算旋转角度时本方法没有矩阵操作，计算量比常见方法小得多；
[0025]2、本专利技术不需要计算匹配线段的投影，既增加了算法的适用性又降低了算法的难度。
附图说明
[0026]图1为计算位置修正值的流程图；
[0027]图2为在待匹配线段和被匹配线段平行时，求解对平移修正值的约束示意图；
[0028]图3为在待匹配线段和被匹配线段不平行时，求解对平移修正值的约束示意图；
具体实施方式
[0029]以下结合附图及具体实施例对本专利技术做进一步详细说明。
[0030]本专利技术一种根据匹配线段对计算其所属坐标系间位姿修正值的方法，旨在得到两个线段集所属坐标系之间的变换关系。
[0031]在SLAM算法中，每个线段集都对应一个位姿，这个位姿位于全局地图坐标系；而每个线段集中的元素其实位于激光雷达坐标系，这个坐标系的原点和方向来自于线段集所对应的位姿，所以不同的线段集合其实属于不同的坐标系。线基扫描匹配过程需要通过计算两个位姿坐标系之间的差值来获取位姿修正值，这两个坐标系中的若干线段分属于两个坐标系，第一个是上一次扫描匹配后得到的优化位姿所对应的坐标系，其中有上一次的全部线段；第二个是当前正在被扫描匹配过程优化的位姿所对应的坐标系，其中是这一次的全部线段。为了匹配不同的线段集，需要把待匹配的线段集放在同一个坐标系下；同时，由于本专利技术旨在计算位姿修正值，所以还需要确保所获取到的位姿修正值属于全局地图坐标系。由于第一个坐标系是经过优化的，其精度比第二个坐标系的精度高得多，所以这第一个坐标系中的信息相对可信；而第二个坐标系即当前位姿所对应的坐标系其实是亟待优化的，而优化位姿就是要去移动坐标系，所以第二个坐标系是需要被移动(平移和/或旋转)的，而坐标变换又需要在一个坐标系中完成，这个坐标系可以是第二个坐标系、第一个坐标系或全局地图坐标系，由此可以得到完成这种坐标变换的两种方式：要么把两个线段集都转换到全局地图坐标系，要么把一个集合转换到另一个集合的坐标系内。如果把两个线段集合都转换到全局地图坐标系内，这中操作需要对两个集合中的每条直线都做变换，在大集合中会比较费时，所以本专利技术选择将前一线段集转化到当前线段集坐标系内，这样在完成修正后只需要再做一次坐标变换，把位于当前坐标系内的位姿修正值转换到全局地图坐标系内，就能得到真实的位姿修正值。
[0032]通过这些描述，还为消除误解而引入两个相对的概念：被匹配线段和待匹配线段，同时相应还有被匹配坐标系本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种根据匹配线段对计算其所属坐标系间位姿修正值的方法，其特征在于：匹配线段对包括被匹配线段和待匹配线段，所有被匹配线段构成被匹配线段集，记为所有待匹配线段构成待匹配线段集，记为被匹配线段集所在坐标系为被匹配坐标系，待匹配线段集所在坐标系为待匹配坐标系，其中，具有匹配关系的待匹配线段和被匹配线段具有相同的第二下角标，即和相匹配，被匹配坐标系为经过优化的位姿所对应的坐标系；该方法包括以下步骤：(1)旋转待匹配线段集，使得旋转后的待匹配线段集和原本的被匹配线段集之间的匹配线段对的夹角最小；(2)计算角度修正值；(3)计算位置修正值；(4)结合角度修正值和位置修正值，得到最终的位姿修正值；所述步骤(1)对待匹配线段集的旋转操作通过一个二维旋转矩阵R(θ)来实现，二维旋转矩阵作用在一个待匹配线段矢量之后，这个待匹配线段变成：它和被匹配线段之间的点积是：对不同的匹配线段对，直接求和：所述步骤(2)的具体计算过程为：利用三角公式将步骤(1)中得到的公式转化为以下形式：两参数F
max
和满足：其中，F
cos
...

【专利技术属性】
技术研发人员：张二阳，翟冬灵，
申请(专利权)人：苏州寻迹智行机器人技术有限公司，
类型：发明
国别省市：