<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.launchkobuki底盘订阅 /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