Monthly Archives: April 2015

Mixing Motors #3

I’ve written about this before (here & here). In the previous code I was making a few assumptions:
1. Overall motor rotation is zero – so there is the same number of clockwise and counter-clockwise motors
2. Thrust vector is always pointing downwards
3. No physical units. The old mixer worked in ratios – full rotation along X axis, 0.3 rotation along Z Axis and so on

Now I want to get rid of all these assumptions and build a mixer that given a desired torque and the motor+quad configuration – it can figure out how much throttle to apply to each motor.

This is proving more complicated than I thought and the fact that I don’t have any formal math background doesn’t help at all.

So my requirements:
– Work in physical units. I want to to send the mixer 2 quantities: a torque in Nm and a collective force in N.
– Any (valid) motor configuration. X and + frames, quad, hexa, octo, spider, Y6 etc. Including motors with random thrust vectors – like a tricopter. Non-downward thrust vectors opens the door to a nice simplification as we’ll see below.
– Unbalanced setups. In reality it often happens that motors differ and I want my mixer to account for this and compensate without forcing the PIDs to do it

So first step is to have a model for the motor. I have the following struct to represent the relevant motor data:

struct Motor
    {
        math::vec3f position;
        math::vec3f thrust_vector;
        bool clockwise = true;
        float max_thrust = 0; //N
        float max_rpm = 0;
        float acceleration = 0; //rpm/s
        float deceleration = 0; //rpm/s
    };

Position is in local space, relative to the center of mass of the UAV.
Thrust vector is normalized.
Max_thrust is in N and is the thrust of the motor at max_rpm.
Acceleration and deceleration are used to model motor+prop inertia.

Using the position and thrust_vector I can calculate the max torque the motor will produce:

max_torque = thrust_vector X position * max_thrust; (X is cross product)

Using max_rpm and max_thrust I can figure out how much throttle too apply to get a certain thrust using the following approximation:

//thrust increases approximately with throttle^2
auto throttle = math::sqrt<float, math::safe>(target_thrust / max_thrust);

Now the problem is – how to figure out what throttle to send to each motor to get a certain torque and total force?

Let’s consider a simple case: a perfectly balanced quadcopter with a + frame. Each motor points downwards so they generate a torque along one of the axis (and arms) but there is the yaw torque as well that needs to be taken into account. This can be modeled by simply tilting a bit the thrust vector which in turn creates a small torque on the Z axis. The tilt factor depends on the motor and prop but it’s usually a factor of the RPM or RPM^2. For now let’s consider it constant.

So each motor produces a max torque: A, B, C, D.
A and C are opposite, just like B and D:

A = -C
B = -D

What I want to find out is the factors a, b, c and d in the equation below:
1. aA + bB + cC + dD = T    where T is the target torque of the quad

As you can imagine there are an infinite number of factors that give a good answer so we need more constraints. These are:
2. I also want a total thrust of Th.
3. I want a, b, c, d to have the minimum values that result in the desired torque. So I want the optimal solution.

For a quad, equations 1 and 2 are enough to calculate the optimal factors but for a hexa, octo or any other setup it’s not enough. I don’t know how to express eq 3 mathematically but this equation should result in an unique set of factors.
So in the past days I tried to figure out how to calculate the coefficients for some very weird (but valid) motor configurations and in the end I gave up on the math approach.

So what’s left when the analytic approach is out of reach? You try brute force of course.
I wrote a small iterative algorithm that tries to find out the factors with a form of binary search. It’s very short and seems to be fast enough. It takes a motor config, a target torque and a step size and computes the factors with a precision of 0.0001.

A step size of 0.01 requires ~3-7000 iterations, 0.1 requires 3-700 and 0.5 can solve it in 50-100 iterations. Pretty good.
However I cannot prove that the algorithm will always converge (if I could prove this, then I could also solve the initial problem to begin with) and I’ve been running the algorithm all day yesterday with random valid motor configs and random target torques and so far I had no bad cases.

Here it is:

void test_mm(math::vec3f const& target, float step)
{
    struct Motor
    {
        math::vec3f torque; //precomputed maximum torque
        float factor;
    };

    //some test configuration with random data
    std::vector<Motor> motors = {{
                                     {{2, 3, 0.1}, 0},
                                     {{5, -1, -0.1}, 0},
                                     {{-1, -7, 0.1}, 0},
                                     {{-4, 2, -0.1}, 0},
                                     {{-0.1, .2, -1}, 0},
                                }};

    size_t iteration = 0;
    while (true)
    {
        //first calculate the crt torque
        math::vec3f crt;
        for (auto& m: motors)
        {
            crt += m.torque * m.factor;
        }

        //check if we're done
        if (math::equals(crt, target, 0.0001f))
        {
            QLOGI("{}: Done in {} iterations", step, iteration);
            break;
        }

        //how far are we for the target?
        //divide by motor count because I want to distribute the difference to all motors
        auto diff = (target - crt) / float(motors.size());

        //distribute the diff to all motors
        for (auto& m: motors)
        {
            //figure out how much each motor can influence the target torque
            //  by doing a dot product with the normalized torque vector
            auto t = math::normalized(m.torque);
            auto f = math::dot(t, diff) * step; //go toqards the difference in small steps so we can converge
            m.factor += f;
        }

        iteration++;
    }
}

And the test code:

q::util::Rand rnd;
while (true)
{
    math::vec3f target(rnd.get_float() * 40.f, rnd.get_float() * 40.f, rnd.get_float() * 10.f);
    test_mm(target, 0.01);
    test_mm(target, 0.1);
    test_mm(target, 0.5);
}

Now I have to test in in the simulator.

Advertisements