Hexacopter Control Allocation

六旋翼 6X / 6H 混控计算

控制效率矩阵 · Moore–Penrose 伪逆 · 通道尺度 · 电机分配

1

6X

1.1 源代码与构型

static void MultiRotor6X_MotorControl(
    double output_throttle,
    double outRoll,
    double outPitch,
    double outYaw,
    bool DbgSafe)
{
    if (output_throttle < 0 || DbgSafe) {
        MainMotor_PullDownAll();
        update_output_throttle(0, 1.0 / CtrlRateHz);
        return;
    }

    double rp_out[6];
    double RollS =
        outRoll * 1.154700538379251529...; // 2/sqrt(3)
    double half_outRoll = 0.5 * RollS;

    rp_out[0] = -outPitch + half_outRoll;
    rp_out[1] = RollS;
    rp_out[2] = +outPitch + half_outRoll;
    rp_out[3] = +outPitch - half_outRoll;
    rp_out[4] = -RollS;
    rp_out[5] = -outPitch - half_outRoll;

    double yaw_out[6];
    yaw_out[0] = -outYaw;
    yaw_out[1] = +outYaw;
    yaw_out[2] = -outYaw;
    yaw_out[3] = +outYaw;
    yaw_out[4] = -outYaw;
    yaw_out[5] = +outYaw;

    MultiRotor_MotorControl(...);
}
+X Front +Y Right M0逆转 M1正转 M2逆转 M3正转 M4逆转 M5正转
6X 构型

1.2 坐标与符号

Motorx/Ry/Rsᵢ
M0+√3/2−1/2−1
M10−1+1
M2−√3/2−1/2−1
M3−√3/2+1/2+1
M40+1−1
M5+√3/2+1/2+1
aT = [ 1, 1, 1, 1, 1, 1 ] aR = -y/R aP = -x/R aY = s

1.3 控制效率矩阵

A_X = [ 1 1 1 1 1 1 ] [ 1/2 1 1/2 -1/2 -1 -1/2 ] [ -√3/2 0 +√3/2 +√3/2 0 -√3/2 ] [ -1 +1 -1 +1 -1 +1 ]

1.4 伪逆

A_X A_Xᵀ = diag(6, 3, 3, 6) (A_X A_Xᵀ)⁻¹ = diag(1/6, 1/3, 1/3, 1/6) A_X⁺ = A_Xᵀ (A_X A_Xᵀ)⁻¹
T
R
P
Y
M0
1/6
1/6
−√3/6
−1/6
M1
1/6
1/3
0
+1/6
M2
1/6
1/6
+√3/6
−1/6
M3
1/6
−1/6
+√3/6
+1/6
M4
1/6
−1/3
0
−1/6
M5
1/6
−1/6
−√3/6
+1/6

1.5 通道尺度与分配矩阵

D_X = diag(6, 2√3, 2√3, 6) B_X = A_X⁺ D_X
T
R
P
Y
M0
1
1/√3
−1
−1
M1
1
2/√3
0
+1
M2
1
1/√3
+1
−1
M3
1
−1/√3
+1
+1
M4
1
−2/√3
0
−1
M5
1
−1/√3
−1
+1

1.6 电机输出

u₀ = T + R/√3 − P − Y
u₁ = T + 2R/√3 + Y
u₂ = T + R/√3 + P − Y
u₃ = T − R/√3 + P + Y
u₄ = T − 2R/√3 − Y
u₅ = T − R/√3 − P + Y
2

6H

2.1 源代码与构型

static void MultiRotor6H_MotorControl(
    double output_throttle,
    double outRoll,
    double outPitch,
    double outYaw,
    bool DbgSafe)
{
    if (output_throttle < 0 || DbgSafe) {
        MainMotor_PullDownAll();
        update_output_throttle(0, 1.0 / CtrlRateHz);
        return;
    }

    double rp_out[6];
    double RollS =
        outRoll * 1.154700538379251529...; // 2/sqrt(3)
    double half_outRoll = 0.5 * RollS;

    rp_out[0] = -outPitch + half_outRoll;
    rp_out[1] = RollS;
    rp_out[2] = +outPitch + half_outRoll;
    rp_out[3] = +outPitch - half_outRoll;
    rp_out[4] = -RollS;
    rp_out[5] = -outPitch - half_outRoll;

    double yaw_out[6];
    yaw_out[0] = -0.5 * outYaw;
    yaw_out[1] = +outYaw;
    yaw_out[2] = -0.5 * outYaw;
    yaw_out[3] = +0.5 * outYaw;
    yaw_out[4] = -outYaw;
    yaw_out[5] = +0.5 * outYaw;

    MultiRotor_MotorControl(...);
}
+X Front +Y Right M0逆转 M1正转 M2逆转 M3正转 M4逆转 M5正转 y = −W y = +W
6H 构型

2.2 坐标与符号

Motorxysᵢ
M0+L−W−1
M10−W+1
M2−L−W−1
M3−L+W+1
M40+W−1
M5+L+W+1
aT = [ 1, 1, 1, 1, 1, 1 ] aR = -y/W aP = -x/L aY = s

2.3 控制效率矩阵

A_H = [ 1 1 1 1 1 1 ] [ 1 1 1 -1 -1 -1 ] [ -1 0 +1 +1 0 -1 ] [ -1 +1 -1 +1 -1 +1 ]

2.4 伪逆

A_H A_Hᵀ = [ 6 0 0 0 ] [ 0 6 0 -2 ] [ 0 0 4 0 ] [ 0 -2 0 6 ] (A_H A_Hᵀ)⁻¹ = [ 1/6 0 0 0 ] [ 0 3/16 0 1/16 ] [ 0 0 1/4 0 ] [ 0 1/16 0 3/16 ] A_H⁺ = A_Hᵀ (A_H A_Hᵀ)⁻¹
T
R
P
Y
M0
1/6
1/8
−1/4
−1/8
M1
1/6
1/4
0
+1/4
M2
1/6
1/8
+1/4
−1/8
M3
1/6
−1/8
+1/4
+1/8
M4
1/6
−1/4
0
−1/4
M5
1/6
−1/8
−1/4
+1/8

2.5 通道尺度与分配矩阵

D_H = diag(6, 8/√3, 4, 4) B_H = A_H⁺ D_H
T
R
P
Y
M0
1
1/√3
−1
−1/2
M1
1
2/√3
0
+1
M2
1
1/√3
+1
−1/2
M3
1
−1/√3
+1
+1/2
M4
1
−2/√3
0
−1
M5
1
−1/√3
−1
+1/2

2.6 电机输出

u₀ = T + R/√3 − P − Y/2
u₁ = T + 2R/√3 + Y
u₂ = T + R/√3 + P − Y/2
u₃ = T − R/√3 + P + Y/2
u₄ = T − 2R/√3 − Y
u₅ = T − R/√3 − P + Y/2
3

常数

√31.7320508075688772
1/√30.5773502691896258
2/√31.1547005383792515