CN104613936A - 一种测绘一体机的倾斜测量方法 - Google Patents

Abstract

本发明涉及测绘技术领域，本发明提供一种测绘一体机的倾斜测量方法，倾斜测量方法包括：对陀螺仪传感器的测量值进行冗余判断和预处理后得到第一姿态角和第一方位角，同时对加速度计传感器的测量值和磁强传感器的测量值进行冗余判断和预处理后得到第二姿态角和第二方位角；将第二姿态角与第一姿态角进行相减后得到姿态角的差值，将第二方位角与第一方位角进行相减后得到方位角的差值；对姿态角的差值和方位角的差值进行预判定和预处理后发送给卡尔曼滤波器进行滤波，以得到修正后的姿态角和方位角，通过采用卡尔曼滤波器来过滤测绘一体机的晃动与磁场的突变，提高倾斜测量装置的稳定度和可靠度。

Description

一种测绘一体机的倾斜测量方法

技术领域

本发明涉及测绘技术领域，尤其涉及一种测绘一体机的倾斜测量方法。

背景技术

传统测绘一体机需要测绘人员利用对中杆的气泡使一体机保持水平，一体机相位中心点的经纬度即杆子底部测量点的经纬度，测量点的高度则是一体机的相位中心减去已知的杆子底部到一体机相位中心距离d即可。但为保证精度，这种方法需要测绘人员维持对中的时间，约半分钟左右，难度较大且影响测绘效率。最新的测绘一体机加入了倾斜测量的功能，测绘人员可以在倾斜最大约20度的范围内保持几秒钟后计算出杆底位置。要实现倾斜测量，需要有实时的一体机的倾角和方位角，需要角度测量系统。现有的做法是使用倾角传感器和磁强传感器组成的系统来分别直接测量姿态角和方位角。但这种做法存在不足：

(1)倾角传感器只在静止时输出的角度精度才可用，如果由于风等外力因素使杆子有晃动，则输出的倾角精度有损失；

(2)在磁环境碰到突然的变化，例如附近有汽车经过时，方位角会有较大的误差，并可能有较大的跳动；

(3)在用户未知磁环境好坏的情况下，进行磁标定之后，可能会造成标定结果带有较大误差，直接降低后边使用时的精度；

(4)为控制成本，传感器一般采用低成本的器件，低成本器件的可靠性难以百分百保证。

综上所述，现有技术中的倾斜测量方法存在输出的倾角精度低以及方位角有较大的误差的问题。

发明内容

本发明的目的在于提供一种测绘一体机的倾斜测量方法，旨在解决针对现有技术中的倾斜测量方法存在输出的倾角精度低以及方位角有较大的误差的问题。

本发明是这样实现的，一种测绘一体机的倾斜测量方法，所述倾斜测量方法包括：

对陀螺仪传感器的测量值进行冗余判断和预处理后得到第一姿态角和第一方位角，同时对加速度计传感器的测量值和磁强传感器的测量值进行冗余判断和预处理后得到第二姿态角和第二方位角；

将所述第二姿态角与所述第一姿态角进行相减后得到姿态角的差值，将所述第二方位角与所述第一方位角进行相减后得到方位角的差值；

对所述姿态角的差值和所述方位角的差值进行预判定和预处理后发送给卡尔曼滤波器进行滤波，以得到修正后的姿态角和方位角。

对陀螺仪传感器、加速度计传感器以及磁强传感器的测量值进行冗余判断和预处理的步骤具体为：

在所述陀螺仪传感器、所述加速度计传感器或所述磁强传感器中任意选择两个传感器，根据其中一个传感器的三轴坐标、另一个传感器的任一轴坐标以及所述两个传感器的三轴坐标系之间的相对角度计算常系数；

根据所述常系数判断所述两个传感器的每一个轴的数据是否合格，是，则采用该轴数据，否，则放弃该轴数据。

