针对Hybrid A*论文解析(5)中的方法的一些验证

用matlab试了一个论文分析5里面的一些方法，感觉还是有很多问题，并没有像文章说的那么好。

curvature梯度方向的优化基本用不上，梯度的下降方向非常大，而且很不稳定，就算限制了梯度下降的步长，效果也不是很好。从开源代码里面也可以看到，curvature部分直接变成了0，说明代码里面也没用curvature这一项的优化。

smoothness 方向的优化，只有当原始曲线噪声比较大的时候优化效果还可以，一旦原始曲线也比较平滑，优化效果也会一般。所谓的原始曲线的噪声比较大，如下如：

如果对一条平滑曲线进行优化，smoothness和 curvature term同时工作，效果也一般：

下图是平均曲率，的确曲率是在下降，修改过的公式没有问题，但是可以看到效果的确很差，下降的速度被限制后效果就没了：

如果我们不限制下降速度，直接用算出来的梯度做下降，平均曲率的确下降的很快，但是曲线就发生了论文中所说的塌缩问题：

而且这里面影响梯度下降的效果的因素比较多，很难调整出一个比较好的组合方案：包括两个梯度方向的占比wSmoothness = 0.5;wCurvature = 0.5;，这里的比例怎么选取，以及梯度下降步长alpha怎么选取都是个问题。

于是还是放弃了这个方案的优化。选择了之前的另一个轨迹平滑的思路，这里面对轨迹平滑的梯度下降方向考虑两点，第一个，希望优化后的曲线与原来曲线尽量相近，第二个，希望优化后的曲线曲率减小，但是没有通过向曲率下降方向实现，而是用了上面链接中的思路，delta_x**2, (delta_(x+1))**2的求和，因为直线的这两个term的求和肯定比曲线小，所以通过向这个方向的梯度下降，一定能够起到将曲线拉直的效果。

同时也考虑了几点终止迭代的要求，第一个迭代次数要fix,确保这个函数在有限时间里一定会停止。第二个，最后优化完的曲线上的点和原始曲线的点的最大距离小于某个值，这可以保证优化后的曲线肯定在原始曲线附近。第三个，新优化后的曲线计算一下曲率，如果每个点上的曲率已经满足了设定的要求比如500m，就停止迭代。

但是这个方案中依旧有很多参数的选择是不确定的。比如像两个方向的梯度下降步长alpha, beta具体怎么选取，设定的曲率要求怎么设定，两条曲线的限制距离选择多大，这些都需要和具体的曲线形状关联起来。

还有一个问题，就是在使用梯度下降方案中必须考虑梯度下降步长的收敛性，这里可以用过验证wolfe第一和第二条件来确保下降步长是不是收敛，更复杂一些的也可以使用柯西点的(Chauchy Point)方法确定最优步长，或者使用信赖域的(trust region)方法来动态修改步长等等。在本次分析里这些内容暂时还没被验证。

通过上述的方案，对一条曲线进行优化，效果如下：

下面为开源的代码，希望大家继续优化：

