【algorithm】自动驾驶常见常考的几个模型和推导，顺便总结自己遇到的考题经验不断更新之———控制版

写在前面

本来快达成目标了，没想到公司遭受了问题，公司和同事我感觉还是挺好的，有国企的正规也有小企业的灵活，大家都很有学习欲望。
作为本次再次复习回忆如下：
把之前面试准备的 机器学习（基本搬运到CSDN平台了）、规划（基于之前的公司同事教导和常见算法轨迹 lattice em 路径 diji a star rrt rrt*）、控制（控制器和模型），工具链（matlab ide repo git anaconda ros linux操作）其他（frenet 、大地车辆坐标转换、点到线距离、找投影、插值、离散点求曲率（以及各种几何问题）、卡尔曼 三次道路多项式 地图参考平滑处理 滤波 轨迹优化）最小二乘：正规方程梯度下降、线性化刷题和有cpp相关的作为小白接触到的知识都总结成了word。 本文先总结搬运模型相关，希望大佬们给点方向提建议，渴望进步，其实对于经典现代控制论本身，很多东西都忘记了，只能作为笔记和考试记录存在了，实践中感觉一直用不上或者不需要再推导了。 现在的烦恼是：其中有很多新东西和非规划控制的内容由于工作方向我只能自己demo，最希望讨论和接触的是实践中积累的问题处理办法，还有能实际在工作中部署最新论文的东西，可惜工作就是工作，同时被专业方向卡住，只能积累再积累。

outline

• stanley pp
• 动力学 运动学 简单推导
• mpc
• pid
• acado 和osqp求解 见另外两篇文章
  Acado：https://blog.csdn.net/weixin_46479223/article/details/133743263的实例中展示了acado如何求解mpc问题，
  另一篇是osqposqp：https://blog.csdn.net/weixin_46479223/article/details/135107257
• 临时简略EM planner 信息
• 车辆动力和运动模型推导

Stanley and pure pursuit

运动学

动力学

并联pid

• 预瞄 2个点 近：lateraldisdiff lateraldisdiffrate 远headingdiff headingdiffrate
• yawrate 要稳 pd要控制好
• 找瞄准点用几何法

串联pid

MPC控制横向 pid纵向

（纵向mpc osqp见另一篇文章的demo）https://blog.csdn.net/weixin_46479223/article/details/135107257

会找投影点 预瞄要选好 用acado osqp ，具体模型一会更新

模型推导

在我的文章Acado：https://blog.csdn.net/weixin_46479223/article/details/133743263的实例中展示了acado如何求解mpc问题

可见acado很方便，直接给出车辆模型的微分方程，以及求解时域（步长）、约束、和目标函数就可以生成相关代码。

因此我们直接得出状态量之间的递推表达关系即可（状态转移方程），推导如下（当然也可以使用之前的动力学和运动学的推导来建立mpc，这里展示使用的另一种横向模型）

以下是除了模型之外的，mpc控制部分的处理（找投影点，预瞄点），以及纵向pid的设置

其他：

-颠簸
-手力
-控制接口转化
-握手
-滤波
-限幅
-限rate
-侧倾
-前置车道线处理
-激活退方向盘稳定渐入渐出
-canoe ape分析
-matlab回放 ros回放
-线性化方法

• usw.
• pid传递函数根轨迹和Nyquist图（工程中直接调参） 现代控制状态空间方法
• 最小二乘正规方程梯度下降
• 找点 向量法：https://zhuanlan.zhihu.com/p/429676544
• 点线距离
• kkt

临时EM planner

补充：找起点时间要顺延一个planning cycle

• 轨迹筛选、碰撞检测
• 巡航为四次多项式 没有终点s
• frenet下的平滑不代表卡迪尔下平滑
  注意 以下图片均出自于老王 bilibili
  
  
  
  
  
  
  
  

特殊情况

以前的杂乱总结版本

最后听讲（自己看）

临时 cpp gpt d 算法简易版本

之前理解 d就是带有权重的bfs问题，所以一直没有去认真实现代码，后来做了很多网格问题，但发现和真的图bfs还有使用优先队列的d还有细节要注意，见和gpt的问答，注意这段代码没有记录父节点

#include <iostream>
#include <vector>
#include <queue>
#include <limits>

