A*算法-地码导航

金陵码聋

已于 2025-03-13 12:55:05 修改

阅读量1.3k

点赞数 33

文章标签：算法 java

于 2025-03-13 10:45:38 首次发布

本文链接：https://blog.csdn.net/u010712367/article/details/144429497

版权

仓储物流行业使用的地面搬运机器人导航一般使用地面贴码或者图像视觉导航。对于地面贴码的导航，一般推荐A*算法进行路径规划。本文就以地面导航，说明下A*算法的实际应用。

假设搬运机器人只能进行前后左右的移动，不支持进行斜角移动；地面地码以二维坐标的形式排列：间距可以不一样，但搬运机器人移动时只能移动到X相同或Y相同的相邻坐标。

A*算法是一种启发式探索算法，具体的定义可以自行百度。

先不考虑A*算法的套用，假设小车想从地码7移动到地码10，此时有哪些路径可达？理论上7->10有非常多的路径可达，比如7->2->3->4->8->9->10。当然7->8->9->10这一条路径肯定是最优的。如何在众多的路径种找到该条路径，就是我们的目的。

曼哈顿距离

从7出发，我们可以直达的节点有2、6、8、12；想要到达到节点10，那么如何从2、6、8、12找到下一个目标点很重要。如何判断那个节点最优？通过曼哈顿距离选择。

曼哈顿距离 = |X轴差值| + |Y轴差值|

节点2-10 = 3 + 1 = 4

节点6-10 = 4 + 0 = 4

节点8-10 = 2 + 0 = 2

节点12-10 = 3 + 1 = 4

通过曼哈顿距离可知8节点最优，此时在以8节点重复上述过程找到下一个最优节点9，直到节点10。此时得到的路径就是最优路径。

代价值

通过上述曼哈顿距离是不是感觉探索路径很简单？但是实际应用中往往比上述情况复杂。

假设节点7到节点8直接有障碍物不可达，此时7可直达节点只有2、6、12，而且他们的曼哈顿距离一样；此时需要通过遍历3个节点周边节点得到曼哈顿的值，并与从节点7到该节点的实际距离（代价）相加后得到和，即为这条路径当前的总代价值。需要选择代价值最小的作为下次探索的起点：

曼哈顿距离节点1-10 = 4 + 1 = 5

曼哈顿距离节点3-10 = 2 + 1 = 3

曼哈顿距离节点11-10 = 4 +1 = 5

曼哈顿距离节点13-10 = 2 + 1 = 3

路径代价值：

节点6-1-10 = 2 + 5 = 7

节点2-3-10 = 2 + 3 = 5

节点6-11-10 = 2 +5 = 7

节点12-13-10 = 2 + 3 = 5

通过进一步的总代价值，可知节点3与13等价且当前最优。

曼哈顿计算公式：

  /**
  * 通过曼哈顿公式计算两个节点间的距离
  *
  * @param node1 起始节点
  * @param node2 目标节点
  * @return 距离
  */
  private int manhattan(Address node1, Address node2) {
      return Math.abs(node1.getX() - node2.getX()) + Math.abs(node1.getY() - node2.getY());
  }

探索方法：

/**
  * 根据上下左右方向探索周边节点
  *
  * @param end        目标节点
  * @param bonds      关系
  * @param openList   待探索的节点
  * @param closedList 已探索的节点
  * @param current    当前节点
  */
private void peripheryNode(Address end, List<CodeBond> bonds,
List<Address> openList, HashMap<Integer, Address> closedList, Address current, Map<String, Address> codeNodes) {
    if (CollectionUtils.isEmpty(bonds)) {
        return;
    }
    for (CodeBondbond : bonds) {
        Address neighbor = codeNodes.get(bond.getTargetCode().toString());
        if (null == neighbor || neighbor.isObstacle() || 
        closedList.containsKey(neighbor.getId())) {
        continue;
     }
     int gScore = current.getGCost() + bond.getSpacing();
     if (isNeedTurn(current, neighbor)) {
         // 如果需要转向，则代价增加转弯的代价值
         gScore = gScore + TURN_SCORE * bond.getSpacing();
     }
     boolean isBestPath = false;
     if (!openList.contains(neighbor)) {
         neighbor.setHCost(manhattan(neighbor, end));
         openList.add(neighbor);
         isBestPath = true;
     } else {
         neighbor = getFromOpenList(openList, neighbor);
     }
     if (gScore <= neighbor.getGCost()) {
         isBestPath = true;
     }
     if (isBestPath) {
         // 当前节点到下个节点的距离
         current.setCost(bond.getSpacing());
         neighbor.addParent(current);
         neighbor.setGCost(gScore);
         neighbor.setFCost(neighbor.getGCost() + neighbor.getHCost());
     }
}


  /**
  * 从待探索的集合中获取已经有的节点，用以更新
  *
  * @param openList 已探索集合
  * @param neighbor 下个节点
  * @return 已存在的NODE
  */
  private Address  getFromOpenList(List<Address > openList, Address  neighbor) {
     for (Address  open : openList) {
         if (open.getId().equals(neighbor.getId())) {
             return open;
         }
     }
      return neighbor;
  }

初始化计算及逆推计算后的路径：

  Address startCode = new Address();
  Address endCode = new Address();
  List<Address> openList = new ArrayList<>();
  startCode.setGCost(0);
  startCode.setHCost(manhattan(startCode, endCode));
  startCode.setFCost(startCode.getGCost() + startCode.getHCost());
  openList.add(startCode);
  HashMap<Integer, Address> closedList = new HashMap<>(codeNodes.size());
  while (!openList.isEmpty()) {
  openList.sort(Comparator.comparing(Address::getFCost).thenComparing(Address::getFCost));
  Address current = openList.get(0);
  if (Objects.equals(current.getX(), endCode.getX()) && Objects.equals(current.getY(),                 
      endCode.getY())) {
      return constructPath(current);
  }
  openList.remove(current);
  closedList.put(current.getId(), current);
  peripheryNode(endCode, codeBonds.get(current.getId().toString()), openList, 
  closedList, current, codeNodes);


  /**
  * 根据目标节点，逆推路径节点
  *
  * @param node 目标节点
  * @return 路径集合
  */
  private List<AddressCodeNode> constructPath(AddressCodeNode node) {
      List<AddressCodeNode> path = new ArrayList<>();
  while (node != null) {
  path.add(node);
  AddressCodeNode par = getParParent(node);
  if (null == par) {
  break;
  }
  par.setChild(node);
  node = par;
  }
  // 将当前节点的上级、下级节点的上/下节点置为null，防止JSON多次递归异常
  path.forEach(codeNode -> {
  List<AddressCodeNode> parent = codeNode.getParent();
  if (CollectionUtils.isNotEmpty(parent)) {
  AddressCodeNode par = new AddressCodeNode();
  BeanUtils.copyProperties(parent.get(0), par);
  par.setParent(null);
  par.setChild(null);
  codeNode.setParent(Collections.singletonList(par));
  }
  AddressCodeNode child = codeNode.getChild();
  if (null != child) {
  AddressCodeNode childNode = new AddressCodeNode();
  BeanUtils.copyProperties(child, childNode);
  childNode.setParent(null);
  childNode.setChild(null);
  codeNode.setChild(childNode);
  }
  });
  Collections.reverse(path);
  return path;
  }