局部路径规划算法—人工势场法（学习笔记）

应用：机器人避障，汽车的避障路径规划

被控对象在这两种势场组成的复合场中受到斥力作用和 引力作用，斥力和引力的合力指引着被控对象的运动， 搜索无碰的避障路径。

引力势场

斥力势场：分别对x,y求偏导

目标不可达的问题

由于障碍物与目标点距离太近，当汽车到达目标点时，根据势场函数可知，目标点的引力降为零， 而障碍物的斥力不为零，此时汽车虽到达目标点，但在斥力场的作用下不能停下来，从而导致目标不可达的问题。

陷入局部最优的问题

车辆在某个位置时，如果若干个障碍物的合斥力与目标点的引力大小相等、方向相反，则合力为0，这将导致车辆不再“受力”，故无法向前搜索避障路径

改进一：通过改进障碍物斥力势场函数来解决局部最优和目标不可达

障碍物对车辆产生两个力：（1）障碍物——车辆

                                           （2）力由障碍物产生，但方向是由车辆——目标点（避免局部最优）

改进二：

避免到达目标点来回震荡

% 在原斥力势场函数增加目标调节因子（即车辆至目标距离），以使车辆到达目标点后斥力也为0
    for j = 1:n-1
        if dist(j,1) >= d0
            F_rep_ob(j,:) = [0,0];
        else

%% 计算合力和方向
    F_rep = [sum(F_rep_ob(:,1))  + F_rep_edge(1,1),...
           sum(F_rep_ob(:,2)) + F_rep_edge(1,2)];                                      % 所有障碍物的合斥力矢量
    F_att = [Eta_att*dist(n,1)*unitVector(n,1), Eta_att*dist(n,1)*unitVector(n,2)];    % 引力矢量
    F_sum = [F_rep(1,1)+F_att(1,1),F_rep(1,2)+F_att(1,2)];                             % 总合力矢量
    UnitVec_Fsum(i,:) = 1/norm(F_sum) * F_sum;                                         % 总合力的单位向量
    
    %计算车的下一步位置
    Pi(1,1:2)=Pi(1,1:2)+len_step*UnitVec_Fsum(i,:);                     

%     %判断是否到达终点
%     if sqrt((Pi(1)-P(n,1))^2+(Pi(2)-P(n,2))^2) < 0.2 
%         break
%     end
end
Path(i,:)=P(n,:);            %把路径向量的最后一个点赋值为目标

还有待改进的地方：某些目标点的运动轨迹的曲率是否符合运动学要求等