How to find heading angle between two coordinates

조회 수: 46 (최근 30일)
Shin
Shin 2023년 1월 12일
댓글: Shin 2023년 1월 12일
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-

채택된 답변

Karim
Karim 2023년 1월 12일
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
Shin
Shin 2023년 1월 12일
Hi karim, thanks for your reply. the code work as i wish, greatly appreciated!!

댓글을 달려면 로그인하십시오.

추가 답변 (0개)

카테고리

Help CenterFile Exchange에서 Marine and Underwater Vehicles에 대해 자세히 알아보기

제품


릴리스

R2022b

Community Treasure Hunt

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

Start Hunting!

Translated by