基于BFS+DFS+CoordinateDescent的motion planning设计思路

作者受论文path planning for on-road autonoumous driving with concetrated iterative search的启发，设计了一个新的基于linear segments的motion planner.

方法分为三部分：首先进行深度搜索，获得粗路径，再用广度搜多获得精确路径，再用CD迭代获得平滑路径。

本文持续更新：
基于BFS的搜索方案

clc
clear all
obs = [4.5, 0.5; 5.5,0.5];
point_ini = [0,0];
i = 1;
alpha = 1;
beta = 10;
point.layer(i).x(1) = point_ini(1);
point.layer(i).y(1) = point_ini(2);
end_length =10;
while i <end_lengthx = point.layer(i).x(1) + 1y_min = max(0, min(point.layer(i).y -1));y_max = min(5, max(point.layer(i).y + 1));temp = [];for j = 1:length(point.layer(i).x)for y = max(y_min, point.layer(i).y(1) - 1) : min(y_max, point.layer(i).y(1) + 1)cost = alpha * norm([1, (y - point.layer(i).y(1))]) + beta * abs(y-3);temp = [temp; [cost,x,y]];endendtemp = sortrows(temp);i = i+1;for k = 1:size(temp,1)point.layer(i).x(k) = temp(k, 2);point.layer(i).y(k) = temp(k, 3);end
endfor i = 1: end_lengthplot_x(i) = point.layer(i).x(1);plot_y(i) = point.layer(i).y(1);
endfigure
plot(plot_x, plot_y,'-o')
grid on
path = [plot_x; plot_y]';
figure
for i = 0.25for j = 0.1alpha = i;beta = j;optPath=PathSmoothing(path,alpha, beta);plot(path(:,1),path(:,2),'*b');hold on;plot(optPath(:,1),optPath(:,2),'-or');hold on;axis([-1 7 -2 6])legend('Before','After');title('Path Smoothing');grid on;hold on;end
end
hold offfunction optPath=PathSmoothing(path,alpha, beta)
optPath=path;%
torelance=0.00001;
change=torelance;
while abs(change)>=torelance change=0;for ip=2:(length(path(:,1))-1) prePath=optPath(ip,:);optPath(ip,:)=optPath(ip,:)-alpha*(optPath(ip,:)-path(ip,:));optPath(ip,:)=optPath(ip,:)-beta*(2*optPath(ip,:)-optPath(ip-1,:)-optPath(ip+1,:));change=change+norm(optPath(ip,:)-prePath);end
end
end

基于BFS+DFS+CoordinateDescent的motion planning设计思路相关推荐

基于界面交互展开的用例设计思路
测试用例是测试人员日常最重要的输出之一,对用例的评价标准一般有三个维度:结构清晰易读.可执行性强.覆盖度高.站在质量维度,最为重要的要属高覆盖度.如何写出高覆盖度的设计用例,离不开以下几个角度的分析. ...
基于SaaS化的低代码平台设计思路（二）
经过多日的整理,数据库结构算是把初版本弄出来了,准备采用DDD的模式进行,类似于金蝶这样的产品,直接通过BOS设计器进行业务单据的开发.摒弃传统的手工一行一行敲代码,将业务进行细化,用业务驱动整个平台 ...
基于OHCI的USB主机 —— OHCI（设计思路）
在OHCI层,主要完成如下功能: l 通过控制端口读写数据(包含SETUP.DATA.STATUS等3个TD): l 通过控制端口发送设置命令(没有DATA的TD): l 通过批量端口读数据: ...
Robotics: Computational Motion Planning（路径规划）笔记（一）:基于图搜索的方法-Grassfire、Dijkstra和A*算法
在本课程中,我们将考虑机器人如何决定如何到达目标的问题.这个问题通常被称为运动规划,它以不同的方式来描述不同的情况.您将学习一些解决这个问题的最常用方法,包括基于图的方法.基于采样的方法和人工势场法. ...
高动态环境下基于随机可及集的Path-Guided APF算法的Motion Planning
文章目录摘要引言相关工作问题假设动态障碍物 Relative robot-obstacle dynamics(运动学) SR Sets for Collision Avoidance 方法! ...
【八数码问题】基于状态空间法的知识表示与状态搜索:无信息搜索(BFS/DFS) 启发式搜索(A*)
前言一.问题引入二.状态空间法 1. 知识及其表示 2. 状态空间法定义 3. 问题求解三.基于状态空间搜索法解决八数码问题 1. 八数码问题的知识表示 2. 状态空间图搜索 1. 无信息搜索 ...
【规划】Motion Planning Approaches 路径规划方法汇总
什么是规划规划的本质是搜索. 基于目标函数,找到一个安全舒适的行驶轨迹. 机器学习的话,其实是一个寻找mapping的过程,state to action 的mapping 搜索是寻找action去 ...
路径规划(Path Planning)与运动规划(Motion Planning)
路径规划(Path Planning)与运动规划(Motion Planning) 评论区有个观点: 也可以把路径规划理解为运动规划的一份．我个人具体的认识是,特别是在实现的时候,路径规划我们强调的是 ...
Apollo进阶课程㉒丨Apollo规划技术详解——Motion Planning with Autonomous Driving
原文链接:进阶课程㉒丨Apollo规划技术详解--Motion Planning with Autonomous Driving 自动驾驶车辆的规划决策模块负责生成车辆的行驶行为,是体现车辆智慧水平的 ...

基于BFS+DFS+CoordinateDescent的motion planning设计思路

基于BFS+DFS+CoordinateDescent的motion planning设计思路相关推荐

最新文章

热门文章