多机器人三维几何地图的融合方法

1.一种多机器人三维几何地图的融合方法，其特征在于，包括以下步骤：
(1)将待融合的两幅三维几何地图M1(x，y，z)，M2(x，y，z)投影到x-y平面上降维得到二维地图，然后以固定的尺寸大小对二维地图进行栅格化得到待融合的两幅栅格地图，其中，有特征存在的栅格为占据栅格，无特征存在的栅格则为空闲栅格，两种不同的栅格赋予不同的数值；
(2)对待融合的两幅栅格地图进行傅里叶-梅林变换，计算两幅栅格地图之间的相对旋转角θ；
(3)利用步骤(2)得到的相对旋转角θ，对三维几何地图M2(x，y，z)进行旋转变换，得到变换后的三维几何地图M′2(x，y，z)；
(4)采用最邻近迭代算法，对M1(x，y，z)和M′2(x，y，z)进行融合。
2.如权利要求1所述的融合方法，其特征在于，所述的步骤(4)中，对M1(x，y，z)和M′2(x，y，z)进行融合的过程如下：
(4.1)计算M′2(x，y，z)中每一点到M1(x，y，z)的最近点，利用四元数算法，求解使得M′2(x，y，z)中的每一点和M1(x，y，z)中对应的最近点之间的欧拉距离平方和最小的变换矩阵K；
(4.2)用所述的变换矩阵矩阵K对M′2(x，y，z)进行变换，得到更新的M′2(x，y，z)，并计算M1(x，y，z)和更新的M′2(x，y，z)之间的距离，为一次迭代的结果；
(4.3)重复步骤(4.1)和(4.2)，计算两次迭代的误差，如果误差大于ε，则继续重复上述的步骤；如果误差小于ε，则迭代结束；所述的ε根据融合精度需要取值。