所述根据其中一个传感器的三轴坐标、另一个传感器的任一轴坐标以及所述两个传感器的三轴坐标系之间的相对角度计算常系数的步骤具体为：

根据以下关系式计算常系数：

a1·x1+b1·y1+c1·z1+d1·x2＝0

x1＝[x1 0 0]

y1＝[0 y1 0]

z1＝[0 0 z1]

x2＝[x2·sinα1·cosβ1 x2·sinα1·sinβ1 x2·cosα1]

其中，a1、b1、c1以及d1为常系数，x1、y1、z1为一个传感器的三轴坐标，x2为另一个传感器的任一轴坐标，α1,β1为两个传感器的三轴坐标系之间的相对角度。

所述根据所述常系数判所述两个传感器的每一个轴的数据是否合格，是，则采用该轴数据的步骤具体为：

根据以下关系式计算常系数对某一个传感器的每一轴的数据进行判断：

a1·x1+b1·y1+c1·z1+d1·x2≈0…(1)

a2·x1+b2·y1+c2·z1+d2·y2≈0…(2)

a3·x1+b3·y1+c3·z1+d3·z2≈0…(3)

其中，x1、y1、z1为一个传感器的三轴坐标，x2、y2、z2为另一个传感器的三轴坐标；

当公式(1)和公式(2)成立，公式(3)不成立时，判定z2轴出现故障；当公式(1)和公式(3)成立，公式(2)不成立时，判定y2轴出现故障；当公式(2)和公式(3)成立，公式(1)不成立时，判定x2轴出现故障。 

所述将所述姿态角的差值和所述方位角的差值经过预判定和预处理后发送给卡尔曼滤波器进行滤波的步骤中将所述姿态角的差值进行预判定和预处理的步骤具体为：

判断所述加速度计传感器的测量值与重力的差的绝对值是否大于第一预设值，是，则放弃所述姿态角差值，否，则判断所述加速度计传感器的测量值与重力的差的绝对值是否大于第二预设值，是，则放弃所述姿态角差值并将所述加速度计传感器的测量值进行降权后发送给卡尔曼滤波器进行滤波，否，则将所述姿态角的差值发送给卡尔曼滤波器进行滤波。

所述将所述姿态角的差值和所述方位角的差值经过预判定和预处理后发送给卡尔曼滤波器进行滤波的步骤中将所述方位角的差值进行预判定和预处理的步骤具体为：

判断卡尔曼滤波器进行滤波时间是否大于第三预设值，否，则将所述方位角的差值发送给卡尔曼滤波器进行滤波，是，则判断所述磁强传感器的测量值 与当地磁模型的差值的绝对值是否大于第四预设值，是，则放弃所述方位角的差值，否，则判断所述方位角的差值的绝对值是否大于第五预设值，是，则放弃所述方位角的差值，否，则判断所述方位角的差值的绝对值是否大于第六预设值，是，则放弃所述方位角差值并将所述磁强传感器的测量值进行降权后发送给卡尔曼滤波器进行滤波，否，则将所述方位角的差值发送给卡尔曼滤波器进行滤波。

所述倾斜测量方法还包括：

A.采集所述磁强传感器测量的磁强数据并进行保存，同时将所述磁强数据经过滤波器滤波后得到修正后的磁强数据；

B.将所述修正后的磁强数据与保存的磁强数据进行残差计算后得到残差统计量；

C.判断所述残差统计量是否小于第七预设值，否，则判定所述修正后的磁强数据无效数据，并返回执行所述步骤A，是，则执行步骤D；

D.判断所述残差统计量是否小于第八预设值，是，则保存所述修正后的磁强数据，否，则删除所述残差统计量中大于第九预设值的数据，并将剩余的残差统计量经过滤波器滤波后得到标定的磁强数据。

所述步骤A中将所述磁强数据经过滤波器滤波后得到修正后的磁强数据的步骤具体为：

