Webots+Matlab：使用Simulink搭建RHex控制器（2）

在上一篇文章中我们完成了控制器的创建，接下来进一步对控制器进行设计，并搭建一个Simulink模型。

2.初始化机器人

在工作区创建新的matlab函数文件，将其命名为InitRobot

接下来调用接口API初始化各设备即六条腿的驱动电机、编码器和GPS、IMU和陀螺仪
为了让初始化获取的设备能够在整个工程中使用，因此将各设备声明为全局变量
首先在初始的启动文件中声明，并在InitRobot函数中调用
修改完成后两文件代码如下
RHex_Matlab.m

% MATLAB controller for Webots
% File:          RHex_Matlab.m
% Date:
% Description:
% Author:
% Modifications:

% uncomment the next two lines if you want to use
% MATLAB's desktop to interact with the controller:
%启动Matlab的界面
desktop;  
% 声明全局变量
global TIME_STEP motor encoder mGps mGyro mIMU
% 初始化机器人配置
InitRobot;

%开启键盘输入，打开该注释有助于调试，否则若设置有问题会闪退
%keyboard;

InitRobot.m

function InitRobot
global TIME_STEP motor encoder mGps mGyro mIMU
    TIME_STEP = wb_robot_get_basic_time_step();
    motor(1) = wb_robot_get_device('leg_motor1');
    motor(2) = wb_robot_get_device('leg_motor2');
    motor(3) = wb_robot_get_device('leg_motor3');
    motor(4) = wb_robot_get_device('leg_motor4');
    motor(5) = wb_robot_get_device('leg_motor5');
    motor(6) = wb_robot_get_device('leg_motor6');
    encoder(1) = wb_robot_get_device('position_sensor1');
    encoder(2) = wb_robot_get_device('position_sensor2');
    encoder(3) = wb_robot_get_device('position_sensor3');
    encoder(4) = wb_robot_get_device('position_sensor4');
    encoder(5) = wb_robot_get_device('position_sensor5');
    encoder(6) = wb_robot_get_device('position_sensor6');
for i=1:1:6
    wb_position_sensor_enable(encoder(i),TIME_STEP);
end
    mGps = wb_robot_get_device('gps');
    wb_gps_enable(mGps,TIME_STEP);
    
    mGyro = wb_robot_get_device('gyro');
    wb_gyro_enable(mGyro,TIME_STEP);
    mIMU = wb_robot_get_device('IMU');
    wb_inertial_unit_enable(mIMU,TIME_STEP);
    
end

修改完成后，点击运行，程序直接跑完并退出，由于我们没对机器人进行任何操作所以是正常的。
至此，机器人的初始化已经完成。

3.Webots+Matlab联合仿真运行机制猜想

在对simulink模型进行设计之前，我们有必要探讨一下webots与matlab直接进行联合仿真的机制。
以下内容仅是我自己的个人理解，并没有在webots提供的文档中找到相关的说明。因此并不能保证是正确的，如果有错误之处还请指正。
我对二者的工作机制的理解如下图所示

虽然二者均是独立运行的大型运算软件，但是我认为在webots 与matlab的联合仿真中还是存在一定的主从关系，即matlab是由webots启动的，当webots中的仿真终止时matlab也同时被关闭，这从启动仿真时webots控制台显示的内容可以窥见一斑

INFO: RHex_Matlab: Starting controller:
 "D:\Program Files\MATLAB\R2020a\bin\win64\MATLAB.exe" -nosplash -nodesktop -minimize -r launcher

首先由webots启动matlab，并建立连接，此时的webots会截取matlab调用webots的接口API时的请求。当我们单独运行matlab并执行launcher.m文件后，webots并不能获取载入的webots接口API时的请求，如wb_robot_get_device('motor')等函数的调用并不能被响应，因此连接的建立是必要的，通过webots启动matlab，建立的这一主从关系，从而截取matlab调用的webots的接口API时的请求。

接下来，当连接建立后，matlab也已经启动，此时需要做的是让matlab能够调用webots的接口API，即通过launcher文件，将webots中的C接口API载入到matlab中。

再以上工作完成后，matlab中即可随意调用webots的接口API，因为连接已经建立，因此不管在matlab的哪里执行webots的接口API都能被webots截取并进行响应。

以上过程的说明，只是为了让我们能够更加自如的在simulink的MATLAB Function中使用webots的接口API。

