一种方案验证阶段的融合定位数据处理方法与流程

本发明用于智能驾驶车辆方案验证阶段的融合定位数据处理和通讯，具体涉及一种方案验证阶段的融合定位数据处理方法。

背景技术：

1、智能驾驶车辆的定位功能开发是一个复杂的系统工程，针对地下车库的场景采用融合定位的方案，融合定位模块的数据来源主要有imu传感器、轮速传感器和环视相机高清图像。在方案验证阶段，需要对传感器实际性能、算法模型效果进行验证，还要针对不同方案进行快速部署和比较，还要考虑到传感器与本机的数据接入、本机内部的数据通讯和本机结果与下游单元的数据通讯，以及定位模块和其它模块的配合调试。其中imu和轮速传感器每条数据量较小，频率高（50-100hz），imu对于角度变化敏感，轮速传感器对于速度变化敏感，高清环视相机图像数据量大，对数据传输带宽要求较高，对原始图像的解析和处理资源消耗较大。相较于单一传感器，多传感器组合的定位方案理论上结果会更准确、更鲁棒，多种传感器的信息输入可以使得模型在不同场景下都能有至少一个输入为可靠有效的。其中高清环视相机在光线较暗或者光线变化强烈的场景下图像质量较差，并且鱼眼相机的畸变非常严重。在畸变程度大的区域图像质量非常差，不能直接提供有效真实环境的映射。所以多传感器组合的定位模块对场景具有更好的适用性，准确的定位结果作为规控输入，在理论上能够让智能车辆系统表现更好，提供给用户更好的体验。

技术实现思路

1、本发明提供了一种方案验证阶段的融合定位数据处理方法，针对地下车库的应用场景，综合考虑系统成本，在传感器的选择上避免使用激光雷达等昂贵的车载传感器，也舍弃掉需要对厂端进行专门改造的方案；兼顾系统实际应用效果，综合各传感器的优点，尽量保证在方案验证阶段的不同情况下至少有一种可靠的输入。

2、本发明提供了一种方案验证阶段的融合定位数据处理方法，包括：

3、数据通讯转发端进程对接收到的多个传感器数据分别进行预处理，并对预处理后的多个传感器数据分别进行封装；

4、针对占字节数小的各第一类传感器数据，数据通讯转发端进程采用消息队列的方式将封装后的第一类传感器数据转发到数据通讯接收端进程；针对占字节数大的第二类传感器数据，数据通讯转发端进程采用共享内存的方式将封装后的各第二类传感器数据转发到数据通讯接收端进程；

5、数据通讯接收端进程对接收到的第一类传感器数据进行航迹推算处理，对接收到的第二类传感器数据进行去畸变处理；

6、数据通讯接收端进程将经过航迹推算处理后得到的结果和对经过去畸变处理后得到的结果以ros格式的消息并发送至融合定位进程；

7、融合定位进程基于接收到的全部ros消息进行融合定位。

8、优选地，数据通讯转发端进程对接收到的多个传感器数据分别开辟线程进行数据异常值剔除预处理。

9、优选地，第一类传感器数据包括轮速传感器数据和imu传感器数据，数据通讯转发端进程采用消息队列的方式对封装后的各第一类传感器数据进行转发时，每一类第一传感器数据的消息队列的键值分别设置为一个对应的预设固定参数。

10、优选地，第二类传感器数据包括：环视相机图像数据，数据通讯转发端进程采用共享内存的方式对封装后的各第二类传感器数据转发时，

11、数据通讯转发端进程将接收到的各第二类传感器数据交替写入两片不同的内存；

12、数据通讯转发端进程在各片内存写入结束时，将包含图像对应的时间戳及对应的内存id的触发消息通过消息队列发送至数据通讯接收端进程的进程通讯接收模块；

13、数据通讯接收端进程的进程通讯接收模块在接收到触发消息后，基于内存id将对应的内存区域进行上锁，再进行数据读取，并在完成数据读取后释放锁。

