CN111862203B - 基于3d地图的球机位置与姿态参数校准方法、存储介质 - Google Patents

Abstract

本发明提供一种基于3D地图的球机位置与姿态参数校准方法、计算机存储介质，基于3D地图的球机位置与姿态参数校准方法包括：S1、根据地图中和球机拍摄的画面中的特征点；S2、根据多个特征点的坐标计算球机的三维坐标点B；S3、将以三维坐标点B为中心，N为边长的正方体划分为M×M×M个区域，将误差值最小的节点作为预设球机位置；S4、判断区域的尺寸是否小于预设阈值，如果是，则将预设球机位置作为球机位置B_n，否则，执行步骤S5；S5、将以预设球机位置为中心，N/M为边长的正方体划分为M×M×M个区域，将误差值最小的节点为预设球机位置，执行步骤S4。本发明的方法能有效精准地计算出了球机的姿态信息。

Description

基于3D地图的球机位置与姿态参数校准方法、存储介质

技术领域

本发明涉及球机位置与姿态的校准方法技术领域，尤其涉及基于3D地图的球机位置与姿态参数校准方法、计算机存储介质。

背景技术

目前球机等设备由人工进行安装,无法保证安装后球机位姿的统一性。在3D实景监控应用中,需要根据球机的位姿信息,将球机所采集的视频画面准确的投影到3D实景地图中，因此需要对球机的位姿参数进行校准。现有的球机校准技术有基于谷歌地图等平面地图对球机位姿参数校准技术。该类技术只能粗略的计算出球机的位置以及朝向(基于水平面的方位角，例如东南西北)，无法满足对球机位姿参数精度要求高的应用场景。

在现有技术中，基于谷歌地图等GPS数据对球机位姿参数校准方法的缺点：受平面地图精度限制，无法获取平面地图上位置点的高精度GPS坐标。同时，平面地图与视频画面并不能完全匹配，无法保证所采集GPS坐标以及PT坐标属于同一位置点。例如，谷歌地图(中国境内)的更新周期为1-2年，最高清晰度为0.5-1.0米像素分辨率。另外，由于采集的GPS坐标中没有位置点的高度信息，因此该方法只能将3D空间的球机位置近似为2D空间，导致球机位姿求解过程中的约束条件与实际的约束条件不匹配。该方法校准的球机位置误差范围在1米以上，而球机的朝向误差范围在5度以上。

发明内容

有鉴于此，本发明提供一种基于3D地图的球机位置与姿态参数校准方法、计算机存储介质及设备，基于3D地图的球机位置与姿态参数校准方法能够有效精准地计算出了球机的姿态信息。

为解决上述技术问题，一方面，本发明提供一种基于3D地图的球机位置与姿态参数校准方法，包括以下步骤：S1、根据地图中和球机拍摄的画面中的特征点，获取多个所述特征点对应的地图中的坐标和球机的PT坐标；S2、根据多个所述特征点的坐标计算球机的三维坐标点B；S3、将以所述三维坐标点B为中心，N为边长的正方体划分为M×M×M个区域，划分后得到若干个网格节点，计算每个所述网格节点坐标的误差值，并将误差值最小的节点作为预设球机位置；S4、判断所述区域的尺寸是否小于预设阈值，如果是，则将所述预设球机位置作为球机位置B_n，否则，执行步骤S5；S5、将以所述预设球机位置为中心，N/M为边长的正方体划分为M×M×M个区域，划分后得到若干个网格节点，计算每个所述网格节点坐标的误差值，并将误差值最小的节点为预设球机位置，执行步骤S4；

根据本发明实施例的基于3D地图的球机位置与姿态参数校准方法，通过获取多个特征点的地图中的坐标和球机的PT坐标，并通过粗算和精算获取球机位置，该方法有效精准地计算出了球机的姿态信息。

根据本发明实施例的基于3D地图的球机位置与姿态参数校准方法还可以具有以下附加技术特征：

