一、你在项目中遇到的最有挑战的一件事情是什么？或者说有没有遇到什么困难？

二、A*算法原理（伪代码）

输入：代价地图、start 、 goal（Node结构，包含x、y、g、h、id、pid信息）

首先初始化：创建一个优先级队列openlist，它是一个最小堆，根据节点的f值排序 （ priority_queue<Node, std::vector<Node>, Node::compare_cost> openlist）；

再创建一个无序集合unordered_map哈希表 closelist，用来存放已访问的节点 ， 其键是节点的id （unordered_map<int, Node> closelist）;

然后再将起点start 加入到openlist中 （openlist.push(start)）;

• 进入while 循环 ，当openlist不为空时 ，执行以下步骤：

* 从 openlist 中取出具有最小f值的节点，称为 current 节点，并将其从 openlist 中移除，

* 将 current 节点添加到 closelist 中。

* 如果当前节点current是目标点goal，则从当前点回溯到起点 ，返回路径path；

否则，对于 current 节点的每个邻域节点 w：（分三种情况）

• 如果 w 是障碍物或已在 closelist 中，则continue，忽略它并继续下一个循环
• 如果 w 不在 openlist 里面 ， 将其加入 openlist 中，
• 如果 w在openlist里面（需要判断是否更新他的父节点，是current还是current.parent)，检查是否通过 current 节点到达 w 的路径更优（即具有更小的g值）：f(w) > f(n)+d(w,n)+h(w)

如果g值更小 ， 则更新w的父节点为当前节点n 并更新 他的g值g(w) = f(n)+d(w,n)

------ 则：f(w) = f(n)+d(w,n)+h(w) --------

------ w.parent = n. -----------

最终没有路径的话就返回false。

核心代码：

 bool Astar::plan(const Node& start, const Node& goal, std::vector<Node>& path, std::vector<Node>& expand) {
     // clear vector
     path.clear();
     expand.clear();

     // openlist  and   closelist
     std::priority_queue<Node, std::vector<Node>, Node::compare_cost> openlist;// 底层queue内部使用vector容器，比较器实现最小堆（从小到大）
     //  return (n1.g() + n1.h() > n2.g() + n2.h()) || ((n1.g() + n1.h() == n2.g() + n2.h()) && (n1.h() > n2.h()));
     std::unordered_map<int, Node> closelist;
     
     openlist.push(start);

     // get all possible motions
     const std::vector<Node> motions = Node::getMotion();// 8领域的坐标x y 和 g

     //main process
     while (!openlist.empty())
     {
         // pop current node from openlist
         Node current = openlist.top();
         openlist.pop();

        
         if (closelist.find(current.id()) != closelist.end())continue; // 如果闭列表里面有current节点，跳过while循环

         //if current node does not exists in closelist
         closelist.insert(std::make_pair(current.id(), current)); // 将current加入到closelist中
         expand.push_back(current);

         //goal found 
         if (current == goal) {
             path = _convertClosedListToPath(closelist, start, goal);// 回溯父节点找到路径
             return true;
         }

         //explore neighbor of current Node
         for (const auto& motion : motions) {
             //explore a new node
             Node node_new = current + motion;
             node_new.set_id(grid2Index(node_new.x(), node_new.y()));

             // if node_new in closelist
             if (closelist.find(node_new.id()) != closelist.end())continue;

             // if node_new is not in closelist
             node_new.set_pid(current.id());//设置node_new的父节点id 

             // next node hit the boundary or obstacle   碰撞检查：没有超出地图边界并且没有碰到障碍物
             // prevent planning failed when the current within inflation   防止current点在障碍物的膨胀区域
             if ((node_new.id() < 0) || (node_new.id() >= map_size_) || //超出地图边界
                 (costmap_->getCharMap()[node_new.id()] >= costmap_2d::LETHAL_OBSTACLE * factor_ &&   //检查 node_new 对应的成本地图 costmap_ 中的值是否表示障碍物
                     costmap_->getCharMap()[node_new.id()] >= costmap_->getCharMap()[current.id()])) //比较 node_new 和当前节点 current 在成本地图中的成本值
                 continue;
            // 如果周围点在openlist中（表面该邻居已有父节点），计算该父节点经过当前点n再到周围点是否使其能够得到更小的g，如果可以，将该周围点的父节点设置为当前点n，并更新其g和h值。

             // if using dijkstra implementation, do not consider heuristics cost
             if (!is_dijkstra_) {
                 node_new.set_h(helper::dist(node_new, goal));//hypot(node1.x() - node2.x(), node1.y() - node2.y())
                 // node_new 到 goal 的直线欧式距离
             }


             // if using GBFS implementation, only consider heuristics cost
             if (!is_gbfs_) {
                 node_new.set_g(0.0);
             }

             openlist.push(node_new);// 将周围的可行邻域加入 openlist 中

         }
     }

     return false;// can't find a path 
 }

三、在决策规划中常用的非线性数值优化算法

1、牛顿法

对目标函数进行二阶泰勒近似，需要计算hessian矩阵：

f(x)/(x-x_new) = f'(x);
x_new = x - f(x)/f'(x);

牛顿法原本是求解函数零点的数值计算方法，利用迭代的方式逼近0点;

利用牛顿法求解函数极值的话，需要求解原函数一阶导的表达式，然后再对这个一阶函数使用牛顿法求解0点，需要求解原函数的二阶导。

2、梯度下降法

对目标函数进行一阶泰勒近似：

x_new = x - LearningRate * func_prime(x);

// 无约束优化算法
//
// Created by lg on 2024/7/12.
//
#include<iostream>
#include<cmath>

constexpr double eps = 1e-6;
constexpr double Delta = 1e-4;

//原函数
double func(const double& x) {
        return  x * x - 4 * x;
}

//原函数的一阶导
double func_prime(const double& x) {
        return (func(x + Delta) - func(x - Delta)) / (2 * Delta);
}

//原函数的一阶导
double func_prime2(const double& x) {
        return 2 * x - 4;;
}

double GradientDescent(const double& x0, const double& LearningRate) {
        double x = x0;
        double x_new = x - LearningRate * func_prime(x);
        while (abs(x_new-x)>eps)
        {
                x = x_new;
                x_new = x - LearningRate * func_prime(x);
        }
        return x;
}

int main() {
        std::cout << GradientDescen