CN106405606A - 一种高精度卫星定位的实现方法 - Google Patents

Abstract

本发明公开了一种高精度卫星定位的实现方法，属于导航定位领域，主要应用于对定位精度要求较高的场合。本发明通过陀螺仪、加速度计和磁力计确立定位载体的方向，角速度和加速度，经计算校正最终输出定位载体的经纬度和高度信息，从而提供更加精确的定位。

Description

一种高精度卫星定位的实现方法

技术领域

本发明涉及涉及卫星导航定位技术。

背景技术

随着导航定位技术的发展，导航定位在日常生活中日渐普及，但是受天气和使用环境的影响，经常发生南辕北辙的情况，定位精度的高低直接影响导航的结果。本发明装置支持北斗和GPS卫星定位装置的接入，通过陀螺仪、加速度计和磁力计确立定位载体的方向，角速度和加速度，经计算校正最终输出定位载体的经纬度和高度信息，经实际定位测试误差小于1米。

目前为了解决定位误差过大的问题，而发射更多的导航定位卫星。发射更多的导航定位卫星固然可以提高导航定位的精度，但是成本高，在接收导航定位卫星信号比较差的环境，导航定位的精度仍很难保证。

故，需要一种新的技术方案以解决上述问题。

发明内容

本发明的目的在于克服上述方案的缺点，通过陀螺仪、加速度计和磁力计确立定位载体的方向，角速度和加速度，经计算校正最终输出定位载体的经纬度和高度信息。

为实现上述目的，本发明可采用如下技术方案：

一种高精度卫星定位的实现方法，包括以下步骤：

(1)、在定位载体上设置高精度定位装置；该高精度定位装置中包括处理器、输入串口、输入串口波特率拨码开关、输入串口协议拨码开关、输出串口、输出串口波特率拨码开关、输出串口协议拨码开关；所述输入串口、输入串口波特率拨码开关、输入串口协议拨码开关、输出串口、输出串口波特率拨码开关、输出串口协议拨码开关均与处理器电性连接；其特征在于：还包括陀螺仪、加速度计、磁力计，陀螺仪、加速度计、磁力计也与处理器电性连接；

(2)、采集陀螺仪、加速度计和磁力计的数据，采用陀螺仪和加速度计互为校正确立定位载体的各轴向的角度和加速度，最后结合磁力计计算各方向上的位移；

(3)、解析北斗/GPS通信协议，获取当前定位载体的经纬度和高度信息；

(4)、计算输出定位载体的经纬度和高度信息。

相对与现有技术，本发明具有如下优点：

1)导航定位精度高，经实际定位测试误差小于1米；

2)本发明装置体积较小，便于安装，直接串联在串口线上即可；

3)支持多种卫星导航定位协议，如：NAEA-0183、NAEA-0180和NAEA-0182等；

4)通过设置输入串口波特率和协议拨码开关可以支持不同厂家的卫星导航定位设备；

5)通过设置输出串口波特率和协议拨码开关，可以和现有系统实现无缝对接，不需任何软件修改。

附图说明

图1是本发明中高精度定位装置的结构示意图。

图2是本发明中软件模块的关系示意图。

图3是传感器采集计算模块工作流程图。

图4是北斗/GPS协议解析模块工作流程图。

图5是计算校正输出模块工作流程图。

具体实施方式

下面结合附图，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

本发明公开一种高精度卫星定位的实现方法，包括以下步骤：

(1)、如图1所示，在定位载体上设置高精度定位装置；该高精度定位装置中包括处理器、输入串口、输入串口波特率拨码开关、输入串口协议拨码开关、输出串口、输出串口波特率拨码开关、输出串口协议拨码开关；所述输入串口、输入串口波特率拨码开关、输入串口协议拨码开关、输出串口、输出串口波特率拨码开关、输出串口协议拨码开关均与处理器电性连接；其特征在于：还包括陀螺仪、加速度计、磁力计，陀螺仪、加速度计、磁力计也与处理器电性连接；

(2)、采集陀螺仪、加速度计和磁力计的数据，采用陀螺仪和加速度计互为校正确立定位载体的各轴向的角度和加速度，最后结合磁力计计算各方向上的位移；

(3)、解析北斗/GPS通信协议，获取当前定位载体的经纬度和高度信息；

(4)、计算输出定位载体的经纬度和高度信息。

其中，步骤(2)至步骤(4)中，需要设置软件模块进行实现，软件模块包括传感器采集计算模块、北斗/GPS协议解析模块、计算校正输出模块，各模块之间的关系如图2所示。

传感器采集计算模块负责采集陀螺仪、加速度计和磁力计的数据，加速度计读数受振动影响较大，采用陀螺仪和加速度计互为校正确立定位载体的各轴向的角度和加速度，最后结合磁力计计算各方向上的位移，其实现过程如图3，即实现步骤(2)，该步骤(2)中，包括如下细分步骤：

(2a)读取加速度传感器的当前值：AxADCn、AyADCn、AzADCn，读取时间：T1、T2、T3，最后一次读取时间：T1n、T2n、T3n；

