大家好,欢迎来到IT知识分享网。
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项目。
免责声明:本站所有文章内容,图片,视频等均是来源于用户投稿和互联网及文摘转载整编而成,不代表本站观点,不承担相关法律责任。其著作权各归其原作者或其出版社所有。如发现本站有涉嫌抄袭侵权/违法违规的内容,侵犯到您的权益,请在线联系站长,一经查实,本站将立刻删除。 本文来自网络,若有侵权,请联系删除,如若转载,请注明出处:https://haidsoft.com/141470.html