CN117705097A - 一种棱镜杆装置、地物碎部点测量方法、设备及介质 - Google Patents

Abstract

本发明公开一种棱镜杆装置、地物碎部点测量方法、设备及介质，方法包括：利用六位置法对棱镜杆装置的MEMS进行惯性误差标定；将棱镜杆装置设于已知控制点，确定其姿态角并转化为初始姿态矩阵，控制其开始移动，根据每一时刻闭环校正的观测数据，利用机械编排算法获取导航参数并根据MEMS中心与棱镜中心的距离进行校正；将校正导航参数误差和惯性误差作为状态量，以MEMS在已知控制点或待测碎部点静止时的零速伪观测值、磁力计计算的姿态角和已知控制点位置信息作为观测值，利用扩展卡尔曼滤波算法进行状态估计，根据状态估计结果，利用RTS平滑滤波算法获得待测碎部点位置信息。本发明无需满足棱镜与全站仪通视便实现地物碎部点测量。

Description

一种棱镜杆装置、地物碎部点测量方法、设备及介质

技术领域

本发明涉及测绘技术领域，尤其是涉及一种棱镜杆装置、地物碎部点测量方法、终端设备及计算机可读存储介质。

背景技术

在现有技术中，城市复杂环境下进行碎部点测量的技术手段通常包括全站仪测量和RTK测量。其中，全站仪测量是通过已知点坐标进行待测碎部点坐标推算，RTK测量是通过对GNSS卫星信号进行实时差分以获取待测碎部点的坐标。然而，在建筑物较为密集的场景下进行全站仪测量时，为了满足通视条件和碎部点的测量，通常需要在较短距离内布设多个测站，影响测量效率的同时，会造成较大的碎部点测量误差。而RTK测量容易受环境影响，在建筑物较为密集的场景下通常无法进行模糊度固定，从而无法准确地进行碎部点测量。

发明内容

本发明提供一种棱镜杆装置、地物碎部点测量方法、设备及介质，通过将基于惯性技术的九轴MEMS传感器与全站仪光电测距技术融合，使得地物碎部点测量不受测量环境的影响，无需满足棱镜与全站仪通视便能够实现地物碎部点的测量，有助于减少全站仪的布设数量，显著提高了地物碎部点的测量效率和测量精度。

为了解决上述技术问题，本发明实施例第一方面提供一种棱镜杆装置，包括棱镜杆、棱镜和九轴MEMS传感器；

所述棱镜设于所述棱镜杆的顶部，所述九轴MEMS传感器设于所述棱镜杆上，所述九轴MEMS传感器包括三轴陀螺仪、三轴加速度计和三轴磁力计。

作为优选方案，所述棱镜杆装置还包括第一连接杆和第二连接杆；所述第一连接杆的一端和所述第二连接杆的一端均设于所述棱镜杆上，所述第一连接杆的另一端和所述第二连接杆的另一端均与所述九轴MEMS传感器连接。

作为优选方案，所述棱镜杆装置还包括电源模块；所述电源模块的供电端与所述九轴MEMS传感器的受电端连接。

本发明实施例第二方面提供一种地物碎部点测量方法，应用如第一方面任一项所述的棱镜杆装置，包括如下步骤：

利用六位置法对棱镜杆装置的九轴MEMS传感器进行惯性误差标定，确定若干标定惯性误差；

将所述棱镜杆装置设置于预设的已知控制点，基于所述已知控制点的位置信息，通过所述九轴MEMS传感器确定所述棱镜杆装置在所述已知控制点的姿态角，并将所述姿态角转化为初始姿态矩阵；

控制所述棱镜杆装置从所述已知控制点开始移动，通过所述九轴MEMS传感器检测移动过程中每一时刻的比力值、角速度值和磁感应强度值，并根据所述初始姿态矩阵、每一时刻的闭环校正后的比力值、角速度值和磁感应强度值，利用机械编排算法获取所述棱镜杆装置每一时刻的导航参数；其中，初始时刻的比力值、角速度值和磁感应强度值通过消除标定的惯性误差以实现闭环校正，非初始时刻的比力值、角速度值和磁感应强度值通过消除上一时刻的滤波估计的惯性误差以实现闭环校正；

根据所述九轴MEMS传感器的中心与所述棱镜杆装置的棱镜中心之间的距离，对每一时刻的导航参数进行校正，获得每一时刻的校正导航参数；

将每一时刻的校正导航参数误差和惯性误差作为状态量，以所述九轴MEMS传感器在所述已知控制点或待测碎部点静止时所获取的零速伪观测值、所述九轴MEMS传感器的三轴磁力计所计算的姿态角和所述已知控制点的位置信息作为观测值，利用扩展卡尔曼滤波算法获得每一时刻的状态矩阵、状态协方差矩阵和状态转移矩阵；

