CN109470255A - 基于高精度定位及车道线识别的高精度地图自动生成方法 - Google Patents

Abstract

本发明公开了一种基于高精度定位及车道线识别的高精度地图自动生成方法，包括：使每次获取的高精度定位数据与车道线数据同步；利用同步后的数据建立地图帧，并存入地图帧数据库；将新获取的车道线数据与已建立的所有的地图帧进行匹配，匹配失败，则建立新的地图帧；若匹配成功，则对已建立的地图帧进行信息更新；对已建立的地图帧中已有的所有帧做帧间平滑处理，以得平滑处理后的三次曲线；通过将三次曲线拼接，以生成高精度地图。本发明以生成基于高精度定位精确到车道线的，并能自动拼接车道线的高精度地图，降低了生成高精度地图的复杂性，并避免了传统高精度地图制作需要的大量的人力消耗以及错误率高的问题。

Description

基于高精度定位及车道线识别的高精度地图自动生成方法

技术领域

本发明涉及无人驾驶技术领域，尤其涉及一种基于高精度定位及车道线识别的高精度地图自动生成方法。

背景技术

高精地图作为无人驾驶领域的稀缺资源以及刚需，在整个领域扮演着核心角色，可以帮助无人车预先感知路面复杂信息，如坡度、曲率、航向等，结合智能路径规划，让无人车做出正确决策，是无人车驾驶不可或缺的数据来源。无人驾驶需要将传感器搜集的信息跟储存的高精地图对比，判断位置和方向，才能保证无人车安全驾驶至目的地，所以高精地图数据采集的准确性对于无人驾驶来说是非常关键的；传统的高精度地图的制作需要大量的人力标注，不仅费时费力，而且因人工标注导致的误差产生的错误率也较高，并不利于无人驾驶的发展，现今高精度地图的制作，因精度要求较高，计算过程复杂，制作过程也相对耗时，提供一种基于高精度定位以及车道线识别基础上可自动拼接车道，耗时较短的高精度地图是很有必要的。

发明内容

本发明的一个目的是解决至少上述问题，并提供至少后面将说明的优点。

本发明还有一个目的是提供一种基于高精度定位及车道线识别的高精度地图自动生成方法，以生成基于高精度定位精确到车道线的，并能自动拼接车道线的高精度地图，降低了生成高精度地图的复杂性，并避免了传统高精度地图制作需要的大量的人力消耗以及错误率高的问题。

为了实现根据本发明的这些目的和其它优点，提供了一种基于高精度定位及车道线识别的高精度地图自动生成方法，包括：

步骤1、通过时间对齐处理，使每次获取的高精度定位数据与车道线数据同步，以得同步后的所述高精度定位数据的位置和姿态。

步骤2、通过步骤1中所得同步后的所述车道线数据和高精度定位数据建立地图帧，并存储至地图帧数据库。

步骤3、将新获取的所述车道线数据与根据已获取的所述车道线数据建立的所有的所述地图帧进行匹配，若匹配失败，则利用新获取的所述车道线数据建立新的地图帧；若匹配成功，则对步骤2中已建立的所述地图帧进行信息更新，直至更新完毕。

步骤4、通过对步骤2以及步骤3中建立的所述地图帧中已有的所有帧做帧间平滑处理，再结合步骤3中更新后的所述地图帧的点集重新计算，以得经所述帧间平滑处理后的所述帧对应的代表车道线信息的三次曲线。

步骤5、通过将所述三次曲线拼接，以生成高精度地图。

优选的是，步骤1进一步包括：

以获取所述车道线数据的时间戳为时间对齐点，则对齐到所述车道线数据的时间戳的所述高精度定位数据的位置p′和姿态r′分别为：

p′＝p+v(t_m-t_l)； (1)

r′＝rω(t_m-t_l)； (2)

其中，t_m表示获取所述车道线数据的时间戳。

t_l表示获取所述高精度定位数据的时间戳。

p、r、v以及ω分别表示对齐前的所述高精度定位数据的位置、姿态、线速

度以及角速度。

优选的是，步骤2进一步包括：

所述地图帧主要包含以下元素：

P_F：所述帧空间信息的位置；R_F：所述帧空间信息的姿态。

C_F：车道线信息的三次曲线；S_F：车道线采样点集。

