CN117824666B - 融合定位用二维码对、二维码标定方法及融合定位方法 - Google Patents

Abstract

本发明涉及一种融合定位用二维码对、二维码标定方法及融合定位方法，对于二维码对，包括第一二维码和第二二维码，第一二维码和第二二维码的id和为一个预设的常数，且第一二维码的id和第二二维码的id不同；第一二维码和第二二维码沿机器人行进路径的中轴线交错布置，且第一二维码和第二二维码在与中轴线垂直方向上的相互远离的侧边的距离小于或等于二维码读码器的扫描范围；第一二维码和第二二维码沿中轴线方向上的互相靠近的侧边的距离大于预设的第一距离且小于预设的第二距离。在对同一位置的二维码进行标定时，可以基于统一位置的两个二维码的相对位置进行标定，提高标定精度，交错布置的二维码，也可以使得对机器人进行定位时的鲁棒性更高。

Description

融合定位用二维码对、二维码标定方法及融合定位方法

技术领域

本发明涉及机器人导航定位领域，具体涉及一种融合定位用二维码对、二维码标定方法及融合定位方法。

背景技术

在机器人定位中，为了应对高动态甚至是完全变化情况下SLAM定位，通常需要结合其他回环检测机制，常用的方法是二维码、RFID等。二维码因为其高精度更常作为融合SLAM的首选。不同于纯二维码定位，二维码融合SLAM定位通常需要标定出二维码在SLAM全局坐标系下的位姿，现有的融合方式存在标定精度不够，激光里程计精度不够等问题，为了保证机器人不会由于累计误差，无法扫描到下一张二维码，通常需要密集的布置二维码，这往往需要很大的部署成本。

发明内容

本申请要解决的技术问题是提供一种融合定位用二维码对、二维码标定方法及融合定位方法，具有可以实现更高的二维码标定精度及更高精度的机器人定位的特点。

第一方面，一种实施例中提供一种融合定位用二维码对，用于对机器人进行融合定位，所述二维码对包括第一二维码和第二二维码，所述第一二维码和第二二维码的id和为一个预设的常数，且第一二维码的id和第二二维码的id不同；所述第一二维码和第二二维码沿机器人行进路径的中轴线交错布置，且第一二维码和第二二维码在与中轴线垂直方向上的相互远离的侧边的距离小于或等于二维码读码器的扫描范围；所述第一二维码和第二二维码沿中轴线方向上的互相靠近的侧边的距离大于预设的第一距离且小于预设的第二距离。

一种实施例中，所述的所述第一二维码和第二二维码沿机器人行进路径的中轴线交错布置，包括：

所述第一二维码和第二二维码等距分布于所述中轴线的两侧，且第一二维码和第二二维码中心连线与所述中轴线的夹角为30度。

一种实施例中，所述第一二维码和第二二维码沿中轴线方向上的互相靠近的侧边的距离大于读码器的扫描范围。

一种实施例中，所述第一二维码和第二二维码沿中轴线方向上的互相靠近的侧边的距离为15cm。

第二方面，一种实施例中提供一种融合定位用二维码标定方法，用于对全局环境地图对应的路径上布置好的二维码进行标定，其特征在于，所述的布置好的二维码为成对布置，其中，任意一个成对布置的二维码对采用上述任意一实施例的二维码对；标定方法包括：

扫描每一张二维码，对于任意一张扫描到的二维码，获取二维码数据帧，同时获取激光数据帧作为激光关键帧；

对于任意一个二维码数据帧，获取该二维码数据帧相对于机器人的坐标作为第一坐标，对于任意一个激光关键帧，获取机器人的坐标作为第二坐标；

对于任意一个二维码对，获取二维码对的相对坐标；

基于所有二维码对的相对坐标及所有二维码的第一坐标和对应的第二坐标，对任意一张二维码的坐标进行非线性优化，得到所有二维码的坐标。

一种实施例中，所述的基于所有二维码对的相对坐标及所有二维码的第一坐标和对应的第二坐标，对任意一张二维码的坐标进行非线性优化，得到所有二维码的坐标，包括：

，

其中，表示二维码j所对应的在i时刻的第一坐标，/>表示二维码j所对应的在i时刻的第二坐标，i表示时刻的索引，1≤i≤I，I表示时刻的总数，j和k均表示二维码的索引，1≤j,s≤J，J表示二维码的总数，/>表示欧式距离计算；/>表示二维码j的坐标，表示二维码k的坐标，且二维码j和二维码s组成二维码对，/>表示二维码j和二维码s的相对坐标，/>和/>随着优化而改变；/>表示待优化的所有二维码的坐标集合。

