导言

本文主要实现的是利用python的serial库，通过串口读取组合惯导的相关数据并解析，然后将经纬度高程坐标转换为当地的北东天坐标系和utm坐标系。

组合惯导简介

组合惯导融合了卫星定位系统和基于陀螺仪的定位定向系统，能获得载体的定位和姿态信息，经纬度高程，航向角，俯仰角，摇摆角等等。本文采用的组合惯导是北斗星通KY-INS112型，主要通过串口进行数据的读取和对惯导的设置，也用过华测的惯导做过测试，原理类似，根据官方说明文档进行操作即可。

实现代码

从串口读取解析数据

从串口读取数据的示例很多，主要利用serial库进行读取，需要了解管道串口的数据协议进行一定修改，在注释中已说明。

# 导入相关的包
import os
import time
import serial
import serial.tools.list_ports# 读取数据
os.system('cls')  # 清空屏幕
serial_portlist = list(serial.tools.list_ports.comports())  #查找已经连接的串口
if len(serial_portlist) <= 0 :print('未发现可用串口')
else :for i in range (len(serial_portlist)):print(serial_portlist[i])  # 将可用串口打印出来port_gps = input('请输入GPS模块所在串口：')# 接入数据，注意查找自己惯导串口的波特率，即460800这个数值，不对应的话会出现乱码或其他报错ser = serial.Serial(port_gps,460800,timeout = 2)  os.system('cls')init = 3  # 过滤初始的3行数据，后面解析时使用sample = []  # 创建一个空列表，方便存取数据# 解析数据while True :if ser.in_waiting > 0 :time.sleep(0.4)  # 这个数值可以根据实际调整，主要看一次循环接收多少数据os.system('cls')GETBYTES = ser.read(ser.in_waiting)if init :  # 跳过前三次数据，也可以不设置print('稳定串口接收中 %d'%init)init = init - 1continueGETSTR = GETBYTES.decode()  # 将数据解析GETSTR_List = GETSTR.split('\n')  #按行分开，串口读取的数据是按协议头标识换行的print('共接收 %d 行数据'%len(GETSTR_List)) #检查换行分割情况 for i in  GETSTR_List:GRTSTR_douhao = i.split(',')  # 每一行数据包含多条信息，按逗号将其分开#  下面的if语句是为了判断得到数据是否是对应协议的数据，因为有些串口包含多条协议数据，选择你需要的协议即可，如$GPCHC，可以提前通过串口助手或惯导官方文档了解。# if len(GRTSTR_douhao) > 14 and GRTSTR_douhao[0] == '$GPCHC':sample.append(GRTSTR_douhao)  # 将数据添加进之前创建的列表breakelse:continueprint(sample[-1])  #打印整行数据# 根据协议的定义，按列表位选取自己需要的数据。print('纬度:', float(sample[-1][6]))  print('经度:', float(sample[-1][7]))print('偏航角:', float(sample[-1][3]))

进行坐标转换

经纬度坐标往往不能直接应用，需要进行坐标转换。本文将经纬度高程转换为地心地固坐标系，以及utm，当地北东天坐标系，仅作参考。

import pyproj
from pyproj import Transformer
import numpy as np
import mathclass transformer:def __init__(self, lat, lon, alt):self.lat = latself.lon = lonself.alt = altdef wgs2utm(self):  # 经纬度到UTM世界平面坐标系crs = pyproj.CRS.from_epsg(4326)crs_cs = pyproj.CRS.from_epsg(32650)transformer = Transformer.from_crs(crs, crs_cs)x, y = transformer.transform(self.lat, self.lon)return [x, y]def wgs2ecef(self): # 经纬度到地心地固(earth-central, earth-fixed)坐标系ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')x, y, z = pyproj.transform(lla, ecef, self.lon, self.lat, self.alt, radians=False)return np.array([x, y, z])# 用户所在坐标原点Pb=(x0,y0,z0),计算点P=(x,y,z)在以点P0为坐标原点的坐标系位置(e,n,u),这里需要用到LLA的数据,P0的LLA坐标点为LLAo=(lon0,lat0,alt0)def ecef2enu(self, point0, point):  # 地心地固到东北天坐标系lat_r = math.radians(self.lat)lon_r = math.radians(self.lon)  # 转换为弧度line1 = np.array([-math.sin(lon_r), math.cos(lon_r), 0])line2 = np.array([-math.sin(lat_r)*math.cos(lon_r), -math.sin(lat_r)*math.sin(lon_r), math.cos(lat_r)])line3 = np.array([math.cos(lat_r)*math.cos(lon_r), math.cos(lat_r)*math.sin(lon_r), math.sin(lat_r)])trans = np.vstack((line1, line2, line3))difference = point - point0[e, n, u] = np.matmul(trans, np.array(difference).T).Treturn [e, n, u]# def enu2imu(self, imu_enu, pitch, roll, yaw):   # enu空间中的点在惯导坐标系下的坐标,俯仰角绕东轴(x)旋转,横滚角绕北轴(y)旋转,航向角绕天轴(z)旋转,这里可以认为def enu2imu(self, imu_enu, yaw, points_enu):    # 简化,只考虑偏航角的情况下Rz = np.array([[math.cos(yaw),-math.sin(yaw),0],[math.sin(yaw),math.cos(yaw),0],[0, 0, 1]])T = np.array(-imu_enu)trans = np.hstack((Rz, T))imu_xyz = np.matmul(trans, points_enu)return imu_xyzif __name__ == '__main__':transformer()