根据每一时刻的状态矩阵、状态协方差矩阵和状态转移矩阵，利用RTS平滑滤波算法获得所述待测碎部点的位置信息。

作为优选方案，所述基于所述已知控制点的位置信息，通过所述九轴MEMS传感器确定所述棱镜杆装置在所述已知控制点的姿态角，具体包括如下步骤：

基于所述已知控制点的位置信息，通过所述九轴MEMS传感器的三轴加速度计确定所述棱镜杆装置在所述已知控制点的俯仰角和横滚角；

通过所述九轴MEMS传感器的三轴磁力计确定所述棱镜杆装置在所述已知控制点的磁偏角，并根据所述磁偏角获得地理坐标系的航向角；

根据所述俯仰角、所述横滚角和所述航向角，确定所述棱镜杆装置在所述已知控制点的姿态角。

作为优选方案，所述根据所述初始姿态矩阵、每一时刻的闭环校正后的比力值、角速度值和磁感应强度值，利用机械编排算法获取所述棱镜杆装置每一时刻的导航参数，具体包括如下步骤：

根据第k-1时刻的姿态矩阵、从第k-1时刻至第k时刻的导航坐标系变化矩阵和载体坐标系变化矩阵的乘积，获得第k时刻的姿态矩阵；其中，k为大于0的整数；当k＝1时，第k-1时刻的姿态矩阵为所述初始姿态矩阵；

根据第k-1时刻的姿态矩阵、第k-1时刻的闭环校正后的比力值与角速度值、第k时刻的闭环校正后的比力值与角速度值，确定第k时刻的比力速度增量和哥氏速度增量，并根据第k-1时刻的速度值、第k时刻的比力速度增量和哥氏速度增量之和，获得第k时刻的速度值；

根据第k时刻的速度值和第k-1时刻的速度值，获取第k时刻的所述棱镜杆装置的位置信息；

根据所述棱镜杆装置每一时刻的姿态矩阵、速度值和位置信息，确定所述棱镜杆装置每一时刻的导航参数。

作为优选方案，所述利用六位置法对棱镜杆装置的九轴MEMS传感器进行惯性误差标定，确定若干标定惯性误差，具体包括如下步骤：

将所述九轴MEMS传感器的六个平面分别朝上并静止，获得一组三轴加速度计的输出值、一组三轴陀螺仪的输出值和一组三轴磁力计的输出值；

基于获得的所述三轴加速度计的输出值，利用高斯牛顿迭代法求解如下表达式，获得加速度计惯性误差：

其中，θ^a表示加速度计惯性误差；k表示时刻；g表示当地重力加速度；h(a^s,θ^a)表示三轴加速度计的输出值经过误差矫正的表达式；所述加速度计惯性误差包括加速度计零偏误差、加速度计安装角误差和加速度计比例因子误差；

基于获得的所述三轴加速度计的输出值和所述三轴陀螺仪的输出值，利用高斯牛顿迭代法求解如下表达式，获得陀螺仪惯性误差：

其中，θ^gry表示陀螺仪惯性误差；表示经过误差矫正的第k时刻的三轴加速度计的输出值；μ_k表示经过姿态变换的加速度值；所述陀螺仪惯性误差包括陀螺仪零偏误差、陀螺仪安装角误差和陀螺仪比例因子误差；

基于获得的所述三轴磁力计的输出值，利用高斯牛顿迭代法求解如下表达式，获得磁力计惯性误差：

其中，θ^m表示磁力计惯性误差；m表示当地磁场强度；h(m^s,θ^m)表示三轴磁力计的输出值经过误差矫正的表达式；所述磁力计惯性误差包括磁力计零偏误差、磁力计安装角误差和磁力计比例因子误差。

本发明实施例第三方面提供一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如第二方面任一项所述的地物碎部点测量方法。

本发明实施例第四方面提供一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如第二方面任一项所述的地物碎部点测量方法。

相比于现有技术，本发明实施例的有益效果在于，通过将基于惯性技术的九轴MEMS传感器与全站仪光电测距技术融合，使得地物碎部点测量不受测量环境的影响，无需满足棱镜与全站仪通视便能够实现地物碎部点的测量，有助于减少全站仪的布设数量，显著提高了地物碎部点的测量效率和测量精度。

附图说明

图1是本发明实施例中的棱镜杆装置的结构示意图；

图2是本发明实施例中的地物碎部点测量方法的流程示意图；

