Difference between revisions of "RotorcraftMixing"

From PaparazziUAV
Jump to navigation Jump to search
 
(13 intermediate revisions by 5 users not shown)
Line 23: Line 23:


<math>
<math>
\overrightarrow{M}_{i}^{B} =  C_t \omega_i^2 \begin{pmatrix}Y_i \\X_i \\D_i \frac{C_m}{C_t}\end{pmatrix}
\overrightarrow{M}_{i}^{B} =  C_t \omega_i^2 \begin{pmatrix}-Y_i \\X_i \\D_i \frac{C_m}{C_t}\end{pmatrix}
</math>
</math>


Line 40: Line 40:
\overrightarrow{M}^{B} = \sum_{i=1}^{N} \overrightarrow{M}_{i}^{B} = K C_t
\overrightarrow{M}^{B} = \sum_{i=1}^{N} \overrightarrow{M}_{i}^{B} = K C_t
   \begin{pmatrix}
   \begin{pmatrix}
                    \sum_{i=1}^{N} Y_i u_i\\
                  -\sum_{i=1}^{N} Y_i u_i\\
                     \sum_{i=1}^{N} X_i u_i\\
                     \sum_{i=1}^{N} X_i u_i\\
     \frac{C_m}{C_t} \sum_{i=1}^{N} D_i u_i
     \frac{C_m}{C_t} \sum_{i=1}^{N} D_i u_i
Line 52: Line 52:
   K C_t
   K C_t
   \begin{pmatrix}
   \begin{pmatrix}
     Y_1&Y_2&\ldots&Y_n\\
     -Y_1&-Y_2&\ldots&-Y_n\\
    X_1&X_2&\ldots&X_n\\
    X_1& X_2&\ldots& X_n\\
     \frac{C_m}{Ct}D_1&\frac{C_m}{Ct}D_2&\ldots&\frac{C_m}{Ct}D_n
     \frac{C_m}{Ct}D_1&\frac{C_m}{Ct}D_2&\ldots&\frac{C_m}{Ct}D_n
   \end{pmatrix}
   \end{pmatrix}
Line 94: Line 94:
A =
A =
   \begin{pmatrix}
   \begin{pmatrix}
    0.17 &-0.17 &0.25&-0.25& 0.33 &-0.33 \\
  -0.17 & 0.17 &-0.25& 0.25&-0.33 & 0.33 \\
   -0.35 &-0.35 &0  & 0  & 0.35 & 0.35 \\
   -0.35 &-0.35 & 0  & 0  & 0.35 & 0.35 \\
   -0.1  & 0.1  &0.1 &-0.1 &-0.1  & 0.1
   -0.1  & 0.1  & 0.1 &-0.1 &-0.1  & 0.1
   \end{pmatrix}
   \end{pmatrix}
</math>
</math>


The first two lines of the <math>A</math> matrix represent the coordinates of each rotor in the <math>X,Y</math> plan, and the third line the direction in which they spin.
The first two lines of the <math>A</math> matrix represent the coordinates of each rotor in the <math>X,Y</math> plane, and the third line the direction in which they spin. Note that in this example the X axis is vertical, and the Y coordinates are in the top row of <math>A</math>.


<math>
<math>
B =  
B =  
   \begin{pmatrix}
   \begin{pmatrix}
    0.24  & -0.71 & -1.47 \\
  -0.24  & -0.71 & -1.47 \\
  -0.241 & -0.71 &  1.47 \\
    0.241 & -0.71 &  1.47 \\
    0.93  &  0.  &  2.44 \\
  -0.93  &  0.  &  2.44 \\
  -0.93  &  0.  & -2.44 \\
    0.93  &  0.  & -2.44 \\
    0.69  &  0.71 & -1.09 \\
  -0.69  &  0.71 & -1.09 \\
  -0.69  &  0.71 &  1.09  
    0.69  &  0.71 &  1.09  
   \end{pmatrix}
   \end{pmatrix}
</math>
</math>


Normalizing <math>B</math> yelds
Normalizing columns of <math>B</math> yelds
 
<math>
<math>
\tilde{B} =  
\tilde{B} =  
   \begin{pmatrix}
   \begin{pmatrix}
    25  & -75 & -154 \\
  -67 & -256 & -154 \\
  -25  & -75 &  154 \\
    67 & -256 &  154 \\
    97  &   0  &  256 \\
  -256 &   0  &  256 \\
   -97  &   0  & -256 \\
   256 &   0  & -256 \\
    72  75 & -115 \\
  -189 256 & -115 \\
   -72  75 &  115
   189 256 &  115 \\
  \end{pmatrix}
  \end{pmatrix}
</math>
</math>
Line 130: Line 131:


<pre>
<pre>
   <section name="SUPERVISION" prefix="SUPERVISION_">
   <section name="MIXING" prefix="MOTOR_MIXING_">
     <define name="MIN_MOTOR" value="3"/>
     <define name="TRIM_ROLL" value="0"/>
    <define name="MAX_MOTOR" value="200"/>
     <define name="TRIM_PITCH" value="0"/>
    <define name="TRIM_A" value="0"/>
     <define name="TRIM_YAW" value="0"/>
     <define name="TRIM_E" value="0"/>
     <define name="TRIM_R" value="0"/>
     <define name="NB_MOTOR" value="6"/>
     <define name="NB_MOTOR" value="6"/>
     <define name="SCALE" value="256"/>
     <define name="SCALE" value="256"/>
     <define name="ROLL_COEF"  value="{ 25, -2597, -97,  7272}"/>
     <define name="ROLL_COEF"  value="{ -67,    67, -256256, -189189}"/>
     <define name="PITCH_COEF"  value="{ -75, -75,   0,   0,   7575}"/>
     <define name="PITCH_COEF"  value="{ -256, -256,   0,   0, 256256}"/>
     <define name="YAW_COEF"    value="{-154, 154, 256,-256, -115, 115}"/>
     <define name="YAW_COEF"    value="{ -154, 154, 256, -256, -115, 115}"/>
     <define name="THRUST_COEF" value="{ 256, 256, 256, 256,  256, 256}"/>
     <define name="THRUST_COEF" value="{ 256, 256, 256, 256,  256, 256}"/>
   </section>
   </section>
</pre>
</pre>


The above computation can be performed using the following simple octave script
== IPython/Numpy ==
'''The [https://github.com/paparazzi/paparazzi/blob/master/sw/tools/motor_mixing.py sw/tools/motor_mixing.py] script in paparazzi can be used to calculate and print configurations.'''
 