第三方面，一种实施例中提供一种融合定位方法，该定位方法基于在全局环境地图对应的路径上布置好的二维码实现，应用于对机器人小车定位装置；定位方法包括：

在机器人的行进路线上扫描任意一张二维码，基于该任意一张二维码的标定结果，得到任意时刻机器人的定位坐标作为第一定位坐标；所述标定结果基于上述任意一实施例的标定方法得到；

在行进路线上扫描任意一张二维码时，将获得的所有激光帧作为激光关键帧，并标记为二维码激光关键帧，对于任意一个二维码激光关键帧，选取该二维码激光关键帧所对应的多个二维码数据帧，对该多个二维码数据帧所一一对应的多个第一定位坐标及机器人的速度进行非线性优化，得到优化后的多个第一定位坐标的集合及优化后的机器人的速度；

对于任意一个二维码激光关键帧，基于其所对应的第一时刻的二维码数据帧所对应的优化后的第一个时刻的第一定位坐标和机器人的速度，采用时间速度和时间差计算出坐标作为该任意一个二维码激光关键帧的坐标；

基于该任意一个二维码激光关键帧的坐标得到对应的机器人的坐标。

一种实施例中，所述的在机器人的行进路线上扫描任意一张二维码，基于该任意一张二维码的标定结果，得到任意时刻机器人的定位坐标作为第一定位坐标，包括：

，

其中，表示i时刻的第一定位坐标，/>表示二维码j所对应的在i时刻的第一坐标，i表示时刻的索引，1≤i≤I，I表示时刻的总数，j表示二维码的索引，1≤j≤J，J表示二维码的总数；/>表示二维码j的坐标，随着优化而改变。

一种实施例中，所述的对于任意一个二维码激光关键帧，选取该二维码激光关键帧所对应的多个二维码数据帧，对该多个二维码数据帧所一一对应的多个第一定位坐标及机器人的速度进行非线性优化，得到优化后的多个第一定位坐标的集合及优化后的机器人的速度，包括：

，

其中，表示该多个二维码数据帧所对应的机器人的速度，/>，/>表示线速度，/>表示角速度；/>表示所述多个第一定位坐标的集合，j表示二维码的索引，1≤j≤J，J表示二维码的总数；f为代价函数，，/>表示欧式距离计算，i表示时刻的索引，1≤i≤I，I表示时刻的总数，/>表示机器人观测的协方差的倒数，/>表示机器人预测的协方差的倒数，基于实际的机器人模型以及二维码传感器的精度确定；/>表示i-1时刻到i时刻机器人的位移，/>，/>表示i-1时刻到i时刻的时间增量，/>表示i-1时刻机器人的yaw角，/>表示x方向的位移，/>表示y方向的位移，/>表示机器人的yaw角的增量。

一种实施例中，所述的对于任意一个二维码激光关键帧，基于其所对应的第一时刻的二维码数据帧所对应的优化后的第一个时刻的第一定位坐标和机器人的速度，采用时间速度和时间差计算出坐标作为该任意一个激光关键帧的坐标，包括：

，

其中，表示任意一个二维码激光关键帧的坐标，表示该任意一个二维码激光关键帧所对应的第一时刻的二维码数据帧所对应的优化后的第一个时刻的第一定位坐标/>，/>表示该任意一个二维码激光关键帧所对应的第一时刻的二维码数据帧所对应的机器人的yaw角，/>表示扫描到二维码的时刻到二维码激光关键帧的时刻的时间差。

本发明的有益效果是：

对于本申请的二维码对，在对同一位置的二维码进行标定时，由于二维对的存在，可以基于统一位置的两个二维码的相对位置进行标定，提高标定精度。同时，交错布置的二维码，也可以使得对机器人进行定位时，即使机器人的路线走偏，也更容易扫描到同一位置的二维码，鲁棒性更高。

附图说明

图1是本申请一种实施例的二维码对交错布置方式示意图；

图2是本申请第二种实施例的二维码对交错布置方式示意图；

图3是本申请第三种实施例的二维码对交错布置方式示意图；

图4是本申请第四种实施例的二维码对交错布置方式示意图；

图5是本申请一种实施例的融合定位用二维码标定方法流程示意图；

图6是本申请一种实施例的融合定位方法的流程示意图；

图7是本申请一种实施例的关键激光帧与二维码数据帧的对应示意图；

