Path Following Algorithm Line-Circle Path Path Switching Problem

Hi everyone ,

I'm trying to implement a path following algorithm for UAV (Carrot Chase) with matlab for a line-circle path.

I have already implemented the algorithm for straight line and the loiter separately and this codes are work fine. But, since they are geometrically different shapes, they have different algorithms. So we need to switch algorithms when switching between these two shapes if we want to follow this two shapes together . And i added a algorithm for that but i think it's not working

i have a variable for that named as " guidance index". You can see this variable at the begginning of the code. When i enter this variable 1 the UAV will follow the straight line path firstly. But if i enter this variable 2, the UAV will follow the circle firstly.

You can see this in the files I attached. But the real problem starts here. After the straight line path ends, it does not follow the circular path but goes out of the path. Likewise, when I set the guidance_index variable to 2, it follows the circle path correctly, but this time it does not continue with the straight line path.

How can i fix it ?

Thanks a lot.

Best Regards

[Divyanshu]

Hi @Muhammed Emin Yavuzaslan,

In the .m code file the functions 'extend_segment_start' & 'extend_segment_end' are called. Can you share the definition of these functions?

[Muhammed Emin Yavuzaslan]

Hi @Divyanshu

I'm really sorry about that. I'm using these files for plotting the path. I added these files too.

Best Regards

Answer 1

[Divyanshu]

Hi @Muhammed Emin Yavuzaslan,

A possible reason of the unexpected behavior of the plot in both the case where 'guidance_index' is 1 or 2 can be the following code-snippet where values of 'xt' and 'yt' are calculated incorrectly:

%For guidance_index 1
Ru = sqrt( (XM0-ugv_x)^2 + (YM0 - ugv_y)^2);
theta = atan2(YT0 - YM0, XT0 - XM0);
theta_u = atan2((ugv_y-YM0),(ugv_x - XM0));
beta  = theta-theta_u;
R = sqrt(Ru^2 - ((Ru*sin(beta))^2));
xt = XM0 + (R+delta)*cos(theta);
yt = YM0+ (R+delta)*sin(theta);

% For guidance_index 2
xt = center_x + r*tan(delta_theta)*cos(theta+alpha);
yt = center_y + r*tan(delta_theta)*sin(theta+alpha);

So you need to re-look at the above code-snippets corresponding to if blocks of 'guidance_index' 1 and 2 respectively and may need to modify the calculation of 'xt' & 'yt' accordingly.

Hope it helps!

[Muhammed Emin Yavuzaslan]

Hi @Divyanshu

As I said before, I implemented this algorithm separately as Straight Path and Loiter Path and they work properly. So there is no problem in calculating xt and yt because I used the same equations in the code I wrote before and it works fine in that code.

Also, if the xt and yt calculations were wrong, they would not follow any path anyway. For example, when I set the guidance_index variable to 2, it follows the loiter path very accurately. This is proof that there is no problem in the calculations and the related code part. I think there is a problem with the guidance_index switching sequence.

Thank a lot

[Divyanshu]

Sure, I understand the algorithms are working separately for both the paths but when merged they are causing the problem.

I have found the root-cause of the problem that lets say when 'guidance_index' is initially '1' then when 'guidance_index' is modified inside 'if sqrt()' blocks the path still continues to be straight and does not circle around.

The reason is not the switching sequence of 'guidance_index' its rather the scope of 'guidance_index'.

Since 'guidance_index' is passed as an argument whatever value is changed and assigned to this argument inside the function 'carrotchase_ode' it doesn't affect the value declared at the starting of script.

Hence the argument passed locally to next call to 'carrotchase_ode' remains unaffected.

As a workaround, I modified the script and made it global and now when initially 'guidance_index' is 1, it follows the expected path. I am attaching the modified script file you can refer it.

Hope it helps!

[Muhammed Emin Yavuzaslan]

@Divyanshu

Hi again Sir,

Thank you for your efforts. I'm appriacted. But we have another problem now..

I have ran the code you shared in attachment. After following the first straight path and following the first loiter path, it should have moved on to the second straight path, but it follows the first loiter path forever.

How can we fix it ?

Thanks a lot sir. I'm really appriacted.

Best Regards