MATLAB与ROS联合仿真——实例程序搭建思路_matlab与ros联合路径规划

一、基础运动控制实例程序搭建思路

1、需要完成的任务：

（1）通过设定小车运动的速度及转角来控制ROS中小车运动。

（2）通过键盘输入指令控制ROS中小车运动，键盘输入w小车前行，s小车后退，a小车左转，d小车右转，输入其他按键小车停止运动。

（3）订阅Gazebo中反馈的ROS小车的位置信息，并实时绘制出小车的运动轨迹图.

2、参考结构图如下

3、程序搭建思路

       通过Speed And Angle Given模块，给定小车运动的速度和舵机打角，并输送给Control signals Management C模块，通过Keyboard Control模块输出键盘控制系数和可控信号口，并输送给Control signals Management C模块，并使用Constant模块设定Control signals Management C模块所需的其他参数，对于参数key为了使用方便再为其添加一个Slider Switch模块，这样就可以在仿真过程中对key的值进行修改了，将Control signals Management C模块模块的输出值送给Publish Ackermann Sport Information C模块来输送给ROS，到这里控制部分就完成了。

      使用Subscribe Ackermann Location Information C模块订阅ROS中发布的小车的位置信息，并输送给Drawing Trajectories模块，将小车实时的位置绘制出来，本实验中并没有用到目标点位置和完整轨迹，所以可将目标点位置也赋值为小车当前位置，完整轨迹可使用constant模块赋值0

二、轨迹跟踪控制实例程序搭建思路

1、需要完成的任务：

（1）回顾实验一，发布运动控制信息至ROS、订阅ROS中小车的位置信息并绘制轨迹图、键盘控制等内容

（2）订阅IMU传感器的数据，并计算出机器人的姿态（方法自选，如：欧拉角、卡尔曼滤波、互补滤波等）

（3）回顾坐标系变换相关知识，推导纯跟踪算法

（4）实现目标点停车功能

（5）编写纯跟踪算法控制器，并调试参数实现目标点跟踪

（6）实现轨迹跟踪，调试参数，观察效果

2、参考结构图如下

3、程序搭建思路

         通过Eight Letter Track  Given C模块，给定小车当前时刻的目标点坐标以及完整的轨迹信息，并输送给Drawing Trajectories模块，进行绘制图像，同时当前时刻的目标点坐标也要输送给Pure Tracking Controller C模块，进行舵机打角控制。此外需要使用constant模块为Eight Letter Track  Given C模块指定参数key，为其再添加一个Slider Switch模块，这样就可以在仿真过程中对key的值进行修改了

        通过Subscribe  Ackermann  Robot Posture C模块订阅小车的姿态信息，并将其中的yaw输送给Pure Tracking Controller C模块，通过Subscribe Ackermann Location Information C模块订阅小车当前的位置信息，并输送给Drawing Trajectories模块以及Pure Tracking Controller C模块

        到这里我们的核心控制器——纯跟踪控制器，得到了小车的位置信息、姿态信息、以及期望的目标点的信息，我们还需要使用Speed  Controller C模块得到小车的速度信息，即小车以什么样的速度从当前点，前往目标点

        而要想使用Speed  Controller C模块得到小车的速度信息，需要使用Constant模块设定比例系数kp和最大速度，除此之外还需要提供当前点与目标点的位置差Xe和Ye，但是这两个参数是由Pure Tracking Controller C模块输出

        这里我们来捋一下Speed  Controller C模块与Pure Tracking Controller C模块的关系，只有Pure Tracking Controller C模块提供给Speed  Controller C模块Xe和Ye，Speed  Controller C模块才能计算出Speed提供给Pure Tracking Controller C，同样的只有Speed  Controller C模块将Speed提供给Pure Tracking Controller C模块，Pure Tracking Controller C模块才能计算出Xe和Ye，给Speed  Controller C模块，所以说这里陷入了死循环，谁也得不到自己想要的参数，也无法提供给对方想要的参数，那么怎样解决呢，这里就要用到memory模块了，这也是为什么在介绍Speed  Controller C模块时，在输出Speed之前一定要加一个memory模块了，通过使用memory模块，使Speed  Controller C模块退了一步，即使你不给我我想要的Xe和Ye信息，我可以把我上一次的Speed信息（对于第一次运行而言，就是默认值0），给你，然后整个循环就打通了。所以说memory模块功不可没

        解决了以上逻辑问题，再返回来看Pure Tracking Controller C模块，想要计算出Angle，还需要使用constant模块为其提供参数K和L

        将Pure Tracking Controller C模块和Speed  Controller C模块的输出输送给Target Point Parking Control C模块，还需要用constant模块提供给它key和accuracy参数，对于key同样为其再添加一个Slider Switch模块，这样就可以在仿真过程中对key的值进行修改了

        接下来的内容就跟实验一类似了，只不过Control signals Management C模块的speed和angle换为由Target Point Parking Control C模块提供

        通过Keyboard Control模块输出键盘控制系数和可控信号口，并输送给Control signals Management C模块，并使用Constant模块设定Control signals Management C模块所需的其他参数，对于参数key为了使用方便再为其添加一个Slider Switch模块，这样就可以在仿真过程中对key的值进行修改了，将Control signals Management C模块模块的输出值送给Publish Ackermann Sport Information C模块来输送给ROS，到这里控制部分也全部完成了。

三、SLAM建图及自主导航实例程序搭建思路

1、需要完成的任务：

（1）使用键盘控制小车运动，借助slam_gmapping功能包体验建图过程

（2）在gazebo地图的基础上绘制小车的运动轨迹

（3）借助move_base导航框架，体验在已知地图的情况下进行导航操作，在小车的运动路径上，添加障碍物，观察小车的避障能力，体会全局路径规划与局部路径规划的区别与配合

（4）订阅并观测小车搭载的深度相机反馈的RGB彩色图像及深度图像、点云图像

（5）将SLAM建图与导航结合起来，实现未知环境的自主探索与建图

（6）体验使用Simulink生成ROS代码，并在ROS中运行

2、参考结构图如下

 

3、程序搭建思路

（1）用键盘控制小车运动

        经过前面的实验，本部分内容大家应该已经不陌生了，使用Keyboard Control模块与Control signals Management C模块配合输出小车的运动控制指令，再通过Publish Ackermann Sport Information C模块将该指令发送给ROS即可实现

（2）观测深度相机发布的RGB图像、深度图像、点云图像

        通过Subscribe to depth and point cloud images和Subscribe to RGB images模块即可完成

（3）在gazebo地图的基础上绘制运动轨迹

        使用Subscribe Ackermann Location Information C模块订阅小车的位置信息，将其输送给Gazebo Track Plotting模块，Gazebo Track Plotting模块的输入参数的计算和调节是本功能实现的重点。

（4）自动发布和切换导航目标点

        使用Subscribe Ackermann Location Information C模块订阅小车的位置信息，将其输送给Navigation Point Setting C模块，完成导航目标点的控制，再由Publish Target Point Information C模块将其发送给ROS