判断所述磁强数据与所述修正后的磁强数据的差值是否小于第十预设值，是，则保存修正后的磁强数据，否，则重新采集所述磁强传感器测量的磁强数据。

本发明提供一种测绘一体机的倾斜测量方法，采用卡尔曼滤波器来过滤测绘一体机的晃动与磁场的突变，提高倾斜测量系统的稳定度和可靠度，使用冗余的低成本传感器进行测量，在降低成本的同时提升了硬件可靠性，解决了现有技术中的倾斜测量方法存在输出的倾角精度低以及方位角有较大的误差的问题，并且提高倾斜测量系统的精度可靠性和可用性，另外在磁强传感器标定过 程中，计算估计的参数的质量，来标示标定参数的质量是否合格。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本发明一种实施例提供的一种测绘一体机的倾斜测量方法的流程图；

图2是本发明另一种实施例提供的一种测绘一体机的倾斜测量方法的流程图；

图3是本发明一种实施例提供的一种测绘一体机的倾斜测量方法中的两组正交的三轴正交示意图；

图4是本发明另一种实施例提供的一种测绘一体机的倾斜测量方法的流程图；

图5是本发明另一种实施例提供的一种测绘一体机的倾斜测量方法的流程图；

图6是本发明另一种实施例提供的一种测绘一体机的倾斜测量方法的流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

为了说明本发明的技术方案，下面通过具体实施例来进行说明。

本发明一种实施例提供一种测绘一体机的倾斜测量方法，如图1所示，倾 斜测量方法包括：

步骤S101.对陀螺仪传感器的测量值进行冗余判断和预处理后得到第一姿态角和第一方位角，同时对加速度计传感器的测量值和磁强传感器的测量值进行冗余判断和预处理后得到第二姿态角和第二方位角。

在本实施例中，步骤S101中对陀螺仪传感器、加速度计传感器以及磁强传感器的测量值进行冗余判断和预处理的步骤具体为：

步骤S1011.在陀螺仪传感器、加速度计传感器或磁强传感器中任意选择两个传感器，根据其中一个传感器的三轴坐标、另一个传感器的任一轴坐标以及两个传感器的三轴坐标系之间的相对角度计算常系数。

在本实施例中，步骤S1011具体为：

根据以下关系式计算常系数：

a1·x1+b1·y1+c1·z1+d1·x2＝0

x1＝[x1 0 0]

y1＝[0 y1 0]

z1＝[0 0 z1]

x2＝[x2·sinα1·cosβ1 x2·sinα1·sinβ1 x2·cosα1]

其中，a1、b1、c1以及d1为常系数，x1、y1、z1为一个传感器的三轴坐标，x2为另一个传感器的任一轴坐标，α1,β1为两个传感器的三轴坐标系之间的相对角度。

其中，为了不失一般性，此处先使用一个传感器的三个轴向x1,y1,z1与另一个传感器的x2，共4个轴向进行来说明，已知在三维空间中，任意的四个三维向量是相关的，即存在a1,b1,c1,d1，使得：

a1·x1+b1·y1+c1·z1+d1·x2＝0

对这个应用，由于x1,y1,z1,x2四个向量的空间关系固定，因此a1,b1,c1,d1是4个常系数，即只要将传感器固定，a1,b1,c1,d1即固定了，此外，下式的α1,β1可以由模具或者标定得到，是已知值，关于其定义请参见图4。

x1＝[x1 0 0]

y1＝[0 y1 0]

z1＝[0 0 z1]

x2＝[x2·sinα1·cosβ1 x2·sinα1·sinβ1 x2·cosα1]

所以，如果有4组以上不同的x1,y1,z1,x2即可以得到a1,b1,c1,d1的最小二乘解。而通过传感器在空间的运动，可以得到4组以上的不同x1,y1,z1,x2数据，至此a1,b1,c1,d1这四个常系数确定完毕。常系数确定在器件事先标定过程中即可完成。

