outlinestanley pp动力学 运动学 简单推导mpcpidacado 和osqp求解 见另外两篇文章Acadohttps://blog.csdn.net/weixin_46479223/article/details/133743263的实例中展示了acado如何求解mpc问题另一篇是osqposqphttps://blog.csdn.net/weixin_46479223/article/details/135107257临时简略EM planner 信息车辆动力和运动模型推导常见横向控制总结控制 可以用pid 无模型 并串联控制pp stanley 简化模型控制 本文lqr mpc 优化控制 自行车运动学带有后轮转向运动学动力学控制涉及到运动学 误差模型 线性化 离散化 计算前馈等信息分别多次总结于https://blog.csdn.net/weixin_46479223/article/details/135777673?spm1011.2415.3001.5331https://blog.csdn.net/weixin_46479223/article/details/152123646?spm1011.2415.3001.5331https://editor.csdn.net/md/?articleId157690791Stanley and pure pursuit运动学动力学并联pid预瞄 2个点 近lateraldisdiff lateraldisdiffrate 远headingdiff headingdiffrateyawrate 要稳 pd要控制好找瞄准点用几何法串联pidMPC控制横向 pid纵向纵向mpc osqp见另一篇文章的demohttps://blog.csdn.net/weixin_46479223/article/details/135107257会找投影点 预瞄要选好 用acado osqp 具体模型一会更新模型推导在我的文章Acadohttps://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轨迹筛选、碰撞检测巡航为四次多项式 没有终点sfrenet下的平滑不代表卡迪尔下平滑注意 以下图片均出自于老王 bilibili特殊情况以前的杂乱总结版本最后听讲自己看临时 cpp gpt d 算法简易版本之前理解 d就是带有权重的bfs问题所以一直没有去认真实现代码后来做了很多网格问题但发现和真的图bfs还有使用优先队列的d还有细节要注意见和gpt的问答注意这段代码没有记录父节点#includeiostream#includevector#includequeue#includelimitsusingnamespacestd;#defineINFnumeric_limitsint::max()// 定义图的邻接矩阵表示法typedefvectorvectorintGraph;// Dijkstra算法voiddijkstra(constGraphgraph,intstart,vectorintdistances,vectorintparents){intngraph.size();// 图的节点数distances.resize(n,INF);// 初始化距离数组全部设置为无穷大parents.resize(n,-1);// 初始化父节点数组全部设置为-1vectorboolvisited(n,false);// 记录节点是否已被访问distances[start]0;// 起始节点到自身的距离为0// 创建优先队列用于按照距离排序节点priority_queuepairint,int,vectorpairint,int,greaterpairint,intpq;pq.push({0,start});// 将起始节点加入优先队列while(!pq.empty()){// 从优先队列中取出当前距离最小的节点intcurr_distpq.top().first;intcurr_nodepq.top().second;pq.pop();// 如果该节点已被访问则跳过if(visited[curr_node])continue;// 将该节点标记为已访问visited[curr_node]true;// 遍历当前节点的所有邻居节点for(intneighbor0;neighborn;neighbor){// 如果当前节点到邻居节点的距离更短则更新距离数组、父节点数组并将邻居节点加入优先队列if(graph[curr_node][neighbor]!0distances[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});}}}}// 输出最短路径voidprintShortestPath(constvectorintparents,intnode){if(parents[node]-1){coutnode;return;}printShortestPath(parents,parents[node]);cout - node;}intmain(){// 定义图的邻接矩阵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}};intstart_node0;// 设置起始节点// 使用Dijkstra算法计算起始节点到所有节点的最短距离和记录父节点vectorintdistances;vectorintparents;dijkstra(graph,start_node,distances,parents);// 输出最短路径coutShortest paths from node start_node:\n;for(inti0;igraph.size();i){coutNode start_node to Node i: ;printShortestPath(parents,i);cout (distance: distances[i])endl;}return0;}//QKF:如果每次向qp里面push相邻的节点 那么qp 里面可能存在相同的节点 是通过 if (visited[curr_node]) continue;跳过去的嘛//GPT:是的如果每次向优先队列 pq 中 push 相邻的节点可能会导致 pq 中存在相同的节点。在这种情况下通过 if (visited[curr_node]) continue; 来跳过已经访问过的节点是有必要的。//在 Dijkstra 算法中如果不跳过已经访问过的节点可能会导致多次将相同节点加入优先队列并且每次加入的节点可能会有不同的距离值。这样会导致算法不断重新考虑相同的节点从而增加了计算的开销而且可能会导致错误的最短路径结果。//通过在代码中添加 if (visited[curr_node]) continue; 这样的语句可以确保在每次迭代中只处理未被访问过的节点避免了重复处理相同节点的情况从而提高了算法的效率。