ros-movebase是开发移动底盘的很好开源框架，在对应的navigation包中，包含了：运动控制move-base、定位amcl、建图gmapping、地图保存与加载管理map-server等模块，便于初学者使用。当对该框架了解后，就可以针对性的替换内部的一些模块，改进一些算法，优化机器人的性能。完全熟练了这套理论之后，能为学习其它一些平台提供铺垫，例如AutoWare、Apollo等。
Rviz变量观察
Rviz是一个可视化窗口，可以监控ros系统中所有模块发布的话题变量、或者TF坐标系，以可视化的方式呈现，便于理解。
通常选择地图坐标系为全局基础坐标系，其它变量的显示都是相对这个坐标系进行定位的。
基础坐标系：/map
观察激光实时数据-LaserScan:/scan
在ros中的地图有多种，关注最多为三种地图：静态地图，即从地图文件中加载出来的地图；全局地区，即在静态地图上根据当下实际环境增添了障碍物的地图；局部地图，及机器人周围一定范围的矩形地区，其障碍物通常会实时更新，是机器人局部路径规划的重要依据。
静态地图Map: /map
全局代价地图globalcostmap：/move_base/global_costmap/costmap
局部代价地图：/move_base/local_costmap/costmap
机器人的路径可以分为三种：完整的全局路径；被局部地图截取的部分全局路径；基于部分全局路径和局部地图规划出的动态路径。
完整路径：/move_base/NavfnROS/plan
全局路径规划：/move_base/DWAPlannerROS/global_plan
局部路径规划：/move_base/DWAPlannerROS/local_plan
局部路径规划代价云：/move_base/DWAPlannerROS/cost_cloud
局部路径云：/move_base/DWAPlannerROS/trajectory_cloud
粒子滤波的定位原理是用粒子概率表示机器人在地图中出现的概率，通过/particlecloud话题可以观察粒子滤波的发散程度，粒子越聚集，表明位置结果一致性高，可信度越高，反之，则表明定位质量很差。
amcl定位粒子分布：/particlecloud
系统坐标系关系
机器人的相对于地图的位置关系，以及机器人上面各部件的相对位置关系，都是通过TF关系表示的。TF-Tree描述了系统中完整的坐标变换关系。
/map
|--/odom
|--/base_footprint
|--/base_link
|--/laser_frame
在发布TF定位约束时，需要注意一点：父节点不可以通过子节点的位置来推算，否则系统会报错。例如，/base_footprint是/base_link的父节点，两者之间的关系恒定，当机器人运动时，需要发布/odom与/base_footprint的关系，进而推算/base_link的关系，反之，则会执行错误。
激光坐标系
在launch文件中部分静态坐标系关系：
<node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link"
args="0.887 0.0 0.2 0.0 0.0 0.0 /base_footprint /base_link 40" />
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser_frame"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser_frame 5" />
呈现如下的父子关系：
/base_footprint
|--/base_link
|--/laser_frame
里程计坐标系
动态发布TF关系：
br_odom.sendTransform(tf::StampedTransform(Trans_odom_in_world, current_time, "odom", "base_footprint"));
呈现如下的父子关系：
|--/odom
|--/base_footprint
底盘与里程计
里程计标定参考文献：《Accurate calibration of kinematic parameters for two wheel differential mobile robots》
里程计的基本原理，参考网站
http://faculty.salina.k-state.edu/tim/robotics_sg/Control/kinematics/odometry.html
差动轮里程信息计算函数
void odom_calc::update_odom(float spd1_pps, float spd2_pps,
int32_t round1_cnt, int32_t round2_cnt,
int32_t pulse1_cnt, int32_t pulse2_cnt)
{
float spd1_mps = spd1_pps/pulse1_num_per_m;
float spd2_mps = spd2_pps/pulse2_num_per_m;
float v_mps = 0.5*(spd1_mps + spd2_mps);
float w_rads = (spd2_mps - spd1_mps)/wide_m;
odom_vx = v_mps;
odom_vy = 0.0;
odom_w = w_rads;
// ----------------------------------------
double delta1_pulse = (round1_cnt - round1_cnt_old)*pulse_resolution + (pulse1_cnt - pulse1_cnt_old);
double delta2_pulse = (round2_cnt - round2_cnt_old)*pulse_resolution + (pulse2_cnt - pulse2_cnt_old);
round1_cnt_old = round1_cnt;
round2_cnt_old = round2_cnt;
pulse1_cnt_old = pulse1_cnt;
pulse2_cnt_old = pulse2_cnt;
float delta1_m = delta1_pulse/(double)pulse1_num_per_m;
float delta2_m = delta2_pulse/(double)pulse2_num_per_m;
float delta_m = 0.5*(delta1_m + delta2_m);
float delta_rad = (delta2_m - delta1_m)/wide_m;
odom_x += delta_m*cos(odom_th + 0.5*delta_rad);
odom_y += delta_m*sin(odom_th + 0.5*delta_rad);
odom_th += delta_rad;
if (odom_th > 3.1415926)
{
odom_th -= 2.0*3.1415926;
}
else if (odom_th < -3.1415926)
{
odom_th += 2.0*3.1415926;
}
}
里程计信息发布
ros::Publisher odom_pub = node.advertise<nav_msgs::Odometry>("/odom", 1);
nav_msgs::Odometry odom_msg;
odom_msg.header.frame_id = "odom";
odom_msg.child_frame_id = "base_footprint";
// ------------------------------
// publish topic odom
// ------------------------------
odom_msg.header.seq ++;
odom_msg.header.stamp = current_time;
// position
odom_msg.pose.pose.position.x = odom.odom_x;
odom_msg.pose.pose.position.y = odom.odom_y;
// orientation
odom_quat = tf::createQuaternionMsgFromYaw(odom.odom_th);
odom_msg.pose.pose.orientation = odom_quat;
// twist
odom_msg.twist.twist.linear.x = odom.odom_vx;
odom_msg.twist.twist.linear.y = odom.odom_vy;
odom_msg.twist.twist.angular.z = odom.odom_w;
odom_pub.publish(odom_msg);
里程计坐标发布
tf::TransformBroadcaster br_odom;
tf::StampedTransform Trans_base_in_odom;
// ------------------------------
// broadcast tf-tree odom
// ------------------------------
Trans_base_in_odom.setOrigin(tf::Vector3(odom_msg.pose.pose.position.x,
odom_msg.pose.pose.position.y,
odom_msg.pose.pose.position.z));
Trans_base_in_odom.setRotation(tf::Quaternion(odom_msg.pose.pose.orientation.x,
odom_msg.pose.pose.orientation.y,
odom_msg.pose.pose.orientation.z,
odom_msg.pose.pose.orientation.w));
br_odom.sendTransform(tf::StampedTransform(Trans_base_in_odom, current_time, "odom", "base_footprint"));
建图
采用gmapping建图，需要里程信息和激光信息；输出/map。
里程信息从订阅tf关系中体现，主要是odom与base_link之间的关系；
/scan坐标系默认位base_link坐标系；
输出的地图最后用map_server保存：
$ rosrun map_server map_saver -f file_name
使用map_server加载地图的命令：
$rosrun map_server map_server file_name.yaml
整体的launch文件如下：
<launch>
<!-- 启动usb驱动摄像头和apriltag识别模块 -->
<include file="$(find ydlidar_ros)/launch/lidar.launch" />
<!-- 启动轮椅驱动模块 -->
<node pkg="wheel_chair" type="wheel_driver" name="wheel_driver"/>
<!-- 启动gmapping建图 -->
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping"/>
<!-- 启动rviz 显示 -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find wheel_chair)/launch/mapping.rviz" />
</launch>
目前发现手推机器人的方式，比遥控机器人行走的方式的建图效果更好。
amcl定位
amcl定位环节，会确定出/map坐标系与/odom坐标系之间的关系，确定出如下父子关系：
/map
|--/odom
思考问题：
1、如果要提升amcl定位精度和可靠性，需要配置哪些参数？
2、gmapping在建图环节定位效果很好，相比于用amcl做纯定位时精度和稳定性都很高，能否将gmapping用来纯定位(不建图、不修改静态地图)呢？
对于上面的问题2，可以参考Google的开源SLAM模块cartographer，这个模块具备建图和纯定位两种工作模式，我在其它文章中有相关介绍。
move_base
在使用move_base模块搭配国产激光雷达时，发现了一些问题，记录如下。
问题1：局部地图障碍物不消失或者消失慢。
当障碍物出现在机器人面前时，local_costmap会出现障碍物，障碍物离开后，出现部分障碍物在地图中不消失的问题。
1、激光数据格式原因
使用的ydlidar激光，当测量距离超出范围后，返回数据为零。需要做修改。
文件：navigation-kinetic-devel/costmap_2d/plugins/obstacle_layer.cpp
函数：ObstacleLayer::laserScanValidInfCallback
修改内容：
for (size_t i = 0; i < message.ranges.size(); i++)
{
float range = message.ranges[ i ];
// changed by kaikai.gao
// if (!std::isfinite(range) && range > 0)
if ((!std::isfinite(range) && range > 0)||(range < 0.01))
{
message.ranges[ i ] = message.range_max - epsilon;
}
}
2、激光分辨率不足原因
当设置地图的分辨率位0.01时，地图上的最小颗粒度位1cmX1cm的矩形。当设置局部地图的范围是5mX5m时，局部地图上距离机器人中心最远距离为5*1.414=7.07m（对角线）。最远距离出相邻两颗粒地图的角度为：弧度0.01/7.07,角度0.081。若要求激光可以扫描每一颗粒，则激光360度分辨率位360/0.081=4444，对于1圈4000一下的激光就无法达到。
当局部地图上障碍物消失后，由于某些颗粒处激光扫描不到，导致此处障碍物无法消失，只有机器人激动时，扫描的原因发生变化，才有概率扫描到那个位置，从而达到清扫障碍物的目的。
如何在不增加精光分辨率的情况下，改变这一现象呢，需要修改清除障碍物的机制。
思想：将激光扫描末端点加大，相当于激光变粗了。
文件：navigation-kinetic-devel/costmap_2d/plugins/obstacle_layer.cpp
函数：ObstacleLayer::raytraceFreespace
修改内容：
void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y)
{
double ox = clearing_observation.origin_.x;
double oy = clearing_observation.origin_.y;
pcl::PointCloud < pcl::PointXYZ > cloud = *(clearing_observation.cloud_);
// get the map coordinates of the origin of the sensor
unsigned int x0, y0;
if (!worldToMap(ox, oy, x0, y0))
{
ROS_WARN_THROTTLE(
1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
ox, oy);
return;
}
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double origin_x = origin_x_, origin_y = origin_y_;
double map_end_x = origin_x + size_x_ * resolution_;
double map_end_y = origin_y + size_y_ * resolution_;
touch(ox, oy, min_x, min_y, max_x, max_y);
// for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
for (unsigned int i = 0; i < cloud.points.size(); ++i)
{
double wx = cloud.points[i].x;
double wy = cloud.points[i].y;
//在检测到的点周围生成6x6的点，
double inflate_dx = 0.01, inflate_dy = 0.01; //在原来点的位置膨胀的尺度
std::vector< std::pair<double,double> > inflate_pts;
inflate_pts.push_back(std::make_pair(wx + 0 , wy + 0 ));
inflate_pts.push_back(std::make_pair(wx - 0 , wy - inflate_dy));
inflate_pts.push_back(std::make_pair(wx - inflate_dx, wy - 0 ));
inflate_pts.push_back(std::make_pair(wx + 0 , wy + inflate_dy));
inflate_pts.push_back(std::make_pair(wx + inflate_dx, wy + 0 ));
inflate_pts.push_back(std::make_pair(wx - 0 , wy - 2*inflate_dy));
inflate_pts.push_back(std::make_pair(wx - 2*inflate_dx, wy - 0 ));
inflate_pts.push_back(std::make_pair(wx + 0 , wy + 2*inflate_dy));
inflate_pts.push_back(std::make_pair(wx + 2*inflate_dx, wy + 0 ));
inflate_pts.push_back(std::make_pair(wx - 0 , wy - 3*inflate_dy));
inflate_pts.push_back(std::make_pair(wx - 3*inflate_dx, wy - 0 ));
inflate_pts.push_back(std::make_pair(wx + 0 , wy + 3*inflate_dy));
inflate_pts.push_back(std::make_pair(wx + 3*inflate_dx, wy + 0 ));
std::vector< std::pair<double,double> >::iterator inflate_iter;
for(inflate_iter = inflate_pts.begin(); inflate_iter != inflate_pts.end(); inflate_iter++)
{
wx = (*inflate_iter).first;
wy = (*inflate_iter).second;
// now we also need to make sure that the enpoint we're raytracing
// to isn't off the costmap and scale if necessary
double a = wx - ox;
double b = wy - oy;
// the minimum value to raytrace from is the origin
if (wx < origin_x)
{
double t = (origin_x - ox) / a;
wx = origin_x;
wy = oy + b * t;
}
if (wy < origin_y)
{
double t = (origin_y - oy) / b;
wx = ox + a * t;
wy = origin_y;
}
// the maximum value to raytrace to is the end of the map
if (wx > map_end_x)
{
double t = (map_end_x - ox) / a;
wx = map_end_x - .001;
wy = oy + b * t;
}
if (wy > map_end_y)
{
double t = (map_end_y - oy) / b;
wx = ox + a * t;
wy = map_end_y - .001;
}
// now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
unsigned int x1, y1;
// check for legality just in case
if (!worldToMap(wx, wy, x1, y1))
continue;
unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
MarkCell marker(costmap_, FREE_SPACE);
// and finally... we can execute our trace to clear obstacles along that line
raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
}
}
}
1102
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