图3是本发明实施例中的九轴MEMS处理器的数据处理架构图；

其中，1、棱镜杆；2、棱镜；3、九轴MEMS传感器；4、第一连接杆；5、第二连接杆。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参见图1，本发明实施例第一方面提供一种棱镜杆装置，包括棱镜杆1、棱镜2和九轴MEMS传感器3；

所述棱镜2设于所述棱镜杆1的顶部，所述九轴MEMS传感器3设于所述棱镜杆1上，所述九轴MEMS传感器3包括三轴陀螺仪、三轴加速度计和三轴磁力计。

值得说明的是，通过设于棱镜杆1顶部的棱镜2，可以利用全站仪光电测距技术实现地物碎部点的测量，当需要在建筑物较为密集的场景下进行地物碎部点测量时，可以基于已知控制点的位置信息，利用基于惯性技术的九轴MEMS传感器3进行地物碎部点测量，使得地物碎部点测量不受测量环境的影响，无需满足棱镜2与全站仪通视便能够实现地物碎部点的测量，有助于减少全站仪的布设数量，显著提高了地物碎部点的测量效率和测量精度。

作为优选方案，所述棱镜杆装置还包括第一连接杆4和第二连接杆5；所述第一连接杆4的一端和所述第二连接杆5的一端均设于所述棱镜杆1上，所述第一连接杆4的另一端和所述第二连接杆5的另一端均与所述九轴MEMS传感器3连接。

值得说明的是，本实施例利用第一连接杆4和第二连接杆5形成双层固定结构，从而能够提高九轴MEMS传感器3设于棱镜杆1上的稳固性，减弱九轴MEMS传感器3本身的震动。

作为其中一种可选的实施例，第一连接杆4和第二连接杆5均可拆卸地与棱镜杆1、九轴MEMS传感器3连接。优选地，第一连接杆4和第二连接杆5的材质为轻盈且坚硬的铝合金。

作为优选方案，所述棱镜杆装置还包括电源模块；所述电源模块的供电端与所述九轴MEMS传感器3的受电端连接。

本发明实施例提供的一种棱镜杆装置，通过在棱镜杆上设置九轴MEMS传感器，实现将基于惯性技术的九轴MEMS传感器与全站仪光电测距技术融合，使得地物碎部点测量不受测量环境的影响，无需满足棱镜与全站仪通视便能够实现地物碎部点的测量，有助于减少全站仪的布设数量，显著提高了地物碎部点的测量效率和测量精度。

参见图2和图3，本发明实施例第二方面提供一种地物碎部点测量方法，应用如第一方面任一实施例所述的棱镜杆装置，包括如下步骤S1至步骤S6：

步骤S1，利用六位置法对棱镜杆装置的九轴MEMS传感器进行惯性误差标定，确定若干标定惯性误差；

步骤S2，将所述棱镜杆装置设置于预设的已知控制点，基于所述已知控制点的位置信息，通过所述九轴MEMS传感器确定所述棱镜杆装置在所述已知控制点的姿态角，并将所述姿态角转化为初始姿态矩阵；

步骤S3，控制所述棱镜杆装置从所述已知控制点开始移动，通过所述九轴MEMS传感器检测移动过程中每一时刻的比力值、角速度值和磁感应强度值，并根据所述初始姿态矩阵、每一时刻的闭环校正后的比力值、角速度值和磁感应强度值，利用机械编排算法获取所述棱镜杆装置每一时刻的导航参数；其中，初始时刻的比力值、角速度值和磁感应强度值通过消除标定的惯性误差以实现闭环校正，非初始时刻的比力值、角速度值和磁感应强度值通过消除上一时刻的滤波估计的惯性误差以实现闭环校正；

步骤S4，根据所述九轴MEMS传感器的中心与所述棱镜杆装置的棱镜中心之间的距离，对每一时刻的导航参数进行校正，获得每一时刻的校正导航参数；

步骤S5，将每一时刻的校正导航参数误差和惯性误差作为状态量，以所述九轴MEMS传感器在所述已知控制点或待测碎部点静止时所获取的零速伪观测值、所述九轴MEMS传感器的三轴磁力计所计算的姿态角和所述已知控制点的位置信息作为观测值，利用扩展卡尔曼滤波算法获得每一时刻的状态矩阵、状态协方差矩阵和状态转移矩阵；

步骤S6，根据每一时刻的状态矩阵、状态协方差矩阵和状态转移矩阵，利用RTS平滑滤波算法获得所述待测碎部点的位置信息。

