D*算法理解以及C++实现

D*算法

1. 算法流程

我对该算法流程的理解：

在该算法中，有两个重要状态$RAISE状态与$LOWER$状态，我认为$$RAISE$状态发生在地图上状态点由空闲状态变为障碍物，所以导致该点会向其子节点传递这种$RAISE$状态。$LOWER$状态发生在障碍物变为空闲状态的状态点，另一个是该点本身是最优的(到达该点的最优路径已经确定，可以说是$CLOSE$中的点，这种点$h=k$)，正是由于这种最优性，使得其子节点从他扩展的时候有可能降低路径代价。以上是我个人理解。

我认为D*算法有几个比较重要的地方，针对于动态障碍物部分，一开始的静态搜索不重要：

1. 当状态点状态发生变化的时候，其周围点的有这些：该点子节点，该点父节点，不指向他的其他节点。
2. 该点的状态会传递到子节点
3. 当$k_{old}=h$的时候，该节点才可以传递他的最有性给别的点用。
4. 父节点不可到达，因此新路径存在的重要方向就是不指向他的其他节点。

算法流程：

1. 首先整个算法执行一个Dijkstra过程，这样地图上所有的状态都计算完毕。
2. 然后通过MODIFY_COST对不可通行的状态点，修改其$h$值，并投放到OPEN中开始由该点进行扩张。
3. 随后就是PROCESS-STATE过程，调整不可通行点附近点的状态，最终形成新的路径，代码中有些注释，关于这些条件的理解

2. 代码实现

2.1地图类：

#ifndef MYMAP_H_
#define MYMAP_H_

#include <vector>

using std::vector;
namespace mymap {
class MyMap {
 public:
  // 构造函数
  MyMap(){};
  MyMap(const int map_xlength, const int map_ylength);
  // 设置平行与X轴的一排障碍物，[x_begin, x_end]
  void SetObsLineX(const int x_begin, const int x_end, const int y);
  // 设置平行与Y轴的一排障碍物，[y_begin, y_end]
  void SetObsLineY(const int y_begin, const int y_end, const int x);
  // 获取(x, y)处的地图状态，1为有障碍物，0为无障碍物
  bool GetMapPointState(const int x, const int y) const;
  // 获取成员变量接口
  void set_point_state(const int x, const int y, const int state);
  vector<vector<int>> map() const { return map_; };
  int map_xlength() const { return map_xlength_; };
  int map_ylength() const { return map_ylength_; };
 private:
  vector<vector<int>> map_;   // 地图矩阵
  int map_xlength_; // 地图列数，长
  int map_ylength_; // 地图行数，宽
  int unfree = 1;   // 地图点状态
  int free = 0;
};
}  // namespace mymap

#endif  // MYMAP_H

#include "mymap.h"

namespace mymap {
// 构造函数，构建指定长宽的地图大小
MyMap::MyMap(const int map_xlength, const int map_ylength)
    : map_xlength_(map_xlength), map_ylength_(map_ylength) {
  map_.resize(map_ylength_);
  for (auto &e : map_) {
    e.resize(map_xlength_);
  }
  int x_scale = map_xlength_ / 5;
  int y_scale = map_ylength_ /5;
  SetObsLineX(0, map_xlength_ - 1, 0);
  SetObsLineX(0, map_xlength_ - 1, map_ylength_ - 1);
  SetObsLineY(0, map_ylength_ - 1, 0);
  SetObsLineY(0, map_ylength_ - 1, map_xlength_ - 1);
  SetObsLineX(x_scale, x_scale * 2, y_scale * 3);
  SetObsLineX(x_scale * 3, x_scale * 4, y_scale * 2);
  SetObsLineX(0, x_scale, y_scale * 2);
  SetObsLineX(x_scale * 4, x_scale * 5, y_scale * 3);
  SetObsLineY(0, y_scale * 3, x_scale * 2);
  SetObsLineY(y_scale * 2, y_scale * 5, x_scale * 3);
}

void MyMap::SetObsLineX(const int x_begin, const int x_end, const int y) {
  for (int i = x_begin; i <= x_end; ++i) {
    map_[i][y] = unfree;
  }
}

void MyMap::SetObsLineY(const int y_begin, const int y_end, const int x) {
  for (int i = y_begin; i <= y_end; ++i) {
    map_[x][i] = unfree;
  }
}

bool MyMap::GetMapPointState(const int x, const int y) const {
  return map_[x][y];
}

void MyMap::set_point_state(const int x, const int y, const int state) {
  map_[x][y] = state;
}

}  // namespace mymap

2.2 Dstar实现

#ifndef DSTAR_H_
#define DSTAR_H_

#include <map>
#include <string>
#include <vector>

#include "mymap.h"

