问:
position control for linear model of quadrotor (problem with tracking task)
Lately, if you notice I have posted some questions regarding position tracking for nonlinear model. I couldn't do it. I've switched to linear model, hope I can do it. For regulation problem, the position control seems working but once I switch to tracking, the system starts oscillating. I don't know why. I have stated what I've done below hope someone guides me to the correct path.

The linear model of the quadrotor is provided here which is

$$
\begin{align}
\ddot{x}    &= g \theta \ \ \ \ \ \ \ \ \ \ (1)\\
\ddot{y}    &= - g \phi \ \ \ \ \ \ \ \ \ \ (2)\\
\ddot{z}    &= \frac{U_{1}}{m} - g \\
\ddot{\phi} &= \frac{L}{J_{x}} U_{2} \\
\ddot{\theta} &= \frac{L}{J_{y}} U_{2} \\
\ddot{\psi} &= \frac{1}{J_{z}} U_{2} \\
\end{align}
$$

In this paper, the position control based on PD is provided. In the aforementioned paper, from (1) and (2) the desired angles $\phi^{d}$ and $\theta^{d}$ are obtained, therefore,

$$
\begin{align}
\theta^{d}  &= \frac{\ddot{x}^{d}}{g}  \\
\phi^{d}    &= - \frac{\ddot{y}^{d}}{g}
\end{align}
$$

where

$$
\begin{align}
\ddot{x}^{d} &= Kp(x^{d} - x) + Kd( \dot{x}^{d} - \dot{x} ) \\
\ddot{y}^{d} &= Kp(y^{d} - y) + Kd( \dot{y}^{d} - \dot{y} ) \\
U_{1} &= Kp(z^{d} - z) + Kd( \dot{z}^{d} - \dot{z} ) \\
U_{2} &= Kp(\phi^{d} - \phi) + Kd( \dot{\phi}^{d} - \dot{\phi} ) \\
U_{3} &= Kp(\theta^{d} - \theta) + Kd( \dot{\theta}^{d} - \dot{\theta} ) \\
U_{4} &= Kp(\psi^{d} - \psi) + Kd( \dot{\psi}^{d} - \dot{\psi} ) \\
\end{align}
$$

with regulation problem where $x^{d} = 2.5 m, \ y^{d} = 3.5 m$ and $z^{d} = 4.5 m$, the results are

Now if I change the problem to the tracking one, the results are messed up.

In the last paper, they state

A saturation function is needed to ensure that the reference roll and
  pitch angles are within specified limits

Unfortunately, the max value for $\phi$ and $\theta$ are not stated in the paper but since they use Euler angles, I believe $\phi$ in this range $(-\frac{\pi}{2},\frac{\pi}{2})$ and $\theta$ in this range $[-\pi, \pi]$
I'm using Euler method as an ODE solver because the step size is fixed. For the derivative, Euler method is used.

This is my code

```<code>%######################( PD Controller & Atittude )%%%%%%%%%%%%%%%%%%%%

clear all;
clc;

dt = 0.001;
 t = 0;

% initial values of the system
 x = 0;
dx = 0;
 y = 0;
dy = 0;
 z = 0;
dz = 0;

   Phi = 0;
  dPhi = 0;
 Theta = 0;
dTheta = 0;
   Psi = pi/3;
  dPsi = 0;

%System Parameters:
m = 0.75;      % mass (Kg)
L = 0.25;      % arm length (m)
Jx = 0.019688; % inertia seen at the rotation axis. (Kg.m^2)
Jy = 0.019688; % inertia seen at the rotation axis. (Kg.m^2)
Jz = 0.039380; % inertia seen at the rotation axis. (Kg.m^2)
g  = 9.81;      % acceleration due to gravity m/s^2

errorSumX = 0;
errorSumY = 0;
errorSumZ = 0;

errorSumPhi   = 0;
errorSumTheta = 0;

pose = load('xyTrajectory.txt');

% Set desired position for tracking task
DesiredX = pose(:,1);
DesiredY = pose(:,2);
DesiredZ = pose(:,3);

% Set desired position for regulation task
% DesiredX(:,1) = 2.5;
% DesiredY(:,1) = 5;
% DesiredZ(:,1) = 7.2;

dDesiredX = 0;
dDesiredY = 0;
dDesiredZ = 0;

DesiredXpre = 0;
DesiredYpre = 0;
DesiredZpre = 0;

dDesiredPhi = 0;
dDesiredTheta = 0;
DesiredPhipre = 0;
DesiredThetapre = 0;

for i = 1:6000

   % torque input
   %&&&&&&&&&&&&( Ux )&&&&&&&&&&&&&&&&&&
   Kpx = 90; Kdx = 25; Kix = 0.0001;

   errorSumX = errorSumX + ( DesiredX(i) - x );

   % Euler Method Derivative
     dDesiredX = ( DesiredX(i) - DesiredXpre ) / dt;
   DesiredXpre = DesiredX(i);

   Ux = Kpx*( DesiredX(i) - x  ) + Kdx*( dDesiredX - dx ) + Kix*errorSumX;
   %&&&&&&&&&&&&( Uy )&&&&&&&&&&&&&&&&&&
   Kpy = 90; Kdy = 25; Kiy = 0.0001;

   errorSumY = errorSumY + ( DesiredY(i) - y );

   % Euler Method Derivative
   dDesiredY = ( DesiredY(i) - DesiredYpre ) / dt;
   DesiredYpre = DesiredY(i);

   Uy = Kpy*( DesiredY(i) - y  ) + Kdy*( dDesiredY - dy ) + Kiy*errorSumY;
   %&&&&&&&&&&&&( U1 )&&&&&&&&&&&&&&&&&&
   Kpz = 90; Kdz = 25; Kiz = 0;

   errorSumZ = errorSumZ + ( DesiredZ(i) - z );

      dDesiredZ = ( DesiredZ(i) - DesiredZpre ) / dt;
   DesiredZpre = DesiredZ(i);

   U1 = Kpz*( DesiredZ(i) - z ) + Kdz*( dDesiredZ - dz ) + Kiz*errorSumZ;
   %#######################################################################
   %#######################################################################
   %#######################################################################
   % Desired Phi and Theta

   %disp('before')
   DesiredPhi   = -Uy/g;
   DesiredTheta =  Ux/g;

   %&&&&&&&&&&&&( U2 )&&&&&&&&&&&&&&&&&&
   KpP = 20; KdP = 5; KiP = 0.001;

   errorSumPhi = errorSumPhi + ( DesiredPhi - Phi );

   % Euler Method Derivative
      dDesiredPhi = ( DesiredPhi - DesiredPhipre ) / dt;
   DesiredPhipre  = DesiredPhi;

   U2 = KpP*( DesiredPhi - Phi ) + KdP*( dDesiredPhi - dPhi )  + KiP*errorSumPhi;

   %--------------------------------------
   %&&&&&&&&&&&&( U3 )&&&&&&&&&&&&&&&&&&

   KpT = 90; KdT = 10; KiT = 0.001;
    errorSumTheta = errorSumTheta + ( DesiredTheta - Theta );

   % Euler Method Derivative
      dDesiredTheta = ( DesiredTheta - DesiredThetapre ) / dt;
   DesiredThetapre = DesiredTheta;

   U3 = KpT*( DesiredTheta - Theta ) + KdP*( dDesiredTheta - dTheta ) + KiT*errorSumTheta;
   %--------------------------------------
   %&&&&&&&&&&&&( U4 )&&&&&&&&&&&&&&&&&&
   KpS = 90; KdS = 10; KiS = 0; DesiredPsi = 0; dDesiredPsi = 0;
   U4 = KpS*( DesiredPsi - Psi ) + KdS*( dDesiredPsi - dPsi );

   %###################( ODE Equations of Quadrotor )###################
   ddx = g * Theta;
    dx = dx + ddx*dt;
     x =  x +  dx*dt;
   %=======================================================================
   ddy = -g * Phi;
    dy = dy + ddy*dt;
     y =  y +  dy*dt;
   %=======================================================================
   ddz = (U1/m) - g;
    dz = dz + ddz*dt;
     z =  z +  dz*dt;
   %=======================================================================
   ddPhi = ( L/Jx )*U2;
    dPhi = dPhi + ddPhi*dt;
     Phi =  Phi +  dPhi*dt;
   %=======================================================================
   ddTheta =  ( L/Jy )*U3;
    dTheta =  dTheta + ddTheta*dt;
     Theta =   Theta +  dTheta*dt;
   %=======================================================================
   ddPsi =  (1/Jz)*U4;
    dPsi = dPsi + ddPsi*dt;
     Psi =  Psi +  dPsi*dt;
   %=======================================================================
   %store the erro
   ErrorX(i)   = ( x - DesiredX(i) );
   ErrorY(i)   = ( y - DesiredY(i) );
   ErrorZ(i)   = ( z - DesiredZ(i) );
   ErrorPsi(i)   = ( Psi - 0 );

   X(i) = x;
   Y(i) = y;
   Z(i) = z;

   T(i) = t;

   t = t + dt;

end

Figure1 = figure(1);
set(Figure1,'defaulttextinterpreter','latex');

subplot(2,2,1)
plot(T, ErrorX, 'LineWidth', 2)
title('Error in $x$-axis Position (m)')
xlabel('time (sec)')
ylabel('$x_{d}(t) - x(t)$', 'LineWidth', 2)

subplot(2,2,2)
plot(T, ErrorY, 'LineWidth', 2)
title('Error in $y$-axis Position (m)')
xlabel('time (sec)')
ylabel('$y_{d}(t) - y(t)$', 'LineWidth', 2)

subplot(2,2,3)
plot(T, ErrorZ, 'LineWidth', 2)
title('Error in $z$-axis Position (m)')
xlabel('time (sec)')
ylabel('$z_{d} - z(t)$', 'LineWidth', 2)

subplot(2,2,4)
plot(T, ErrorPsi, 'LineWidth', 2)
title('Error in $\psi$ (m)')
xlabel('time (sec)')
ylabel('$\psi_{d} - \psi(t)$','FontSize',12);
grid on

Figure2 = figure(2);
set(Figure2,'units','normalized','outerposition',[0 0 1 1]);

figure(2)
plot3(X,Y,Z, 'b')
grid on

hold on
plot3(DesiredX, DesiredY, DesiredZ, 'r')

pos = get(Figure2,'Position');
set(Figure2,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3),pos(4)]);
print(Figure2,'output2','-dpdf','-r0');
</code>```

For the trajectory code

```<code>clear all;
clc;

fileID = fopen('xyTrajectory.txt','w');

 angle = -pi;
radius = 3;
z = 0;
t = 0;

for i = 1:6000
    if ( z < 2 )
        z = z + 0.1;
        x = 0;
        y = 0;
    end
    if  ( z >= 2 )
        angle = angle + 0.1;
        angle = wrapToPi(angle);
        x = radius * cos(angle);
        y = radius * sin(angle);
        z = 2;
    end

    X(i) = x;
    Y(i) = y;
    Z(i) = z;

    fprintf(fileID,'%f \t %f \t %f\n',x, y, z);
end

fclose(fileID);
plot3(X,Y,Z)
grid on
</code>```

答:
In your code:

```<code>for i = 1:6000
    if ( z < 2 )
        z = z + 0.1;
        x = 0;
        y = 0;
    end
    if  ( z >= 2 )
        angle = angle + 0.1;
        angle = wrapToPi(angle);
        x = radius * cos(angle);
        y = radius * sin(angle);
        z = 2;
    end

    X(i) = x;
    Y(i) = y;
    Z(i) = z;

    fprintf(fileID,'%f \t %f \t %f\n',x, y, z);
end
</code>```

