简体   繁体   English

计算目标滚转、俯仰和偏航角给定初始和目标位置以及初始滚转、俯仰和偏航角

[英]Compute target roll, pitch and yaw angles given initial and target positions and intial roll, pitch and yaw angles

I have a model that moves in simulation world.我有一个在模拟世界中移动的模型。 The frame of operation is right handed with X - forward, Y - left and Z - Up.操作框架是右手,X - 向前,Y - 左和 Z - 向上。

I know the current position, P1 (x1, y1, z1) and current RPY angles, (R1, P1, Y1) .我知道当前位置P1 (x1, y1, z1)和当前 RPY 角度(R1, P1, Y1) I have a goal position, P2 (x2, y2, z2) .我有一个目标位置P2 (x2, y2, z2) I would like to move my model from P1 to P2 .我想将我的模型从P1移动到P2 I am stuck at computing the goal RPY angles.我一直在计算目标 RPY 角度。 I would like my model to turn around first in the direction from P1 to P2 and move in that direction to reach goal.我希望我的模型首先转向从 P1 到 P2 的方向,然后朝那个方向移动以达到目标。

How do I calculate the target RPY angles, (R2, P2, Y2) , given the above information ?鉴于上述信息,我如何计算目标 RPY 角度(R2, P2, Y2)

  • Calculate the direction vector D = P2 - P1 = (x2 - x1, y2 - y1, z2 - z1)计算方向向量D = P2 - P1 = (x2 - x1, y2 - y1, z2 - z1)

  • Pitch P2 = asin(Dz / |D|)音高P2 = asin(Dz / |D|)

  • Yaw Y2 = atan2(Dy, Dx)偏航Y2 = atan2(Dy, Dx)

(NB can use P2 = atan2(Dz, sqrt(Dx^2 + Dy^2)) instead for more robustness near the spherical poles) (NB 可以使用P2 = atan2(Dz, sqrt(Dx^2 + Dy^2))来代替球极附近的更多鲁棒性)

声明:本站的技术帖子网页,遵循CC BY-SA 4.0协议,如果您需要转载,请注明本站网址或者原文地址。任何问题请咨询:yoyou2525@163.com.

 
粤ICP备18138465号  © 2020-2024 STACKOOM.COM