フィルターのクリア

How to find heading angle between two coordinates

51 ビュー (過去 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 Center および File ExchangeMarine 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