Hello everyone! As a result of the transition to remote work, I got more free time to develop a hexapod (+2 hours a day due to savings on the road). I finally was able to make a universal algorithm for constructing a motion path in real time. New mathematics made it possible to implement basic movements by changing only two parameters. This is another step towards the implementation of "autopilot". In this article I will try to talk in detail about the new mathematics and how it generally works. There will be many pictures and gifs.

Development stages:

Part 1 - design
Part 2 - assembly
Part 3 - kinematics
Part 4 - mathematics of trajectories and sequences
Part 5 - electronics
Part 6 - transition to 3D printing
Part 7 - new housing, application software and communication protocols
Part 8 - advanced maths of movement

Back in time

Up to this point, all basic movements (forward, backward, rotation) were set in the form of “move the leg from the current position to the point (X, Y, Z) using linear / sinusoidal / any movement”. This works quite well and reliably, but severely limits movement. For example, in order to realize movement along an arc with radius R1, you need to calculate in advance the coordinates of the beginning and end of movement for each limb, add an additional button to the application so that you can select this movement. Accordingly, to add movement along an arc with a different radius R2, you need to calculate the coordinates again and add another button. Extremely uncomfortable.

Now, for the implementation of basic movements, one mathematical unit is used, which can be adapted to various situations with two parameters. The main bonus of the new mathematics was the support for movement along an arc with a change in its radius right during movement!

Idea of ​​the algorithm

First, it’s worth explaining how it works on the fingers. What happens if you select a fixed-size window on a circle and start increasing its radius? That's what:


Is everything just true? By changing the radius of the circle, we can get different trajectories of movement and with some accuracy a straight line. It was possible to stop at this, but not everything is so rosy. You can’t just take and implement this on the basis of the radius alone - there are nuances.

The initial positions of the limbs of the hexapod can be on different circles, respectively, the parameters of the equation will already be different. In my case, the limbs are arranged as follows (approximately):


The value of the distance that a limb must travel depends on the radius of the circle on which it is located. This forces us to switch to an individual calculation of the trajectory for each limb. To solve this problem, you need to find a circle with a maximum radius, calculate the starting and ending angles of the arc. Further relative to the resulting arc are arcs for other limbs. I made an animation of this piece of algorithm:


There is animation to illustrate the work of all this magic (how glad I am that in our time there is Excel). At the beginning, the value of the distance that the hexapod must travel per cycle (similar to speed), then the value of the curvature of the trajectory (similar to turning the steering wheel).


In general, this is the idea of ​​new mathematics, I hope it turned out to be accessible to explain. Now you can analyze the algorithm in more detail and try to count everything by hand in practice.

Mathematics

Input parameters

Variable input parameters are distance (distance) and curvature of the motion path (curvature). The value of the curvature of the trajectory should be in the ranges [-1.999; -0.001] and [0.001; 1.999], while the maximum distance value depends on the physical characteristics of the hexapod. In my case, the maximum distance per cycle is 110 mm, with large values, the limbs begin to abut each other. For an example of calculation, we take the values ​​curvature = 1.5 and distance = 20.

Also, for the algorithm to work, it is necessary to know the initial positions of the limbs. These are the points where the limbs are located when the hexapod is on its feet. For example, we will use the following points (the origin of each limb is located at the COXA attachment point):


Note: I work in the XZ plane and you can ignore the Y coordinate. You can get acquainted with the hexapod coordinate system in the third part of the cycle.

As a result, we have the following:

Formulas and calculations

We start by calculating the coordinates of the center of movement of the hexapod depending on the value of curvature and distance:

$$

$$R = tg(\frac{(2 - curvature) * \Pi}{4}) * distance$$

$$

$$R = tg(\frac{(2 - 1.5) * \Pi}{4}) * 20 = 8.28$$

As a result, we got the point [R; 0] and the trajectory of the body of the hexapod. Relative to it, trajectories for each limb will be calculated.


Next, it is necessary to calculate the radii of the trajectories for each limb relative to the center of motion (point [R; 0]) taking into account the initial position of the limb [x0; z0]. A more understandable language is to find the length of the vector drawn from the point [R; 0] to the point [x0; z0]:

$$

$$R_i = \sqrt{(R - x_{0i})^2 + z_{0i}^2}$$

$$

$$R_0 = \sqrt{(8.28 - (-20))^2 + 20^2} = 34.64$$

$$

$$R_1 = \sqrt{(8.28 - (-35))^2 + 0^2} = 43.28$$

$$

$$R_2 = \sqrt{(8.28 - (-20))^2 + (-20)^2} = 34.64$$

$$

$$R_3 = \sqrt{(8.28 - 20)^2 + (-20)^2} = 23.17$$

$$

$$R_4 = \sqrt{(8.28 - 35)^2 + 0^2} = 26.71$$

$$

$$R_5 = \sqrt{(8.28 - 20)^2 + 20^2} = 23.17$$

Picture for clarity. Blue shows the desired vectors.


From the obtained values ​​we find the maximum:

$$

$$R_{max} = maximum(R_{0-5})$$

$$

$$R_{max} = 43.28$$

Next, you need to find the angle for each vector that we tried in the previous step.

$$

$$\alpha_{0i} = atan2(z_{0i}; -(R - x_{0i}))$$

$$