根据本发明的一个实施例，在步骤S1中，获取至少4个所述特征点，每个所述特征点分别记为A_i(X_i，Y_i，Z_i，P_i，T_i)(i＝1，2，3…n)，其中，X_i，Y_i，Z_i分别为多个所述特征点对应的3D地图中的坐标，P_i，T_i分别为多个所述特征点对应的球机的PT坐标。

根据本发明的一个实施例，步骤S1还包括以下步骤：S11、令i＝k，k＝1；S12、调整所述球机的视频画面，当所述球机画面中心对准一个所述特征点时，记录所述球机的PT坐标P_i，T_i；S13、在所述3D地图中找到所述特征点，并记录该特征点的在所述3D地图中的坐标X_i，Y_i，Z_i；S14、记录所述特征点的坐标为A_i(X_i，Y_i，Z_i，P_i，T_i)；S15、i＝k+1，若i＜5，则执行S12。

根据本发明的一个实施例，在步骤S2中，利用牛顿迭代法计算球机的三维坐标点B。

根据本发明的一个实施例，在步骤S3中，N＝1米，M＝4，所述区域为正方体，所述误差值为：

其中，η为误差值，∠A_iB_nA_j和分别为在三维坐标系中和球机坐标系中点Bn与两个点之间的连线形成的夹角，/>表示在球机坐标系中第i个特征点的位置,/>表示在球技坐标系中第j个特征点的位置。

根据本发明的一个实施例，在步骤S4中，所述误差值小于1cm时，将所述预设球机位置作为球机位置B_n。

根据本发明的一个实施例，在步骤S12中，还包括对所述特征点A_i的PT坐标进行校准，球机传感器坐标为(P_i，T_i)，球机角度偏移值为(ΔP_i，ΔT_i)，A_i的真实PT为所述特征点Ai的PT校准包括以下步骤：S121、设定ΔT的初始值，ΔT＝0.01度；S122、将的T值加ΔT，按照式(16)计算误差值η'，如果η'＞η，则执行步骤S123，如果η'＜η，则令η'＝η，并执行步骤S124；S123、ΔT＝-ΔT，并将/>的T值加ΔT，按照式(16)计算误差值η'，如果η'＞η，则执行步骤S125，如果η'＜η，则令η'＝η，并执行步骤S124；S124、ΔT＝2*ΔT，并将/>的T值加ΔT，按照式(16)计算误差值η'，如果η'＞η，则令ΔT＝ΔT/2，并则执行步骤S125，如果η'＜η，则令η'＝η，并执行步骤S124；S125、结束，并将所得的η'值当作网格节点可作为球机真实位置的误差值。

根据本发明的一个实施例，还包括对球机姿态校准：用ΔT对 的T值进行补偿。

根据本发明的一个实施例，还包括对球机角度偏移值ΔP校准：以球机的轴向L和Y轴所构成的平面作为球机局部坐标系的Y－B－Z平面，并将L当作Z轴，计算球机局部坐标系的X、Y轴以得到临时的局部球面坐标系，假设计算所得的PT坐标为(P′_k，T′_k)，实际采集的PT坐标为(P_k，T_k)，则ΔP的计算方法如式(17)所示

第二方面，本发明实施例提供一种计算机存储介质，包括一条或多条计算机指令，所述一条或多条计算机指令在执行时实现上述任一项所述的方法。

附图说明

图1为根据本发明实施例的基于3D地图的球机位置与姿态参数校准方法的流程图；

图2为根据本发明实施例的三维空间的网格划分示意图；

图3为根据本发明实施例的坐标系模型示意图；

图4为根据本发明实施例的直角坐标与球面坐标的关系示意图；

图5为根据本发明实施例的球机校准流程图；

图6为根据本发明实施例的旋转轴求解示意图；

图7为根据本发明实施例的视频画面中采集位置点的PT坐标示意图；

图8为根据本发明实施例的3D地图中采集位置点的位置信息图；

图9为根据本发明实施例的球机校准工具数据采集界面示意图；

图10为根据本发明实施例的球机校准参数输出格式示意图；

图11为根据本发明实施例的球机校准参数的应用效果图；

图12为本发明实施例的电子设备的示意图。

附图标记：

基于3D地图的球机位置与姿态参数校准方法100；

电子设备300；

