MATLAB与Gazebo联机


在MATLAB中有关于MATLAB与Gazebo的联合仿真的例程“Control LBR Manipulator Motion Through Joint Torque Commands”，好奇之余便尝试了一下。下面做一些简单的总结。

1.ROS基础


首先简单介绍一下ROS的基本知识。

ROS Network由一个ROS master 和多个ROS node组成。


每个node在被创建后必须先在ROS master上进行登记(register)，于是ROS master中记录了所有node的地址等信息。然后各个node就可以在没有ROS master参与下进行node之间的数据交换。

在这里插入图片描述

MATLAB提供了两种方式参与ROS Network。一种是通过MATLAB创建ROS master，另一种是创建global node来连接外部的ROS mater（如另一台电脑上运行的robot）。

%% 1.Create a ROS Master in MATLAB
rosinit
%% 2.Create a global node and connect to an External ROS Master
% '192.168.1.104':The IP address of the External ROS Master
% 12000:The running port of the External ROS Master. The default port is 11311.
rosinit('192.168.1.104',12000)

2.MATLAB与Gazebo的联合仿真例程

下面介绍一下例程“Control LBR Manipulator Motion Through Joint Torque Commands”的大致流程和结构。

Step 1：建立ROS连接


首先需要创建一个ROS node，然后连接上外部的ROS Master。在这个例程中，外部ROS Master特指在虚拟机上运行的Gazebo。因此，在连接之前，需要先把虚拟机上的相关Gazebo程序打开，然后才能让MATLAB顺利连接上ROS Master。*“192.168.1.104”*是我电脑上的虚拟机的IP地址，可以通过ifconfig查看。

ipaddress = '192.168.1.104';
rosinit(ipaddress,11311);

顺便一提，只要知道了虚拟机的IP地址，任意一台电脑都可以通过上述MATLAB代码连接上ROS Master。当然，你也可以在一台电脑上完成MATLAB与Gazebo的连接。

在连接之前，可以通过ping查看IP地址之间连接是否通畅。如果无法顺利连接，可能是以下原因之一：

没有打开相关防火墙设置，需要手动开启下图中被选中的入站规则；


检查两个IP地址是否为同一区段的，同一区段的可以直接连接，但不同区段可能会出现无法连接的情况。比如一个IP是在一个路由器下分配的IP，另一个IP是外部的.


Step 2：建立机械臂模型


这步就不再赘述了，例程中是直接导入已经存在的urdf文件来导入机械臂。你也可以使用3D建模软件来自定义机械臂。

lbr = importrobot('iiwa14.urdf');
lbr.DataFormat = 'row';
lbr.Gravity = [0 0 -9.80];

Step 3：轨迹规划

在满足位置、速度和加速度曲线平滑的前提下，根据已知的必须经过的点来生成相应的轨迹。

% 导入tWaypoints和qWaypoints数据
% tWaypoints:各个点对应的关节角度
% qWaypoints:各个点对应的时间戳
load lbr_waypoints.mat
cdt = 0.001;
tt = 0:cdt:5;
% 轨迹生成
[qDesired, qdotDesired, qddotDesired, tt] = exampleHelperJointTrajectoryGeneration(tWaypoints, qWaypoints, tt);

Step 4：预计算关节转矩

根据上述生成的轨迹点，通过逆动力学计算公式求出运行到各个轨迹点所需要的各个关节转矩，并将其存入矩阵*“tauFeedForward”*当中。

n = size(qDesired,1);
tauFeedForward = zeros(n,7);
for i = 1:n
    tauFeedForward(i,:) = inverseDynamics(lbr, qDesired(i,:), qdotDesired(i,:), qddotDesired(i,:));
end

该例程采用的控制方式是基于动力学前馈的PD控制法。也就说，先通过动力学公式计算出所需关节转矩，在实际运行时再用PD控制器来修正误差，从而实现比较精确的机械臂控制。

Step 5：ROS设置

