How to find heading angle between two coordinates

53 views (last 30 days)
Hi there, I have a random generate point with heading angle (e.g. A = [X, Y, theta]) for my mobile robot and a fixed point, B = [4,5], and I wanted to calculate what is the angle (theta in radian) should my mobile robot tilt to point toward point B, which formula or method can I use?
A = [1+10*(rand(2,1)); pi*rand(1,1)]; % random generate point A (with theta)
B = [4,5]; % point B
-Chann-

Accepted Answer

Karim
Karim on 12 Jan 2023
see below for one approach
% random generate point position and direction of the robot
currPos = 1+10*(rand(1,2)); % the x-y coordinates
currAng = pi*rand(1,1); % the current heading
current_direction = [cos(currAng) sin(currAng) 0]; % create the vector for the current direction
current_direction = current_direction ./ sqrt( sum( current_direction.^2 )); % normalize the vector
% target point of the robot
targetPos = [4,5];
% now determine the angle between the current heading and the target position
target_direction = [targetPos - currPos 0];
target_direction = target_direction ./ sqrt( sum( target_direction.^2 )); % normalize the vector
% finaly determine the angle between the two directions
delta_theta = atan2(norm(cross(target_direction,current_direction)), dot(target_direction,current_direction))
delta_theta = 1.9253
% visualize the state
figure
hold on
scatter(targetPos(1),targetPos(2),100,'r','filled')
scatter(currPos(1),currPos(2),100,'g','filled')
quiver(currPos(1),currPos(2),current_direction(1),current_direction(2),'Color','k','LineWidth',1.5,'MaxHeadSize',5)
quiver(currPos(1),currPos(2),target_direction(1),target_direction(2),'Color','r','LineWidth',1.5,'MaxHeadSize',5)
hold off
grid on
legend('target position','robot position','current heading','target heading','Location','best')
axis equal
  1 Comment
Shin
Shin on 12 Jan 2023
Hi karim, thanks for your reply. the code work as i wish, greatly appreciated!!

Sign in to comment.

More Answers (0)

Categories

Find more on ROS Toolbox in Help Center and File Exchange

Products


Release

R2022b

Community Treasure Hunt

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

Start Hunting!