存储器310；操作系统311；应用程序312；

处理器320；网络接口330；输入设备340；硬盘350；显示设备360。

具体实施方式

下面将结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

如图1和图11所示，根据本发明实施例的基于3D地图的球机位置与姿态参数校准方法100，包括以下步骤：

S1、根据地图中和球机拍摄的画面中的特征点，获取多个所述特征点对应的地图中的坐标和球机的PT坐标；

S2、根据多个所述特征点的坐标计算球机的三维坐标点B；

S3、将以所述三维坐标点B为中心，N为边长的正方体划分为M×M×M个区域，计算每个所述区域节点坐标的误差值，并将误差值最小的节点作为预设球机位置；

S4、判断所述区域的尺寸是否小于预设阈值，如果是，则将所述预设球机位置作为球机位置B_n，否则，执行步骤S5；

S5、将以所述预设球机位置为中心，N/M为边长的正方体划分为M×M×M个区域，计算每个所述区域节点坐标的误差值，并将误差值最小的节点为预设球机位置，执行步骤S4。

换言之，如图1所示，球机位姿校准是指当球机已经安装完毕后，通过算法获取球机位姿信息的过程。在获得球机的位姿信息后，才能将球机与周围环境联系起来。这个位姿信息可以理解为球机的真实方位朝向。因为软件中采用3D地图表示真实的地理信息，所以这里的球机位姿又可以理解为球机在3D地图中的空间位姿。

首先，采集一系列位置点数据。然后，根据特征点联立等式粗略求得球机的大致位置，但是这个位置有比较大的误差(大概在一两米内，具体误差与采集的点以及设备相关)。而这里的B点就是粗校准后的估计位置，以这个位置为中心，向外扩展N/2米的范围，得到边长为N×N×N的立方体区域。

在这个立方体区域中，将立方体的沿着长宽高划分M等分(例如可以划分为4等分)，划分后得到若干个网格节点，假设每一个网格节点都是球机的真实位置，再计算其可信度(或者说是误差值)，误差值最小的点Bn被认为是最有可能是球机的真实位置。因为网格节点之间的距离为N/M，所以Bn点到真实球机位置的距离小于N/M，相比于粗校准得到的B点，Bn更加接近真实球机的位置。

接下来是进一步的迭代计算，将Bn点位置认为是粗校准得到的位置，并构造一个以Bn点为中心的立方体，立方体的边长为N/M，同样进行M等分，得到若干个更细的网格节点，如图2中的较细小网格所示。同样的方法找出误差最小的网格节点，不断的缩小球机搜索位置的范围。

因为搜索范围就是球机真实位置的浮动范围，所以在不断细分网格的过程中，当网格边长小于预设阈值的时候，可以认为查找到的网格节点就是真实的球机位置。采用球机位置粗校准方法，能够快速逼近球机的真实位置，但是不能控制球机的校准精度。这里采用网格细分的方式，对球机位置进行精准迭代。如图2所示，假设A点是粗校准估算位置，则将A点附近长宽高为N米的范围划分为若干个小区域。

由此，根据本发明实施例的基于3D地图的球机位置与姿态参数校准方法100，通过获取多个特征点的地图中的坐标和球机的PT坐标，并通过粗算和精算获取球机位置，该方法有效精准地计算出了球机的姿态信息。

根据本发明的一些具体实施例中，在步骤S1中，获取至少4个所述特征点，每个所述特征点分别记为A_i(X_i，Y_i，Z_i，P_i，T_i)(i＝1，2，3…n)，其中，X_i，Y_i，Z_i分别为多个所述特征点对应的3D地图中的坐标，P_i，T_i分别为多个所述特征点对应的球机的PT坐标。