具体地，将棱镜杆装置设置于预设的已知控制点，静止一段时间，如4s，通过静止时间长短来区分棱镜杆位于碎部点还是控制点，其中，已知控制点的位置信息为通过RTK测量或全站仪测量所获得，通过九轴MEMS传感器采集每一时刻的观测数据：比力值、角速度值和磁感应强度值，本实施例通过九轴MEMS传感器的静止状态来实现所测地物的标定，同时，静止时间内，所测量的比力值理论上约等于零，角速度值理论上等于地球自转角速度，增加了多个伪观测值，从而可有效抑制九轴MEMS传感器的误差累计。

在采集观测数据后，需要进行MEMS数据处理，首先需要进行MEMS误差标定及姿态初始化。

对于MEMS误差标定，优选地，所述利用六位置法对棱镜杆装置的九轴MEMS传感器进行惯性误差标定，确定若干标定惯性误差，具体包括如下步骤：

将所述九轴MEMS传感器的六个平面分别朝上并静止，获得一组三轴加速度计的输出值、一组三轴陀螺仪的输出值和一组三轴磁力计的输出值；

基于获得的所述三轴加速度计的输出值，利用高斯牛顿迭代法求解如下表达式，获得加速度计惯性误差：

其中，θ^a表示加速度计惯性误差；k表示时刻；g表示当地重力加速度；h(a^s,θ^a)表示三轴加速度计的输出值经过误差矫正的表达式；所述加速度计惯性误差包括加速度计零偏误差、加速度计安装角误差和加速度计比例因子误差；

基于获得的所述三轴加速度计的输出值和所述三轴陀螺仪的输出值，利用高斯牛顿迭代法求解如下表达式，获得陀螺仪惯性误差：

其中，θ^gry表示陀螺仪惯性误差；a_k ^s表示经过误差矫正的第k时刻的三轴加速度计的输出值；μ_k表示经过姿态变换的加速度值；所述陀螺仪惯性误差包括陀螺仪零偏误差、陀螺仪安装角误差和陀螺仪比例因子误差；

基于获得的所述三轴磁力计的输出值，利用高斯牛顿迭代法求解如下表达式，获得磁力计惯性误差：

其中，θ^m表示磁力计惯性误差；m表示当地磁场强度；h(m^s,θ^m)表示三轴磁力计的输出值经过误差矫正的表达式；所述磁力计惯性误差包括磁力计零偏误差、磁力计安装角误差和磁力计比例因子误差。

具体地，九轴MEMS传感器是一种封闭的自主位姿传递传感器，受外界干扰较小，在短时间内能够实现载***姿的高精度传递。基于九轴MEMS传感器进行位姿传递，首先需要对九轴MEMS传感器进行初始化，初始化包括两部分：MEMS惯性误差标定和姿态初始化。

九轴MEMS传感器的惯性误差包括了零偏误差、比例因子误差、安装角误差，惯性误差的标定一般在室内完成。考虑到成本与实用性，MEMS惯性误差的标定不借助任何外部工具，其中三轴加速度计的标定是以当地重力加速度进行约束，三轴陀螺仪的标定是以加速度计与姿态角进行约束，三轴磁力计的标定是以地球磁场量进行标定。传感器噪声模型通过功率谱密度确定，噪声的功率谱密度通过Allan方差标定。

MEMS惯性误差具体采用六位置法标定：首先将九轴MEMS传感器的六个面分别朝上连续静止放置一段时间，得到一组连续的加速度计输出数据、陀螺仪输出数据、磁力计输出数据。首先标定的精度依赖于MEMS静止和运动状态的探测，这可通过三轴加速度计的观测数据的协方差进行MEMS运动状态的探测，而后基于探测的MEMS静止时间段，首先标定加速度计。定义加速度计惯性误差为θ^a，包括了加速度计零偏误差加速度计安装角误差[α_yz α_zy α_zx]^T、加速度计比例因子误差/>即可表示为：

其中，三轴加速度计的输出值经过误差矫正的表达式h(a^s,θ^a)，表示如下：

当九轴MEMS传感器静止时，三轴加速度计输出值等于当地重力加速度，当地重力加速度可通过重力模型计算得到，既有如下准则：

利用高斯牛顿迭代法求解便获得加速度计惯性误差。

MEMS陀螺仪标定与加速计不同，由于MEMS陀螺仪精度较低，无法感知地球自转角速度，所以陀螺仪的标定需要基于加速度计的标定结果。首先陀螺仪的零偏误差可通过静止时间段内的陀螺仪输出数据分段平均获得，将原始陀螺仪输出角速度减去零偏误差即可以获得仅包含安装角误差和比例因子误差的观测数据。MEMS在不同平面朝上过程中包含了姿态变化，则有：

