How to find heading angle between two coordinates

56 次查看(过去 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-

采纳的回答

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

更多回答(0 个)

类别

Help CenterFile Exchange 中查找有关 ROS Toolbox 的更多信息

产品


版本

R2022b

Community Treasure Hunt

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

Start Hunting!

Translated by