本发明涉及一种用于确定运动物体相对于环境的相对位置的方法和系统。特别地，本发明涉及一种用于确定无人机(航空的，例如是多旋翼或直升机类型的)或表面检查机器人在已知环境中的位置的方法和系统，该已知环境例如是包含其表面要被检查的物体的飞机库。
背景技术：
运动物体，诸如在空间中自主运动的无人机或机器人，需要不断地知道其中它们四处运动的环境中的位置。
如果它们在开放环境(室外)中运动，则运动物体通常是使用地理位置而定位的，例如通过全球定位系统(gps)，这使得可以实现高精度。
然而，全球定位系统的主要缺点是它们不能在室内工作。因此，已经寻求解决方案以允许在室内对运动物体进行定位。
例如，提出的一种解决方案是从运动物体的已知位置开始并通过测量物体从该已知位置做出的运动来确定物体的位置。然而，该解决方案不能检查随着时间运动中的错误或任何精度不足，而这些随着时间累积并因此造成不能校正的不正确的位置。更进一步地，该解决方案使用惯性参考系统，尽管其具有良好的精度水平使得可以最小化这些误差，但惯性参考系统是昂贵且笨重(常常超过1kg)，这与在空中无人机中的使用不兼容。
替代地，已经提出使用预先放置在环境中的信标系统，信标是诸如无线电收发器。通过为运动物体配备收发器，因此可以通过涉及相对于其它信标的三角测量的技术来估计其位置。然而，这种解决方案是昂贵的并且需要花费长时间来实现，因为需要布置信标并且需要确定它们的精确位置(通过校准)。
已经寻求一种解决方案以允许克服这些缺点中的至少一些的室内定位。
技术实现要素：
本发明的目的是克服已知定位方法和系统的至少一些缺点。
特别地，本发明的目的是在本发明的至少一个实施例中提供一种定位方法，该定位方法使得可以定位室内环境中的运动物体。
在至少一个实施例中，本发明的目的还在于提供一种可以在低功率处理器上执行的快速定位方法。
在至少一个实施例中，本发明的目的还在于提供一种精确定位方法。
在至少一个实施例中，本发明的目的还在于提供一种定位方法，该定位方法不需要对其中物体运动的环境进行任何修改。
在至少一个实施例中，本发明的目的还在于提供一种可以装载在运动物体上的定位系统。
为此，本发明涉及一种用于确定运动物体相对于由一组几何表面建模的环境的相对位置的方法，所述一组几何表面根据环境中的参照系形成3d模型，该方法包括：
-在至少一个方向上测量从物体到环境的多个距离，以便获得运动物体的参照系中的一组限定的点的步骤，
-评估所述一组点的与环境的3d模型之间的差异的步骤，
-从所述差异确定运动物体在环境的参照系中的相对位置的步骤。
因此，根据本发明的方法允许在完全或部分已知的环境中对运动物体进行定位，而不需要定位信标。“环境”应被理解为意指运动物体在其中运动的体积连同构成所述体积的元素，例如包含其表面需要检查其的飞机的机库。当对具有大尺寸的元素执行表面检查时，所检查的元素是已知的并且可以通过计算机辅助设计(cad)软件进行3d建模或者已经被建模以用于其它应用。
该方法始终使用物体距环境的距离的测量，并使得可以确定其相对于环境的相对位置。例如，为了检查目的在飞行器周围飞行的无人机将始终知道它的位置，并且如果飞行器表面上存在缺陷，则能够确定该缺陷相对于其自身位置的位置，并且因此将其定位在飞行器上。
因此，就用于执行的资源而言，实施的方法是快速且具有成本效益的；它可以容易地集成在运动物体上，并且使用消耗很少能量且允许实时处理的机载处理器。该方法的一部分，特别是仅需要执行一次并且与运动物体的位置或测量的距离无关的步骤(例如，与3d建模关联的预处理步骤)可以在运动物体外部执行，这些步骤的结果被提供给机载处理器，以便提高由其处理的步骤的执行速度。
该方法的执行的速度还允许它被更频繁地执行，并且从而确保快速地追踪运动物体的位置，并且因此所述物体可以更快地运动。
虽然由于缺乏地理位置该方法在室内特别地有利，但是它也可以在室外使用，用于在已知环境中需要改善的精度的应用，例如对室外飞行器、风力涡轮机的表面或干船坞中的船舶的检查。
有利地并且根据本发明，3d模型是3d多边形网格，并且几何表面是多边形。
使用3d多边形网格对3d环境进行建模允许简单建模并且更快速地处理该方法的不同步骤。另外，任何涉及更复杂的几何表面的3d模型可以根据本领域技术人员公知的技术通过3d多边形网格来近似。
有利地并且根据本发明，测量多个距离的步骤由至少一个激光扫描仪(也称为激光测距仪)执行。
有利地并且根据本发明的该前一方面，测量多个距离的步骤由被配置为在正割平面中进行扫描的至少两个激光扫描仪执行。
优选地，每个附加的激光扫描仪被配置为在与其它激光扫描仪的扫描平面正割的平面中执行扫描。
有利地并且根据本发明，评估所述一组点与环境的3d模型之间的差异的步骤包括：
-从位置的估计并且从运动物体的姿态，将运动物体的参考系中的所述一组点转换为环境的参照系中的点云的步骤，
-针对所述点云中的每个点，计算所述点与环境的3d模型的表面之间的范数(norm)的步骤。
运动物体的姿态对应于物体在空间中的定向，由欧拉角θ，φ和ψ表示，这是本领域技术人员已知的。
有利地，在针对所述点云中的每个点，计算所述点与环境的3d模型的表面之间的范数的步骤之前，根据本发明的方法包括：
-将环境分解为叠加的立方体，以使得环境中的每个点包含在多个立方体内的步骤，
-针对每个立方体，确定与立方体具有非零交叉的环境的3d模型的表面的步骤，
并且针对所述点云中的每个点计算所述点与环境的3d模型的表面之间的范数的步骤包括：
-选择各立方体中被称为居中的立方体的一个立方体的步骤，点云中的该点位于该居中的立方体中，并且在居中的立方体中点云中的该点距离居中的立方体的每个面最远，
-取回与居中的立方体具有交叉的被称为近表面的表面的列表的步骤，
-计算该点与每个近表面之间的范数的步骤，
-从近表面中确定与该点最接近的表面的步骤，所述最近的表面是所述表面与该点之间的其范数最低的表面，所述范数被认为是该点与环境的3d模型之间的范数。
有利地，将环境分解成叠加的立方体的步骤以及针对每个立方体确定与立方体具有非零交叉的环境的3d模型的表面的步骤可以被预先执行，并且这些步骤的结果可以保存在运动物体的存储器中，以允许更快速的处理。实际上，这些步骤并未与物体的位置或与所取得的距离测量关联。
根据本发明的这个方面，将环境分解成立方体并计算每个点与近表面之间的范数允许在资源方面快速且有成本效益地计算每个点与环境的3d模型之间的范数，因为仅执行了与近表面的范数的计算，而不是与所有表面的范数的计算。
点云距3d模型的距离是从点云中的每个点到3d模型的所有范数，特别是到最接近该点的3d模型的表面的所有范数。
有利地并且根据本发明，点的范数大于预定的阈值的被称为孤立点的点是从点云中提取的，并且该方法包括检查障碍物的步骤，其中近孤立点被组合在一起以形成代表障碍物的体积，并记录所述体积。
根据本发明的该方面，该方法使得可以隔离对应于未包括在环境的3d模型中的障碍物的点，处理所述点并且在定位运动物体时将它们考虑在内，以便防止运动物体与所述障碍物接触。表示障碍物的体积被记录，并且可以在导航中被考虑并且可以在运动物体四处运动时被追踪。
有利地，根据本发明的方法包括从点云中移除对应于环境的地面或顶部的点的步骤。
根据本发明的这个方面，该方法使得可以通过从点云中移除对应于环境的地面或顶部的点来限制要处理的点的数量，这些对应于环境的地面或顶部的点在运动物体的高度或该高度的估计可以获得时是几乎没有用的；运动物体知道地面和顶部对应于特定的恒定高度，并且不需要知道形成地面或顶部的点。高度是通过直接测量或者通过估计而可知的。
因此，该方法更快，因为已经减少了要处理的点的数量。
有利地，根据本发明的方法包括从点云中移除模糊或冗余点的步骤。
根据本发明的该方面，该方法使得可以通过从点云中移除对应于模糊或冗余的距离测量的点来限制要处理的点的数量。模糊点是对应于表面边缘上的点测量或具有可能不正确的低入射角的表面上的测量的点。冗余点是位于如下表面上的点：该表面的点云已经包括足以限定所述表面的点。
因此，该方法更快，因为已经减少了要处理的点的数量。
本发明还涉及一种用于在环境中导航运动物体的方法，其包括将移动运动物体的至少一个步骤，其特征在于，在移动步骤期间，通过根据本发明的确定方法来确定运动物体的位置。
本发明还涉及一种用于在环境中导航运动物体的方法，其包括移动运动物体的至少一个步骤，其特征在于，运动物体的位置和对应于障碍物的体积的位置是由根据本发明的确定方法而确定的，并且该方法包括躲避对应于障碍物的所述体积的至少一个步骤。
本发明还涉及一种用于确定运动物体相对于由根据环境中的参照系的3d模型建模的环境的相对位置的系统，该系统装载在所述运动物体上并且其特征在于该系统包括：
-测量部件，用于在至少一个方向上测量从物体到环境的多个距离，以便获得运动物体的参照系中的一组限定的点，
-评估模块，用于评估所述一组点与环境的3d模型之间的差异，
-确定模块，用于从所述差异确定运动物体在环境参照系中的相对位置。
测量部件例如是模块上装载的传感器，诸如至少一个激光扫描仪，优选地被配置为在正割平面中进行扫描的两个激光扫描仪，深度测量相机(红绿蓝深度(rgbd)类型的)，立体视觉装置，毫米波雷达等，或所述传感器的组合。
用于评估距离的模块和用于确定运动物体的相对位置的模块装载在运动物体上。然而，未与测量的距离或与运动物体的位置关联的一些计算可以在分离的计算机上执行，并且可以预先提供给确定系统，以便改善其包括的不同部件的处理速度。
本发明还涉及一种计算机程序产品，其可以从通信网络下载和/或存储在计算机可读和/或处理器可执行的介质上，其特征在于，它包括用于实施根据本发明的确定方法的程序代码指令。
这种类型的计算机程序产品使得可以确定其中快速执行该计算机程序产品的运动物体的位置，其需要很少的处理资源并因此具有低能耗。
本发明还涉及一种计算机可读存储部件，其完全或部分地可移动并存储计算机程序，该计算机程序包括用于实施根据本发明的确定方法的一组计算机可执行指令。
有利地，根据本发明的确定系统实现根据本发明的确定方法。
有利地，根据本发明的确定方法是由根据本发明的系统实施的。
本发明还涉及一种计算机程序产品，其可以从通信网络下载和/或存储在计算机可读和/或处理器可执行介质上，其特征在于，它包括用于实施根据本发明的确定方法的程序代码指令。
本发明还涉及一种计算机可读存储部件，其完全或部分可移动并存储计算机程序，该计算机程序包括用于实施根据本发明的确定方法的一组计算机可执行指令。
本发明还涉及一种确定方法，一种导航方法，一种系统，一种计算机程序产品以及一种存储部件，其特征在于，由前面或下文中描述的一些或所有特征组合。
【附图说明】
从阅读参照随附的附图通过非限制性示例的方式给出的随后的描述，本发明的其它目的，特征和优点将变得明显，其中：
-图1是根据本发明实施例的方法的示意图；
-图2是环境的3d网格的多边形的图示。
【具体实施方式】
以下实施例是示例。虽然描述涉及一个或多个实施例，但是这并不一定意味着每个参考涉及相同的实施例或者特征仅适用于单个实施例。还可以组合不同实施例的单个特征以提供其它实施例。在附图中，为了说明和清楚的目的，没有严格遵守缩放和比例。
图1示意性示出了根据本发明的实施例的用于确定运动物体相对于由一组几何表面建模的环境的位置的方法，所述一组几何表面根据环境中的参照系形成3d模型。根据本发明的实施例，该确定方法的全部或部分是由装载在运动物体上的确定系统实施的。该方法有利地描述了用于在三维空间中移动的运动物体并且是诸如无人机的飞行物体，例如直升机或多旋翼类型的无人机。
在该实施例中，3d模型是3d多边形网格，即形成3d模型的几何表面是多边形。然而，本发明还适用于任何类型的3d模型，特别是使用更复杂的几何表面(特别是球形，圆柱形，椭圆形和锥形)的3d模型。
该确定方法包括第一步骤10，该步骤10在至少一个方向上测量从运动物体到环境的多个距离，以便使用测量部件来获得运动物体的参照系中一组限定的点，测量部件用于在至少一个方向上测量从物体到环境的多个距离。
所述测量部件例如是传感器，诸如至少一个激光扫描仪，优选地在正割平面中进行扫描的两个激光扫描仪、深度测量相机(红绿蓝深度(rgbd)类型的)，立体视觉装置，毫米波雷达等，或所述传感器的组合。
根据运动物体的参照系并且相对于运动物体上的已知点(典型地测量部件的位置，其相对于运动物体的重心是已知的)，距离测量是以成对的形式的(立体角；在该立体角中测量的距离)。
激光扫描仪可以例如以低角度间距(例如0.25°)并且在宽角度范围(270°或更大)上执行距离测量，这使得可以获得大量的对(立体角；距离)。使用具有不同定向和/或反射镜的多个激光扫描仪使得可以在不同平面中获得测量结果。
根据其它实施例，诸如rgbd相机或立体视觉相机之类的采集部件，或雷达或声纳类型的采集部件使得可以在不需要扫描的情况下获得若干个维度上的距离测量。
对(立体角；距离)形成在运动物体的参考系中的一组点；该对对应于运动物体的参照系中的所述一组点中的点的球面坐标。
该方法然后包括可选步骤12，其借助于用于从点云中移除对应于环境的地面或顶部的点的移除模块，从点云中移除对应于环境的地面或顶部的点。
从所有测量的距离中提取对应于地面或顶部的测量距离，使得可以减少在确定方法的其余部分中考虑的距离的数量。
该提取需要知道物体的姿态，并且在一个实施例中，需要知道物体的高度，该高度是由特定传感器测量的，或者需要估计运动物体的高度，该高度是考虑到从该最后位置的近似运动而从运动物体的先前位置得到的。
从姿态，在第一种情况下，可以确定与运动物体的参照系中的对(立体角；距离)关联的每个点的高度，即，获得沿着垂直轴oz的坐标，垂直轴oz指向上方并且源于运动物体。因此，正z坐标处的点位于物体上方，并且负z坐标处的点位于物体下方。
然后针对每个待溢出的表面(典型地是地面和顶部)，构造了其具有中心zc和幅度2dz的间隔[zc-dz，zc+dz]。值dz可以是恒定的，例如10cm，或者可以是垂直速度vz的函数，例如dz＝dzmin+k*|vz|，其中|vz|指示vz的绝对值，并且dzmin和k是系数。
根据第一实施例，使用物体的高度来计算zc：
-针对地面：zc＝-ze，其中ze是物体相对于地面的高度
-针对顶部：zc＝h-ze，其中h是顶部的高度
根据第二实施例，直接从点的坐标z来计算oz：
-针对地面：zc＝(所有z坐标的最小值)+dz
-针对顶部：zc＝(所有z坐标的最大值)-dz
第一实施例具有更快速的优点，因为立即知道间隔而不需要在所有点处计算最小值或最大值，并且具有即使在地面或顶部中存在“孔”时也可行的优点。
第二实施例具有不使用物体的高度的优点，并且即使高度的估计不再起作用也可以特别可行的。
第三个优选实施例是当可获得高度的良好估计时使用第一实施例，而当不可获得高度的良好估计时使用第二实施例。
最后，于是提取其垂直坐标包括在该间隔中的所有点。
所述提取的点还可以用于通过将ransac类型算法应用于所述点的垂直坐标来执行高度的新估计，在该算法中，计算所有垂直坐标的平均值，其垂直坐标距离平均值最远的点的预定百分比(例如10％)被移除，并从剩余点重新计算平均值；所述平均值是针对对应于地面的点的地面高度的估计以及针对对应于顶部的点的顶部高度的估计。
该方法然后包括使用移除部件从点云中移除模糊或冗余点的可选步骤14，移除部件用于从点云中移除模糊或冗余点。
该步骤具有若干个目的；第一个目的是防止过于相似的测量距离被复制，例如代表同一平面的一组测量距离。不必为了限定平面表面而保持数百个点。
第二个目的是减少不可靠的测量距离的数量，特别是当距离测量部件是通过扫描进行测量的激光扫描仪时。例如，针对近立体角，当环境包括遥远的表面时，测量距离可能显着地变化；所述长距离之间的中间点可能是不正确的。更进一步地，在扫描仪的激光器与其具有低入射角(接近0°)的表面上的测量可能是不正确的。
第三个目的是确保在所有方向上存在最小数量的点，以便获得一致的结果。
为了实现这些目的，移除距离的步骤包括：
-赋予非模糊度分数s1＝f1(到最近边缘的距离/测量距离)的步骤，f1例如是返回0(非常接近边缘)与1(远离任何边缘)之间的数字的高斯函数，
-赋予非模糊度分数s2＝f2(入射角)的步骤，f2例如是返回0(0°的掠射角)与1(90°的正交角度)之间的数字的高斯函数，
-组合分数以形成全局分数s＝s1xs2的步骤，s介于0与1之间，
-通过球体的角扇区来分解测量结果的步骤，例如(360/30)2＝30°×30°的144个角扇区，
-针对每个角度扇区，并且当扇区的测量结果的全局分数高于预定阈值(例如0.5)时，选择具有最佳全局分数的测量结果并且移除最接近于该点的p个近点(例如p＝20)的步骤。优选地，非常接近于选择的测量结果的点可以用于在被移除之前合并所述选择的测量。合并例如是3个点上的平均值或中值。
因此获得了距离测量结果的减小的列表，这使得可以加速后续步骤的处理，同时去除可能不正确的数据并在每个角扇区中保持足够数量的点。
该方法然后包括评估所述一组点与环境的3d网格之间的差异的步骤，以及从所述差异确定运动物体在环境的参照系中的相对位置的步骤20。这些步骤可以在循环中执行若干次，以便允许改进运动物体的位置。
评估所述一组点与环境的3d网格之间的差异的步骤包括步骤16和步骤18，步骤16从运动物体的位置的估计将运动物体的参照系中的所述一组点转换为环境的参照系中的点云，步骤18针对所述点云中的每个点计算所述点与环境的3d网格的多边形之间的范数。
通过基于物体的位置和姿态的估计来确定运动物体的参照系的基础矩阵相对于环境的参照系的变化而执行转换。环境的参照系优选地是oxyz型正交参照系。
根据实施例，可以采用若干个不同的方式来估计物体的位置。
物体的姿态一般是使用惯性系统(诸如低成本惯性测量系统)而知道的。
如果该方法已经实施，则由该方法提供的最后的位置可以用作位置估计。如果运动物体近似地知道物体的运动的速度和方向，例如借助于加速度计，则可能可以改进该最后的位置。
如果尚未实施该方法(典型地在启动运动物体时以及在首次发起该方法时)，将使用以下方法来确定初始位置(x，y，z，psi)：
在建立了运动物体的初始位置和定向(运动物体是以精确的、已知的位置和定向而布置的)的情况下，提供该位置和定向作为物体位置的估计并且确定方法的后续步骤被执行固定的次数(例如5次)，并且然后验证该位置的一致性；如果精确指示符(代表每个点与环境的表面的范数的平均值(如下所述))低于阈值(例如平均距离小于20厘米)并且其在所述点与环境之间的范数小于最大距离的点的百分比高于阈值(例如超过75％)，则认为位置是一致的。如果位置是一致的，则将该位置用作位置的估计；否则，使用下面的被称为网格方法的方法。
如果未指定初始位置和定向，则唯一已知的信息是运动物体相对于地面的高也就是其高度(表示为h)。因此，已知无人机在方程z＝h的平面中。该平面(实际上，由于环境具有有限的尺寸，所以该平面是矩形的)以一定的间隔(例如1米)进行网格划分，其中每个点构成对方法的随后步骤中使用的位置的估计。如果可以获得定向的估计，例如在在室外利用磁场，则使用所述估计。否则，针对每个位置，运动物体的定向psi还使用间隔(例如20°)而被离散化。提供每个对(位置，定向)以发起该方法的随后步骤，并且这些步骤被执行固定的次数(例如5次)。一旦对满足上面定义的一致性标准，该对就被用作初始位置的估计。
一些环境允许近似地确定运动物体的初始位置。例如，在环境包含飞机的情况下，可以提取运动物体与飞机机身之间的距离和/或运动物体与飞机机翼之间的距离，这使得可以显著地限制要测试的位置的数量。
如果先前的位置估计可用，则将优先测试近的位置。
另外，当运动物体的位置在运动时丢失时，网格划分方法允许找到有效的位置。为此，由围绕最后知道的位置的球面(所述球面具有例如1米的半径)上的点构成搜索网格。同样地，将优先测试最接近最后知道的位置的点。算法继续，直到找到确认一致性标准的位置。如果在球面上没有找到位置，则将其半径增加(例如，按照1米的间隔)并且算法再次开始。
更进一步地，高度的估计可以由提取对应于环境的地面的测量距离的可选步骤的结果而得出，如果该可选步骤被执行的话。
环境的3d网格例如是以一组多边形的形式的环境的表示，所述一组多边形被组合以形成环境的表面的近似。例如，这些多边形是三角形或四边形。
针对所述点云中的每个点计算所述点与环境的3d网格的多边形之间的范数的步骤18的目的是针对点云中的每个点确定该点与最近的多边形的范数，并且从其中推导出点云与3d网格的范数，其表示将测量的距离转换为在环境的参照系中点云的步骤中使用的位置的误差。
为此，环境的3d网格(由附图标记100表示)由立方体图案预先划分，该立方体图案包括覆盖整个环境的叠加的立方体。所述将环境分解成叠加的立方体以使得环境中的每个点包含在多个立方体内的步骤，在以下步骤之后进行：
-将环境划分为具有相同尺寸且不叠加的相邻立方体的图案，其覆盖整个环境。例如，立方体边长为1米。
-复制立方体图案并沿着环境参照系的ox轴平移立方体的复制品半个长度。因此，获得初始图案中两倍的立方体。
-复制新的立方体图案并沿着环境的参照系的oy轴平移立方体的复制品半个长度。因此，获得前一步骤两倍的立方体，并且是初始图案中的四倍。
-复制新的立方体图案并沿着环境的参照系的oz轴平移立方体的复制器半个长度。因此，获得前一步骤两倍的立方体，并且是初始图案中的八倍。
由于这个立方体图案，环境中未包含在立方体的面中并且不在环境的边缘的任何点都准确地在八个立方体中被找到，而在这八个立方体中总是存在一个立方体(称为居中立方体)，其中该点离所述立方体的每个面最远，即，其位于立方体的每个面的边缘长度的四分之一以上处(对于具有1m边缘的立方体而言，为25cm)。
有利地组织立方体图案中的立方体的列表，以使得从其坐标在环境的参照系中表示的点，简单的数学公式使得可以容易地找到对应于该点的居中的立方体：立方体具有与其在环境中的位置关联的索引。
该方法然后包括，针对每个立方体确定与立方体具有非零交叉的环境的3d网格的多边形的步骤。在此步骤中，针对每个立方体，扫描多边形列表以取回其与立方体体积的交叉是非零的那些多边形，即包含在立方体中或与立方体的至少一个面交叉的多边形。存储该步骤的结果，以便机载系统可以快速地访问。该方法将类似地应用于表面列表，用于限定哪些表面与立方体具有非零交叉。
这些将环境分解为叠加的立方体并且针对每个立方体确定与立方体具有非零交叉的环境的3d网格的多边形的步骤，是与距离测量或与运动物体的位置没有联系的步骤，并且其可以针对环境执行一次。因此，它们可以由外部计算机执行，并且由板上系统仅存储包括经索引的立方体列表以及具有与该立方体交叉的每个经索引的立方体关联的三角形的结果。
该方法包括，在计算步骤18中，针对所述点云中的每个点，计算所述点与环境的3d网格的多边形之间的范数，选择各立方体中的一个立方体(被称为居中的立方体)的步骤，其中点云中的点距离其每个面最远。多亏上述立方体的优选的索引，由于该点的坐标，所以快速地确定了居中的立方体。例如，对于具有坐标(xp，yp，zp)的点p而言，用于确定立方体的索引的计算是：
索引＝k1+k2*floor(xp/l)+k3*floor(yp/l)+k4*floor(zp/l)
其中k1，k2，k3，k4是作为网格的位置和其尺寸的函数的整数，l是立方体的边缘的半个长度，floor()是整数部分数学函数。
该方法然后包括取回与居中的立方体具有交叉的被称为近多边形的多边形的列表的步骤。由于上述存储器将每个立方体与多边形关联(该立方体与该多边形具有非零交叉)，从居中的立方体中快速地取回这些多边形。
如果列表不包含任何多边形，则该点被称为孤立点，并且对于评估所述一组点与环境的3d网格之间的距离的步骤的剩余部分而言，从点的列表中移除该孤立点。孤立点在孤立点的列表中被组合在一起，这允许检查到障碍物。
该方法接着包括计算点与每个近多边形之间的范数的步骤。
例如，如果多边形是三角形，则以下列方式计算每个范数：
即：
-p是要计算其自身与三角形之间的范数的点，
-具有通过以下等式配置的顶点a，b和c的三角形t
t(s,t)＝a+sb+tc其中(s,t)∈d＝{(s,t),t∈[0,1],s+t≤1}
从p到三角形的点t(s,t)处的平方距离是
q(s,t)＝|t(s,t)-p|2
该算法的目的是找到对应于最小化q的点的对(s,t)。用于快速计算三角形上的点的范数的算法定义如下：
1.计算最小化q的梯度的对(s,t)，对(s,t)限定被称为候选点的点。
2.如果候选点属于三角形(因此s和t属于d)，则找到最小化对(s,t)，并终止算法。
3.如果候选点不在三角形中，则在三角形的边缘处实现q的最小值。
在这种情况下，候选点是p在由三角形限定的平面上的正交投影，但是该点不属于由三角形包围的表面。
由三角形限定的平面内的空间因此被划分为多个区域，如参考图2所示，其示出了三角形abc并且是在2d参照系(a,s,t)上。因此，候选点位于区域1至6中的一个区域中，区域1至6分别被编号为r1，r2，r3，r4，r5，r6，区域0(编号为r0)对应于上述点2。
4.根据候选点的s和t值确定对应的区域：
简单的几何考虑直接给出对应于候选点的区域。示例：如果s<＝0且t<＝0，则候选点因此位于区域4中。
5.确定最小化到候选点的距离的三角形的周线上的点，该点被称为最小化点：
-区域1：q水平处的曲线是以候选点为中心的椭圆。因此最小化点或者位于]cb[上(如果梯度在该区域中自身消除的话)，在这种情况下获得最小化对(s,t)，或者位于在c或b处。算法终止。
-区域2：q水平处的曲线也是椭圆，并且因此最小化点位于[ca]上或[cb]上。对c处的q的梯度的几何考虑使得可以确定最小化点是否在[ca]或[cb]上。根据与区域1相同的原理，区段中的梯度的零度或非零度使得可以获得最小化对(s,t)。算法终止。
-区域3和区域5：推理类似于针对区域1的推理。
-区域4和区域6：推理类似于针对于区域2的推理。
最后返回点p与最小化对(s，t)之间的范数，该最小值对表征位于三角形的区域d中的最小化点，该范数对应于点p与三角形之间的范数。
通过预先计算三角形上的一定数量的参数，可以进一步加速计算，并且可以将计算结果与所述三角形的信息一起存储在存储器中。
对于其它多边形形状而言，可以通过适配多边形的特定几何形状以类似的方式做出计算。替代地，每个多边形可以由一组三角形代替。
计算步骤18然后包括确定最接近来自近多边形的点的多边形的步骤，所述最接近的多边形是其在所述多边形与该点之间的范数最低的多边形，所述范数被认为是该点和环境的3d网格之间的范数。
所有点的范数之和除以点的数量(除了孤立点之外)被称为精度指标，使得可以估计位置估计的精度。
该方法还以三维矢量的形式来计算每个点与最接近的多边形之间的矢量误差。
矢量误差包括沿着环境的参照系的三个轴x，y，z的三个分量。
在确定运动物体在环境的参照系中的相对位置的步骤20中，处理每个点之间的所有这些范数(其代表该组点与环境的3d网格之间的距离)，以便最小化该距离。为此，可以使用高斯-牛顿(gauss-newton)梯度下降算法。算法的每次迭代使用位置(沿x，y和z)的pos_init估计来计算位置(沿x，y和z)的pos_calc估计，并包括以下步骤：
-创建大小为n的向量r，其在每个坐标处包括每个点与3d网格之间的范数，
-创建大小为m×n的矩阵j，雅可比矩阵对应于r相对于pos_init向量的梯度，
-选择0到1之间的实数β的值(如果运动物体没有运动，或者仅轻微运动，则一般地为1或减小)，
-应用高斯-牛顿下降(如果jt*j是可逆的)：
pos_calc＝pos_init-β*(jt*j)-1*j*r
如果已知运动物体的高度，则可以通过将沿z轴的位置固定为已知的高度值来进行计算，从而提高计算的速度。
当pos_init与pos_calc之间的变化被认为足够低时，算法终止并且pos_calc值被认为是运动物体在环境的参照系中的相对位置。否则，算法再次启动，并且pos_calc值被用作接下来的迭代的pos_init值。
为了允许加速该方法，可以考虑矢量误差，以便针对每个x，y和z分量对估计的位置与校正的位置之间的校正方向进行估计。
为此，从所有的向量误差来确定每个分量的最小值和最大值，例如针对x分量的xmin和xmax：
-如果这些最小值和最大值具有不同的符号，则无法推导出方向。
-如果这两个值为正，则要进行的校正是在正方向上，并且校正至少等于xmin。因此可以直接进行xmin的该校正，以便更快地到达校正的位置。
-如果这两个值为负，则要进行的校正是在负方向上，并且校正至少等于xmax。因此可以直接进行xmax的该校正，以便更快地到达校正的位置。
对y和z分量执行相同的处理。
该方法还包括处理孤立点的步骤22，其被称为检查障碍物的步骤，其中将孤立点组合在一起以形成代表障碍物的体积。
例如，环境被分解为相邻的体元(boxel)类型立方体，并且该方法扫描该组体元以确定它是否包括至少一个孤立点；如果是这种情况，则体元被认为是障碍物体元。将接近的障碍物体元组合在一起以形成体积，例如借助数学形态学操作，诸如两个膨胀步骤，随后是腐蚀步骤。包含在这些体积中的体元被索引并记录为障碍物，从而限定了检查到的障碍物的位置。关于经记录的障碍物位置的信息可以帮助导航(避开障碍物)，并使得可以随时追踪障碍物。
该方法是以预定频率而实施的以便根据想要的约束(例如50hz)来追踪运动物体的位置上的改变。
一旦确定了位置并确定了潜在的障碍物，运动物体就可以使用该信息来实施用于针对所述物体运动所需的导航和引导的方法24。导航方法24包括至少一个运动步骤，在运动步骤期间，确定运动物体的位置和任何潜在的障碍物，特别是在运动过程中以定期的方式确定。
该方法的不同步骤是由不同的模块实施的，诸如处理器、微控制器、计算器等。步骤可以由单个专用模块实施，或者多个步骤可以由同一模块实施。
本发明不限于所描述的实施例。特别地，本发明可以适用于在轮子上的运动物体，在这种情况下，可以通过将计算限制于对应于地面上的2d运动的x和y坐标来简化计算。