L_F：所述帧间拓扑信息的前后所述帧关联。

所述元素均以帧坐标系表示，其中帧坐标系以横向为x轴，纵向为y轴，垂直x轴、y轴方向为z轴。

优选的是，步骤2还进一步包括：

所述地图帧建立的前提是代表所述车道线的类型和/或颜色发生变化和/或所述车道线断开和/或驾驶中的无人车换道和/或所述地图帧y轴方向长度超出阈值。

优选的是，步骤3中匹配成功的条件为：

新获取的所述车道线数据与根据已获取的所述车道线数据建立的所有的所述地图帧中的一个或多个有重叠部分，且所述重叠部分达到所述阈值；和/或

新获取的所述车道线数据与已建立的所述地图帧中的任何一个前后相连。

优选的是，步骤3中所述地图帧的信息更新进一步包括：

步骤C、通过按一定的间隔对代表所述车道线的三次曲线采样，以得采样点集。

步骤D、通过计算所述采样点集相对于未更新的所述地图帧的位置和姿态，并将所述采样点集的位置投影到未更新的所述地图帧的帧坐标系内，以得所述帧坐标系内的采样点集。

步骤E、若所述帧坐标系内的所述采样点集的位置超出所述帧坐标系的长度限制，通过将超出部分切割出，以作为新的所述车道线数据进入步骤C进行所述地图帧的更新。

步骤F、通过将所述未更新的所述地图帧的采样点集和步骤D中所述帧坐标系内的采样点集合并，以得合并后按一定间隔重新采样的采样点集，并将所述未更新的所述地图帧的采样点集更新为所述重新采样的采样点集。

步骤G、通过对步骤F中重新采样的采样点集进行三次曲线拟合，以得拟合后的采样点集，并将所述未更新的所述地图帧的采样点集三次曲线拟合的结果更新为所述拟合后的采样点集。

步骤H、通过计算所述新的地图帧与已建的所述地图帧的连接关系，完成相连接帧的前帧和后帧的关联关系，以完成所述已建立的地图帧的信息更新。

优选的是，步骤4中做所述帧间平滑处理的前提是无新的所述车道线数据的输入。

优选的是，步骤4中帧间平滑处理进一步包括：

建立三次曲线，其中，拟合所述三次曲线的点集由有关联关系的所述前帧和后帧中的所述前帧的点集中的后半部分点集和所述后帧的点集中的前半部分点集混合所得。

通过将所述后半部分点集沿垂直于所述帧坐标系的y轴的方向，投影到所述三次曲线上，以得投影点集。

通过对所述前帧坐标系下的点与所述后半部分点集在所述帧坐标系的y轴上对应的投影点做平滑处理后，以得平滑后的点，从而得平滑后的帧；所述后帧坐标系下点集的平滑处理类似；点与点间平滑处理的公式为：

P″＝((1-a)x+ax′,y,(1-a)z+az′) (3)

其中，所述前帧点集的平滑系数a＝y/L；L表示所述帧的长度。

所述后帧点集的平滑系数a＝1-y/L。

P＝(x,y,z)表示所述前帧坐标系下的所述后半部分点集中的任一点。

P′＝(x′,y,z′)表示所述后帧坐标系下的所述前半部分点集中的任一点。

本发明至少包括以下有益效果：

本发明高精度定位数据和车道线数据之间，因高精度定位数据的超前性或滞后性，使得两者之间存在时间间隔，通过时间对齐处理，将时间对齐到车道线数据的获取时间戳，以保证数据处理时，数据时间上的一致性，以作为高精度地图生成的前提，数据处理上首先需进行高精度定位与车道线数据之间的坐标系转换；通过高精度定位数据以及车道线数据为先验条件，在获取新的车道线数据建立地图帧的时候，帧中空间信息的位置以及姿态即朝向，即为所述时间对齐处理后的高精度定位数据的位置和姿态，车道线数据发生变化时，首先需将其与已建立的地图帧匹配，以需求重叠或前后关联部分，从而对获取的车道线数据进行更新替换，若匹配不成功，获取的新的车道线数据再建立新的地图帧，从而通过匹配更新以及重建，自动完成车道线的拼接；再通过所述帧间平滑处理，使得帧间平滑过渡，帧内点集重叠部分平滑连接，以生成基于高精度定位精确到车道线的，并能自动拼接车道线的高精度地图，通过高精度定位以及车道线获取的先验条件，生成自动拼接车道线的高精度地图，降低了生成高精度地图的复杂性，并避免了传统高精度地图制作需要的大量的人力消耗以及错误率高的问题，对无人驾驶可靠并安全地行驶具有重要的意义。