$$\alpha_{00} = atan2(20; -(8.28 - (-20))) = 2.52 (144.7°)$$

$$

$$\alpha_{01} = atan2(0; -(8.28 - (-35))) = 3.14 (180°)$$

$$

$$\alpha_{02} = atan2(-20; -(8.28 - (-20))) = -2.52 (-144.7°)$$

$$

$$\alpha_{03} = atan2(-20; -(8.28 - 20)) = -1.04 (-59.6°)$$

$$

$$\alpha_{04} = atan2(0; -(8.28 - 35)) = 0 (0°)$$

$$

$$\alpha_{05} = atan2(20; -(8.28 -20)) = 1.04 (59.6°)$$

Now we find the angle of the arc on the largest circle of radius R_max (the most distant trajectory from the center of motion), the length of which should be equal to the distance value. This angle determines the starting and ending angles of other arcs along which the limbs will move. I think the picture below will help to understand this.


The angle is calculated as follows:

$$

$$arcMax = sign(R) * \frac{distance}{R_{max}}$$

$$

$$arcMax = \frac{20}{43.28} = 0.462 (26°)$$

Further using this angle, we can calculate the starting and ending angles of the arcs for other limbs. Something like this should turn out:


A small digression. To implement this algorithm, it is necessary to introduce the concept of time, the value of which lies in the range [0; 1]. It is also necessary that each limb with a value of time 0.5 return to its starting point. This rule is a check of the correctness of calculations - all circles must pass through the starting points of each limb.

Next, the calculation of points by the obtained parameters of the arcs begins, using the following formulas:

$$

$$arcAngle_i = (time - 0.5) * \alpha_{0i} + arcMax$$

$$

$$x_i = R + R_i * cos(arcAngle_i)$$

$$

$$z_i = R_i * sin(arcAngle_i)$$

Function source code

static bool process_advanced_trajectory(float motion_time) {

    // Check curvature value
    float curvature = (float)g_current_trajectory_config.curvature / 1000.0f;
    if (g_current_trajectory_config.curvature == 0)    curvature = +0.001f;
    if (g_current_trajectory_config.curvature > 1999)  curvature = +1.999f;
    if (g_current_trajectory_config.curvature < -1999) curvature = -1.999f;
    
    //
    // Calculate XZ
    //
    float distance = (float)g_current_trajectory_config.distance;

    // Calculation radius of curvature
    float curvature_radius = tanf((2.0f - curvature) * M_PI / 4.0f) * distance;

    // Common calculations
    float trajectory_radius[SUPPORT_LIMBS_COUNT] = {0};
    float start_angle_rad[SUPPORT_LIMBS_COUNT] = {0};
    float max_trajectory_radius = 0;
    for (uint32_t i = 0; i < SUPPORT_LIMBS_COUNT; ++i) {
        
        float x0 = g_motion_config.start_positions[i].x;
        float z0 = g_motion_config.start_positions[i].z;

        // Calculation trajectory radius
        trajectory_radius[i] = sqrtf((curvature_radius - x0) * (curvature_radius - x0) + z0 * z0);

        // Search max trajectory radius
        if (trajectory_radius[i] > max_trajectory_radius) {
            max_trajectory_radius = trajectory_radius[i];
        }

        // Calculation limb start angle
        start_angle_rad[i] = atan2f(z0, -(curvature_radius - x0));
    }
    if (max_trajectory_radius == 0) {
        return false; // Avoid division by zero
    }

    // Calculation max angle of arc
    int32_t curvature_radius_sign = (curvature_radius >= 0) ? 1 : -1;
    float max_arc_angle = curvature_radius_sign * distance / max_trajectory_radius;

    // Calculation points by time
    for (uint32_t i = 0; i < SUPPORT_LIMBS_COUNT; ++i) {
        
        // Inversion motion time if need
        float relative_motion_time = motion_time;
        if (g_motion_config.time_directions[i] == TIME_DIR_REVERSE) {
            relative_motion_time = 1.0f - relative_motion_time;
        }

        // Calculation arc angle for current time
        float arc_angle_rad = (relative_motion_time - 0.5f) * max_arc_angle + start_angle_rad[i];

        // Calculation XZ points by time
        g_limbs_list[i].position.x = curvature_radius + trajectory_radius[i] * cosf(arc_angle_rad);
        g_limbs_list[i].position.z = trajectory_radius[i] * sinf(arc_angle_rad);
        
        // Calculation Y points by time
        if (g_motion_config.trajectories[i] == TRAJECTORY_XZ_ADV_Y_CONST) {
            g_limbs_list[i].position.y = g_motion_config.start_positions[i].y;
        }
        else if (g_motion_config.trajectories[i] == TRAJECTORY_XZ_ADV_Y_SINUS) {
            g_limbs_list[i].position.y = g_motion_config.start_positions[i].y;
            g_limbs_list[i].position.y += LIMB_STEP_HEIGHT * sinf(relative_motion_time * M_PI);  
        }
    }
    
    return true;
}

Then I decided to show also the calculation of the Y coordinate (Calculation Y points by time). The calculation depends on the chosen trajectory, which is set rigidly in the code and is necessary for realizing the movement of the limb on the ground and in the air.

There is also a piece for implementing the reverse direction of movement (Inversion motion time if need). It is necessary that during the movement of three limbs on the ground, the other three limbs move in the opposite direction through the air.

results