具体地，一个位置点数据包括两个部分，即该位置点在3D地图中的空间直角坐标(笛卡尔坐标系O-XYZ)，以及该位置点相对于球机的PT坐标(属于球面坐标系，从球机默认位置算起，当球机正对该位置点时，球机在水平和竖直两个方向的旋转的角度值，这里的水平和竖直指定的球机自身旋转的两个轴向)。如图3所示，假设O-XYZ是3D地图中的直角坐标系，B点是球机在实景地图中的直角坐标位置，L是球机的自身水平(该水平方向并不是真实的水平方向,而是球机在自身局部坐标系中所认为的水平方向)旋转的轴向，F是球机自身默认朝向(球机P、T值都为零的方向)。球机自身以L、F两个方向构成球面坐标系。基于图3描述的坐标系模型，则对任意位置点A_i所采集的数据为直角坐标(X_i，Y_i，Z_i)和PT坐标(P_i，T_i)。

在本发明的一个实施例中，步骤S1还包括以下步骤：

S11、令i＝k，k＝1；

S12、调整所述球机的视频画面，当所述球机画面中心对准一个所述特征点时，记录所述球机的PT坐标X_i，Y_i，Z_i；

S13、在所述3D地图中找到所述特征点，并记录该特征点的在所述3D地图中的坐标P_i，T_i；

S14、记录所述特征点的坐标为A_i(X_i，Y_i，Z_i，P_i，T_i)；

S15、i＝k+1，若i＜5，则执行S12。

也就是说，获取至少4个特征点以计算球机的三围坐标点B，在步骤S1中，采集设备主要由两个部分，一个是球机的偏航角(PT坐标)，一个是3D地图中的点坐标，其中球机的偏航角由球机设备本身提供。

具体地，首先，控制(旋转操作)球机，调整球机所采集的视频画面，当球机画面中心对准某一具有明显特征的点，例如斑马线的顶点、建筑的棱角(在航拍过程中，这些特征都容易被记录到3D地图中)，记录此时的球机PT坐标，如图7所示；然后，在3D地图中找到上述提到的特征点，由于3D地图属于三维几何模型，带有坐标系，可以获取到该特征点在3D地图中的坐标，记录下该坐标，如图8所示；前2步所记录的两种坐标信息表示的是同一个空间点在不同坐标系下的坐标表示，这两个数据合在一起作为一个位置点的数据。重复前两步，记录多个位置点数据，如图9和图10所示。

优选地，利用牛顿迭代法计算球机的三维坐标点B。具体计算过程如下：

获取四个所述特征点A_i(X_i，Y_i，Z_i，P_i，T_i)(i＝1,2,3,4)的数据后，通过牛顿迭代法快速估计球机的三维坐标B点。如图4所示，三维坐标系中的位置为A_i，在球机球面坐标系中的位置为则有

公式(1)中的可以由/>和/>的PT坐标计算得出

其中，

由于(2)、(3)、(4)式中都是已知量，故式(1)中的变量都可以认为是已知量，在O-XYZ直角坐标系中，∠A_iBA_j可以表示为

其中，

V_i＝A_i-B＝(X_i-X₀，Y_i-Y₀，Z_i-Z₀)^T (6)

V_j＝A_j-B＝(X_j-X₀,Y_j-Y₀，Z_j-Z₀)^T (7)

将式(1)、(6)、(7)代入式(5)得

变形得

式(9)为包含了球机坐标位置B与采集的数据点之间的等式关系，其中，

利用牛顿迭代法对B点位置进行逼近，计算F(B)函数对B点X、Y、Z的偏导F_ijx、F_ijy、F_ijz，

其中，k用于指代X、Y、Z，B_k、A_ik、A_jk分别表示点B、A_i、A_j、中的k的坐标，假设有N个数据点，两两结合，可以得到个式(9)，

以及个式(14)，将所有偏导数编成雅可比矩阵，

求解矩阵的伪逆/>最终得到牛顿迭代法的表达式

选取其中一个数据点或者多有数据点的平均值作为B点的初始迭代值B0，并以||B_n+1-B_n||＜f为迭代终止条件，其中，f为某一迭代阈值。

通过牛顿迭代法快速校准球机位置，在快速校准后进一步以网格划分的方式对球机位置进行精校准，该方法可以控制球机位置校准结果的精度。

根据本发明的一个实施例，在步骤S3中，N＝1米，M＝4，所述区域为正方体，所述误差值为：

