3.RTK定位原理和数据解析

---

前言

我发现工程代码《圆曲线拟合和多项式拟合》的阅读量要少于其他文章，应该是这种工程代码实际应用的不多。
这两天没更新，原因是摔了一下，感觉受了内伤。
昨天工作中遇到了定位数据丢帧的问题，可能是驱动问题、系统阻塞、惯导本身问题。和朋友交流，采用ros系统，用422串口解析的话还真有可能出现串口通信不稳定的现象，串口通信稳定性问题可以再看看别的文章，建议使用can通信。本文介绍一下ROS实际应用中的定位数据解析。

一、RTK定位原理

差分定位是根据基准站给出的定位修正值去修正接收机的定位，得到高精度定位结果。采用位置差分满足车辆实际定位需求。基本流程如下图，最终得到的是差分后的坐标。

基于RTK的定位系统有基准站、移动站和数据通信连接构成。以千寻的CORS站作为基准站，INS550D为移动站，DTU为数据通信连接，该套组合导航设备在遮挡或者信号较弱的场景具有一定的保持作用。按照产品说明书《INS550D惯导安装说明和配置步骤 V2.05》进行配置，就可以在域控制器接收串口数据，按照产品协议进行数据解析，得到算法需要的定位数据。

二、ROS定位数据解析

在实际工程开发中，需要写好算法详细设计说明书，这里简单介绍一下汽车软件开发V模型。V模型大体可划分为几个不同的阶段步骤即：功能需求、功能开发、软件开发、软件集成测试、功能集成测试、整车集成测试（系统合格性测试），如下图所示，左边为需求分析和设计开发的过程，右边则为针对左边的测试验证，左边的每个过程是与右边的过程正好相对应。
我们开发人员一般会对系统需求、软件需求进行研究，然后对需求进行分析，拆分多个步骤（就是分工，多个模块），这个是架构设计阶段，会有各模块功能设计和模块间接口设计（RTE接口），对齐接口后，各模块并行开发，这里的接口也会有迭代，但一般在前期就会固定下来，避免后期开发因接口变动导致软件变更。这里又涉及到AUTOSAR架构了，不扩展了。各模块又称为SWC，也需要作需求分析，也就是模块内也要分工，毕竟一个人开发起来比较慢，此处一般有一个软件需求分析文档，划分不同模块（画个算法流程图，描述各模块功能），作内部接口设计，每个模块都对应详细设计说明书的设计方案，有追溯关系。这里不展开了，需求分析和详细设计都有规范，有兴趣的可以联系我或者自行百度。这两个文档算是算法开发人员比较重要的文字交付物了，此外我还写过源代码变更清单。
言归正传，对于定位数据解析，基本不用写什么算法，就是作数据解析。按照数据协议，接收串口数据，解析数据成需要的接口数据，该数据是堆其他模块可见的，其他模块可以直接调用。
用ROS开发，首先就是要新建节点，这里作串口数据解析，需要引入serial库，写好CMakeLists.txt，如果对此不太熟悉，可以参见《2.CMake介绍及CMakeLists.txt文件编写》。
核心是解析代码

（1）定义接收变量，并打开串口

// 一些准备
unsigned char	r_buf[1024];
bzero(r_buf, 1024);
fd = uart_open(fd, "/dev/ttyUSB0");
if (fd == -1)
{
	fprintf(stderr, "uart_open error\n");
	exit(EXIT_FAILURE);
}
if (uart_set(fd, BAUD, 8, 'N', 1) == -1)
{
	fprintf(stderr, "uart set failed!\n");
	exit(EXIT_FAILURE);
}

（2）循环解析数据

while (ros::ok())
{
	ret = recv_data(fd, r_buf, 58);
	for (int i = 0; i < ret; i++)
	{
		ParseData(r_buf[i]);
	}
}
// 解析函数内部
{
	if ((chrBuf[0] == 0xBD) && (chrBuf[1] == 0xDB) && (chrBuf[2] == 0x0B) && (cTemp == chrBuf[57]))
	{
		int offset = 0;
		int tempValue = 0;
		short tempInt = 0;

		tempInt = bca::charArray2Int(chrBuf, 3 - offset, 2);
		rp.roll = tempInt * 360.0 / 32768;

		tempInt = bca::charArray2Int(chrBuf, 5 - offset, 2);
		rp.pitch = tempInt * 360.0 / 32768;

		tempInt = bca::charArray2Int(chrBuf, 7 - offset, 2);
		rp.heading = tempInt * 360.0 / 32768;

		if (rp.heading < 0)
			rp.heading += 360;

		tempInt = bca::charArray2Int(chrBuf, 9 - offset, 2);
		rp.roll = tempInt * 360.0 / 32768; //deg/s

		tempValue = bca::charArray2Int(chrBuf, 21 - offset, 4);
		rp.lat	= tempValue * 0.0000001; //lon

		tempValue = bca::charArray2Int(chrBuf, 25 - offset, 4);
		rp.lon = tempValue * 0.0000001;

		tempInt = bca::charArray2Int(chrBuf, 33 - offset, 2);

		double speedx = tempInt * 100.0 / 32768;

		tempInt = bca::charArray2Int(chrBuf, 35 - offset, 2);

		double speedy = tempInt * 100.0 / 32768;

		rp.velocity = sqrt(speedx * speedx + speedy * speedy);

		tempInt  bca::charArray2Int(chrBuf, 52 - offset, 4);
		rp.gpstime = tempInt * 2.5 * 0.0001;

		unsigned char type = chrBuf[56 - offset];

		if (type == 32)
		{
			tempInt = bca::charArray2Int(chrBuf, 46 - offset, 2);

			if (tempInt == 32 || tempInt == 33 || tempInt == 34)
			{
				rp.status = 4;
			}
			else if (tempInt == 48 || tempInt == 49 || tempInt == 50)
			{
				rp.status = 4;
			}
			else 
			{
				rp.status = 5;
			}

			tempInt = bca::charArray2Int(chrBuf, 48 - offset, 2);
			rp.satenum = tempInt;
		}
	}
}
//最后进行数据发布

通过上面的解析代码，得到经纬度、航向数据，自动驾驶一般会选一个坐标原点，需要根据实际情况调整坐标原点处的坐标值。

---

总结

本文简单介绍了RTK定位原理和RTK数据解析，还引申了软件V型开发流程相关内容，如果大家对这个有兴趣，可以后续再结合实际项目写文章介绍一下，我这边主要是代客泊车和城市NOA项目。