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

二、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 << GradientDescent(-2, 0.01) << std::endl;
        return 0;
}

// 梯度下降法求二元函数极值
//
// Created by lg on 2024/7/12.
//

#include<iostream>
#include<cmath>
#include <valarray>
using namespace std;
#include<vector>

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

/*
 * 使用梯度下降法求解多元函数极值：f(x,y)=4(x-1)^2+(y-2)^4
 */

/*
  对x的偏导：8（x-1）     对y的偏导：4*（y-2）^3
  */

double func(const double& x, const double& y) {
        return 4 * pow(x-1,2) + pow(y-2,4);
}

vector<double>func_prime(const double& x, const double& y) {
        vector<double> prime;
        prime.push_back(8 * (x - 1));
        prime.push_back(4 * pow(y - 2, 3));
        return prime;
}

vector<double>GradientDescent(const vector<double>& x0, const double LearningRate) {
        vector<double>x = x0;
        vector<double>x_new(2);
        x_new[0] = x[0] - func_prime(x[0], x[1])[0]*LearningRate;
        x_new[1] = x[1] - func_prime(x[0], x[1])[1]*LearningRate;
        while (abs(x_new[0]-x[0])>eps||abs(x_new[1]-x[1])>eps)
        {
                x[0] = x_new[0];
                x[1] = x_new[1];
                x_new[0] = x[0] - func_prime(x[0], x[1])[0] * LearningRate;
                x_new[1] = x[1] - func_prime(x[0], x[1])[1] * LearningRate;
        }
        return x;
}


int main() {
        vector<double>x0 = { 3,4 };
        double learningrate = 0.01;
        vector<double>ans = GradientDescent(x0, learningrate);
        for (auto num : ans) {
                cout << num << " ";
        }
        cout << endl;
}

3、高斯牛顿法

四、针对A*算法中，机器人旋转和大曲率移动代价较高，如何改进？

利用插值函数对拐点处位置和速度进行平滑。

五、采样和优化算法有什么区别？

• 优化研究侧重于评估和预测问题；
• 而采样研究侧重于需要评估不确定性的任务，如形成置信区间和进行假设测试。

然而，在两个研究领域中使用共同的方法要素开始成为一种趋势。

六、TEB算法原理

Time Elastic Band算法把路径规划问题描述为一个多目标优化问题，即对最小化轨迹执行时间、与障碍物保持一定距离并遵守运动动力学约束等目标进行优化。因为优化的大多数目标都是局部的，只与机器人的某几个连续的状态有关，所以该优化问题为对稀疏模型的优化。通过求解稀疏模型多目标优化问题，可以有效获得机器人的最佳运动轨迹。

Time Elastic Band算法有很多的优点，可以满足时间最短、距离最短和远离障碍物等目标以及满足机器人运动动力学的约束。

七、A*算法有什么缺点？如何改善

A*算法没有考虑汽车的运动学模型，比如运动物体的方向和实际运动，受到汽车转向角的限制，可以用 Hybrid A*来算法改进。

八、A*和Dijkstria算法本质有什么区别？

Dijkstra算法计算一个点到其他点的最短路径，而 Astar算法关注点到点的最短路径。Dijkstra算法是广度优先算法（BFS），Astar算法是启发式算法，类似于深度优先算法（DFS）。

九、全局路径规划与局部路径规划本质区别是什么？

全局规划和局部规划是根据对环境信息获取程度来划分的。

• 全局规划依赖于已知的地图或者说事先slam建好的地图，可以得到最优解，但是其精度取决于环境信息的准确度，由于环境地图事先已知，所以对系统的实时计算能力要求不高，但是鲁棒性较差。
• 局部规划是当环境中出现了动态障碍物的时候，需要依赖传感器捕获到的信息，进行实时建图，并做出规划，要求系统高速的实时计算和实时处理能力，鲁棒性要比较好。

十、Hybrid A*的原理是什么？和A*相比做了哪些改进或者优化？

Hybrid A*和A*的区别在于：

（1）A*在节点的扩张时，只会进行相邻节点的直线移动，没有考虑实际底盘的运动学模型（物理模型），而Hybrid A*在每次节点扩张的时候，会对下个位置进行离散状态空间的采样（比如 旋转角度、前进距离、前进/后退），选择动作之后就可以根据运动学模型计算下一个位置点的位姿，每次扩展的时候选择代价最小的节点进行扩展就能逐步到到目标点。这样以来，Hybrid A*得到的路径就具备物理可行性，符合底盘的运动学模型，已经具备基础的轨迹特性。