分别表示k、k-1时刻经过误差矫正的三轴加速度计的输出值，q表示姿态四元数，为保证姿态精度，采用四阶Runge-Kutta积分算法，μ_k为经过姿态变换的加速度值，经过姿态变换后的加速度值等于经过误差矫正的三轴加速度计的输出值，则有准则：

利用高斯牛顿迭代法求解便获得陀螺仪惯性误差。

磁力计误差标定与加速度计标定方法一致，首先定义磁力计的零偏误差安装角误差/>和比例因子误差/>则可表达为：

则三轴磁力计的输出值经过误差矫正的表达式h(m^s,θ^m)，表示如下：

三轴磁力计输出值等于当地磁场强度，通过如下准则，利用高斯牛顿迭代法即可获得磁力计惯性误差：

基于MEMS的原始输出，分别计算加速度计、陀螺仪、磁力计的Allan方差，而后计算Allan方差的双对数曲线，根据双对数曲线获得不同传感器的噪声功率谱密度。

对于姿态初始化，姿态初始化是确定载体在起始位置的相对于地理坐标系的姿态角(俯仰角、横滚角和航向角)，在本实施例中，优选地，所述基于所述已知控制点的位置信息，通过所述九轴MEMS传感器确定所述棱镜杆装置在所述已知控制点的姿态角，具体包括如下步骤：

基于所述已知控制点的位置信息，通过所述九轴MEMS传感器的三轴加速度计确定所述棱镜杆装置在所述已知控制点的俯仰角和横滚角；

通过所述九轴MEMS传感器的三轴磁力计确定所述棱镜杆装置在所述已知控制点的磁偏角，并根据所述磁偏角获得地理坐标系的航向角；

根据所述俯仰角、所述横滚角和所述航向角，确定所述棱镜杆装置在所述已知控制点的姿态角。

值得说明的是，在初始化航向角时，由于陀螺仪精度较差，需要借助磁力计，磁力计计算得到磁偏角，然后由磁偏角获得地理坐标系的航向角，再将姿态角转换为初始姿态方向余弦矩阵，作为初始姿态矩阵。

进一步地，根据初始姿态矩阵、每一时刻的闭环校正后的比力值、角速度值和磁感应强度值，利用机械编排算法获取棱镜杆装置每一时刻的导航参数，优选地，其具体包括如下步骤：

根据第k-1时刻的姿态矩阵、从第k-1时刻至第k时刻的导航坐标系变化矩阵和载体坐标系变化矩阵的乘积，获得第k时刻的姿态矩阵；其中，k为大于0的整数；当k＝1时，第k-1时刻的姿态矩阵为所述初始姿态矩阵；

根据第k-1时刻的姿态矩阵、第k-1时刻的闭环校正后的比力值与角速度值、第k时刻的闭环校正后的比力值与角速度值，确定第k时刻的比力速度增量和哥氏速度增量，并根据第k-1时刻的速度值、第k时刻的比力速度增量和哥氏速度增量之和，获得第k时刻的速度值；

根据第k时刻的速度值和第k-1时刻的速度值，获取第k时刻的所述棱镜杆装置的位置信息；

根据所述棱镜杆装置每一时刻的姿态矩阵、速度值和位置信息，确定所述棱镜杆装置每一时刻的导航参数。

具体地，MEMS的机械编排是指根据加速度计和陀螺仪输出值来获取载体当前时刻相对于前一时刻的姿态信息、速度信息和位置信息，初始姿态的方向余弦矩阵已通过姿态初始化获取。

首先是基于陀螺仪输出值的姿态更新，即获得载体坐标系b相对于导航坐标系n(东北地坐标系)下k时刻的方向余弦矩阵该方向余弦矩阵等于k-1到k时刻n系的变化k-1时刻的方向余弦矩阵/>和载体坐标系变化/>的乘积，即：

φ_k为b系下的等效旋转矢量，φ_k×为其反对称矩阵，基于双子样假设下可表达为：

Δθ_k、θ_k-1分别为不同时刻下的角速度积分；

ζ_k为n系下的等效旋转矢量，ζ_k×为其反对称矩阵，可表达为：

为地球自转角速度在n下的投影，/>为n系下的牵连角速度，Δt＝t(k)-t(k-1)。

速度更新是基于姿态更新和加速度计输出值来估计载体的速度。当前时刻k的速度可表达为k-1的速度加速度(比力)速度增量/>以及哥氏速度增量/>之和，即

其中哥氏速度增量可表示为：

式中gⁿ是导航系下的重力加速度，为地球自转角速度，/>为牵连角速度；

而比力速度增量表示为：