多机器人三维几何地图的融合方法\n技术领域\n[0001] 本发明涉及机器人同时定位与地图重建领域，具体来说是一种对多机器人所创建的几何地图进行融合的方法。\n背景技术\n[0002] 随着应用领域的不断扩大，基于单机器人系统的同时定位与地图重建算法在很多场合下已经不能满足需求；而多机器人系统由于具有并行作业、功能互补、信息共享等优点，受到了人们越来越多的关注。在移动机器人同时定位与地图重建问题中，地图是根据机器人的当前位姿信息创建的，因此，如果能够知道每个机器人的初始位姿信息及其对应的运动方程，那么就可以通过多机器人联合滤波的方式来实现地图的同时创建。然而，在很多场合下，这个条件是不能得到满足的，每个机器人都必须独立的完成局部地图的创建工作，因而就需要面临以下的一个问题：当每个机器人都维护了一张关于局部环境的地图时，该如何利用这些局部地图以得到环境的全局信息？这就是多机器人的地图融合问题(如图1所示)。\n[0003] 针对机器人相互位置关系未知情况下的地图融合问题，一种方法是通过机器人之间的相互测量来估计地图间的变换关系，然后对地图中的特征进行关联，最终实现地图的融合；或者是机器人通过相互交换测量信息，然后采用一种自适应粒子滤波算法来估计机器人之间的相对位姿信息，并由此来决定多机器人协作探索环境的策略。针对多机器人栅格地图的融合问题，可以把它看作是求解最优变换的过程，或者是通过图像配准的方式来解决。然而，目前的方法大都针对的是二维地图的融合问题。对三维几何地图的融合问题，通常直接采用三维点集配准算法来解决，但是三维点集配准算法没有考虑到地图能够反映出环境中的几何特征这一情况。\n发明内容\n[0004] 本发明提供了一种多机器人三维几何地图的融合方法，通过考虑三维地图中的几何特征信息，结合二维地图融合和三维点集配准算法，完成多机器人三维几何地图的融合，明显了改进三维几何地图融合的准确性。\n[0005] 一种多机器人三维几何地图的融合方法，包括以下步骤：\n[0006] (1)将待融合的两幅三维几何地图M1(x，y，z)和M2(x，y，z)投影到x-y平面上降维得到二维地图，然后以固定的尺寸大小对二维地图进行栅格化得到待融合的两幅栅格地图，其中，有特征存在的栅格为占据栅格，无特征存在的栅格则为空闲栅格，两种不同的栅格赋予不同的数值；\n[0007] (2)对待融合的两幅栅格地图进行傅里叶-梅林变换，计算两者之间的相对旋转角θ；\n[0008] (3)利用步骤(2)得到的相对旋转角θ，对三维几何地图M2(x，y，z)进行旋转变换，得到旋转变换后的三维几何地图M′2(x，y，z)；\n[0009] (4)采用最邻近迭代算法，对M1(x，y，z)和M′2(x，y，z)进行融合。\n[0010] 进一步，所述的步骤(4)为：\n[0011] (4.1)计算M′2(x，y，z)中每一点到M1(x，y，z)的最近点，利用四元数算法，求解使得M′2(x，y，z)中的每一点和M1(x，y，z)中对应的最近点之间的欧拉距离平方和最小的变换矩阵K；\n[0012] (4.2)用所述的变换矩阵矩阵K对M′2(x，y，z)进行变换，得到更新的M′2(x，y，z)，并计算M1(x，y，z)和更新的M′2(x，y，z)之间的距离，为一次迭代的结果；\n[0013] (4.3)重复步骤(4.1)和(4.2)进行迭代，计算两次迭代的误差，如果误差大于ε，继续迭代过程；如果误差小于ε，迭代结束。ε根据融合精度需要取值。\n[0014] 本发明多机器人三维几何地图的融合方法，通过考虑三维地图中的几何特征信息，结合二维地图融合和三维点集配准算法，完成多机器人三维几何地图的融合，明显了改进三维几何地图融合的准确性。本发明多机器人三维几何地图的融合方法主要用于在多机器人同时定位与地图重建应用中，对单个机器人所创建的局部地图进行融合以形成全局地图。\n附图说明\n[0015] 图1为多机器人三维几何地图融合问题的示意图；\n[0016] 图2为本发明的多机器人三维几何地图的融合方法的流程图；\n[0017] 图3为作为测试输入的两幅待融合的三维几何地图；\n[0018] 图4为图3的测试输入未融合前在二维平面上的投影；\n[0019] 图5为对图3所示的测试输入采用本发明方法融合后的测试结果在二维平面上的投影；\n[0020] 图6为对图3所示的测试输入采用现有技术中的方法融合后的测试结果在二维平面上的投影。\n具体实施方式\n[0021] 下面结合实施例和附图来详细说明本发明，但本发明并不仅限于此。\n[0022] 如图2所示，一种多机器人三维几何地图的融合方法，对多个机器人用视觉传感器感知环境，并各自创建关于环境的局部三维几何地图进行融合，其流程包括以下步骤：\n[0023] 考虑到机器人是在x-y平面上运动，定义降维映射(即投影模型)[0024] f：(x，y，z)→(x，y)\n[0025] 其中(x，y，z)为几何特征的空间坐标，(x，y)为几何特征的平面坐标。\n[0026] 将待融合的两幅三维几何地图M1(x，y，z)和M2(x，y，z)投影到x-y平面上降维，然后以固定的尺寸大小对投影到x-y平面上的地图进行栅格化得到待融合的两幅栅格地图m1(x，y)和m2(x，y)。栅格地图由若干个相同尺寸大小的栅格单元组成，一个栅格单元对应x-y平面上一个子区域，子区域的大小可以根据实际情况进行调整。栅格地图中，有特征存在的栅格为占据栅格，无特征存在的栅格则为空闲栅格，两种不同的栅格赋予不同的数值。\n[0027] 设待融合的两幅栅格地图m1(x，y)和m2(x，y)中，m2(x，y)是通过对m1(x，y)进行平移、旋转和缩放变换后得到的，则m1(x，y)和m2(x，y)存在以下如式(I)所示的变换关系：\n[0028] m2(x，y)＝m1(s(xcosα+ysinα)-x0，s(-xsinα+ycosσ)-y0) (I)[0029] 式(I)中，s为缩放因子，α为旋转角度，x0和y0为平移量；\n[0030] 则m1(x，y)和m2(x，y)对应的傅里叶变换幅值F1(ξ，ζ)和F2(ξ，ζ)之间满足式(II)，如下所示：\n[0031] \n[0032] 从式(II)可以看出，待融合的两幅栅格地图的傅里叶变换幅值F1(ξ，ζ)和F2(ξ，ζ)之间的关系只和缩放因子s以及旋转角度α有关，而和平移量x0和y0无关。\n[0033] 因此，栅格地图m1(x，y)和m2(x，y)融合分两步进行：先通过幅度谱求缩放因子s及旋转角α，然后再求解平移参数x0和y0。\n[0034] 对式(II)引入极坐标来表示(ξ，ζ)，可以得到式(III)，如下所示：\n[0035] \n[0036] 式(III)中，θ为相对旋转角，ρ为极半径；\n[0037] 进一步采用对数坐标的表示形式，并忽略系数s-2，可以得到式(IV)，如下所示：\n[0038] |F2(logρ，θ)|＝|F1(logρ-logs，θ-α)| (IV)\n[0039] 式(IV)可以应用傅里叶位移理论加以解决：\n[0040] 设待融合的两幅栅格地图m1(x，y)和m2(x，y)中存在的两幅图像I1(x，y)和I2(x，y)中，图像I2(x，y)由图像I1(x，y)在x和y方向上分别平移x0和y0得到，则I1(x，y)和I2(x，y)存在如式(V)所示的变换关系：\n[0041] I2(x，y)＝I1(x-x0，y-y0) (V)\n[0042] 并设F1(ξ，ζ)和F2(ξ，ζ)分别为I1(x，y)和I2(x，y)对应的傅立叶变换，则F1(ξ，ζ)和F2(ξ，ζ)之间满足如式(VI)所示的关系：\n[0043] \n[0044] 定义图像I1(x，y)和I2(x，y)的互功率谱为\n[0045] \n[0046] 其中 为F2的复共轭。对式(VII)进行傅里叶反变换，在图像空间上的(x0，y0)处得到一个脉冲函数，脉冲的位置即为两幅图像I1(x，y)和I2(x，y)之间的相对平移量x0和y0。\n[0047] 在得到缩放因子s及其旋转角α之后，对地图m2(x，y)进行缩放和旋转，生成变换后的地图m′2(x，y)，则m′2(x，y)和m1(x，y)只在x和y方向上存在平移关系[0048] m′2(x，y)＝m1(x-x0，y-y0) (VIII)[0049] 对式(VIII)再次运用傅里叶位移理论即可得到两幅地图m′2(x，y)和m1(x，y)之间的相对平移量x0和y0。\n[0050] 由于三维几何地图M1(x，y，z)和M2(x，y，z)在平面上的投影m1(x，y)和m2(x，y)之间的相对旋转角θ可通过栅格地图的融合求解，因此，依据栅格地图的融合求解得到的相对旋转角θ，可以得到三维几何地图M1(x，y，z)和M2(x，y，z)的相对旋转角θ，并以此对三维几何地图M2(x，y，z)进行旋转后得到M′2(x，y，z)，然后采用最邻近迭代算法进行地图的融合。\n[0051] 最邻近迭代算法的每一次迭代过程都分两步进行。第一步：先求得M′2(x，y，z)中每一点到M1(x，y，z)的最近点，然后利用四元数算法求解使得M′2(x，y，z)中的每一点和M1(x，y，z)中对应的最近点之间的欧拉距离平方和最小的变换矩阵K；第二步；用变换矩阵K对M′2(x，y，z)进行变换得到更新的M′2(x，y，z)，然后计算M1(x，y，z)和M′2(x，y，z)之间的距离。重复上述的迭代过程，计算两次迭代的误差，如果误差大于ε，继续迭代过程；如果误差小于ε，迭代结束。ε根据融合精度需要取值。\n[0052] 使用Hausdroff距离来衡量M1(x，y，z)和M′2(x，y，z)之间的距离。定义三维几何地图M1(x，y，z)和M2(x，y，z)之间的Hausdroff距离D(M1，M2)为：\n[0053] D(M1，M2)＝max(d(M1，M2)，d(M2，M1))\n[0054] 其中，\n[0055] d(M1，M2)＝||p-q||，p＝max(M1)，q＝min(M2)\n[0056] d(M2，M1)＝||p-q||，p＝max(M2)，q＝min(M1)\n[0057] 以如图3所示的两幅三维几何地图作为测试输入，其未融合之前在二维平面上的投影如图4所示，采用本发明的方法融合后在二维平面上的投影如图5所示，而采用现有技术中的方法融合后在二维平面上的投影如图6所示。根据图4～6的比较，可以得出：使用本发明的方法可以得到更客观的融合效果。