如何使用额外的轴防止万向节锁定并计算该轴的位置？

Question

我有一个类似theodolite的设备，它试图跟踪某个点，以使其“前进”始终面对该点。

这是GIF到目前为止的工作。

如预期的那样，当“红角”的值接近90或270时，可能会接近万向节锁定的情况。

如何为当前未使用的蓝色轴计算位置，以防止红色/绿色轴以这种方式对齐？

主循环：

    public void Main(string argument, UpdateType updateSource)
    {
        float pitch, yaw;
        GetOrientationTo(target, origin, out pitch, out yaw);

        UpdateSteeringVectors();
        // Update yaw to the Green rotor
        UpdateRotor(r_green_inner, yaw);
        // Update pitch to the Red rotor
        UpdateRotor(r_red_middle, pitch);

        float green = GetRotorAngle(r_green_inner);
        if (green == 0) green = 360;
        float red = GetRotorAngle(r_red_middle);
        if (red == 0) red = 360;
        float blue = GetRotorAngle(r_blue_outer);
        if (blue == 0) blue = 360;

        debugText = "Green Angle: " + green.ToString() + "\n";
        debugText += "Red Angle: " + red.ToString() + "\n";
        debugText += "Blue Angle: " + blue.ToString() + "\n";
        debug.WritePublicText(debugText);
        debugText = "";
    }

跟踪功能：

    void GetOrientationTo(Vector3 Target, IMyTerminalBlock Origin, out float Pitch, out float Yaw)
    {

        // Origin point
        Vector3 OV = Origin.GetPosition();
        // Thrusters are two blocks long so we need to make it shorter
        OV = Vector3.Subtract(OV, Origin.WorldMatrix.Forward * 1.25);
        // Target point
        Vector3 TV = Vector3.Subtract(OV, Target);
        // Get reference directional vectors
        Vector3 FV = OV + Origin.WorldMatrix.Forward;
        Vector3 UV = OV + Origin.WorldMatrix.Up;
        Vector3 RV = OV + Origin.WorldMatrix.Right;

        double TVOV = (OV - TV).Length();  // Get magnitudes of vectors

        double TVFV = (FV - TV).Length();
        double TVUV = (UV - TV).Length();
        double TVRV = (RV - TV).Length();

        double OVFV = (FV - OV).Length();
        double OVUV = (UV - OV).Length();
        double OVRV = (RV - OV).Length();

        double ThetaP = Math.Acos((TVUV * TVUV - OVUV * OVUV - TVOV * TVOV) / (-2 * OVUV * TVOV));  //Use law of cosines to determine angles.
        double ThetaY = Math.Acos((TVRV * TVRV - OVRV * OVRV - TVOV * TVOV) / (-2 * OVRV * TVOV));

        double RPitch = 90 - (ThetaP * 180 / Math.PI);  //Convert from radians to degrees.
        double RYaw = 90 - (ThetaY * 180 / Math.PI);

        if (TVOV < TVFV) RPitch = 180 - RPitch;  //Normalize angles to -180 to 180 degrees.
        if (RPitch > 180) RPitch = -1 * (360 - RPitch);

        if (TVOV < TVFV) RYaw = 180 - RYaw;
        if (RYaw > 180) RYaw = -1 * (360 - RYaw);

        Pitch = (float)RPitch;
        Yaw = (float)RYaw;
    }