4.搭建simulink模型

打开simulink模块，创建一个空模型，新建一个Matlab_Function命名为Rhex作为控制系统的Plant
即该Matlab_Function从simulink中接收相应输入，然后输入到webots中，并从webots中获取机器人传感器的信息，从直观上来看，类似于我们常见的系统中的Plant
双击打开该函数，对其进行编辑
该函数的功能设置为

1. 读取输入，将其赋值给各电机
2. 读取机器人传感器信息并输出
3. 每循环一次调用一次wb_robot_step(TIME_STEP); 将simulink的仿真循环同步到webots的仿真循环
   该函数代码如下
   InitRobot.m

function [Pos,Vel,Angle,Wangle] = RHex(u)
coder.extrinsic('evalin');
motor = evalin('base','motor');
mGps = evalin('base','mGps');
mIMU = evalin('base','mIMU');
mGyro = evalin('base','mGyro');
TIME_STEP = evalin('base','TIME_STEP');
for i=1:1:6
    wb_motor_set_position(motor(i),u(i));
end

Pos = zeros(1,3);
Vel = zeros(1,1);
Angle = zeros(1,3);
Wangle = zeros(1,3);
Pos = wb_gps_get_values(mGps);
Vel = wb_gps_get_speed(mGps);
Angle =  wb_inertial_unit_get_roll_pitch_yaw(mIMU);
Wangle = wb_gyro_get_values(mGyro);
wb_robot_step(TIME_STEP);

注意
此处需要进行特别说明，由于Matlab Function的工作空间与基础工作空间并不相同，因此在使用base workspace中的全局变量时需要使用evalin函数将其从base workspace中载入。

evalin - 在指定的工作区中执行 MATLAB 表达式
    此 MATLAB 函数 执行 expression，它是一个字符向量或字符串标量，包含任何有效的 MATLAB 表达式，这些表达式使用工作区 ws
    中的变量。ws 的值可以为 'base' 或 'caller'，以表示 MATLAB 基础工作区或调用方函数的工作区。

除此之外，由于对于Matlab Function的work space来说，evalin 和各接口函数中调用的载入的WebotsC语言库API来说均为外部函数，因此需要使用函数coder.extrinsic进行声明，关于该部分的详细说明参加参考链接[5]
对接口函数进行处理的方法为，右键Matlab Function中的接口函数，打开

在所有被调用的函数上方添加其声明


否则会报错，提示找不到wb_xxx_xxx_xxxx
编辑完成后点击保存，并将simulink保存为RHex.slx
此时simulink模块中变为

接下来加一下辅助的模块

将输入设置为常数，将输出连接到示波器上，并将仿真时间设置为无穷inf
注意到此时我们的Matlab Function的输入即使机器人六条腿的电机角度
保存后点击运行，发现机器人能够运动

至此，simulink模块搭建完成，后续我会继续向simulink中添加一些有意思的模块，并实现机器人能够使用三足步态进行行走。

小结

在这里记录一个使用simulink进行调试的小技巧，不需要在主程序中添加任何代码，在webots调用matlab建立连接之后，我们只需要双击打开RHex.slx文件或将其拖到命令栏运行即可
使用该方法能够自由的在simulink中暂停、终止或重新开启仿真调试而不需要再webots中进行反复的操作。
启动过程如下：

1. 在webots中将控制器选择为RHex_Matlab
2. 点击启动仿真
   
3. 运行后webots调用matlab，matlab启动并完成起始文件的运行
   
4. 将RHex.slx文件拖入命令行窗口，打开simulink模块
   
   注:首次打开速度极慢，需要耐心等待，不过再次打开会很快
5. 点击启动仿真，可以看到webots中的机器人发生运动，并且simulink中可以随时终止，并重新开始调试
   
   注:由于我的机器人腿位置初始设置为1.5弧度使其趴下，所以此处输入给0让其站起来
6. 效果
   

参考链接

[1] Webots+Matlab Simulink youBot Kuka Demo
[2] 开发经验分享(3) - Webots + Matlab/Simulink联合仿真方法
[3] Simulink+Webots基于MPC的移动机器人轨迹追踪仿真（3）
[4] Why you should never break a continuous algebraic loop with a Memory block
[5] Use MATLAB Engine to Execute a Function Call in Generated Code