本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。

附图说明

图1为本发明所述基于高精度定位及车道线识别的高精度地图自动生成方法的流程图；

图2为本发明所述帧坐标系下地图帧包含的元素的示意图；

图3为本发明所述所述车道线数据存在盲区时的示意图；

图4为本发明所述所述地图帧信息更新过程的示意图；

图5为本发明所述所述地图帧信息更新后的示意图。

具体实施方式

下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

应当理解，本文所使用的诸如“具有”、“包含”以及“包括”术语并不排除一个或多个其它元件或其组合的存在或添加。

如图1所示，本发明提供一种基于高精度定位及车道线识别的高精度地图自动生成方法，包括：

步骤1、通过时间对齐处理，使每次获取的高精度定位数据与车道线数据同步，以得同步后的所述高精度定位数据的位置和姿态。

步骤2、通过步骤1中所得同步后的所述车道线数据和高精度定位数据建立地图帧，并存储至地图帧数据库。

步骤3、将新获取的所述车道线数据与根据已获取的所述车道线数据建立的所有的所述地图帧进行匹配，若匹配失败，则利用新获取的所述车道线数据建立新的地图帧；若匹配成功，则对步骤2中已建立的所述地图帧进行信息更新，直至更新完毕。

步骤4、通过对步骤2以及步骤3中建立的所述地图帧中已有的所有帧做帧间平滑处理，再结合步骤3中更新后的所述地图帧的点集重新计算，以得经所述帧间平滑处理后的所述帧对应的代表车道线信息的三次曲线。

步骤5、通过将所述三次曲线拼接，以生成高精度地图。

在上述方案中，高精度定位数据和车道线数据之间，因高精度定位数据的超前性或滞后性，使得两者之间存在时间间隔，通过时间对齐处理，将时间对齐到车道线数据的获取时间戳，以保证数据处理时，数据时间上的一致性，以作为高精度地图生成的前提，数据处理上首先需进行高精度定位与车道线数据之间的坐标系转换；通过高精度定位数据以及车道线数据为先验条件，在获取新的车道线数据建立地图帧的时候，帧中空间信息的位置以及姿态即朝向，即为所述时间对齐处理后的高精度定位数据的位置和姿态，车道线数据发生变化时，首先需将其与已建立的地图帧匹配，以需求重叠或前后关联部分，从而对获取的车道线数据进行更新替换，若匹配不成功，获取的新的车道线数据再建立新的地图帧，从而通过匹配更新以及重建，自动完成车道线的拼接；再通过所述帧间平滑处理，使得帧间平滑过渡，帧内点集重叠部分平滑连接，以生成基于高精度定位精确到车道线的，并能自动拼接车道线的高精度地图，通过高精度定位以及车道线获取的先验条件，生成自动拼接车道线的高精度地图，降低了生成高精度地图的复杂性，并避免了传统高精度地图制作需要的大量的人力消耗以及错误率高的问题，对无人驾驶可靠并安全地行驶具有重要的意义。

一个优选方案中，步骤1进一步包括：

以获取所述车道线数据的时间戳为时间对齐点，则对齐到所述车道线数据的时间戳的所述高精度定位数据的位置p′和姿态r′分别为：

p′＝p+v(t_m-t_l)； (1)

r′＝rω(t_m-t_l)； (2)

其中，t_m表示获取所述车道线数据的时间戳。

t_l表示获取所述高精度定位数据的时间戳。

p、r、v以及ω分别表示对齐前的所述高精度定位数据的位置、姿态、线速

度以及角速度。

在上述方案中，通过对车道线数据获取的时间设置时间戳，以车道线数据获取的时间戳为参考，高精度定位数据的获取，因为数据采样时刻的不同，会有一定的超前性或滞后性，为保证车道线数据与高精度数据在生成方法中同步，通过公式1转换，从而得到算法同步后的高精度定位数据。