(2b)读取陀螺仪传感器的当前值：∠xADCn、∠yADCn、∠zADCn；

(2c)重复步骤(2a)和(2b)100次并将其累计后，求其平均值分别为：AxADC、AyADC、AzADC和∠xADC、∠yADC、∠zADC；

(2d)分别将AxADC、AyADC、AzADC代入(ADC*Vref/1023-Vzg)/S计算得到Rx、Ry、Rz方向上的加速度；

上式中ADC为AxADC、AyADC、AzADC；

Vref为传感器的参考电压；

1023为2^传感器的分辨率位数-1计算得到，如当前传感器分辨率为10，固有2^10-1＝1023；

Vzg为传感器的零加速度的电压值；

S为传感器的灵敏度；

(2e)由三维空间勾股定理通过Rx、Ry、Rz可以计算得到X、Y和Z轴的角度∠X1、∠Y1、∠Z1；

(2f)和步骤(2d)中的公式类似可以计算出陀螺仪传感器在X、Y和Z轴的角度∠X2、∠Y2、∠Z2；

(2g)计算比较∠X1/∠X2、∠Y1/∠Y2、∠Z1/∠Z2的值，如果3个值都在1以内继续，否则重复(2a)～(2g)；

(2h)由位移＝VoT+1/2*A*T^2令Vo为0，T＝T1n-T1，将Rx、Ry、Rz代入，计算各轴的位移：Sx、Sy、Sz；

(2i)采集磁力计的当前值，计算校正各轴的角度得到角度∠x、∠y、∠z。

北斗/GPS协议解析模块负责解析北斗/GPS通信协议，获取当前定位载体的经纬度和高度信息，其实现过程如图4，即实现步骤(3)，该步骤(3)中，包括如下细分步骤：

(3a)建立环形数据缓冲区；

(3b)将输入串口的数据存入环形数据缓冲区，同时移动保存数据位置指针，当保存数据位置指针移到环形数据缓冲区末尾时，令保存数据位置指针指向环形数据缓冲区头；

(3c)从环形数据缓冲区首位置读取数据，解析数据，同时移动读取数据位置指针，当读取数据位置指针移到环形数据缓冲区末尾时，令读取数据位置指针指向环形数据缓冲区头；

(3d)将解析出来的经纬度和高度信息保存到X、Y、Z。

计算校正输出模块将传感器采集计算模块输出的位移信息：Sx、Sy、Sz，角度信息：∠x、∠y、∠z，加速度信息：Rx、Ry、Rz和北斗/GPS协议解析模块输出的经纬度和高度信息：X、Y、Z，进行互为校正，最终计算输出定位载体的经纬度和高度信息，其实现过程如图5所示，即实现步骤(4)，该步骤(4)中，包括如下细分步骤：

(4a)读取位移信息：Sx、Sy、Sz，角度信息：∠x、∠y、∠z，加速度信息：Rx、Ry、Rz，经纬度和高度信息：X、Y、Z分别存入缓冲区：FSx、FSy、FSz、F∠x、F∠y、F∠z、FRx、FRy、FRz、Fx、Fy、Fz；

(4b)读取位移信息：Sx、Sy、Sz，角度信息：∠x、∠y、∠z，加速度信息：Rx、Ry、Rz，经纬度和高度信息：X、Y、Z；

(4c)将Sx、Sy、∠x、∠y、Fx、Fy代入下式

Xn＝Fx+Sx*cos(∠x)；

Yn＝Fy+Sy*sin(∠y)；

(4d)根据Xn、Yn和高斯算法计算出新点的经纬度和高度：X2，Y2，Z2；

(4e)根据Sx、Sy、Sz、∠x、∠y、∠z、Rx、Ry、Rz、X、Y、Z与FSx、FSy、FSz、F∠x、F∠y、F∠z、FRx、FRy、FRz、Fx、Fy、Fz计算判断X、Y、Z的合法性；若合法根据X2、Y2、Z2、X、Y和Z计算后，按照输出串口协议和波特率输出定位载体的经纬度和高度信息；若不合法直接将X2、Y2和Z2按照输出串口协议和波特率输出定位载体的经纬度和高度信息。

综上所述，本发明的关键点是利用陀螺仪、加速度计和磁力计计算当前定位载体的方向，角速度和加速度信息，与北斗/GPS解析出来的经纬度和高度信息进行互为校正最终输出定位载体的经纬度和高度信息。

本发明应用于高精度导航定位的场合，体现了如下优点：

1)导航定位精度高，经实际定位测试误差小于1米；

2)本发明装置体积较小，便于安装，直接串联在串口线上即可；

3)支持多种卫星导航定位协议，如：NAEA-0183、NAEA-0180和NAEA-0182等；

4)通过设置输入串口波特率和协议拨码开关可以支持不同厂家的卫星导航定位设备；

5)通过设置输出串口波特率和协议拨码开关，可以和现有系统实现无缝对接，不需任何软件修改。

Claims (4)

