坐标系之间存在旋转,根据偏航角、俯仰角、翻滚角三个角度计算重力补偿角度的公式
时间: 2023-08-11 19:04:40 浏览: 144
我在上一个回答中已经给出了根据偏航角、俯仰角、翻滚角计算重力补偿角度的公式,但是这个公式是针对机体坐标系中的重力补偿角度的计算,如果需要将重力补偿角度转换到地球坐标系中,则需要进行坐标系转换。具体方法如下:
1. 将机体坐标系转换到地球坐标系中,即:
$Acc_x^{earth} = Acc_x^{body}cos(pitch)cos(yaw) + Acc_y^{body}(cos(yaw)sin(pitch)sin(roll) - cos(roll)sin(yaw)) + Acc_z^{body}(cos(roll)sin(pitch)sin(yaw) + cos(yaw)sin(roll))$
$Acc_y^{earth} = Acc_x^{body}cos(pitch)sin(yaw) + Acc_y^{body}(sin(yaw)sin(pitch)sin(roll) + cos(yaw)cos(roll)) + Acc_z^{body}(cos(yaw)sin(pitch)sin(roll) - cos(roll)sin(yaw))$
$Acc_z^{earth} = -Acc_x^{body}sin(pitch) + Acc_y^{body}cos(pitch)sin(roll) + Acc_z^{body}cos(pitch)cos(roll)$
其中,$Acc_x^{body}$、$Acc_y^{body}$、$Acc_z^{body}$分别为加速度计测得的三轴加速度值在机体坐标系中的分量,$pitch$、$roll$、$yaw$分别为俯仰角、翻滚角、偏航角,$Acc_x^{earth}$、$Acc_y^{earth}$、$Acc_z^{earth}$分别为加速度计测得的三轴加速度值在地球坐标系中的分量。
2. 根据地球坐标系中的加速度值$Acc_x^{earth}$、$Acc_y^{earth}$、$Acc_z^{earth}$计算重力补偿角度,即:
$\theta_{comp} = atan2(-Acc_y^{earth}, \sqrt{(Acc_x^{earth})^2 + (Acc_z^{earth})^2})$
这个公式可以计算出地球坐标系中的重力补偿角度。
阅读全文