The above computation can be performed in a simple python script with numpy (or interactively using ipython). [http://nbviewer.ipython.org/6909831 example RotorcraftMixing.ipynb in IPython notebook viewer]
 
<source lang="python">
import numpy as np
 
A = np.array([[-0.17,  0.17, -0.25,  0.25, -0.33,  0.33],
            [-0.35, -0.35,  0.,    0.,    0.35,  0.35],
            [-0.1,  0.1,  0.1,  -0.1,  -0.1,  0.1 ]])
 
# Moore-Penrose pseudoinverse of A
B = np.linalg.pinv(A)
# normalizing columns of B and transpose
B_normalized = (B.T / B.max(axis=0)[:, np.newaxis])
 
# scale and round to 256 to return final coefficients
coeffs = np.around(256 * B_normalized).astype(int)
 
# nicely output result as defines for copy-pasting
import string
rows = ['ROLL_COEF"  ', 'PITCH_COEF" ', 'YAW_COEF"  ']
for i, r in enumerate(rows):
    print('<define name="' + r + 'value="{' + string.join(['{:>4d}'.format(c) for c in coeffs[i]], ', ') + '}"/>')
</source>
 
resulting in the output:
<pre>
<define name="ROLL_COEF"  value="{ -67,  67, -256,  256, -189,  189}"/>
<define name="PITCH_COEF" value="{-256, -256,    0,    0,  256,  256}"/>
<define name="YAW_COEF"  value="{-154,  154,  256, -256, -115,  115}"/>
</pre>
 
== Octave ==
 
The above computation can be performed using the following simple octave script (sudo apt-get install octave3.2)
octave
copy the flowing commands in the terminal: (ncol is the number of col from matrix B. in this example = 3 )
exit


<pre>
<pre>
A = [
A = [
   0.17   -0.17  0.25  -0.25  0.33  -0.33
   -0.17 0.17  -0.25  0.25  -0.33  0.33
   -0.35  -0.35  0.    0.    0.35    0.35
   -0.35  -0.35  0.    0.    0.35    0.35
   -0.1    0.1    0.1  -0.1  -0.1    0.1
   -0.1    0.1    0.1  -0.1  -0.1    0.1
Line 156: Line 194:
B = pinv(A)
B = pinv(A)


m = max(max(B));
m = max(abs(B))
 
[nrow,ncol] = size(B)
 
for i=1:ncol
  Btilde(:,i) = B(:,i)/m(i);
end
 
Btilde =  round(256*Btilde)
 
</pre>


Btilde =  round(B*256/m)
[[Category:Software]] [[Category:User_Documentation]]

Latest revision as of 15:09, 9 July 2015

This page describe how to compute "mixing" for an arbitrary multirotors configuration.

Hexa 3D.png

Introduction

"Mixing" consist in converting rotational accelerations commands computed by the autopilot into throttle commands for each of the motor controllers.

Let us consider a vehicle comprising a set of identical fixed pitch rotors located at coordinates and spinning in the same plane in the direction at a rotational speed .

Assuming a quasi hovering regime, the force produced by each rotor can be considered normal to the rotor plane and proportional to the square of its rotational speed. Expressed in body frame ( front, right, down ), this leads to

Under the same assumption, the torque produced by each rotor can also be assumed to be in the same direction and proportional to the square of the rotational speed.

The torque produced by each rotor around the CG, expressed in body frame can then be writen

Where is a "thrust" coefficient and is a "torque" coefficient. It has been measured experimentally that on a mikrokopter.

As a first approximation we can consider that the rotational speed of the propeller is proportional to the square root of the applied command


This allows us to express the torque produced by the set of rotors as

which can be rewriten as a matrix vector product

is a matrix describing the geometric positions of our rotors and is the vector of throttle commands for our set of motor controllers.

In order to express the command applied to each power train as a function of the momentum we want to apply to our vehicle, we need to find , a matrix such as

If has rank 3, we know that such a matrix exists (yeah, we can't do 2 rotors or have all rotors aligned), and in this case, we have the relationship

We know that one solution of this equation is the Moore-Penrose pseudoinverse of . Furthermore, this solution is the one leading to the power train command vector having the smallest euclidian norm, which is interesting as it optimizes the repartion of our control effort across our power trains.

Example

Let's consider the following H hexarotors configuration.

Hexa h.png

The first two lines of the matrix represent the coordinates of each rotor in the plane, and the third line the direction in which they spin. Note that in this example the X axis is vertical, and the Y coordinates are in the top row of .

Normalizing columns of yelds

which in turns yelds the following supervision section

   <section name="MIXING" prefix="MOTOR_MIXING_">
    <define name="TRIM_ROLL" value="0"/>
    <define name="TRIM_PITCH" value="0"/>
    <define name="TRIM_YAW" value="0"/>
    <define name="NB_MOTOR" value="6"/>
    <define name="SCALE" value="256"/>
    <define name="ROLL_COEF"   value="{ -67,    67, -256,  256, -189,  189}"/>
    <define name="PITCH_COEF"  value="{ -256, -256,    0,    0,  256,  256}"/>
    <define name="YAW_COEF"    value="{ -154,  154,  256, -256, -115,  115}"/>
    <define name="THRUST_COEF" value="{  256,  256,  256,  256,  256,  256}"/>
  </section>

IPython/Numpy

The sw/tools/motor_mixing.py script in paparazzi can be used to calculate and print configurations.

The above computation can be performed in a simple python script with numpy (or interactively using ipython). example RotorcraftMixing.ipynb in IPython notebook viewer

import numpy as np

A = np.array([[-0.17,  0.17, -0.25,  0.25, -0.33,  0.33],
             [-0.35, -0.35,  0.,    0.,    0.35,  0.35],
             [-0.1,   0.1,   0.1,  -0.1,  -0.1,   0.1 ]])

# Moore-Penrose pseudoinverse of A
B = np.linalg.pinv(A)
# normalizing columns of B and transpose
B_normalized = (B.T / B.max(axis=0)[:, np.newaxis])

# scale and round to 256 to return final coefficients
coeffs = np.around(256 * B_normalized).astype(int)

# nicely output result as defines for copy-pasting
import string
rows = ['ROLL_COEF"  ', 'PITCH_COEF" ', 'YAW_COEF"   ']
for i, r in enumerate(rows):
    print('<define name="' + r + 'value="{' + string.join(['{:>4d}'.format(c) for c in coeffs[i]], ', ') + '}"/>')

resulting in the output:

<define name="ROLL_COEF"  value="{ -67,   67, -256,  256, -189,  189}"/>
<define name="PITCH_COEF" value="{-256, -256,    0,    0,  256,  256}"/>
<define name="YAW_COEF"   value="{-154,  154,  256, -256, -115,  115}"/>

Octave

The above computation can be performed using the following simple octave script (sudo apt-get install octave3.2)

octave
copy the flowing commands in the terminal: (ncol is the number of col from matrix B. in this example = 3 )
exit
A = [
   -0.17  0.17   -0.25  0.25   -0.33   0.33
  -0.35   -0.35   0.     0.     0.35    0.35
  -0.1     0.1    0.1   -0.1   -0.1     0.1
];

B = pinv(A)

m = max(abs(B))

[nrow,ncol] = size(B)

for i=1:ncol
   Btilde(:,i) = B(:,i)/m(i);
end

Btilde =  round(256*Btilde)