其中，η为误差值，∠A_iB_nA_j和分别为在三维坐标系中和球机坐标系中点Bn与两个点之间的连线形成的夹角，/>表示在球机坐标系中第i个特征点的位置,/>表示在球技坐标系中第j个特征点的位置。

需要说明的是，令N为1米(N是一个可调参数，根据实际校准效果，可以适当的调整，由于前面的粗校准的到的B点位置与实际的球机真实位置相差在一米以内，所以这里设置N为1米)。

优选地，在步骤S4中，所述误差值小于1cm时，将所述预设球机位置作为球机位置B_n。

因为搜索范围就是球机真实位置的浮动范围，所以在不断细分网格的过程中，当网格边长小于1cm的时候，可以认为查找到的网格节点就是真实的球机位置(误差在1cm以内)。遍历网格节点，假设Bn是其中一个网格节点，计算Bn是球机真实位置的可能性。判断依据：假定该点是球机位置，计算采样点的误差值η，误差值小的可能性越大。其中，误差值η的计算方法如式(16)所示。

根据本发明的一个实施例，在步骤S12中，还包括对所述特征点A_i的PT坐标进行校准，球机传感器坐标为(P_i，T_i)，球机角度偏移值为(ΔP_i，ΔT_i)，A_i的真实PT为所述特征点A_i的PT校准包括以下步骤：

S121、设定ΔT的初始值，ΔT＝0.01度；

S122、将的T值加ΔT，按照式(16)计算误差值η'，如果η'＞η，则执行步骤S123，如果η'＜η，则令η'＝η，并执行步骤S124；

S123、ΔT＝-ΔT，并将的T值加ΔT，按照式(16)计算误差值η'，如果η'＞η，则执行步骤S125，如果η'＜η，则令η'＝η，并执行步骤S124；

S124、ΔT＝2*ΔT，并将的T值加ΔT，按照式(16)计算误差值η'，如果η'＞η，则令ΔT＝ΔT/2，并则执行步骤S125，如果η'＜η，则令η'＝η，并执行步骤S124；

S125、结束，并将所得的η'值当作网格节点可作为球机真实位置的误差值。

需要说明的是，由于球机制造以及安装导致的球机真实的PT坐标与球机传感器所获取的PT坐标之间存在差异，本申请将这个差异表示为球机角度偏移值(ΔP，ΔT)。在数据采集过程中采集的PT坐标是传感器反馈回来的坐标(P，T)，因此，球机真实的PT坐标为(P+ΔP，T+ΔT)。为了得到准确的误差值，式(16)中的点都需要根据球机角度偏移值(ΔP，ΔT)做偏移校准。但是目前为止ΔP、ΔT两个值还是未知量，故通过上述枚举的方式求解ΔP、ΔT。

进一步地，从所有网格节点中筛选出最有可能的节点，然后以该节点为中心，继续将网格细分，直到网格直径小于指定精度，这时网格节点可以在可接受范围(指定精度)内近似等价为球机的真实位置B点。同时，该节点所对应的ΔT值便是本提案所求的T偏移值。

在本发明的一个实施中，基于3D地图的球机位置与姿态参数校准方法100还包括对球机姿态校准：用ΔT对的T值进行补偿。

具体而言，球机的位姿信息归纳为球机位置B(X_B，Y_B，Z_B)、球机自转轴向L(X_L，Y_L，Z_L)、球机角度偏移值(ΔP，ΔT)三个部分，整个校准流程如图5所示。

其中，经过球机位置精校准操作，可以得出球机的精确位置坐标B，以及球机角度偏移值ΔT。用ΔT对的T值进行补偿。由于知道了球机的位置，因此可以计算A_k四个点在以球机为中心的单位球上的位置，同样以A_k表示，如图6所示，在图6中存在一个重要的几何关系就是，四根线(A_k与/>的连线(k＝1，2，3，4))所表示的向量共面，而面的法向量便是它们之间的旋转轴L。