using namespace std;

#define INF numeric_limits<int>::max()

// 定义图的邻接矩阵表示法
typedef vector<vector<int>> Graph;

// Dijkstra算法
void dijkstra(const Graph& graph, int start, vector<int>& distances, vector<int>& parents) {
    int n = graph.size(); // 图的节点数
    distances.resize(n, INF); // 初始化距离数组，全部设置为无穷大
    parents.resize(n, -1); // 初始化父节点数组，全部设置为-1
    vector<bool> visited(n, false); // 记录节点是否已被访问
    distances[start] = 0; // 起始节点到自身的距离为0

    // 创建优先队列，用于按照距离排序节点
    priority_queue<pair<int, int>, vector<pair<int, int>>, greater<pair<int, int>>> pq;
    pq.push({0, start}); // 将起始节点加入优先队列

    while (!pq.empty()) {
        // 从优先队列中取出当前距离最小的节点
        int curr_dist = pq.top().first;
        int curr_node = pq.top().second;
        pq.pop();

        // 如果该节点已被访问，则跳过
        if (visited[curr_node]) continue;
        
        // 将该节点标记为已访问
        visited[curr_node] = true;

        // 遍历当前节点的所有邻居节点
        for (int neighbor = 0; neighbor < n; ++neighbor) {
            // 如果当前节点到邻居节点的距离更短，则更新距离数组、父节点数组并将邻居节点加入优先队列
            if (graph[curr_node][neighbor] != 0 && 
                distances[curr_node] + graph[curr_node][neighbor] < distances[neighbor]) {
                distances[neighbor] = distances[curr_node] + graph[curr_node][neighbor];
                parents[neighbor] = curr_node; // 更新邻居节点的父节点
                pq.push({distances[neighbor], neighbor});
            }
        }
    }
}

// 输出最短路径
void printShortestPath(const vector<int>& parents, int node) {
    if (parents[node] == -1) {
        cout << node;
        return;
    }
    printShortestPath(parents, parents[node]);
    cout << " -> " << node;
}

int main() {
    // 定义图的邻接矩阵
    Graph graph = {
        {0, 4, 0, 0, 0, 0, 0, 8, 0},
        {4, 0, 8, 0, 0, 0, 0, 11, 0},
        {0, 8, 0, 7, 0, 4, 0, 0, 2},
        {0, 0, 7, 0, 9, 14, 0, 0, 0},
        {0, 0, 0, 9, 0, 10, 0, 0, 0},
        {0, 0, 4, 14, 10, 0, 2, 0, 0},
        {0, 0, 0, 0, 0, 2, 0, 1, 6},
        {8, 11, 0, 0, 0, 0, 1, 0, 7},
        {0, 0, 2, 0, 0, 0, 6, 7, 0}
    };

    int start_node = 0; // 设置起始节点

    // 使用Dijkstra算法计算起始节点到所有节点的最短距离和记录父节点
    vector<int> distances;
    vector<int> parents;
    dijkstra(graph, start_node, distances, parents);

    // 输出最短路径
    cout << "Shortest paths from node " << start_node << ":\n";
    for (int i = 0; i < graph.size(); ++i) {
        cout << "Node " << start_node << " to Node " << i << ": ";
        printShortestPath(parents, i);
        cout << " (distance: " << distances[i] << ")" << endl;
    }

    return 0;
}


//QKF:如果每次向qp里面push相邻的节点 ，那么qp 里面可能存在相同的节点 是通过 if (visited[curr_node]) continue;跳过去的嘛

//GPT:是的，如果每次向优先队列 pq 中 push 相邻的节点，可能会导致 pq 中存在相同的节点。在这种情况下，通过 if (visited[curr_node]) continue; 来跳过已经访问过的节点是有必要的。

//在 Dijkstra 算法中，如果不跳过已经访问过的节点，可能会导致多次将相同节点加入优先队列，并且每次加入的节点可能会有不同的距离值。这样会导致算法不断重新考虑相同的节点，从而增加了计算的开销，而且可能会导致错误的最短路径结果。

//通过在代码中添加 if (visited[curr_node]) continue; 这样的语句，可以确保在每次迭代中只处理未被访问过的节点，避免了重复处理相同节点的情况，从而提高了算法的效率。