参考：

1、GPS经纬度坐标WGS84到东北天坐标系ENU的转换 - 一抹烟霞 - 博客园 (cnblogs.com)

2、python 读取串口数据的示例_Python_萬仟网手机版 (10qianwan.com)

python读取组合惯导数据,并进行坐标转换到北东天、utm坐标系相关推荐

激光雷达与组合惯导联合标定--方案二（matlab）
目录原理 matlab代码对程序的改进 [说明]: 网上关于雷达与惯导联合标定的方法有很多,大多数方法在实际操作起来并不太方便,为了获得矩阵变换关系有的还用到了slam建图.神经网络等方法. 之前 ...
python 显示表格数据_python显示excel表格数据-怎么用python读取excel表格的数据
怎么用python读取excel表格的数据 #导入包 import xlrd #设置路径 path='C:\\Users\\jyjh\\Desktop\\datap.xlsx' #打开 data=xl ...
激光slam学习笔记１－－RTK组合惯导、激光雷达传感器一些经验知识分享
前言:跟组合惯导和激光雷达打交道半年了,过程中查找学习了这两方面的资料,这里来个小结.如果有理解错误的,望大佬们不吝赐教. 一.RTK组合惯导个人理解有两部分组成,一个提供gps信息的rtk,另外一 ...
python读取mysql中表内数据_Python读取MySQL表数据的方法介绍
这篇文章主要为大家详细介绍了Python如何读取MySQL数据库表数据,具有一定的参考价值,感兴趣的小伙伴们可以参考一下本文实例为大家分享了Python读取MySQL数据库表数据的具体代码,供大家参 ...
python读取csv表格的数据并用matplotlib画曲线图
前言 python读取csv表格的数据并用matplotlib回曲线图 1.导入相关库 import matplotlib.pyplot as plt import pandas as pd impo ...
python读取mysql中的数据_Python笔记：用Python读取MySQL中的数据
Python处理数据分析的优势,很多人都知道(可以实现更复杂.更灵活的操作,包括数据预处理.数据可视化输出等),但是加载到Python中的数据,只是临时存储在内存中的一张虚拟表(退出之后就会被释放掉了 ...
python读取excel某列数据
文章目录一.python读取excel某列数据二.将读取的数据变为浮点数一.python读取excel某列数据 import xlrdworksheet = xlrd.open_workbook ...
GPS组合惯导的组成-惯导
GPS组合惯导的组成惯性导航系统-INS是一种不依赖于外部信息.也不向外部辐射能量的自主式导航系统.其工作环境不仅包括空中.地面,还可以在水下.惯导的基本工作原理是以牛顿力学定律为基础,通过测量载体 ...
使用python读取excel中的数据，并绘制折线图
使用python读取excel中的数据,并绘制折线图做实验的时候采集到一些数据,从文本拷贝到excel,然后从十六进制转换成十进制.图表是分析数据的有利工具,使用python绘制出的图表简明美观.所 ...
python开发的程序中以电子表格显示数据_使用 Python 读取电子表格中的数据实例详解...
Python 是最流行.功能最强大的编程语言之一.由于它是自由开源的,因此每个人都可以使用.大多数 Fedora 系统都已安装了该语言.Python 可用于多种任务,其中包括处理逗号分隔值(CSV)数 ...

python读取组合惯导数据,并进行坐标转换到北东天、utm坐标系

导言

组合惯导简介

实现代码

从串口读取解析数据

进行坐标转换

python读取组合惯导数据,并进行坐标转换到北东天、utm坐标系相关推荐

最新文章

热门文章