class Dstar {
 public:
  // 构造函数
  Dstar(mymap::MyMap&, std::pair<int, int>&, std::pair<int, int>&);
  // D*的重要过程，compute optimal path costs to the goal
  double process_state();
  // 获取opne中最小状态点以及对应的k,放在一起避免找两遍
  std::pair<std::pair<int, int>, double> min_state() const;
  // 获取最小的k值
  double get_k_min() const;
  // 添加状态点到open中
  void insert_state(std::pair<int, int>&, double);
  // 从open中删除状态点
  void delete_state(std::pair<int, int>&);
  // 改变状态点信息，用来表现动态障碍物
  void modify(std::pair<int, int>&);
  void modify_cost(std::pair<int, int>&);
  // 获取周围点
  std::vector<std::pair<int, int>> get_neigbors(std::pair<int, int>&) const;
  // 计算两点之间的代价
  double cost(std::pair<int, int>&, std::pair<int, int>&) const;
  // 两点之间是否可通行
  bool is_collision(std::pair<int, int>&, std::pair<int, int>&) const;
  // 回溯以获取路径点集
  void make_path();

  mymap::MyMap map_;
  std::map<std::pair<int, int>, std::string> t_;                // 地图上每个点的状态
  std::map<std::pair<int, int>, double> k_;                     // 地图每个点的k值
  std::map<std::pair<int, int>, double> h_;                     // 地图每个点的h值
  std::map<std::pair<int, int>, std::pair<int, int>> parent_;   // 每个点的父节点
  std::map<std::pair<int, int>, double> open_;                  // 待访问集合，与对应的h
  std::pair<int, int> start_point_;                //搜索起始点
  std::pair<int, int> end_point_;                  //搜索终点
  std::vector<std::pair<int, int>> path_;          //搜索路径
  std::pair<int, int> end_point_parent_;           //终点的父节点设置为(-1, -1)
};

#endif  // DSTRA_H_

#include "dstar.h"

#include <climits>
#include <cmath>
Dstar::Dstar(mymap::MyMap& map, std::pair<int, int>& start_point,
             std::pair<int, int>& end_point)
    : map_(map) {
  // 遍历地图上所有点，设置初始状态
  for (int x = 0; x < map_.map_xlength(); x++) {
    for (int y = 0; y < map_.map_ylength(); y++) {
      std::pair<int, int> point = std::make_pair(x, y);
      t_[point] = "NEW";
      k_[point] = 0.0;
      h_[point] = 100000.0;
      //不可通行的代价设为一个大值，初始设置为最大值没有关系，因为是NEW状态，第一次遍历会计算h
    }
  }
  parent_.clear();
  path_.clear();
  open_.clear();
  start_point_ = start_point;
  end_point_ = end_point;
  h_[end_point_] = 0.0;
  parent_[end_point] = end_point_parent_;
}

