DJI RS 2稳定器:如何通过关节角计算姿态角
DJI Ronin RS 2 稳定器姿态角计算问题
我正在研究DJI Ronin RS 2稳定器,稳定器上搭载一台相机,红色箭头标记了稳定器的轴,关节角alpha、beta、gamma已测得且已知,绿色坐标系为固定参考系。
核心问题
如何结合alpha、beta、gamma这三个关节角,计算出参考系下的偏航角(绕绿色Z轴)、滚转角(绕绿色X轴)和俯仰角(绕绿色Z轴),以确定相机的指向?
我知道可以通过发送获取指令直接从稳定器获取姿态信息,但因为同时需要关节角和姿态角,所以认为通过关节角计算姿态角会更便捷。
已尝试的实现
我已构建绕X、Y、Z轴的基础旋转矩阵,并尝试通过以下代码计算姿态,但未得到正确结果。猜测这些公式仅适用于特定顺序的X、Y、Z轴旋转,而该稳定器因中间关节的存在采用了不同的旋转轴。
import numpy as np def Rx(a): # 转换为弧度 a = np.radians(a) Rx = [[1, 0, 0], [0, np.cos(a), np.sin(a)], [0, -np.sin(a), np.cos(a)]] Rx = np.round(Rx, decimals = 3) return Rx def Ry(a): # 转换为弧度 a = np.radians(a) Ry = [[np.cos(a), 0, -np.sin(a)], [0, 1, 0], [np.sin(a), 0, np.cos(a)]] Ry = np.round(Ry, decimals = 3) return Ry def Rz(a): # 转换为弧度 a = np.radians(a) Rz = [[np.cos(a), np.sin(a), 0], [-np.sin(a), np.cos(a), 0], [0, 0, 1]] Rz = np.round(Rz, decimals = 3) return Rz # 绕z轴旋转 R10 = Rz(alpha) # 绕y轴旋转 R21 = Ry(55) # 绕x轴旋转 R32 = Rx(beta) # 绕y轴旋转 R43 = Ry(gamma) R20 = np.matmul(R21, R10) R30 = np.matmul(R32, R20) R = np.matmul(R43, R30) pitch = -np.arcsin(R[2,0]) roll = np.arctan2(R[2,1]/np.cos(pitch), R[2,2]/np.cos(pitch)) yaw = np.arctan2(R[1,0]/np.cos(pitch), R[0,0]/np.cos(pitch))
内容的提问来源于stack exchange,提问作者Timm
相关产品推荐
相关产品推荐