图8是本申请一种实施例的对第一定位坐标进行非线性优化前的定位示意图；

图9是本申请图8所示实施例的对第一定位坐标进行非线性优化后的定位示意图。

具体实施方式

下面通过具体实施方式结合附图对本发明作进一步详细说明。其中不同实施方式中类似元件采用了相关联的类似的元件标号。在以下的实施方式中，很多细节描述是为了使得本申请能被更好的理解。然而，本领域技术人员可以毫不费力的认识到，其中部分特征在不同情况下是可以省略的，或者可以由其他元件、材料、方法所替代。在某些情况下，本申请相关的一些操作并没有在说明书中显示或者描述，这是为了避免本申请的核心部分被过多的描述所淹没，而对于本领域技术人员而言，详细描述这些相关操作并不是必要的，他们根据说明书中的描述以及本领域的一般技术知识即可完整了解相关操作。

另外，说明书中所描述的特点、操作或者特征可以以任意适当的方式结合形成各种实施方式。同时，方法描述中的各步骤或者动作也可以按照本领域技术人员所能显而易见的方式进行顺序调换或调整。因此，说明书和附图中的各种顺序只是为了清楚描述某一个实施例，并不意味着是必须的顺序，除非另有说明其中某个顺序是必须遵循的。

本文中为部件所编序号本身，例如“第一”、“第二”等，仅用于区分所描述的对象，不具有任何顺序或技术含义。

为便于对本申请的发明构思进行说明，以下对机器人定位技术进行简要说明。

在机器人的定位中，一种常用的方式是二维码结合轮式里程计，这种方式当轮子出现打滑时会导致精度下滑，甚至出现无法沿路径扫描到下一个二维码，导致定位丢失的问题。有些方法也结合了激光导航，在机器人的激光定位中，通过对激光关键帧的选取，来降低后端回环检测的算例需求，通常情况下，通过对角度阈值、距离阈值、时间阈值来过滤掉不需要的激光帧，剩下的激光帧作为激光关键帧，基于激光关键帧实现对机器人的定位，得到激光定位的全局位姿。

为了应对高动态环境下，激光定位失效，使用二维码辅助机器人定位是一种常见的做法，但现有方式存在部署要求高、鲁棒性低等缺点。现有融合激光导航的方法更多用在二次定位，使用激光导航到站点附近，扫描二维码后开始局部定位，从而实现高精度的站点取货，而激光导航在环境变化的情况下很容易出现失效的情况，导致激光导航无法到达对应的站点。也有方法实现了激光和二维码的融合定位，然而现有的融合方式存在标定精度不够，激光里程计精度不够等问题，为了保证机器人不会由于累计误差，无法扫描到下一张二维码，通常需要密集的布置二维码，这往往需要很大的部署成本。

鉴于此，本申请提供一种融合定位用二维码对，用于对机器人进行融合定位，基于该二维码对，可以提高二维码的标定精度，从而提高激光里程计的精度。

请参考图1，本申请的一种实施例中，该二维码对包括第一二维码和第二二维码，布置时在同一个位置进行布置。为了便于对二维码对进行识别，第一二维码和第二二维码的id和为一个预设的常数，且第一二维码的id和第二二维码的id不同。一种实施例中，该预设的常数可以是1000000，也可以是100000等，根据用户的需求进行设置。其中，二维码的id可以是DM码，也可以是QR码。

第一二维码和第二二维码沿机器人的行进路径的中轴线交错布置，且第一二维码和第二二维码在与中轴线垂直方向上的相互远离的侧边的距离小于或等于二维码读码器的扫描范围。

一方面，通过交错布置，使得第一二维码和第二二维码位于中轴线的两侧，这样，在对机器人进行定位时，对于同一个位置，如果机器人的行进方向的中心线不在路径的中轴线上，如果左偏，则机器人更大概率扫描到位于中轴线左侧的二维码，如果右偏，则机器人更大概率扫描到位于中轴线右侧的二维码，使得机器人在行进时，不论左偏还是右偏，都更容易扫描到该二维码对的任意一个二维码实现融合定位，鲁棒性更高，而对于该二维码对，对机器人进行定位时，扫描到二维码对中的任意一个二维码都可以实现融合定位。

另一方面，交错布置的同时，为提高二维码的标定精度，需基于二维码对中的第一二维码和第二二维码的相对位置进行标定，因此，标定时，机器人通过该二维码对所在的位置时，既需要扫描到第一二维码，又需要扫描到第二二维码，这就需要第一二维码和第二二维码在与中轴线垂直方向上的相互远离的侧边的距离小于或等于二维码读码器的扫描范围，包括：d₁≤w₁，其中，d₁表示第一二维码和第二二维码在与中轴线垂直方向上的相互远离的侧边的距离，w₁表示二维码读码器扫描宽度。

