问:
Yaw angle calculation for drone PID from two distance sensors
I'm building a control system with a Parrot AR 2.0 drone where I have access to thrust controls for up/down (z), left/right (y), forward/backwards (x), turn left and turn right (yaw) through a Ruby library on my computer.

The goal of the system is to keep the drone a particular distance from and parallel to a wall while moving in the up/down and left/right directions. We have added two sonar distance sensors to the left and right forward props.

The main problem I am having is figuring out how the two distance sensors equal a yaw reading (ψ) so I can feed that into the PID and then take action on thrust to the turn left or right for correction.

Maybe just getting some help with the conversion from two distances to the yaw angle would be a big help, but any thoughts on the PID are greatly appreciated too since it is my first time working with it.

答:
This sounds like pretty simple geometry.

The two ultrasonic sensors are reporting distances L1 and L2. The Yaw angle can then be calculated:

a = atan( (L1-L2) / W )


问:
12V Arduino Dual Bridge to supply at least 5A
I am looking for a 12V Dual Motor Controller that can supply at least 5A per channel for two 12V motors, and that can be used with an arduino. Do you know of any product with those specs?

Thanks

答:
This one may be a nice board for you
http://www.pololu.com/product/2502

they say it is unavailable, but you may find something..

or just take two of them:
http://www.pololu.com/product/1451

ContentLicense: CC BY-SA 3.0


问:
How to obtain dense point clouds from stereo cameras?
I am trying to use a stereo camera for scene reconstruction, but I can usually only obtain sparse point clouds (i.e. over half the image does not have any proper depth information).

I realize that stereo processing algorithms rely on the presence of texture in the images and have a few parameters that can be tweaked to obtain better results, such as the disparity range or correlation window size. As much as I tune these parameters, though, I am never able to get results that are even remotely close to what can be obtained using an active sensor such as the Kinect.

The reason why I want that is because very often point clouds corresponding to adjacent regions don't have enough overlap for me to obtain a match, so reconstruction is severely impaired.

My question to the Computer Vision experts out there is the following: what can I do to obtain denser point clouds in general (without arbitrarily modifying my office environment)?

答:
When you say, "over half the image does not have any proper depth information", which half ?

One issue we ran into is that if distance-to-object is of the same order of magnitude than your baseline (usually associated with very wide angle cameras), then the "standard" dense stereo algorithms don't work so well. We've been using the libelas library, and its developers told us that this is called "large baseline stereo" and is yet another problem.


问:
which type of camera should be used for detecting road lanes for good processing in matlab
What are the parameters which should be selected to choose camera for lane detection system.What parameters should be kept in mind (like picture quality,frame rate,cost e.t.c). Which camera will suit best to my application.</p>

答: This depends on several things, mostly what your processing the images with, if you have a 1080p camera running at 60hz and your using a raspberry pie, that is clearly overkill. I would recommend you benchmark your platform of choice to come up with your ideal resolution to fps, and based on that select your resolution. The other factor will be focal length, witch  determines how wide an image your camra picks up (see the wikipedia article) having a lower focal length will allow you to pick up objects more to the left or right, but the only downside will be he bigger it is the less pixels your tracking target will pick up witch can become a problem as your resolution drops.</p>


问:
Can I reuse the hall sensors in a brushless motor as an encoder?
I have upgraded the motors in my robotic arm to sensored, brushless RC car motors. The hope was to reuse the Hall sensors to double as a rotary encoder, by tapping 2 Hall sensors and treating the 2 bits as a quadrature signal (a crude quadrature since 2 of the 4 states will be longer than the other 2).

This works when none of the motor phases are powered and I just rotate the motor manually. But once the stator coils are energized, the encoder no longer counts correctly: When running at low power, the counting is correct, but when running under high power, the count is monotonic (only increases or decreases) no matter if I run in reverse or forward.

I'm almost certain this is because of the stator coils overpowering the permanent magnets on the rotors. So is there still a way to use the Hall sensors as an encoder?

Sorry if this is an obvious question. I'd love to research this problem more if I had more time.

Update:
I've measured the wave forms with my DSO quad and see the expected 120 degree separated signals (the measurement for phase C gets more inaccurate over time because I only had 2 probes, so I measured phases A & B first, then A & C, and then merged them.

When ESC speed is 0.1:

When ESC speed is 0.3:

Previously, I was using a hardware quadrature counter (EQEP module on a BeagleBone). At speed=0.3, this was counting backwards no matter if I do forward or reverse!