需要说明的是，基于三维空间构造球机校准的数学模型，更为全面的将球机的位置参数细分为球机位置B(X_B，Y_B，Z_B)、球机自转轴向L(X_L，Y_L，Z_L)、球机角度偏移值(ΔP，ΔT)三个部分。而现有技术只将球机参数划分为球机GPS坐标，球机角度偏移值(ΔP，ΔT)。另外，现有技术所描述的ΔT值是一个角度偏移值列表，在应用球机校准参数时，使用者需要根据球机的当前PT坐标，选择该列表中的一个值作为当前ΔT的值。采用角度偏移值列表的根本原因是，因为其校准后的ΔT值不能准确反映出T值的真实角度偏移，所以需要多个ΔT值进行纠偏。角度偏移值列表的形式本身就说明了现有技术对球机校准参数建模的不合理性。

进一步地，基于3D地图的球机位置与姿态参数校准方法100还包括对球机角度偏移值ΔP校准：

以球机的轴向L和Y轴所构成的平面作为球机局部坐标系的Y－B－Z平面，并将L当作Z轴，计算球机局部坐标系的X、Y轴以得到临时的局部球面坐标系，假设计算所得的PT坐标为(P′_k，T′_k)，实际采集的PT坐标为(P_k，T_k)，则ΔP的计算方法如式(17)所示

采用3D地图，提高了数据采集精度，并且增加了位置点的高度数据，使得球机校准过程中约束条件更充分。而现有技术是基于平面地图采集位置点的位置信息，精度很低，同时缺失位置点的高度信息，其所构造的数学模型缺少必要的约束条件，无法准确地计算出球机的姿态信息。

通过细分校准流程，将球机的位置校准方案细分为粗校准和精校准，权衡了校准的效率与精度。在球机位置精校准方法中，采用了网格细分的方法，实现按网格细分程度控制球机位置的校准精度的目的。而现有的技术方案对球机位置的校准精度低(误差在1米以上)，更不能控制校准精度级别。

此外，倾斜摄影航拍的3D地图代替平面地图，有效弥补以上数据源精度不够以及高度数据缺失的缺陷。并基于3D空间坐标以及PT坐标建立了3D球机位姿模型，最终将球机位置校准误差降低到10厘米以内，球机朝向误差降低到1度以内。如图11所示，基于本提案所采用的校准方法，球机所采集的视频画面能够准确的叠加到3D地图中，有效验证了该方法对球机位姿的校准精度。

总而言之，根据本发明实施例的基于3D地图的球机位置与姿态参数校准方法100，通过获取多个特征点的地图中的坐标和球机的PT坐标，并通过粗算和精算获取球机位置，该方法有效精准地计算出了球机的姿态信息。

另外，本发明实施例还提供了一种计算机存储介质，所述计算机存储介质包括一条或多条计算机指令，所述一条或多条计算机指令在执行时实现上述任一所述的数据处理方法。

也就是说，所述计算机存储介质存储有计算机程序，所述计算机程序被处理器运行时，使得所述处理器执行上述任一所述的数据处理方法。

如图12所示，本发明实施例提供了一种电子设备300，包括存储器310和处理器320，所述存储器310用于存储一条或多条计算机指令，所述处理器320用于调用并执行所述一条或多条计算机指令，从而实现上述任一所述的方法100。

也就是说，电子设备300包括：处理器320和存储器310，在所述存储器310中存储有计算机程序指令，其中，在所述计算机程序指令被所述处理器运行时，使得所述处理器320执行上述任一所述的方法100。

进一步地，如图12所示，电子设备300还包括网络接口330、输入设备340、硬盘350、和显示设备360。

上述各个接口和设备之间可以通过总线架构互连。总线架构可以是可以包括任意数量的互联的总线和桥。具体由处理器320代表的一个或者多个中央处理器(CPU)，以及由存储器310代表的一个或者多个存储器的各种电路连接在一起。总线架构还可以将诸如外围设备、稳压器和功率管理电路等之类的各种其它电路连接在一起。可以理解，总线架构用于实现这些组件之间的连接通信。总线架构除包括数据总线之外，还包括电源总线、控制总线和状态信号总线，这些都是本领域所公知的，因此本文不再对其进行详细描述。

所述网络接口330，可以连接至网络(如因特网、局域网等)，从网络中获取相关数据，并可以保存在硬盘350中。

