The history of one diploma or how to make a sharabota

The history of this project begins in 2014, when I was studying for the 4th year in the leading technical university of Russia at the Department of Robotic Systems. At that time I started to think about the subject of the diploma and was looking for a project that would be interesting to me, and at the same time there was some novelty in it. Then one day, after seeing the video of the scarabber Rezero , my friends and I wanted to try to repeat the success. Who cares what came out of it - I ask for cat.

Introduction

In the beginning I would like to talk about the merits of the sharabota. Thanks to a single point of contact with the surface, the sharabot is equally easy to move in all directions, being extremely mobile and maneuverable, compared to conventional wheeled robots. The maneuverability of the sharabot is limited only by its dynamics, in contrast to the mechanical limitations imposed by the wheels (for example, the impossibility of moving sideways).

The next important advantage is that a robot can be high, and the higher it is, the more stable it will be. Why steadier? This is evident from the equation of the dynamics of the inverse pendulum. Acceleration of the deviation from the vertical equilibrium position is inversely proportional to the distance to the center of mass, i.e. the higher reverse pendulum will fall more slowly. This reduces the response speed requirements of the control system, but perhaps increases the moment that the drive should develop.

Another advantage is that it can travel on inclined and moving surfaces, for example, the deck of a ship or the floor of an aircraft during takeoff. And you see, it moves much more beautifully than ordinary wheeled robots.

One of the main drawbacks of the sharabota is the possibility of losing the vertical position of balance. But personally, I think this is a completely solvable engineering problem. The engineers of the robots Aido and Mobi decided it as follows: when a certain angle of deflection is exceeded, the “legs” are advanced so that the robot does not lose stability.

My story consists of the following parts:
- Mathematical model
- Development of control algorithms
- Construction
- Hardware and software
- Results

1 Mathematical model

The derivation of the equations of motion is necessary for the further synthesis of control and the simulation of motion. In this section there will be some math and mechanics.

At the moment, all existing mathematical models of the sharabota are made taking into account some simplifications using the Lagrange equations of the 2nd kind. And since the scarabot is a nonholonomic mechanical system, it is incorrect to apply the Lagrange equations of the 2nd kind to such a system. In the CMU robot, the scarab model is considered as three independent flat models, thereby not taking into account the mutual influence of these models. Rezero developed a three-dimensional mathematical model, which does not take into account the gyroscopic effects that occur during the rotation of the omnic wheel.

I set a goal to create the most complete mathematical model of the robot with the minimum number of assumptions. My scientific adviser S.L. helped me to achieve this goal. Krutikov, for which I express my deep gratitude to him.

1.1 Kinematics

For the kinematic description of the system, I introduced the moving coordinate systems shown in the figure.

Inertial coordinate system is designated as $xyz$ . Coordinate system $x_1y_1z_1$ located in the center of the ball. To go from the inertial frame of reference to the center of the ball, the following homogeneous matrix is used:

$\ begin {equation *} \ m {A} _c = \ begin {pmatrix} \ nonumber 1 & amp; 0 & amp; 0 & amp; x \\ 0 & amp; 1 & amp; 0 & amp; y \\ 0 & amp; 0 & amp; 1 & amp; r_ {ball} \\ 0 & amp; 0 & amp; 0 & amp; 1 \ end {pmatrix}, \ end {equation *}$

Where $x$ and $y$ the displacement of the center of the ball along the axis $x$ and $y$ respectively.

To describe the rotation of the ball, it is convenient to use the gimbal angles or the Tait – Bryan angles. So the coordinate system $x'_1y'_1z'_1$ associated with a ball is obtained by turning $Cx_1y_1z_1$ on the corners $\ varphi_ {x}$ , $\ varphi_ {y}$ and $\ varphi_ {z}$ around the axes $x_1, y_1$ and $z_1$ respectively. This sequence of turns can be represented as a product of turning matrices:

$\ begin {equation *} \ label {eq: m_rotaion_ball} \ m {R} _ {\ varphi} = \ m {R} _x (\ varphi_x) \ m {R} _y (\ varphi_y) R_z (\ varphi_z). \ end {equation *}$

Similarly, one can describe the rotation of the body of a scarabot in space. Coordinate system $x_2y_2z_2$ The 2nd link associated with the body is obtained by turning $x_1y_1z_1$ on the corners $\ vartheta_ {x}$ , $\ vartheta_ {y}$ and $\ vartheta_ {z}$ around the axes $x_1, y_1$ and $z_1$ respectively. This sequence of turns can also be represented as a product of turning matrices:

$\ begin {equation} \ label {eq: m_rotaion_body} \ m {R} _ {\ vartheta} = \ m {R} _x (\ vartheta_ {x}) \ m {R} _y (\ vartheta_ {y}) \ m {R} _z (\ vartheta_ {z}). \ nonumber \ end {equation}$

The 3rd, 4th and 5th coordinate systems are associated with omniwheels. To move from the body's coordinate system to the omni-wheel coordinate system, you need to perform a series of elementary transformations (two rotations, transference, then rotation again), I will not describe them here.

The sequence of transitions is represented as the following kinematic graph:

To describe the position of the system, the following vector of generalized coordinates is used:

$\ begin {equation} \ ve {q} = (x, y, \ varphi_x, \ varphi_y, \ varphi_z, \ vartheta_x, \ vartheta_y, \ vartheta_z, \ psi_1, \ psi_2, \ psi_3) ^ T, \ nonumber \ end {equation} \\$

Where $x, y$ - coordinates of the center of the ball, $\ varphi_x, \ varphi_y, \ varphi_z, \ vartheta_x, \ vartheta_y, \ vartheta_z$ - cardan angles describing the rotation of the ball and the body, respectively, $\ psi_1, \ psi_2, \ psi_3$ - angles of rotation of omnic wheel around the axis of the engine.

1.2 Dynamics

On the basis of the nonholonomicity of the system, it was decided to use the Appel equations to compose the differential equations of motion of the sharabota.

6 nonholonomic constraints are imposed on the system: three omniwheel rolling bonds along the ball, two bonds for the ball center speed and one bond of the ball spinning. Thus, the number of generalized coordinates is m = 11, the number of constraints is s = 6, and the number of degrees of freedom is n = 5. We will use the following pseudo-velocity vector:

$\ begin {equation *} \ bm {\ dot {\ pi}} = (\ dot {x}, \ dot {y}, \ dot {\ vartheta_x}, \ dot {\ vartheta_y}, \ dot {\ vartheta_z} ) ^ T. \ end {equation *}$

To compile the equations of motion, it is necessary to calculate the acceleration energy for each link:

$\ begin {equation *} S = \ sum \ limits_ {i = 1} ^ N \ frac {1} {2} \ cdot tr (\ ddot {\ m {T}} _ {i} \ m {H} _ {i} \ ddot {\ m {T}} ^ \ m {T} _ {i}}), \ end {equation *}$

Where $N$ - the number of links $\ m {T} _ {i}$ - transition matrix from inertial coordinate system to coordinate system $i$ link, $\ m {H} _ {i}$ - inertia matrix $i$ link, and $tr$ - trace matrix . In general $S$ is a function of $\ ddot {q} _1, ..., \ ddot {q} _m, t$ . Using the equations of the coupling, the acceleration energy is reduced to a function depending only on $\ ddot {\ pi} _1, ..., \ ddot {\ pi} _n, t$ .

Mass inertia parameters

The matrix of inertia of the ball has a diagonal view, since coordinate system axes $Ox_1y_1z_1$ are the main central axes of inertia:

$\ begin {equation *} \ m {H} _ {1} = \ begin {pmatrix}% \ nonumber I_ {1, xx} & amp; 0 & amp; 0 & amp; 0 \\ 0 & amp; I_ {1, yy} & amp; 0 & amp; 0 \\ 0 & amp; 0 & amp; I_ {1, zz} & amp; 0 \\ 0 & amp; 0 & amp; 0 & amp; m_ {1} \ end {pmatrix}. \ end {equation *}$

Since plane $Cx_2z_2$ and $Cy_2z_2$ are the planes of symmetry of the body, the centrifugal moments of inertia $I_ {xy}, I_ {xz}$ and $I_ {yz}$ are zero. The body inertia matrix will look like this:

$\ begin {equation *} \ m {H} _ {2} = \ begin {pmatrix}% \ nonumber I_ {2, xx} & amp; 0 & amp; 0 & amp; 0 \\ 0 & amp; I_ {2, yy} & amp; 0 & amp; 0 \\ 0 & amp; 0 & amp; I_ {2, zz} & amp; m_ {2} \ cdot l \\ 0 & amp; 0 & amp; m_ {2} \ cdot l & amp; m_ {2} \ end {pmatrix}, \ end {equation *}$

Where $l$ - distance from the center of the ball to the center of mass of the body along the axis $Cz_2$ .

The omni-wheel inertia matrix has a diagonal view, since The axes of the omni-wheel coordinate system form planes of symmetry:

$\ begin {equation *} \ m {H} _ {3,4,5} = \ begin {pmatrix}% \ nonumber I_ {w, xx} & amp; 0 & amp; 0 & amp; 0 \\ 0 & amp; I_ {w, yy} & amp; 0 & amp; 0 \\ 0 & amp; 0 & amp; I_ {w, zz} & amp; 0 \\ 0 & amp; 0 & amp; 0 & amp; m_ {wheel} \ end {pmatrix}. \ end {equation *}$

In the moment of inertia $I_ {w, zz}$ it is also necessary to take into account the moment of inertia of the rotor of the engine:

$I_ {w, zz} = I '_ {w, zz} + i ^ 2 \ cdot I _ {\ text {mouth}},$

Where $i$ - The gear ratio of the gearbox.

Appel's differential equation of motion in pseudo-coordinates:

$\ begin {equation} \ label {eq: appel} \ frac {\ partial {S}} {\ partial {\ boldsymbol {\ ddot {\ pi}}}} = \ m {Q} _1 + \ m {Q} _2, \ end {equation}$

Where $\ m {Q} _1$ - the generalized force from the moments of the drives, $\ m {Q} _2$ - generalized force from gravity.

Write the Appel equations in matrix form and decide on $\ ddot {\ pi}$ :

$\ begin {equation *} \ boldsymbol {\ ddot {\ pi}} = \ m {A} ^ {- 1} (\ boldsymbol {\ ve {q}}) (\ m {Q} _ {1} - \ boldsymbol {b (\ ve {q}, \ dot {\ pi})} - \ boldsymbol {c (\ ve {q})}) \ end {equation *}$

A little bit more

The equations of motion can be written in matrix form.

$\ begin {equation *} \ boldsymbol {f} (\ boldsymbol {q}, \ boldsymbol {\ dot {\ pi}}, \ boldsymbol {\ ddot {\ pi}}, \ boldsymbol {\ tau}, t) = \ m {A} (\ boldsymbol {q}) \ boldsymbol {\ ddot {\ pi}} + \ boldsymbol {b} (\ boldsymbol {q}, \ boldsymbol {\ dot {\ pi}}) + \ boldsymbol { c} (\ boldsymbol {q}) - \ m {Q} _1 = 0, \ end {equation}$

Where

$\ begin {equation *} \ m {A} (\ boldsymbol {q}) = \ frac {\ partial {\ boldsymbol {f}}} {\ partial {\ boldsymbol {\ ddot {\ pi}}}}, \ \ \ boldsymbol {b} (\ boldsymbol {q}, \ boldsymbol {\ dot {\ pi}}) = \ boldsymbol {f} - \ m {A} \ cdot \ boldsymbol {\ ddot {\ pi}}, \ \ & amp; \ boldsymbol {c} (\ boldsymbol {q}) = - \ m {Q} _ {2}. \ end {equation *}$

All calculations were performed symbolically using Maple. Then the resulting equations were transferred from Maple to Matlab for modeling.

2 Development of control algorithms

A little more control theory and the match ends here. In control theory, methods for analyzing linear systems are well developed. For completely observable systems, optimal control with a quadratic functional (linear quadratic regulator, LQR) is most often used, which guarantees the stabilization of the system if the system is completely controllable. Sharobot is a completely observable system, since state vector can be fully measured. Having calculated the rank of the controllability matrix, I was also convinced of the controllability of the system.

To obtain a linear-quadratic controller at the beginning, it is necessary to linearize the system in the vicinity of the vertical position of unstable equilibrium $\ boldsymbol {q} = 0, \ boldsymbol {\ dot {\ pi}} = 0, \ boldsymbol {\ ddot {\ pi}} = 0$

$\ begin {equation *} \ boldsymbol {\ dot {x}} & amp; = \ m {A} \ cdot \ boldsymbol {x} + \ m {B} \ cdot \ boldsymbol {u}, \ nonumber \\ \ boldsymbol {y} & amp; = \ m {C} \ cdot \ boldsymbol {x} + \ m {D} \ cdot \ boldsymbol {u}, \ nonumber \\ \ boldsymbol {x} & amp; (x, y, \ vartheta_x, \ vartheta_y, \ vartheta_z, \ dot {x}, \ dot {y}, \ dot {\ vartheta_x}, \ dot {\ vartheta_y}, \ dot {\ vartheta_z}) ^ T, \ nonumber \\ \ m {C} & amp; = \ m {E} _ {10}, \ quad \ m {D} = 0. \ nonumber \ end {equation *}$

Vector $\ boldsymbol {x}$ - state vector or phase vector, $\ boldsymbol {y} = \ boldsymbol {x}$ - vector of measurement and $\ boldsymbol {u} = (\ tau_1, \ tau_2, \ tau_3) ^ T$ - control (drive moments).

The LQR controller has the following optimality criterion:

$\ begin {equation *} \ label {eq: cost} \ m {J} = \ int \ limits_ {0} ^ {\ infty} (\ boldsymbol {x} ^ T \ m {Q} \ boldsymbol {x} + \ boldsymbol {u} ^ T \ m {R} \ boldsymbol {u}) dt, \ end {equation *}$

Where $\ m {Q}$ and $\ m {R}$ - positive definite matrix. The task of minimizing this functional is reduced to solving a matrix algebraic Riccati equation:

$\ begin {equation *} \ label {eq: ricatti} \ Phi \ cdot \ m {B} \ cdot \ m {R} ^ {- 1} \ cdot \ m {B} ^ T \ cdot \ Phi - \ Phi \ cdot \ m {A} - \ m {A} ^ T \ cdot \ Phi - \ m {Q} = 0. \ end {equation *}$

For a linear-quadratic controller, the control is written as $\ boldsymbol {u} = - \ m {K} \ boldsymbol {x}$ where $\ m {K} = \ m {R} ^ {- 1} \ m {B} \ Phi$ , $\ Phi$ - solution of the Riccati equation. As a result, we have a matrix of feedback coefficients:

$\ begin {equation *} \ m {K} = \ begin {pmatrix} 0 & amp; -1.0 & amp; 9.63 & amp; 0 & amp; -0.7 & amp; 0 & amp; -1.34 & amp; 1.88 & amp; 0 & amp; -0.18 \ 0.86 & amp; 0.5 & amp; -4.81 & amp; 8.34 & amp; -0.7 & amp; 1.16 & amp; 0.67 & amp; -0.9 & amp; 1.63 & amp; -0.18 \\ -0.86 & amp; 0.5 & amp; -4.81 & amp; -8.34 & amp; -0.7 & amp; -1.16 & amp; 0.67 & amp; -0.9 & amp; -1.63 & amp; -0.18 \\ \ end {pmatrix}. \ nonumber \ end {equation *}$

When multiplying this matrix by the state vector $\ boldsymbol {x}$ we get three points that need to be applied to the engines to stabilize the system. LQR is a PD regulator if the state vector consists of coordinates and their derivatives. As you understand, it is almost impossible to use the usual PID controller and manually select coefficients in such a system.

Model in Matlab simulink c three-dimensional visualization

I will give a small explanation of each unit. In the block “Ballbot 3D model” the Appel equations are realized. At the entrance is administered $\ boldsymbol {u}$ (three moments), the output is the state vector $\ boldsymbol {x}$ (see item 2).

The VRML transform block transforms the state vector into coordinates for visualization. “VR Sink” contains a model in vrml format and it is this block that draws the graphics.

The state vector is fed to the input of the “controller” block $\ boldsymbol {x}$ and the desired state vector $\ boldsymbol {x} _d$ , with which you can set the trajectory of movement and speed. The block produces elementary deceleration of the matrices for three moments. $\ boldsymbol {u} = - \ m {K} (\ boldsymbol {x} - \ boldsymbol {x} _d)$ .

Since the moment that the drive develops is limited, I added a saturation link.

For the three-dimensional visualization was made a simple model of a scaffold in SolidWorks. It was then exported in vrml format and added to the VR Sink block.

Simulation results with an initial deviation of 10 degrees

3 Construction

The design of the robot is developed in the Siemens NX CAD system. On the basis of the constructed model, the mass and inertial characteristics of the robot links were determined.

The design consists of two aluminum bases. Three engines are mounted on the lower base, which drive the omnic wheel. The upper base is attached to the bottom with shock absorbers and guides. The task of shock absorbers was to reduce the vibrations of the upper part of the body, where all the electronics are located, although this did not help in practice.

The shelves are made of plexiglass to facilitate construction. They contain all the onboard electronics: battery, drive controllers, microcontroller, inertial, etc.

The ball is a basketball with a diameter of 240 mm. The ball holders press it against omnic wheels, thereby increasing friction. Unfortunately, they could not be made, as there is no 3D printer in our department, and printing on order is expensive, because they are not small and the fill factor is needed big for durability.

Omnicoles were purchased on Aliexpress for $ 120 apiece. The most expensive part of the robot, after the drives, of course.

All metal parts are made of duralumin to order at one Moscow factory. At the same factory we made shelves of plexiglass. The order came in the amount of 30 000r, approximately.

Some photos of the assembly with comments

A friend helped me dissolve the board on which the DC-DC converter, IMU and logical level converter for I2C are located. ODROID stuck on top

This is the very first design with Dynamixel MX64 drives. Drive boxes are printed.

HSP shock absorbers for radio-controlled models with a scale of 1:10.

