坐标系之间存在旋转,根据偏航角、俯仰角、翻滚角计算重力补偿角度
时间: 2023-07-10 15:30:27 浏览: 294
如果坐标系之间存在旋转,则需要根据偏航角、俯仰角、翻滚角进行转换,才能得到正确的重力补偿角度。具体方法如下:
1. 将加速度计测得的三轴加速度值$Acc_x$、$Acc_y$、$Acc_z$转换到机体坐标系中,即:
$Acc_x^{body} = Acc_x^{earth}cos(pitch)sin(roll) + Acc_y^{earth}(cos(yaw)sin(pitch)sin(roll) - sin(yaw)cos(roll)) + Acc_z^{earth}(sin(yaw)sin(pitch)sin(roll) + cos(yaw)cos(roll))$
$Acc_y^{body} = Acc_x^{earth}cos(pitch)cos(roll) + Acc_y^{earth}(cos(yaw)sin(pitch)cos(roll) + sin(yaw)sin(roll)) + Acc_z^{earth}(sin(yaw)sin(pitch)cos(roll) - cos(yaw)sin(roll))$
$Acc_z^{body} = -Acc_x^{earth}sin(pitch) + Acc_y^{earth}cos(pitch)sin(yaw) + Acc_z^{earth}cos(pitch)cos(yaw)$
其中,$Acc_x^{earth}$、$Acc_y^{earth}$、$Acc_z^{earth}$分别为加速度计测得的三轴加速度值在地球坐标系中的分量,$pitch$、$roll$、$yaw$分别为俯仰角、翻滚角、偏航角。
2. 根据机体坐标系中的加速度值$Acc_x^{body}$、$Acc_y^{body}$、$Acc_z^{body}$计算重力补偿角度,即:
$\theta_{comp} = atan2(-Acc_y^{body}, \sqrt{(Acc_x^{body})^2 + (Acc_z^{body})^2})$
这个公式可以计算出机体坐标系中的重力补偿角度。如果需要将重力补偿角度转换到地球坐标系中,则需要对重力补偿角度进行坐标系转换。
阅读全文