自从我们发布无人驾驶汽车研究系统(SDRS)以来,我花了一些时间来寻找创新的方法,以突破可能的范围,并提供令人兴奋的产品。在本博客中,我将讨论如何利用MATLAB Navigation Toolbox建立全局框架并定位两辆Qcar,以及如何使用QUARC™通信模块集实现它们之间的相互通信。
如果您和我一样是个曲棍球迷,下图你应该很熟悉。是的,这是一个曲棍球场的轮廓! 自从在家远程工作以来,我已经接管了我孩子的地下曲棍球场,并将其改造成为工作室。这是一个突出SDRS某些功能的好地方。
几周前,我决定挑战自己,开发一套算法,该算法将实现a,b,c三种功能。a:生成固定的全局框架并在同一张地图上定位两辆车,b:命令两辆车行驶到各个位置,c:让车辆彼此共享数据。
因此,希望在我描述下面的经历时,您能对作为一名研究人员在这个令人兴奋的领域所拥有的一些能力和一些选择有所了解。
我从两台Quanser QCars开始了我的探索,这是一个高保真的四轮移动计算平台,带有许多与自动驾驶汽车应用相关的传感器。几年前,当我们开始开发该平台时,关键设计思路之一是该系统不仅支持我们通常系统应用的MATLAB/Simulink环境,而且还为用户提供这个领域已经取得了不错的进展的其他环境,如Python、ROS、TensorFlow。对于我的应用程序,我希望依靠ROS的Hector SLAM算法为我提供一些自定位功能。也就是说,根据我开始的位置,当我在一个给定的空间里移动时,算法会从起点开始跟踪我的相对位置。然后,我计划在两辆车上运行相同的算法,我最初的假设是,如果我在已知的位置启动这两辆车,用这些坐标,我就能提取出两辆车运行的全局参考坐标系。最后,我打算将ROS信息发布到MATLAB或Simulink模型中,进行一些附加的传感器融合,最终使这些信息在车与车之间传递。
不幸的是,当我开发此程序并利用Hector SLAM算法时(作为Simulink用户),在ROS中调试算法遇到了问题。我确信,如果我回到办公室并得到了团队的全力支持,我将能够成功解决ROS方面的问题。
知道这些工具确实存在于MATLAB / Simulink环境中之后,我搁置了ROS,并决定尝试使用MATLAB Navigation Toolbox。尽管许多MATLAB工具都用于后期或脱机处理,但我想看看是否可以实时利用它们来实现对两辆车进行全球定位的目标。事后看来,我可能有点野心勃勃,但是到最后,这一切都是值得的,并迫使我学到了很多,我能够突出一些有趣的能力,在我们目前提供的无人驾驶汽车研究系统的基础上。
QCar的核心是NVIDIA®Jetson™TX2处理器。他们之所以脱颖而出,是因为他们提供了出色的处理能力,并且将其与GPU技术相结合。我们还为QCar提供了四个摄像头,你可以把它们拼接在一起,以获得360度全景,以及一个英特尔RealSense RGBD和深度摄像头。但在我的应用程序中,我使用了QCar的2D激光雷达。像很多人一样,我对激光雷达技术有一些了解,但是经过数周的经验,我对这种功能有了更深刻的了解。
当我开始探索导航Navigation Toolbox时,我很快意识到有大量内置示例的存在。它使我能够快速启动并运行一个简单的应用程序。下面的视频显示了Qcar在我的地下室自动行驶时我第一次测试的后处理结果(您可以在右侧看到曲棍球网)。每次激光雷达进行扫描时,都会将其融合,并得出一个精确的姿态估计。这在离线时效果很好。但当我试图实时完成这项工作时,数据很快变得难以应付,我的电脑无法跟上激光雷达生成的大量扫描。它似乎没有被配置成在车辆行驶时实时执行。所以,在这一点上,我有点担心,也许试图实现一个实时的全局框架比我想象的要难。
通过进一步挖掘,我发现了一篇由Hess等人发表的论文,[1],在工具箱文档中已引用。他们在Navigation Toolbox中开发了一种称为matchScansGrid()的算法。使用该功能进行了两次激光雷达扫描,并尝试给出一个相对于另一个的姿态估计–这本质上是我的目标。该算法的优势在于与代码兼容,这意味着我可以在Simulink中实现它,并为其生成优化的实时C代码。使用此功能的模型如下图所示,下面的文章里我将介绍其主要组成部分。
以下是我在一辆Qcar上使用上述模型的第一次成功视频之一。所示的地图是由第一次激光雷达扫描生成的,我使用它作为我的全局框架。当Qcar四处行驶时,我们会将逐级激光雷达扫描结果与我的全局帧扫描结果进行比较,然后估算出其位置和方向,并绘制在图中(四处移动的蓝色斑点就是Qcar)。如您所见,它在全局框架中对Qcar进行了很好的定位。
让我们仔细看看模型的关键组件。下面显示的部分捕获了初始和后续的激光雷达扫描。CompareScans嵌入MATLAB Function使用上面描述的matchScansGrid()函数来比较初始扫描(距离1)和每个渐进的雷达扫描(距离2),并以10厘米的分辨率计算车辆的相对姿态。
但是有一个小问题。即使我的激光雷达以20 Hz的频率扫描,我的算法也只能提取4 Hz左右的实时姿态数据,考虑到它的驱动速度,这不足以对我的系统进行定位。因此,我必须把这些数据与其他数据融合起来。
幸运的是,我的团队给我提供了QCar的自行车模型(如下所示)。它融合了汽车的实际速度(使用车载高分辨率编码器测量)、偏航数据(使用车载IMU)以及汽车的物理参数,从而得出我所谓的pose_dot的信息, 我能预估出汽车的行驶速度。然后,我使用了一个辅助滤波器来融合此姿态估计值(顺便说一句,它是以1KHz的速度计算的,因为它是基于汽车的内部传感器),其中较慢的真实姿态数据为我的校正项(以大约4 Hz的频率计算)得出我的最终姿态估计值。
我的模型还实现了一个名为driveing_states的简单状态机。它有两种状态:(1)汽车达到了预期目标;(2)汽车朝着预期目标行驶。一旦车辆在给定的半径范围内达到其目标,状态机就有助于将抖动和犹豫不决降到最低。
这是我第一次尝试在两辆车上运行模型的视频。在模型运行时,两辆车都自动驶向两个不同的命令目的地。如您所见,尽管两辆车都知道对方的位置,但在中途它们相撞了,这是因为我没有实施任何防撞措施。
最后,如您在下面的视频中看到的那样,在对模型进行一些调整之后,我能够使两辆车导航到两个不同的位置而不会发生碰撞。
研究人员欣赏我们系统的一个关键特征是:QCar是个开放式体系结构平台,执行编程好的操作。例如,您可以开发一个集中式系统,该系统可以根据每辆车的实时位置来更改运动计划。或者,您可以在车辆上开发避障算法。汽车中嵌入了大量传感器,可以共享位置数据,这些都可以实现。
总而言之,我达到了我的目标多亏了Navigation Toolbox。即为两辆汽车建立一个全局参照系,让两辆汽车相互通信,并命令车辆去我测试区域内的不同位置。
该文章由我们工程师研发总监Paul Karam所写
本文原链接:https://www.quanser.com/blog/real-time-localization-and-vehicle-2-vehicle-communication-using-the-matlab-navigation-toolbox/