How to find heading angle between two coordinates
조회 수: 48 (최근 30일)
이전 댓글 표시
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-
댓글 수: 0
채택된 답변
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))
% 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
추가 답변 (0개)
참고 항목
카테고리
Help Center 및 File Exchange에서 Marine and Underwater Vehicles에 대해 자세히 알아보기
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!