MATLAB Simulink开发ROS无人车与机器人应用 详细教程

引言：MATLAB在机器人中的应用

现在大多数机器人开发者都会选择ROS，在ROS整个框架下“调包”极其容易。很多ROS开发者热衷于“调包”来实现功能，却难以在机器人学的理论知识上有所突破。MATLAB的使用者则以理论研究人员为主，很多机器人学的硕士或者博士都需要靠MATLAB来验证理论，但是缺少将算法部署在机器人上的实际经验。ROS和MATLAB是非常好的能够将机器人开发的理论与实践相结合的工具。大量的机器人方向研究者，希望能将ROS和MATLAB结合起来。

我在研究生阶段，主要的开发工具就是MATLAB和C++，当时用MATLAB做过粒子滤波算法，以及用Robotics Toolbox求解双足机器人的逆运动学，还完成过现代控制、神经网络和机器视觉课程作业。后来研究机器人空间认知的时候就是MATLAB和ROS相结合，那时候工具箱不完善，很多功能都要靠自己搭建，非常耗费时间。下面的回答中，提及了很多我在MATLAB中的工作，算得上资深的MATLAB用户啦。

  研究SLAM，对编程的要求有多高？（https://www.zhihu.com/question/51707998/answer/127192245）

在职业生涯的大部分时间里，我觉得MATLAB就是一个科研工具，离实用有比较大的差距。甚至一直都觉得用Python编写的ROS程序和MATLAB程序相比，效率和可靠性肯定更高。

然而，最近在刷知乎的时候看到一篇回答，有点颠覆我的认知。

MATLAB在逐渐被Python淘汰吗？

https://www.zhihu.com/question/367881424/answer/2415598493

大致是说在和硬件结合的时候，在工业用途上，Python相比于MATLAB就是弟弟。

通过ROS Toolbox Simulink工具与真实机器人进行连接

这几年MATLAB不断升级对ROS的支持，现在连接非常容易。通过ROS无人竞速车和MATLAB结合，可以快速进行无人车的算法研究和开发。官方油管上也有视频介绍如何将MATLAB和ROS无人车结合进行入门学习[1]。

我们采用Tianracer（https://github.com/tianbot/tianracer）ROS无人竞速车，在MATLAB中通过Simulink与ROS通信。Tianracer是阿克曼结构，搭载一个单线激光雷达和鱼眼镜头。Tianracer是一个软硬件完全开源的项目，国内不少课题组也采用了Tianracer进行无人车算法开发[2]。

ROS开源项目：（一）中文语音交互系统ROSECHO （二）教学级别无人车Tianracer

https://zhuanlan.zhihu.com/p/90182597

控制背景的研究者都比较熟悉Simulink。通过Simulink连接ROS，大多数操作都可以通过图形化界面实现，低代码的方式也比较适合入门学习者。要和ROS通信并使用完整功能，需要在Simulink APP中打开ROS后设置网络，并且在订阅消息模块中设置ROS网络。注意要设置好ROS的安装目录以及工作空间以便后面Coder部署的时候使用。

◼ROS端的网络设置，ROS版本及工作空间设置
这时候可以将无人车启动。ROS无人小车都差不多，Tianracer的话需要ssh进入系统执行。

roslaunch tianracer_bringup tianracer_bringup.launch
后面建模使用消息接收模块的时候也需要配置ROS的网络。同时要根据消息发布频率设置Sample Time，仿真时间才能和实际时间符合。

◼订阅一个30 FPS的USB摄像头数据
ROS和MATLAB Simulink打通后，只要心中有架构，就可以通过拖拉拽的方式来快速进行很多功能的搭建，例如可以用PID Tuner来进行电机线形控制的整定，可以用Pure Pursuit实现移动机器人路径跟踪，还可以用Image Classifier进行图像处理。

用MATLAB Simulink实现ROS无人车纯跟踪算法

纯跟踪是移动机器人路径规划中比较简单但却实用的一个模型。我们试着从头开始在Simulink中搭建一个阿克曼结构无人车的纯跟踪模型。

 ◼在Simulink库中选择Pure Pursuit纯跟踪模块

输入参考路径，然后利用Pure Pursuit模块进行跟踪，将执行信号传送给Robot Model。

打开Robot Model的子模块，这里要根据无人车的话题名称修改输入输出。对于Tianracer无人车，就是需要下发控制指令（/tianracer/ackermann_cmd），接收里程计信息（/tianracer/odom）。大家如果自己有ROS无人车，可以根据自己的小车话题进行修改。当然Gazebo仿真中的ROS无人车也是可以的。

Robot Model部分的内部构建如下所示： 

 

修改完成后可以在参考路径Reference Path中输入数据就可以运行了。这里仅用里程计信息作为最终位姿进行反馈。要在真实环境中稳定运行的话，还需要建图，利用AMCL定位输出的Pose作为反馈。这个可以参考《编队跟随的运动学推导与纯跟踪》的视频，在ROS中利用全局路径或者样条曲线生成参考路径。

一定要注意对于在真车上的测试，开始要尽量给小一点的数值。这个模块还可以扩展激光雷达进行避障。

如果测试没有问题，就可以通过Build & Load直接生成独立的代码，通过刚才配置的SSH和工作空间，将模型部署到机器人上，就可以脱离Simulink运行无人车纯跟踪算法。

◼Simulink的模型可以直接部署到ROS机器人上
在MATLAB中还有很多运动规划的模块，比如使用Mini Snap多项式或者模型预测控制MPC来生成车辆运动控制的信号。在最近更新的MATLAB R2022b中还加入了用神经网络做预测模型。