Hybrid A*缺点：

但缺点是Hybrid A*算法在每次扩展节点时只考虑机身朝向，没考虑前轮转角（方向盘转角）的连续性，所以每次前往下一个节点都必须停下来转动前轮角度，不具备行驶的连续性。

（2）A*的启发式函数h是当前节点的扩展节点到目标点的距离，一般是直线距离（欧式距离），是无障碍物的非完整约束启发代价。而Hybrid A*启发函数是有障碍物的完整约束启发代价，其启发式函数是同时考虑基于A*或者Dijstra算法计算得到的从当前节点的扩展节点到目标节点的实际距离 （考虑有障碍）和 使用RS曲线（考虑无障碍）连接从当前节点的扩展节点和终点的距离，取2者的最大值作为h值。（注：如果当前节点到终点的距离小于阈值，10米或者15米，直接使用RS曲线连接，并加入车身轮廓，判断是否与障碍物发生碰撞，若无碰撞则保留，否则继续扩展节点）。

十一、蚁群算法和常见的搜索算法有什么不同？

蚁群算法是正反馈。当第一只蚂蚁行走到一个路径的时候，会留下信息素，其他蚂蚁会感知这种物质，从而指导自己的运动方向。当某一路径行走的蚂蚁越多，后来者选择这个路径的概率就越大。此外，蚁群算法在求解性能上具有比较强的鲁棒性，容易并行实现，并且适合和多种启发式算法相结合，提升算法的性能。

十二、A*算法中openlist和closelist的作用，节点如何更新？

openlist是一个优先级队列priority_queue,是一个最小堆，根据节点Node的f值进行从小到大排序，存放的是获取到的邻近节点，但还没有探索过；

Closelis是一个无序集合unordered_map，存放的是已经探索过的节点；

节点更新：

对于current节点的邻域节点，分三种情况更新：

