虽然,Gazebo中自带了绘图工具,当需要绘制一些简单的图像时,非常的方便,但是当需要绘制复杂的图像时,还是MATLAB更加合适一些。
   当我们仅仅使用MATLAB以可视化图像的形式去观察Gazebo中小车的运动轨迹,并不参与小车的控制时,完全没有必要以栅格地图的形式去复现gazebo中的仿真环境,当环境较复杂或需要多变的时候往往需要很大的工作量,本文通过图片的形式,将gazebo中仿真环境
   本文介绍一种通过图片的形式,将gazebo中仿真环境快速复现到MATLAB中,并在此基础上叠加显示仿真小车运动轨迹的方法


一、计算两幅地图间的变换比例

   (1)首先确定gazebo中仿真环境的长度及宽度,先通过工具栏中的工具,将视野调至俯视图

   依次选中gazebo的仿真环境的上下左右边界,并在左侧的pose栏中查看其坐标值,比如本文所用的例子中上边界为的X值为9.925000,下边界为-9.925,即在gazebo的仿真环境中,其在X轴上长度为19.85m。左边界的Y值为10.1235,右边界的Y值为-10.2265,即仿真环境在Y轴上长度为20.35


   (2)将俯视图下的仿真环境,截图并保存,作为MATLAB中的地图使用,截图前,可在view菜单栏,取消Origin坐标轴的显示

   本例中的截图如下所示

   在属性中可查看其像素值,也就是在MATLAB中以图片显示的地图的长度和宽度分别为666和656

   (3)现在拿到了两幅地图的长度和宽度,但是在计算两幅地图的比例变换系数前,还要注意一个特别特别重要的问题,坐标系的变换问题,在MATLAB读取照片的时候,其坐标系的原点在左上角,而不是左下角,如下所示:

   也就是说我们得到的gazebo中X轴长度应该对应MALTAB中的Y轴的长度,gazebo中Y轴长度应该对应MALTAB中的X轴的长度,因此我们得到MATLAB中地图与gazebo中地图的比例为 X轴(以MATLAB为准)=666/20.35=32.72727,Y轴(以MATLAB为准)=656/19.85=33.04786
   到这里我们就得到了两幅地图间的变换比例,也就是说在gazebo中小车沿着gazebo的的坐标系的X轴正方向移动1m,对应在MATLAB的地图中就是沿着Y轴的负方向移动了33.04786个像素点,同理,gazebo中小车沿着gazebo的的坐标系的Y轴正方向移动1m,对应在MATLAB的地图中就是沿着X轴的负方向移动了32.72727个像素点


二、调试获取两幅地图间的原点偏移量

   (1)首先我们在MATLAB的命令行窗口,通过imread函数读取我们刚才保存的截图,最好是jpg格式,在MATLAB的命令行中png等格式也可以,但是在simulink的MATLAB Function模块中只允许使用jpg格式,如下图所示:

   刚才的照片我保存为gazebomap.jpg,在MALTAB的命令行输入以下语句将其读取,imread是读取图像,im2gray是将其转换成灰度图像(不转也可以),并命名为Image1

     Image1=im2gray(imread('gazebomap.jpg'));

   在工作空间使用imshow函数将其显示出来,如下图所示:

    imshow(Image1);


   (2)建立MATLAB与ROS的通讯,并通过subscribe模块订阅gazebo中发布的小车的位置信息,本部分内容在前面的文章中已经介绍过了,这里不再缀叙
   (3)使用MATLAB Function模块来编写程序,在刚才MALTAB显示的图片基础上绘制小车的实时位置,其代码如下:

function Gazebo_Track_Plotting(xr,yr,r,X_factor,Y_factor,X_Offset,Y_Offset)
hold on
%计算偏移量
rectangle('Position',[(-yr*X_factor+X_Offset)-r,(-xr*Y_factor+Y_Offset)-r,2*r,2*r],'Curvature',[1,1],'LineWidth',3,'FaceColor','y','EdgeColor','r'),axis equal
end

   该函数的输入参数中xr,yr是订阅的小车的位置信息(是在gazebo的坐标系下的),X_factor和Y_factor是我们在第一部分计算出的两幅地图之间的变换比例关系,通过constant模块输入给该函数,即X_factor=32.72727,Y_factor=33.04786,X_Offset和Y_Offset就是本步我们要调试的两幅地图下原点的偏移量,由于本例中在gazebo地图中原点差不多在其地图的中点出,而在MATLAB中其原点在左上角,因此X_Offset和Y_Offset大概为图片像素值的一半,即X_Offset应该在666/2=333附近,Y_Offset应该在656/2=328附近,因此将这两个值作为初始值赋给该函数

   由于matlab绘制单个单独的点不易观察,这里采用rectangle函数以该点为圆心绘制一个小圆的形式来代替了该点,r是该圆的半径

   (4)在gazebo中选中仿真小车,在左侧将其X和Y坐标均设为0,这时,gazebo中小车就会移动至gazebo中地图的零点,如下所示:

   (5)启动Simulink的仿真,观察此时根据大概的偏移量映射出的小车位置,如下所示:

   可以发现其相对gazebo中小车的位置有点偏左和偏下,因此应该适当减少Y轴的偏移量,同时适当增加X轴的偏移量,调节传送给Gazebo_Track_Plotting函数的变量X_Offset和Y_Offset,再次观察红点的位置,如此循环直至红点显示的位置为gazebo中原点的位置,经过调节本例中的X_Offset应该改为341,Y_Offset应该改为324,如下图所示

   到这里偏移量也就是找到了,同时,两个地图间的映射关系也就找到了



三、观察效果

   控制gazebo中小车运动,观察MATLAB绘制的轨迹,如下所示:

在MATLAB中绘制Gazebo中小车的运行轨迹

   虽然由于通讯的原因,轨迹的绘制有一定的延时性,但是大体上满足了预期的设想


本文介绍的内容到这里就结束了,在探索过程中遇到了一个奇怪的问题,在Simulink的MATLAB Function模块中,我曾经成功使用过imread()函数,但是过了一天后同样的内容(或者说同样的文件),再也用不了了,只要在该模块里使用了imread函数,就会报以下错误(点击或拖动查看大图),一直没解决,期望得到大佬的指导


Logo

开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!

更多推荐