I为单位矩阵，ζ_n(k-1),n(k)为n系的角速度变化，×为反对称运算，为k-1时刻的方向余弦矩阵，/>为比力速度项，其中角速度变化ζ_n(k-1),n(k)可表示为：

双子样假设下的比力速度增量等于：

式中Δv是比力积分项，Δθ为角速度积分项，分别表示如下：

位置更新是对载体的速度进行积分的结果，基于大地坐标椭球高h、纬度经度λ的位置更新算法公式可如下表示。t_k时刻的椭球高h_k可表示为：

式中h_k-1为t_k-1时刻的高程，v_D,k-1、v_D,k分别为t_k-1时刻、t_k时刻的地向速度。

对于纬度更新，忽略积分区间内子午圈和高程的变化，t_k时刻的纬度表示为；

式中，是t_k-1时刻的纬度，v_N,k、v_N,k-1分别是t_k、t_k-1的北向速度，R_M,k-1是t_k-1时刻的子午圈半径，/>为t_k、t_k-1的平均高程，即/>

同理可得经度更新算法，则有t_k时刻的经度λ_k：

λ_k-1为t_k-1时刻的经度，v_E,k、v_E,k-1分别是t_k、t_k-1的东向速度，R_N,k-1/2是中间时刻的卯酉圈半径，是t_k与t_k-1时刻的平均纬度，即/>

在进行机械编排前，需要从加速度计和陀螺仪的原始观测数据中消去标定或滤波估计的惯性误差以实现闭环校正，从而减弱误差累积。

进一步地，由于九轴MEMS传感器固定于棱镜杆上，与棱镜的中心不重合，需要进行九轴MEMS传感器与棱镜之间的杆臂校正，杆臂长度即为九轴MEMS传感器的中心与棱镜中心之间的距离，其可通过钢尺量取，而后将杆臂投影到MEMS的载体坐标系下，将相对于MEMS中心的导航参数校正至棱镜中心，获得每一时刻的校正导航参数。

进一步地，扩展卡尔曼滤波算法是一种非线性滤波估计算法，通过卡尔曼增益来平衡观测矩阵与状态矩阵，从而获得最佳状态估计。基于扩展卡尔曼滤波进行状态估计，首先确定状态方程与观测方程。

扩展卡尔曼滤波算法的状态量包括：位置误差δr、速度误差δv、姿态角误差φ、陀螺仪零偏误差b_g、加速度计零偏误差b_a、陀螺仪比例因子误差s_g、加速度计比例因子误差s_a，其中，陀螺仪零偏误差b_g、加速度计零偏误差b_a、陀螺仪比例因子误差s_g、加速度计比例因子误差s_a以及其噪声功率谱密度已通过误差标定获取，值得说明的是，当前时刻滤波估计的惯性误差用于下一时刻的机械编排过程使用，因此状态矩阵表示为：

状态方程可表示为：

δx_k＝Φ_k/k-1δx_k-1+ω_k-1

Φ_k/k-1＝I+F(t_k-1)Δt

式中Φ_k/k-1为状态转移矩阵，ω_k-1为状态矩阵的噪声，F(t_k-1)为t_k-1时刻状态量的系数矩阵。

扩展卡尔曼滤波算法的观测量包括：九轴MEMS传感器的三轴磁力计所检测的姿态角、零速伪观测值的速度以及角速度/>控制点坐标r_z，其中零速伪观测值根据九轴MEMS传感器在所述已知控制点或待测碎部点静止时所获取，本实施例根据静止时间长度来对已知控制点与待测碎部点进行标定，本发明实施例采用原始输出的三轴加速度阈值小于0.5m/s²来实现对棱镜杆装置的零速探测。

姿态角观测方程如下：

零速伪速度值观测方程如下：

零速伪角速度值观测方程如下：

控制点坐标r_z等于MEMS的坐标rⁿ加上杆臂l_b的校正，则有基于控制点坐标的观测方程；

式中为方向余弦矩阵，G^-1是将东北地坐标系下的位置转换到地理坐标系，可表示为；

式中，R_M、R_N、h、分别是子午圈半径、卯酉圈半径、椭球高以及纬度值。/>

总的观测方程为：

Z＝Hδx_k+R_z

观测矩阵为：

噪声矩阵为：

观测值的噪声q_m为磁力计的噪声项，q_vv为速度噪声项，q_va为角速度噪声项，q_r为控制点噪声项。利用观测方程与状态方程，再基于扩展卡尔曼滤波算法进行载体状态估计。整个滤波更新算法可表达为时间更新与量测更新的过程，首先是时间更新，即进行状态和状态协方差预测，时间更新可表示为：