所述输入设备340，可以接收操作人员输入的各种指令，并发送给处理器320以供执行。所述输入设备340可以包括键盘或者点击设备(例如，鼠标，轨迹球(trackball)、触感板或者触摸屏等。

所述显示设备360，可以将处理器320执行指令获得的结果进行显示。

所述存储器310，用于存储操作系统运行所必须的程序和数据，以及处理器320计算过程中的中间结果等数据。

可以理解，本发明实施例中的存储器310可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(ROM)、可编程只读存储器(PROM)、可擦除可编程只读存储器(EPROM)、电可擦除可编程只读存储器(EEPROM)或闪存。易失性存储器可以是随机存取存储器(RAM)，其用作外部高速缓存。本文描述的装置和方法的存储器310旨在包括但不限于这些和任意其它适合类型的存储器。

在一些实施方式中，存储器310存储了如下的元素，可执行模块或者数据结构，或者他们的子集，或者他们的扩展集：操作系统311和应用程序312。

其中，操作系统311，包含各种系统程序，例如框架层、核心库层、驱动层等，用于实现各种基础业务以及处理基于硬件的任务。应用程序312，包含各种应用程序，例如浏览器(Browser)等，用于实现各种应用业务。实现本发明实施例方法的程序可以包含在应用程序312中。

上述处理器320，当调用并执行所述存储器310中所存储的应用程序和数据，具体的，可以是应用程序312中存储的程序或指令时，S1、根据地图中和球机拍摄的画面中的特征点，获取多个所述特征点对应的地图中的坐标和球机的PT坐标；S2、根据多个所述特征点的坐标计算球机的三维坐标点B；S3、将以所述三维坐标点B为中心，N为边长的正方体划分为M×M×M个区域，计算每个所述区域节点坐标的误差值，并将误差值最小的节点作为预设球机位置；S4、判断所述区域的尺寸是否小于预设阈值，如果是，则将所述预设球机位置作为球机位置B_n，否则，执行步骤S5；S5、将以所述预设球机位置为中心，N/M为边长的正方体划分为M×M×M个区域，计算每个所述区域节点坐标的误差值，并将误差值最小的节点为预设球机位置，执行步骤S4。

本发明上述实施例揭示的方法可以应用于处理器320中，或者由处理器320实现。处理器320可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器320中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器320可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件，可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器310，处理器320读取存储器310中的信息，结合其硬件完成上述方法的步骤。

可以理解的是，本文描述的这些实施例可以用硬件、软件、固件、中间件、微码或其组合来实现。对于硬件实现，处理单元可以实现在一个或多个专用集成电路(ASIC)、数字信号处理器DSP)、数字信号处理设备(DSPD)、可编程逻辑设备(PLD)、现场可编程门阵列(FPGA)、通用处理器、控制器、微控制器、微处理器、用于执行本申请所述功能的其它电子单元或其组合中。

对于软件实现，可通过执行本文所述功能的模块(例如过程、函数等)来实现本文所述的技术。软件代码可存储在存储器中并通过处理器执行。存储器可以在处理器中或在处理器外部实现。

具体地，处理器320还用于读取所述计算机程序，执行上述任一所述的方法。

在本申请所提供的几个实施例中，应该理解到，所揭露方法和装置，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理包括，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。

上述以软件功能单元的形式实现的集成的单元，可以存储在一个计算机可读取存储介质中。上述软件功能单元存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述收发方法的部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，简称ROM)、随机存取存储器(Random Access Memory，简称RAM)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (7)

1.一种基于3D地图的球机位置与姿态参数校准方法，其特征在于，包括以下步骤：

S1、根据地图中和球机拍摄的画面中的特征点，获取多个所述特征点对应的地图中的坐标和球机的PT坐标；

S2、根据多个所述特征点的坐标计算球机的三维坐标点B；

S3、将以所述三维坐标点B为中心，N为边长的正方体划分为M×M×M个区域，划分后得到若干个网格节点，计算每个所述网格节点坐标的误差值，并将误差值最小的节点作为预设球机位置；

