二自由度舵机云台球面角度、坐标转换笔记

二自由度舵机云台分别由两个舵机组成，通过两个舵机的协调，可以将最上方的云台转到任意目标位置。

在转动过程中到底转了多少度、坐标是多少，都是需要实时可控的，因此要求写一套简单的算法求出当前云台的坐标。

设 \(\alpha\) 为向量与 XOY 平面夹角，\(\beta\) 为向量在 XOY 平面的投影与 OX 轴夹角。

所以云台三个轴的坐标值可通过如下公式求得：

• \(x = r * \cos\alpha * \cos\beta\)
• \(y = r * \cos\alpha * \sin\beta\)
• \(z = r * \sin\alpha\)

范围：\(\dfrac{-\pi}{2}<=\alpha<=\dfrac{\pi}{2}\)，\(0<=\beta<=2\pi\)

所以按照上述公式计算 \(\left(x,y,z\right)\) 坐标时，只要求出 \(\alpha\) 角和 \(\beta\) 角即可。

在写程序前，需要一个能够计算 cos 和 sin 的函数如下：

const double kServoPi = 3.14159265; // π

// sin(x) 函数
double Servo_Sin(double rad) {
    int8 _flag = 1;
    double sine;

    if (rad >= kServoPi) {
        rad -= kServoPi;
        _flag = -1;
    }

    if (rad < 0)
        sine = rad * (1.27323954f + 0.405284735f * rad);
    else
        sine = rad * (1.27323954f - 0.405284735f * rad);
    if (sine < 0)
        sine = sine * (-0.225f * (sine + 1) + 1);
    else
        sine = sine * (0.225f * (sine - 1) + 1);

    return sine * _flag;
}

// cos(x) 函数
float Servo_Cos(double rad) {
    int8 _flag = 1;
    rad += kServoPi / 2.0;

    if (rad >= kServoPi) {
        _flag = -1;
        rad -= kServoPi;
    }

    return Servo_Sin(rad) * _flag;
}

cos 和 sin 的函数准备好后，那么就可以开始计算舵机角度了。

该套算法通过获取当前舵机的占空比值与已确定的舵机最小、最大占空比值的差值进行相应计算，转换为角度，而后再由角度计算得出云台三轴坐标值。因此程序本身并逻辑不难。

struct PanTilt {
    float coordinate_x;  // 云台 x 轴坐标
    float coordinate_y;  // 云台 y 轴坐标
    float coordinate_z;  // 云台 z 轴坐标
};

struct PanTilt pan_tilt = {0, 0, 0};

// 舵机角度坐标计算。输入变量 _pwm1 为上面舵机的占空比，_pwm2 下面舵机的占空比
void Servo_Coordinates_Calculate(uint16 _pwm1, uint16 _pwm2) {

    // 上面的舵机转到最上侧的占空比，转到正中间的占空比，转到最下侧的占空比
    const float up_servo_high_pwm = 1105, up_servo_mid_pwm = 672.5, up_servo_low_pwm = 240;

    // 下面的舵机转到最左侧的占空比，转到最右侧的占空比
    const float dw_servo_left_pwm = 1105, dw_servo_right_pwm = 234;

    // 云台旋转半径为 51mm
    const float r = 51;

    // 定义云台的 α 角和 β 角静态变量
    static float _angle_alpha = 0, _angle_beta = 0;

    // 计算云台的 α 角和 β 角
    _angle_alpha = kServoPi * ((float)(_pwm1)-up_servo_mid_pwm) / (up_servo_high_pwm - up_servo_low_pwm);
    _angle_beta = kServoPi * ((float)(_pwm2)-dw_servo_right_pwm) / (dw_servo_left_pwm - dw_servo_right_pwm);

    // 计算云台的 x, y, z 三轴坐标值
    pan_tilt.coordinate_x = r * Servo_Cos(_angle_alpha) * Servo_Cos(_angle_beta);
    pan_tilt.coordinate_y = r * Servo_Cos(_angle_alpha) * Servo_Sin(_angle_beta);
    pan_tilt.coordinate_z = r * Servo_Sin(_angle_alpha);
}

因此，云台的角度以及 x, y, z 三轴坐标值均可以算出，需要使用数据的时候，直接调用结果即可。如上程序还有很多需要优化的地方，仅供参考。