一种实施例中，第一二维码和第二二维码交错布置的方式如图1所示，第一二维码和第二二维码等距分布于中轴线的两侧，且第一二维码和第二二维码中心连线与中轴线的夹角β为30度。

其他实施例中，第一二维码和第二二维码交错布置的方式还可以采用如图2到图4任意一种所示的具体布置方式。

为了便于布置，根据实际需求，第一二维码和第二二维码沿中轴线方向上的互相靠近的侧边的距离需要小于预设的第二距离，然而，申请在研究中发现，如果距离太近，标定的精度也会降低，因此，根据实际精度需求，第一二维码和第二二维码沿中轴线方向上的互相靠近的侧边的距离需要大于预设的第一距离。

考虑到并不是所有的读码器均可以同时实现对两个二维码的识别，本申请的一种实施例中，第一二维码和第二二维码沿中轴线方向上的互相靠近的侧边的距离大于读码器的扫描范围，考虑到交错布置的二维码，读码器扫描时的扫描范围要考虑对角线的扫描范围，则包括：d₂≥w₂，其中，d₂表示第一二维码和第二二维码沿中轴线方向上的互相靠近的侧边的距离，w₂表示二维码读码器对角线的扫描宽度。

一种实施例中，第一二维码和第二二维码沿中轴线方向上的互相靠近的侧边的距离为15cm。

基于上述任意一实施例步骤的二维码对，在对同一位置的二维码进行标定时，由于二维对的存在，可以基于统一位置的两个二维码的相对位置进行标定，提高标定精度。同时，交错布置的二维码，也可以使得对机器人进行定位时，即使机器人的路线走偏，也更容易扫描到同一位置的二维码，鲁棒性更高。

本申请的一种实施例中提供一种融合定位用二维码标定方法，该标定方法用于对全局环境地图对应的路径上布置好的二维码进行标定，其中，布置好的二维码为成对布置，任意一个成对布置的二维码对采用上述任意一实施例中的二维码对，该标定方法中，基于二维码的相对位置进行标定，提高了标定精度，从而有助于进一步提升机器人的定位精度。

为清楚地说明该标定方法，首先对其应用的一种具体环境进行说明，一种实施例中，可以通过SLAM构建全局环境地图，构建完全局环境地图后，根据需要在路径上布置二维码对，布置的相邻二维码对之间间隔取决于环境和激光精度。一种实施例中，为保证环境变化后运行的稳定性，每5m-10m布置一二维码对。布置完二维码后，一种具体实施例中，则可以控制AGV小车扫描每一张布置好的二维码实现二维码的标定。

请参照图5，该标定方法包括：

步骤S101，扫描每一张二维码，对于任意一张扫描到的二维码，获取二维码数据帧，同时获取激光数据帧作为激光关键帧。

步骤S102，对于任意一个二维码数据帧，获取该二维码数据帧相对于机器人的坐标作为第一坐标，对于任意一个激光关键帧，获取机器人的坐标作为第二坐标。

步骤S103，对于任意一个二维码对，获取二维码对的相对坐标。

步骤S104，基于所有二维码对的相对坐标及所有二维码的第一坐标和对应的第二坐标，对任意一张二维码的坐标进行非线性优化，得到所有二维码的坐标。

基于上述应用环境，一种实施例中，步骤S104包括：

，

其中，表示二维码j所对应的在i时刻的第一坐标，/>表示二维码j所对应的在i时刻的第二坐标，i表示时刻的索引，1≤i≤I，I表示时刻的总数，j和k均表示二维码的索引，1≤j,s≤J，J表示二维码的总数，/>表示欧式距离计算；/>表示二维码j的坐标，表示二维码k的坐标，且二维码j和二维码s组成二维码对，/>表示二维码j和二维码s的相对坐标，/>和/>随着优化而改变；/>表示待优化的所有二维码的坐标集合。

扫描到二维码后，通过上述标定，可以根据SLAM定位和扫描到的二维码的坐标，联合优化二维码在全局坐标系中的坐标，由于是基于二维码的相对位置进行标定，因此可以标定出更精确的二维码坐标，又由于该步骤紧接建图步骤，此时无环境变化，可以标定出较好的二维码坐标。自此，完成了环境的布置。