同理，对于y2和z2也有类似关系，所以有：

a1·x1+b1·y1+c1·z1+d1·x2＝0

a2·x1+b2·y1+c2·z1+d2·y2＝0

a3·x1+b3·y1+c3·z1+d3·z2＝0

进一步的，由于传感器都带有误差和噪声，此处首先将上面的关系式写为

a1·x1+b1·y1+c1·z1+d1·x2≈0

a2·x1+b2·y1+c2·z1+d2·y2≈0

a3·x1+b3·y1+c3·z1+d3·z2≈0

步骤S1012.根据常系数判断两个传感器的每一个轴的数据是否合格，是，则执行步骤S1014，否，则执行步骤S1013。

步骤S1013.放弃该轴数据。

步骤S1014.采用该轴数据。

在本实施中，具体的，根据以下关系式计算常系数对某一个传感器的每一轴的数据进行判断：

a1·x1+b1·y1+c1·z1+d1·x2≈0…(1)

a2·x1+b2·y1+c2·z1+d2·y2≈0…(2)

a3·x1+b3·y1+c3·z1+d3·z2≈0…(3)

其中，x1、y1、z1为一个传感器的三轴坐标，x2、y2、z2为另一个传感器的三轴坐标；

当公式(1)和公式(2)成立，公式(3)不成立时，判定z2轴出现故障；当公式(1)和公式(3)成立，公式(2)不成立时，判定y2轴出现故障；当公式(2)和公式(3)成立，公式(1)不成立时，判定x2轴出现故障。 

同理，在对x1、y1、z1轴进行判断时，先根据步骤S1011计算常系数，再根据以下公式进行判断：

a4·x2+b4·y2+c4·z2+d4·x1≈0…(4)

a5·x2+b5·y2+c5·z2+d5·y1≈0…(5)

a6·x2+b6·y2+c6·z2+d6·z1≈0…(6)

如果出现(4)和(5)成立，而(6)一直不成立，那可以判定z1轴出现故障；同理，如果出现(5)和(6)成立，而(4)一直不成立，那可以判定x1轴出现故障；出现(4)和(6)成立，而(5)一直不成立，那可以判定y1轴出现故障。

步骤S102.将第二姿态角与第一姿态角进行相减后得到姿态角的差值，将第二方位角与第一方位角进行相减后得到方位角的差值。

步骤S103.对姿态角的差值和方位角的差值进行预判定和预处理后发送给卡尔曼滤波器进行滤波，以得到修正后的姿态角和方位角。

在本实施例中，步骤S103中将姿态角的差值进行预判定和预处理的步骤具体为：

步骤S1031.判断加速度计传感器的测量值与重力的差的绝对值是否大于第一预设值，是，则执行步骤S1032，否，则执行步骤S1033。

步骤S1032.放弃姿态角差值。

步骤S1033.判断加速度计传感器的测量值与重力的差的绝对值是否大于第二预设值，是，则执行步骤S1034，否，则执行步骤S1035。

步骤S1034.放弃姿态角差值并将加速度计传感器的测量值进行降权后发送给卡尔曼滤波器进行滤波。

步骤S1035.则将姿态角的差值发送给卡尔曼滤波器进行滤波。

在本实施中步骤S103中将方位角的差值进行预判定和预处理的步骤具体为：

步骤S1036.判断卡尔曼滤波器进行滤波时间是否大于第三预设值，否，则执行步骤S1037，是，则执行步骤S1038。

步骤S1037.将方位角的差值发送给卡尔曼滤波器进行滤波。

步骤S1038.判断磁强传感器的测量值与当地磁模型的差值的绝对值是否大于第四预设值，是，则执行步骤S1039，否，则执行步骤S1040。

步骤S1039.放弃方位角的差值。

步骤S1040.判断方位角的差值的绝对值是否大于第五预设值，是，则执行步骤S1039，否，则执行步骤S1041。

