CN111862203A - 基于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坐标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。

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

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

其中，η为误差值，∑∠A_iB_nA_j分别为在三维坐标系中和球机坐标系中点Bn与两个点之间的连线形成的夹角。

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

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

所述特征点A_i的PT校准包括以下步骤： S131、设定ΔT的初始值，ΔT＝0.01度；S132、将

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

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

的T值加ΔT，按照式(16)计算 误差值η'，如果η'＞η，则令ΔT＝ΔT/2，并则执行步骤S135，如果η'＜η，则 令η'＝η，并执行步骤S134；S135、结束，并将所得的η'值当作网格节点可作 为球机真实位置的误差值。

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

3，4)的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_iX_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点的初始迭代值B₀， 并以||B_n+1-B_n||＜f为迭代终止条件，其中，f为某一迭代阈值。

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

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

其中，η为误差值，∑∠A_iB_nA_j分别为在三维坐标系中和在球机坐标系中点 Bn与两个点之间的连线形成的夹角。

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

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

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

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

所述特征点A_i的PT校准包括以下步骤：

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

S132、将

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

S133、ΔT＝-ΔT，并将

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

S134、ΔT＝2*ΔT，并将

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

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

需要说明的是，由于球机制造以及安装导致的球机真实的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对

(k＝1，2，3，4)的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 (10)

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

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

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

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

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

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

2.根据权利要求1所述的基于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坐标。

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

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

其中，η为误差值，∑∠A_iB_nA_j分别为在三维坐标系中和球机坐标系中点Bn与两个点之间的连线形成的夹角。

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

7.根据权利要求5所述的基于3D地图的球机位置与姿态参数校准方法，其特征在于，在步骤S13中，还包括对所述特征点A_i的PT坐标进行校准，球机传感器坐标为(P_i，T_i)，球机角度偏移值为(ΔP_i，ΔT_i)，A_i的真实PT为

所述特征点A_i的PT校准包括以下步骤：

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

S132、将

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

S133、ΔT＝-ΔT，并将

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

S134、ΔT＝2*ΔT，并将

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

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

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

用ΔT对

的T值进行补偿。

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

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

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

Images