通过MATLAB Simulink进行深度学习图片分类

上面提到的是对ROS无人车进行简单控制，更“切题”的是采用深度学习的方法处理无人车摄像头采集的画面，我们依然是通过Simulink来完成。MATLAB因为矩阵运算的基因，很适合现代控制理论和计算机视觉等需要大量矩阵运算的方向。实际上MATLAB在深度学习上也是非常强大的，有很完善的深度学习工具箱。只是相对缺少MATLAB和ROS机器人结合的例程。现在图片分类主要都是基于深度学习的方法，我们可以采用googlenet进行分类[3]。需要有Deep Learning Toolbox的Image Classifier组件并且下载googlenet的网络模型。MATLAB中有很多预训练的网络，可以根据准确度和速度要求进行选择[4]，我所知道的ResNet和DarkNet都有预训练模型。最新的网络可以在MATLAB深度学习的Github上看看（https://github.com/matlab-deep-learning/MATLAB-Deep-Learning-Model-Hub）。

◼简单的图片分类功能
模型很简单，我们将画面在MATLAB中实时显示，分类结果在连接ypred输出的Display模块中，大家可以看到coffeeMug_label的字样，正如画面内容。将信号通过话题再传回ROS也可以通过Simulink模块来完成。低代码的方式直接获取机器人上的图像采集数据进行识别，对于入门来说非常方便。想要切换不同的网络，只需要双击Image Classifier模块设置一下就好。

深度学习进行物体检测

除了Simulink的方式和ROS通信，我们也可以编写Script和ROS通信。这样就很容易将硬件捕获的图像传入MATLAB，再进行图像处理或者使用深度学习进行分类或者物体检测。MATLAB中有很多利用深度学习进行的物体检测，场景分割，数字、手势识别的例程，也有无人车和移动机器人相关的视觉SLAM，点云处理，运动估计等例程。

这里，我们尝试利用无人驾驶的Kitti数据集（kitti_2011_09_26_drive_0001_synced），用ROS进行数据包的播放，然后用MATLAB接收图像并使用Yolo V4进行物体检测 。我们先根据Detecting objects usingYOLO v4 object detector教程分别使用tiny-yolov4-coco和csp-darknet53-coco网络进行检测。然后使用MATLAB深度学习Github上的预训练网络进行检测（https://github.com/matlab-deep-learning/pretrained-yolo-v4）[5][6][7]。

使用 https://github.com/tomas789/kitti2bag 工具可以将Kitti数据集转换成rosbag （这里也不赘述了，大家可以自行探索，有问题可以评论区留言）。首先要启动roscore并在ROS端播放数据集。

下面我们在命令行窗口输入指令，以tiny-yolov4-coco网络为例。根据无人车的ROS Master 地址进行初始化。

rosinit('192.168.0.37')
然后定义一个空图像消息

emptyimg = rosmessage("sensor_msgs/Image",DataFormat="struct")
 订阅摄像头消息 

img_sub =rossubscriber("/kitti/camera_color_left/image_raw","DataFormat","struct")
接收

img_data = receive(img_sub,10)
然后转换成正常MATLAB的图像格式   

img = rosReadImage(img_data)
获取了正常的图像画面，可以用深度学习来进行处理。这里我们使用tiny-yolov4-coco

name = "tiny-yolov4-coco";
detector = yolov4ObjectDetector(name);
检测

[bboxes,scores,labels] = detect(detector,img);
显示图片及标注结果

detectedImg = insertObjectAnnotation(img,"Rectangle",bboxes,labels);
figure
imshow(detectedImg)
看一下效果，虽然tiny-yolov4-coco网络小，检测速度快，但是识别的精度就差强人意。

当然，除了检测结果，还可以使用tic toc来测试识别速度，根据我们的硬件性能和精度要求选择合适的网络。

结语

介绍了几个入门级的Demo，能够体现出MATLAB在ROS无人车上应用的方便与强大。将用于算法研究的MATLAB和支持最多机器人硬件的系统的ROS相结合后，像打开了潘多拉魔盒，研究不用只停留在仿真或者数据集上。而且，除了上面介绍的控制，图像识别等内容，MATLAB与ROS机器人在研究多机协同、强化学习等方面应该也有巨大的潜力。最后，MATLAB有了ROS的支持可以进行机器人硬件在环的仿真测试，又可以通过Coder部署，可以让程序在生产环境运行，让ROS的开发者也能得到MATLAB工业上强大生态链的支持。

参考：

Develop Autonomous Algorithms using ROS 

https://www.youtube.com/watch?v=TDTYFJA3L5w

Decentralized Planning for Car-Like Robotic Swarm in Unstructured Environments 

https://www.bilibili.com/video/BV11e4y1n7JL

使用 GoogLeNet 对图像进行分类

 https://ww2.mathworks.cn/help/deeplearning/ug/classify-image-using-googlenet.html

预训练的深度神经网络

 https://ww2.mathworks.cn/help/deeplearning/ug/pretrained-convolutional-neural-networks.html

Yolo V2 

https://ww2.mathworks.cn/help/vision/ug/multiclass-object-detection-using-yolo-v2-deep-learning.html

Work with Specialized ROS Messages 

https://www.mathworks.com/help/ros/ug/work-with-specialized-ros-messages.html

https://www.mathworks.com/help/vision/ref/yolov4objectdetector.html