S4、判断所述区域的尺寸是否小于预设阈值，如果是，则将所述预设球机位置作为球机位置B_n，否则，执行步骤S5；

S5、将以所述预设球机位置为中心，N/M为边长的正方体划分为M×M×M个区域，划分后得到若干个网格节点，计算每个所述网格节点坐标的误差值，并将误差值最小的节点为预设球机位置，执行步骤S4；

在步骤S1中，获取至少4个所述特征点，每个所述特征点分别记为A_i(X_i，Y_i，Z_i，P_i，T_i)，其中i＝1，2，3…n，其中，X_i，Y_i，Z_i分别为多个所述特征点对应的3D地图中的坐标，P_i，T_i分别为多个所述特征点对应的球机的PT坐标；

步骤S1还包括以下步骤：

S11、令i＝k，k＝1；

S12、调整所述球机的视频画面，当球机画面中心对准一个所述特征点时，记录所述球机的PT坐标P_i，T_i；

S13、在所述3D地图中找到所述特征点，并记录该特征点在所述3D地图中的坐标X_i，Y_i，Z_i；

S14、记录所述特征点的坐标为A_i(X_i，Y_i，Z_i，P_i，T_i)；

S15、i＝k+1，若i＜5，则执行S12；

在步骤S12中，还包括对所述特征点A_i的PT坐标进行校准，球机传感器坐标为(P_i，T_i)，球机角度偏移值为(ΔP_i，ΔT_i)，A_i的真实PT为 所述特征点A_i的PT校准包括以下步骤：

S121、设定ΔT的初始值，ΔT＝0.01度；

S122、将的T值加ΔT，其中k＝i，j，按照式(16)计算误差值η′，如果η′＞η，则执行步骤S123，如果η′＜η，则令η′＝η，并执行步骤S124；

S123、ΔT＝-ΔT，并将的T值加ΔT，其中k＝i，j，按照式(16)计算误差值η′，如果η′＞η，则执行步骤S125，如果η′＜η，则令η′＝η，并执行步骤S124；

S124、ΔT＝2*ΔT，并将的T值加ΔT，其中k＝i，j，按照式(16)计算误差值η′，如果η′＞η，则令ΔT＝ΔT/2，并则执行步骤S125，如果η′＜η，则令η′＝η，并执行步骤S124；

S125、结束，并将所得的η′值当作网格节点可作为球机真实位置的误差值。

2.根据权利要求1所述的基于3D地图的球机位置与姿态参数校准方法，其特征在于，在步骤S2中，利用牛顿迭代法计算球机的三维坐标点B。

3.根据权利要求2所述的基于3D地图的球机位置与姿态参数校准方法，其特征在于，在步骤S3中，N＝1米，M＝4，所述区域为正方体，所述误差值为：

其中，η为误差值，∠A_iB_nA_j和分别为在三维坐标系中和球机坐标系中点Bn与两个点之间的连线形成的夹角，/>表示在球机坐标系中第i个特征点的位置，/>表示在球机坐标系中第j个特征点的位置。

4.根据权利要求3所述的基于3D地图的球机位置与姿态参数校准方法，其特征在于，在步骤S4中，所述误差值小于1cm时，将所述预设球机位置作为球机位置B_n。

5.根据权利要求1所述的基于3D地图的球机位置与姿态参数校准方法，其特征在于，还包括对球机姿态校准：

用ΔT对的T值进行补偿，其中k＝1，2，3，4。

6.根据权利要求1所述的基于3D地图的球机位置与姿态参数校准方法，其特征在于，还包括对球机角度偏移值ΔP校准：

以球机的轴向L和Y轴所构成的平面作为球机局部坐标系的Y-B-Z平面，并将L当作Z轴，计算球机局部坐标系的X、Y轴以得到临时的局部球面坐标系，假设计算所得的PT坐标为(P′_k，T′_k)，实际采集的PT坐标为(P_k，T_k)，则ΔP的计算方法如式(17)所示

7.一种计算机存储介质，其特征在于，包括一条或多条计算机指令，所述一条或多条计算机指令在执行时实现如权利要求1-6中任一项所述的方法。