1.一种高精度卫星定位的实现方法，其特征在于，包括以下步骤：

(1)、在定位载体上设置高精度定位装置；该高精度定位装置中包括处理器、输入串口、输入串口波特率拨码开关、输入串口协议拨码开关、输出串口、输出串口波特率拨码开关、输出串口协议拨码开关；所述输入串口、输入串口波特率拨码开关、输入串口协议拨码开关、输出串口、输出串口波特率拨码开关、输出串口协议拨码开关均与处理器电性连接；其特征在于：还包括陀螺仪、加速度计、磁力计，陀螺仪、加速度计、磁力计也与处理器电性连接；

(2)、采集陀螺仪、加速度计和磁力计的数据，采用陀螺仪和加速度计互为校正确立定位载体的各轴向的角度和加速度，最后结合磁力计计算各方向上的位移；

(3)、解析北斗/GPS通信协议，获取当前定位载体的经纬度和高度信息；

(4)、计算输出定位载体的经纬度和高度信息。

2.根据权利要求1所述高精度卫星定位的实现方法，其特征在于：步骤(2)中，包括如下细分步骤：

(2a)读取加速度传感器的当前值：AxADCn、AyADCn、AzADCn，读取时间：T1、T2、T3，最后一次读取时间：T1n、T2n、T3n；

(2b)读取陀螺仪传感器的当前值：∠xADCn、∠yADCn、∠zADCn；

(2c)重复步骤(2a)和(2b)100次并将其累计后，求其平均值分别为：AxADC、AyADC、AzADC和∠xADC、∠yADC、∠zADC；

(2d)分别将AxADC、AyADC、AzADC代入(ADC*Vref/1023-Vzg)/S计算得到Rx、Ry、Rz方向上的加速度；

上式中ADC为AxADC、AyADC、AzADC；

Vref为传感器的参考电压；

1023为2^传感器的分辨率位数-1计算得到，如当前传感器分辨率为10，固有2^10-1＝1023；

Vzg为传感器的零加速度的电压值；

S为传感器的灵敏度；

(2e)由三维空间勾股定理通过Rx、Ry、Rz可以计算得到X、Y和Z轴的角度∠X1、∠Y1、∠Z1；

(2f)和步骤(2d)中的公式类似可以计算出陀螺仪传感器在X、Y和Z轴的角度∠X2、∠Y2、∠Z2；

(2g)计算比较∠X1/∠X2、∠Y1/∠Y2、∠Z1/∠Z2的值，如果3个值都在1以内继续，否则重复(2a)～(2g)；

(2h)由位移＝VoT+1/2*A*T^2令Vo为0，T＝T1n-T1，将Rx、Ry、Rz代入，计算各轴的位移：Sx、Sy、Sz；

(2i)采集磁力计的当前值，计算校正各轴的角度得到角度∠x、∠y、∠z。

3.根据权利要求1所述高精度卫星定位的实现方法，其特征在于：步骤(3)中，包括如下细分步骤：

(3a)建立环形数据缓冲区；

(3b)将输入串口的数据存入环形数据缓冲区，同时移动保存数据位置指针，当保存数据位置指针移到环形数据缓冲区末尾时，令保存数据位置指针指向环形数据缓冲区头；

(3c)从环形数据缓冲区首位置读取数据，解析数据，同时移动读取数据位置指针，当读取数据位置指针移到环形数据缓冲区末尾时，令读取数据位置指针指向环形数据缓冲区头；

(3d)将解析出来的经纬度和高度信息保存到X、Y、Z。

4.根据权利要求1所述高精度卫星定位的实现方法，其特征在于：步骤(4)中，包括如下细分步骤：

(4a)读取位移信息：Sx、Sy、Sz，角度信息：∠x、∠y、∠z，加速度信息：Rx、Ry、Rz，经纬度和高度信息：X、Y、Z分别存入缓冲区：FSx、FSy、FSz、F∠x、F∠y、F∠z、FRx、FRy、FRz、Fx、Fy、Fz；

(4b)读取位移信息：Sx、Sy、Sz，角度信息：∠x、∠y、∠z，加速度信息：Rx、Ry、Rz，经纬度和高度信息：X、Y、Z；

(4c)将Sx、Sy、∠x、∠y、Fx、Fy代入下式

Xn＝Fx+Sx*cos(∠x)；

Yn＝Fy+Sy*sin(∠y)；

(4d)根据Xn、Yn和高斯算法计算出新点的经纬度和高度：X2，Y2，Z2；

(4e)根据Sx、Sy、Sz、∠x、∠y、∠z、Rx、Ry、Rz、X、Y、Z与FSx、FSy、FSz、F∠x、F∠y、F∠z、FRx、FRy、FRz、Fx、Fy、Fz计算判断X、Y、Z的合法性；若合法根据X2、Y2、Z2、X、Y和Z计算后，按照输出串口协议和波特率输出定位载体的经纬度和高度信息；若不合法直接将X2、Y2和Z2按照输出串口协议和波特率输出定位载体的经纬度和高度信息。