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