there is a jump when the altitude reachs 2. The x coordinates goes in only one single step from the value 0 to the value stored in the variable radius.

Please arrange your code and let the generation of a much more soft trajectory in your code, in which you have more interpolated points between the position 0 and the radius. As a general rule, you must not have too much distance between two contiguous points. In the case of a quadrotor the fact that those points too far away are, leads to a very huge pitch and/or roll angle (the quadrotor tries to do its best to reach the next point in a very small time, so it accelerates suddenly by setting a very steep angle). This causes the asin function to not work properly, since the arguments are not anymore in the wanted function's domain ($]-1,1[$)

An example could be:

```<code>for i = 1:6000
    if ( z < 2 )
        z = z + 0.1;
        x = 0;
        y = 0;
    end
    if  ( z >= 2 )
        angle = angle + 0.1;
        angle = wrapToPi(angle);
        x = ( i / 6000 ) * radius * cos(angle);
        y = ( i / 6000 ) * radius * sin(angle);
        z = 2;
    end

    X(i) = x;
    Y(i) = y;
    Z(i) = z;

    fprintf(fileID,'%f \t %f \t %f\n',x, y, z);
end
</code>```

regards

Body:
In your code:

for i = 1:6000
    if ( z < 2 )
        z = z + 0.1;
        x = 0;
        y = 0;
    end
    if  ( z >= 2 )
        angle = angle + 0.1;
        angle = wrapToPi(angle);
        x = radius * cos(angle);
        y = radius * sin(angle);
        z = 2;
    end

    X(i) = x;
    Y(i) = y;
    Z(i) = z;

    fprintf(fileID,'%f \t %f \t %f\n',x, y, z);
end

there is a jump when the altitude reachs 2. The x coordinates goes in only one single step from the value 0 to the value stored in the variable radius.

Please arrange your code and let the generation of a much more soft trajectory in your code, in which you have more interpolated points between the position 0 and the radius. As a general rule, you must not have too much distance between two contiguous points. In the case of a quadrotor the fact that those points too far away are, leads to a very huge pitch and/or roll angle (the quadrotor tries to do its best to reach the next point in a very small time, so it accelerates suddenly by setting a very steep angle). This causes the asin function to not work properly, since the arguments are not anymore in the wanted function's domain ($]-1,1[$)

An example could be:

for i = 1:6000
    if ( z < 2 )
        z = z + 0.1;
        x = 0;
        y = 0;
    end
    if  ( z >= 2 )
        angle = angle + 0.1;
        angle = wrapToPi(angle);
        x = ( i / 6000 ) * radius * cos(angle);
        y = ( i / 6000 ) * radius * sin(angle);
        z = 2;
    end

    X(i) = x;
    Y(i) = y;
    Z(i) = z;

    fprintf(fileID,'%f \t %f \t %f\n',x, y, z);
end

regards


问:
Telemetry with Ardupilot 2.6
I'm using the telemetry kit from 3DR robotics (433MHz) to interface with Ardupilot Mega 2.6, controlling a quadcopter. The Mission Planner (v1.2.84) by Michael Oborne works well with the telemetry kit, transmitting flight data (IMU, compass, GPS etc.) from the quadcopter to the GCS and displaying them in their GUI.

However, I would like to see the same data in the hyperterminal (windows system). The radio receiver on the GCS connects to my PC through a USB drive. I have tried calling the remote radio station using all possible Baud rates, starting from 110 to 921600 (including 57600). I've set the data bits to 8 and stop bits to 1. 'None' for parity and flow control.

However, all that I ever get on my terminal is either gibberish or no data at all. I also tried burning this software to the radio receiver and tried using AT commands on the radio.

It connects OK with '+++', but keeps returning error for AT1, ATT etc.

Please give me an idea about how to get flight data at the hyperterminal.

Thanks.

答:
have a look to my Java Mavlink Library : https://code.google.com/p/mavlinkjava/ It can be used on Android, Windows, Linux and Java RT JVM from IS2T.
Guillaume</p>

ContentLicense: CC BY-SA 3.0

Title: Telemetry with Ardupilot 2.6


问:
PID control of tank-like robot and IMU
Consider a tank like robot with a motor driver channel for each side of the robot (two motors on the left and two motors on the right) and an IMU.
I'm interested in driving the robot in a straight line using the yaw data from the gyro and magnetometer of the IMU, removing the noise caused by slightly different behaving motors, and the possibility to change the desired direction angle. For example some event comes and I want the car to switch the desired direction to +120 degrees and turn while driving.

I'm using Arduino Uno, MinIMU-9 v3 and two DRV8838 Single Brushed DC Motor Driver from Pololu.

Can you please give me some hints and a short pseudo-code example?
Thanks!

答:
Do you have, or can you add encoders to the motors? If you do, I would advise to try to use them as a first step. Encoders allow you to control speed of the motors in terms of their actual rotation speed not PWM. If robot still doesn't drive straight, you can try to apply UMBmark method to correct it. For me it yields very satisfying results, although I have not tried it with tank-link platforms.

If encoders are not the option, you can try IMU. Remember, that it is also subject to errors, so filtering its readings will be crucial here. You can get heading either reading magnetometer directly, or by integrating gyro readings. To use both, you should use Kalman Filter - some reading here, or here.

Then, just insert heading calculated by KF to regular PID controller, so that the bigger is the error between desired and actual heading, the bigger is speed difference between left and right wheels.

Title: PID control of tank-like robot and IMU


问:
Image based 3d position estimation with one camera
There is too many 2d position estimation with one camera. Is there any 3d position estimation application or technique with one camera? If there is no application or technique why?</p>

答:
It is possible to use one camera for 3d position estimation but it is significantly more difficult.

Depth/distance data must be obtained to generate the 3d representation and several techniques have been developed to this end such as rapidly moving the camera to a known, fixed secondary position to capture a second image for processing or mounting a fixed mirror in the camera view to provide the offset image.

However the reliability and complexity of these methods haven't led to their widespread uptake. Arguably the IR CMOS sensor in the Kinect is not a 'camera' and this system uses a 'point cloud' projection and interpretation method to obtain it's depth data.

Overall the simplified two camera approach has historically been the most cost effective and the maths behind the processing algorithms came from sound processing and radar signal interpretation, distance and range finding.


问:
how to implement and code inner and outer PD controllers for quadrotor for position tracking
The quadrotor system is  multi-ODEs equations. The linearized model is usually used especially for position tracking, therefore one can determine the desired x-y positions based on the roll and pitch angles. As a result,  one nested loop which has inner and outer controllers is needed for controlling the quadrotor. For implementation, do I have to put while-loop inside ode45 for the inner attitude controller? I'm asking this because I've read in a paper that the inner attitude controller must run faster (i.e. 1kHz) than the position controller (i.e. 100-200 Hz). In my code, both loops run at 1kHz, therefore inside ode45 there is no while-loop. Is this correct for position tracking? If not, do I have to insert while-loop inside ode45 for running the inner loop? Could you please suggest me a pseudocode for position tracking?

To be more thorough, the dynamics equations of the nonlinear model of the quadrotor is provided  here, if we assume the small angles, the model is reduced to the following equations

$$
\begin{align}
\ddot{x}      &= \frac{U_{1}}{m} ( \theta \cos\psi + \phi \sin\psi) \\
\ddot{y}      &= \frac{U_{1}}{m} ( \theta \sin\psi - \phi \cos\psi) \\
\ddot{z}      &= \frac{U_{1}}{m} - g   \\
\ddot{\phi}   &= \frac{l}{I_{x}} U_{2} \\
\ddot{\theta} &= \frac{l}{I_{y}} U_{3} \\
\ddot{\psi}   &= \frac{1}{I_{z}} U_{4} \\
\end{align}
$$

The aforementioned equations are linear. For position tracking, we need to control $x,y,$ and $z$, therefore we choose the desired roll and pitch (i.e. $\phi^{d} \ \text{and} \ \theta^{d}$)

$$
\begin{align}
\ddot{x}^{d} &= \frac{U_{1}}{m} ( \theta^{d} \cos\psi + \phi^{d} \sin\psi) \\
\ddot{y}^{d} &= \frac{U_{1}}{m} ( \theta^{d} \sin\psi - \phi^{d} \cos\psi) \\
\end{align}
$$

Therefore, the closed form for the desired angles can be obtained as follows

$$
\begin{bmatrix}
\phi_{d} \\
\theta_{d}
\end{bmatrix}
=
\begin{bmatrix}
\sin\psi & \cos\psi \\
-\cos\psi & \sin\psi
\end{bmatrix}^{-1}
\left( \frac{m}{U_{1}}\right)
\begin{bmatrix}
\ddot{x}^{d} \\
\ddot{y}^{d}
\end{bmatrix}
$$

My desired trajectory is shown below

The results are

And the actual trajectory vs the desired one is

My code for this experiment is