一种实施例中，为了保证标定精度，可以来回多次、多角度经过二维码，以便优化出更为精确的坐标。

目前的激光和二维码的融合定位中，一种方案中，分别采集通过二维码得到的全局位姿和激光定位得到的全局位姿，然后通过对齐得到的两个全局位姿来纠正机器人全局位姿来实现对机器人的定位。

然而，申请人在研究中发现，上述定位方法中，想要得到较高精度定位需要保证二维码传感器和激光雷达相对位姿的高精度标定，也需要二维码传感器和激光雷达的时间戳精确地同步，才能得到在相同时刻时机器人在两个坐标系下的定位。但实际场景中，特别是当需要适配各种传感器与用户设备时，很难保证较高精度的时间同步以及相对位姿的标定，这就导致了激光和二维码定位两者融合的精度很难保证，进一步导致定位的精确性很难提高。

鉴于上述问题，本申请的一种实施中提供了一种融合定位方法，该定位方法基于在全局环境地图对应的路径上布置好的二维码实现，可以应用于对机器人小车定位装置。该定位方法中，通过激光的坐标估计得到所有二维码的坐标，从而实现对所有二维码的标定。基于每张二维码的标定结果，对任意时刻的机器人进行定位。定位时，在行进路线上扫描任意一张二维码时，将获得的所有激光帧作为激光关键帧，并标记为二维码激光关键帧，对于任意一个二维码激光关键帧，选取该二维码激光关键帧所对应的多个二维码数据帧，对该多个二维码数据帧所一一对应的多个第一定位坐标及机器人的速度进行非线性优化，从而实现多帧融合解算出更准确的坐标，将机器人与二维码之间的相对坐标转化为机器人在全局坐标系下的坐标，提高机器人定位的精确性。

请参考图6，本申请一种实施例中提供的融合定位方法包括：

步骤S201，在机器人的行进路线上扫描任意一张二维码，基于该任意一张二维码的标定结果，得到任意时刻机器人的定位坐标作为第一定位坐标。

一种实施例中，该标定结果基于上述任意一实施例的标定方法得到。

一种实施例中，机器人可以为AGV小车。当AGV运行定位时，激光导航程序由前端里程计+后端回环检测构成。前端里程计得到小车在里程计坐标系下实时的定位，同时激光经过关键帧选择器的过滤后，得到激光关键帧进行回环检测。当小车检测到回环时，可以优化得到里程计坐标系到全局坐标系的转换关系，以消除里程计的累计误差。当二维码读码器扫描到二维码时，可以获得二维码相对于小车的坐标，此时，关键帧选择器会将所有激光帧打上二维码激光关键帧的标签，传入后端，而不会进行过滤，二维码激光关键帧也不会进行回环检测。

一种实施例中，在机器人的行进路线上扫描任意一张二维码，基于该任意一张二维码的标定结果，得到任意时刻机器人的定位坐标作为第一定位坐标，包括：

，

其中，表示i时刻的第一定位坐标。

以此，可以将AGV小车与二维码之间的相对坐标转化为小车在全局坐标系下的坐标。

步骤S202，对于任意一个二维码激光关键帧，选取该二维码激光关键帧所对应的多个二维码数据帧，对该多个二维码数据帧所一一对应的多个第一定位坐标及机器人的速度进行非线性优化，得到优化后的多个第一定位坐标的集合及优化后的机器人的速度。在行进路线上扫描任意一张二维码时，将获得的所有激光帧作为激光关键帧，并标记为二维码激光关键帧。

当扫描到二维码时，将所有的激光帧作为激光关键帧，并标记为二维码激光关键帧，以进行二维码激光关键帧坐标的结算。请参照图7，通常情况下，激光帧率为20Hz，二维码相机帧率为100Hz，因此，在一个二维码激光关键帧时刻，会对应有多个二维码数据帧。

为了进行多帧融合算出更准确的位姿，根据AGV小车的机动性，假设在任意个二维码激光关键帧的时间内是匀速运动的，则有：一种实施例中，对于任意一个二维码激光关键帧，选取该二维码激光关键帧所对应的多个二维码数据帧，对该多个二维码数据帧所一一对应的多个第一定位坐标及机器人的速度进行非线性优化，得到优化后的多个第一定位坐标的集合及优化后的机器人的速度，包括：

，