δx_k/k-1＝Φ_k/k-1δx_k-1

式中Q_p为状态协方差矩阵的噪声项，其他符号的含义如前所述。

测量更新可表示为；

δx_k＝δx_k/k-1+K_k(z_k-H_kδx_k/k-1)

上式中K_k是增益矩阵，P是状态量的协方差，H是观测方程的系数矩阵，R是观测值的噪声，z_k为观测量，其他符号的含义如前所述。进行滤波的同时，需要将状态矩阵、状态协方差矩阵、状态转移矩阵进行存储，为RTS(Rauch-Tung-Striebel，固定点容积平滑滤波)平滑滤波做准备。

进一步地，为进一步提升载体状态估计精度，本实施例采用RTS固定区间平滑滤波算法，等效于前向滤波与后向滤波的组合。首先基于扩展卡尔曼滤波获得的状态矩阵、状态协方差矩阵、状态转移矩阵，从观测数据的次最后历元向前滤波，最终估计得到载体所处的地物位置信息。

δx_k|n＝δx_k|k+T_k(δx_k+1|n-δx_k+1|k)

/>

式中，n表示总观测历元数，k＝n-1,n-2,…,1为历元观测时刻，T_k为k时刻状态量的增益矩阵，P为状态量的协方差矩阵，Φ为状态量的系数矩阵。

最终，当棱镜杆装置移动至待测碎部点时，控制棱镜杆装置静止，如2s，从而实现棱镜杆装置所测碎部点的标定，通过九轴MEMS传感器数据处理后输出待测碎部点的位置信息。

本发明实施例提供的一种地物碎部点测量方法，通过将基于惯性技术的九轴MEMS传感器与全站仪光电测距技术融合，使得地物碎部点测量不受测量环境的影响，无需满足棱镜与全站仪通视便能够实现地物碎部点的测量，有助于减少全站仪的布设数量，显著提高了地物碎部点的测量效率和测量精度。

本发明实施例第三方面提供一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如第二方面任一实施例所述的地物碎部点测量方法。

所述终端设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述终端设备可包括，但不仅限于，处理器、存储器。所述终端设备还可以包括输入输出设备、网络接入设备、总线等。

所述处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述终端设备的控制中心，利用各种接口和线路连接整个终端设备的各个部分。

所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述终端设备的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作***、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

本发明实施例第四方面提供一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如第二方面任一实施例所述的地物碎部点测量方法。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到本发明可借助软件加必需的硬件平台的方式来实现，当然也可以全部通过硬件来实施。基于这样的理解，本发明的技术方案对背景技术做出贡献的全部或者部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例或者实施例的某些部分所述的方法。

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

Claims (9)

1.一种棱镜杆装置，其特征在于，包括棱镜杆、棱镜和九轴MEMS传感器；

所述棱镜设于所述棱镜杆的顶部，所述九轴MEMS传感器设于所述棱镜杆上，所述九轴MEMS传感器包括三轴陀螺仪、三轴加速度计和三轴磁力计。

2.如权利要求1所述的棱镜杆装置，其特征在于，所述棱镜杆装置还包括第一连接杆和第二连接杆；所述第一连接杆的一端和所述第二连接杆的一端均设于所述棱镜杆上，所述第一连接杆的另一端和所述第二连接杆的另一端均与所述九轴MEMS传感器连接。

3.如权利要求1所述的棱镜杆装置，其特征在于，所述棱镜杆装置还包括电源模块；所述电源模块的供电端与所述九轴MEMS传感器的受电端连接。

4.一种地物碎部点测量方法，应用如权利要求1至3任一项所述的棱镜杆装置，其特征在于，包括如下步骤：

利用六位置法对棱镜杆装置的九轴MEMS传感器进行惯性误差标定，确定若干标定惯性误差；

将所述棱镜杆装置设置于预设的已知控制点，基于所述已知控制点的位置信息，通过所述九轴MEMS传感器确定所述棱镜杆装置在所述已知控制点的姿态角，并将所述姿态角转化为初始姿态矩阵；

控制所述棱镜杆装置从所述已知控制点开始移动，通过所述九轴MEMS传感器检测移动过程中每一时刻的比力值、角速度值和磁感应强度值，并根据所述初始姿态矩阵、每一时刻的闭环校正后的比力值、角速度值和磁感应强度值，利用机械编排算法获取所述棱镜杆装置每一时刻的导航参数；其中，初始时刻的比力值、角速度值和磁感应强度值通过消除标定的惯性误差以实现闭环校正，非初始时刻的比力值、角速度值和磁感应强度值通过消除上一时刻的滤波估计的惯性误差以实现闭环校正；