```<code>%%
%######################( Position Controller )%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all;
clc;

dt = 0.001;
 t = 0;

% initial values of the system
 x = 0;
dx = 0;
 y = 0;
dy = 0;
 z = 0;
dz = 0;

   Phi = 0;
  dPhi = 0;
 Theta = 0;
dTheta = 0;
   Psi = pi/3;
  dPsi = 0;

%System Parameters:
m  = 0.75;      % mass (Kg)
L  = 0.25;      % arm length (m)
Jx = 0.019688; % inertia seen at the rotation axis. (Kg.m^2)
Jy = 0.019688; % inertia seen at the rotation axis. (Kg.m^2)
Jz = 0.039380; % inertia seen at the rotation axis. (Kg.m^2)
g  = 9.81;      % acceleration due to gravity m/s^2

errorSumX = 0;
errorSumY = 0;
errorSumZ = 0;

errorSumPhi   = 0;
errorSumTheta = 0;

pose = load('xyTrajectory.txt');

DesiredX = pose(:,1);
DesiredY = pose(:,2);
DesiredZ = pose(:,3);

dDesiredX = 0;
dDesiredY = 0;
dDesiredZ = 0;

DesiredXpre = 0;
DesiredYpre = 0;
DesiredZpre = 0;

dDesiredPhi = 0;
dDesiredTheta = 0;
DesiredPhipre = 0;
DesiredThetapre = 0;

for i = 1:6000

   % torque input
   %&&&&&&&&&&&&( Ux )&&&&&&&&&&&&&&&&&&
   Kpx = 50; Kdx = 8; Kix = 0;
   Ux = Kpx*( DesiredX(i) - x  ) + Kdx*( dDesiredX - dx ) + Kix*errorSumX;

   errorSumX = errorSumX + ( DesiredX(i) - x );

     dDesiredX = ( DesiredX(i) - DesiredXpre ) / dt;
   DesiredXpre = DesiredX(i);

   %&&&&&&&&&&&&( Uy )&&&&&&&&&&&&&&&&&&
   Kpy = 100; Kdy = 10; Kiy = 0;
   Uy = Kpy*( DesiredY(i) - y  ) + Kdy*( dDesiredY - dy ) + Kiy*errorSumY;

   errorSumY = errorSumY + ( DesiredY(i) - y );

     dDesiredY = ( DesiredY(i) - DesiredYpre ) / dt;
   DesiredYpre = DesiredY(i);

   %&&&&&&&&&&&&( U1 )&&&&&&&&&&&&&&&&&&
   Kpz = 100; Kdz = 20; Kiz = 0;
   U1 = Kpz*( DesiredZ(i) - z ) + Kdz*( dDesiredZ - dz ) + Kiz*errorSumZ;

   errorSumZ = errorSumZ + ( DesiredZ(i) - z );

      dDesiredZ = ( DesiredZ(i) - DesiredZpre ) / dt;
   DesiredZpre = DesiredZ(i);

   %#######################################################################
   %#######################################################################
   %#######################################################################
   % Desired Phi and Theta
   R = [  sin(Psi),cos(Psi);
         -cos(Psi),sin(Psi)];

   DAngles = R\( (m/U1)*[Ux; Uy]);

   %Wrap angles
   DesiredPhi   = wrapToPi( DAngles(1) ) /2;
   DesiredTheta = wrapToPi( DAngles(2) );

   %&&&&&&&&&&&&( U2 )&&&&&&&&&&&&&&&&&&
   KpP = 100; KdP = 10; KiP = 0;
   U2 = KpP*( DesiredPhi - Phi ) + KdP*( dDesiredPhi - dPhi )  + KiP*errorSumPhi;

   errorSumPhi = errorSumPhi + ( DesiredPhi - Phi );

     dDesiredPhi = ( DesiredPhi - DesiredPhipre ) / dt;
   DesiredPhipre = DesiredPhi;

   %--------------------------------------
   %&&&&&&&&&&&&( U3 )&&&&&&&&&&&&&&&&&&

   KpT = 100; KdT = 10; KiT = 0;
   U3 = KpT*( DesiredTheta - Theta ) + KdP*( dDesiredTheta - dTheta ) + KiT*errorSumTheta;

    errorSumTheta = errorSumTheta + ( DesiredTheta - Theta );

     dDesiredTheta = ( DesiredTheta - DesiredThetapre ) / dt;
   DesiredThetapre = DesiredTheta;

   %--------------------------------------
   %&&&&&&&&&&&&( U4 )&&&&&&&&&&&&&&&&&&
   KpS = 80; KdS = 20.0; KiS = 0.08;
   U4 = KpS*( 0 - Psi ) + KdS*( 0 - dPsi );

   %###################( ODE Equations of Quadrotor )###################
   %===================( X )=====================
   ddx = (U1/m)*( Theta*cos(Psi) + Phi*sin(Psi) );

    dx = dx + ddx*dt;
     x =  x +  dx*dt;
   %===================( Y )=====================
   ddy = (U1/m)*( Theta*sin(Psi) - Phi*cos(Psi) );

    dy = dy + ddy*dt;
     y =  y +  dy*dt;

   %===================( Z )=====================
   ddz = (U1/m) - g;

    dz = dz + ddz*dt;
     z =  z +  dz*dt;

   %===================( Phi )=====================
   ddPhi = ( L/Jx )*U2;

    dPhi = dPhi + ddPhi*dt;
     Phi =  Phi +  dPhi*dt;

   %===================( Theta )=====================
   ddTheta =  ( L/Jy )*U3;

    dTheta =  dTheta + ddTheta*dt;
     Theta =   Theta +  dTheta*dt;

   %===================( Psi )=====================
   ddPsi =  (1/Jz)*U4;

    dPsi = dPsi + ddPsi*dt;
     Psi =  Psi +  dPsi*dt;

   %store the erro
     ErrorX(i)   = ( x - DesiredX(i) );
     ErrorY(i)   = ( y - DesiredY(i) );
     ErrorZ(i)   = ( z - DesiredZ(i) );
%    ErrorPhi(i)   = ( Phi - pi/4 );
%    ErrorTheta(i) = ( Theta - pi/4 );
     ErrorPsi(i)   = ( Psi - 0 );

   X(i) = x;
   Y(i) = y;
   Z(i) = z;

   T(i) = t;

%    drawnow
%    plot3(DesiredX, DesiredY, DesiredZ, 'r')
%    hold on
%    plot3(X, Y, Z, 'b')
   t = t + dt;

end

Figure1 = figure(1);
set(Figure1,'defaulttextinterpreter','latex');
%set(Figure1,'units','normalized','outerposition',[0 0 1 1]);

subplot(2,2,1)
plot(T, ErrorX, 'LineWidth', 2)
title('Error in $x$-axis Position (m)')
xlabel('time (sec)')
ylabel('$x_{d}(t) - x(t)$', 'LineWidth', 2)

subplot(2,2,2)
plot(T, ErrorY, 'LineWidth', 2)
title('Error in $y$-axis Position (m)')
xlabel('time (sec)')
ylabel('$y_{d}(t) - y(t)$', 'LineWidth', 2)

subplot(2,2,3)
plot(T, ErrorZ, 'LineWidth', 2)
title('Error in $z$-axis Position (m)')
xlabel('time (sec)')
ylabel('$z_{d} - z(t)$', 'LineWidth', 2)

subplot(2,2,4)
plot(T, ErrorPsi, 'LineWidth', 2)
title('Error in $\psi$ (m)')
xlabel('time (sec)')
ylabel('$\psi_{d} - \psi(t)$','FontSize',12);
grid on

Figure2 = figure(2);
set(Figure2,'units','normalized','outerposition',[0 0 1 1]);

figure(2)
plot3(X,Y,Z, 'b')
grid on

hold on
plot3(DesiredX, DesiredY, DesiredZ, 'r')

pos = get(Figure2,'Position');
set(Figure2,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3),pos(4)]);
print(Figure2,'output2','-dpdf','-r0');

legend('actual', 'desired')
</code>```

The code of the desired trajectory is

```<code>clear all; clc;

fileID = fopen('xyTrajectory.txt','w');

 angle = -pi; radius = 5; z = 0; t = 0;

for i = 1:6000
    if ( z < 2 )
        z = z + 0.1;
        x = 0;
        y = 0;
    end
    if  ( z >= 2 )
        angle = angle + 0.1;
        angle = wrapToPi(angle);
        x = radius * cos(angle);
        y = radius * sin(angle);
        z = 2;
    end

    X(i) = x;
    Y(i) = y;
    Z(i) = z;

    fprintf(fileID,'%f \t %f \t %f\n',x, y, z); end

fclose(fileID); plot3(X,Y,Z) grid on
</code>```

答:
As extension to this answer, I'd like to share with the community a Simulink model implementing the system described above. It does not represent a solution for the code reported by CroCo but it may give a better insight and, as such, it might be helpful.

The system is depicted below:

In red we have the blocks running at $1\,\text{KHz}$, which deal with the attitude control, whereas in green are shown those blocks that allow tracking the linear displacement and run at $100\,\text{Hz}$.

As side note, since we have to ensure that the "small angles" assumption remains valid throughout the simulation, the take-off phase (i.e. reaching the desired height $z_d$) needs to be almost complete before commencing the displacement tracking of the $x_d\left(t\right)$ and $y_d\left(t\right)$ time-varying linear coordinates (a circle of radius $5\,\text{m}$ in the model). Further, for the same reason, the desired circular trajectory must start from the quadcopter's initial position $\left(x\left(0\right),y\left(0\right)\right)$.

Depending on the controllers' gains, we can get quite nice tracking results:

The first top three diagrams shows the $x$, $y$ and $z$ components (green), respectively, while following the targets (blue). The last plot reports on the roll (blue), pitch (green) and yaw (red). Time is expressed in seconds, displacements are in meters and angles are in degrees.


问:
Comparing maps to groundtruth
When you've created a map with a SLAM implementation and you have some groundtruth data, what is the best way to determine the accuracy of that map?  </p>

<p>My first thought is to use the Euclidean distance between the map and groundtruth. Is there some other measure that would be better?  I'm wondering if it's also possible to take into account the covariance of the map estimate in this comparison. </p>

答:
Assuming the map is a point cloud and that you know the alignment between the ground truth data and the map then calculating the mean squared error (MSE) would give you a relative understanding of the accuracy. A lower MSE would indicate they are very similar, 0 of course mean identical, and a high MSE would idicate they are very different.

If you do not know the alignment between the ground truth and the map then you could use expectation maximization over the alignments to find the best fit and then use MSE.

Title: Comparing maps to groundtruth


答:
One camera gives you no depth information, so you have to have some information about the scene before you start (a priori).

The most common way to handle this is with structured light, where you project a known pattern and evaluate deviations from the pattern.

Title: Image based 3d position estimation with one camera

Body: There is too many 2d position estimation with one camera. Is there any 3d position estimation application or technique with one camera? If there is no application or technique why?</p>


答: There is one really simple solution: take a snapshot of the scene, move to the side a few centimetres and take another snapshot. Alternatively, move forward with the camera pointed to one side. Now you have two images and can process them as though you have two cameras or one stereoscopic pair. The only problem I can see with this method is that any parts of the scene that have moved will cause some issues, but it probably wouldn't be too hard to filter those parts of the scene out.</p>


问:
Why do my ESCs stop working?
I'm new to robotics and this is my first time building a quadcopter.  I'm unable to work out why I keep losing ESCs.

Most recently in testing, I've managed to calibrate all 4 ESCs and accurately control the speed of all 4 motors.  But after neatly securing them to the frame, 1 motor didn't work.  I recalibrated the ESCs again and, when running them again, the motor still didn't work.  However, the other 3 motors continued to run at first, but also suddenly just stopped altogether.

Research suggested that ESCs have a cut-off voltage, indicating that my battery might be too flat, so I immediately looked to recharging it. To my surprise, the (still very new) battery appeared to have bulged out, indicating that it had been damaged.

Further research suggested that the size of the battery I was using is insufficient for the amount of current drawn by the motors.  So, without any PWM applied, I reconnected a new fully charged battery in the hope of listening for any beeps to diagnose, and one ESC immediately coughed up a huge puff of smoke.

Before all of this happened, I only managed to get 2 of the ESCs to run their motors.  Despite several attempts at tweaking PWM signals and calibrating them, I ended up replacing the other 2.

Unless there's some obvious reason for my ESCs to keep dying on me, I can only assume that these specific ESCs are badly made and I should ask for my money back.

These are the components I'm using:

Raspberry Pi 2 Model B

Adafruit 16-Channel 12-bit PWM/Servo Driver - I2C interface - PCA9685

RCTimer Mini ESC 40A OPTO BLHeli Firmware (Oneshot125, Support 2-6S)

RCTimer 2208-8 2600kV Outrunner Brushless Motor

Gens ace 2200mAh 11.1V 25C 3S1P Lipo Battery Pack

The Raspberry Pi is powered through its micro USB interface by a 5V Step-Up Voltage Regulator connected to a 5000mAh 3.7V LiPo battery.  The PWM Controller is powered to its Vcc pin by the GPIO1 (3.3V) pin from the Raspberry Pi, that also happens to power other sensors.

At the time (when all 4 motors worked), I was able to accurately control them at either 50Hz or 400Hz with 1-2 millisecond duty cycles.

答:
It doesn't seem like those motors are well matched to your battery. They recommend a 2or3 cell LiPo (7.2V or 11.1V) while you are using a 1 cell LiPo (3.6V). I think you'll find most motors will recommend 2 or 3 cells. With only 1 cell you won't likely get enough lift for a multirotor.

Also as a side note, your ESCs should be set to have no cut-off when using it on a multirotor. If your ESCs cut a motor off it will result in a crash, guaranteed. Multirotor ESCs usually disable any sort of cut-off (ie. set the cut-off voltage to 0V).

Title: Why do my ESCs stop working?