其中，表示该多个二维码数据帧所对应的机器人的速度，/>，/>表示线速度，/>表示角速度；/>表示所述多个第一定位坐标的集合；f为代价函数，，/>表示欧式距离计算，/>表示机器人观测的协方差的倒数，/>表示机器人预测的协方差的倒数，基于实际的机器人模型以及二维码传感器的精度确定，/>表示i-1时刻到i时刻机器人的位移，，/>表示i-1时刻到i时刻的时间增量，表示i-1时刻机器人的yaw角，也是变量/>中的角度部分，/>表示x方向的位移，/>表示y方向的位移，/>表示机器人的yaw角的增量。

一种实施例中，对于任意一个二维码激光关键帧，选取该二维码激光关键帧所对应的多个二维码数据帧，包括：对于二维码时刻，将任意一个二维码激光关键帧之前一个时刻的二维码数据帧和之后两个时刻的二维码数据帧作为任意一个激光关键帧所对应的二维码数据帧。例如，请参照图7，对于第一个二维码激光关键帧，取第二个二维码数据帧（二维码时刻，二维码激光关键帧之前一个时刻的二维码数据帧）和第三、四个（二维码时刻，二维码激光关键帧之后两个时刻的二维码数据帧）作为第一个二维码激光关键帧所对应的二维码数据帧。对于第二个二维码激光关键帧，取第六个二维码数据帧（二维码时刻，二维码激光关键帧之前一个时刻的二维码数据帧）和第七、八个（二维码时刻，二维码激光关键帧之后两个时刻的二维码数据帧）作为第二个二维码激光关键帧所对应的二维码数据帧。

请参考图8，为优化前的定位。请参考图9，为优化后的定位。通过上述优化方法，可以将识别的误差降低。

当二维码激光关键帧到达后端后，会经过关键帧坐标结算，该过程经过两次非线性优化，拿到二维码激光关键帧的全局坐标，所以即使读码器和激光的时间戳不是特别精确、没有做对齐，也能保证较好的精度的AGV坐标估计，对激光雷达和相机的坐标转换要求降低，时间同步要求降低，同时提高了定位的精度。

步骤S203，对于任意一个二维码激光关键帧，基于其所对应的第一时刻的二维码数据帧所对应的优化后的第一个时刻的第一定位坐标和机器人的速度，采用时间速度和时间差计算出坐标作为该任意一个二维码激光关键帧的坐标。

我们可以使用二维码激光关键帧所对应的第一个时刻的坐标作为初始坐标，使用速度时间差计算出二维码激光关键帧的坐标，一种实施例中，包括：

，

其中，表示任意一个二维码激光关键帧的坐标，表示该任意一个二维码激光关键帧所对应的第一时刻的二维码数据帧所对应的优化后的第一个时刻的第一定位坐标/>，/>表示该任意一个二维码激光关键帧所对应的第一时刻的二维码数据帧所对应的机器人的yaw角，/>表示扫描到二维码的时刻到二维码激光关键帧的时刻的时间差。

步骤S204，基于该任意一个二维码激光关键帧的坐标得到对应的机器人的坐标。

一种实施例中，将该任意一个二维码激光关键帧的坐标作为机器人的坐标。

然而，申请人在研究中发现，如果时间戳较为准确，则得到的二维码激光关键帧的坐标也较为准确，但世界物理环境中调整这些时间戳比较困难，特别是适配不同传感器不同车型时。

鉴于此，为了得到更准确的二维码激光关键帧，基于地图和激光的匹配进行了二次非线性优化，包括：将任意一个二维码激光关键帧的坐标作为残差加入非线性优化函数进行优化，将优化后的该任意一个二维码激光关键帧的坐标作为机器人的坐标。

将栅格地图进行双三次插值，对二维码激光关键帧进行优化，为了更好地实现即使环境变化下，优化的结果不会比优化前更差，一种实施例中，将任意一个二维码激光关键帧的坐标作为残差加入非线性优化函数进行优化，包括：

，

其中，表示优化后的所述任意一个二维码激光关键帧的坐标，/>表示所述任意一个二维码激光关键帧的代价函数，，/>表示所述任意一个二维码激光关键帧扫描的激光点云，k表示激光点云中点的索引，K表示激光点云中点的总数，/>表示二维码观测的权重，/>表示栅格地图双三次插值函数，根据输入的点云坐标，得到栅格地图在此处插值后的结果。

基于上述实施例，将二维码计算出的初始位姿作为残差项加入非线性优化函数进行优化，确保只有当栅格地图和二维码激光关键帧较好时（H矩阵的特征值较大，比较满足凸函数性质），才会调整较多的坐标，可以得到更准确的二维码激光关键帧的坐标。由于进行了激光的优化，即使二维码计算出的坐标较差也能确保具有较高的精度。同时，代价函数还保证了即使点云无法和地图匹配，还有二维码坐标保底，提高了程序的鲁棒性。

