[path planning] how to add moving objects to a path

Preface

In the previous path planning article , Use multi segment fifth order Bezier curve to optimize RRT Path after search , Although the video looks good , But it's not fun enough , To make it interesting , Suddenly want to add a moving object ( For example, a triangle ), After generating a smooth curve , Move along a smooth curve , This is the goal of this issue , After implementation , Later, we can consider adding some motion models to make this moving object conform to some moving objects in life .

Draw a triangle

First, we use a triangle to represent a moving object , Need to find a way to draw triangles , This needs help from matlab Of  fill  function , This function works roughly like this ： Give the horizontal and vertical coordinates of several points , And then use fill Function can fill the closed graph enclosed by these coordinate points with color , And you can construct an object .

The following is a sample program I give ：

The effect of its implementation is like this ：

The yellow triangle in the upper left corner of the figure above is the effect of this code . Here is a function that I made to generate the coordinates of three points of a triangle Triangle , except fill, This is the function you need to understand , Here we can introduce the basic principle of generating coordinates , The code of this self compiled function is as follows ：

%*************************************************************************%
                %pos： Coordinates of the center of the triangle bottom 
                %yaw： The triangle points to the point 
                %size： Triangle Size 
                
                %      (1)
                %       *
                %      * *
                %     *   *
                %    * * * *
                %  (2)     (3)
                %x1,x2,x3： Of the three vertices of the resulting triangle x coordinate 
                %y1,y2.y3:  Of the three vertices of the resulting triangle y coordinate 
%**********************************************************************%
function [x,y] = Triangle(pos, yaw, size)
% pos  Define the coordinates of the center of the triangle bottom 
% vec  Define the triangle direction 
% dis  Define Triangle Size 
pos_yaw = [yaw(1) - pos(1), yaw(2) - pos(2)];  % Find the center line vector 
pos_yaw_I = pos_yaw / sqrt(pos_yaw(1) * pos_yaw(1) + pos_yaw(2) * pos_yaw(2)); % Find the unit vector 

P1 = pos + pos_yaw_I * 3 * size;   % seek 1 Point coordinates 

% seek 2 Point coordinates 
P2_P3 = [1,-pos_yaw_I(1) / pos_yaw_I(2)];  % Find perpendicular to pos_yaw_I Vector 
P2_I = P2_P3 / sqrt(P2_P3(1) * P2_P3(1) + P2_P3(2) * P2_P3(2));  % Find the unit vector 
P3 = pos + P2_I * size;   % seek P2 The coordinates of point 
% seek 3 Point coordinates 
P3_P2 = [-1,pos_yaw_I(1) / pos_yaw_I(2)];  % Find perpendicular to pos_yaw_I Vector 
P3_I = P3_P2 / sqrt(P3_P2(1) * P3_P2(1) + P3_P2(2) * P3_P2(2));  % Find the unit vector 
P2 = pos + P3_I * size;   % seek P3 The coordinates of point 

x = [P1(1),P2(1),P3(1)];
y = [P1(2),P2(2),P3(2)];

At first I wanted to use trigonometric function to solve it , Add to the parameters I passed in pos It represents the midpoint of the base of the triangle , Finally, we want to generate an isosceles triangle , The distance between the midpoint and the vertices on both sides is size,pos The distance from another vertex is 3*size , The center line of this triangle points in the direction of pos - yaw,yaw Point is the coordinate point to point to , The schematic diagram is as follows ：

If you use trigonometric functions directly , You can also find out , But when the bottom edge of this triangle is X When the axes are not parallel , It would be particularly troublesome to beg , So I use the vector method to find the coordinates of ：

We can do this by finding the vector pos_yaw , Then you can find pos_yaw Unit vector pos_yaw_I, And then you can go through pos The coordinates of plus the unit vector times 3*size The length is obtained P1 The coordinates of point , The calculation process is relatively simple , Then we can solve P2,P3 coordinate , because P2,P3 And vector pos_P1 vertical , Therefore, the solution process needs to bypass , However, it is relatively simple .

We know that the vector points perpendicular to each other are multiplied by 0, For example, vector a And vector b vertical , that a.x * b.x + a.y * b.y = 0 establish , We can find out according to this law P2 And P3 coordinate ：

Animation

After we get the method of calculating the triangle coordinates , Is to make animation , The optimization path points generated by Bessel curve in the previous issue , We can save them all , Then let the triangle move . Do you still remember that it was said once ？ We are using fill Function time , You can get an object , This object can be used  set  Function to reset , That is, you can erase only the closed object , Then draw again , It is equivalent to moving , The code is as follows ：

pos = [route(route_count,1),route(route_count,2)];
yaw = [route(route_count-1,1),route(route_count-1,2)];
size = 8;
[x,y] = Triangle(pos, yaw, size);
triangle1 = fill(x,y,'y');   % Define a triangle object triangle1

for i = route_count:-1: 2
    pos = [route(i,1),route(i,2)];   % Read path points 
    yaw = [route(i-1,1),route(i-1,2)];  % Read the next path point 
    size = 8;
    [x,y] = Triangle(pos, pos + [1,1], size);  % Get the triangle coordinates 
    set(triangle1,'XData',x,'YData',y); %% Reset drawing object 
    hold on;
    pause(0.01); % Pause 0.1s, bring RRT The expansion process is easy to observe     
end

The result is like this ：

Add simple moving objects to the planning path

Is it starting to be fun