步骤S1041.判断方位角的差值的绝对值是否大于第六预设值，是，则执行步骤S1042，否，则执行步骤S1043。

步骤S1042.放弃方位角差值并将磁强传感器的测量值进行降权后发送给卡尔曼滤波器进行滤波。

步骤S1043.将方位角的差值发送给卡尔曼滤波器进行滤波。

本发明一种实施例提供一种测绘一体机的倾斜测量方法，如图1所示，倾斜测量方法还包括：

步骤S201.采集磁强传感器测量的磁强数据并进行保存，同时将磁强数据经过滤波器滤波后得到修正后的磁强数据。

在本实施例中，步骤S201中将磁强数据经过滤波器滤波后得到修正后的磁强数据的步骤具体为：

判断磁强数据与修正后的磁强数据的差值是否小于第十预设值，是，则保存修正后的磁强数据，否，则重新采集磁强传感器测量的磁强数据。

步骤S202.将修正后的磁强数据与保存的磁强数据进行残差计算后得到残差统计量。

步骤S203.判断残差统计量是否小于第七预设值，否，则执行步骤S204，是，则执行步骤S205。

步骤S204.判定修正后的磁强数据无效数据，并返回执行步骤S201。

步骤S205.判断残差统计量是否小于第八预设值，是，则执行步骤S206，否，则执行步骤S207。

步骤S206.保存修正后的磁强数据。

步骤S207.则删除残差统计量中大于第九预设值的数据，并将剩余的残差统计量经过滤波器滤波后得到标定的磁强数据。

本发明提供一种测绘一体机的倾斜测量方法，采用卡尔曼滤波器来过滤测绘一体机的晃动与磁场的突变，提高倾斜测量系统的稳定度和可靠度，使用冗余的低成本传感器进行测量，在降低成本的同时提升了硬件可靠性，并且提高倾斜测量系统的精度可靠性和可用性，另外在磁强传感器标定过程中，计算估计的参数的质量，来标示标定参数的质量是否合格。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明由所提交的权利要求书确定的专利保护范围。

Claims (8)

1.一种测绘一体机的倾斜测量方法，其特征在于，所述倾斜测量方法包括：

对陀螺仪传感器的测量值进行冗余判断和预处理后得到第一姿态角和第一方位角，同时对加速度计传感器的测量值和磁强传感器的测量值进行冗余判断和预处理后得到第二姿态角和第二方位角；

将所述第二姿态角与所述第一姿态角进行相减后得到姿态角的差值，将所述第二方位角与所述第一方位角进行相减后得到方位角的差值；

对所述姿态角的差值和所述方位角的差值进行预判定和预处理后发送给卡尔曼滤波器进行滤波，以得到修正后的姿态角和方位角。

2.如权利要求1所述的倾斜测量方法，其特征在于，对陀螺仪传感器、加速度计传感器以及磁强传感器的测量值进行冗余判断和预处理的步骤具体为：

在所述陀螺仪传感器、所述加速度计传感器或所述磁强传感器中任意选择两个传感器，根据其中一个传感器的三轴坐标、另一个传感器的任一轴坐标以及所述两个传感器的三轴坐标系之间的相对角度计算常系数；

根据所述常系数判断所述两个传感器的每一个轴的数据是否合格，是，则采用该轴数据，否，则放弃该轴数据。

3.如权利要求2所述的倾斜测量方法，其特征在于，所述根据其中一个传感器的三轴坐标、另一个传感器的任一轴坐标以及所述两个传感器的三轴坐标系之间的相对角度计算常系数的步骤具体为：

根据以下关系式计算常系数：

a1·x1+b1·y1+c1·z1+d1·x2＝0

x1＝[x1 0 0]

y1＝[0 y1 0]

z1＝[0 0 z1]

x2＝[x2·sinα1·cosβ1 x2·sinα1·sinβ1 x2·cosα1]