一个优选方案中，步骤2进一步包括：

所述地图帧主要包含以下元素：

P_F：所述帧空间信息的位置；R_F：所述帧空间信息的姿态。

C_F：车道线信息的三次曲线；S_F：车道线采样点集。

L_F：所述帧间拓扑信息的前后所述帧关联。

所述元素均以帧坐标系表示，其中帧坐标系以横向为x轴，纵向为y轴，垂直x轴、y轴方向为z轴。

在上述方案中，如图2所示，帧坐标系下，地图帧包含的元素。

一个优选方案中，步骤2还进一步包括：

所述地图帧建立的前提是代表所述车道线的类型和/或颜色发生变化和/或所述车道线断开和/或驾驶中的无人车换道和/或所述地图帧y轴方向长度超出阈值。

在上述方案中，地图帧代表的每帧均对应车道线数据获取时的一个位置以及姿态，不同的位置和姿态，即不同的地图帧构成基于车道识别的高精度地图，在车道线识别时，车道线数据的变化，在匹配不成功的前提下，就会建立新的地图帧，车道线数据改变，地图帧被建立时，若车道线的在地图帧y轴方向超出阈值，该帧的位置P_F和姿态R_F分别被赋值为p′和r′，如图3所示，W所示区域内的车道线数据提取至地图帧数据库，由于获取的车道线数据在y轴方向存在盲区[0,y_b]，即G所在范围内，在已有的地图帧中搜寻该区域所对应的数据，如果能找到，则将对应数据填补到盲区中。

一个优选方案中，步骤3中匹配成功的条件为：

新获取的所述车道线数据与根据已获取的所述车道线数据建立的所有的所述地图帧中的一个或多个有重叠部分，且所述重叠部分达到所述阈值；和/或

新获取的所述车道线数据与已建立的所述地图帧中的任何一个前后相连。

在上述方案中，新获取的所述车道线数据首先需要与已经建立的地图帧进行匹配，以获取与新获取的车道线数据有重叠部分或前后相连的地图帧，从而确定新获取的车道线数据的位置，从而进行地图帧的更新。

一个优选方案中，步骤3中所述地图帧的信息更新进一步包括：

步骤C、通过按一定的间隔对代表所述车道线的三次曲线采样，以得采样点集。

步骤D、通过计算所述采样点集相对于未更新的所述地图帧的位置和姿态，并将所述采样点集的位置投影到未更新的所述地图帧的帧坐标系内，以得所述帧坐标系内的采样点集。

步骤E、若所述帧坐标系内的所述采样点集的位置超出所述帧坐标系的长度限制，通过将超出部分切割出，以作为新的所述车道线数据进入步骤C进行所述地图帧的更新。

步骤F、通过将所述未更新的所述地图帧的采样点集和步骤D中所述帧坐标系内的采样点集合并，以得合并后按一定间隔重新采样的采样点集，并将所述未更新的所述地图帧的采样点集更新为所述重新采样的采样点集。

步骤G、通过对步骤F中重新采样的采样点集进行三次曲线拟合，以得拟合后的采样点集，并将所述未更新的所述地图帧的采样点集三次曲线拟合的结果更新为所述拟合后的采样点集。

步骤H、通过计算所述新的地图帧与已建的所述地图帧的连接关系，完成相连接帧的前帧和后帧的关联关系，以完成所述已建立的地图帧的信息更新。

在上述方案中，如图4及5所示，地图帧信息更新的基本流程如下：

按一定间隔对代表车道线A的三次曲线进行采样，得到采样点集S_A；

计算A相对于当前地图帧F的位置姿态，并把采样点集S_A的位置姿态投影到F上，记为

如果超出了当前地图帧F的长度限制，将超出部分切割出来并把其作为新的车道线数据重新进行地图帧更新的步骤；

将当前地图帧F的采样点S_F与合并，并按一定间隔重采样得到S_F′，并将S_F更新为更新为S_F′；

对S_F′做三次曲线拟合得到C_F′，并将C_F更新为C_F′；

对于新增的地图帧计算其与已有地图帧的连接关系，据此更新相连接帧的前后帧关联关系L_F。

一个优选方案中，步骤4中做所述帧间平滑处理的前提是无新的所述车道线数据的输入。

