Finding the relative orientation to a robot

7 views (last 30 days)
Hello! I am writing a pure pursuit algorithm and one of the caveats to pure pursuit in general is that it only really works well if the point is infront of the robot. I am wondering if there is a way to find the relative point orientation with relation to the robots position? What I mean by this is, say I have a point that is behind the robot, I know that it is going to have to make a point-turn then go to that point, now the next point is now behind that old point so the robot will have to make another point-turn. My question, is there some kind of algorithm to always be able to determine that point? For this assignment, a flag is used to determine if the robot will calculate angular velocity, have to point turn left or point turn right (values 0,1,-1). The robot starts by pointing east at the origin, I know that I could do this using just quadrant math and the actual values of points. The problem with logic that I am having is that once it turns to the left and goes to a point in the top left quadrant, that algoritm will no longer work because now a point that used to be infront of the robot, is behind it.
Please let me know if you need clarification! Thank you guys so much.
Pawel Grum
Pawel Grum on 20 Sep 2022
Edited: Pawel Grum on 20 Sep 2022
Very similar to this, you know you are in Miami and need to get to New York but you do not know which direction New York is in relation to you. The idea is really clear in the phase 3 I showed. The robot is facing west, it knows where it is and the coordinates of the next point but it does not know the direction of that point. I know that the point is behind it so I know that it needs to make a point turn to either the right or left until the point is infront of it. I dont know how to code it so that the robot knows the direction of the point in relation to its current coordinate.
This is kinda hard to follow and I apologize about that. To try and make it more clear, if the robot was at the origin, and I knew the point was in the Northwest quadrant, I could use the coordinates of that point to determine that quadrant (using if statements) and then having the robot do a point turn to the left and going to that point. The problem is, when it finally gets to that point (phase 3) and it goes through the quadrant cipher again, the orignal code I would have written would tell it not to point turn if the point was in the Northeast quadrant; this is because the original position of the robot meant that a point in the NE quadrant would be infront of it. I am trying to find an algorithm or mathematical solution that I could turn into an algorithm that would be able to always determine if the next point is behind or infront of the robot.

Sign in to comment.

Accepted Answer

Sam Chak
Sam Chak on 20 Sep 2022
Generally, you can the transformation matrix to obtain the relative orientation. For simplicity, you can try studying the following and code them in your algorithm. You need to translate the "Direction" and "Rotation" into 2-wheel Robot's Differential Velocities.
Origin = [0 0]; % [X, Y]
Attitude0 = 0; % orientation facing north
% Heading to Waypoint 1 from Origin
WayPoint1 = [-1 1];
Robot_Pos = Origin;
Direction = WayPoint1 - Robot_Pos % Go 1 unit to the West and 1 unit to the North
Direction = 1×2
-1 1
Rotation = atan(Direction(1)/Direction(2))*180/pi - Attitude0 % tells robot to rotate Left
Rotation = -45
Attitude1 = Rotation % orientation facing northwest
Attitude1 = -45
% Heading to Waypoint 2 from Waypoint 1
WayPoint2 = [2 2];
Robot_Pos = WayPoint1;
Direction = WayPoint2 - Robot_Pos
Direction = 1×2
3 1
Rotation = atan(Direction(1)/Direction(2))*180/pi - Attitude1 % tells robot to rotate Right
Rotation = 116.5651
Attitude2 = Rotation + Attitude1 % orientation facing northeast
Attitude2 = 71.5651
% Heading to Waypoint 3 from Waypoint 2
WayPoint3 = [2 -2];
Robot_Pos = WayPoint2;
Direction = WayPoint3 - Robot_Pos % use If-else logic when it happens
Direction = 1×2
0 -4
Rotation = 180 - Attitude2 % tells robot to rotate Right
Rotation = 108.4349
Attitude3 = Rotation + Attitude2 % orientation facing south
Attitude3 = 180
% Heading to Waypoint 4 from Waypoint 3
WayPoint4 = [4 -1];
Robot_Pos = WayPoint3;
Direction = WayPoint4 - Robot_Pos
Direction = 1×2
2 1
Rotation = atan(Direction(1)/Direction(2))*180/pi - Attitude3 % tells robot to rotate Left
Rotation = -116.5651
Attitude4 = Rotation + Attitude3 % orientation facing northeast
Attitude4 = 63.4349

More Answers (0)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!