其中，a1、b1、c1以及d1为常系数，x1、y1、z1为一个传感器的三轴坐标，x2为另一个传感器的任一轴坐标，α1,β1为两个传感器的三轴坐标系之间的相对角度。

4.如权利要求3所述的倾斜测量方法，其特征在于，所述根据所述常系数判所述两个传感器的每一个轴的数据是否合格，是，则采用该轴数据的步骤具体为：

根据以下关系式计算常系数对某一个传感器的每一轴的数据进行判断：

a1·x1+b1·y1+c1·z1+d1·x2≈0…(1)

a2·x1+b2·y1+c2·z1+d2·y2≈0…(2)

a3·x1+b3·y1+c3·z1+d3·z2≈0…(3)

其中，x1、y1、z1为一个传感器的三轴坐标，x2、y2、z2为另一个传感器的三轴坐标；

当公式(1)和公式(2)成立，公式(3)不成立时，判定z2轴出现故障；当公式(1)和公式(3)成立，公式(2)不成立时，判定y2轴出现故障；当公式(2)和公式(3)成立，公式(1)不成立时，判定x2轴出现故障。

5.如权利要求1所述的倾斜测量方法，其特征在于，所述将所述姿态角的差值和所述方位角的差值经过预判定和预处理后发送给卡尔曼滤波器进行滤波的步骤中将所述姿态角的差值进行预判定和预处理的步骤具体为：

判断所述加速度计传感器的测量值与重力的差的绝对值是否大于第一预设值，是，则放弃所述姿态角差值，否，则判断所述加速度计传感器的测量值与重力的差的绝对值是否大于第二预设值，是，则放弃所述姿态角差值并将所述加速度计传感器的测量值进行降权后发送给卡尔曼滤波器进行滤波，否，则将所述姿态角的差值发送给卡尔曼滤波器进行滤波。

6.如权利要求1所述的倾斜测量方法，其特征在于，所述将所述姿态角的差值和所述方位角的差值经过预判定和预处理后发送给卡尔曼滤波器进行滤波的步骤中将所述方位角的差值进行预判定和预处理的步骤具体为：

判断卡尔曼滤波器进行滤波时间是否大于第三预设值，否，则将所述方位角的差值发送给卡尔曼滤波器进行滤波，是，则判断所述磁强传感器的测量值与当地磁模型的差值的绝对值是否大于第四预设值，是，则放弃所述方位角的差值，否，则判断所述方位角的差值的绝对值是否大于第五预设值，是，则放弃所述方位角的差值，否，则判断所述方位角的差值的绝对值是否大于第六预设值，是，则放弃所述方位角差值并将所述磁强传感器的测量值进行降权后发送给卡尔曼滤波器进行滤波，否，则将所述方位角的差值发送给卡尔曼滤波器进行滤波。

7.如权利要求1所述的倾斜测量方法，其特征在于，所述倾斜测量方法还包括：

A.采集所述磁强传感器测量的磁强数据并进行保存，同时将所述磁强数据经过滤波器滤波后得到修正后的磁强数据；

B.将所述修正后的磁强数据与保存的磁强数据进行残差计算后得到残差统计量；

C.判断所述残差统计量是否小于第七预设值，否，则判定所述修正后的磁强数据无效数据，并返回执行所述步骤A，是，则执行步骤D；

D.判断所述残差统计量是否小于第八预设值，是，则保存所述修正后的磁强数据，否，则删除所述残差统计量中大于第九预设值的数据，并将剩余的残差统计量经过滤波器滤波后得到标定的磁强数据。

8.如权利要求7所述的倾斜测量方法，其特征在于，所述步骤A中将所述磁强数据经过滤波器滤波后得到修正后的磁强数据的步骤具体为：

判断所述磁强数据与所述修正后的磁强数据的差值是否小于第十预设值，是，则保存修正后的磁强数据，否，则重新采集所述磁强传感器测量的磁强数据。