定位与地图构建方法、装置、机器人及计算机存储介质制造方法及图纸

本发明专利技术涉及一种定位与地图构建方法、装置、机器人及计算机存储介质，该方法包括：在t时刻对获取到的点云数据进行直线特征提取，得到对应的多个线段；预估机器人在所述t时刻相较于t‑1时刻的相对姿态；根据所述相对姿态，对与所述t时刻对应的多个线段及在所述t‑1时刻进行直线特征提取所得到的多个线段进行线段匹配，得到各条线段所属的直线；将所述机器人在各个时刻的姿态及所有直线的二直线参数化表示形式进行联合优化，得到所述机器人在所述各个时刻的姿态及所有直线的直线参数。由于在表示直线时，采用的是二直线参数化表示形式，因此，可以尽可能地减少计算的计算量，从而可以提高计算效率，缩短计算时间。

【技术实现步骤摘要】
定位与地图构建方法、装置、机器人及计算机存储介质
本申请属于自主导航领域，具体涉及一种定位与地图构建方法、装置、机器人及计算机存储介质。
技术介绍
移动机器人在运行过程中，需要不断获取数据，从而对自身进行实时定位，此外，还需要根据获取到的数据对周围环境进行地图构建。一般而言，移动机器人通过SLAM(SimultaneousLocalizationAndMapping，实时定位和地图构建)算法来获取并确定用于进行实时定位和地图构建的数据，因此，SLAM算法的优劣直接决定了移动机器人在移动过程中的定位精度和稳定性，因此有必要提出稳定高效且高精度的SLAM算法。在现有技术中，提出一种利用图优化技术实时优化直线特征与机器人姿态的SLAM算法，但是，在该算法中，利用与直线特征对应的6个参数来表示直线，使得对于直线的描述不是最小参数化表示，因此，造成后续的计算量较大，计算效率较低，从而进一步影响实时定位和地图构建的效率。
技术实现思路
有鉴于此，本申请的目的在于提供一种定位与地图构建方法、装置、机器人及计算机存储介质，可以减少定位与地图构建过程中的计算量，从而提高定位与地图构建效率，以便提高后续实时定位和地图构建的效率。本申请的实施例是这样实现的：第一方面，本申请实施例提供一种定位与地图构建方法，所述方法包括：在t时刻对获取到的点云数据进行直线特征提取，得到对应的多个线段；预估机器人在所述t时刻相较于t-1时刻的相对姿态；根据所述相对姿态，对与所述t时刻对应的多个线段及在所述t-1时刻进行直线特征提取所得到的多个线段进行线段匹配，得到各条线段所属的直线；将所述机器人在各个时刻的姿态及所有直线的二直线参数化表示形式进行联合优化，得到所述机器人在所述各个时刻的姿态及所有直线的直线参数。由于在表示直线时，采用的是二直线参数化表示形式，因此，可以尽可能地减少确定用于进行后续实施定位以及地图构建的数据的计算量，从而提高地图定位与地图构建效率，以便提高后续实时定位和地图构建的效率。结合第一方面实施例，在一种可能的实施方式中，所述预估机器人在所述t时刻相较于t-1时刻的相对姿态，包括：设置所述机器人在所述t时刻的姿态为设置所述机器人在所述t-1时刻的姿态为设置所述机器人在所述t时刻的机器人坐标系相对于世界坐标系的旋转矩阵及平移向量分别为根据预先保存的机器人轮式编码器姿态估计算法，计算得到所述t时刻相较于所述t-1时刻的相对姿态其中，之间满足公式：(xk，yk)表示所述机器人在t＝k时刻时相对于世界坐标系w的坐标，表示所述机器人在t＝k时刻时相对于世界坐标系w的朝向，为所述旋转矩阵，为所述平移向量，v为所述机器人的线速度，ω为所述机器人的角速度，△t为所述t时刻与所述t-1时刻之间的时间间隔。结合第一方面实施例，在一种可能的实施方式中，所述对与所述t时刻对应的多个线段及在所述t-1时刻进行直线特征提取所得到的多个线段进行线段匹配，包括：确定所述机器人在所述t-1时刻时所形成的机器人坐标系为参考坐标系；根据所述相对姿态，将与所述t时刻对应的线段的两个端点的坐标转换到所述参考坐标系中，得到与所述t时刻对应的线段在所述参考坐标系下的表达式；将与所述t时刻对应的线段在所述参考坐标系下的表达式及与所述t-1时刻对应的多个线段的表达式进行匹配。结合第一方面实施例，在一种可能的实施方式中，在所述将与所述t时刻对应的线段在所述参考坐标系下的表达式及与所述t-1时刻对应的多个线段的表达式进行匹配之前，所述方法还包括：针对与所述t时刻对应的每个线段，根据预先确定的属性特征及预先保存的属性特征阈值，从与所述t-1时刻对应的所有线段中过滤差异线段，并将与所述t-1时刻对应的剩余的线段确定为与该线段对应的候选线段；相应的，所述将与所述t时刻对应的线段在所述参考坐标系下的表达式及与所述t-1时刻对应的多个线段的表达式进行匹配，包括：针对与所述t时刻对应的每个线段，将该线段在所述参考坐标系下的表达式及与该线段对应的候选线段的表达式进行匹配。通过减少线段匹配时的所需匹配的线段的数量，从而减少计算量，提高计算效率。结合第一方面实施例，在一种可能的实施方式中，在所述将该线段在所述参考坐标系下的表达式及与该线段对应的候选线段的表达式进行匹配之前，所述方法还包括：确定与该线段对应的候选线段的个数不为零；否则，所述方法还包括：确定该线段属于新的直线。结合第一方面实施例，在一种可能的实施方式中，所述将与所述t时刻对应的线段在所述参考坐标系下的表达式及与所述t-1时刻对应的多个线段的表达式进行匹配，包括：针对与所述t时刻对应的每个线段，将该线段和与所述t-1时刻对应的各个单一线段形成线段对，并根据预先构建的损失函数计算与该线段对应的各个线段对的损失值；将所述损失值最小时所对应的线段对确定为属于同一直线；其中，d1，d2为该线段的两个端点到线段对中的另一线段的距离，d0是线段对中两线段的重合区域的长度，为该线段的长度，α是预先设定的控制因子参数，且d1，d2，d0，均由线段对中的两线段的表达式来确定。结合第一方面实施例，在一种可能的实施方式中，所述将所述机器人在各个时刻的姿态及所有直线的二直线参数化表示形式进行联合优化，得到所述机器人在所述各个时刻的姿态及所有直线的直线参数，包括：设置所述机器人在所述各个时刻的姿态的表达式；设置经过所述线段匹配后所得到的所有直线的表达式；通过所述姿态的表达式及所述所有直线的表达式，构建残差向量；通过最小化所述残差向量的平方和，得到所述各个时刻的姿态与所述所有直线之间的最优估计公式；对所述最优估计公式进行迭代求解，得到所述各个时刻的姿态及所有直线的直线参数。第二方面，本申请实施例提供一种定位与地图构建装置，所述装置包括：提取模块、预估模块、匹配模块以及优化模块。提取模块，用于在t时刻对获取到的点云数据进行直线特征提取，得到对应的多个线段；预估模块，用于预估机器人在所述t时刻相较于t-1时刻的相对姿态；匹配模块，用于根据所述相对姿态，对与所述t时刻对应的多个线段及在所述t-1时刻进行直线特征提取所得到的多个线段进行线段匹配，得到各条线段所属的直线；优化模块，用于将所述机器人在各个时刻的姿态及所有直线的二直线参数化表示形式进行联合优化，得到所述机器人在所述各个时刻的姿态及所有直线的直线参数。结合第二方面实施例，在一种可能的实施方式中，所述预估模块，用于设置所述机器人在所述t时刻的姿态为设置所述机器人在所述t-1时刻的姿态为设置所述机器人在所述t时刻的机器人坐标系相对于世界坐标系的旋转矩阵及平移向量分别为根据预先保存的机器人轮式编码器姿态估计算法，计算得到所述t时刻相较于所述t-1时刻的相对姿态其中，与之间满足公式：(xk，yk)表示所述机器人在t＝k时刻时相对于世界坐标系w的坐标，表示所述机器人在t＝k时刻时相对于世界坐标系w的朝向，为所述旋转矩阵，为所述平移向量，v为所述机器人的线速度，ω为所述机器人的角速度，△t为所述t时刻与所述t-1时刻之间的时间间隔。结合第本文档来自技高网...