基于上述任意一个实施例得二维码和激光的融合定位结果，可以将该定位结果与其他回环检测得结果一起，经过非线性优化得到AGV里程坐标系相对于全局坐标系得坐标，所以，即使在环境变化较大时，没有有效的回环约束，也能得到较为精确的位姿估计。

一种实施例中，通过回环检测和二维码激光关键帧的坐标结算，可以得到一些约束，通过这些约束，可以优化得到里程计坐标系到全局坐标系的变换关系，这一步其实是优化每一个激光关键帧在全局坐标系中的坐标，包括：

，

其中，表示所有激光关键帧的坐标，包括二维码激光关键帧。/>表示代价函数，/>。第一个残差块为里程计约束，表示相邻两个激光关键帧之间的欧式变换，p和q均表示激光关键帧的索引，x_p和x_q表示两个相邻的激光关键帧，/>表示里程计约束。第二个残差块为观测约束，包括回环检测的约束和二维码激光关键帧的约束，/>表示激光关键帧到全局坐标系的欧式变换，m表示全局坐标系的原点，/>表示观测的约束，也就是回环检测的结果或二维码激光关键帧解算的结果。Ω_pq代表里程计的协方差，Ω_p表示回环检测的协方差，取决于激光和二维码传感器精度。通过优化，可以得到每一个激光关键帧在全局地图中的坐标估计。取最后一帧激光关键帧作为全局地图到局部地图的坐标变换。

本申请的一种实施例中提供了一种计算机可读存储介质，存储介质上存储有程序，存储的程序包括能够被处理器加载并处理上述任意一实施例中的二维码标定方法和/或融合定位方法。

本领域技术人员可以理解，上述实施方式中各种方法的全部或部分功能可以通过硬件的方式实现，也可以通过计算机程序的方式实现。当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器、随机存储器、磁盘、光盘、硬盘等，通过计算机执行该程序以实现上述功能。例如，将程序存储在设备的存储器中，当通过处理器执行存储器中程序，即可实现上述全部或部分功能。另外，当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序也可以存储在服务器、另一计算机、磁盘、光盘、闪存盘或移动硬盘等存储介质中，通过下载或复制保存到本地设备的存储器中，或对本地设备的系统进行版本更新，当通过处理器执行存储器中的程序时，即可实现上述实施方式中全部或部分功能。

以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。

Claims (8)

1.一种融合定位方法，其特征在于，该定位方法基于在全局环境地图对应的路径上布置好的二维码实现，应用于对机器人小车定位装置；定位方法包括：

在机器人的行进路线上扫描任意一张二维码，基于该任意一张二维码的标定结果，得到任意时刻机器人的定位坐标作为第一定位坐标；

在行进路线上扫描任意一张二维码时，将获得的所有激光帧作为激光关键帧，并标记为二维码激光关键帧，对于任意一个二维码激光关键帧，选取该二维码激光关键帧所对应的多个二维码数据帧，对该多个二维码数据帧所一一对应的多个第一定位坐标及机器人的速度进行非线性优化，得到优化后的多个第一定位坐标的集合及优化后的机器人的速度；

对于任意一个二维码激光关键帧，基于其所对应的第一个时刻的二维码数据帧所对应的优化后的第一个时刻的第一定位坐标和机器人的速度，采用时间速度和时间差计算出坐标作为该任意一个二维码激光关键帧的坐标；

基于该任意一个二维码激光关键帧的坐标得到对应的机器人的坐标；

所述标定结果基于融合定位用二维码标定方法获取，所述标定方法用于对全局环境地图对应的路径上布置好的二维码进行标定，所述的布置好的二维码为成对布置，标定方法包括：

扫描每一张二维码，对于任意一张扫描到的二维码，获取二维码数据帧，同时获取激光数据帧作为激光关键帧；

对于任意一个二维码数据帧，获取该二维码数据帧相对于机器人的坐标作为第一坐标，对于任意一个激光关键帧，获取机器人的坐标作为第二坐标；

对于任意一个二维码对，获取二维码对的相对坐标；

基于所有二维码对的相对坐标及所有二维码的第一坐标和对应的第二坐标，对任意一张二维码的坐标进行非线性优化，得到所有二维码的坐标；