% 创建Publisher和Subscriber
[jointTauPub, jtMsg] = rospublisher('/iiwa_matlab_plugin/iiwa_matlab_joint_effort');
jointStateSub = rossubscriber('/iiwa_matlab_plugin/iiwa_matlab_joint_state');
% 定义message的组成
mdlConfigClient = rossvcclient('gazebo/set_model_configuration');
msg = rosmessage(mdlConfigClient);
msg.ModelName = 'mw_iiwa';
msg.UrdfParamName = 'robot_description';
msg.JointNames = {'mw_iiwa_joint_1', 'mw_iiwa_joint_2', 'mw_iiwa_joint_3',...
                  'mw_iiwa_joint_4', 'mw_iiwa_joint_5', 'mw_iiwa_joint_6', 'mw_iiwa_joint_7'};
msg.JointPositions = homeConfiguration(lbr);

call(mdlConfigClient, msg)

Step 6：PD控制器
整个控制过程就是每当到达新的时间戳，就会从Gazebo中接收到一个数据，数据内容包括关节位置、速度以及Gazebo的仿真时间。这些数据可以用于计算误差，作为PD控制器的输入。

在整个主循环中，程序主要做了以下一些事情：

解析message，更新机械臂各个关节状态（即关节角度和速度）；

根据时间戳找到对应的前馈关节转矩（预先计算好），即tau1；

根据关节实际状态和希望状态得到误差，导入PD控制器后可得到补偿的关节转矩，即tau2；

联合前馈关节转矩和补偿关节转矩，即可得到最终的控制器输出；

把最终关节转矩tau1+tau2通过ROS Network传输给Gazebo，从而控制机械臂运动.

% 设置PD参数
weights = [0.3,0.8,0.6, 0.6,0.3,0.2,0.1];
Kp = 100*weights;
Kd = 2* weights;

once = 1;
% 数据初始化
feedForwardTorque = zeros(n, 7);
pdTorque = zeros(n, 7);
timePoints = zeros(n,1);
Q = zeros(n,7);
QDesired = zeros(n,7);

% 控制主循环
% 当MATLAB从Gazebo收到一个新关节状态，它会先查看之前计算生成的转矩"tauFeedForward"，从中找到对应时间的转矩值
% 它同样也会计算PD转矩来补偿关节位置和速度误差
for i = 1:n
    % Get joint state from Gazebo.
    jsMsg = receive(jointStateSub);
    data = jsMsg.Data;
    
    % Parse the received message. 
    % The Data in jsMsg is a 1-by-15 vector.
    % 1:7  - joint positions
    % 8:14 - joint velocities
    % 15   - time (Gazebo sim time) when the joint state is updated
    q = double(data(1:7))';
    qdot = double(data(8:14))';
    t = double(data(15));
    
    % Set the start time.
    if once
        tStart = t;
        once = 0;
    end
    
    % Find the corresponding index h in tauFeedForward vector for joint 
    % state time stamp t.
    h = ceil((t - tStart + 1e-8)/cdt);
    if h>n
        break
    end
    
    % Log joint positions data.
    Q(i,:) = q';
    QDesired(i,:) = qDesired(h,:);
    
    % Inquire feed-forward torque at the time when the joint state is
    % updated (Gazebo sim time).
    tau1 = tauFeedForward(h,:);
    % Log feed-forward torque.
    feedForwardTorque(i,:) = tau1;
    
    % Compute PD compensation torque based on joint position and velocity
    % errors.
    tau2 = Kp.*(qDesired(h,:) - q) + Kd.*(qdotDesired(h,:) - qdot);
    % Log PD torque.
    pdTorque(i,:) = tau2';
    
    % Combine the two torques.
    tau = tau1 + tau2;
    
    % Log the time.
    timePoints(i) = t-tStart;
    
    % Send torque to Gazebo.
    jtMsg.Data = tau;
    send(jointTauPub,jtMsg);    
end

Step 7：结果可视化

绘制实际的关节转矩和关节角度，并于希望值比较，观察实际的控制效果。可以发现实际关节角度和期望值拟合的非常好。

exampleHelperLBRPlot(i-1, timePoints, feedForwardTorque, pdTorque, Q, QDesired )

在这里插入图片描述在这里插入图片描述

但当我使用另外一台电脑进行ROS通信和仿真时，发现结果波动很大。这可能是不同电脑之间的通信延迟导致的数据滞后产生的控制波动，但究竟是什么原因还需要后续分析和

实验。