【技术保护点】
1.一种定位与地图构建方法，其特征在于，所述方法包括：/n在t时刻对获取到的点云数据进行直线特征提取，得到对应的多个线段；/n预估机器人在所述t时刻相较于t-1时刻的相对姿态；/n根据所述相对姿态，对与所述t时刻对应的多个线段及在所述t-1时刻进行直线特征提取所得到的多个线段进行线段匹配，得到各条线段所属的直线；/n将所述机器人在各个时刻的姿态及所有直线的二直线参数化表示形式进行联合优化，得到所述机器人在所述各个时刻的姿态及所有直线的直线参数。/n

【技术特征摘要】
1.一种定位与地图构建方法，其特征在于，所述方法包括：
在t时刻对获取到的点云数据进行直线特征提取，得到对应的多个线段；
预估机器人在所述t时刻相较于t-1时刻的相对姿态；
根据所述相对姿态，对与所述t时刻对应的多个线段及在所述t-1时刻进行直线特征提取所得到的多个线段进行线段匹配，得到各条线段所属的直线；
将所述机器人在各个时刻的姿态及所有直线的二直线参数化表示形式进行联合优化，得到所述机器人在所述各个时刻的姿态及所有直线的直线参数。


2.根据权利要求1所述的方法，其特征在于，所述预估机器人在所述t时刻相较于t-1时刻的相对姿态，包括：
设置所述机器人在所述t时刻的姿态为设置所述机器人在所述t-1时刻的姿态为
设置所述机器人在所述t时刻的机器人坐标系相对于世界坐标系的旋转矩阵及平移向量分别为
根据预先保存的机器人轮式编码器姿态估计算法，计算得到所述t时刻相较于所述t-1时刻的相对姿态
其中，与之间满足公式：(xk，yk)表示所述机器人在t＝k时刻时相对于世界坐标系w的坐标，表示所述机器人在t＝k时刻时相对于世界坐标系w的朝向，为所述旋转矩阵，为所述平移向量，v为所述机器人的线速度，ω为所述机器人的角速度，Δt为所述t时刻与所述t-1时刻之间的时间间隔。