问:
Help with code that is supposed to drive a servo controller
I posted a similar question before as I was just getting started with the project but I wasn't specific enough so got a weak response from the SE community. But now I am at a point where I have Python code which is supposed to rotate a servo through Pololu's Maestro Serial servo controller (https://www.pololu.com/product/207). Based on the "Serial Interface" section of the user's guide (https://www.pololu.com/product/207/resources), I sent a sequence of numbers starting with decimal 170 and 12, which are the "first command byte" and the "device number data byte", respectively. The user guide says that 12 is the default device number, so I tried changing it to 18 because that's how many servos my servo controller can drive. But that doesn't make much difference because the servo doesn't rotate at all. The numbers after that are the same as the example from the user's guide. I am not sure what the 4, 112 and 46 are doing, but the 0 targets the servo port "0" on the servo controller (the port to which my servo is connected). The servo doesn't move, regardless of what sequence of numbers I put in. I have very little experience, so if you guys could point me in the right direction or at least point to some useful resources on the web, I'd be very grateful.</p>

```<code>import serial
import struct
import time
import Adafruit_BBIO.UART as UART

drive_Motor_Port = serial.Serial(port = "/dev/ttyO1", baudrate=9600)
drive_Motor_Port.close()
drive_Motor_Port.open()

drive_Motor_Port.write(chr(170)); # User guide says we must start with OxAA = 170
drive_Motor_Port.write(chr(18)); # Device number
drive_Motor_Port.write(chr(04));
drive_Motor_Port.write(chr(00));
drive_Motor_Port.write(chr(112));
drive_Motor_Port.write(chr(46));

time.sleep(5);
</code>```

答:
Have you figured out yet that these two devices use incompatible protocols?:

Pololu Mini Maestro 18-Channel USB Servo Controller https://www.pololu.com/product/1354

Pololu 8-channel Micro Serial Servo Controller https://www.pololu.com/product/207

I'm guessing that you're using a Pololu Mini Maestro 18-Channel USB Servo Controller, connected to UART1 of a Beaglebone Black,
following the instructions at
"Setting up IO Python Library on BeagleBone Black: UART".

If I were you, here are the first few things I'd do to debug the problem of "servo doesn't move":

I would print out the pinout and pin labels of the hardware on paper -- see "Mini Maestro Pinout and Components"

Is the ground of the Maestro connected to the ground of the BeagleBone?

To rule out power interference, does it work any better when I hook it up to a test jig with an independent, overkill power supply for each part?

5V, at least 2A, power to the BeagleBone barrel connector

5V to 16 V, at least 500 mA, power to the Pololu Maestro (see "Powering the Maestro")

5V to 16 V, at least 2A, power to the servos (check servo datasheet for details)

(In particular, trying to run all 3 using the power from a single laptop USB port is unlikely to work).

See "Powering the Maestro"

Check power between VCC and GND on the Maestro board; between VCC and GND on the 3-wire servo cable, and between VCC and GND on the Beaglebone -- ideally check that voltage with an o'scope while the test program is running, but even a multimeter is enough to diagnose some problems.

Are the LEDs blinking the way that the "Indicator LEDs" section of the manual says they should?

I might consider disconnecting the Beaglebone entirely, and trying to get an Arduino to control the Maestro over the "serial" pins -- see "Arduino library". That would at least distinguish between a problem in the Beaglebone software vs a problem in the power supply hardware.

Next I would try disconnecting the Beaglebone entirely, and seeing if I could get a servo to turn with the Maestro Control Center running on my Linux laptop (or a Windows laptop or a Mac OSX laptop), as described in "Installing Linux Drivers and Software".

If I could get a laptop to make the servos turn with USB control, I would use that laptop with USB control to reset the Maestro to the default settings, as described in "Using the Maestro without USB".

After getting a servo to move while that servo is connected to a Maestro which is in turn connected to a USB cable to a laptop, and resetting everything to the default settings, I would disconnect the USB cable and re-connect the GND and other pins between the Maestro and the Beaglebone -- see "Connecting to a Microcontroller".

Then I would try to make a program on the Beaglebone as close as possible to the example code ( PIC18F4550, etc. ):

```<code>import serial
import struct
import time
import Adafruit_BBIO.UART as UART
# warning: untested code
def main():
    drive_Motor_Port = serial.Serial(port = "/dev/ttyO1", baudrate=9600)
    drive_Motor_Port.close()
    drive_Motor_Port.open()
    while True:
        # move to position 1000 us (typical minimum for servos)
        drive_Motor_Port.write(chr(0xAA)) # User guide says we must start with OxAA.
        drive_Motor_Port.write(chr(0x0c)) # "The Pololu Protocol device number is 12" -- https://www.pololu.com/docs/0J40/3.c
        drive_Motor_Port.write(chr(0x04)) # command 4: set target
        drive_Motor_Port.write(chr(0x00)) # channel
        # target position 0x1f20 = 1000 us
        drive_Motor_Port.write(chr(0x20))
        drive_Motor_Port.write(chr(0x1f))
        time.sleep(2)
        #
        # move to position 1500 us (typical neutral for servos)
        drive_Motor_Port.write(chr(0xAA)) # User guide says we must start with OxAA.
        drive_Motor_Port.write(chr(0x0c)) # "The Pololu Protocol device number is 12" -- https://www.pololu.com/docs/0J40/3.c
        drive_Motor_Port.write(chr(0x04)) # command 4: set target
        drive_Motor_Port.write(chr(0x00)) # channel
        # target position 0x2e70 = 1000 us
        drive_Motor_Port.write(chr(0x70))
        drive_Motor_Port.write(chr(0x2e))
        time.sleep(2)

main()
</code>```

Body:
Have you figured out yet that these two devices use incompatible protocols?:

Pololu Mini Maestro 18-Channel USB Servo Controller https://www.pololu.com/product/1354

Pololu 8-channel Micro Serial Servo Controller https://www.pololu.com/product/207

I'm guessing that you're using a Pololu Mini Maestro 18-Channel USB Servo Controller, connected to UART1 of a Beaglebone Black,
following the instructions at
"Setting up IO Python Library on BeagleBone Black: UART".

If I were you, here are the first few things I'd do to debug the problem of "servo doesn't move":

I would print out the pinout and pin labels of the hardware on paper -- see "Mini Maestro Pinout and Components"

Is the ground of the Maestro connected to the ground of the BeagleBone?

To rule out power interference, does it work any better when I hook it up to a test jig with an independent, overkill power supply for each part?

5V, at least 2A, power to the BeagleBone barrel connector

5V to 16 V, at least 500 mA, power to the Pololu Maestro (see "Powering the Maestro")

5V to 16 V, at least 2A, power to the servos (check servo datasheet for details)

(In particular, trying to run all 3 using the power from a single laptop USB port is unlikely to work).

See "Powering the Maestro"

Check power between VCC and GND on the Maestro board; between VCC and GND on the 3-wire servo cable, and between VCC and GND on the Beaglebone -- ideally check that voltage with an o'scope while the test program is running, but even a multimeter is enough to diagnose some problems.

Are the LEDs blinking the way that the "Indicator LEDs" section of the manual says they should?

I might consider disconnecting the Beaglebone entirely, and trying to get an Arduino to control the Maestro over the "serial" pins -- see "Arduino library". That would at least distinguish between a problem in the Beaglebone software vs a problem in the power supply hardware.

Next I would try disconnecting the Beaglebone entirely, and seeing if I could get a servo to turn with the Maestro Control Center running on my Linux laptop (or a Windows laptop or a Mac OSX laptop), as described in "Installing Linux Drivers and Software".

If I could get a laptop to make the servos turn with USB control, I would use that laptop with USB control to reset the Maestro to the default settings, as described in "Using the Maestro without USB".

After getting a servo to move while that servo is connected to a Maestro which is in turn connected to a USB cable to a laptop, and resetting everything to the default settings, I would disconnect the USB cable and re-connect the GND and other pins between the Maestro and the Beaglebone -- see "Connecting to a Microcontroller".

Then I would try to make a program on the Beaglebone as close as possible to the example code ( PIC18F4550, etc. ):

import serial
import struct
import time
import Adafruit_BBIO.UART as UART
# warning: untested code
def main():
    drive_Motor_Port = serial.Serial(port = "/dev/ttyO1", baudrate=9600)
    drive_Motor_Port.close()
    drive_Motor_Port.open()
    while True:
        # move to position 1000 us (typical minimum for servos)
        drive_Motor_Port.write(chr(0xAA)) # User guide says we must start with OxAA.
        drive_Motor_Port.write(chr(0x0c)) # "The Pololu Protocol device number is 12" -- https://www.pololu.com/docs/0J40/3.c
        drive_Motor_Port.write(chr(0x04)) # command 4: set target
        drive_Motor_Port.write(chr(0x00)) # channel
        # target position 0x1f20 = 1000 us
        drive_Motor_Port.write(chr(0x20))
        drive_Motor_Port.write(chr(0x1f))
        time.sleep(2)
        #
        # move to position 1500 us (typical neutral for servos)
        drive_Motor_Port.write(chr(0xAA)) # User guide says we must start with OxAA.
        drive_Motor_Port.write(chr(0x0c)) # "The Pololu Protocol device number is 12" -- https://www.pololu.com/docs/0J40/3.c
        drive_Motor_Port.write(chr(0x04)) # command 4: set target
        drive_Motor_Port.write(chr(0x00)) # channel
        # target position 0x2e70 = 1000 us
        drive_Motor_Port.write(chr(0x70))
        drive_Motor_Port.write(chr(0x2e))
        time.sleep(2)

main()


问:
Arduino triggers a camera to start recording
I've already made an Arduino device which detects the trigger event, but now I want it to trigger the recording and storage of video when this event occurs.  If the camera could be wirelessly triggered a few feet away from the Arduino unit, that would be optimal, but I can settle for running wires if need be.

I'm looking for suggestions because I'm on a limited budget for this project. I want to avoid reinventing the wheel and ordering parts which I can't get to work with an Arduino.

I'm considering the use of this camera.
https://www.sparkfun.com/products/11418

This is my first Arduino project.  Any help is very welcome.

答:
Sticking with SparkFun, here are some fairly east to use, fairly cheep parts that can help accomplish what I believe you are trying to do.

It would appear as if triggering the camera is a simple as a digital pulse from the Arduino - next step is to figure out when to make that pulse.

If you are looking to build a remote control device, might I suggest something as simple as SF's 434MHz transmitter / receiver pair?

Transmitter

Receiver

The transmitter would be your remote control device, which would send a broadcast on button press. The receiver on the camera wold pick up this broadcast, and trigger the camera.

Now these modules are serial communication, so you would need an Arduino on the remote to command the transmitter - but you could easily build your own small remote for pretty cheap if you opt to buy just an ATMEGA328, the processor of the Arduino.


答: take an image than take with little zoom out. crop two specific points from first and match (tepmlate)from second. than calculate difference this gives relation between two images than you can compute depth with camera angle.</p>


问:
Mechanical design for base of robotic arm
I am using DC motors to build a robotic arm.  I want to make the base shoulder (which rotates and lifts) more stable and stronger.  How should I design this using DC motors?

Also I would like to put the motor for the elbow in the base for efficiency.  Which design best suits this?

UPDATE
I am building a robotic arm for a payload of approx. 1-2 kg and using DC high torque motors. In this model, I am using only a shoulder with a gripper. The gripper is self made by me weighing approximately 400 grams. I want to have a proper design and material choice so that the shoulder part remains less heavy and more stable.

In addition to this I want to operate the movement of the gripper, i.e. the up and down motion, by using the motor in the base part. What should be my design and better alternative?

答:
We built a robot arm similar to the model in the following link.

http://www.3ders.org/articles/20131113-make-your-own-3d-printed-trainable-robotic-arm.html

We used 3D-printer to print all the links so the link mass is small but strong enough. BTW we only used cheap servos but the arm moved not bad so I think the high torque motors are okay. I suggest you start with a 3 DOF arm and if you want any design reference you can search on grabcad.com. There are plenty of great design models. Have fun!


问:
At what frequency should I input and read values?
I am at the moment trying to identify a system using frequency sweep. I have been using Mathematica and have created a frequency sweep as such.

```<code>g[t_] := 0.799760*Sin[2 Pi (3 t/333.3 + 1) t];
Plot[g[t], {t, 0, 1000000000000000000000000000000}]
g[#] & /@ Range[0, 5, 0.001]
</code>```

The max frequency is 10 Hz, I sample the data using 1000 Hz. But at what rate should I input it to the system, and what rate should I read from it?

答:
Nyquist sampling theory says you are required to sample at least 2x the highest frequency of interest in order to prevent aliasing.

You want to sweep (chirp) the system, so for inputs to the system you want to provide the samples at a rate greater than or equal to twice the highest frequency of interest. For your sweep the highest frequency is 10Hz, so you have to sample at least 20Hz, but some extra margin is generally desirable, so I would go for 25Hz.

Regarding outputs, the sampling rate is a different story. You haven't said what the system is or what you're expecting, so nobody here can comment on an appropriate output sampling rate.

For example, your input could be a PWM signal to a servo - input needs to be very fast, but the output (servo motion) is generally very slow by comparison. Or you could be sending a sync to a high speed process, where the process output is happening significantly faster than the input. In terms of actuators, two motors of otherwise similar construction but with different numbers of poles will operate at different speeds given the same inputs. Same goes for gearing, etc. etc. You have to identify a frequency of interest in the output to know what to choose as a sampling rate.


问:
Interfacing GPU image processing with motor control at 30+Hz
I would like to make a robotic system which takes as input a video feed, runs some GPU-based image recognition on the video, and outputs commands to a set of motors. The goal is to have the motors react to the video with as little latency as possible, hopefully of the order of 10s of ms. Currently I have a GTX 770m on a laptop running Ubuntu 14.04, which is connected to the camera and doing the heavy image processing. This takes frames at 30Hz and will output motor commands at the same frequency.

After a few days of looking around on the web for how to design such a system, I'm still at a loose end whether (a) it is even feasible (b) if so, what the best approach is to interface the laptop with the motors? The image processing must run on Linux, so there is no leeway to change that part of things.

答: So I mainly use a ZED camera with a Nvidia K1, using the provided plugins for OpenCV GPU compute and I get a processing time of around 16-20ms for low res images (e.g. 640x480, or 1024x768) and use this to control a flight controller, using a uart interface, I assume your motors take pwm input, as for controlling the motors I would suggest somthing like the polou maestro to control them. Can you give more information on your application? why do you have these image processing requirements?</p>


问:
idea for web application in robotics
I am learning and I am interested robotics, but also I need to update my web development skills so the question is - is there any idea for good web application that could be connected with robotics - service robots, industrial robots etc. Maybe there already is some open source ongoing web application projects for robotics in which I can make contribution.

Thanks!

答: web based robotic simulator will be a good project. You can do simulators like gazebo and morse </p>

Title: idea for web application in robotics


问:
What is the difference between Multiple robots and swarm robots?
What is the difference between Multiple robots and swarm robots? What is the key point? Also what is multi agent systems? Do multi agent systems works only for computer simulations or games? These terms are used similar applications.</p>

答:
These concepts are similar in the sense that they require multiple robots that communicate/cooperate. Apart from that, their application, and thus their design and implementation differ.

Swarm Robots

Swarm robots are designed after ants, bees and such creatures. The idea therefore is not just about multiple robots cooperating, but it's about many robots, each of which is very simple.  With swarm robots, you would expect each robot to be too stupid to possibly even know why it's doing something (i.e., what is the ultimate goal).  Their simple function however, would result in a complex algorithm when a bunch of robots are doing it.

To recap, with swarm robots, each robot is unable to do anything meaningful. The desired behavior emerges only when there are many of them. Usually, the robots all look the same.

Multi-Agent Systems

In multi-agent systems, you have multiple agents, which could be either computer programs or robots, each of which is able to do some meaningful part of a task.  It is possible that without having all the agents, the ultimate goal cannot be reached, but parts of it perhaps could still be done.  Think of multi-agent systems as a society of humans. Each of us have a profession and we thrive together, but we are not useless on our own.

To recap, a multi-agent system consists of entities (e.g., robots) that are each specialized for a certain task. They cooperate to achieve the ultimate goal, but individually they are also able to do some things.

Multiple Unrelated Robots

You mentioned multiple robots. That's not something that relates to a specific methodology (as opposed to e.g., swarm robots). I'm going to thus talk about multiple unrelated robots.

It is not unthinkable to imagine unrelated robots cooperating at some point in time. In such a case, each robot has its own goal, or is part of its own swarm, or is a member of its own multi-agent system, etc. Based on circumstances, the robots may meet and they may interact. One simple case is the robots avoiding crash while each goes on about its business.

In this case, there is really no common algorithm, design, methodology or whatever. It's just individual robots who happen to meet.


问:
Suggestion for a camera
Are there good low cost cameras that are frequently used in robotics?

I am assuming there are cameras that are good fit for robotics ...

Works well with OpenCV

PC Windows support - USB2/USB3 (GigE, USB3 vision cameras seem pricey)

Good image sensing performance

Adjustable focus - manual or motorized (fine focus control would be great)

Do IP cameras make good cameras for Robotic vision projects?

答: I personally love the ZED as I do a lot of work with stereo vision and come with opencv modules built for them, but they are quite pricy, I find that most higher end webcams work great with openCV, IP cams are a poor choice in my experience with unpredictable latency (you may not care about this), built in compression which can mess with openCV and they are much more expensive than a comparable webcam.</p>

Title: Suggestion for a camera


答:
In addition to @Shahbaz

According to this book

Multiple Mobile Robot Systems is main topic and swarm robotics is a sub topic
both of them motivated from

the task complexity is too high for a single robot to accomplish

the task is inherently distributed

building several resource-bounded robots is much easier than having a single powerful robot

multiple robots can solve problems faster using parallelism

the introduction of multiple robots increases robustness through redundancy

Swarm Robots: Historically, some of the earliest work in multirobot
systems dealt with large numbers of homogeneous robots, called swarms.

This means multi robot systems can be homogeneous and heteregeneous but swarm systems have to homegeneous.

Multi agent systems is a computerized systems composed of multiple interacting intelligent agents within an environment. But also it can be robot.

Body: What is the difference between Multiple robots and swarm robots? What is the key point? Also what is multi agent systems? Do multi agent systems works only for computer simulations or games? These terms are used similar applications.</p>


问:
How many quadcopters would it take to lift a burrito?
I am investigating a possible business opportunity in which quadcopters perform high-precision nutritional delivery via a burrito medium. I have never used a burrito, but I have read on the internet that they typically weigh 600-700 grams (1). This is much too heavy for commercially available platforms.

How many quadcopters would it take to lift a single burrito?

(1): https://www.facebook.com/chipotle/posts/390817319252

答:
Almost all 300 class quadcopters (300mm diagonally from motor centre to motor centre) will be able to accomplish this.  For a quadcopter to lift more you will want bigger props, and lower KV motors.   Bigger props will allow better flight times and more thrust, but will decrease the responsiveness of the craft.  This is because it is harder to spin up and slow down bigger props, this can be fix by using a 3S mutirotor with CP (collective pitch) rotors.

Here is a quadcopter that is for aerial photography, it is designed to life a relatively heavy DSLR camera and gimbal.

Here is a quadcopter that is designed for FPV (First Person View) racing, only designed to hold small cameras.


问:
Quadcopter - is iPhone the ultimate flight controller?
iPhone contains

Gyroscope

GPS

Two photo and video cameras

Self-sufficient battery that outlives the motor battery

Wifi

Backup connectivity (cellular, bluetooth)

Programmable computer

Real-time image processing capabilities and face detection

General purpose IO (with something like this)

and old models are available very cheap.

What is the main benefit of having a separate dedicated flight controller and camera on hobbyist rotorcraft rather than a general purpose device like the iPhone?

答:
Using a smart-phone may have advantages, but anything that can be accomplished using other traditional flight controllers.

Some advantages

Easier to implement

Relatively cheap

Relatively user-friendly

Usually a built in way to recover the craft in the event of a flyaway (caused by what is doing the flight controls)

Some disadvantages

Limited to the operating system's abilities and quarks

Without rooting the device you may not be able to kill a process that is not responding

Not as customizable as using an opens-ource flight controller

latency caused by running a full operating system

Smart-phones are fragile and ca

Here is a list of common flight controllers and their capabilities.


问:
Inter-processor communication for robotic arm
I'm building a hobby 6-DOF robotic arm and am wondering what the best way is to communicate between the processors (3-4 AVRs, 18 inches max separation). I'd like to have the control loop run on the computer, which sends commands to the microprocessors via an Atmega32u4 USB-to-??? bridge.

Some ideas I'm considering:

RS485

Pros: all processors on same wire, differential signal more robust

Cons: requires additional chips, need to write (or find?) protocol to prevent processors from transmitting at the same time

UART loop (ie, TX of one processor is connected to RX of next)

Pros: simple firmware, processors have UART built in

Cons: last connection has to travel length of robot, each processor has to spend cycles retransmitting messages

CANbus (I know very little about this)

My main considerations are hardware and firmware complexity, performance, and price (I can't buy an expensive out-of-box system).

答:
You want to use USB for communications with the computer. If you have a number of microcontrollers, you will probably only connect one of the microcontrollers directly to the computer. The other microcontrollers will need to get their commands from the main microcontroller.

The communication you choose will depend on a number of factors:

required bandwidth (we will assume you are running them at 16MHz)

complexity (wiring and coding)

bi-directional, or master-slave

Almost all options have built-in support on the AVR microcontroller. There is no option you might reasonably prefer over the built-in options which would require additional hardware. Because they have built-in support, the software complexity is all similar, in that you just configure the port (using registers), put the data to transmit in another register, then trigger the transmission by setting a bit in another register. Any data received is found in another register, and an interrupt is triggered so you can handle it. Whichever option you choose, the only difference is the change in register locations, and some changes to the configuration registers.

A USART loop has the following features:

Maximum baud rate of CLK/16 = 1MHz (at 16MHz clock) which is a transfer rate of around 90KB/s

fully bi-directional communications (no master or slave designation)

requires separate wires between each pair of microcontrollers - the Atmega32u4 supports two USART ports natively, limiting the number of microcontrollers you can connect in a network in practice (or else you end up with a long string of microcontrollers - ie. connected in a linked list manner)

Note: this is also what you would use to get RS232 communication, except that because RS232 requires 10V, it requires a driver to get those voltage levels. For communication between microcontrollers, this is not useful (only voltage levels are changed).

RS485:

Essentially, you use the USART port in a different mode - there is no advantage in bandwidth, and it may only simplify the wiring slightly, but it also complicates it. This is not recommended.

Two-wire interface:

This is also referred to as I2C. This means that all devices share the same two wires.

You need a pull-up resistor on both wires

It is slow (because the pull-up resistors are limited in value, and there is increasing capacitance as the number of devices increases, and the wire length increases). For this AVR microcontroller, the speed is up to 400 kHz - slower than USART (but this speed depends on limiting your capacitance). The reason is that although a device drives the data wire low, the opposite transition is accomplished by letting the wire float high again (the pull-up resistor).

It is even slower when you consider that ALL communication shares the same limited bandwidth. Because all communication shares the same limited bandwidth, there may be delays in communication where data must wait until the network is idle before it can be sent. If other data is constantly being sent, it may also block the data from ever being sent.

It does rely on a master-slave protocol, where a master addresses a slave, then sends a command/request, and the slave replies afterwards. Only one device can communicate at a time, so the slave must wait for the master to finish.

Any device can act as both a master and/or a slave, making it quite flexible.

SPI

This is what I would recommend/use for general communication between microcontrollers.

It is high speed - up to CLK/2 = 8MHz (for CLK at 16MHz), making it the fastest method. This is achievable because of its separate wire solely for the clock.

The MOSI, MISO data, and SCK clock wires are shared across the whole network, which means it has simpler wiring.

It is a master-slave format, but any device can be a master and/or slave. However, because of the slave select complications, for shared wiring (within the network), you should only use it in a hierarchical manner (unlike the two-wire interface). IE. if you organise all devices into a tree, a device should only be master to its children, and only a slave to its parent. That means that in slave mode, a device will always have the same master. Also, to do this correctly, you need to add resistors to MISO/MOSI/SCK to the upstream master, so that if the device is communicating downstream (when not selected as a slave), the communications will not affect communications in other parts of the network (note the number of levels you can do this using resistors is limited, see below for better solution using both SPI ports).

The AVR microcontroller can automatically tri-state the MOSI signal when it is slave-selected, and switch to slave mode (if in master).

Even though it might require a hierarchical network, most networks can be organised in a tree-like manner, so it is usually not an important limitation

The above can be relaxed slightly, because each AVR microcontroller supports two separate SPI ports, so each device can have different positions in two different networks.

Having said this, if you need many levels in your tree/hierarchy (more than 2), the above solution using resistors gets too fiddly to work. In this case, you should change the SPI network between each layer of the tree. This means each device will connect to its children on one SPI network, and its parent on the other SPI network. Although it means you only have a single tree of connections, the advantage is that a device can communicate with both one of its children and its parent at the same time and you don't have fiddly resistors (always hard to choose the right values).

Because it has separate MOSI and MISO wires, both the master and slave can communicate at the same time, giving it a potential factor of two increase in speed. A extra pin is required for the slave-select for each additional slave, but this is not a big burden, even 10 different slaves requires only 10 extra pins, which can be easily accommodated on a typical AVR microcontroller.

CAN is not supported by the AVR microcontroller you have specified. As there are other good options, it is probably not important in this case anyways.

The recommendation is SPI, because it is fast, the wiring isn't too complex, and doesn't require fiddly pull-up resistors. In the rare case where SPI doesn't fully meet your needs (probably in more complicated networks), you can use multiple options (eg. use both SPI ports, along with the two-wire interface, as well as pairing some of the microcontrollers using a USART loop!)

In your case, using SPI means that naturally, the microcontroller with the USB connection to the computer can be the master, and it can just forward the relevant commands from the computer to each slave device. It can also read the updates/measurements from each slave and send these to the computer.

At 8MHz, and 0.5m wire length, I don't think it will become a problem. But if it is, try be more careful of capacitance (keep ground and signal wires getting too close, and also be careful of connections between different conductors), and also signal termination. In the unlikely event that it remains a problem, you can reduce the clock rate, but I don't think it is necessary.


问:
Are LiPo really 100 times more energy dense than model rockets?
Lately I've been interested in comparing the energy density of model rocket engines to lithium polymer batteries (attached to motors and propellers) for propelling things upwards.

To get a feel for this, I decided to compare an Estes C6-5 motor to a 3DR Iris + quadcopter.

Estes C6-5 has initial mass of 25.8g, and produces 10 N s total impulse. So, the "Impulse density" is about 10 N s / 25.8g = 0.38 N s g^-1.

3DR Iris+ weighs 1282g without battery. 3.5ah battery weighs 250g and will power hover for about 20 minutes (so about 10.5a draw). Thrust produced to hover on Earth is 9.8N kg^-1 * 1.532 kg = 14.7N. "Impulse density" is 14.7N * 1200s / 250g = 70.6 N s g^-1 .

So, according to my math here, the LiPo is about 0.38/70.6 = 186 times more energy dense than the model rocket engine.

Of course, the model rocket engine will lose 12.48g of propellant by the end of the flight so it will be effectively a little lighter, but that's not going to affect things by a factor of 100.

Does this seem right to you? Am I missing anything?

答:
The short answer is "Yes", Rocket motors are less energy dense because they are built to expend that energy more quickly, while Lithium Ion Polymer batteries are designed to work longer.  However saying that they are "100 times" cannot always be correct.

The reasoning being that not all Lithium Ion Polymer batteries are made equal.   You have different C rates, cell density (affects C AND mAh), and different mAh.   For example the Zippy 3S 20C/40C 3000 mAh battery weighs about the same as the Mutistar 3S 10C/20C 5200mAh.  In general it would be correct to say that LiPo batteries store more energy than model rocket engines, this because of the different run times of the two fuel sources.  A capacitor may discharge more voltage and amperage than a battery, but the battery can generally run longer.


问:
Quadcopter frame design
Quadcopter frames seem to consistently follow the same X design. For example:

I'm curious to know why that is. It certainly seems like the most efficient way to use space but is it the only frame design that would work for quadcopters?

For instance, would a design like this work?

Why or why not?

答:
Any frame design that:

Is not too heavy

Does not flex too much

keeps the motors the same distance apart when measured diagonally(to use default x or + motor layouts)

can fit all required equipment

allows the flight controller board to be mounted in the centre

There are many other frame designs like the V-tail or dead cat that require you to use different motor mix settings.

Also you may want to look into H frame designs, they are relatively simple, and easy to repair.

Title: Quadcopter frame design


答:
One.

Here's my quadcopter lifting a burrito, and lowering it using an onboard winch, in 2013. Built from the cheap commercially available X-aircraft x650v4 kit, except I replaced the ESCs and the flight controller (I think that's an APM 1.0 on there).

A rule of thumb in multirotor design is that you should be able to hover at 50% throttle. This means that a quadcopter which can fly well at 1kg total weight will actually be able to get off the ground at around 2kg, but you'll have lost almost any ability to control attitude or position.

The winch is built from a servo and a fishing reel. Happy to provide more details.

https://youtu.be/wGwa2DG2Cs0

Title: How many quadcopters would it take to lift a burrito?

Body:
I am investigating a possible business opportunity in which quadcopters perform high-precision nutritional delivery via a burrito medium. I have never used a burrito, but I have read on the internet that they typically weigh 600-700 grams (1). This is much too heavy for commercially available platforms.

How many quadcopters would it take to lift a single burrito?

(1): https://www.facebook.com/chipotle/posts/390817319252


问:
Autonomous car steering using IR sensors
I want to steer a RC car in a straight line.The car has 4 sharp IR sensors on each corner of the car to help it steer the corridor.The corridor is irregular and looks something similar to the picture below.

The car needs to be stay exactly at the middle(shown by lighter line) and take help of the IR sensors to correct its path.

The car has a servo on the front wheel to steer and another that controls the speed.

I tried running it using a algorithm where it summed the values on each side of the car and took the difference.THe difference was then fed to a pid control the output of which went to steer the car.The greater the value from the pid (on either sides), the greater the value of the steering angle till it reaches the middle.

It works for the part where the walls are at similar distance from the center and even then it oscillates a lot around the center and fails miserably around the bumps in the corridor.

I need to make changes to the algorithm and need some help in steering me  in the right direction.

The IR sensors are too finicky and is there a way to filter out the noise and make the readings more stable?

Any help regarding the changes that needs to be implemented is much appreciated.

Currently the car only uses 4 IR sensors to guide.I can also use 2 ultrasonic sensors.

答:
This is a new interpretation of Grey Walter's Tortoise.

http://en.wikipedia.org/wiki/William_Grey_Walter

Try a BEAM robotics approach of a very simple control system that reacts to
nearing obstacle by steering the opposite direction.  Look at a subsumption / augemented finite state machine (enter link description here) to layer the higher goal of driving onto near term obstacle avoidance.

This is a solved problem. But I caution you to consider bumper switches as well in case the reflectant geometry misses nearby objects to give you another layer of safety.


问:
Conceptual problem regarding electronic shutters
I have been looking at CCD and CMOS sensors and cameras to decide which one to use in the process of automatic control of a printing process. By now I am getting the grips on almost all the essential numbers and abbreviations but there remains a problem with shutters.

I understand that there are different types of shutters, both mechanical and electronic, and I can understand how they work. My problem concerns shutter speed. If I use a mechanical shutter, well then the maximum shutter speed depends on that particular element in the assembly, but how does it work for electronic shutters? I have never read "Max shutter speed" in any specs. The only thing I usually see floating around are frames per second. But those do usally not pass a limit of about 120 fps. Depending on how the sensor it is built one could think that the maximum shutter speed therefore is 1/120 or 1/240 if it uses half frames.

Can this be right? It seems really slow. I will be faced with the task of recording crisp and clear images of paper which moves at about 17 m/s. That is never possible with shutter speeds that slow. Will I be forced to use a mechanical shutter or am I misunderstanding something?

答:
I believe that in the electronic world, the main determinant will be processing power. Your speed (or fps) will be limited by two factors:

the length of exposure required for a given ISO setting and lighting levels

between each exposure, the electronics will need to process the data from the sensor and store it

The amount of time it takes for those two things to happen at once will dictate your framerate. So, its not really a specification of the sensor itself, but depends on the supporting electronics and software.

Technically speaking, the sensor itself doesn't have an ISO setting, rather that depends on the software and it is about SNR or signal to noise ratio (see this article).

Another thing to consider when filming your high speed scenes is that a CMOS sensor will use a rolling shutter. This can give some strange stroboscopic effects because of the way it only gathers info from one part of the sensor at a time and rolls through all of the rows (or columns) in the sensor successively.  CCD may be the better choice to minimise that artifact.

Here's a couple videos that demonstrate the effect of rolling shutters in CMOS cameras:

A strange effect

curious stroboscopic effect


答:
Those two designs you posted are identical as far as the placement of propellers goes, so yes, they would both work, and with exactly the same flight electronics.

The second ring design though, uses approximately twice as much material for the frame hence it will be heavier, while in practice you'd probably want to minimize the weight of the airframe. There doesn't seem to be a convenient place to mount a central flight computer, but I'm sure a little creativity could lead to a ring design that is practical.

Here's a slightly different configuration for a quadcopter (called V-tail) that seems to be gaining popularity. It would need slightly different logic.

Title: Quadcopter frame design

Body:
Quadcopter frames seem to consistently follow the same X design. For example:

I'm curious to know why that is. It certainly seems like the most efficient way to use space but is it the only frame design that would work for quadcopters?

For instance, would a design like this work?

Why or why not?


答:
There are the awesome "ROS Robot Web Tools" http://robotwebtools.org/ that give your ROS enabled robot a web interface.
We use it as high-level interface for many of our service robots, as we can then use a tablet computer to control them and don't have to care about the Tablet OS (Apple vs. MS vs. Android...)</p>

Title: idea for web application in robotics

Body:
I am learning and I am interested robotics, but also I need to update my web development skills so the question is - is there any idea for good web application that could be connected with robotics - service robots, industrial robots etc. Maybe there already is some open source ongoing web application projects for robotics in which I can make contribution.

Thanks!


答:
Did you have a look into Orocos http://www.orocos.org/ ?
Compared with ROS it has the huge advantage of real time capability.
That would be my choice to implement complicated real time controllers.

In my work (http://www.gpu-voxels.org) I am also using a GPU for 3D collision avoidance calculations on our robots, but I am using our own MCA2 http://www.mca2.org framework for the real time stuff. Our actuators are all controlled via CANOpen interfaces.

30 Hz should be no problem at all! We run some motor controllers with 100 Hz and more. Andy my GPU (NVidia TITAN) processes 3D pointclouds of multiple Kinect cameras at 30Hz.
So 2D image processing should be possible, even on a Laptop GPU.

Title: Interfacing GPU image processing with motor control at 30+Hz

Body:
I would like to make a robotic system which takes as input a video feed, runs some GPU-based image recognition on the video, and outputs commands to a set of motors. The goal is to have the motors react to the video with as little latency as possible, hopefully of the order of 10s of ms. Currently I have a GTX 770m on a laptop running Ubuntu 14.04, which is connected to the camera and doing the heavy image processing. This takes frames at 30Hz and will output motor commands at the same frequency.

After a few days of looking around on the web for how to design such a system, I'm still at a loose end whether (a) it is even feasible (b) if so, what the best approach is to interface the laptop with the motors? The image processing must run on Linux, so there is no leeway to change that part of things.


问:
Electric piston (longitudinal electric motor)?
Are there electric motors, which apply force not in rotational motion, but in longitudinal motion?

They should have electromagnetic design and no gears and worms.

Such motors would be great for linear actuators. They could transfer both force and feedback.

What is the name of such devices?

答:
There are solenoids, which are electromagnetic linear actuators, and there are linear voltage differential transformers LVDTs, which are electromagnetic linear position sensors.

I don't think you could use one for positioning and sensing because solenoids operate primarily on DC while LVDTs operate on AC. Additionally, solenoid positioning is generally poor and they do not output a position but rather a force, like pneumatics or hydraulics.

Hydraulics can utilize the fact that their working fluid is essentially incompressible for position control - i.e., proportional control until position is reached then just shut inlet and outlet valves, but "compressible" actuators such as pneumatics and solenoids have a SIGNIFICANTLY more difficult time achieving performance even remotely on par with hydraulics.

Controllers for pneumatic positioning exist, as I'm sure they do for solenoids as well, but solenoids are primarily used for on/off switching. Anything more advanced and you're looking at an expensive position controller. LVDTs are also expensive.

Any particular reason a regular geared linear actuator is insufficient?


问:
What are methods for dealing with compass lag (rate dependent hysteresis)?
I've got a tread-driven robot, with low precision wheel encoders for tracking distance and an electronic compass for determining heading.  The compass has significant (> 1 second) lag when the robot turns quickly, e.g. after reaching a waypoint — pivoting in place to point to its new heading.

What are ways for dealing with the lag?  I would think one could take a lot of measurements and model the compass response.  However, this seems problematic since it's rate-dependent and I don't know the instantaneous rate.

As a simple-but-slow approach, I have the robot turn until it's very roughly pointed in the right direction, then make very small incremental turns with brief measurement pauses until it's pointed the right way.  Are there other ways of dealing with this?

答:
A gyro is the simple answer. I've always heard, gyro for the short measurements, compass for the long. And realistically a cup of kallman filter between the two most of the time. The price of a 6DOF gyro/acc board is less than $20 these days, far too cheap to not use one.

At one time, I worked through someone else's Kallman filter. and got it working. A kallman filter is actually more of an approach, not a exact implementation, and in the gyro case, the end result does not need to use matrix math. It makes for much simpler code.


问:
3D scanner from Phone Camera
123D software can construct a 3D model from photos taken from your phone. It doesn't process the photos in your phone.  Instead, it sends them to the cloud to create 3d model. How can i construct a 3d model like this (only with one camera)? I searched it but i can only find information on laser/procetor scanners (simple and desktop use only). I think 123D uses only IMU sensors and camera why do they use the cloud?  Can a beaglebone or rasperry pi create 3d models like this?

答: Creating 3D models with this method is very compute intensive, 123d uses many pictures (at least 20), and examines them for feature points that are common in several pictures and by examining how they change between pictures it can help build up a 3ds point cloud which is then textured using the pictures, this is very resource intensive, and could be done by a beaglebone or raspberry pi, but it would take a very long time. If you wanted to make your own system to 3d scan objects with cameras it is soing to be a lot easier to have an array of them pre calibrated to work in sync to generate models. This is how many professional setups work.</p>

Title: 3D scanner from Phone Camera


问:
Hector SLAM, Matching algorithm
I'm trying to understand the scan-matching part of Hector SLAM (PPT summary). It seems a little difficult to understand, in some cases, how is it possible to actually perform the alignment of the scans. Can anyone explain about it?

In my case, I'm working with a simulation. I'm moving my robot in a corridor-like featureless environment (only two walls) and I don't get a map. Nevertheless, if I move in a sinewave motion, I'm able to get a map. Moreover, if I have an additional feature, the algorithm even shows the real path as long as this feature is seen (right part of the image), otherwise it shows a very weird-looking oscillatory path which does not resemble a sinewave at all. Something important to notice is that the width of the map is pretty accurate (real=4m, map's=4.014m), and the length of the movement is also somehow accurate (real=15m, map's= 15.47). I'm using a Hokuyo URG-04LX laser range finder, no odometry, no IMU. I'm running in Ubuntu 14.04 and using ROS Indigo.

I more or less understand how Hector works, but I have no idea about why I'm getting this map and specially trajectory.

Thank you.

答:
Imagine someone put you in a wheelchair and blindfolded you, then let you reach your arm out and touch a wall. You could tell how far away the wall was, but as long as you were pushed parallel to the wall, how would you know how far you had gone? You can't count steps or see the end of the hall, so you do not have a way to index your samples of where the wall is.

In a similar way, you have handicapped your robot in the simulation. You state you are using aHokuyo URG 04LX, and you also state that your hall is 15 meters long. The range on that scanner is 4 meters, so it cannot "see" the end of the hall and cannot use that information to index its scans. You are also not providing any odometry, so it can't guess where to index the scans either.

SLAM is simultaneous localization and mapping - if the current "image" (scan) looks just like the previous image, and you provide no odometry, it does not update its position and thus you do not get a map. This is true as long as you move parallel to the wall, which is your problem case.

When you advance or retreat from a wall (feature) within range, as you do when you take the sinusoidal path, the images are different from one scan to the next. The algorithm looks for the rotation and translation that yields a "best fit" between those two different images and assumes that the difference between the two was due to motion, updates its estimate of location accordingly, then indexes that scan to the new position.

Regarding the path you are shown, I would look for a range issue somewhere, probably in a tangent expression. Always use atan2 instead of atan whenever possible to avoid errors due to quadrant location; (-y)/x and y/(-x) both evaluate to -(y/x) inside an atan expression but those are two totally different angles. :EDIT: See below, but this may also be due to a lack of features.

RECOMMENDATIONS
Get a scanner that has a range greater than the hall length, OR reduce the hall length, OR start your craft within "sight" of the end of the hall, OR add features such as alcoves/recesses along the hall to give the algorithm a key point with which it can localize and index.

A final note regarding the alcoves: (In Bruges reference) they must be spaced closer together than the twice the maximum scan range because the craft is operating in the center of the hall. Distance to wall (squared) + distance to alcove (squared) = max range (squared). You need a new feature to enter the frame before the old feature leaves, so if the max distance to one alcove is sqrt (max range (squared) - distance to wall (squared)), then the spacing needs to be less than (or equal, if you enjoy pain and borderline cases) double that distance. Finally note that this is assuming your scanner has a 360 degree view and can see the feature behind it until it passes the maximum scanning range. In actuality your scanner has a 240 degree field of view, but I'll let you work that math out on your own.

:EDIT:
With a little further reflection, I don't know that a straight, infinitely long wall (which is what you have if both ends are out of range) is sufficient to provide lateral position estimation. This may also help to explain your weird path. As an easy check, try putting it within sight of the end of the hall without your feature to see if you get the smooth sine you would expect. Then I would add alcoves at the 1x spacing I described above (range^2 - (wall dist)^2) and see if that also fixes it. I would do both of these before hunting through the algorithm for trig out of range errors.

Title: Hector SLAM, Matching algorithm


问:
Fingerprinting/ model matching algorithms for localization
This paper mentioned the fingerprinting/model matching case. But I could not find an image based algorithm. Any suggestion about image based localization</p>

答:
I'm not sure what you mean by "image based," but it seems like the term "model matching" is used to describe the notion of using an a-priori map of an environment to localize to.

You can build an a-priori map in many ways such as using a lidar, or using detected features from an image. Once you know this global map, your robot can use its sensor data to position itself on the global map by using localization algorithms such as a particle filter or EKF.


问:
Determine the configuration space for a robotic arm
I'm working with a 4DOF Parallel-Mechanism arm. I'm interested in writing planners for this arm (PRM or RRT) in the configuration space, but I'm not sure how to identify obstacles/collisions.

When writing planners for mobile robots in a 2d workspace, it was easy to define and visualize the workspace and obstacles in which the planner/robot was operating. This website (link) shows a great example of visualizing the workspace and configuration space for a 2DOF arm, but how can I do this for higher dimensions?

答:
This is one of the basic problems in planning robot trajectories:
The transformation from Configuration-Space to Workspace via direct kinematic is unambiguous, while the inverse way from Workspace to Config-Space is ambivaltent (in all cases where Dim(Config-Space) > Dim(Workspace)).

So what almost all planers do (especially the sampling based ones like RRT) is sampling in Config-Space, transforming the sample to Workspace and performing a collision check there. If no collision occurs, add sample to planning-graph.

The same has to be done for longer trajectories in between the samples: Break them down to smaller subtrajectories (< Robot/Endeffector-Size) and do a collision check for each waypoint.


问:
How can a memory alloy be used as an alternative to a compressor found in a refrigerator?
I'm curious about this alloy and how they say it can be used as an alternative to a traditional compressor. Can anyone explain how this would work?

My goal is to understand that use case so I can adapt alloys in other robotic projects.  My gut tells me this is perfect for some kinematics, or other mechanisms, but I'm missing some pieces in this puzzle (how would it work?)

答:
I don't think they're proposing building a compressor for use in refrigeration.  They're proposing a solid-state cooler/heat pump based on the elastocaloric effect:

As the shape memory alloy block is strained, it gets hot.  This heat would be extracted - I guess by blowing air through the device to somewhere outside the refrigerator?

The air flow is then stopped, and the device unloaded and allowed to return to its normal shape.  This causes it to cool.  This part of the cycle will presumably cool the refrigerator casing somehow...

I wasn't able to find any physical designs of elastocaloric refrigerators.  However there is a book on the magnetocaloric effect, and chapter 11 shows some basic ideas for how a heat exchanger might work.

By the way, I think the novelty in the recent research is their material is more reliable - shape memory alloys have been known for a long time, but their recent formulation has a longer life before it begins to break down with repeated stretching and shrinking.


问:
Simple wireless connection between two circuits
I'm relatively new to robotics, and I'm building a project for which I need a simple wireless connection between two circuits such that when the first circuit is switched on, the other circuit gets switched on too. I'm looking to preferably build something like this on my own, but I have no idea about wireless connections. I only know basic wired robotics. I also know C++ programming if that helps. Apologies if such a question has already been asked.

Regards,
Hanit Banga

答:
If the curcuits are placed stationary the most simplistic solution might be using an IR-Led and a reciever. Like the :

http://www.farnell.com/datasheets/1835437.pdf

http://wwwtest.vishay.com/docs/81007/tsal5100.pdf

Just powerup the Led. The reciever will recognize this light and switches the output.


问:
Matlab toolbox (Windows) for Sick lasers?
Does anybody know where I can get a matlab toolbox or functions to work with a SICK laser-scanner (Windows OS)? I'm using a SICK-LDRS2110 with ethernet cable, but SOPAS software does not allow me to program recording times and other specific tasks. Any tips are more than welcome!

Thanks!

答: http://sicktoolbox.sourceforge.net/ but I can't vouch for it; at my company we've coded our own drivers from scratch. </p>


问:
Calculating the efficiency of Mecanum wheels
I'm part of a FIRST Robotics team, and we're looking into using Mecanum wheels for our robot.

What are the advantages and disadvantages of using Mecanum wheel versus regular ones? From looking through Google, it looks like Mecanum wheels give more mobility but don't have as much traction. Are there any other advantages or disadvantages?

Compared to regular wheels, are Mecanum wheels less efficient or more efficient in any way? And if so, is there a quantifiable way to determine by how much?

Are there equations I can use to calculate efficiency (or inefficiency) and/or speed of moving forwards, sideways, or at arbitrary angles?

A picture of a robot with mecanum wheels:

答:
I have some experience with using mecanum wheels, both indoors and outdoors (on grass, sand and dirt no less).

Obvious advantage is holonomic movement.  Disadvantages are weight (commercially available wheels are ridiculously heavy) and cost.  For a given tread material traction will be along the lines of 65-70% that of a regular wheel due to the smaller contact area and 45% rollers.

Compared to skid steer, mecanum wheels have no additional friction when turning which is an advantage.

Mecanum wheels will be simpler and perhaps more reliable than a swerve/crab drive system.

Everything is always about tradeoffs.  If I compared an ideal mecanum system to an ideal swerve drive system (full 360 degree rotation, extremely fast wheel orientation) the swerve wins in performance because it has full traction.

An ideal swerve system will be expensive and difficult to implement compared to mecanum wheels though.

Efficiency is a loaded question.  An ideal mecannum wheel will be equivalent to any other system when moving in any direction, and will turn better than a skid steer system.
In reality the rollers have reasonable friction so going sideways will be less efficient than going forwards.

Getting exact numbers would require a lot of fairly accurate measurements which isn't practical.


答:
The lag in the compass is because of a low-pass filter, to suppress high frequency noise.

There exist more expensive magnetometers which have less noise, and therefore, less lag.

It is also possible to use a gyroscope to improve accuracy. In fact, this is what Inertial Measurement Units (IMUs) do. This can be accomplished by using a Kalman filter. Improving accuracy helps to decrease lag, because increased accuracy reduces the dependency on a low pass filter to suppress noise. The Kalman filter fuses the data from the magnetometer, and also the gyroscope (which measures rate of change in heading).

If you stick with your current compass, there are two possible solutions (Warning, this does get increasingly advanced, but option 1 should be accessible to most people without too much work).

You can try to cancel out the filter. This can remove lag, but also increases high frequency noise. After doing this, you can try to control the robot based on the new estimate of heading. To do this, you must experiment to work out the low pass filter parameters. For example, in discrete time, you might find:

$$\hat\theta(t)=a_0\theta(t)+a_1\theta(t-1)+\cdots+a_k\theta(t-k)$$
where $\hat\theta(t)$ is the estimated heading (compass output) at time $t$, $\theta$ is the actual heading (ground truth) at time $t$.

You can find the parameters $a_i$ by doing an experiment where you measure the ground truth using some other external means. Given $n+k+1$ samples, you have this equation:
$$\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]$$

And you can solve by finding:
$$\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]^{+}\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]$$ where $M^+$ is the pseudo-inverse matrix of $M$. There is no definitive way to work out $k$, so you will probably just guess. For bonus points, this assumes that the noise is white and independent, but you can whiten it first to remove bias, and therefore improve your estimate of the parameters.

