[Lib库 1]CoppeliaSim差分避障小车的Simuli

2020-06-06 本文已影响0人 monkey61

前言

前言是一堆牢骚话，不影响教程阅读，可直接跳过~~
自从发布了连载5之后，已经有一年半的时间没有再更新教程了。做这个系列的教程最初的目的是为了让大家能够快速入门，掌握Vrep软件的使用和Matlab的一些协同仿真，但是越往后面做，涉及到的东西越多，知识和工具越具体，耗费的时间当然也是成倍的增加，因此关于机械臂的动力学建模，我一直在想用一个什么模式能够把这个讲清楚，而不是单纯的炫技（很多教程和书本都喜欢堆公式和简单问题高大上化(￣ω￣;)，但是总结下来就是那几个公式，分成牛顿欧拉法和拉格朗日法（其他的方法没有用过），这里还是建议直接看《机器人学导论（原书第4版）》这本书。换句话说，你学到了动力学这部分之后，对问题的理解就需要自己去看书了，教程只能说个大概，效率绝对没有老老实实啃书高。
当然不更新教程一方面是因为自己懒（机器人领域涉及太大，实在学不过来），另一方面是准备一个教程真的很费时间！！！这个不是和大家抱怨，因为每个教程都需要先回想我当时的状态和知识储备，然后怎么安排才能把这个教程的内容表达出来，然后准备案例，写代码注释，然后开始截图等等事情，期间至少两周所有的业余时间，过程真的非常折磨，然后发出去又没人看(￣ω￣;)，心理拔凉拔凉的。这就是恶性循环，如果世界按照往常一样，那这个教程可能真的像大部分教程一样，成为历史和回忆了。
然而一场疫情改变了全部的生活节奏，在家办公的日子一下子让自己有了很多业余时间，如果关注我淘宝店铺的同学们应该会知道我之前在店铺里发布过一个Vrep和Simulink的工具箱（现在Vrep都已经更名为CoppeliaSim了(⊙o⊙)…），这个工具箱是结合Simulink和vrep两大仿真工具的桥梁，方便搭建控制模型。说实话这几年python发展特别迅猛，在知乎上也是看到各种问题说是matlab的份额是不是被python取代了很多，这个的确是，但是要问Simulink是不是也是这样，那我只能说，Simulink目前毫无对手……这个就不展开讨论了，如果你没有用过Simulink，可以尝试学一学，毕竟现在Simulink的帮助文档已经大部分翻译成中文了。回到之前的问题，CoppeliaSim没有与Simulink的通信接口，没有办法连接起来，所以发布了第一个版本后，我们团队做了很多更新，但是淘宝店里很少人买这个库，就一直没有重新整理一下这个库的内容（给之前买库的老用户道歉了，(。・＿・。)ﾉI’msorry~），现在终于更新了，并且准备花一段时间去做一些例程，希望可以快速辅助大家入门这个库，并进行自己的开发。

Simulink开发优势

对于Simulink，首先不要认为Simulink是matlab的一个模块……Simulink是非常独立的，如果你不想用matlab语言的话，在Simulink你甚至可以不用写一行matlab的代码（额……有点嚣张了，不写matlab可能要写C……），所以Simulink他自己也是自成体系的。下面我截图MathWorks官网的Simulink介绍。

Simulink介绍

从上图中可以看出，Simulink的核心是基于模型的设计：从概念到代码，从这个可以引申出一大堆的概念，什么数字孪生、V字开发流程、预测性维护、代码生成等等，总之就是更新之前的开发流程，让开发者更容易开发，让调试者更容易调试，让维护者更容易维护。对于我们自身而言，Simulink在某些方面的开发效率会比matlab高一些，有些方面又没有matlab高，所以结合两者优势就可以很快的开发你的控制原型，下面我将使用Simulink和Stateflow这两个工具，演示如果搭建一个避障小车的模型。教程用到的库和模型请点击购买

避障算法思路

（算法思路部分可以看B站视频链接）
避障算法我们采用的一个基本逻辑是没有障碍物的话就朝向目标点运行，如果有障碍物的话就进行右转，然后沿着障碍物移动，直至没有障碍物，可以参照下面这张流程图。后面的Stateflow编写就是依据这张流程图，但是实际过程中Stateflow用的是状态机的模式，和流程图的有相似之处，但是又有不同点，状态机的图的话和Stateflow的程序就非常类似了，这里就不再贴图了。

流程图

小车模型构建

视频中小车模型构建没有讲，这里补充一些内容。
从提供的modelLib库中找到DiffCar.ttm模型，拖动到CoppeliaSim场景中，然后在菜单栏中Add添加一个Dummy，将其名字改为tar即可。