其中，任意一个成对布置的二维码采用融合定位用二维码对，该二维码对用于对机器人进行融合定位，所述二维码对包括第一二维码和第二二维码，所述第一二维码和第二二维码的id和为一个预设的常数，且第一二维码的id和第二二维码的id不同；所述第一二维码和第二二维码沿机器人行进路径的中轴线左右交错布置，且第一二维码和第二二维码在与中轴线垂直方向上的相互远离的侧边的距离小于或等于二维码读码器的扫描范围；所述第一二维码和第二二维码沿中轴线方向上的互相靠近的侧边的距离大于预设的第一距离且小于预设的第二距离。

2.如权利要求1所述的融合定位方法，其特征在于，所述的所述第一二维码和第二二维码沿机器人行进路径的中轴线左右交错布置，包括：

所述第一二维码和第二二维码等距分布于所述中轴线的两侧，且第一二维码和第二二维码中心连线与所述中轴线的夹角为30度。

3.如权利要求1所述的融合定位方法，其特征在于，所述第一二维码和第二二维码沿中轴线方向上的互相靠近的侧边的距离大于读码器的扫描范围。

4.如权利要求1或3所述的融合定位方法，其特征在于，所述第一二维码和第二二维码沿中轴线方向上的互相靠近的侧边的距离为15cm。

5.如权利要求1所述的融合定位方法，其特征在于，所述的基于所有二维码对的相对坐标及所有二维码的第一坐标和对应的第二坐标，对任意一张二维码的坐标进行非线性优化，得到所有二维码的坐标，包括：

，

其中，表示二维码j所对应的在i时刻的第一坐标，/>表示二维码j所对应的在i时刻的第二坐标，i表示时刻的索引，1≤i≤I，I表示时刻的总数，j和k均表示二维码的索引，1≤j,s≤J，J表示二维码的总数，/>表示欧式距离计算；/>表示二维码j的坐标，/>表示二维码k的坐标，且二维码j和二维码s组成二维码对，/>表示二维码j和二维码s的相对坐标，/>和/>随着优化而改变；/>表示待优化的所有二维码的坐标集合。

6.如权利要求1所述的融合定位方法，其特征在于，所述的在机器人的行进路线上扫描任意一张二维码，基于该任意一张二维码的标定结果，得到任意时刻机器人的定位坐标作为第一定位坐标，包括：

，

其中，表示i时刻的第一定位坐标，/>表示二维码j所对应的在i时刻的第一坐标，i表示时刻的索引，1≤i≤I，I表示时刻的总数，j表示二维码的索引，1≤j≤J，J表示二维码的总数；/>表示二维码j的坐标，随着优化而改变。

7.如权利要求1所述的融合定位方法，其特征在于，所述的对于任意一个二维码激光关键帧，选取该二维码激光关键帧所对应的多个二维码数据帧，对该多个二维码数据帧所一一对应的多个第一定位坐标及机器人的速度进行非线性优化，得到优化后的多个第一定位坐标的集合及优化后的机器人的速度，包括：

，

其中，表示该多个二维码数据帧所对应的机器人的速度，/>，/>表示线速度，/>表示角速度；/>表示所述多个第一定位坐标的集合，j表示二维码的索引，1≤j≤J，J表示二维码的总数；f为代价函数，，/>表示欧式距离计算，/>表示二维码j所对应的在i时刻的第二坐标，i表示时刻的索引，1≤i≤I，I表示时刻的总数，/>表示机器人观测的协方差的倒数，/>表示机器人预测的协方差的倒数，基于实际的机器人模型以及二维码传感器的精度确定；/>表示i-1时刻到i时刻机器人的位移，，/>表示i-1时刻到i时刻的时间增量，表示i-1时刻机器人的yaw角，/>表示x方向的位移，/>表示y方向的位移，/>表示机器人的yaw角的增量。

8.如权利要求7所述的融合定位方法，其特征在于，所述的对于任意一个二维码激光关键帧，基于其所对应的第一个时刻的二维码数据帧所对应的优化后的第一个时刻的第一定位坐标和机器人的速度，采用时间速度和时间差计算出坐标作为该任意一个激光关键帧的坐标，包括：

，

其中，表示任意一个二维码激光关键帧的坐标，表示该任意一个二维码激光关键帧所对应的第一个时刻的二维码数据帧所对应的优化后的第一个时刻的第一定位坐标/>，表示该任意一个二维码激光关键帧所对应的第一个时刻的二维码数据帧所对应的机器人的yaw角，/>表示扫描到二维码的时刻到二维码激光关键帧的时刻的时间差。