double Dstar::process_state() {
  // open为空则返回-1
  if (open_.empty()) return -1;
  // 获取最小状态点以及对应的k
  std::pair<std::pair<int, int>, double> state_and_kold = this->min_state();
  // 提取信息
  std::pair<int, int> current_point = state_and_kold.first;
  double kold = state_and_kold.second;
  // 从列表中删除该点，做相应的标记
  this->delete_state(current_point);
  // 获取周围点
  std::vector<std::pair<int, int>> neighbors =
      this->get_neigbors(current_point);

  // -----------------------主要条件过程--------------------------------------
  // rasie状态，该状态表明，由于修改了状态点的可通行状态，即：h（不可通行h很大），
  // 导致该点曾经的优代价k_old已经不在等于h（曾经最优k_old=h），而是k_old小于h
  // 这种点不仅仅是，直接修改的点(faster)，还包括faster扩张后的点，
  // 这些点由于曾经指向faster,导致其h值在faster扩张过程中被变得很大很大。
  // 对于这样的raise点，首先希望他能够从周围点中获取更短的代价路径，
  // 一旦有这样的点，就指向他。然而，一般不存在，因为，曾经的路线本来就是最优的，周围点的代价都是大于k_old的
  // 当然，由于扩展的进行，状态改的越来越多，说不定就有这样的情况发生
  // 以上解释仅供参考
  if (kold < h_[current_point]) {
    for (auto neighbor : neighbors) {
      if (h_[neighbor] <= kold and
          h_[current_point] >
              h_[neighbor] + this->cost(neighbor, current_point)) {
        parent_[current_point] = neighbor;
        h_[current_point] = h_[neighbor] + this->cost(neighbor, current_point);
      }
    }
  }
  // k_old = h表示，当前状态点已经是最优路径了
  // 既然已经是最优路径的了，那么周围点就可以直接连上去
  if (kold == h_[current_point]) {
    for (auto neighbor : neighbors) {
      if (t_[neighbor] == "NEW" or
          (parent_[neighbor] == current_point and
           h_[neighbor] !=
               h_[current_point] + this->cost(neighbor, current_point)) or
          (parent_[neighbor] != current_point and
           h_[neighbor] >
               h_[current_point] + this->cost(neighbor, current_point))) {
        // 1.子节点是新的点，那么直接在父节点的基础上扩展，这对应初始搜索情况，跟动态添加障碍物没关系
        // 2.子节点与父节点的关系没有变化，但是由于父节点之前有cost减少（比如本来是障碍物的地方变得不是障碍物），带来了变化
        //   也可以认为，由于子父节点关系已经确定，那么任何在父节点中的信息变化，都应该扩展到子节点。
        // 3.当前节点是更加好的父节点，那么当然更新子节点的指针啦。
        parent_[neighbor] = current_point;
        this->insert_state(
            neighbor, h_[current_point] + this->cost(neighbor, current_point));
      }
    }
  } else {
    // 注意这个else分支包含k_old > h，k_old < h;
    for (auto neighbor : neighbors) {
      if (t_[neighbor] == "NEW" or
          (parent_[neighbor] == current_point and
           h_[neighbor] !=
               h_[current_point] + this->cost(neighbor, current_point))) {
        // 1.子节点是新的节点，这没啥说的
        // 2.周围点与当前点是父子节点关系，那么不管怎么样子节点要继承父节点带来的变化影响
        parent_[neighbor] = current_point;
        this->insert_state(
            neighbor, h_[current_point] + this->cost(neighbor, current_point));
      } else {
        // 以下针对的是曾经不是子节点的周围点，这些点不会受到“修改”的影响，是重新找路径的重要方向
        if (parent_[neighbor] != current_point and
            h_[neighbor] >
                h_[current_point] + this->cost(neighbor, current_point)) {
          // 周围点的代价可以通过该点降低
          // 当前状态点能够用来减少路径代价，但是由于其本身k_old ！= h
          // 其本身的不能立即传播他的影响 所以将其再次添加到open中
          this->insert_state(current_point, h_[current_point]);
        } else {
          if (parent_[neighbor] != current_point and
              h_[current_point] >
                  h_[neighbor] + this->cost(neighbor, current_point) and
              t_[neighbor] == "CLOSED" and h_[neighbor] > kold) {
            // 当前点的代价可以通过周围点来降低，并且曾经是次优的 h[s_n] > k_old
            // 这种情况发生在，直接修改的点上，这些点由于h被人为增大，使得其原来的路径不在最优，所以要查找周围点
            // 周围点曾经就是最优的(前面说过 h[s_n] <= k_old)前面就处理了。
            // 周围点是次优的，说明这是重要的可行路径，因此将这些点放到open中待访问
            this->insert_state(neighbor, h_[neighbor]);
          }
        }
      }
    }
  }
  // 返回最小的k
  return this->get_k_min();
}

std::pair<std::pair<int, int>, double> Dstar::min_state() const {
  // 很大的一个初始值
  double kmin = 1000000.0;
  std::pair<int, int> state;
  // 遍历opne
  for (auto element : open_) {
    if (element.second < kmin) {
      kmin = element.second;
      state = element.first;
    }
  }
  return std::make_pair(state, kmin);
}

double Dstar::get_k_min() const {
  double kmin = 100000.0;
  for (auto element : open_) {
    if (element.second < kmin) {
      kmin = element.second;
    }
  }
  return kmin;
}

void Dstar::insert_state(std::pair<int, int>& current_point, double new_h) {
  // 根据状态调整状态点的k信息
  if (t_[current_point] == "NEW") {
    k_[current_point] = new_h;
  } else if (t_[current_point] == "OPEN") {
    k_[current_point] = std::min(k_[current_point], new_h);
  } else if (t_[current_point] == "CLOSED") {
    // 状态点为CLOSED，我觉得意味着该点已经完成过传播过程，本身是最优的，此时k与h相等
    // 我修改下面的h写成k得到的效果一样
    k_[current_point] = std::min(h_[current_point], new_h); 
  }
  // 调整h,t信息
  h_[current_point] = new_h;
  t_[current_point] = "OPEN";
  // 添加到open中
  open_[current_point] = k_[current_point];
}

void Dstar::delete_state(std::pair<int, int>& current_point) {
  if (t_[current_point] == "OPEN") {
    t_[current_point] = "CLOSED";
  }
  open_.erase(current_point);
}

void Dstar::modify(std::pair<int, int>& current_point) {
  // 修改某处的点的状态
  // 首先修改边界代价
  this->modify_cost(current_point);
  // 修改完以后开始重新找路径
  while (true) {
    double kmin = this->process_state();
    if (kmin >= h_[current_point]) {
      break;
    }
  }
}

