机器人自主探索系列解析(三)——terrain_analysis地形分析
terrain_analysis代码功能包分析
在之前的内容中，我们主要对仿真环境和路径生成的内容进行了讲解。实际代码中最关键的部分在local_planner和terrain_analysis。本节主要对其中核心之一的terrain_analysis进行分析。
整个源码中包含了10个ROS功能包，每个功能包的具体内容如下：
terrain_analysis功能介绍
terrain_analysis：主要和地面可通行性判断有关。
-
订阅:
- /state_estimation ：车辆在地图下的位姿(里程计)
- /registered_scan： 投影到
map下的点云
-
发布：
- /terrain_map 地形图
-
区分几个变量的概念：
- laserCloudCrop: 回调函数中，经过高度裁剪后的点云
- terrainVoxelCloud： 地形体素地图点云
- terrainVoxelUpdateNum： 更新的地形体素地图的点云数量
- terrainCloudElev： 最终地形高程地图————最终输出地形点云地图
- planarVoxelElev： 平面体素高程地图
- planarVoxelEdge： 平面体素地图边缘(无数据的点)
- planarVoxelDyObs： 平面体素地图的动态障碍物
- planarPointElev： 平面体素地图中每个点云的高度，vector数组。
先将在map坐标下的点云数据/registered_scan，转换到车辆坐标系下。并根据需求建立地面体素地图terrainVoxelCloud。为了确保车辆和体素地图保持一致，这里采用了点云滚动的方法，确保车辆和体素坐标系保持一致。将符合条件的点云存储在terrainVoxelCloud中。接着根据一系列条件判断筛选出合适点云，并对terrainvovelcloud进行点云滤波和体素更新。这里的体素地图是10x10，分辨率为1m。接着定义一个50x50的，分辨率为0.2m的地面体素地图planarPointElev，该地图存放每一个平面点的高程信息z，采用的是复制一个3x3的邻域表示。最后将小于车辆高度、大于附近最小值的这部分点云信息放入地面的点云信息，发布地面点/terrain_map。
相关参数解析：
double scanVoxelSize = 0.05; // 扫描体素大小: 5cm
double decayTime = 2.0; // 时间阈值： 2.0s
double noDecayDis = 4.0; // 车辆初始距离阈值： 4.0m
double clearingDis = 8.0; // 清除距离： 8.0m
bool clearingCloud = false; // 清楚点云： 否-false；是-true
bool useSorting = true; // 使用排序： 是
double quantileZ = 0.25; // Z轴分辩数： 0.25m
bool considerDrop = false; // 考虑下降： 否
bool limitGroundLift = false; // 地面升高高度限制
double maxGroundLift = 0.15; // 地面上升最大距离 0.15m
bool clearDyObs = false; // 清楚障碍标志位
double minDyObsDis = 0.3; // 最小的障碍物距离阈值
double minDyObsAngle = 0; // 通过障碍物的最小角度
double minDyObsRelZ = -0.5; // 通过障碍物最小的Ｚ轴相对高度
double minDyObsVFOV = -16.0; // 左侧最大转向角
double maxDyObsVFOV = 16.0; // 右侧最大转向角
int minDyObsPointNum = 1; // 障碍物点的数量
bool noDataObstacle = false; // 无障碍物数据
int noDataBlockSkipNum = 0; // 无障碍物阻塞跳过的点数
int minBlockPointNum = 10; // 最小阻塞的点数
double vehicleHeight = 1.5; // 车辆的高度
int voxelPointUpdateThre = 100; // 同一个位置的雷达点数阈值
double voxelTimeUpdateThre = 2.0; // 同一个位置的雷达点时间阈值
double minRelZ = -1.5; // Z轴最小的相对距离
double maxRelZ = 0.2; // Z轴最大的相对距离
double disRatioZ = 0.2; // 点云处理的高度与距离的比例-与激光雷达性能相关
// 地面体素参数
float terrainVoxelSize = 1.0; // 地面体素网格的大小
int terrainVoxelShiftX = 0; // 地面体素网格翻转时的X位置
int terrainVoxelShiftY = 0; // 地面体素网格翻转时的Y位置
const int terrainVoxelWidth = 21; // 地面体素的宽度
int terrainVoxelHalfWidth = (terrainVoxelWidth - 1) / 2; // 地面体素的宽度 10
const int terrainVoxelNum = terrainVoxelWidth * terrainVoxelWidth; // 地面体素的大小 21×21
// 平面体素参数
float planarVoxelSize = 0.2; // 平面体素网格的尺寸大小 0.2m
const int planarVoxelWidth = 51; // 点云存储的格子大小
int planarVoxelHalfWidth = (planarVoxelWidth - 1) / 2; // 平面体素的宽度 25
const int planarVoxelNum = planarVoxelWidth * planarVoxelWidth; // 平面体素的大小 51×51
pcl::PointCloud<pcl::PointXYZI>::Ptr
laserCloud(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
laserCloudCrop(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
laserCloudDwz(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
terrainCloud(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
terrainCloudElev(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloud[terrainVoxelNum]; // 每个像素对应存储一个点云指针
int terrainVoxelUpdateNum[terrainVoxelNum] = {0}; // 记录每一个地面体素网格中需要更新的点云的数量
float terrainVoxelUpdateTime[terrainVoxelNum] = {0}; // 地形高程点云更新时间存储数组
float planarVoxelElev[planarVoxelNum] = {0}; // 平面体素网格地图地面的高度
int planarVoxelEdge[planarVoxelNum] = {0}; // 平面边缘体素网格信息
int planarVoxelDyObs[planarVoxelNum] = {0}; // 平面体素网格地图的动态障碍物
vector<float> planarPointElev[planarVoxelNum]; // 存储了地面体素网格中每一个点云的高程信息
double laserCloudTime = 0; // 雷达第一帧数据时间
bool newlaserCloud = false; // 雷达数据接受标志位
double systemInitTime = 0; // 系统初始化时间，根据第一帧点云信息的时间设定
bool systemInited = false; // 系统初始化标志位 false-未初始化；true-已经初始化
int noDataInited = 0; // 车辆初始位置的标志位 0-未赋值，将收到的第一个车辆位置赋值；1-表示已经初始化；2-车辆初始距离误差大于初始阈值
float vehicleRoll = 0, vehiclePitch = 0, vehicleYaw = 0;
float vehicleX = 0, vehicleY = 0, vehicleZ = 0;
float vehicleXRec = 0, vehicleYRec = 0;
float sinVehicleRoll = 0, cosVehicleRoll = 0;
float sinVehiclePitch = 0, cosVehiclePitch = 0;
float sinVehicleYaw = 0, cosVehicleYaw = 0;
pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter; // 三维体素化下采样
1. 点云滚动操作
为了确保车辆与地面点云体素地图对齐，确保车辆移动过程中，地面点云和车辆保持同步。采用了点云滚动操作。其中vehicleX是车辆当前位置x轴， terrainVoxelCentX是体素地图中心，当车辆移动超过一个体素时， 地图体素会同步移动，更新点云体素。
使用点云滚动的优点是只需要维护更新车辆附近一个小范围的点云。
//地图体素左侧翻滚
while (vehicleX - terrainVoxelCenX < -terrainVoxelSize) { // 车辆位置X-地面体素中心X < 负的一个体素网格大小
//外层循环 indY 遍历体素网格每一列
for (int indY = 0; indY < terrainVoxelWidth; indY++) {
pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloudPtr =
terrainVoxelCloud[terrainVoxelWidth * (terrainVoxelWidth - 1) +
indY];
//内层循环 indX 从右向左遍历每一列的元素，将当前列的点云数据
for (int indX = terrainVoxelWidth - 1; indX >= 1; indX--) {
terrainVoxelCloud[terrainVoxelWidth * indX + indY] =
terrainVoxelCloud[terrainVoxelWidth * (indX - 1) + indY];
}
terrainVoxelCloud[indY] = terrainVoxelCloudPtr;
terrainVoxelCloud[indY]->clear();
}
//更新体素网格的 X 轴偏移量
terrainVoxelShiftX--;
terrainVoxelCenX = terrainVoxelSize * terrainVoxelShiftX;
}
//地图体素右侧翻滚 // 车辆位置X-地面体素中心X > 正的一个体素网格大小
//后续三个功能类似。
2.点云转平面体素网络
利用坐标转换公式，将map坐标系的点云，转换为车辆坐标系下的体素点云地图(体素地图坐标原点在左上角)中。并进行四舍五入偏移。小于0的往左边偏移。
将符合条件的点云存储到对应的体素数组中, terrainVoxelCloud[i]存储体素中点云数量，terrainVoxelUpdateNum[i]存储了该体素中需要更新的点云数量。
int indX = int((point.x - vehicleX + terrainVoxelSize / 2) /
terrainVoxelSize) +
terrainVoxelHalfWidth;
int indY = int((point.y - vehicleY + terrainVoxelSize / 2) /
terrainVoxelSize) +
terrainVoxelHalfWidth;
// -------------------------------2. 将map坐标系的下的laserCloudCrop点转换到体素坐标系下（中间经过车体坐标系转换）-------------------------------
//计算terrainVoxelCloud中每个体素网格的点云数量
// stack registered laser scans 点云数据填充体素网格
pcl::PointXYZI point;
int laserCloudCropSize = laserCloudCrop->points.size();// map坐标系的点云转换到体素坐标系
for (int i = 0; i < laserCloudCropSize; i++) {
point = laserCloudCrop->points[i];
//INFO 将map坐标系下的点云转换到车辆的体素坐标系下。
int indX = int((point.x - vehicleX + terrainVoxelSize / 2) /
terrainVoxelSize) +
terrainVoxelHalfWidth;
int indY = int((point.y - vehicleY + terrainVoxelSize / 2) /
terrainVoxelSize) +
terrainVoxelHalfWidth;
// 位于车辆中心左侧、后侧，往左移动一个体素
if (point.x - vehicleX + terrainVoxelSize / 2 < 0)
indX--;
if (point.y - vehicleY + terrainVoxelSize / 2 < 0)
indY--;
if (indX >= 0 && indX < terrainVoxelWidth && indY >= 0 &&//索引不超限的点保存到terrainVoxelCloud中
indY < terrainVoxelWidth) {
terrainVoxelCloud[terrainVoxelWidth * indX + indY]->push_back(point);
terrainVoxelUpdateNum[terrainVoxelWidth * indX + indY]++;
}
}
3. 点云滤波更新操作
根据判断条件，判断每个体素地图是否需要更新体素，主要执行降采样和点云筛选操作。更新体素地图：
判断条件如下：
- 1.高度滤波，当体素中一个体素含有需要更新的点云数量大于100时。
- 2.时间滤波： 点云当前时间戳与接受时的时间戳的差值小于阈值
- 3.雷达滤波：清除雷达数据标志。
满足条件的点云进行降采样，再进行过滤。过滤更新的方法同之前。将更新后的点云存储到terrainVoxelCloudPtr中
//INFO :体素地图更新操作：判断是否需要更新体素、执行降采样、筛选点云数据，到最后更新体素状态
for (int ind = 0; ind < terrainVoxelNum; ind++) {
/**
* @brief 处理激光雷达数据，重置地面体素网格
* 判断条件1： 同一个位置的雷达点数 > 100
* 判断条件2： id数据的时间差大于时间阈值
* 判断条件3： 清除激光雷达数据标志位为true
*/
if (terrainVoxelUpdateNum[ind] >= voxelPointUpdateThre ||
laserCloudTime - systemInitTime - terrainVoxelUpdateTime[ind] >=
voxelTimeUpdateThre ||
clearingCloud) {
pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloudPtr =
terrainVoxelCloud[ind];
laserCloudDwz->clear();
downSizeFilter.setInputCloud(terrainVoxelCloudPtr);
downSizeFilter.filter(*laserCloudDwz);
//以小车为中心取点
//还是那个圆锥形状条件过滤
terrainVoxelCloudPtr->clear();
int laserCloudDwzSize = laserCloudDwz->points.size();
for (int i = 0; i < laserCloudDwzSize; i++) {
point = laserCloudDwz->points[i];
float dis = sqrt((point.x - vehicleX) * (point.x - vehicleX) +
(point.y - vehicleY) * (point.y - vehicleY));
// 对于激光雷达数据的滤波
/*
* 在体素栅格中，需要被进行地面分割的点云满足以下要求，这些点云会被放入terrainCloud,用于地面分割
* 点云高度大于最小阈值
* 点云高度小于最大阈值
* 当前点云的时间与要处理的点云时间差小于阈值 decayTime，或者距离小于 noDecayDis
* 此时不会清除距离外的点云，或者不在需要被清除的距离之内
*/
if (point.z - vehicleZ > minRelZ - disRatioZ * dis &&
point.z - vehicleZ < maxRelZ + disRatioZ * dis &&
(laserCloudTime - systemInitTime - point.intensity <
decayTime ||
dis < noDecayDis) &&
!(dis < clearingDis && clearingCloud)) {
terrainVoxelCloudPtr->push_back(point);//将筛选后的点放入terrainVoxelCloudPtr数组
}
}
terrainVoxelUpdateNum[ind] = 0;
terrainVoxelUpdateTime[ind] = laserCloudTime - systemInitTime;
}
}
最后取10x10的网络大小，用计算得到的填充点作为terrainCloud的地形图。
terrainCloud->clear();
////取11x11的 网格地图terrainVoxelCloudPtr点作为terrainCloud 地形图
for (int indX = terrainVoxelHalfWidth - 5;
indX <= terrainVoxelHalfWidth + 5; indX++) {
for (int indY = terrainVoxelHalfWidth - 5;
indY <= terrainVoxelHalfWidth + 5; indY++) {
*terrainCloud += *terrainVoxelCloud[terrainVoxelWidth * indX + indY];//用于地面分割的点云 terrainCloud
}
}
4.点云z轴高度判断
再构建一个50x50的平面体素图，存储每个点的高程值z。这里的操作和前面构造10x10的体素地图比较类似，都是先将点云坐标转换为体素坐标系体素的位置。不同点在，这里这里的planerPointElev存储的是像素每个点的高度。且是用一个3x3类似卷积的操作进行复制。planarPointElev[i]记录了该体素中所有点云的高度，后续进行排序。
//--------------------------4.对点云信息的ｚ轴高度进行筛选，将最小的ｚ轴高度存储到tplanarVoxelElev----------------------------------------------------
// estimate ground and compute elevation for each point 对点云信息的ｚ轴高度进行筛选，将最小的ｚ轴高度存储到tplanarVoxelElev
for (int i = 0; i < planarVoxelNum; i++) {
planarVoxelElev[i] = 0;
planarVoxelEdge[i] = 0;
planarVoxelDyObs[i] = 0;
planarPointElev[i].clear();
}
int terrainCloudSize = terrainCloud->points.size();
for (int i = 0; i < terrainCloudSize; i++) {
point = terrainCloud->points[i];
int indX =
int((point.x - vehicleX + planarVoxelSize / 2) / planarVoxelSize) +
planarVoxelHalfWidth;
int indY =
int((point.y - vehicleY + planarVoxelSize / 2) / planarVoxelSize) +
planarVoxelHalfWidth;
if (point.x - vehicleX + planarVoxelSize / 2 < 0)
indX--;
if (point.y - vehicleY + planarVoxelSize / 2 < 0)
indY--;
//计算每一个点的高程信息，创建了一个二维数组用于存放平面点高程，存入的方法和之前一样，不过分辨率从1变成了0.2。除此之外，每一个点云不只放在同一个网格中，而是复制到一个3*3的邻域
if (point.z - vehicleZ > minRelZ && point.z - vehicleZ < maxRelZ) { //点满足相对高度范围
for (int dX = -1; dX <= 1; dX++) {
for (int dY = -1; dY <= 1; dY++) {
if (indX + dX >= 0 && indX + dX < planarVoxelWidth &&
indY + dY >= 0 && indY + dY < planarVoxelWidth) {
planarPointElev[planarVoxelWidth * (indX + dX) + indY + dY]
.push_back(point.z);
}
}
}
}
}
5. 其他操作
在地形分析中，设定了一些参数用于动态障碍物分析、以及点云高度排序。
clearDyObs决定是否进行动态障碍物判断，useSorting决定是否对高度图进行排序。
动态障碍物检测和清除
动态障碍物剔除进行了多层次判断：
- 距离判断：只考虑相对位置大于
minDyObsDis的点。(过近的点认为是障碍物) - 俯仰角判断， 根据点云相对高度
pointZ1-minDyObsRelZ计算俯仰角判断，大于最小角才考虑 - 视角和绝对高度判断： 俯仰角在指定范围内(-16, 16)度, 绝对高度小于absDyObsRelZThre 0.2
//位于 for (int i = 0; i < terrainCloudSize; i++)循环中
if (clearDyObs) { //是否清除动态障碍物 默认为false
if (indX >= 0 && indX < planarVoxelWidth && indY >= 0 &&
indY < planarVoxelWidth) {
//点云在车辆坐标系的相对位置
float pointX1 = point.x - vehicleX;
float pointY1 = point.y - vehicleY;
float pointZ1 = point.z - vehicleZ;
float dis1 = sqrt(pointX1 * pointX1 + pointY1 * pointY1);
if (dis1 > minDyObsDis) {//只处理距离大于最小阈值的点
float angle1 = atan2(pointZ1 - minDyObsRelZ, dis1) * 180.0 / PI; //求出俯仰角
if (angle1 > minDyObsAngle) {//通过俯仰角过滤掉过低的点（如地面点）
//偏航角yaw
float pointX2 =
pointX1 * cosVehicleYaw + pointY1 * sinVehicleYaw;
float pointY2 =
-pointX1 * sinVehicleYaw + pointY1 * cosVehicleYaw;
float pointZ2 = pointZ1;
//俯仰角pitch
float pointX3 =
pointX2 * cosVehiclePitch - pointZ2 * sinVehiclePitch;
float pointY3 = pointY2;
float pointZ3 =
pointX2 * sinVehiclePitch + pointZ2 * cosVehiclePitch;
//横滚角 roll
float pointX4 = pointX3;
float pointY4 =
pointY3 * cosVehicleRoll + pointZ3 * sinVehicleRoll;
float pointZ4 =
-pointY3 * sinVehicleRoll + pointZ3 * cosVehicleRoll;
float dis4 = sqrt(pointX4 * pointX4 + pointY4 * pointY4);
float angle4 = atan2(pointZ4, dis4) * 180.0 / PI;
if (angle4 > minDyObsVFOV && angle4 < maxDyObsVFOV || fabs(pointZ4) < absDyObsRelZThre) {
planarVoxelDyObs[planarVoxelWidth * indX + indY]++;// 障碍物标记
}
}
} else {
planarVoxelDyObs[planarVoxelWidth * indX + indY] +=
minDyObsPointNum;
}
}
}
在for循环之外，对所有点云，计算它离车辆的距离，并计算俯仰角，如果该位置有激光点云数据，且该点的俯仰角合理，则清除该位置的动态障碍物标记
if (clearDyObs) {//默认为false 不运行
for (int i = 0; i < laserCloudCropSize; i++) {
point = laserCloudCrop->points[i];
int indX = int((point.x - vehicleX + planarVoxelSize / 2) /
planarVoxelSize) +
planarVoxelHalfWidth;
int indY = int((point.y - vehicleY + planarVoxelSize / 2) /
planarVoxelSize) +
planarVoxelHalfWidth;
if (point.x - vehicleX + planarVoxelSize / 2 < 0)
indX--;
if (point.y - vehicleY + planarVoxelSize / 2 < 0)
indY--;
if (indX >= 0 && indX < planarVoxelWidth && indY >= 0 &&
indY < planarVoxelWidth) {
float pointX1 = point.x - vehicleX;
float pointY1 = point.y - vehicleY;
float pointZ1 = point.z - vehicleZ;
float dis1 = sqrt(pointX1 * pointX1 + pointY1 * pointY1);
float angle1 = atan2(pointZ1 - minDyObsRelZ, dis1) * 180.0 / PI;
if (angle1 > minDyObsAngle) {//俯仰角大于最小角度阈值，则清除该体素格的动态障碍物标记
planarVoxelDyObs[planarVoxelWidth * indX + indY] = 0;
}
}
}
}
地面(平面)高度排序和提取
使用了地面高度提取算法，从每个体素的点云数据planarPointElev[i]中确定每个平面体素网格的地面高度，得到平面高程地图planarVoxelElev[i]，平面高程地图每个平面体素格子确定一个代表性的地面高度值planarVoxelElev[i]，用于后续的地形分析和障碍物检测。
这里设计了两种策略，如果useSorting=true，采用分位树排序，对该像素的所有点云按照高度进行排序，取其中百分位quantileZ作为该像素的地面高度。同时在判断时添加了地面抬升限制，避免地面点差别太大。 如果useSorting=fasle则将最低点作为该点地面高度。
if (useSorting) { //策略一：基于分位数的排序方法 (useSorting = true)
// 排序，取第1/4低的高度作为 planarVoxelElev[i]网格高度
for (int i = 0; i < planarVoxelNum; i++) {
int planarPointElevSize = planarPointElev[i].size();
if (planarPointElevSize > 0) {
sort(planarPointElev[i].begin(), planarPointElev[i].end());//将每个体素格内的所有高度值按升序排列
int quantileID = int(quantileZ * planarPointElevSize);
if (quantileID < 0)//边界检查
quantileID = 0;
else if (quantileID >= planarPointElevSize)
quantileID = planarPointElevSize - 1;
if (planarPointElev[i][quantileID] >//地面抬升限制
planarPointElev[i][0] + maxGroundLift &&
limitGroundLift) {
planarVoxelElev[i] = planarPointElev[i][0] + maxGroundLift;
} else {
planarVoxelElev[i] = planarPointElev[i][quantileID];
}
}
}
} else {//策略二：最小值方法 (useSorting = false)
for (int i = 0; i < planarVoxelNum; i++) { //不使用排序，使用最小的z值填充planarVoxelElev
int planarPointElevSize = planarPointElev[i].size();
if (planarPointElevSize > 0) {
float minZ = 1000.0;
int minID = -1;
for (int j = 0; j < planarPointElevSize; j++) {
if (planarPointElev[i][j] < minZ) {
minZ = planarPointElev[i][j];
minID = j;
}
}
if (minID != -1) {
planarVoxelElev[i] = planarPointElev[i][minID];
}
}
}
}
无数据区域障碍物填充
在地形分析中还添加了对无数据区域填充障碍物操作。 对应参数为noDataObstacle。当车辆移动速度超过激光雷达扫描更新频率时。会产生未扫描的点。当体素地同中的平面点云数量少于最小阈值时，对该位置进行距离变换拓展，拓展后仍无数据的区域填充为障碍物点：
//无数据区域障碍物填充
if (noDataObstacle && noDataInited == 2) {//当车辆移动速度超过激光雷达扫描更新频率时，会产生未扫描的区域。算法将这些无数据区域标记为障碍物，采用保守策略确保行驶安全。
//识别无数据网格
for (int i = 0; i < planarVoxelNum; i++) {
int planarPointElevSize = planarPointElev[i].size();
if (planarPointElevSize < minBlockPointNum) {
planarVoxelEdge[i] = 1;// 标记为边缘/无数据区域
}
}
//距离变换扩展
for (int noDataBlockSkipCount = 0;
noDataBlockSkipCount < noDataBlockSkipNum;
noDataBlockSkipCount++) {
for (int i = 0; i < planarVoxelNum; i++) {
if (planarVoxelEdge[i] >= 1) {
int indX = int(i / planarVoxelWidth);
int indY = i % planarVoxelWidth;
bool edgeVoxel = false;
// 检查8邻域
for (int dX = -1; dX <= 1; dX++) {
for (int dY = -1; dY <= 1; dY++) {
// 如果邻域中有更小的边缘值，说明仍在边缘
if (indX + dX >= 0 && indX + dX < planarVoxelWidth &&
indY + dY >= 0 && indY + dY < planarVoxelWidth) {
if (planarVoxelEdge[planarVoxelWidth * (indX + dX) + indY +
dY] < planarVoxelEdge[i]) {
edgeVoxel = true;
}
}
}
}
if (!edgeVoxel)
planarVoxelEdge[i]++;
}
}
}
//生成虚拟障碍物点 将拓展后的无数据的区域填充为障碍物点
for (int i = 0; i < planarVoxelNum; i++) {
if (planarVoxelEdge[i] > noDataBlockSkipNum) {
int indX = int(i / planarVoxelWidth);
int indY = i % planarVoxelWidth;
point.x =
planarVoxelSize * (indX - planarVoxelHalfWidth) + vehicleX;
point.y =
planarVoxelSize * (indY - planarVoxelHalfWidth) + vehicleY;
point.z = vehicleZ;
point.intensity = vehicleHeight;
point.x -= planarVoxelSize / 4.0;
point.y -= planarVoxelSize / 4.0;
terrainCloudElev->push_back(point);
point.x += planarVoxelSize / 2.0;
terrainCloudElev->push_back(point);
point.y += planarVoxelSize / 2.0;
terrainCloudElev->push_back(point);
point.x -= planarVoxelSize / 2.0;
terrainCloudElev->push_back(point);
}
}
}
5. 结果发布
重新对点云信息进行滤波与高度处理，从原始的体素地图中terrainCloud中筛选出有效的高度信息点。对于terrainCloud中的每个点，计算它在对应的平面体素地图中的坐标，并记录该点z坐标相对平面体素z坐标的差值 disZ。
如果该点的disZ>0且小于车辆高度，认为具有高度价值点，将处理的有价值的点存储到高程地图terrainCloudElev中，高程地图中每个点的intensity为相对地面点云的高度disZ。
//对车辆周围的terrainCloudSize点云以planarVoxelSize细分网格
terrainCloudElev->clear();
//点云信息进行滤波与高度处理，将处理结果存储到terrainCloudElev中
int terrainCloudElevSize = 0;
for (int i = 0; i < terrainCloudSize; i++) {// //取地形点云的点 11x11
point = terrainCloud->points[i];
if (point.z - vehicleZ > minRelZ && point.z - vehicleZ < maxRelZ) { //高度范围滤波
int indX = int((point.x - vehicleX + planarVoxelSize / 2) /
planarVoxelSize) +
planarVoxelHalfWidth;
int indY = int((point.y - vehicleY + planarVoxelSize / 2) /
planarVoxelSize) +
planarVoxelHalfWidth;
if (point.x - vehicleX + planarVoxelSize / 2 < 0)
indX--;
if (point.y - vehicleY + planarVoxelSize / 2 < 0)
indY--;
if (indX >= 0 && indX < planarVoxelWidth && indY >= 0 &&
indY < planarVoxelWidth) {
if (planarVoxelDyObs[planarVoxelWidth * indX + indY] <
minDyObsPointNum ||
!clearDyObs) {
float disZ =
point.z - planarVoxelElev[planarVoxelWidth * indX + indY];//点云中某点z坐标和对应的平面体素z坐标(体素最低坐标)的差值
if (considerDrop)
disZ = fabs(disZ);
int planarPointElevSize =
planarPointElev[planarVoxelWidth * indX + indY].size();
if (disZ >= 0 && disZ < vehicleHeight &&
planarPointElevSize >= minBlockPointNum) {//该点高度>0，小于车辆高度，具有高度价值点，记录
terrainCloudElev->push_back(point);
terrainCloudElev->points[terrainCloudElevSize].intensity = disZ;//将点云强度值设为点云距离地面点云高度
terrainCloudElevSize++;
}
}
}
}
}
总的来说，这部分的思路是将map坐标下的点云转换到我们的车辆坐标系下，根据需要构建了地图体素网格，用网格填充地图特性，选择合适的点云，求解最低的点云高度。所用的思想就是计算当前位置与附近位置最小高程的一个相对高程。这样看来，在一个平滑的坡上效果表现还是可以，也可以改变邻域的大小，不过邻域也有0.6m的范围了。
地形分析结果
这里面涉及到很多参数调节，可以在rviz中查看可通行区域分析结果：
其中比较关键的参数：
- clearDyObs：动态障碍物剔除————决定是否进行动态障碍物剔除(非常影响地形可通行性)
- vehicleHeight：车辆/传感器高度————影响高度判断
- minRelZ maxRelZ：点云相对车辆高度阈值———— 相对高度在阈值范围内的点云考虑，之外的不考虑
- useSorting/quantileZ: 是否使用排序进行可行性分析，以及排序作为地面点的中值———— 一定程度上影响可行性分析
其中绿色代表可通行区域，红色代表不可通行。可以根据关键参数调节。
浙公网安备 33010602011771号