You can convert this to a transfer function (also known as Z-transform in the discrete time domain):

$$\frac{\hat\Theta(z)}{\Theta(z)}=a_0+a_1 z^{-1}+...+a_k z^{-k}$$

To cancel this out, we can take the inverse (where $\bar\theta(t)$ is our new estimate of heading):

$$\frac{\bar\Theta(z)}{\hat\Theta(z)}=\frac{1}{a_0+a_1 z^{-1}+\cdots+a_k z^{-k}}$$

Converting back to the time domain:

$$a_0\bar\theta(t)+a_1\bar\theta(t-1)+\cdots+a_k \bar\theta(t-k)=\hat\theta(t)$$

$$\bar\theta(t)=\frac{\hat\theta(t)-a_1\bar\theta(t-1)-\cdots-a_k \bar\theta(t-k)}{a_0}$$

we can then use $\bar\theta$ to control the robot.

This will be very noisy, so you might want to still put $\bar\theta$ through a low-pass filter before use (although perhaps one with less lag).

The above solution is still not the best way. The noisy estimate may not be very useful. If we put this into a state space equation, we can design a Kalman filter, and a full-state feedback controller using LQR (linear quadratic regulator). The combination of a Kalman filter and LQR controller is also known as an LQG controller (linear quadratic gaussian), and use loop-transfer recovery to get a good controller.

