MATLAB与ROS联合仿真——控制类功能模块介绍

1、Keyboard Control

（1）输入参数：无

（2）输出参数：Speed Factor为输出的速度系数（1代表前行，0停止，-1代表后退），Turn Factor为输出的舵机系数（1代表左转，0代表前行或后退，-1代表右转），Switching Signals为一路控制信号，当键盘输入0时，该参数输出0，键盘输入1时，该参数输出1。

（3）主要作用：键盘输入w小车前行，键盘输入s小车后退，键盘输入a小车左转，键盘输入d小车右转，键盘输入0第三个端口输出0, 键盘输入1第三个端口输出1，键盘输入其他按键小车停止运动。

（4）内部结构：

（5）具体实现过程：从实验资料中获取Keyboard模块，该模块有两个输出，当你从键盘按下一个按键时，第一个输出会输出其对应的Ascall码值，第二个输出我们并不关心。

接下来使用if模块分情况进行讨论，按照我们的需求，将keyboard第一个输出的分为119、115、97、100、48、49和其他7种情况

接下来使用if action模块配合if模块，将if模块所分的7种情况，配置其对应的输出，比如当if模块输出为119时，也就是对应键盘输入了w，此时我们想让小车直着往前走，那么速度系数应该输出1，转向系数输出0，为了简洁，我们将其用constant模块写成向量[1 0]的形式，if模块输出为119时对应的if action模块如下图所示：

以此类推，当键盘输入s时对应的if action模块应该输出[-1 0]，当键盘输入a时对应的if action模块应该输出[1 1]，当键盘输入d时对应的if action模块应该输出[1 -1]，当键盘输入其他按键时对应的if action模块应该输出[0 0]

以上五种情况每种情况各对应一个包含两个元素的向量，但是他们应该对应于同一个变量，因此需要使用merge模块将其融合为一个变量

此时我们想将速度系数与转向系数进行拆分，并独立输出，此时就要用到Demux模块

如果你想看一下输出的数字是多少，可以使用display模块，到这里前五种情况就配置好了，对应的第一个和第二个输出也就配置好了，还剩下的第六第七种情况对应第三个输出，有了上面的介绍大家可以很简单的写出，就不介绍了，不过有一点需要注意，为了让默认情况下（当键盘没有输入0和1时，该变量默认为0）小车是运动的，当键盘输入0时，应该输出1，键盘输入1时，应该输出0

（6）其他说明：无

2、Pure Tracking Controller C

（1）输入参数：speed为小车的运动速度，k为需要调节的纯跟踪算法的系数，L为小车前轮与后轮之间的距离，如实验中用到的阿克曼小车为0.335，单位m，yaw是小车坐标系（机器人坐标系）X轴与世界坐标系X轴之间的夹角，xr和yr为小车在世界坐标系下当前的位置坐标，X和Y世界坐标系下当前目标点的位置坐标

（2）输出参数：angel为舵机打角（弧度制），Xe和Ye分别为世界坐标系下当前位置与目标位置的横纵坐标之差

（3）主要作用：在已知小车当前位置及目标位置的情况下，通过纯跟踪算法计算出小车应该输出的舵机打角，从而控制小车从当前位置移动到目标位置，同时输出目标位置与当前位置的距离差

（4）内部结构：

（5）具体实现过程：在这里大家需要回顾机器人学中的坐标变换的相关知识，及简单了解纯跟踪算法的推导过程及工作原理，然后就可以写出以下的纯跟踪控制器的代码

（6）其他说明：参考资料

    

   机器人学基础知识回顾

      纯跟踪算法推导过程

（9中的sina可以用8和ld=K*Vx进一步化简）

3、Target Point Parking Control C

（1）输入参数：key为预留的交互按钮接口，用来控制该模块是否发挥作用，当key为1时开启目标点停车，为0时关闭目标点停车 ，inspeed和inangle为小车的速度和舵机打角，Xe和Ye分别为世界坐标系下当前位置与目标位置的横纵坐标之差 ，Accuracy用来设定目标点停车的控制精度，由于阿克曼转向车的灵活性有限，很难完美的到达我们设定的停车点，这个参数用来设定我们允许的误差是多少，该参数不宜太小，太小后一直达不到，阿克曼小车到达目标点附近时就会不停的围绕目标点画圈，比如车速为0.7m/s时，可设定该误差为0.1m左右

