Matlab 控制ROS下的机器人做S型加减速度控制

<1> Matlab 与 ROS 通信

    Ubuntu下IP地址为 192.168.0.104   

   win10 Matlab IP地址 为 192.168.0.103

确保  Ubuntu 与 win10  相互能ping通

<2>在ubuntu 下 修改.bashrc文件

cd ~
gedit .bashrc

在文件尾添加

export ROS_HOSTNAME=192.168.0.104
export ROS_IP=192.168.0.104
export ROS_MASTER_URI=http://192.168.0.104:11311

另起终端 启动roscore

roscore

另起终端 启动kobuki底盘

roslaunch kobuki.launch

kobuki底盘订阅   /cmd_vel   的主题

<3>在Win10 matlab命令窗口下执行

setenv('ROS_MASTER_URI','http://192.168.0.104:11311')

setenv('ROS_IP','192.168.0.103')
rosinit()

提示：

The value of the ROS_MASTER_URI environment variable, http://192.168.0.104:11311, will be used to connect to the ROS master.
The value of the ROS_IP environment variable, 192.168.0.103, will be used to set the advertised address for the ROS node.

运行 rosnode list  或者 rostopic list 验证 matlab与ros是否连接上  正常会显示

>> rosnode list
/diagnostic_aggregator
/matlab_global_node_17825
/mobile_base
/mobile_base_nodelet_manager
/rosout

<4> 认识 S型 sigma 函数  y = 1./(1+exp(-x)); 

sigma 函数有这么个特性 

定义域x 在 负无穷 到 正无穷

值域 被限定在 (0,1) 

clc;
clear;
N = 5
x = linspace(-N,N,100);
y = 1./(1+exp(-x));
plot(x,y)
grid on

1.对 sigma函数 伸缩因子 B    y = B*(1./(1+exp(-x)));     值域被伸缩到 （0，B）之间

2.对 sigma函数 y轴方向平移 A    y =A + B*( 1./(1+exp(-x)));     值域被伸缩平移到 （A，B）之间

3.对 自变量区间压缩 X= a*(x-mid)/mid   mid 是 len/2   

clc;
clear;
Fmin = 3; % Y轴方向整体平移  最小
Fmax = 10; % Y轴方向 最大值
Flexible = 5;  % 自变量 [-1,1] 区间伸缩 5倍

N = 1000;
mid = 500;
for k = 1:N
    % 自变量X 限定在 [-Flexible,Flexible]区间
    X(k) = Flexible*(k-mid)/mid; 
    % Fmax - Fmin 伸缩
    Fcurrent(k) = Fmin + (Fmax - Fmin)/(1+exp(-X(k)));  
end
figure (1)
plot(Fcurrent)
figure (2)
plot(X)

<5> Matlab控制 ROS 做S型加减速控制完整代码

clc;
%%% 与ros通信
cmdpub = rospublisher('/cmd_vel',rostype.geometry_msgs_Twist);
pause(0.5)% Wait to ensure publisher is setup
cmdmsg = rosmessage(cmdpub);


%%% 1000 步
N = 1000;
Fmin = 0;
Fmax = 0.4;
mid = N/2;
Flexible = 4;
t = 2;% 模拟时间 2s
deta_t = t/N;% 时间步长
FAll = [];
% 向前加速度
for k = 1:N
    % num 中值 
    % i(k)-num 当前值距离中值差
    % (i(k)-num)/num  映射到[-1,1]区间
    % Flexible 对 [-1,1]区间 拉伸
    %X(k) = Flexible*(i(k)-num)/num;
    X(k) = Flexible*(k-mid)/mid;
    Fcurrent(k) = Fmin + (Fmax - Fmin)/(1+exp(-X(k))); 
    cmdmsg.Linear.X = Fcurrent(k);
    cmdmsg.Angular.Z = 0;
    send(cmdpub,cmdmsg)
    pause(deta_t)%
end
    FAll = [FAll Fcurrent];
    % 向前减速
for k = 1:N
    X(k) = Flexible*(k-mid)/mid;
    Fcurrent2(k) = Fmax - (Fmax - Fmin)/(1+exp(-X(k))); 
    cmdmsg.Linear.X = Fcurrent2(k);
    cmdmsg.Angular.Z = 0;
    send(cmdpub,cmdmsg)
    pause(deta_t)%
end
FAll = [FAll Fcurrent2];
figure (1)
plot([Fcurrent' Fcurrent2'])
%%%%%%%%%%%%%%%%%%%%%%%
pause(1)
% 向后加速度
for k = 1:N
    X(k) = Flexible*(k-mid)/mid;
    Fcurrent(k) = Fmin + (Fmax - Fmin)/(1+exp(-X(k))); 
    cmdmsg.Linear.X = -Fcurrent(k);
    cmdmsg.Angular.Z = 0;
    send(cmdpub,cmdmsg)
    pause(deta_t)%
end
FAll = [FAll -Fcurrent];
% 向后减速度
    for k = 1:N
    X(k) = Flexible*(k-mid)/mid;
    Fcurrent2(k) = Fmax - (Fmax - Fmin)/(1+exp(-X(k))); 
    cmdmsg.Linear.X = -Fcurrent2(k);
    cmdmsg.Angular.Z = 0;
    send(cmdpub,cmdmsg)
    pause(deta_t)%
    end
FAll = [FAll -Fcurrent2];
disp('回到原点')
figure (2)
plot(FAll)
grid on