I then implemented quadrature counting on an LPC1114FN28 uController. The result was still bad at high speeds (count didn't change at all). The logic was:

```<code>void HandleGPIOInterrupt()
{
  const uint8_t allowableTransitions[4][2] = {1, 2, 3, 0, 0, 3, 2, 1};
  static int prevState = -1;
  int state = phaseA | (phaseB * 2)
  if (prevState != -1)
  {
    if (allowableTransitions[prevState][0] == state)
    {
       ++rotations;
    }
    else if (allowableTransitions[prevState][1] == state)
    {
      --rotations;
    }
  }
  prevState = state;
}
</code>```

Then I got the idea to change the code to not update prevState until an expected state happens (to deal with glitches):

```<code>  int state = phaseA | (phaseB * 2)
  if (prevState != -1)
  {
    if (allowableTransitions[prevState][0] == state)
    {
       ++rotations;
       prevState = state;
    }
    else if (allowableTransitions[prevState][1] == state)
    {
      --rotations;
      prevState = state;
    }
    else
    {
        // assume transition was a glitch
    }
  }
  else
    prevState = state;
</code>```

Now the counting finally is correct in both directions, even at speeds higher than 0.3!
But are there really glitches causing this? I don't see any in the waveforms?

答:
I've figured out the problem a while ago. It turns out to be a power supply problem. I'm using a computer power supply (Antec HCG 850M) with the under voltage & over current protections removed, so it doesn't trip.

There was also another huge problem I didn't mention. The motor was randomly stopping and starting several times a second.

Then I saw that the electronic speed controller I'm using only had a 330uF filter capacitor. It was also was very hot to touch, despite being a low resistance, electrolytic polymer capacitor. After replacing it with a similar 1000uF capacitor, the problems are gone.


问:
DC motor direct loading
I found many tutorials and online calculators for the selection of dc motor to drive wheel, I understood how the torque affect the driving of wheel.

But what happen when I change the orientation of motor and load? What is the main criteria for a dc motor to work when I want to rotate a plate which is vertically mounted on motor's shaft, when the motor is placed vertically also (as shown in the picture)?

I am not an engineering student so please provide me an answer as simple as possible.

答:
I don't know how to calculate toque in this kind of vertical setup.
  All information i found on the web always lead me to the torque
  calculation for motor driving wheels(which is simply the tangential
  force multiply the moment arm).

The torque is the tangential force multiplied by the moment arm, no matter what the orientation.

Say you are trying to spin up something like a 0.5 kg bicycle wheel that is 1 m in diameter.
At 1 rpm, the tread has a linear velocity of pir(1 revolution/60 seconds) =~= 0.026 m/s.
To get this up to speed from a standing start in, say, 1/60 of a second requires an acceleration of every particle of the rim of a = velocity/time =~= (0.026 ms/s)/( 1/60 second ) =~= 1.6 m/s^2.

Since practically all of the mass of a bicycle wheel is at the rim, the moment arm of that 1 m diameter wheel is 0.5 m. (Other kinds of objects have more mass nearer the center; for the same total weight and diameter, they have lower moment of inertia and so require less torque).

So, as always, F = ma, so in this case F = (0.5 kg)(1.6 m/s^2) = 0.8 kg*m/2^2 = 0.8 N.

So, as always, the torque is the tangential force multiplied by the moment arm, or in this case t = rF = (0.5 m)(0.8 N) = 0.4 Nm = 40 Ncm.

The actual torque required would be somewhat more to overcome static friction, dynamic friction, etc.

If you were willing to gradually bring it up to speed over a longer time, then you would require less torque.
(Lots of standard NEMA 17 motors can easily supply 20 Ncm of torque).

Once it is up to speed, the motor only needs to supply enough torque to overcome the dynamic friction in order to maintain that speed.

Motors are generally designed to supply torque, not handle heavy radial-thrust loads.
If the stuff you want to turn is unbalanced,
spinning it at high speeds causes radial forces ("centrifugal force", aka "unbalanced washing-machine effect").
As OricTosh pointed out,
this has no effect on the torque required from the motor or the energy required to turn it;
but you may need to either
(a) add more weight on the other side to balance it, or
(b) add a bearing or two to support those radial forces, or
both;
to prevent those loads from pushing the motor rotor "sideways" enough to cause damage.

Motors are generally designed to supply torque, not to handle heavy axial-thrust loads.
If you have a heavy load on a vertical axis, you might need to
support that load with a
thrust bearing
(perhaps on the "other" end of a double-shaft motor --
Single-shaft vs Double-shaft motors
)
to prevent the load from pushing the axle through the motor enough to cause damage.

Title: DC motor direct loading


问:
How can one determine whether a LiPo battery is going bad?
In our lab we use LiPo batteries to power our quadrotors. Lately we have been experiencing stability issues when using certain batteries. The batteries seem to charge and balance normally and our battery monitor indicates they are fine even when putting them under load. However when we attempt to fly the quadrotor with one of these batteries, manually or autonomously, it has a severe tendency to pitch and/or roll. My guess is that the battery is not supplying sufficient power to all the motors which brings me to my question. Is this behavior indicative of a LiPo going bad? If so what is the best way to test a battery to confirm my suspicions?</p>

答: There's a possibility that the battery is going bad under those conditions. Assuming you have some kind of battery supervisor, just adding an interrupt indicating that the battery is below a certain level would be very informative. That way, if you have a freshly charged battery and the interrupt is triggered sooner than expected, then you know that the battery is going bad. That is, unless the problem is more obvious by observing swelling of the battery, in which case you should probably (and safely) dispose of it. You could also try the fresh battery with a comparable load to that expected while in flight (e.g. when all motors are on and all processors and sensors are running nominally).</p>


答:
Yes, there are several products that can independently drive two DC motors at 12 V and 5 A, which can be used with an Arduino.

OpenCircuits has a list of motor drivers, some of which meet your criterial.

Sparkfun carries many motor drivers that meet your criteria, including the "Monster Moto Shield", the "T'Rex Robot/Motor Controller", and several Pololu drivers.

Robot Power carries many motor drivers that meet your criteria.

Have you heard that shopping questions are off-topic on this site?

Title: 12V Arduino Dual Bridge to supply at least 5A

Body:
I am looking for a 12V Dual Motor Controller that can supply at least 5A per channel for two 12V motors, and that can be used with an arduino. Do you know of any product with those specs?

Thanks


问:
What are the calculations for a badminton robot and mechanisms?
I am designing a badminton robot but i am very confused about mechanisms needed for a badminton robot and various calculations needed for millisecond response.I am also confused about calculations needed about the forces needed and efficient angles needed for hitting the shuttlecock.Please suggest me some ideas or suggestions needed for construction of badminton robot.</p>

答:
In short, what you are trying to do is well beyond the capabilities of top robotics research labs.

That said, here is a short list of general areas you need to look into:

Robotic arm dynamics (to swing the racket)

Vision processing to track the shuttle

Shuttle dynamics to predict shuttle path (this is not well studying so you would most likely have to create new math here)

Nonlinear control control the arm as well as algorithms to move the robot where it needs to go.

Note that a wheeled robot will probably not do well because it can't move side-to-to.


问:
Can we use this line sensor as a proximity sensor?
I have an RSL Line Sensor which is designed to distinguish black and white lines. It detects white surface and gives me digital 1 as output, with 0 in case of black, but the surface needs to be close to it.

As it uses infra-red-sensors, I wanted to use this sensor as a proximity sensor, to tell me if there is a white surface near it. Is it possible to do this?

I think the only problem here is that we need to increase it's range of giving 1. Currently, it gives 1 only when white surface is too close to the sensors. I want 1 even if the white surface is there at a bit more distance.

Also there is an adjustable screw there to adjust something, under which POT is written. I am working with an Arduino.

答: The sensors which you using as got just an comparator with a variable resistor to adjust the threshold to differentiate black and white line... Sensors with filters can you configured to find the distance from an obstacle which can be used as a proximity sensors .. Anyways proximity sensors are IR based sensors (you can verify using phones camera)... </p>


问:
PD Algorithm for a Quadrotor [Simulation]
I have a big problem trying to stabilize a quadrotor with a PD controller.
The model and the program has been written in C++ and the model dynamic has been taken from this source in internet:

Well, in my code I wrote the model like in the eq. system ( see eq. 3.30 on page 21):

```<code>/* Calculate the acceleration about all 6 axis */
body_pos_current_.x_dot_2 =  ( thrust_.total / masse_ ) * ( sin( body_ang_current_.theta ) * cos( body_ang_current_.phi ) * cos( body_ang_current_.psi ) + sin( body_ang_current_.psi ) * cos( body_ang_current_.phi ) );
body_pos_current_.y_dot_2 =  ( thrust_.total / masse_ ) * ( sin( body_ang_current_.theta ) * sin( body_ang_current_.psi ) * cos( body_ang_current_.phi ) - cos( body_ang_current_.psi ) * sin( body_ang_current_.phi ) * cos( body_ang_current_.psi ) );
body_pos_current_.z_dot_2 =  ( thrust_.total / masse_ ) * ( cos( body_ang_current_.theta ) * cos( body_ang_current_.phi ) ) - 9.81;

body_ang_current_.phi_dot_2   = ( torque_.phi   / Jxx_ );
body_ang_current_.theta_dot_2 = ( torque_.theta / Jyy_ );
body_ang_current_.psi_dot_2   = ( torque_.psi   / Jzz_ );
</code>```

where body_ang_current.<angle> and body_pos_current_.<position> are structures defined in a class to store position, velocities and accelerations of the model given the 4 motor velocities about all 3 axis.

$$
\large \cases{
 \ddot X = ( \sin{\psi} \sin{\phi} + \cos{\psi} \sin{\theta} \cos{\phi}) \frac{U_1}{m}  \cr
 \ddot Y = (-\cos{\psi} \sin{\phi} + \sin{\psi} \sin{\theta} \cos{\phi}) \frac{U_1}{m} \cr
 \ddot Z = (-g + (\cos{\theta} \cos{\phi}) \frac{U_1}{m} \cr
 \dot p  = \frac{I_{YY} - I_{ZZ}}{I_{XX}}qr - \frac{J_{TP}}{I_{XX}} q \Omega + \frac{U_2}{I_{XX}} \cr
 \dot q  = \frac{I_{ZZ} - I_{XX}}{I_{YY}}pr - \frac{J_{TP}}{I_{YY}} p \Omega + \frac{U_3}{I_{YY}} \cr
 \dot r  = \frac{I_{XX} - I_{YY}}{I_{ZZ}}pq - \frac{U_4}{I_{ZZ}}
}
$$

Once I get the accelerations above I m going to integrate them to get velocities and positions as well:

```<code>/* Get position and velocities from accelerations */
body_pos_current_.x_dot = body_pos_current_.x_dot_2 * real_duration + body_pos_previous_.x_dot;
body_pos_current_.y_dot = body_pos_current_.y_dot_2 * real_duration + body_pos_previous_.y_dot;
body_pos_current_.z_dot = body_pos_current_.z_dot_2 * real_duration + body_pos_previous_.z_dot;

body_ang_current_.phi_dot   = body_ang_current_.phi_dot_2   * real_duration + body_ang_previous_.phi_dot;
body_ang_current_.theta_dot = body_ang_current_.theta_dot_2 * real_duration + body_ang_previous_.theta_dot;
body_ang_current_.psi_dot   = body_ang_current_.psi_dot_2   * real_duration + body_ang_previous_.psi_dot;

body_pos_current_.x = 0.5 * body_pos_current_.x_dot_2 * pow( real_duration, 2 ) + ( body_pos_previous_.x_dot * real_duration ) + body_pos_previous_.x;
body_pos_current_.y = 0.5 * body_pos_current_.y_dot_2 * pow( real_duration, 2 ) + ( body_pos_previous_.y_dot * real_duration ) + body_pos_previous_.y;
body_pos_current_.z = 0.5 * body_pos_current_.z_dot_2 * pow( real_duration, 2 ) + ( body_pos_previous_.z_dot * real_duration ) + body_pos_previous_.z;

body_ang_current_.phi   = 0.5 * body_ang_current_.phi_dot_2   * pow( real_duration, 2 ) + ( body_ang_previous_.phi_dot   * real_duration ) + body_ang_previous_.phi;
body_ang_current_.theta = 0.5 * body_ang_current_.theta_dot_2 * pow( real_duration, 2 ) + ( body_ang_previous_.theta_dot * real_duration ) + body_ang_previous_.theta;
body_ang_current_.psi   = 0.5 * body_ang_current_.psi_dot_2   * pow( real_duration, 2 ) + ( body_ang_previous_.psi_dot   * real_duration ) + body_ang_previous_.psi;

/* Copy the new value into the previous one (for the next loop) */
body_pos_previous_.x = body_pos_current_.x;
body_pos_previous_.y = body_pos_current_.y;
body_pos_previous_.z = body_pos_current_.z;

body_pos_previous_.x_dot = body_pos_current_.x_dot;
body_pos_previous_.y_dot = body_pos_current_.y_dot;
body_pos_previous_.z_dot = body_pos_current_.z_dot;

body_ang_previous_.phi   = body_ang_current_.phi;
body_ang_previous_.theta = body_ang_current_.theta;
body_ang_previous_.psi   = body_ang_current_.psi;

body_ang_previous_.phi_dot   = body_ang_current_.phi_dot;
body_ang_previous_.theta_dot = body_ang_current_.theta_dot;
body_ang_previous_.psi_dot   = body_ang_current_.psi_dot;
</code>```

The model seems to work well but, as like reported in many papers, is very unstable and needs some controls.

The first approach for me was to create a controller (PD) to keep the height constant without moving the quadcopter, but just putting a value (for example 3 meter) and see how it reacts.

Here the small code I tried:

```<code>/* PD Controller */
double e = ( 3.0 - body_pos_current_.z );  // 3.0 is just a try value!!!
thrust_.esum = thrust_.esum + e;
thrust_.total = 1.3 * e + 0.2 * real_duration * thrust_.esum;
</code>```

The problem, as you can see here in this video, is that the copter starts falling down into the ground and not reaching the desired altitude (3.0 meters).
Then it comes back again again like a spring, which is not damped.
I tried already many different value for the PD controller but it seems that it doesn't affect the dynamic of the model.

Another strange thing is that it goes always to a negative point under the ground, even if I change the desired height (negative or positive).

What s wrong in my code?
Could you me please point to some documents or code which is understandable and well documented to start?

Thanks

EDIT:
Many thanks to your suggestion.
Hi was really surprise to know, that my code had lots of potential problems and was not very efficient. So I elaborate the code as your explanation and I implementers a RK4 for the integration. After I ve read those articles: here and here I got an idea about RK and its vantage to use it in simulations and graphics PC.
As an example I rewrote again the whole code:

```<code>/* Calculate the acceleration about all 6 axis */
pos_.dVel.x =  ( ( thrust_.total / masse_ ) * ( -sin( body_position_.angle.theta ) * cos( body_position_.angle.phi ) * cos( body_position_.angle.psi ) - sin( body_position_.angle.phi ) * sin( body_position_.angle.psi ) ) );
pos_.dVel.y =  ( ( thrust_.total / masse_ ) * ( sin( body_position_.angle.phi ) * cos( body_position_.angle.psi ) - cos( body_position_.angle.phi ) * sin( body_position_.angle.theta ) * sin( body_position_.angle.psi ) ) );
pos_.dVel.z =  ( ( thrust_.total / masse_ ) * ( -cos( body_position_.angle.phi ) * cos( body_position_.angle.theta ) ) - 9.81 );

pos_.dOmega.phi   = ( torque_.phi   / Jxx_ );
pos_.dOmega.theta = ( torque_.theta / Jyy_ );
pos_.dOmega.psi   = ( torque_.psi   / Jzz_ );

/* Get position and velocities from accelerations */
body_position_ = RKIntegrate( body_position_, real_duration );
</code>```

which is much more clear and easy to debug. Here some useful functions I implemented:

```<code>QuadrotorController::State QuadrotorController::evaluate( const State &initial, const Derivative &d, double dt ) {

State output;
output.position.x = initial.position.x + d.dPos.x * dt;
output.position.y = initial.position.y + d.dPos.y * dt;
output.position.z = initial.position.z + d.dPos.z * dt;

output.velocity.x = initial.velocity.x + d.dVel.x * dt;
output.velocity.y = initial.velocity.y + d.dVel.y * dt;
output.velocity.z = initial.velocity.z + d.dVel.z * dt;

output.angle.phi   = initial.angle.phi   + d.dAngle.phi   * dt;
output.angle.theta = initial.angle.theta + d.dAngle.theta * dt;
output.angle.psi   = initial.angle.psi   + d.dAngle.psi   * dt;

output.omega.phi   = initial.omega.phi   + d.dOmega.phi   * dt;
output.omega.theta = initial.omega.theta + d.dOmega.theta * dt;
output.omega.psi   = initial.omega.psi   + d.dOmega.psi   * dt;

return output;

};

QuadrotorController::Derivative QuadrotorController::sampleDerivative( double dt, const State &sampleState ) {

Derivative output;

output.dPos = sampleState.velocity;
output.dVel.x = pos_.dVel.x;
output.dVel.y = pos_.dVel.y;
output.dVel.z = pos_.dVel.z;

output.dAngle = sampleState.omega;
output.dOmega.phi   = pos_.dOmega.phi;
output.dOmega.theta = pos_.dOmega.theta;
output.dOmega.psi   = pos_.dOmega.psi;

return output;

};

QuadrotorController::State QuadrotorController::RKIntegrate( const State &state, double dt ) {

const double C1 = 0.0f;
const double C2 = 0.5f, A21 = 0.5f;
const double C3 = 0.5f, A31 = 0.0f, A32 = 0.5f;
const double C4 = 1.0f, A41 = 0.0f, A42 = 0.0f, A43 = 1.0f;

const double B1 = 1.0f/6.0f, B2 = 1.0f/3.0f, B3 = 1.0f/3.0f, B4 = 1.0f/6.0f;

Derivative k1 = sampleDerivative( 0.0f, state );
Derivative k2 = sampleDerivative( C2 * dt, evaluate( state, k1 * A21, dt ) );
Derivative k3 = sampleDerivative( C3 * dt, evaluate( state, k1 * A31 + k2 * A32, dt ) );
Derivative k4 = sampleDerivative( C4 * dt, evaluate( state, k1 * A41 + k2 * A42 + k3 * A43, dt ) );

const Derivative derivativeSum = k1 * B1 + k2 * B2 + k3 * B3 + k4 * B4;

return evaluate( state, derivativeSum, dt );
}
</code>```

Now I m really lost because...because the simulated qudrotor has the same behavior as before. Nevertheless I ve implemented the same PD algorithm as discussed in the paper, it stabilize on Z (height) but it get really crazy due to unstable behavior.
So... I dunno what is wrong in my code and my implementation. And above all I cannot find any source in internet with a good self explaned dynamic model for a quadrotor.

Regards

答:
I don't have any recommended reading for you. However I see a lot of potential problems here.

First, and probably most obvious, there could be a bug. I don't see anything immediately wrong in the code you have provided but then there is a lot there. You could improve the code by decomposing it further. For instance, make a simple function to do the integration for a scalar value and use it instead of re-writing it for every variable. On that note you would be well served to change your integration to RK4 if you want to do anything more than hover.

Another possible problem could be with your time step. If it's too large you will get odd behavior. Given what you are seeing I'm guessing this is actually the problem. But that really is a guess because there isn't really enough to go on here even though you have clearly attempted to provide a good explanation.

Still another problem could be one of numeric stability. The cos and sin functions in your equations are sensitive around certain values. For instance cos is troublesome around 0 and the output can fluctuate wildly with only small perturbations of the input. Given that you are only trying to hover I suspect this is not actually the problem. Even still seeking out a good numerical computing library is a good idea.

The problem could also be with the gains. In fact, if you're using a reasonable time step I suspect this is most likely the problem given the behavior you're seeing. Unfortunately PD control on a quadrotor isn't ideal because the system is non-linear and the guarantees for PID controllers only apply to linear time invariant systems. As such the best way to find gains for the system is to hand tune them. You would be well served to linearize the system and apply LQR control instead.


问:
How to fuse linear and angular data from sensors?
My team and I are setting up an outdoor robot that has encoders, a commercial-grade IMU, and GPS sensor. The robot has a basic tank drive, so the encoders sufficiently supply ticks from the left and right wheels. The IMU gives roll, pitch, yaw, and linear accelerations in x, y, and z. We could later add other IMUs, which would give redundancy, but could also additionally provide angular rates of roll, pitch, and yaw. The GPS publishes global x, y, and z coordinates.

Knowing the robot's x y position and heading will useful for the robot to localize and map out its environment to navigate. The robot's velocity could also be useful for making smooth movement decisions. It's a ground-based robot, so we don't care too much about the z axis. The robot also has a lidar sensor and a camera--so roll and pitch will be useful for transforming the lidar and camera data for better orientation.

I'm trying to figure out how to fuse all these numbers together in a way that optimally takes advantage of all sensors' accuracy. Right now, we're using a Kalman filter to generate an estimate of [x, x-vel, x-accel, y, y-vel, y-accel] with the simple transition matrix:

```<code>[[1, dt, .5*dt*dt, 0,  0,        0],
 [0,  1,       dt, 0,  0,        0],
 [0,  0,        1, 0,  0,        0],
 [0,  0,        0, 1, dt, .5*dt*dt],
 [0,  0,        0, 0,  1,       dt],
 [0,  0,        0, 0,  0,        1]]
</code>```

The filter estimates state exclusively based on the accelerations provided by the IMU. The IMU isn't the best quality; within about 30 seconds it will show the robot (at rest) drifting a good 20 meters from its initial location. I want to know out how to use roll, pitch, and yaw from the IMU, and potentially roll, pitch, and yaw rates, encoder data from the wheels, and GPS data to improve the state estimate.

Using a bit of math, we can use the two encoders to generate x, y, and heading information on the robot, as well as linear and angular velocities. The encoders are very accurate, but they can be susceptible to slippage on an outdoor field.

It seems to me that there are two separate sets of data here, which are difficult to fuse:

Estimates of x, x-vel, x-accel, y, y-vel, y-accel

Estimates of roll, pitch, yaw, and rates of roll, pitch, and yaw

Even though there's crossover between these two sets, I'm having trouble reasoning about how to put them together. For example, if the robot is going at a constant speed, the direction of the robot, determined by its x-vel and y-vel, will be the same as its yaw. Although, if the robot is at rest, the yaw cannot be accurately determined by the x and y velocities. Also, data provided by the encoders, translated to angular velocity, could be an update to the yaw rate... but how could an update to the yaw rate end up providing better positional estimates?

Does it make sense to put all 12 numbers into the same filter, or are they normally kept separate? Is there already a well-developed way of dealing with this type of problem?

答:
You can greatly simplify the problem in most common cases:

A lot of "commercial grade" IMus (e.g. Xsens) have very noisy accelerometers. Don't even bother fusing them to get speed, the odometry is already order of magnitudes better. The only usable data the IMU is going to provide is the pitch and roll, and to some extent the heading (see next point)

heading from IMUs is not that trustworthy. It uses magetometers, and will show huge drifts (up to 25 degrees over 2m in our case) near ferromagnetic masses, such as the one you can find in building walls. What we did to solve this is to use the IMU heading, but estimate a heading bias.

If you are outdoors, don't forget that travelling 10m on a 10 degree incline does not lead to the same change in X and Y than travelling 10m on a flat terrain. This is usually accounted for by estimating Z, but I guess it can be estimated differently.

GPS is also a lying bitch, typically in high-multipath environments. Plus low-grade (and even in some conditions high-grade) GPSes have a tendency to report very wrong standard deviations. We used some simple chi-square tests to check whether a particular GPS measurement should be integrated (i.e. checking that it matches the current filter estimate up to a certain point), which gave us decent results.

The "typical" solution for us is to use odometry + IMU to get an ego-motion estimate and then use GPS to correct X,Y,Z and heading bias.

Here is an EKF implementation that we extensively used. If you need to estimate the IMU's orientation (i.e. if it does not already have a built-in filter), you can also use on of these two filter: UKF and EKF.

ContentLicense: CC BY-SA 4.0


问:
Using kinect for medical application but without computer. Is it possible?
I have to use kinect for an application. However,  the final work must be mobile: it means no computer. Consequently, I thought using a microcontroller to handle data from kinect. But is it possible? My job is mesuring some points of a body (axis X, Y, Z) and get back these coordinates. I don't know if I'm enough accurate. </p>

答:
There MCU which provide enough calculation power. I'd say "the bigger, the better". For example a ARM-CORTEX M4

This is enough with you just want to extract some coordinates

The protocol of the kinect is pretty simple (http://openkinect.org/wiki/Protocol_Documentation). You can easily implement a communication between the Kinect and the MCU


答:
Well I saw in the first line an error.

```<code>body_pos_current_.x_dot_2 =  ( thrust_.total / masse_ ) * ( sin( body_ang_current_.theta ) * cos( body_ang_current_.phi ) * cos( body_ang_current_.psi ) + sin( body_ang_current_.psi ) * cos( body_ang_current_.phi ) );
</code>```

I think it should be:

```<code>body_pos_current_.x_dot_2 =  ( thrust_.total / masse_ ) * ( sin( body_ang_current_.theta ) * cos( body_ang_current_.phi ) * cos( body_ang_current_.psi ) + sin( body_ang_current_.psi ) * sin( body_ang_current_.phi ) );
</code>```

Last cos() changed to sin(), then it should represent the same as the formula

I'm not sure if this solve your problem, there may be other code-errors i didn't noticed on the first glance

Title: PD Algorithm for a Quadrotor [Simulation]

Body:
I have a big problem trying to stabilize a quadrotor with a PD controller.
The model and the program has been written in C++ and the model dynamic has been taken from this source in internet:

Well, in my code I wrote the model like in the eq. system ( see eq. 3.30 on page 21):

```<code>/* Calculate the acceleration about all 6 axis */
body_pos_current_.x_dot_2 =  ( thrust_.total / masse_ ) * ( sin( body_ang_current_.theta ) * cos( body_ang_current_.phi ) * cos( body_ang_current_.psi ) + sin( body_ang_current_.psi ) * cos( body_ang_current_.phi ) );
body_pos_current_.y_dot_2 =  ( thrust_.total / masse_ ) * ( sin( body_ang_current_.theta ) * sin( body_ang_current_.psi ) * cos( body_ang_current_.phi ) - cos( body_ang_current_.psi ) * sin( body_ang_current_.phi ) * cos( body_ang_current_.psi ) );
body_pos_current_.z_dot_2 =  ( thrust_.total / masse_ ) * ( cos( body_ang_current_.theta ) * cos( body_ang_current_.phi ) ) - 9.81;

body_ang_current_.phi_dot_2   = ( torque_.phi   / Jxx_ );
body_ang_current_.theta_dot_2 = ( torque_.theta / Jyy_ );
body_ang_current_.psi_dot_2   = ( torque_.psi   / Jzz_ );
</code>```

where body_ang_current.<angle> and body_pos_current_.<position> are structures defined in a class to store position, velocities and accelerations of the model given the 4 motor velocities about all 3 axis.

$$
\large \cases{
 \ddot X = ( \sin{\psi} \sin{\phi} + \cos{\psi} \sin{\theta} \cos{\phi}) \frac{U_1}{m}  \cr
 \ddot Y = (-\cos{\psi} \sin{\phi} + \sin{\psi} \sin{\theta} \cos{\phi}) \frac{U_1}{m} \cr
 \ddot Z = (-g + (\cos{\theta} \cos{\phi}) \frac{U_1}{m} \cr
 \dot p  = \frac{I_{YY} - I_{ZZ}}{I_{XX}}qr - \frac{J_{TP}}{I_{XX}} q \Omega + \frac{U_2}{I_{XX}} \cr
 \dot q  = \frac{I_{ZZ} - I_{XX}}{I_{YY}}pr - \frac{J_{TP}}{I_{YY}} p \Omega + \frac{U_3}{I_{YY}} \cr
 \dot r  = \frac{I_{XX} - I_{YY}}{I_{ZZ}}pq - \frac{U_4}{I_{ZZ}}
}
$$

Once I get the accelerations above I m going to integrate them to get velocities and positions as well:

```<code>/* Get position and velocities from accelerations */
body_pos_current_.x_dot = body_pos_current_.x_dot_2 * real_duration + body_pos_previous_.x_dot;
body_pos_current_.y_dot = body_pos_current_.y_dot_2 * real_duration + body_pos_previous_.y_dot;
body_pos_current_.z_dot = body_pos_current_.z_dot_2 * real_duration + body_pos_previous_.z_dot;

body_ang_current_.phi_dot   = body_ang_current_.phi_dot_2   * real_duration + body_ang_previous_.phi_dot;
body_ang_current_.theta_dot = body_ang_current_.theta_dot_2 * real_duration + body_ang_previous_.theta_dot;
body_ang_current_.psi_dot   = body_ang_current_.psi_dot_2   * real_duration + body_ang_previous_.psi_dot;

body_pos_current_.x = 0.5 * body_pos_current_.x_dot_2 * pow( real_duration, 2 ) + ( body_pos_previous_.x_dot * real_duration ) + body_pos_previous_.x;
body_pos_current_.y = 0.5 * body_pos_current_.y_dot_2 * pow( real_duration, 2 ) + ( body_pos_previous_.y_dot * real_duration ) + body_pos_previous_.y;
body_pos_current_.z = 0.5 * body_pos_current_.z_dot_2 * pow( real_duration, 2 ) + ( body_pos_previous_.z_dot * real_duration ) + body_pos_previous_.z;

body_ang_current_.phi   = 0.5 * body_ang_current_.phi_dot_2   * pow( real_duration, 2 ) + ( body_ang_previous_.phi_dot   * real_duration ) + body_ang_previous_.phi;
body_ang_current_.theta = 0.5 * body_ang_current_.theta_dot_2 * pow( real_duration, 2 ) + ( body_ang_previous_.theta_dot * real_duration ) + body_ang_previous_.theta;
body_ang_current_.psi   = 0.5 * body_ang_current_.psi_dot_2   * pow( real_duration, 2 ) + ( body_ang_previous_.psi_dot   * real_duration ) + body_ang_previous_.psi;

/* Copy the new value into the previous one (for the next loop) */
body_pos_previous_.x = body_pos_current_.x;
body_pos_previous_.y = body_pos_current_.y;
body_pos_previous_.z = body_pos_current_.z;

body_pos_previous_.x_dot = body_pos_current_.x_dot;
body_pos_previous_.y_dot = body_pos_current_.y_dot;
body_pos_previous_.z_dot = body_pos_current_.z_dot;

body_ang_previous_.phi   = body_ang_current_.phi;
body_ang_previous_.theta = body_ang_current_.theta;
body_ang_previous_.psi   = body_ang_current_.psi;

body_ang_previous_.phi_dot   = body_ang_current_.phi_dot;
body_ang_previous_.theta_dot = body_ang_current_.theta_dot;
body_ang_previous_.psi_dot   = body_ang_current_.psi_dot;
</code>```

The model seems to work well but, as like reported in many papers, is very unstable and needs some controls.

The first approach for me was to create a controller (PD) to keep the height constant without moving the quadcopter, but just putting a value (for example 3 meter) and see how it reacts.

Here the small code I tried:

```<code>/* PD Controller */
double e = ( 3.0 - body_pos_current_.z );  // 3.0 is just a try value!!!
thrust_.esum = thrust_.esum + e;
thrust_.total = 1.3 * e + 0.2 * real_duration * thrust_.esum;
</code>```

The problem, as you can see here in this video, is that the copter starts falling down into the ground and not reaching the desired altitude (3.0 meters).
Then it comes back again again like a spring, which is not damped.
I tried already many different value for the PD controller but it seems that it doesn't affect the dynamic of the model.

Another strange thing is that it goes always to a negative point under the ground, even if I change the desired height (negative or positive).

What s wrong in my code?
Could you me please point to some documents or code which is understandable and well documented to start?

Thanks

EDIT:
Many thanks to your suggestion.
Hi was really surprise to know, that my code had lots of potential problems and was not very efficient. So I elaborate the code as your explanation and I implementers a RK4 for the integration. After I ve read those articles: here and here I got an idea about RK and its vantage to use it in simulations and graphics PC.
As an example I rewrote again the whole code:

```<code>/* Calculate the acceleration about all 6 axis */
pos_.dVel.x =  ( ( thrust_.total / masse_ ) * ( -sin( body_position_.angle.theta ) * cos( body_position_.angle.phi ) * cos( body_position_.angle.psi ) - sin( body_position_.angle.phi ) * sin( body_position_.angle.psi ) ) );
pos_.dVel.y =  ( ( thrust_.total / masse_ ) * ( sin( body_position_.angle.phi ) * cos( body_position_.angle.psi ) - cos( body_position_.angle.phi ) * sin( body_position_.angle.theta ) * sin( body_position_.angle.psi ) ) );
pos_.dVel.z =  ( ( thrust_.total / masse_ ) * ( -cos( body_position_.angle.phi ) * cos( body_position_.angle.theta ) ) - 9.81 );

pos_.dOmega.phi   = ( torque_.phi   / Jxx_ );
pos_.dOmega.theta = ( torque_.theta / Jyy_ );
pos_.dOmega.psi   = ( torque_.psi   / Jzz_ );

/* Get position and velocities from accelerations */
body_position_ = RKIntegrate( body_position_, real_duration );
</code>```

which is much more clear and easy to debug. Here some useful functions I implemented:

```<code>QuadrotorController::State QuadrotorController::evaluate( const State &initial, const Derivative &d, double dt ) {

State output;
output.position.x = initial.position.x + d.dPos.x * dt;
output.position.y = initial.position.y + d.dPos.y * dt;
output.position.z = initial.position.z + d.dPos.z * dt;

output.velocity.x = initial.velocity.x + d.dVel.x * dt;
output.velocity.y = initial.velocity.y + d.dVel.y * dt;
output.velocity.z = initial.velocity.z + d.dVel.z * dt;

output.angle.phi   = initial.angle.phi   + d.dAngle.phi   * dt;
output.angle.theta = initial.angle.theta + d.dAngle.theta * dt;
output.angle.psi   = initial.angle.psi   + d.dAngle.psi   * dt;

output.omega.phi   = initial.omega.phi   + d.dOmega.phi   * dt;
output.omega.theta = initial.omega.theta + d.dOmega.theta * dt;
output.omega.psi   = initial.omega.psi   + d.dOmega.psi   * dt;

return output;

};

QuadrotorController::Derivative QuadrotorController::sampleDerivative( double dt, const State &sampleState ) {

Derivative output;

output.dPos = sampleState.velocity;
output.dVel.x = pos_.dVel.x;
output.dVel.y = pos_.dVel.y;
output.dVel.z = pos_.dVel.z;

output.dAngle = sampleState.omega;
output.dOmega.phi   = pos_.dOmega.phi;
output.dOmega.theta = pos_.dOmega.theta;
output.dOmega.psi   = pos_.dOmega.psi;

return output;

};

QuadrotorController::State QuadrotorController::RKIntegrate( const State &state, double dt ) {

const double C1 = 0.0f;
const double C2 = 0.5f, A21 = 0.5f;
const double C3 = 0.5f, A31 = 0.0f, A32 = 0.5f;
const double C4 = 1.0f, A41 = 0.0f, A42 = 0.0f, A43 = 1.0f;

const double B1 = 1.0f/6.0f, B2 = 1.0f/3.0f, B3 = 1.0f/3.0f, B4 = 1.0f/6.0f;

Derivative k1 = sampleDerivative( 0.0f, state );
Derivative k2 = sampleDerivative( C2 * dt, evaluate( state, k1 * A21, dt ) );
Derivative k3 = sampleDerivative( C3 * dt, evaluate( state, k1 * A31 + k2 * A32, dt ) );
Derivative k4 = sampleDerivative( C4 * dt, evaluate( state, k1 * A41 + k2 * A42 + k3 * A43, dt ) );

const Derivative derivativeSum = k1 * B1 + k2 * B2 + k3 * B3 + k4 * B4;

return evaluate( state, derivativeSum, dt );
}
</code>```

Now I m really lost because...because the simulated qudrotor has the same behavior as before. Nevertheless I ve implemented the same PD algorithm as discussed in the paper, it stabilize on Z (height) but it get really crazy due to unstable behavior.
So... I dunno what is wrong in my code and my implementation. And above all I cannot find any source in internet with a good self explaned dynamic model for a quadrotor.

Regards

Body:
Well I saw in the first line an error.

body_pos_current_.x_dot_2 =  ( thrust_.total / masse_ ) * ( sin( body_ang_current_.theta ) * cos( body_ang_current_.phi ) * cos( body_ang_current_.psi ) + sin( body_ang_current_.psi ) * cos( body_ang_current_.phi ) );

I think it should be:

body_pos_current_.x_dot_2 =  ( thrust_.total / masse_ ) * ( sin( body_ang_current_.theta ) * cos( body_ang_current_.phi ) * cos( body_ang_current_.psi ) + sin( body_ang_current_.psi ) * sin( body_ang_current_.phi ) );

Last cos() changed to sin(), then it should represent the same as the formula

I'm not sure if this solve your problem, there may be other code-errors i didn't noticed on the first glance


问:
Non-linear complementary filter on so3: Corrected equations?
While reading the paper "Multirotor Aerial Vehicles: Modeling, Estimation, and Control of Quadrotor" by Mahony, Kumar and Corke, I stumbled across the following equations for a non-linear attitude observer, which I would like to implement, but I believe there is something wrong.

$\dot{\hat{R}} := \hat{R} \left( \Omega_{IMU} - \hat{b} \right)_\times - \alpha \\
\dot{\hat{b}} := k_b \alpha \\
\alpha := \left( \frac{k_a}{g^2}((\hat{R}^T \vec z) \times a_{IMU}) + \frac{k_m}{|^Am|^2} ((\hat{R}^T {^Am}) \times m_{IMU}) \right)_\times + k_E \mathbb{P}_{so(3)} (\hat{R} R_E^T)$

Where $\hat{R}$ and $\hat{b}$ are etimates of orientation and gyroscope bias, $\Omega_{IMU}, a_{IMU}, m_{IMU}, R_E^T$ are measurements and $k_X$ are scalar gains, which may be set to 0 for measurements that are not evailable.

Now $\dot{\hat{R}}$ and $\alpha$ need to be matrices $\in \mathbb{R}^{3\times 3}$ due to their definitions. $\hat{b}$ and thus $\dot{\hat{b}}$ need to be vectors $\in \mathbb{R}^3$. But then what is the correct version of the second equation $\dot{\hat{b}} := k_b \alpha$?

答:
I've implemented this algorithm before but I found a different paper easier to read. Try find the paper by Hamel and Mahoney (with Hamel listed as the first author).

From that paper, which I don't have available at the moment, I believe the first equation should was
$$
\begin{align}
\dot{\hat{R}} &= \hat{R} \left ( \Omega_{IMU} - \hat{b} - \alpha \right )_\times
\end{align}
$$
but in that paper $\alpha$ ($\omega$ in the paper) was a vector.

Another thing to look at for the equations you have listed is if the second equation shouldn't have the operator transforming $\alpha$ from its skew-symmetric matrix form to a vector on $\mathbb{R}^3$.


问:
Reverse lift mechanism
I have made a RC robot from a wheelchair and I'm planning to attach a snow plow. I'm wondering if there is any mechanism that would be able to lift the plow when reversing.
I have only 2 channel transmitter so I can't control the plow's movement through it so I was thinking of some mechanical lift that triggers when reversing.

Do you guys know about something I could use for it?
Thanks.

答:
I assume that your 2 channels control forward/backward and left/right.  But even if the 2 channels control forward/backward in each wheel (differential-drive style), it should still be possible to do what you are suggesting electrically instead of mechanically.

You should be able to read the input signal to the motors, decide whether those signals are commanding a "reverse" movement, and trigger your snow plow lift motor accordingly.

Title: Reverse lift mechanism


问:
Quadcopter cannot balance!
I am bulding a quadcopter using these compenents:

Microcontroller: Tiva C Lanchpad (ARM Cortex M4 - 80 MHz), but
running at 40MHz in my code

MPU 9150 - Sensorhub TI

ESC - Hobbywing Skywalker 40A

I use the sample project comp_dcm from Tivaware and use that angles for my PID which running at 100Hz

I test PID Control on 2 motors, but the motors oscillate as in the video i found on youtube from one guy!

Quadcopter  Unbalance

答: It looks like your I term is turned up too high.  This can cause the system to overcompensate before it has even started to return to equilibrium.  </p>

Title: Quadcopter cannot balance!


问:
Is there an analytical solution for inverse kinematics of a 6 DOF serial chain?
Let's take a 6 DOF robotic structure. It's consisting of the 3 DOF global structure for the position - and the 3 DOF local structure for the orientation of the endeffector.

If the last 3 axis (of the local structure) are coincident in one point, the inverse kinematics can be solved analytically by decomposing it into a position- and orientation-problem.

But is it possible to solve the inverse kinematics analytically if the last 3 axis are NOT coincident in one point? I've read several papers that claim that due to high non-linearity of the trigonometric functions and motion complexity in 3D-space, a 6 DOF serial chain cannot be solved analytically.

Does anybody know if this is right?

答:
The problem of inverse kinematics to a general 6-degrees-of-freedom serial robot was considered hard for a long time. Nevertheless it was solved and the solution in Raghavan and Roth (1993) is a widely acknowledged method, and improvements have also been made since (see e.g., Husty, Pfurner and Schröcker (2007)).

Although they provides a strategy to solve the inverse kinematics analytically, they do not give the solutions in closed form. All methods stop at a point where a single equation in one unknown variable, but a polynomial of degree 16 is obtained. The solutions to the remaining five variables are expressed in terms of this unknown, which can be found once the polynomial is solved numerically.  Further, this polynomial is of degree 16 only in the worst case scenario, where all the joints are rotary. Any further simplification in the architecture only reduce the degree of this polynomial.

These methods use advanced mathematical techniques to solve the problem, which are beyond the scope of this space but a simplified outline of the steps followed in Raghavan and Roth (1993) can be seen in slides 82-91 of this article.


问:
Algebraic and geometric in inverse kinematic
I'm just wondering that is there any case that when algebraic way can't solve the problem while the geometric can ? Cause I'm working on a 2DOF robotics arm This one, I know the length of L1 and L2, location that I want for the end effector, then I tried calculating the angles by using algebraic but it gave me cos(alpha) > 1, but when I tried solving with geometric, I can find the solution, so is it because I use a wrong way in algebraic ?

Thank you very much.

答:
Absolutely not! If the geometric method yields a solution, so should the algebraic method or any other method for that matter, if done correctly.

The problem posed here is the inverse kinematics of a 2R serial robot. This is a trivial example solved in many books in different ways.  I would recommend this article for an exposition on obtaining the solution to this problem (and more) in different ways.

The question of defining a "better" method is better left alone and depends on the problem at hand. Algebraic methods are more generic and can be applied to any mechanism, whereas geometric methods are easy to visualize and so seem simple but are prone to errors as the mechanism at hand gets complicated (imagine solving spatial robots of more than 3-degrees of freedom or a general 6R robot geometrically!).


答:
"behavior indicative of a LiPo going bad" - in my experience this is usually a puffy battery, or a fire. My understanding of it is when a LiPo goes bad, the individual cells within it are not charging and/or discharging evenly, and that indirectly leads to the previously mentioned symptoms.

Can you check the voltage balance after a bad flight? Are they more than a 1/10  volt different?  Can you verify other exact same batteries are performing adequately, and thus your C rating (burst amperage) is appropriate?

I suppose it is possible to exhibit the behavior you describe, but that would mean the specifications were very close to begin with, you should try allow a little more room (ie a higher C rating, at a higher cost $$ of course).

This is pretty much the same thing movrev said in his answer - so mark it instead of this one.

Title: How can one determine whether a LiPo battery is going bad?

Body: In our lab we use LiPo batteries to power our quadrotors. Lately we have been experiencing stability issues when using certain batteries. The batteries seem to charge and balance normally and our battery monitor indicates they are fine even when putting them under load. However when we attempt to fly the quadrotor with one of these batteries, manually or autonomously, it has a severe tendency to pitch and/or roll. My guess is that the battery is not supplying sufficient power to all the motors which brings me to my question. Is this behavior indicative of a LiPo going bad? If so what is the best way to test a battery to confirm my suspicions?</p>


问:
What is a PID as is related to quadcopters
I'm trying to make a Quadcopter from scratch, I have a fair amount of experience with adruinos, and I'm trying to understand how to necessary systems work, and I can't seem to figure out what PID means, is it a method of regulating pitch and roll? like a stabilizer? I think from what I've read that its a system that detects orientation of the craft and tries to correct it</p>

答: The term PID itself refers to Proportional plus Integral plus Derivative control, which is a very common control technique. The IMU (inertial measurement unit) measures the orientation of the quadrotor, and the roll/pitch/yaw values are used in the PID controller to calculate motor commands.</p>


问:
Are consumer grade CNC machines capable of cutting tile?
I'd like to slice and dice floor tile into pieces so I can arrange it in geometric patterns. I have CAD designs for the parts. Would any consumer grade CNC machine be capable of doing the job?</p>

答:
The answer is no!

Even if you ask: Is any professional CNC machine, build for metal/plastics/wood cutting, capable of cutting tile?
The answer is still no!
You can try it. It will work in the beginning but the machine will be destroyed soon.

Why?

Stone cannot be cut, except soapstone. Stone machining consist of crushing/cracking/grinding, cutting is impossible. Most common milling machines are build for cutting.

During the stone milling process there won't be any chips only "stone dust"
There are parts of your machine which are able to protect all the moving parts of your machine from cutting chips.
These parts have to be designed to provide protection against very small particles like dust. Usually they aren't, so the dust will come into your spindles, guides and gears and in the end it will damage them.

If you want to work with your milling machine for a long time then I would rather not recommend it.

You can do it. Then you definitely need water for the "cutting" process ( this will decrease the amount of dust) and very hard tools (diamond tools are recommended).


问:
Euler Angles from 9DOF IMU
Using the Adafruit 9DoF module I Need to convert the Accel + Magneto + Gyro into Euler Angles for a motion capture application. Any hints on where to start?

Managed to get X,Y,Z when the IMU is facing upward but when that orientation changes the axes dont behave normally that is because i am not using Euler angles. So Any hints to any reference where to start?

The Euler Compass App is an example of what I am trying to get to.
Get Pitch,Yaw, Roll for the IMU module irrespective to how its kept.

答:
Using this reference
RAZOR 9DOF DCM Algorithms

DCM Sorts it problem. :) Solution works as good as the EUler Compass on the PlayStore.
If anyone is working on it, convert the Adruino code to C++ and use your own interfacing libs.

Works like a charm.

Title: Euler Angles from 9DOF IMU


问:
Comprehensive comparison of SLAM algorithms
I'm looking for a research paper or series of papers that compare the performance of various simultaneous localization and mapping algorithms for rovers in a variety of real world environments.  In particular, i'm looking for computational speed, accuracy (compared to the real world environment) and memory & power efficiency metrics.  Is there a journal that regularly publishes experimental performance comparisons?</p>

答:
This might be useful

An evaluation of 2D SLAM techniques available in Robot Operating System


问:
Udoo board + Kinect sensor?
I am wondering if it would be possible to get Kinect to work with Udoo board (Quad). I have found that there is now support for ROS + Udoo. Also saw a question asked about Xtion + Udoo which shows some more interest. It would really be great if it could be possible for Kinect+Udoo. Was hoping to implement perhaps a miniature version of TurtleBot. I wish someone could give some insights on this matter. Thanks.</p>

答:
Someone I know has tested the Kinect on the Udoo and it works fairly well on the quad core Udoo, the resources might be tighter on the dual core. You can install the debs for everything you need in ROS, including the freenect packages, from .deb binaries. Just follow the guide here:

http://wiki.ros.org/hydro/Installation/UDOO

Then you can install the debs with this command:

```<code>$ sudo apt-get install ros-hydro-freenect-launch
</code>```

Afterwards you should be able to plugin the Kinect and run this:

```<code>$ roslaunch freenect_launch freenect.launch
</code>```

In another terminal you can do something like this to make sure it is working:

```<code>$ rostopic hz /camera/rgb/image_raw
</code>```

Some tips to using the Kinect and the freenect drivers in ROS on a resource constrained system like the Udoo:

Use nodelets (wiki.ros.org/nodelet)

freenect_camera provides a nodelet version and if you write your code as nodelet's you can run them in the same process and save a lot of processing due to IPC.

If you need a simulated laserscan and the Kinect is level with the ground, use depthimage_to_laserscan

depthimage_to_laserscan is more efficient than pointcloud_to_laserscan because then you don't need to convert the images from the Kinect into a point cloud first. (Both of these come in nodelet form as well)

Title: Udoo board + Kinect sensor?

Body:
Someone I know has tested the Kinect on the Udoo and it works fairly well on the quad core Udoo, the resources might be tighter on the dual core. You can install the debs for everything you need in ROS, including the freenect packages, from .deb binaries. Just follow the guide here:

http://wiki.ros.org/hydro/Installation/UDOO

Then you can install the debs with this command:

$ sudo apt-get install ros-hydro-freenect-launch

Afterwards you should be able to plugin the Kinect and run this:

$ roslaunch freenect_launch freenect.launch

In another terminal you can do something like this to make sure it is working:

$ rostopic hz /camera/rgb/image_raw

Some tips to using the Kinect and the freenect drivers in ROS on a resource constrained system like the Udoo:

Use nodelets (wiki.ros.org/nodelet)

freenect_camera provides a nodelet version and if you write your code as nodelet's you can run them in the same process and save a lot of processing due to IPC.

If you need a simulated laserscan and the Kinect is level with the ground, use depthimage_to_laserscan

depthimage_to_laserscan is more efficient than pointcloud_to_laserscan because then you don't need to convert the images from the Kinect into a point cloud first. (Both of these come in nodelet form as well)


答:
It sounds like you're asking 2 questions:

Is an imbalance in motor performance indicative of a failing battery?

How would you test for a failing battery?

The answer to the first question is "maybe".  We had an issue on one of the AUVs I used to work on, where sometimes at the end of the day it would lose all heading control.  By chance, we discovered that one of the motor controllers (used for differential drive) stopped working when it dipped below a certain voltage, where the other one were unaffected.  So in that sense, the imbalance in motor performance was related to the battery but not necessarily a battery failure.

The best way to rule out the battery would be to tie your quadrotor (with only a few inches of freedom) to a table, and measure the battery voltage while you manually control it.  Does it drop below what you are expecting, indicating an inability to supply enough current?  Another way to do it would be to replace the battery with a simulated battery (variable voltage source + variable resistor) and test whether dips in the output voltage or increased internal resistance are to blame.

The answer to the second question is more complicated, but a decent resource for it is here:
Testing Lithium-based Batteries

Essentially, you must measure the response times of the battery to changes in load.


问:
Can't Read current on Pololu Dual MC33926 Motor Driver Shield for Arduino
I purchased a Pololu Dual MC33926 Motor Driver Shield for Arduino, and for some reason I cannot read current from the motor controller. On the Serial.println() it just prints weird data (garbage), and when I use ROS (Robot Operating System) I only see -0.0 (minus zero) value for both motors.

All I've done is plug the shield on my Arduino UNO R3 model, and run the demo that comes with the sample library --
http://github.com/pololu/dual-mc33926-motor-shield .

How can I fix this issue?

答:
When there is miscommunications between the microcontroller and a chip, assuming the electronics are not damaged, there could be a couple of things that can go wrong. Of course, further diagnosis is required. Typical things that could go wrong are (generally, not just in your particular case):

Connections: Is the chip connected to the correct power supply? Is it soldered correctly? Did you connect the right pins together?

A tester (multimeter) along with the datasheets are helpful to verify that the connections are alright.

Bus requirements: Are all requirements of the bus fulfilled? Some buses (e.g. CAN) need terminating resistors. Some have limitations on the the length.

The microcontroller datasheets usually have information and even useful example pictures that show how the bus must be connected.

Noise: Assuming no external noise, on high frequencies the bus has to meet other criteria. For example 90 degree bends cause reflection.

An oscilloscope helps here. If there are reflections, you would see noise over your signal. If the noise makes the signal go as far as reaching the middle voltage (between logical 0 and 1), there is a good chance the hardware would misread the bit. This is especially bad on the clock bus, if any.

Configuration: Are the pins and peripherals of the microcontroller correctly configured?

Read the datasheets very carefully and pay attention to every single line. Don't take one line "notes" lightly, they often contain information that if missed would make the peripheral not work. Look at the errata of the microcontroller as well for possible hardware bugs.

Since your bus is UART, I'd say number 2 and 3 are likely not an issue. After checking the connections, I would verify that the configuration is ok. As kinkilla commented, it could very well be that you configured the wrong baud rate for your bus.

If you still cannot figure out what the problem is, it is best to attach an oscilloscope and see what signals are actually on the wires.  There are very good oscilloscopes that are expensive (but you may have access to if your lab or work environment has one), but there are also very cheap ones that attach to the computer with USB and all the software runs there. The cheap ones cannot measure with as much detail as the expensive ones, but with UART speeds that is not at all an issue.

That said, you can check with oscilloscope what signal is on the wire. If the signal has a bad shape and is coming from a chip you didn't configure (so we can assume it should work correctly out of the box), it could be that the bus is shorted with another or the microcontroller is badly configured (so there are more than one person driving the bus. If the signal is coming from the microcontroller, there could be similar reasons as well as bad configuration. If the signal is ok, then you are receiving it badly. If the bus is correctly connected to the microcontroller, then the cause is most likely the configuration of the microcontroller.


问:
Linear slider motor mount location - Pros/cons
I'm currently designing a linear camera slider, that will be used to hold camera equipment weighing just about 15 Kgs including all of the lenses and monitors and everything else.

For those who don't know what a camera slider is, it's a linear slider on top of which a camera is mounted and then the camera is slided slowly to create some nice footage like this.

The problem

Now, looking at the commercially available camera sliders out there, there seems to be two ways in which the motor maybe mounted on these sliders:

Motor mounted on the side:

Motor mounted directly on the carriage:

I would like to know which option would be optimal - Performance-wise (this slider maybe used vertically too, to create bottom to top slide shots), efficiency-wise and
which one of these two will be resistant to motor vibration (these motors vibrate a lot, the effects of which may sometimes leak into the produced footage).

Additional Questions

Motor mounted on the carriage directly maybe, just maybe more efficient, but it also has to carry it's own weight in addition to the 15kg camera load?

Pulling force is greater than pushing force (I have no idea why, would be great if someone explained why, atleast in this case?), so a motor mounted in the end should be able to lift vertically with ease?

Does a belt setup as shown in the first figure above really dampen the motor vibrations? Will/won't the motor vibrating on the end get amplified (because, the whole setup will be attached to a single tripod in the exact center of the slider)

Which design will be less stressful for the motor, taking inertia into consideration for both cases?

Which one of these designs will be best suitable for vertical pulling of load against gravity?

Manufacturers use both designs interchangeably, so it's hard to predict which design is better than which.

Any help would be much appreciated!

Please note, this question has been migrated from the Stackexchange Physics (and Electrical) forum by me because the mods thought it would be appropriate here.

答:
Performance-wise, first version wins as the motor's own weight is not added to the payload for motion.

Efficiency-wise, it depends on the elasticity and material properties of the drive belt, versus the material of the powered roller surface, gripping force of rollers, and other energy loss factors.

The belt drive will be far more vibration-damped: Note the spring-loaded tensioning pin through which the belt passes in the top photo. This will flex and absorb a lot of the vibration - acting like a low-pass filter for mechanical energy.

With regard to "these motors vibrate a lot": If vibration reduction is important, a stepper motor might be the worst possible choice. Use a linear encoder on the slider for closed-loop position feedback, and drive the unit using a BLDC motor - effectively making a linear servo out of the whole thing. BLDCs are much less vibration prone: Note the smoothness of rotation of a CDROM drive main motor. Of course, for meaningful traversal speeds, the BLDC would be geared down significantly, with the side effect of even more vibration reduction. For that matter, even a brushed DC motor with suitable gearing down would work great, again with the linear encoder.

The first version will require slightly less power, and will suffer marginally less inertia effects: The motor in the photograph shown most likely weighs under 1 Kg, that will add to the payload weight for movement energy as well as starting and stopping inertia. However, the flexing drive belt will eat up some (maybe a lot) of the improvement in efficiency.

Pulling versus pushing force, not sure how that applies. The first uses a closed loop belt with tensioner, i.e. pulling force is all that applies. The second uses rotation-translation, no pushing there either.

A belt drive with a spring loaded tensioner will significantly damp vibration of the motor, except in the odd chance of the motor hitting a resonant harmonic frequency of the belt assembly (can't quite see that happening).

Lower inertia = lower stress on motor. So clearly the first version is better, see above.

For vertical pulling, a belt drive instinctively feels safer: Dust getting between rollers and slide, or the powered rollers wearing out, may cause the entire assembly to slide down under gravity. Of course, this safety is illusory, as the drive belt could break too. Adding an auto-brake design to protect against slippage or breakage (like some elevators have) just adds complication.


问:
What are some low cost alternatives for lidar?
It need not be as effective as lidar or it may have some disadvantages when compared with lidar.   What are the probable alternatives?

Edit:
I'm intending to use it outdoors for navigation of autonomous vehicle. Is there any low cost LIDAR or is there any alternative sensor for obstacle detection?

答:
Depending on what your actual use case is, you might be able to use

Stereo cameras: Needs more processing power on board, usually smaller field of view

Kinect

There are some kickstarter projects that promise some low cost alternatives as well. Example


答: Your cheapest option will probably be to use a ultrasonic sensor on a stepper, by advancing the server one step and reading the distance. The only downside to this will be the resolution you can get, which will be depend on your ultrasonic sensor. Another thing to consider is that ultrasonic waves don't work well on curved surfaces so the accuracy could decrease if you were detecting say a ball.</p>

Title: What are some low cost alternatives for lidar?

Body:
It need not be as effective as lidar or it may have some disadvantages when compared with lidar.   What are the probable alternatives?

Edit:
I'm intending to use it outdoors for navigation of autonomous vehicle. Is there any low cost LIDAR or is there any alternative sensor for obstacle detection?


问:
Starting out advice on making robots and tinkering with microcontrollers
I'd like to start making robots and tinkering with microcontrollers. Where do I start, and what do I need?

I'd like to make my own robots. I'm comfortable with programming (assembly and C) so I've got that part covered, but my electronics/circuits knowledge is a little weak. I have no idea what material to start with and which tools I need, nor how to put stuff together.

For the microcontroller, I'm thinking about going with the Pololu Orangutan LV-168 or the Arduino Duemilanove, although I'm leaning more towards the Orangutan because of the built-in LCD and pushbuttons (which I would expect to use, especially for debugging and user interaction). Am I on the right track? It seems to me like the number of I/O ports is small, but is that the case in practice?

答:
If you have no idea what to do with your microcontroller, I would start slower. I wouldn't say using a Kinect is that great an idea right now.

Some of the other answers mentioned using development kits and boards to avoid soldering, which I think is a great way to get started with embedded programming without having to worry about soldering practically anything.

My tip is to look into a great website I found when I knew nothing about robots: Society of Robots and their great $50 Robot Tutorial! There you will find a lot of basic information for people who want to build their own robots.


问:
Quadrature Encoder Counts
Actually , I have been since two weeks looking for convinced and final solution for my problem , actually I am completely lost , I am working on mobile robot (Rover 5) with 2 motors , 2 encoders . the controller that designed to the robot needs to know the odometery of mobile robot (X ,Y, Heading Angle ) , actually I am trying to function the encoders for this purpose , getting X ,Y, Heading Angle by measuring the traveled distance by each wheel , so to get the X ,Y, Heading Angle values , I should compute a accurate readings without missing any counts or ticks as could as possible .

The problem now is :

In the code in the attachment , while I am testing the encoders counts , I noticed that there is a difference between counts of encoders even when they spin in the same constant speed (PMW) , the difference increases as the two motors continue . so I thought that is the main cause of inaccurate odometery results .

In the output of the code (in the attachment also) the first two columns are right and left motors speed , the third & forth columns are right and left encoder counts , the fifth column is the difference between two encoders count , as you could see ,that even when the speed of two motors are approximately the same (each motor feed up with 100 PWM) there is a difference in the encoder counts and as you could see that the difference become big and big as the motors continuing spin .

One thing I thought that sending the same PWM value to two different motors will almost never produce the exact same speed , so I think that I should detect the absolute motion of the motors and adjust the power to get the speed/distance , but when I test the speed of motors after feed them with 100 PWM at same time , the two speeds were almost identical , but I noticed that there is a difference between counts of two encoders even when the motors spin in the same constant speed .

Actually , I don't know where is the problem , Is it in the code ? Is it in the hardware ? or what ? I am completely lost , I need for patient someone to help.

```<code>/* Encoder-ino.ino
*/
#define encoder0PinA 2
#define encoder0PinB 4
#define encoder1PinA 3
#define encoder1PinB 5

volatile int encoder0Pos = 0;
volatile int encoder1Pos = 0;
int WR=100;  // angular velocity of right wheel
int WL=100;  // angular velocity of right wheel

long newposition;
long oldposition = 0;
unsigned long newtime;
unsigned long oldtime = 0;
long vel;

long newposition1;
long oldposition1 = 0;
unsigned long newtime1;
unsigned long oldtime1 = 0;
long vel1;

int ENA=8;    // SpeedPinA connected to Arduino's port 8
int ENB=9;    // SpeedPinB connected to Arduino's port 9

int IN1=48;    // RightMotorWire1 connected to Arduino's port 48
int IN2=49;    // RightMotorWire2 connected to Arduino's port 49

int IN3=50;    // RightMotorWire1 connected to Arduino's port 48
int IN4=51;    // RightMotorWire2 connected to Arduino's port 49

void setup() {
 pinMode(ENA,OUTPUT);
 pinMode(ENB,OUTPUT);
 pinMode(IN1,OUTPUT);
 pinMode(IN2,OUTPUT);
 pinMode(IN3,OUTPUT);
 pinMode(IN4,OUTPUT);

 digitalWrite(ENA,HIGH);    //enable motorA
 digitalWrite(ENB,HIGH);    //enable motorB
  pinMode(encoder0PinA, INPUT);
  pinMode(encoder0PinB, INPUT);
  pinMode(encoder1PinA, INPUT);
  pinMode(encoder1PinB, INPUT);
// encoder pin on interrupt 0 (pin 2)
attachInterrupt(0, doEncoderA, CHANGE);

// encoder pin on interrupt 1 (pin 3)

attachInterrupt(1, doEncoderB, CHANGE);
  Serial.begin (9600);
}

void loop(){

    int rightPWM;
  if (WR > 0) {
    //forward
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);

  }  else if (WR < 0){
    //reverse
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  }

  if (WR == 0) {
   rightPWM = 0;
   analogWrite(ENA, rightPWM);
  } else {
    rightPWM = map(abs(WR), 1, 100, 1, 255);
    analogWrite(ENA, rightPWM);
  }

 int leftPWM;

  if (WL > 0) {
     //forward
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);
  }  else if (WL < 0) {
     //reverse
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW);}

  if (WL == 0) {
    leftPWM = 0;
    analogWrite(ENB, leftPWM);
  } else {
    leftPWM = map(abs(WL), 1, 100, 1, 255);
    analogWrite(ENB, leftPWM);
  }

// to determine the speed of motors by encoders

 newposition = encoder0Pos;
 newtime = millis();
 vel = (newposition-oldposition) * 1000 /(long)(newtime-oldtime);
 oldposition = newposition;
 oldtime = newtime;

 newposition1 = encoder1Pos;
 newtime1 = millis();
 vel1 = (newposition1-oldposition1) * 1000 /(long)(newtime1-oldtime1);
 oldposition1 = newposition1;
 oldtime1 = newtime1;

Serial.print (vel);
Serial.print ("\t");
Serial.print (vel1);
Serial.print ("\t");
Serial.print (encoder0Pos*-1);
Serial.print("\t");
Serial.print (encoder1Pos*-1);
Serial.print("\t");
Serial.println ((encoder0Pos*-1) -( encoder1Pos*-1));
}

// 1 encoder counts

void doEncoderA(){
  // look for a low-to-high on channel A
  if (digitalRead(encoder0PinA) == HIGH) {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder0PinB) == LOW) {
      encoder0Pos = encoder0Pos + 1;         // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;         // CCW
    }
  }
  else   // must be a high-to-low edge on channel A
  {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder0PinB) == HIGH) {
      encoder0Pos = encoder0Pos + 1;          // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }
  }
}

// 2 encoder counts

void doEncoderB(){
  // look for a low-to-high on channel B
  if (digitalRead(encoder1PinB) == HIGH) {
   // check channel A to see which way encoder is turning
    if (digitalRead(encoder1PinA) == HIGH) {
      encoder1Pos = encoder1Pos + 1;         // CW
    }
    else {
      encoder1Pos = encoder1Pos - 1;         // CCW
    }
  }
  // Look for a high-to-low on channel B
  else {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder1PinA) == LOW) {
      encoder1Pos = encoder1Pos + 1;          // CW
    }
    else {
      encoder1Pos = encoder1Pos - 1;          // CCW
    }
  }
}
</code>```

the result:

```<code>0   0   0   0   0
0   0   0   0   0
0   0   0   0   0
0   0   0   0   0
0   0   0   0   0
0   0   0   0   0
0   0   0   0   0
0   0   0   0   2
-181    -90 3   2   1
-111    -55 5   4   1
-187    -187    9   8   2
-176    -235    12  12  1
-200    -200    16  16  1
-250    -250    21  21  1
-250    -250    26  26  1
-210    -210    31  31  1
-238    -285    36  36  1
-315    -263    41  41  1
-300    -200    47  46  2
...
-227    -272    184 182 3
-285    -285    190 187 4
-260    -217    195 193 3
-238    -285    201 199 3
...
-250    -250    1474    1473    2
-250    -250    1480    1479    0
-208    -291    1485    1485    1
-304    -260    1491    1492    1
-240    -240    1498    1498    1
-260    -260    1504    1505    0
-250    -291    1510    1511    1
-280    -240    1516    1517    1
-260    -260    1523    1523    1
...
-250    -250    2953    2948    5
-250    -291    2959    2955    6
-250    -250    2965    2961    6
-291    -250    2971    2967    5
-250    -291    2978    2973    5
-304    -250    2985    2980    8
-320    -250    2992    2986    8
...
-320    -240    3085    3075    10
-291    -291    3092    3082    12
-269    -230    3099    3089    11
-250    -291    3105    3095    11
-280    -280    3112    3102    11
-269    -230    3118    3108    12
-250    -291    3125    3115    11
...
-291    -250    3607    3587    19
-115    -269    3610    3594    17
-240    -240    3617    3601    18
-375    -291    3625    3607    19
-269    -269    3632    3614    20
-291    -250    3638    3620    20
-240    -280    3645    3627    20
-280    -240    3652    3633    18
-200    -280    3657    3640    19
-269    -230    3664    3647    19
-333    -291    3674    3653    23
-400    -280    3682    3659    23
-280    -240    3688    3666    24
-240    -280    3695    3673    24
...
-230    -269    4677    4644    32
-208    -291    4681    4651    32
-280    -240    4690    4657    35
-320    -280    4696    4664    34
-240    -240    4703    4670    34
-291    -291    4710    4677    34
-269    -230    4716    4683    34
-240    -280    4723    4690    34
-280    -240    4727    4697    32
-160    -280    4736    4703    35
-416    -291    4745    4709    38
-346    -230    4753    4716    39
...
-360    -240    6240    6190    51
-375    -291    6247    6197    51
-269    -269    6253    6203    52
-291    -250    6261    6210    53
...
-192    -269    6428    6374    56
-240    -280    6436    6380    57
-291    -250    6443    6387    57
-269    -269    6449    6394    57
...
-269    -269    7763    7687    78
-240    -280    7770    7694    78
-291    -250    7776    7700    76
-192    -269    7781    7707    76
...
-269    -230    8263    8179    84
-250    -291    8269    8186    85
-240    -240    8276    8192    88
-384    -269    8286    8199    88
-250    -291    8292    8206    88
-269    -230    8299    8212    87
-291    -291    8305    8219    88
-240    -240    8310    8225    85
...
-160    -120    8359    8276    83
-125    -166    8362    8280    82
-115    -115    8365    8283    83
-80 -120    8367    8285    82
-125    -83 8370    8288    82
-83 -125    8371    8290    82
-43 -43 8373    8291    81
-83 -83 8374    8293    82
-45 -90 8375    8294    81
-43 -43 8376    8296    81
-43 -43 8377    8296    81
-43 -43 8378    8297    81
</code>```

答:
Good job writing a test program that shows lots of data -- that's an excellent first step in figuring out what's going on.

With two physical motors, there is always going to be at least a little internal difference between them -- slightly different internal winding resistance, slightly different internal bearing friction -- which is often negligible compared to differences in the external loads applied to them.

So when driving two DC motors with exactly the same PWM, the actual physical rotation speed is always going to be slightly different.

That's why many robot setup manuals have a "Calibration for Straight Line Travel" procedure.

Sometimes people walk in circles, perhaps for similar reasons. (a) (b)

"robots that have a separate motor for each wheel is that driving in a
  perfectly straight line  can be difficult. If the motors run at just
  slightly different speeds, your robot will tend to drive in a large
  circle  when you want it to go straight. The NXT is able to overcome
  this problem by using motors with rotation  sensors built in, allowing
  you to synchronise them."
  -- Ross Crawford. "Programming Lego Robots". p. 30

After working with robots for a while, you'll figure out ways of debugging a problem.

Y cable test

If I were you I would run a Y cable test:

make a test Y cable so the A output of a single quadrature encoder is connected to both your Arduino encoder0PinA pin and encoder1pinA pin (2 and 3), and likewise for the B pins.

With that Y cable, you know that what the Arduino sees as encoder0 and what the Arduino sees as encoder1 are spinning at exactly the same physical speed, since it's the same physical encoder.

That test helps me narrow down the problem:
If a Y cable test with your test program still shows encoder0Pos and encoder1Pos are diverging,
then there's a software problem.

On the other hand,
if a Y cable test with your test program shows encoder0Pos and encoder1Pos are always identical to each other,
then the divergence you saw in the 2-motor test may be normal.
We expect from normal physical variations from one motor to another to lead to slightly different speeds when driven by exactly the same PWM.
The data you gave shows the difference in speed to be less than 1%.
That is excellent matching.

Swap testing

Often when one system acts differently from another allegedly "identical" system, people swap one small part of the system back and forth to see if the problem "follows" the thing that was swapped or if it stays behind.
Currently the "slow" one is the one plugged into encoder1.

If I were you, I might do a series of tests something like

"If ... your robot is not going straight, try swapping the wheels and see if it deviates into the other direction. That would indicate a small difference in wheel size. Adjust wheel size accordingly." -- Lejos: DifferentialPilot

Unplug everything connected to motor0 and motor1, swap them, and plug them back in. Then test: Is motor1 still the slow one, or is motor0 now the slow one?

Unplug everything connected to the motor0 driver circuit, swap it with the motor1 driver circuit, and then plug them back in. Then test: is the motor1 driver circuit still associated with the slow one, or is the motor0 driver circuit now the slow one?

etc.

Title: Quadrature Encoder Counts


问:
Mahalanobis distance between 2 line features
I am implementing the ATLAS SLAM framework for a ground robot, using EKF Slam for local maps and using line segment features. The line segment features can be abstracted to their respective lines [d,α] where d and α represent the distance and angle in the distance-angle representation of lines.

In the given framework, there is a local map matching step where lines of the local maps will be matched, and there is a need for a distance metric between 2 lines. The mahalanobis distance is suggested in the literature, however strictly a mahalanobis distance is between a single measurement and a distribution and not between 2 distributions.

How do I find the mahalanobis distance between line 1 [d1,α1] with covariance matrix S1 and line 2 [d2,α2] with covariance matrix S2?

In the EKF Algorithm from the book Probabilistic Robotics by Sebastian Thrun, there is a computation during the feature update step, where it looks like the covariances (of a new measurement and an existing measurement) are multiplied to give a resultant covariance matrix, and then the inverse is used in the Mahalanobis distance computation.

That would be similar to

```<code>Mahalanobis_Distance = [d2-d1,α2-α1] * Inverse(S1*S2) * [d2-d1,α2-α1]'
</code>```

Is that correct?

答: This is might not be an answer but check out this book Robotic Navigation and Mapping with Radar  page 126.  They discuss  the Mahalanobis distance better than what stated in Probabilistic Robotics. Also, they mention why this method also known as the Nearest Neighbor (NN) is not a good solution in some situations and they state another solutions. </p>


问:
Robotc color sensor error
I am trying to write a simple program where the robot(Lego NXT2) will follow a blue line.

```<code>#pragma config(Sensor, S1,     ,               sensorCOLORBLUE)
//!!Code automatically generated by 'ROBOTC' configuration wizard               !!//

task main()
{
    while(true)
    {
        if(SensorValue[S1] == sensorCOLORBLUE)
        {
            motor[motorB] = 0;
            motor[motorC] = -50;
        }
        else
        {
            motor[motorB] = -50;
            motor[motorC] = 0;
        }
    }
    wait1Msec(1);
}
</code>```

I am using an nxt color sensor and the problem is that only 1 motor is moving. I know that none of the motors are broken either because I tested them out.
Can somebody help me diagnose my problem?

答:
Someone who owns a Lego NXT2 system may be able to check your code and provide a good answer.

Meanwhile, load some of the color sensor programs from the following links, run them with the sensor pointed at known colors, and edit your question to report whether your color sensor is working properly.

Color Range Block v1.0 from teamhassenplug.org

G1-FindColor, G2-SumoColor, etc from     nxtprograms.com

Also see philohome.com's  “Color Sensors Showdown” page.

Title: Robotc color sensor error


问:
For robot wheel control : Brushless DC motor or Servo Motor?
Simply, when to use brushless dc motor and when to use Servo Motor ? </p>

<p>what are the differences , specially when adding an encoder to the dc motor you can have the position and it will be similar to Servo Motor ? </p>

答:
Let's try and clear things up a little bit...

"RC Servo" motors

These can be found in radio controlled cars and airplanes to e.g. turn the wheel or actuate various controls in the airplane.  They are pretty cheap brushed DC motors, with gears and some sort of crude feedback mechanism.  They typically convert an input voltage into a position within a given range.

```<code>"Exploded Servo" by oomlout - Exploded Servo. Licensed under Creative Commons Attribution-Share Alike 2.0 via Wikimedia Commons
</code>```

Brushless DC motors

Brushed motors rely on "brushes" for commutation to drive current through the rotating motor coils.  In a brushless motor in contrast the coils are fixed and potentially some sort of feedback mechanism is used to control the current through those coils to rotate the motor.  Brushless motors can also be run open loop (e.g. stepper motors).  The DC is a bit of a misnomer as the motors themselves run on AC.

Brushless DC Servo aka "Servo Motor"

These would be typically found in applications such as industrial robots or CNC machines.  They come in a very wide range of sizes and power.  They are a combination of a BLDC motor with a rotary encoder, a hall effect sensor or both.  They are meant to be used with a closed-loop controller.  Pretty much any type of motor can be used in a closed loop servo but since those are more commonly used in that sort of application the term "servo motor" stuck.

```<code>"3417 09 Servogetriebe" by SEW-EURODRIVE GmbH & Co KG. Original uploader was Sewmecm at de.wikipedia - Transferred from de.wikipedia; transferred to Commons by User:Brackenheim using CommonsHelper.(Original text : SEW-EURODRIVE GmbH & Co KG). Licensed under Creative Commons Attribution-Share Alike 3.0 via Wikimedia Commons
</code>```

For driving a robot wheel you could consider:

A DC brush motor with no rotor position feedback (cheapest).

A DC brush motor with feedback and some sort of controller.

A brushless DC motor with electronic speed control (such as the one you'd find in a quadcopter).  These are quite different than the industrial BLDCs mentioned above and I won't go into the details.  They can be had relatively cheaply due to the popularity of all these "toys" and are pretty high performance.

A more industrial BLDC/encoder combination with a closed loop controller.  Those would probably be the most expensive option.

Body:
Let's try and clear things up a little bit...

"RC Servo" motors

These can be found in radio controlled cars and airplanes to e.g. turn the wheel or actuate various controls in the airplane.  They are pretty cheap brushed DC motors, with gears and some sort of crude feedback mechanism.  They typically convert an input voltage into a position within a given range.

"Exploded Servo" by oomlout - Exploded Servo. Licensed under Creative Commons Attribution-Share Alike 2.0 via Wikimedia Commons

Brushless DC motors

Brushed motors rely on "brushes" for commutation to drive current through the rotating motor coils.  In a brushless motor in contrast the coils are fixed and potentially some sort of feedback mechanism is used to control the current through those coils to rotate the motor.  Brushless motors can also be run open loop (e.g. stepper motors).  The DC is a bit of a misnomer as the motors themselves run on AC.

Brushless DC Servo aka "Servo Motor"

These would be typically found in applications such as industrial robots or CNC machines.  They come in a very wide range of sizes and power.  They are a combination of a BLDC motor with a rotary encoder, a hall effect sensor or both.  They are meant to be used with a closed-loop controller.  Pretty much any type of motor can be used in a closed loop servo but since those are more commonly used in that sort of application the term "servo motor" stuck.

"3417 09 Servogetriebe" by SEW-EURODRIVE GmbH & Co KG. Original uploader was Sewmecm at de.wikipedia - Transferred from de.wikipedia; transferred to Commons by User:Brackenheim using CommonsHelper.(Original text : SEW-EURODRIVE GmbH & Co KG). Licensed under Creative Commons Attribution-Share Alike 3.0 via Wikimedia Commons

For driving a robot wheel you could consider:

A DC brush motor with no rotor position feedback (cheapest).

A DC brush motor with feedback and some sort of controller.

A brushless DC motor with electronic speed control (such as the one you'd find in a quadcopter).  These are quite different than the industrial BLDCs mentioned above and I won't go into the details.  They can be had relatively cheaply due to the popularity of all these "toys" and are pretty high performance.

A more industrial BLDC/encoder combination with a closed loop controller.  Those would probably be the most expensive option.


问:
Particles not behaving correctly in the implementation of particle filter
I am implementing a particle filter in Java. The problem with my particle filter implementation is that the particles suddenly go away from the robot i.e the resampling process is choosing particles which are away from robot more than those which are near.It is like particles chase the robot, but always remain behind it. I am trying to find the root cause, but to no luck. Can anyone please help me where I am going wrong?

I am adding all the imp. code snippets and also some screenshots in consecutive order to make it more clear.

Details:

I am using a range sensor which only works in one direction i.e. its fixed and tells the distance from the obstacle in front. If there is no obstacle in its line of vision, then it tells the distance to boundary wall.

Code:
Calculating Range

```<code>/*
 * This method returns the range reading from the sensor mounted on top of robot.
 * It uses x and y as the actual position of the robot/particle and then creates Vx and Vy as virtual x and y.
 * These virtual x and y loop from the current position till some obstruction is there and tell us distance till there.
 */

private int calculateRange(double x, double y, double Vx, double Vy, int counter, int loop_counter)
{
    while(robotIsWithinBoundary(Vx, Vy))
    {
        int pace = 2;
        Vx += pace* Math.sin(Math.toRadians(robot_orientation));
        Vy += pace* Math.cos(Math.toRadians(robot_orientation));
        counter++;
        Line2D line1 = new Line2D.Double(x,y,Vx,Vy);
        if(line1.intersects(obst1))
        {
            //System.out.println("Distance to obst1:"+counter);
            loop_counter++;
            break;
        }
        if(line1.intersects(obst2))
        {
            //System.out.println("Distance to obst2:"+counter);
            loop_counter++;
            break;
        }

    }
    return counter;
}

/*
 * This method tells us whether the robot/particle is within boundary or not.
 */
private boolean robotIsWithinBoundary(double x, double y)
{
    boolean verdict = true;
    if(x>680||x<0)
    {
        verdict = false;
    }
    if(y<0||y>450)
    {
        verdict = false;
    }
    return verdict;
}   /*
 * This method returns the range reading from the sensor mounted on top of robot.
 * It uses x and y as the actual position of the robot/particle and then creates Vx and Vy as virtual x and y.
 * These virtual x and y loop from the current position till some obstruction is there and tell us distance till there.
 */
private int calculateRange(double x, double y, double Vx, double Vy, int counter, int loop_counter)
{
    while(robotIsWithinBoundary(Vx, Vy))
    {
        int pace = 2;
        Vx += pace* Math.sin(Math.toRadians(robot_orientation));
        Vy += pace* Math.cos(Math.toRadians(robot_orientation));
        counter++;
        Line2D line1 = new Line2D.Double(x,y,Vx,Vy);
        if(line1.intersects(obst1))
        {
            //System.out.println("Distance to obst1:"+counter);
            loop_counter++;
            break;
        }
        if(line1.intersects(obst2))
        {
            //System.out.println("Distance to obst2:"+counter);
            loop_counter++;
            break;
        }

    }
    return counter;
}

/*
 * This method tells us whether the robot/particle is within boundary or not.
 */
private boolean robotIsWithinBoundary(double x, double y)
{
    boolean verdict = true;
    if(x>680||x<0)
    {
        verdict = false;
    }
    if(y<0||y>450)
    {
        verdict = false;
    }
    return verdict;
}
</code>```

Calculating Weights

```<code>/*
 * This method calculates the importance weights for the particles based on the robot_range which is
 * the reading of the range sensor for the robot.
 */
private double measurementProbability(int index)
{
    double probability=1;
    double particle_x_position=particleListX.get(index);
    double particle_y_position=particleListY.get(index);
    double particle_Vx=particle_x_position;
    double particle_Vy=particle_y_position;
    int range_counter=0;
    int loop_counter=0;

    int distance = calculateRange(particle_x_position, particle_x_position, particle_Vx, particle_Vy ,range_counter, loop_counter);
    probability *= calculateGaussianDistance(distance, senseNoise, robot_range);
    //System.out.println(probability);
    return probability;
}

private double calculateGaussianDistance(double mu, double sigma, double x )
{
    double gDistance=Math.exp(-(((Math.pow((mu - x),2))/(Math.pow(sigma,2)) / 2.0) / (Math.sqrt(2.0 * Math.PI * (Math.pow(sigma,2))))));
    return gDistance;
}
</code>```

Resampling

```<code>/*
 * This method provides a resampled particle back to the list. It chooses a particle randomly
 * from the list based on the weights with replacement.
 */
private int giveResampledParticle()
{
    int index = randomInteger(0, n-1);
    double sample =0.0;
    double maxWeight = maximumWeight();

    sample += randomDouble(0, maxWeight);
    while(sample > particleListProbability.get(index))
    {
        sample -= particleListProbability.get(index);
        index = (index +1) % n;
    }
    return index;
}
</code>```

答:
Your function calculateRange() uses a variable robot_orientation which I cannot see from your code snippets that it is set or changed.

I would expect something like:

```<code>double particle_orientation = particleListOrientation.get(index);
</code>```

inside your function measurementProbability() and then call calculateRange() with that additional parameter.

```<code>private int calculateRange(double x, double y, double robot_orientation)
{
    double Vx = x, double Vy = y;
    int pace = 2;
    int counter = 0;
    while(robotIsWithinBoundary(Vx, Vy))
    {
        Vx += pace* Math.sin(Math.toRadians(robot_orientation));
        Vy += pace* Math.cos(Math.toRadians(robot_orientation));
        counter++;
        Line2D line1 = new Line2D.Double(x,y,Vx,Vy);
        if(line1.intersects(obst1))
        {
            break;
        }
        if(line1.intersects(obst2))
        {
            break;
        }

    }
    return counter*pace;
}
</code>```

Body:
Your function calculateRange() uses a variable robot_orientation which I cannot see from your code snippets that it is set or changed.

I would expect something like:

double particle_orientation = particleListOrientation.get(index);

inside your function measurementProbability() and then call calculateRange() with that additional parameter.

private int calculateRange(double x, double y, double robot_orientation)
{
    double Vx = x, double Vy = y;
    int pace = 2;
    int counter = 0;
    while(robotIsWithinBoundary(Vx, Vy))
    {
        Vx += pace* Math.sin(Math.toRadians(robot_orientation));
        Vy += pace* Math.cos(Math.toRadians(robot_orientation));
        counter++;
        Line2D line1 = new Line2D.Double(x,y,Vx,Vy);
        if(line1.intersects(obst1))
        {
            break;
        }
        if(line1.intersects(obst2))
        {
            break;
        }

    }
    return counter*pace;
}


问:
Developing for 8-bit AVR-s, what are the current, open and free libraries out there?
I would be very interested to ask for a list of repos of free open code, applicable to 8-bit avr-s and having relation to robotics - object avoidance, process controllers, battery management, etc. This would be of huge help for me, preventing me from wasting weeks and months to invent the wheel.</p>

答:
I can say from my own experience three libraries are awesome to use with 8-bit AVR microcontrollers:

AVR Libc is the standard C library for AVR programming. It is very low-level in the sense that you really need to know your hardware down to the register level, but it gives you great freedom to do exactly what you want.

Even if you don't want to dive so deep, you should become somewhat familiar with it since every now and then Arduino users end up using bits of it to surpass the capabilities of the standard Arduino API. An interesting fact is that you can use AVR Libc snippets within your Arduino code since the Arduino API is based on the AVR GNU Toolchain.

AVR Liberty is currently my favorite library to develop with. It is both very straightforward and very powerful, so I feel I get the best of both worlds: I don't have to reinvent the wheel as with AVR Libc, but I also don't have to be agnostic of everything that goes on with the hardware as with the Arduino API. I strongly recommend it.

The Arduino API can be used in the Arduino boards, in breadboard versions[1][2] of the Arduino board, or even in standalone microcontrollers using a programmer such as the USBasp.

I like to use Arduino boards for quick prototyping, but I find its API to be somewhat limiting at times. However, they have many specific libraries that extend the standard API's functionality.

The Arduino board can also be used with any other AVR library as long as you understand how to upload your code (either through the bootloader or a programmer), so it is definitely a dev board worth buying to get started.