三轴增稳云台：惯性传感器角速度到关节轴上角速度解耦公式

当惯性传感器轴与电机关节角轴不重合时，可以通过矩阵变换的方式将惯性传感器的角速度解耦到关节轴上的角速度。\n假设惯性传感器测量到的角速度为ω_sensor = [ωx_sensor, ωy_sensor, ωz_sensor]，电机关节轴的角速度为ω_joint = [ωx_joint, ωy_joint, ωz_joint]。\n假设惯性传感器轴与电机关节角轴之间的旋转矩阵为R，那么有：\n[ωx_joint] [ωx_sensor]\n[ωy_joint] = R * [ωy_sensor]\n[ωz_joint] [ωz_sensor]\n根据矩阵乘法的定义，可以得到：\nωx_joint = R[1,1] * ωx_sensor + R[1,2] * ωy_sensor + R[1,3] * ωz_sensor\nωy_joint = R[2,1] * ωx_sensor + R[2,2] * ωy_sensor + R[2,3] * ωz_sensor\nωz_joint = R[3,1] * ωx_sensor + R[3,2] * ωy_sensor + R[3,3] * ωz_sensor\n其中R[1,1]、R[1,2]、R[1,3]等表示旋转矩阵R的元素。\n这样，通过这个公式，可以将惯性传感器测量到的角速度解耦到电机关节轴上的角速度。