void Dstar::modify_cost(std::pair<int, int>& current_point) {
  if (t_[current_point] == "CLOSED") {
    this->insert_state(current_point,
                       h_[parent_[current_point]] +
                           this->cost(parent_[current_point], current_point));
  }
}

std::vector<std::pair<int, int>> Dstar::get_neigbors(
    std::pair<int, int>& current_point) const {
  std::vector<std::pair<int, int>> neighbors;
  int mid_x = current_point.first;
  int mid_y = current_point.second;
  // 开始生成周围点
  for (int i = -1; i <= 1; i++) {
    for (int j = -1; j <= 1; j++) {
      std::pair<int, int> next_point(mid_x + i, mid_y + j);
      if (mid_x + i >= 0 and mid_x + i < map_.map_xlength() and
          mid_y + j >= 0 and mid_y + i < map_.map_ylength() and
          !this->is_collision(current_point, next_point)) {
        neighbors.push_back(next_point);
      }
    }
  }
  return neighbors;
}

double Dstar::cost(std::pair<int, int>& first_point,
                   std::pair<int, int>& second_point) const {
  // 不可通行两点代价为最大值
  if (this->is_collision(first_point, second_point)) {
    return 100000.0;
  }
  return std::sqrt(std::pow(first_point.first - second_point.first, 2) +
                   std::pow(first_point.second - second_point.second, 2));
}

bool Dstar::is_collision(std::pair<int, int>& current,
                         std::pair<int, int>& next) const {
  // 两个点本身是障碍物
  if (map_.GetMapPointState(current.first, current.second) ||
      map_.GetMapPointState(next.first, next.second)) {
    return true;
  }
  std::pair<int, int> p1, p2;
  // 当两点在反对角线上时
  if (next.first - current.first == current.second - next.second) {
    // 反对角线上的两个点不能为障碍物
    p1 = std::make_pair(std::min(current.first, next.first),
                        std::min(current.second, next.second));
    p2 = std::make_pair(std::max(current.first, next.first),
                        std::max(current.second, next.second));
  } else {
    // 对角线或其他平行情况
    p1 = std::make_pair(std::min(current.first, next.first),
                        std::max(current.second, next.second));
    p2 = std::make_pair(std::max(current.first, next.first),
                        std::min(current.second, next.second));
  }
  // 创建的p1,p2不能是障碍物
  if (map_.GetMapPointState(p1.first, p1.second) ||
      map_.GetMapPointState(p2.first, p2.second)) {
    return true;
  }
  return false;
}

void Dstar::make_path() {
  // 先清理原来的path
  path_.clear();
  std::pair<int, int> current_point = start_point_;
  while (current_point != end_point_parent_) {
    path_.push_back(current_point);
    current_point = parent_[current_point];
  }
}

2.3 main.cpp

#include <vector>

#include "dstar.h"
#include "matplotlibcpp.h"

namespace plt = matplotlibcpp;

int main() {
  std::pair<int, int> start_point(5, 5);
  std::pair<int, int> end_point(45, 45);
  mymap::MyMap map(51, 51);
  Dstar dstar(map, start_point, end_point);

  std::vector<int> unfree_x;
  std::vector<int> unfree_y;
  std::vector<int> path_x;
  std::vector<int> path_y;

  dstar.insert_state(end_point, 0);
  while (true) {
    dstar.process_state();
    if (dstar.t_[start_point] == "CLOSED") {
      break;
    }
  }
  dstar.make_path();
  plt::figure();
  for (int i = 20; i >= 0; i--) {
    // 所要改变的点的位置
    int x = 25;
    int y = 20 + i;
    // 修改其在地图上的状态
    dstar.map_.set_point_state(x, y, 1);
    std::pair<int, int> temp = start_point;
    while (temp != end_point) {
      if (dstar.is_collision(temp, dstar.parent_[temp])) {
        dstar.modify(temp);
        continue;
      }
      temp = dstar.parent_[temp];
    }
    //---------------一下为绘图相关---------
    dstar.make_path();
    unfree_x.clear();
    unfree_y.clear();
    for (int i = 0; i < dstar.map_.map_xlength(); ++i) {
      for (int j = 0; j < dstar.map_.map_ylength(); ++j) {
        if (dstar.map_.GetMapPointState(i, j) == 1) {
          unfree_x.push_back(i);
          unfree_y.push_back(j);
        }
      }
    }
    path_x.clear();
    path_y.clear();
    for (auto p : dstar.path_) {
      path_x.push_back(p.first);
      path_y.push_back(p.second);
    }
    plt::clf();
    plt::plot(unfree_x, unfree_y, "ko");
    plt::plot(path_x, path_y, "ro");
    plt::pause(0.1);
  }
  plt::show();
}

5. 仿真实验

Dstar第一次搜索的结果：

添加障碍物后的结果，源代码中是动画形式，这里只展示最后一次的结果：