1. 如果w是障碍物或者已经在closelist里面，则不处理，continue，忽略他进入下一个邻域节点的判断；
2. 如果w不在openlist中，则将其加入到优先级队列openlist中；
3. 如果w在openlist中(w本来就有一个之前的父节点），需要对比判断是否需要更新父节点（是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

十三、轨迹优化是否需要初始解？作用是什么？

初始解的是为了加快搜索的速度，是问题在最开始的求解比较简单，可能靠近局部最优解、能以较少的迭代次数到达最优值。

十四、Theta*算法原理？碰撞检测算法原理？

和A*的区别：theta* 在扩展邻域节点时 ， 如果当前节点的父节点到邻域节点之间没有障碍物可以正常通行的话，那么就直接舍弃当前节点，直接连接当前节点的父节点 和 这个邻域节点。这样得到的路径会更加笔直、直接，避免了多个路径点堆积在一起影响后续的数据处理。

A*受限于邻近栅格之间的连线，A*在搜索过程中扩展节点的方向被网格形状固定，一般为8邻接，导致路径呈波浪状，不平滑，发生抖动，冗余节点过多。

碰撞检测：Bresenham算法 ，判断当前节点current的父节点current.parent和当前点的扩展点w之间是否有障碍物从起点开始，以1/-1为步长沿着路径迭代，判断沿途经过的所有栅格点的代价是否超过阈值即可完成碰撞检测。

碰撞检测代码：

bool ThetaStar::_lineOfSight(const Node& parent, const Node& child)
{
 //计算从父节点到子节点的 x 和 y 方向上的步长（s_x 和 s_y），以及 x 和 y 方向上的绝对距离（d_x 和 d_y）。
  int s_x = (parent.x() - child.x() == 0) ? 0 : (parent.x() - child.x()) / std::abs(parent.x() - child.x());
  int s_y = (parent.y() - child.y() == 0) ? 0 : (parent.y() - child.y()) / std::abs(parent.y() - child.y());
  int d_x = std::abs(parent.x() - child.x());
  int d_y = std::abs(parent.y() - child.y());

  // check if any obstacle exists between parent and child
//从起点开始，沿着直线路径迭代，直到达到终点。
//在每一步中，根据当前的误差项和直线的斜率来决定是只增加 x 坐标、只增加 y 坐标，还是同时增加 x 和 y 坐标。
  if (d_x > d_y) //沿着 x 轴方向迭代
  {
    int tau = d_y - d_x;
    int x = child.x(), y = child.y();
    int e = 0;
    while (x != parent.x())
    {
      if (e * 2 > tau)
      {
        x += s_x;
        e -= d_y;
      }
      else if (e * 2 < tau)
      {
        y += s_y;
        e += d_x;
      }
      else
      {
        x += s_x;
        y += s_y;
        e += d_x - d_y;
      }
      if (costmap_->getCharMap()[grid2Index(x, y)] >= costmap_2d::LETHAL_OBSTACLE * factor_ &&
          costmap_->getCharMap()[grid2Index(x, y)] >= costmap_->getCharMap()[parent.id()])
        // obstacle detected
        return false;
    }
  }
  else
  {
    // similar. swap x and y
    int tau = d_x - d_y;
    int x = child.x(), y = child.y();
    int e = 0;
    while (y != parent.y())
    {
      if (e * 2 > tau)
      {
        y += s_y;
        e -= d_x;
      }
      else if (e * 2 < tau)
      {
        x += s_x;
        e += d_y;
      }
      else
      {
        x += s_x;
        y += s_y;
        e += d_y - d_x;
      }
      if (costmap_->getCharMap()[grid2Index(x, y)] >= costmap_2d::LETHAL_OBSTACLE * factor_ &&
          costmap_->getCharMap()[grid2Index(x, y)] >= costmap_->getCharMap()[parent.id()])
        // obstacle detected
        return false;
    }
  }
  return true;
}

十五、代码题：一个机器人位于一个 m*n网格的左上角，机器人每次只能向下或者向右移动一步，机器人试图到达网格的右下角，请问有多少种不同的路径？

动态规划，创建二维dp数组：

#include <iostream>
#include <vector>
using namespace std;

int uniquePaths(int m, int n)
{
    vector<vector<int>> dp(m, vector<int>(n, 0));
    for (int i = 0; i < n; i++)
    {
        dp[0][i] = 1;
    }

    for (int i = 0; i < m; i++)
    {
        dp[i][0] = 1;
    }

    for (int i = 1; i < m; i++)
    {
        for (int j = 1; j < n; j++)
        {
            dp[i][j] = dp[i - 1][j] + dp[i][j - 1];
        }
    }
    return dp[m - 1][n - 1];
}

int main()
{
    cout << uniquePaths(5, 6) << endl;
    return 0;
}

十六、启发式路径规划搜索算法中，启发式函数有哪些构建方法？如何设计对应的代价函数？

对于grid map ， 启发式函数主要包括以下三种：

1. 曼哈顿距离：只允许上下左右四个方向移动
2. 欧式距离：两点之间的连线直线距离
3. 对角距离：允许向斜边移动

十七、讲一下Lattice planner 和EM planner

Lattice planner：

采样轨迹末状态就可以生成一系列候选轨迹簇，然后根据cost function来选取最优。

EM planner迭代过程如下图：

Apollo EM planner &&lattice planner

基本思想就是先利用上个周期的规划轨迹以及本周期的目标预测先进行路径规划（S-L）再进行速度规划(S-T)，其中路径规划与速度规划均采用DP+QP求解最优，最后将路径与速度结合生成轨迹送往控制模块以及作为下一周期规划的条件。

DP：SL图（frenet坐标系）上lattice采样，使用五次多项式连接，计算每个连接曲线的cost，使用DP迭代求解取最优

QP：根据DP得到的路径和约束确定凸空间，设计目标函数利用QP求解处平滑且贴近DP最优路径的最终路径；然后将路径投影到S-T图上，再使用DP+QP得到最终速度曲线；最后将路径和速度合成轨迹，传输给控制并用作下一周期的规划条件。

两者比较：

Lattice 适合高速道路、EM适合城市道路

十八、lattice planner的代价函数的作用是什么？

损失函数实为了对采样得到的轨迹簇进行评估，从而使目标轨迹尽量靠近静态参考线轨迹，速度尽量不发生大的突变，满足舒适性要求，尽量远离障碍物、

十九、讲一下LQR和MPC

二十、在非线性规划问题中，KKT条件有哪些？

最优化问题的最优解满足：