在上述方案中，已建地图帧信息更新完毕的前提是，没有新的车道数据线被获取，否则一旦新的车道线数据被获取，新的车道线数据会进入与已建地图帧的匹配，或建立新的地图帧或进入已建地图帧的更新中，所以只有在无新的车道线数据输入的情况下，才能保证地图帧已更新完毕以进行帧间平滑处理。

一个优选方案中，步骤4中帧间平滑处理进一步包括：

建立三次曲线，其中，拟合所述三次曲线的点集由有关联关系的所述前帧和后帧中的所述前帧的点集中的后半部分点集和所述后帧的点集中的前半部分点集混合所得。

通过将所述后半部分点集沿垂直于所述帧坐标系的y轴的方向，投影到所述三次曲线上，以得投影点集。

通过对所述前帧坐标系下的点与所述后半部分点集在所述帧坐标系的y轴上对应的投影点做平滑处理后，以得平滑后的点，从而得平滑后的帧；所述后帧坐标系下点集的平滑处理类似；点与点间平滑处理的公式为：

P″＝((1-a)x+ax′,y,(1-a)z+az′) (3)

其中，所述前帧点集的平滑系数a＝y/L；L表示所述帧的长度。

所述后帧点集的平滑系数a＝1-y/L。

P＝(x,y,z)表示所述前帧坐标系下的所述后半部分点集中的任一点。

P′＝(x′,y,z′)表示所述后帧坐标系下的所述前半部分点集中的任一点。

在上述方案中，设前后有关联关系的两帧分别为F₁和F₂，取前帧点集的后半部分点集和后帧点集的前半部分点集混合，拟合三次曲线C_m；对于点集将其沿着垂直于y轴的方向投影到三次曲线C_m上，得到投影点集对于帧F₁坐标系下的点集中的每一点P＝(x,y,z)及与其在中对应的点P′＝(x′,y,z′)，按如下公式计算平滑后的点P″＝((1-a)x+ax′,y,(1-a)z+az′)，其中平滑系数a＝y/L，L为帧F₁的长度。

帧F₂的点集平滑与此类似，区别在于平滑系数变为a＝1-x/L；具体为，对于点集将其沿着垂直于y轴的方向投影到三次曲线C_m上，得到投影点集对于帧F₂坐标系下的点集中的每一点P＝(x,y,z)及与其在中对应的点P′＝(x′,y,z′)，按如下公式计算平滑后的点P″＝((1-a)x+ax′,y,(1-a)z+az′)，其中平滑系数a＝1-y/L，L帧F₂的长度；

对于做完点集平滑的帧，根据更新后的点集重新计算该帧的车道线三次曲线C_F。

尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用，它完全可以被适用于各种适合本发明的领域，对于熟悉本领域的人员而言，可容易地实现另外的修改，因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。

Claims (8)

1.一种基于高精度定位及车道线识别的高精度地图自动生成方法，其中，包括：

步骤1、通过时间对齐处理，使每次获取的高精度定位数据与车道线数据同步，以得同步后的所述高精度定位数据的位置和姿态；

步骤2、通过步骤1中所得同步后的所述车道线数据和高精度定位数据建立地图帧，并存储至地图帧数据库；

步骤3、将新获取的所述车道线数据与根据已获取的所述车道线数据建立的所有的所述地图帧进行匹配，若匹配失败，则利用新获取的所述车道线数据建立新的地图帧；若匹配成功，则对步骤2中已建立的所述地图帧进行信息更新，直至更新完毕；

步骤4、通过对步骤2以及步骤3中建立的所述地图帧中已有的所有帧做帧间平滑处理，再结合步骤3中更新后的所述地图帧的点集重新计算，以得经所述帧间平滑处理后的所述帧对应的代表车道线信息的三次曲线；

步骤5、通过将所述三次曲线拼接，以生成高精度地图。

2.如权利要求1所述基于高精度定位及车道线识别的高精度地图自动生成方法，其中，步骤1进一步包括：

以获取所述车道线数据的时间戳为时间对齐点，则对齐到所述车道线数据的时间戳的所述高精度定位数据的位置p′和姿态r′分别为：

p′＝p+v(t_m-t_l)； (1)

r′＝rω(t_m-t_l)； (2)

