栅格地图建立-Grid-Mapping

  • 0.引言
  • 1.建图前提假设
  • 2.建图推导
  • 3.逆观测模型
  • 4.Occupancy Mapping Algorithm
  • 5.demo

0.引言

本文只是根据资料自己学习的过程记录。主要参考的资料有：

  • 波恩大学PPT

  • 曾书格大佬PPT

  • 代码参考

问题：给定机器人的位姿和传感器的观测数据(主要指激光雷达)。data ={x1,z1,x2,z2,⋯xn,zn}\text { data }=\left\{x_{1}, z_{1}, x_{2}, z_{2}, \cdots x_{n}, z_{n}\right\}  data ={x1​,z1​,x2​,z2​,⋯xn​,zn​}
求解：最可能的地图。m∗=arg⁡max⁡mP(m∣x1:t,z1:t)m^{*}=\arg \max _{m} P\left(m \mid x_{1: t}, z_{1: t}\right) m∗=argmmax​P(m∣x1:t​,z1:t​)

// 初始位置
int size_x_, size_y_, init_x_, init_y_;
// 地图分辨率
double cell_size_;
// 整个地图， 初始化概率全为 0.5 (unknown)
Eigen::MatrixXd bel_data_;
  • nav_msgs/OccupancyGrid

void GridMap::toRosOccGridMap( const std::string& frame_id, nav_msgs::OccupancyGrid& occ_grid)
{occ_grid.header.frame_id = frame_id;occ_grid.header.stamp = ros::Time::now();occ_grid.info.width = size_x_;occ_grid.info.height = size_y_;occ_grid.info.resolution = cell_size_;occ_grid.info.origin.position.x = -init_x_*cell_size_;occ_grid.info.origin.position.y = -init_y_*cell_size_;// 没有赋值旋转// # The map data, in row-major order, starting with (0,0).  Occupancy// # probabilities are in the range [0,100].  Unknown is -1.const int N = size_x_ * size_y_;for(size_t i= 0; i < N; i ++){double& value = bel_data_.data()[i];if(value == 0.5)occ_grid.data.push_back( -1);elseocc_grid.data.push_back( value * 100);}
}

1.建图前提假设