（2）输出参数：outspeed和outangle经过该模块控制后的小车的速度及舵机打角，当启用该模块时，到达目标点的误差范围之内时，outspeed和outangle输出值均为0，其他情况下 outspeed和outangle的输出值分别与输入的inspeed和inangle相等

（3）主要作用：当启用该模块的情况下，若小车到达目标点的误差范围之内，则将小车的速度及舵机打角设为0，使小车停止运动

（4）内部结构：

（5）具体实现过程：根据以上的介绍，不难写出以下代码

（6）其他说明：无

4、Speed  Controller C

（1）输入参数：kp为需要调节的比例系数，Xe和Ye分别为世界坐标系下当前位置与目标位置的横纵坐标之差 ，speed_max为设定输出的速度上现是多少，实验中车身长为0.335m，结合实际设置即可，比如可设为3m/s

（2）输出参数：speed为小车的运动速度

（3）主要作用：根据小车的当前位置与目标点位置的距离差，对小车的速度进行调节，距离差越大输出速度就越大，通过比例系数kp进行调节，来达到更好的跟踪效果

（4）内部结构：

（5）具体实现过程：根据以上的介绍，不难写出以下代码

（6）其他说明：注意一定要在speed的输出之前加一个memory模块，这里涉及到逻辑问题，在实验中会对此进行介绍

5、Control signals Management C

（1）输入参数：key为预留的交互按钮接口，用来控制是否开启键盘控制，当key为1时开启键盘控制，为0时关闭键盘控制 ，speed和angle为小车的速度和舵机打角，Speed_Factor和Turn_Factor为键盘控制模块输出的小车的速度系数和舵机打角系数，Switching为键盘控制模块输出可控0和1信号，key_Speed_Set和key_Angle_Set为使用键盘对小车的运动进行控制时的小车速度和舵机打角，均应该设为正数，比如0.7和0.4

（2）输出参数：speed为小车的运动速度，Angle为小车的舵机打角

（3）主要作用：对小车的控制信号进行宏观控制，避免小车同时接受键盘的控制信号和跟踪轨迹或目标点时控制器输出的控制信号而造成的运动冲突，当key的值为1时，该模块切断跟踪目标点或轨迹控制器输出的speed和angle信息，小车仅接收来自键盘的控制信息，并根据键盘信息与key_Speed_ Set和key_Angle_Set的值配合得到并输出小车的Speed和Angle，当key的值为0时，小车按照跟踪轨迹或目标点时控制器输出的控制信号进行运动，此时可使用键盘输入0来停止运动或键盘输入1来继续运动，而除此之外的键盘输入信息被屏蔽，不再发挥作用。

（4）内部结构：

（5）具体实现过程：根据以上的介绍，不难写出以下代码

（6）其他说明：在键盘控制模块中，键盘输入0时，Switching输出为1，键盘输入1时，Switching输出为0，要想达到输入0小车停止运动，输入1小车继续运动的效果，需要用（1-Switching） 作为系数。

6、Navigation Point Setting C

（1）输入参数：xr和yr是当前小车的位置，point是设定的一系列导航目标点，使用constant模块以一维向量的形式给定，可以给定任意多个点，比如想要小车依次经过点（5，6）点（3,4），则point输入端输入为[5 6 3 4]

Error_accuracy用于设定与目标点距离多近时，认为已到达目标点，可设为0.2，用于多个目标点的自动切换

（2）输出参数：x和y是当前给定的目标点的坐标

（3）主要作用：给定一系列目标点，根据小车的实时位置，自主判断是否已经到达当前的目标点，若已到达，则自主将目标点切换为下一个目标点。可用于对未知环境的自主探索及建图

（4）内部结构：

（5）具体实现过程：根据以上的介绍，不难写出以下代码

（6）其他说明：实现过程中一定要加pause（）函数，比如pause（1），每一秒发送一次目标点，用于间歇性发送