根据所述九轴MEMS传感器的中心与所述棱镜杆装置的棱镜中心之间的距离，对每一时刻的导航参数进行校正，获得每一时刻的校正导航参数；

将每一时刻的校正导航参数误差和惯性误差作为状态量，以所述九轴MEMS传感器在所述已知控制点或待测碎部点静止时所获取的零速伪观测值、所述九轴MEMS传感器的三轴磁力计所计算的姿态角和所述已知控制点的位置信息作为观测值，利用扩展卡尔曼滤波算法获得每一时刻的状态矩阵、状态协方差矩阵和状态转移矩阵；

根据每一时刻的状态矩阵、状态协方差矩阵和状态转移矩阵，利用RTS平滑滤波算法获得所述待测碎部点的位置信息。

5.如权利要求4所述的地物碎部点测量方法，其特征在于，所述基于所述已知控制点的位置信息，通过所述九轴MEMS传感器确定所述棱镜杆装置在所述已知控制点的姿态角，具体包括如下步骤：

基于所述已知控制点的位置信息，通过所述九轴MEMS传感器的三轴加速度计确定所述棱镜杆装置在所述已知控制点的俯仰角和横滚角；

通过所述九轴MEMS传感器的三轴磁力计确定所述棱镜杆装置在所述已知控制点的磁偏角，并根据所述磁偏角获得地理坐标系的航向角；

根据所述俯仰角、所述横滚角和所述航向角，确定所述棱镜杆装置在所述已知控制点的姿态角。

6.如权利要求4所述的地物碎部点测量方法，其特征在于，所述根据所述初始姿态矩阵、每一时刻的闭环校正后的比力值、角速度值和磁感应强度值，利用机械编排算法获取所述棱镜杆装置每一时刻的导航参数，具体包括如下步骤：

根据第k-1时刻的姿态矩阵、从第k-1时刻至第k时刻的导航坐标系变化矩阵和载体坐标系变化矩阵的乘积，获得第k时刻的姿态矩阵；其中，k为大于0的整数；当k＝1时，第k-1时刻的姿态矩阵为所述初始姿态矩阵；

根据第k-1时刻的姿态矩阵、第k-1时刻的闭环校正后的比力值与角速度值、第k时刻的闭环校正后的比力值与角速度值，确定第k时刻的比力速度增量和哥氏速度增量，并根据第k-1时刻的速度值、第k时刻的比力速度增量和哥氏速度增量之和，获得第k时刻的速度值；

根据第k时刻的速度值和第k-1时刻的速度值，获取第k时刻的所述棱镜杆装置的位置信息；

根据所述棱镜杆装置每一时刻的姿态矩阵、速度值和位置信息，确定所述棱镜杆装置每一时刻的导航参数。

7.如权利要求4所述的地物碎部点测量方法，其特征在于，所述利用六位置法对棱镜杆装置的九轴MEMS传感器进行惯性误差标定，确定若干标定惯性误差，具体包括如下步骤：

将所述九轴MEMS传感器的六个平面分别朝上并静止，获得一组三轴加速度计的输出值、一组三轴陀螺仪的输出值和一组三轴磁力计的输出值；

基于获得的所述三轴加速度计的输出值，利用高斯牛顿迭代法求解如下表达式，获得加速度计惯性误差：

其中，θ^a表示加速度计惯性误差；k表示时刻；g表示当地重力加速度；h(a^s,θ^a)表示三轴加速度计的输出值经过误差矫正的表达式；所述加速度计惯性误差包括加速度计零偏误差、加速度计安装角误差和加速度计比例因子误差；

基于获得的所述三轴加速度计的输出值和所述三轴陀螺仪的输出值，利用高斯牛顿迭代法求解如下表达式，获得陀螺仪惯性误差：

其中，θ^gry表示陀螺仪惯性误差；表示经过误差矫正的第k时刻的三轴加速度计的输出值；μ_k表示经过姿态变换的加速度值；所述陀螺仪惯性误差包括陀螺仪零偏误差、陀螺仪安装角误差和陀螺仪比例因子误差；

基于获得的所述三轴磁力计的输出值，利用高斯牛顿迭代法求解如下表达式，获得磁力计惯性误差：

其中，θ^m表示磁力计惯性误差；m表示当地磁场强度；h(m^s,θ^m)表示三轴磁力计的输出值经过误差矫正的表达式；所述磁力计惯性误差包括磁力计零偏误差、磁力计安装角误差和磁力计比例因子误差。

8.一种终端设备，其特征在于，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求4至7任一项所述的地物碎部点测量方法。

9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如权利要求4至7任一项所述的地物碎部点测量方法。