开发者说丨组合导航调试与驱动实践
目前主流的无人驾驶方案离不开GNSS/IMU，加之大多数厂家直接使用组合导航设备，但是关于如何调试的网上教程比较零散，所以社区开发者—胡想成将这一部分的工作整理出来供大家参考。
本文内容包括组合导航安装配置流程、协议解析、精度测试等模块，也会讲解地理系LLA与平面系XYZ和惯导系的对齐及可视化的简单编码等。
下面是由社区开发者—胡想成提供的文章，对组合导航调试与驱动实践进行详细讲解，希望这篇文章能给感兴趣的开发者带来更多帮助。
以下，ENJOY
组合导航INS550D
通用4G模块DTU (型号MD-649)
串转USB转接线，4G吸盘天线
千寻服务差分账号、SIM卡
Ins Asstiant(厂商提供)、dtucfg相关软件(官网下载)
▲图1 使用地基增强参考站的系统框图
1、只有移动站（推荐,本文的案例）
由惯性导航单元、四频双天线RTK 卫星导航接收机和4G 差分模块组成。4G 差分模块通过4G 网络接收千寻位置等RTCM 差分信息，发送给卫星导航板卡。导航计算机接收3 轴陀螺、3 轴加速度计和卫导板卡信息，实时得到精确的位置、速度、方位。
2、包括移动站和CORS 站
移动站由惯性导航单元、四频双天线RTK 卫星导航接收机和数传电台组成。卫星导航接收机通过RTK 差分解算，测得载体的位置和方位角，惯性导航单元给出载体的姿态角、角速度和加速度，并运行组合导航算法得到实时位置和方位数据。CORS站由四频卫星导航接收机和数传电台组成，发送RTCM和载波信息。
▲图2 自建基站的系统框图
MD-649是一款具有高速数据传输能力的4G DTU产品，特别适合传输数据量大，实时性要求高的场合。
在MD-649中设置数据中心的IP（或域名）和端口后，MD-649利用4G无线网络拨号连上Internet，随后发起对所配的IP和端口（即mServer的监听端口）的连接，另外，用户软件系统通过虚拟串口等接口连接到mServer，进而实现了从用户设备到用户软件系统之间的无线、双向数据通信。
▲MD-649工作原理
由于我并未进行过组合导航的标定工作，所以以下内容暂时不会涉及，后续有经验了再补上。
双天线位置安装，距离大于1米。
惯导安装朝向，一般惯导X轴向为车头方向。
惯导、DTU、工控机连接
千寻差分账号：淘宝可买，每个手机号均可申请试用2小时。
硬件连接：笔记本（win/linux）->RS232转USB->DTU
将SIM卡激活并安装到DTU中，打开dtucfg.exe，设置正确的串口号以及波特率(115200)
按连续回车进行配置，直到恢复出厂设置
修改数据中心端口号8002
修改波特率115200（此波特率为差分口输出的波特率，注意与后面RS422口输出区分开来）
输入NTRIP账号和密码（你的差分账号和密码，切记不同于千寻登录账号和密码）
确认正常情况：绿灯常亮（连接到数据中心）、红灯闪烁（正在传送数据）。
从上电到DTU连接正常可能需要半分钟左右，属正常情况。
打开Ins Asstiant选择波特率230400和串口COM6，打开串口，若当前信息显示数据在更新，即，则硬件连接成功。这里数据更新主要看以下三点：
位置更新
姿态更新
组合导航状态
如果位置更新显示绿色，姿态更新显示蓝色，即组合导航初始化成功。
查看组合导航RTK状态。FLAG这里显示NARROW_INT，解释为窄巷固定解，收星数为19，初始化成功，RTK状态很好。
这里对RTK STATUS做一个简单说明：
STATUS=0，即RTK无解。
STATUS = 48/49/50，L1 固定解、宽巷固定解、窄巷固定解，即为厘米级定位。
STATUS=32/33/34，浮动解，即为亚米级定位。
后面会展示一下测试结果，有RTK，亚米级RTK，厘米级RTK。
如果组合导航初始化未成功，具体检查是姿态还是位置初始化不成功。
1、位置初始化失败
DTU直接连接个人电脑。
检查DTU信号灯，若非绿灯常亮，红灯闪烁，则重新配置DTU，检查配置流程是否出错。
检查千寻差分账号是否正在服务中，可以更换账号测试，切勿弄成千寻网站登录账号（本人就犯过这个错，浪费不少时间）。这里注意，即使差分账号配置错误，也是红灯闪烁，绿灯常亮的。
这几步做完，基本可以排除位置更新的大部分问题了。
如果还是失败，那么就需要分析千寻服务从串口返回的数据了，可以用串口工具UartTool查看是否有数据输出，并根据数据字段联系千寻厂家加以分析原因。（千寻服务很稳定，一般不会走到这一步）。
2、姿态初始化失败
惯导差分口直接连接电脑。
打开串口工具，我这里使用的是UartTool，查看惯导COM口是否有数据输出，检查数据状态是否正常（这里需要联系厂商）。
如果无数据输出，则检查波特率是否选错，都尝试一下。
有数据输出并且乱码了，则可能是hex数据，点一下hex显示即可。有些厂家用的差分口输出ins数据，直接发的GTIMU协议数据。
数据正常输出的话，可能需要重新配置一下。
命令行向串口发送config指令，查看输出结果，如图所示即正常。
继续发送指令CONFIG COM2 115200 8 n 1，观察是否有response ok字样返回，最后发送SAVECONFIG即可配置完成。（这里联系惯导厂家）
再用INS550D差分口直接连接电脑，打开上位机软件观察姿态能否初始化。
如果初始化失败，则用万用表量下550D的通讯线束通断情况，DB9差分口pin3对应DB15的10，DB9差分口pin2对应DB15的14，这两组通断，测量差分口和DB15。有DB15外壳上有引脚标志。
我手上拿到的这款INS550D输出数据有三路输出、输入接口 :
RS232：差分4G定位
RS422: 记录原始数据，即给用户使用的INS融合数据
CAN口：转发CAN口，可接四轮轮速，档位和方向盘转角
这里是R422口输出融合的ins数据，输出波特率默认为230400，这里可以通过上位机软件重新设定波特率和频率。
正常情况下从RS232差分口输出的是GPGGA/GPRMC协议格式的数据，如果想要GNSS原始数据，可以从差分口分两路出来，一路连接DTU，另外一路串口输出GGA和RMC格式数据。
RS422端口输出的是给用户的融合数据，这里是BD DB 0B为帧头的定长二进制数据。（有些厂商，比如星网宇达，直接输出类似差分口nmea数据，GPGGA/GTIMU/GPFPD等，传输的时候都是逐字节传，用char字符数组来接收数据，切割字符串即可）。
下面贴出550D的协议字段，解析程序非我写，所以只是大致说一下解析流程，同时也给出非字节流传输的，nmea数据的解析代码。
1.INS550D
以上协议为例分析，帧头为BD DB 0B，一帧完整的数据58字节，从serial流中读取字节，在每个buffer中去查找帧头，连帧头共58帧封装成一帧完整的数据，按字段和厂商提供的换算方式进行解析。最后用ROS发布出来，或sensor_msg/Imu、sensor_msg/NavSatFix标准格式，或者自定义msg格式。这里的难点在于，尽可能不要丢帧，注意超时处理。
2.星网宇达M2
一般来说，如果直接读取差分口数据，类似星网宇达(无DTU)，则可直接获取类似以上$GPGGA开头的nmea数据。这种相对来说比较容易，因为一行一帧数据，只需要校验每一行数据是否正常，无需手动通过帧头帧尾去分封帧。而是逐行读取，按字段分割即可，这里感谢自动化所飞哥提供的代码，网上也可以搜到很多类似的。
1while not rospy.is_shutdown():
2 g = 9.81
3
4 if (ser.read() != '$'):
5 continue
6
7 line0 = ser.readline() #逐行解析，按'，'分割
8 cs = line0[-4:-2]
9 cs1 = int(cs, 16)
10 cs2 = checksum(line0)
11 if (cs1 != cs2):
12 continue
13 line = line0.replace("\r\n", " ")
14 line = line.replace("*", ",")
15 words = string.split(line, ",")
16
17 # 协议字段解析
18 if words[0] == "GTIMU":
19 GPSWeek = string.atoi(words[1])
20 GPSTime = string.atof(words[2])
21 GyroX = string.atof(words[3])
22 GyroY = string.atof(words[4])
23 GyroZ = string.atof(words[5])
24 AccX = string.atof(words[6])
25 AccY = string.atof(words[7])
26 AccZ = string.atof(words[8])
27 Tpr = string.atof(words[9])
28 elif words[0] == "GPFPD":
29 GPSWeek = string.atoi(words[1])
30 GPSTime = string.atof(words[2])
31 Heading = string.atof(words[3])
32 Pitch = string.atof(words[4])
33 Roll = string.atof(words[5])
34 Lattitude = string.atof(words[6])
35 Longitude = string.atof(words[7])
36 Altitude = string.atof(words[8])
37 Ve = string.atof(words[9])
38 Vn = string.atof(words[10])
39 Vu = string.atof(words[11])
40 Baseline = string.atof(words[12])
41 NSV1 = string.atoi(words[13])
42 NSV2 = string.atoi(words[14])
43 Status = words[15]
44
45 if (ser.read() != '$'):
46 continue
47
48 line0 = ser.readline()
49 cs = line0[-4:-2]
50 cs1 = int(cs, 16)
51 cs2 = checksum(line0)
52 if (cs1 != cs2):
53 continue
54 line = line0.replace("\r\n", " ")
55 line = line.replace("*", ",")
56 words = string.split(line, ",")
57
58 if words[0] == "GTIMU":
59 GPSWeek = string.atoi(words[1])
60 GPSTime = string.atof(words[2])
61 GyroX = string.atof(words[3])
62 GyroY = string.atof(words[4])
63 GyroZ = string.atof(words[5])
64 AccX = string.atof(words[6])
65 AccY = string.atof(words[7])
66 AccZ = string.atof(words[8])
67 Tpr = string.atof(words[9])
68 elif words[0] == "GPFPD":
69 GPSWeek = string.atoi(words[1])
70 GPSTime = string.atof(words[2])
71 Heading = string.atof(words[3])
72 Pitch = string.atof(words[4])
73 Roll = string.atof(words[5])
74 Lattitude = string.atof(words[6])
75 Longitude = string.atof(words[7])
76 Altitude = string.atof(words[8])
77 Ve = string.atof(words[9])
78 Vn = string.atof(words[10])
79 Vu = string.atof(words[11])
80 Baseline = string.atof(words[12])
81 NSV1 = string.atoi(words[13])
82 NSV2 = string.atoi(words[14])
83 Status = words[15]
84
85
86 q_avg = tf.transformations.quaternion_from_euler(numpy.deg2rad(Roll), numpy.deg2rad(Pitch), numpy.deg2rad(Heading))
87 Imumsg.angular_velocity.x = numpy.deg2rad(GyroX)
88 Imumsg.angular_velocity.y = numpy.deg2rad(GyroY)
89 Imumsg.angular_velocity.z = numpy.deg2rad(GyroZ)
90 Imumsg.linear_acceleration.x = AccX*g
91 Imumsg.linear_acceleration.y = AccY*g
92 Imumsg.linear_acceleration.z = AccZ*g
93 Imumsg.orientation.w = q_avg[3]
94 Imumsg.orientation.x = q_avg[0]
95 Imumsg.orientation.y = q_avg[1]
96 Imumsg.orientation.z = q_avg[2]
97 Imumsg.header.stamp = rospy.Time.now()
98 Imumsg.header.frame_id = 'imu_node'
99 Imumsg.header.seq = seq
100
101 GPSmsg.latitude = Lattitude
102 GPSmsg.longitude = Longitude
103 GPSmsg.altitude = Altitude
104 #GPSmsg.status.status = Status
105 GPSmsg.header.stamp = rospy.Time.now()
106 GPSmsg.header.frame_id = 'gps_node'
107 GPSmsg.header.seq = seq
108
109 Velmsg.twist.linear.x = math.sqrt(Ve**2 + Vn ** 2)
110 Velmsg.twist.linear.y = 0
111 Velmsg.twist.linear.z = 0
112 Velmsg.twist.angular.x = 0
113 Velmsg.twist.angular.y = 0
114 Velmsg.twist.angular.z = numpy.deg2rad(GyroZ)
115
116 if (Status == '00'):
117 print("init")
118 if (Status == '01'):
119 print("coarse alignment")
120 if(Status == '02'):
121 print("fine alignment")
122 if(Status == '03'):
123 print("gps direction")
124 if(Status == '04'):
125 print("gps localization")
126 if(Status == '05'):
127 print("rtk")
128 print('pub')
129 pub_gps.publish(GPSmsg)
130 pub_imu.publish(Imumsg)
131 pub_vel.publish(Velmsg)
132 seq = seq + 1
133 rate.sleep()
参考：INS550D、MD-649说明手册。
感谢：一清科技各位大佬、广州导远电子程工、中科院自动化所飞哥、主线科技刘工、哈工大导航专家刘锦鲤刘博等。
以上是"组合导航调试与驱动实践"的全部内容，更多话题讨论、技术交流可以扫描下方二维码添加『Apollo小哥哥』为好友，进开发者交流群。
* 以上内容为开发者原创，不代表百度官方言论。
内容来自攻城狮の家：
http://xchu.net/2019/12/30/35nmea/，欢迎大家收藏点赞。已获开发者授权，原文地址请戳阅读原文。