clc
clear all
close all
D0 = 0;
Ti = 80;
Di = 3.5;
path = [];
for i = 0:0.1:Tipath = [path;Bezierfrenet_5(D0, Ti, Di,i)]; % generate an initial bezier curve
% p = [p;[i,sin(i)]];
end
path(:,2) = path(:,2);%+ rand(length(p),1)/10;
p = path;
% calculate the first derivativesfor i = 1:length(p)-1pd(i) = (p(i+1,2)-p(i,2))/(p(i+1,1)-p(i,1));pd(length(p)) = 0;end
% calculate the second derivativesfor i =2: length(p)-1pdd(1) = 0;pdd(length(p)) = 0;pdd(i) = (p(i+1,2)-2*p(i,2) + p(i-1,2))/(0.5*(-p(i-1,1)+p(i+1,1)))^2;
%    pdd(i) = (p(i+1,2)-2*p(i,2) + p(i-1,2))/(-p(i,1)+p(i+1,1))^2;endfor i  = 1:length(p)k(i) = (pdd(i))/((1+pd(i)^2)^(1.5));endkappa = zeros(1,length(p));
for i = 2 : length(p) - 1xi = p(i,:);xim1 = p(i-1,:);xip1 = p(i+1,:);Dxi = xi - xim1; % 1 by 2Dxip1 = xip1 - xi; % 1 by 2absDxi = norm(Dxi); % 1absDxip1 = norm(Dxip1); % 1 gradient = [0,0]; % 1 by 2Dphi = acos(clamp((Dxi * Dxip1' / (absDxi * absDxip1)),-1,1)); % 1 kappa(i) = Dphi / absDxi; % 1
end
figure
plot(1:length(p),k(1:length(p)),'-b')
hold on
plot(1:length(p),kappa(1:length(p)),'--r')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% above parts provide a comparison between to difference curvature
% calculation, while we decide to trust the first one
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
for i = 0.01%0.1:0.1:0.5for j = 0.5%0.05:0.05:0.2alpha = i;beta = j;optPath=PathSmoothing(path, alpha, beta);end
endplot(path(:,1),path(:,2),'-r');hold on;plot(path(619,1),path(619,2),'or');hold on;plot(optPath(:,1),optPath(:,2),'--b');hold on;plot(optPath(619,1),optPath(619,2),'ob');legend('Before','After');title('Path Smoothing');grid on;hold off;function optPath=PathSmoothing(path, alpha, beta)optPath=path;%TOL = 0.7;iter = 0;COND = 1;while COND == 1
%         change=0;for ip=2:(length(path(:,1))-1) optPath(ip,:)=optPath(ip,:)-alpha*(optPath(ip,:)-path(ip,:));term_dx_1 = 2*optPath(ip,:)-optPath(ip-1,:)-optPath(ip+1,:);
%             term_dx_2 = optPath(ip-2,:) - 4*optPath(ip-1,:) + 6 * optPath(ip,:)...
%                 -4 * optPath(ip+1,:) - optPath(ip+2,:);optPath(ip,:) = optPath(ip,:)-beta * term_dx_1;
%           change=change+norm(optPath(ip,:)-prePath);endkappa = calculate_kappa(optPath);iter = iter + 1change = zeros(1,length(optPath));for i = 1:length(optPath)change(i) = norm(optPath(i,:)-path(i,:));if max(abs(change)) >= TOLCOND = 2[value, loc] = max(abs(change))break;endendif iter > 10000COND = 3;endsum = 0;for i = 1: length(kappa)    if abs(kappa(i)) > 0.002sum = sum + 1; endendif sum == 0COND = 4endend
endfunction [p] = Bezierfrenet_5(D0, Ti, Di,t)p0 = [ 0, D0];p1 = [0.25 * Ti, D0];p2= [0.5 *Ti, D0];p3 =[ 0.5 * Ti, Di];p4 =[ 0.75 * Ti, Di];p5 = [Ti, Di];%generate a fifth order bezier curvep= (1-(t)/Ti)^5*p0 + 5*(1-(t)/Ti)^4*((t)/Ti)*p1 + 10*(1-(t)/Ti)^3*((t)/Ti)^2*p2 +...10*(1-(t)/Ti)^2*((t)/Ti)^3*p3 +5*(1-(t)/Ti)*((t)/Ti)^4*p4 + ((t)/Ti)^5*p5;endfunction y =clamp(n, lower, upper)y = max(lower, min(n,upper));
endfunction k = calculate_kappa(p) % calculate the first derivativesfor i = 1:length(p)-1pd(i) = (p(i+1,2)-p(i,2))/(p(i+1,1)-p(i,1));pd(length(p)) = 0;end% calculate the second derivativesfor i =2: length(p)-1pdd(1) = 0;pdd(length(p)) = 0;pdd(i) = (p(i+1,2)-2*p(i,2) + p(i-1,2))/(0.5*(-p(i-1,1)+p(i+1,1)))^2;%    pdd(i) = (p(i+1,2)-2*p(i,2) + p(i-1,2))/(-p(i,1)+p(i+1,1))^2;endfor i  = 1:length(p)k(i) = (pdd(i))/((1+pd(i)^2)^(1.5));end
end

针对Hybrid A*论文解析(5)中的方法的一些验证相关推荐

Hybrid A*论文解析(4)
本文解析Autonomous driving in semi-structured environments: Mapping and planning,这篇文章其实是Hybrid A*论文解析(1) ...
Hybrid A*论文解析(3)
本文解析论文Autonomous Parking using Optimization-Based Collision Avoidance.文章来自uc berkley,这个方案也是非常主流的混合A* ...
Hybrid A*论文解析(2)
本文解析Application of Hybrid A* to an Autonomous Mobile Robot for Path Planning in Unstructured Outdoor ...
解析java中的方法重写
解析java中的方法重写 1 含义子类继承父类后,可以在子类中书写一个与父类同名同参的方法,从而实现对父类中同名同参数的方法的覆盖,我们把这一过程叫做方法的重写(override) 2 为什么要使用 ...
Hybrid A*论文解析(5)
本文解析论文Enhanced path smoothing based on conjugate gradient descent for firefighting robots in petroch ...
Hybrid A*论文解析(1)
本文是对Practical Search Techniques in Path Planning for Autonomous Driving的解析. 本文使用混合A方案结合共轭梯度法解决停车场泊车, ...
无法解析 list 中的方法 iterator_Python-list中的append()和extend()方法区别
一.append()和extend()方法都是用来添加数据到list末尾的,两者的区别: append()添加的时候会把添加的数据当成一个整体进行添加,允许添加任意类型的数据 extend()添加的时 ...
面部表情视频中进行远程心率测量：ICCV2019论文解析
面部表情视频中进行远程心率测量:ICCV2019论文解析 Remote Heart Rate Measurement from Highly Compressed Facial Videos: an ...
分层条件关系网络在视频问答VideoQA中的应用：CVPR2020论文解析
分层条件关系网络在视频问答VideoQA中的应用:CVPR2020论文解析 Hierarchical Conditional Relation Networks for Video Question ...

针对Hybrid A*论文解析(5)中的方法的一些验证

针对Hybrid A*论文解析(5)中的方法的一些验证相关推荐

最新文章

热门文章