As such, I got a set of drives and controllers.

In order to put the omniwire on the motor shaft, the sleeve had to be turned out.

This is the final result.

4 Hardware and software

Almost all hardware components of a scarab are represented in a functional diagram. I will say a few words about each element, moving "from the bottom up."

4.1 Drives and Drive Controllers

It was harder for everyone to find drives and drive controllers. At the output of the LQR regulator we have a moment, therefore we need to have a controller with the ability to control current (i.e. by moment). Apparently, this task is very rare in everyday life, and we found only one affordable solution, Dynamixel. We bought and tried a Dynamixel MX64 drive, which has a current control mode. Unfortunately, their speed was not enough to stabilize the robot.

I had already lost hope of creating a real sharabota, but fortunately Yaroslav from the RCC Robotics helped us and provided for some time three Maxon drives with controllers, for which we are very grateful. As a result, we had a Maxon EC-max 30 40W brushless DC motor with the following characteristics:

Rated voltage 24V
Nominal rotational speed of 7220 rev / s
Nominal torque of 33.8 mNm
Holding moment 160 mNm

Maxon GP 32 planetary gearbox with a ratio of n = 14 and Maxon EPOS 24/5 drive controller, which has a current control mode.

As you can see, the drive is not very powerful and the gear ratio is small, so the moment at the output is hardly enough to stabilize the scatter beam. The Rezero, for example, uses 200W motors and a gearbox with a gear ratio of 51.

4.2 Microcontroller, inertial module, transceiver

The STM32F4-Discovery was used as a microcontroller, which has the interfaces we need: CAN, UART and I2C. It receives data from the gyroscope and accelerometer via I2C and encoders via CAN bus. Based on the received data, it calculates the control and sends the task to the drive controllers via CAN bus. In order not to implement the protocol for communicating with EPOS controllers myself, I used the library of libepos . In order to connect the STM to the CAN network, a CAN transceiver (transceiver) for $ 4 is needed.

As an inertial module, we used a $ 3 GY-521 board based on the MPU6050 chip, which includes a 3-axis gyroscope and a 3-axis accelerometer. For processing the readings of these sensors, I used the filter Madjvika , which recently so loved kopterovody.

To simplify development under STM, I used the STM32Cube HAL (hardware abstraction layer).

The frequency at which the control works is about 300 Hz, i.e. 300 times a second, we read all the sensors, calculate the control and send the task to the drive. All this happens in an infinite loop, which can be represented as the following pseudocode:

int main() { initialize_imu();/*  IMU  I2C */ initialize_motors(); /*    CAN */ while (1) { read_imu(); /*     IMU */ get_omniwheels_speed(); /*     CAN */ /*    UART  odroid */ if (uart_rx_flag) { uart_rx_flag = 0; struct joystick_data* joystick = (struct joystick_data*)UARTdev_Get_RX_buf(); process_joystick_input(joystick); } calculate_control(); /*   */ set_torque(); /*      CAN */ } }

4.3 Single board computer, battery, DC-DC converter, joystick

The onboard single board computer ODROID U3 receives data from the joystick via a Bluetooth adapter and transmits it to the microcontroller via the UART. It has a lubuntu operating system with Linux 3.8.13.26-rt31 kernel with real-time support.

Lead-acid battery Delta 12045 with a capacity of C = 4.5 Ah. It lasts about an hour of work.

The voltage of the single-board computer Odroid-U3 5V, the maximum current consumption of 2A. Since power supply 12V, a buck $ 2 LM2596S DC-DC converter is needed.

To control the scarab, Bluetooth uses a Terios joystick. The joystick transmits commands via Bluetooth to the ODROID, which contains the Asus USB-BT400 adapter. To read the commands of the joystick, I used the following library . The incoming command is parsed, packed and sent over the UART to the STM32, where an interrupt occurs at the reception of each byte.

You can also connect to ODROID via SSH. When started, the odroyd creates an ad-hoc network via a wifi adapter.

results

Total we managed to collect a working sharabota. It took exactly 2 years to implement this project. Let's just say that we didn’t manage to get close to the results of Rezero, as there were no such human, industrial and financial resources as students from Switzerland. I think if there were more powerful engines and a more advanced inertial module, the results would be much better.

The source code I do not open for some reason. If you want to know the details of implementation - write to us, we will be happy to share with you the developments. The engines had to be returned and at the moment the piece of iron stands without them. Perhaps the project will be further developed by students, if there are suitable engines and there are those who wish. For example, it would be interesting to develop nonlinear control algorithms and improve the IMU filtering algorithm.

I spoke rather superficially about the development process, so I will be glad to answer your questions.