[Lib庫 1]CoppeliaSim差分避障小車的Simulink實現

前言

前言是一堆牢騷話，不影響教程閱讀，可直接跳過~~
自從發布了連載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?(。???。)?

最后編輯于 ：
?著作權歸作者所有,轉載或內容合作請聯系作者
平臺聲明：文章內容（如有圖片或視頻亦包括在內）由作者上傳并發布，文章內容僅代表作者本人觀點，簡書系信息發布平臺，僅提供信息存儲服務。