小车模型

然后往场景里添加长方体，配置参数如下：

长方体配置参数

长方体属性设置

多复制几个，利用平移或者旋转工具进行排放，修改地板大小，同时移动tar的位置，构建的模型效果如下：

构建的模型场景

至此，我们完成了模型的搭建过程。

Simulink模块搭建

我现在在用的是2019b版本的Simulink，界面和之前的版本差别很大，但是目前这台电脑只安装了这一个版本……所以，解决不了的话，就拥抱新版本的Matlab吧。

模型配置

新建一个Simulink模型之后，第一个要修改的就是Simulink的求解器，将其设置为定步长，步长为0.05s（与CoppeliaSim中的时间步长一致）。按照下图操作：

设置求解器

image.png

然后根据CoppeliaLib库自带的说明文件，将库添加到Simulink模型中，效果如下：

CoppeliaLib库界面

模型搭建

（下面的操作可以看B站视频了链接，下面就把一些重要截图放上来）
从库中寻找对应的模块添加到模型中，得到的效果如下（教程用到的库和模型请点击购买）：

从图中可以清晰看出每个模块的功能和类型，在构建复杂代码的时候也可以轻松进行阅读。而且每个模块都写了详尽的中文注释说明，以测距传感器为例，描述了每个接口的说明和意义，大家几乎不需要查看CoppeliaSim的函数帮助文档就可以直接使用，非常方便。

image.png

构造好于CoppeliaSim通信的接口模块后，建立一个Matlab Function模块，实现目标点处于小车的方位和距离计算。B站上的视频可能不是太清楚，这里直接放代码：

function [dis,theta] = getTarInfo(pos,gamma)
%计算目标点距离小车当前位置以及朝向

% y=zeros(1,2);
dis = sqrt(pos(1).^2+pos(2).^2);%计算小车到目标位置的距离
theta_abs = atan2(pos(2),pos(1));%计算在世界坐标系中，目标点相对小车的方位
%注意angdiff是第二个角度减去第一个角度，然后再转换到[-pi,pi]中
theta = angdiff(gamma,theta_abs);%转化为小车自身坐标系当中，目标点相对小车的方位

具体解释还是看视频吧（以后可能文字版更简单一点，这次录制视频踩的坑有点多，导致最后画面分辨率上不去，以后的视频应该会改善）。
这部分的搭建效果如下：

然后插入Stateflow的chart模块，添加输入输出和一些控制参数，如下图：

Chart中的Symbols配置

搭建Stateflow模型，最终的模型如下所示（教程用到的库和模型请点击购买）：

Stateflow模型

最后将生成的转向角和车速转化为两轮差速小车的两个轮子的转速（这里生成转向角是为了兼容前轮转动的车辆的），其代码如下：

function [vel_left,vel_right] = DiffCar(steer_angle,v)
%steer_angle表示转向角
%v表示运行速度
% 车辆参数
D=0.25;%左右轮距的一半

%计算计算角速度
w = steer_angle/0.05;

%调整轮速
vel_left = v-w*D;
vel_right = v+w*D;

最后模型搭建完成，就是下面这个样子

最终的Simulink模型

结语

这样就搭建完成整个控制流程了，我们再来总结一下，通过CoppeliaLib库可以将CoppeliaSim的工作和Simulink的构建独立开来，也就是建模型的负责建模型，搭算法的负责搭算法，大家合作起来可以更协调一些。对于有Simulink开发经验的同学来说，完成上述一个控制器是很容易的（前提是你有Stateflow的经验）；而对于一个学CoppeliaSim的同学，构建上面的仿真模型也是比较快的（学习完我写的连载1就可以实现了）；但是要想用代码控制小车，之前的手段是学习内嵌的Lua脚本或者是Matlab脚本（比如连载2和连载3），大家要研究CoppeliaSim究竟都有哪些api，api怎么调用等等，这些工作往往琐碎而且浪费时间、耗费激情（也就是所说的虚实结合，把注意力集中在你要研究的问题上为实，其他的实现方式和手段为虚）。结合两者，拥有CoppeliaLib库的你，专心学习Simulink搭建你的控制模型和算法，花费点时间搞明白CoppeliaSim模型构建的方式，就可以开始你的机器人仿真之旅了。下一期我会做一个用CoppeliaLib控制机械臂的例程，比连载5的更快、更简单。
最后，希望对看到这里的你有帮助，恳请一赞hi✿(。◕ᴗ◕。)✿