使用树莓派4B搭建简易的控制器硬件在环仿真平台

---

前言

在上篇教程中，基于运动学模型和dlqr离散时间线性二次型调节器原理搭建了轨迹跟踪的demo，恰好家里有一块树莓派，于是我突发奇想，把控制算法刷写进树莓派中，然后通过和Matlab/simulink的通信，Simulink中跑车辆模型。这样一个简单的验证平台就可以搭建起来啦。前提是这样的平台是完全不具备产品化的试验价值，该上Labcar或者Simulator的就上，由于个人买不起“大冰箱”，就自己搭建个简易的玩一玩，因此本次教程适合个人业余玩家，职业玩家谢绝参赛。本次教程只谈过程，细节问题不会描述的十分详细，有问题可以在下方评论区讨论。

一、实验准备

1.安装Ubuntu和Ros的树莓派开发板一块；
2.安装Matlab/Simulink的电脑一台；
3.网线，u盘等其他外设。

我对这类控制器在环仿真平台的搭建有两种理解。一种是带ROS的，一种是不带ROS的。我分别用两种架构图描述一下。

上图也是本次实验需要用到的，核心就是ROS架构。Matlab和树莓派都运行ROS。Matlab跑win，树莓派跑ubuntu。两个硬件用网线连接。需要您按照Matlab ROS工具箱。

上图是第二种我也会用到的一种架构。对于普通单片机跑C代码，通过USBCAN(Simulink仅支持以下四家的CAN设备：Kvaser;Peak;Vector;NI)接入电脑，使用Simulink车辆通讯工具箱完成CAN或者CANFD通讯。如果你使用Instrument Toolbox也可以使用RS232,TCP,UDP通信。这个看您的单片机通讯接口了。我平时会用到PCAN，这个相对来说便宜点。当然你也可以反向做快速原型操作，只是Matlab是非实时的，验证一下大致控制策略是没有问题的。

二、具体实施

1.Ubuntu ROS和Matlab通信

网络上许多教程都讲过如何进行ROS和Matlab的通信。为了大家看得懂，我就再讲一次如何连接。
首先用网线将树莓派和笔记本电脑连上，在树莓派的终端输入：ifconfig 即可查看树莓派的ip地址。

其次，在Home界面下键入 Ctrl +H 显示隐藏文件，双击打开 .bashrc 文件，在最后一行输入树莓派的IP地址，保存文件，退出终端。新建终端键入：roscore 启动ROS，观察ROS_MASTER_URI是否是树莓派IP，红框所示。


以上就完成了树莓派的操作。下面打开Matlab，输入以下代码，将树莓派作为主节点，Matlab作为从节点。

从图片可以看到Matlab与树莓派已经完成了通讯。无论从Ubuntu还是Matlab，都可以看到节点信息。

2.控制算法部署树莓派

下图即在上篇博客中搭建的轨迹跟踪控制算法，我将模型只留出控制算法部分，因为此部分才是刷写进实际控制器中的。

那么结合Simulink ROS工具箱，新建几个Topics，Message type选择ROS自带的。通过一顿操作即可完成下面的匹配，从下图可以看到。参考路径xr，yr，kr，thetar的数据保存在Model Workspace中。整个模型目前有三个话题。控制算法订阅了GPS话题(x、y、横摆角)和Speed(期望车辆行驶速度)话题，并且发布了Ctrl话题，其中包括速度和前轮转角数据。


如果您没有用过Simulink ROS toolbox，你可能对上面的模型不理解，建议跟着看两个简单的Examples就可以熟悉了。接下来，就需要对该模型进行C++代码生成，需要您的Simulink中有Embedded Coder工具箱。进入设置界面，进入Hardware Implementation 选择Hardware board为ROS。当然这里您用ROS2也是可以的。

然后回到模型界面，直接ctrl+B编译模型，等待编译完成(注意：如果您将树莓派作为主节点，请确保树莓派系统已经完成了roscore，不然会报丢主节点错误)。模型编译完成，会弹出代码生成报告。当然你不做SiL测试直接用代码也行，毕竟是个人玩家，不那么讲究。在你的Matlab工作目录下会出现一个tgz后缀的压缩包，名字和模型名字一样。

这里面就是节点代码，将这个压缩包解压到该名字的文件夹里，拷贝到U盘中，直接放入树莓派ROS工作空间的SRC源文件目录下。如下图所示，可以看到我的工作空间就这一个文件夹。

然后，编译该源文件即可。在工作空间输入 catkin build 即可。

等待片刻，可以看到已经将代码完成编译。报错是不会报错的，因为模型是正确的，这一点Matlab还是做得非常完美的，就是强(尬吹一下)。

编译完成过程，设置环境变量后，直接rosrun即可。接下来，节点就run起来，那么在matlab中输入 rostopic list就可以看到树莓派发布和订阅的话题了，至此在树莓派上的工作就全部完成了。

3.Simulink车辆模型环境

在此需要郑重声明下，Simulink不是实时仿真，TCP传输也会有延迟，所以验证算法只能说可以做，但是完全达不到实时的工业级别要求。既然树莓派发布了Ctrl话题以及需要订阅GPS话题和Speed话题(树莓派需要期望车速，那么车辆模型就需要提供接口，这里的模型用prescan也好，carsim也好，车辆动力学工具箱也好，都是可以的。将ROS订阅和发布匹配树莓派ROS即可。车辆模型我用了上一篇博客的车辆动力学工具箱模型，为了省事。下图就是搭建好的模型了，已经完全匹配了ROS节点。

需要注意的是：Simulink仿真中可以实时，需要Simulink real-time 工具箱中的模块，我的2019b没有此工具箱，就通过以下方式，使仿真和实际同步，这个原则就是在Simulink能计算得过来的时候同步，算不过来了还是同步不了，就会很慢。

搭建完成后，直接run起来就可以了。可以看见车辆还是按照期望轨迹跑了起来。我将硬件在环的仿真结果和离散仿真的实际结果进行了对比。


为了考虑树莓派的实际性能，我将控制周期设置为了50ms，并且黎卡提方程最大迭代次数为1500。离线仿真用的是10ms，黎卡提方程最大迭代次数为5000。可以看见，轨迹还是有差别的，具体因素太多了，这里就不一一分析了。

总结

本文针对个人玩家搭建的硬件在环平台，适用人群为个人玩家或者我这种在校学生用来做快速验证的。树莓派作为一款性能还算不错的嵌入式设备(比Arduino性能好多了)还是值得一用的，Jetson Nano也很好，Matlab也支持GPU代码生成直接部署。搭建这样简单平台的难点就在你能不能熟练地在Matlab环境下使用ROS工具箱，也大致浏览下help就可以快速上手。如果您将罗技G29或者游戏手柄装上，就可以实现驾驶员和控制器都在环的仿真啦！如果您对本次分享有疑问，评论私信交流。