To do this, come up with the (discrete-time) state-space equations:

$\vec{x}(t)=A\vec{x}(t-1)+B\vec{u}(t-1)$, $\vec{y}(t)=C\vec{x}(t)$

or:
$$\vec{x}(t)=\left[\matrix{\theta(t)\\\theta(t-1)\\\cdots\\\theta(t-k)}\right]=
\left[\matrix{
    A_1&A_2&\cdots&0&0&0\\
    1&0&\cdots&0&0&0\\
    0&1&\cdots&0&0&0\\
    \vdots&\vdots&&\vdots&\vdots&\vdots\\
    0&0&\cdots&1&0&0\\
    0&0&\cdots&0&1&0
}\right]
\vec{x}(t-1)
+
\left[\matrix{B_0\\B_1\\0\\\vdots\\0\\0}\right]\vec{u}(t-1)$$

$$\vec{y}(t)=\left[\matrix{\hat\theta(t)}\right]=\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]\vec{x}(t)$$

where $\vec{u}(t-1)$ represents the power in the motors to turn the robot, and $A_0$, $A_1$, $B_0$, $B_1$ is how much it affects the heading based on position and speed (you can choose non-zero values for the other elements of the $B$ matrix, and first row of the $A$ matrix too).

Then, you can build your observer (Kalman Filter), by choosing noise estimates $Q_o$ and $R_o$ for the process noise and measurement noise. The Kalman Filter can then find the optimal estimate of heading, given those assumptions about the noise. After choosing the noise estimates, the implementation just depends on implementing code for the Kalman Filter (equations can be found on Wikipedia, so I won't go over it here).

After that, you can design an LQR controller, this time, choosing $Q_c$ and $R_c$ representing the weighting given to regulating the heading, and trying to limit the use of the actuators. In this case, you might choose $Q_c = \left[\matrix{1&0&0&\cdots&0\\0&0&0&\cdots&0\\\vdots&\vdots&\vdots&&\vdots\\0&0&0&\cdots&0}\right]$ and $R_c = \left[1\right]$. This is done because LQR finds the optimal controller to minimise a cost function: $J = \sum{(\vec{x}^T Q\vec{x} + \vec{u}^T R \vec{u})}$

Then, you just put it through the discrete time algebraic Riccati equation:

$$P = Q + A^T \left( P - P B \left( R + B^T P B \right)^{-1} B^T P \right) A$$

and solve for a positive definite matrix $P$.

Thus, your control law can be given by:

$$\vec{u}(t)=-K(\vec{x}(t)-\vec{x}_{ref}(t))$$

where $K = (R + B^T P B)^{-1}(B^T P A)$

Finally, just doing this won't work very well, and is likely to be unstable because of the noise. Indeed, that means option 1 probably won't work unless you first put $\bar\theta$ through a low-pass filter (albeit not necessarily with such a long effective lag time). This is because while LQR is guaranteed stable, as soon as you use a Kalman filter, the guarantee is lost.

To fix this, we use the Loop Transfer Recovery technique, where you adjust the Kalman filter, and instead choose a new $Q_o = Q_0 + q^2BVB^T$, where $Q_0$ is your original $Q$ matrix, tuned so that the Kalman filter is optimal. $V$ is any positive definite symmetric matrix, which you can just choose as the identity matrix ($V=I$). Then, just choose a scalar $q$. The resulting controller should become (more) stable as $q \rightarrow \infty$, although the $Q_o$ matrix becomes de-tuned, which means it becomes less optimal.

Therefore, you just increase $q$ until it is stable. Another way you can try to make it stable, is to increase $R_c$ (or decrease $Q_c$) to make the LQR controller slower.

The concepts in this post does get quite advanced, but if you need to solve things like the Riccati equation, you can use MATLAB or other software to do this. There may also be libraries already implementing the Kalman filter (again, I believe MATLAB also does this).

For an embedded application, you may need to implement the Kalman filter yourself, although there is probably a C implementation.

Title: What are methods for dealing with compass lag (rate dependent hysteresis)?

Body:
I've got a tread-driven robot, with low precision wheel encoders for tracking distance and an electronic compass for determining heading.  The compass has significant (> 1 second) lag when the robot turns quickly, e.g. after reaching a waypoint — pivoting in place to point to its new heading.

What are ways for dealing with the lag?  I would think one could take a lot of measurements and model the compass response.  However, this seems problematic since it's rate-dependent and I don't know the instantaneous rate.

As a simple-but-slow approach, I have the robot turn until it's very roughly pointed in the right direction, then make very small incremental turns with brief measurement pauses until it's pointed the right way.  Are there other ways of dealing with this?


问:
Manipulator link applied torque
I want to implement a manipulator link using a physic library. I can only apply some torque to the centre of mass, but the torque should be applied at the beginning of the link.

Shifting a reference frame from the centre of mass and recalculating inertia tensor in the new frame is not a problem, neither is recalculating a new torque, based on the change of distance, but I think it is not the correct solution.

In short, how can I scale a torque of a control signal applied at the beginning of the link to a torque of a physic simulation applied to the centre of mass. Thanks.

答:
Use the parallel axis theorem

$$
\tau=I\alpha \\
\alpha = \frac{\tau}{I} \\
I_{new} = I_{from.COM} + mr^2
$$
Assuming you want to keep the same acceleration as though you applied the torque at the center of mass, the new torque you need to apply to create the same angular acceleration from a different location is:

$$
\frac{\tau_{COM}}{I_{from.COM}} = \frac{\tau_{new}}{I_{new}}
$$
$$
\tau_{new} = I_{new} \frac{\tau_{COM}}{I_{from.COM}} \\
\tau_{new} = (I_{from.COM} + mr^2)\frac{\tau_{COM}}{I_{from.COM}}
$$

where $\tau_{COM}$ was the torque that had to be applied at the center of mass to get the desired angular acceleration, $I_{from.COM}$ is the moment of inertia through the center of mass, $m$ is the mass of the load, and $r$ is the distance from the center of mass to where you are now rotating the load.

Title: Manipulator link applied torque