其中，t_m表示获取所述车道线数据的时间戳；

t_l表示获取所述高精度定位数据的时间戳；

p、r、v以及ω分别表示对齐前的所述高精度定位数据的位置、姿态、线速度以及角速度。

3.如权利要求1所述基于高精度定位及车道线识别的高精度地图自动生成方法，其中，步骤2进一步包括：

所述地图帧主要包含以下元素：

P_F：所述帧空间信息的位置；R_F：所述帧空间信息的姿态；

C_F：车道线信息的三次曲线；S_F：车道线采样点集；

L_F：所述帧间拓扑信息的前后所述帧关联；

所述元素均以帧坐标系表示，其中帧坐标系以横向为x轴，纵向为y轴，垂直x轴、y轴方向为z轴。

4.如权利要求1所述基于高精度定位及车道线识别的高精度地图自动生成方法，其中，步骤2还进一步包括：

所述地图帧建立的前提是代表所述车道线的类型和/或颜色发生变化和/或所述车道线断开和/或驾驶中的无人车换道和/或所述地图帧y轴方向长度超出阈值。

5.如权利要求4所述基于高精度定位及车道线识别的高精度地图自动生成方法，其中，步骤3中匹配成功的条件为：

新获取的所述车道线数据与根据已获取的所述车道线数据建立的所有的所述地图帧中的一个或多个有重叠部分，且所述重叠部分达到所述阈值；和/或

新获取的所述车道线数据与已建立的所述地图帧中的任何一个前后相连。

6.如权利要求4所述基于高精度定位及车道线识别的高精度地图自动生成方法，其中，步骤3中所述地图帧的信息更新进一步包括：

步骤C、通过按一定的间隔对代表所述车道线的三次曲线采样，以得采样点集；

步骤D、通过计算所述采样点集相对于未更新的所述地图帧的位置和姿态，并将所述采样点集的位置投影到未更新的所述地图帧的帧坐标系内，以得所述帧坐标系内的采样点集；

步骤E、若所述帧坐标系内的所述采样点集的位置超出所述帧坐标系的长度限制，通过将超出部分切割出，以作为新的所述车道线数据进入步骤C进行所述地图帧的更新；

步骤F、通过将所述未更新的所述地图帧的采样点集和步骤D中所述帧坐标系内的采样点集合并，以得合并后按一定间隔重新采样的采样点集，并将所述未更新的所述地图帧的采样点集更新为所述重新采样的采样点集；

步骤G、通过对步骤F中重新采样的采样点集进行三次曲线拟合，以得拟合后的采样点集，并将所述未更新的所述地图帧的采样点集三次曲线拟合的结果更新为所述拟合后的采样点集；

步骤H、通过计算所述新的地图帧与已建的所述地图帧的连接关系，完成相连接帧的前帧和后帧的关联关系，以完成所述已建立的地图帧的信息更新。

7.如权利要求1所述的基于高精度定位及车道线识别的高精度地图自动生成方法，其中，步骤4中做所述帧间平滑处理的前提是无新的所述车道线数据的输入。

8.如权利要求5所述的基于高精度定位及车道线识别的高精度地图自动生成方法，其中，步骤4中帧间平滑处理进一步包括：

步骤a、建立三次曲线，其中，拟合所述三次曲线的点集由有关联关系的所述前帧和后帧中的所述前帧的点集中的后半部分点集和所述后帧的点集中的前半部分点集混合所得；

步骤b、通过将所述后半部分点集沿垂直于所述帧坐标系的y轴的方向，投影到所述三次曲线上，以得投影点集；

步骤c、通过对所述前帧坐标系下的点与所述后半部分点集在所述帧坐标系的y轴上对应的投影点做平滑处理后，以得平滑后的点，从而得平滑后的帧；所述后帧坐标系下点集的平滑处理类似；点与点间平滑处理的公式为：

P″＝((1-a)x+ax′,y,(1-a)z+az′) (3)

其中，所述前帧点集的平滑系数a＝y/L；L表示所述帧的长度；

所述后帧点集的平滑系数a＝1-y/L；

P＝(x,y,z)表示所述前帧坐标系下的所述后半部分点集中的任一点；

P′＝(x′,y,z′)表示所述后帧坐标系下的所述前半部分点集中的任一点。