3.根据权利要求1所述的方法，其特征在于，所述对与所述t时刻对应的多个线段及在所述t-1时刻进行直线特征提取所得到的多个线段进行线段匹配，包括：
确定所述机器人在所述t-1时刻时所形成的机器人坐标系为参考坐标系；
根据所述相对姿态，将与所述t时刻对应的线段的两个端点的坐标转换到所述参考坐标系中，得到与所述t时刻对应的线段在所述参考坐标系下的表达式；
将与所述t时刻对应的线段在所述参考坐标系下的表达式及与所述t-1时刻对应的多个线段的表达式进行匹配。


4.根据权利要求3所述的方法，其特征在于，在所述将与所述t时刻对应的线段在所述参考坐标系下的表达式及与所述t-1时刻对应的多个线段的表达式进行匹配之前，所述方法还包括：
针对与所述t时刻对应的每个线段，根据预先确定的属性特征及预先保存的属性特征阈值，从与所述t-1时刻对应的所有线段中过滤差异线段，并将与所述t-1时刻对应的剩余的线段确定为与该线段对应的候选线段；
相应的，所述将与所述t时刻对应的线段在所述参考坐标系下的表达式及与所述t-1时刻对应的多个线段的表达式进行匹配，包括：
针对与所述t时刻对应的每个线段，将该线段在所述参考坐标系下的表达式及与该线段对应的候选线段的表达式进行匹配。


5.根据权利要求4所述...

【专利技术属性】
技术研发人员：贺一家，赖文芊，刘骁，沈毅，
申请(专利权)人：北京迈格威科技有限公司，
类型：发明
国别省市：北京;11