14、优选地，针对不同类型的第一类传感器数据，数据通讯接收端进程的航迹推算模块从数据处理进程的进程通讯接收模块的消息队列中将imu数据读取出压入imu队列以及将轮速数据压入轮速队列；

15、若轮速队列中的元素数量大于2个，则航迹推算模块从轮速队列中取出队首元素，并拷贝第二个元素；

16、航迹推算模块利用取出的队首元素的时间戳进行二分查找；

17、航迹推算模块在imu队列中找到时间戳大于轮速队列中队首元素的时间戳的第一个元素；

18、航迹推算模块在imu队列中找到第一个元素之前的前一元素；

19、航迹推算模块从轮速队列中读取轮速脉冲因子和修正参数，并加载用户选定的航迹推算模型，利用从mu队列中找到的前一元素、轮速脉冲因子和修正参数进行航迹推算；

20、航迹推算模块将经过航迹推算处理后得到的结果以ros格式发送给融合定位进程。

21、优选地，针对第二类传感器数据，数据通讯接收端进程的鱼眼图像去畸变模块对从数据处理进程的进程通讯接收模块的内存中读取出的图像数据压入图像队列；

22、若轮速队列中的元素数量大于2个，则鱼眼图像去畸变模块从图像队列取出队首的鱼眼yuv图像，将队首的鱼眼yuv图像解析成rgb格式；

23、鱼眼图像去畸变模块从解析成rgb格式的鱼眼yuv图像中确定并提取出待处理相机图像；

24、鱼眼图像去畸变模块获取用户选定的鱼眼yuv去畸变模型；

25、鱼眼图像去畸变模块将提取出的待处理相机图像以ros消息发送到鱼眼yuv去畸变模型中，并建立与待处理相机数目对应的监听器；

26、当监听器监听到待处理相机图像时进行回调，触发鱼眼图像去畸变模块读取对应的待处理相机的畸变参数，并基于读取的畸变参数求解对应待处理相机的去畸变映射表；

27、鱼眼图像去畸变模块利用鱼眼yuv去畸变模型基于待处理相机的去畸变映射表，对待处理相机图像进行去畸变处理；

28、鱼眼图像去畸变模块将经过去畸变处理后得到的结果以ros格式发送给融合定位进程。

29、优选地，鱼眼图像去畸变模块从解析成rgb格式的鱼眼yuv图像中确定待处理相机图像的步骤包括：

30、若用户指定待处理相机参数，鱼眼图像去畸变模块结合用户指定的待处理相机参数，从解析成rgb格式的鱼眼yuv图像中确定用户指定的待处理相机图像；或

31、若用户未指定待处理相机参数，鱼眼图像去畸变模块将解析成rgb格式的全部鱼眼yuv图像确定为用户指定的待处理相机图像。

32、优选地，融合定位进程17中的视觉定位模块利用去畸变处理后得到的去畸变结果进行视觉定位，再将视觉定位消息发送给ekf融合定位模块，ekf融合定位模块基于视觉定位消息和经过航迹推算处理后得到的航迹推算结果输出融合定位结果，并将融合定位结果以udp的行驶输出到融合定位进程17中的规控仿真模块以供其进行处理。

33、本发明的有益效果为：

34、针对地下车库的应用场景，综合考虑系统成本，在传感器的选择上避免使用激光雷达等昂贵的车载传感器，也舍弃掉需要对厂端进行专门改造的方案；兼顾系统实际应用效果，综合各传感器的优点，尽量保证在不同情况下至少有一种可靠的输入，该方案验证阶段的融合定位数据处理和通讯系统主要由普通imu传感器、轮速传感器、高清鱼眼相机、数据传输设备、本地工控机和数据处理通讯软件组成，其中在通讯方面针对相机输入采用共享内存和消息队列的方法来保证效率和安全。