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

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

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

组合惯导简介

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

实现代码

1. 从串口读取解析数据         从串口读取数据的示例很多，主要利用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 - 1
                   continue
   
               GETSTR = 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)  # 将数据添加进之前创建的列表
                       break
                   else:
                       continue
               print(sample[-1])  #打印整行数据
               # 根据协议的定义，按列表位选取自己需要的数据。
               print('纬度:', float(sample[-1][6]))  
               print('经度:', float(sample[-1][7]))
               print('偏航角:', float(sample[-1][3]))

2. 进行坐标转换

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

import pyproj
from pyproj import Transformer
import numpy as np
import math

class transformer:


    def __init__(self, lat, lon, alt):
        self.lat = lat
        self.lon = lon
        self.alt = alt

    def 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).T
        return [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_xyz

if __name__ == '__main__':
    transformer()

参考：

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

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