Wondering about Pure Pursuit Algorithm Code

I have read many documents about this algorithm and understand how it works.

However, it is hard to use for my report to my lecturer if there is nothing to sure that this block works as it is described in the desciption. Can someone help me find out the code or some way to prove that?

Thank you so much, guys.

[Tran]

I have did a check on this pure pursuit block, with initial position [x y yaw] is [0 0 0] and the postion for 2 points that i created is (1,1) and (2, 0.5).

Then I run the model to check the target direction of PP block, the result is 16.27 degree. I have checked atan2 of that 2 point which gave me 45deg for (1,1) and 14.04deg for ( 2, 0.5). At this time i set lookahead distance is 1m so no point will be caught right? For the result is 16.27, so I thought that the point matlab was chasing is the interpolated point between 2 points.

After that, I create a check line (pink one) try to know what is that block function chasing. But i cannot explain what is happenning.

So I change the ld to 1.5m, the Target direction change to 14.04 deg which is the second point. But if as describtion is chasing the interpolated point, the result shoule be the point (1.2, 0.9) on the figure 4.

Even if i change the initial yaw to pi/2 the result will be -75.96 deg, which is still the (2, 0.5).

Can someone help me with this.

Thank you for helping me.

Answer 1

[Walter Roberson]

edit controllerPurePursuit

The source code is provided.

[Tran]

Thank you so much, Sir.

Have a good day.

Guys just need to type 'edit controllerPurePursuit' in Command Window to see the code.