1
$\begingroup$

My Code gives the following convergence characteristics, I wanted to know if it is correct Convergence for non-holonomic body 2000 iterations

Updated code

{

    %Basic RRT star algorithm for non-holonomic body with obstacles
    close all
    clc
   clear all
   %Map and Initialization Data
   x_max = 100;
y_max = 100;
obs1 = [30,0,20,20];
obs2=  [30,60,20,20];
EPS = 5; % Step Size
Iter = 2000;

q_start.pos = [10 10];
q_start.cost = 0;
q_start.parent = 0;
q_start.child=[];
q_goal.pos = [90,90];
q_goal.cost = 1e9;
q_goal.child=[];
q_new.pos =[0,0];
q_new.cost=0;
q_new.parent=0;
q_new.child=[];
goal_reached=0;
tree(1) = q_start;
figure(1)
axis([0 x_max 0 y_max])
rectangle('Position',obs1,'FaceColor','b')
rectangle('Position',obs2,'FaceColor','b')
hold on
plot(q_start.pos(1),q_start.pos(2),'.','MarkerSize',10,'Color','m')
plot(q_goal.pos(1),q_goal.pos(2),'.','MarkerSize',10,'Color','b')
goal_nodes=[];
best_goal=[];
finalnodes=[];
  goals=0;
fc=[];
t=cputime;
for i=1:1:Iter
    q_rand=random_state(x_max,y_max);%Sampling a random state from the configuration space
    [q_near,val,idx] = nearest_neighbour(q_rand,tree); % Obtaining the nearest neighbour from the tree and its distance from q_rand
     q_new.pos=move(q_rand,q_near.pos,val,EPS);

 %Checking if goal has been reached    
  stat= distance(q_rand,q_goal.pos);
        if (stat<=4 &&~isCollision(q_rand,q_near.pos,obs1)&&~isCollision(q_rand,q_rand,obs2))
            goal_reached = 1;
            goals=goals+1;
            goal_nodes(goals,:)=q_rand;
            q_new.pos=q_rand;
        end





   if(~isCollision(q_new.pos,q_near.pos,obs1)&&~isCollision(q_new.pos,q_near.pos,obs2))

   %line([q_near.pos(1), q_new.pos(1)], [q_near.pos(2), q_new.pos(2)], 'Color', 'k', 'LineWidth', 1);
        q_new.cost = distance(q_new.pos, q_near.pos) + q_near.cost;

    % Within a radius of r, find all existing nodes
        q_nearest = [];
        r = 10;
        neighbor_count = 0;
        ni=[];
        for j = 1:1:length(tree)
            if (noCollision(tree(j).pos,q_new.pos,obs1)&&noCollision(tree(j).pos,q_new.pos,obs2)&&distance(tree(j).pos, q_new.pos) <= r)
                neighbor_count = neighbor_count+1;
                q_nearest(neighbor_count).pos = tree(j).pos;
                q_nearest(neighbor_count).cost = tree(j).cost;
                ni=[ni,j];
            end
        end

        % Initialize cost to currently known value
        q_min = q_near;
        C_min = q_new.cost;

        % Iterate through all nearest neighbors to find alternate lower
        % cost paths

        for k = 1:1:length(q_nearest)
            if (q_nearest(k).cost + distance(q_nearest(k).pos, q_new.pos) < C_min)
                q_min = q_nearest(k);
                C_min = q_nearest(k).cost + distance(q_nearest(k).pos, q_new.pos);           

            end
        end

        % Update parent to least cost-from node
        for j = 1:1:length(tree)
            if tree(j).pos == q_min.pos  
                q_new.parent = j;
                q_new.cost = C_min;
                break
            end
        end


        % Add to tree
        tree = [tree,q_new];
        tree(q_new.parent).child=[tree(q_new.parent).child,length(tree)];

      %Rewire
      %Iterate through all nearest neighbors to rewire them with lower cost
      %path
        for k = 1:1:length(q_nearest)
            if ( q_new.cost + distance(q_nearest(k).pos, q_new.pos) < q_nearest(k).cost)
                tree(ni(k)).parent=length(tree);
                tree(ni(k)).cost= q_new.cost + distance(q_nearest(k).pos, q_new.pos);
                updatecost(tree,ni(k)); %Update cost of children

            end
        end   
   end
  best_cost=1e9; 
  best_goalindex=0;
    if(goal_reached)

for k=1:1:length(goal_nodes(:,2))
  for j = 1:1:length(tree)
      if(tree(j).pos(1)==goal_nodes(k,1)&&tree(j).pos(2)==goal_nodes(k,2)&&tree(j).cost<best_cost)
            best_goal=tree(j);
            best_cost=tree(j).cost;
            best_goalindex=j;

      end

  end    
end

% Search backwards from goal to start to find the optimal least cost path
q_goal.parent = best_goalindex;
q_end = q_goal;
finalcost=0;
tree = [tree q_goal];
while q_end.parent ~= 0
    start = q_end.parent;
    finalcost=finalcost+distance(q_end.pos,tree(start).pos);
    plot(q_end.pos(1),q_end.pos(2),'*','Color','r')
    line([q_end.pos(1), tree(start).pos(1)], [q_end.pos(2), tree(start).pos(2)], 'Color', 'r', 'LineWidth', 2);
    hold on
    q_end = tree(start);
end
   finalnodes=[finalnodes,i];
   fc=[fc,finalcost];
   drawnow
    end
end
e=cputime-t
figure(2)
plot(finalnodes,fc);
title('RRT* Convergence')
xlabel('Iteration')
ylabel('Path Cost')
}
$\endgroup$
1
$\begingroup$

No, at every iteration your RRT* should give a more optimized path once it has found it. The fluctuations are strange. You should try to get a convergence similar to this one.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.