答:
6-DOF is kind of difficult.. You could do vision but that is hard to implement robustly. You say the UAVs are localizing themselves using GPS; are you using an IMU+Kalman filtering as well? For proximity detection you could try infrared/ultrasonic sensors, e.g. https://www.sparkfun.com/products/242. You could combine this with the Kalman filter to get a relative position estimate and get orientation from the Kalman filter directly.

Although I reckon if you are using GPS+IMU+Kalman then broadcasting the estimated state to each other (XBee?) would probably suffice, given that the GPS units are calibrated similarly. You could easily calculate the relative orientation by subtracting their orientation in the inertial frame.

Title: Spatial tracking between two UAVs

Body:
I have two Unmanned Aerial Vehicles (planes) which work well. They can fly to various waypoints automatically using GPS.

Now I would like them to fly together in formation. I would like them to fly side-by-side fairly close. This is too close to reliably use GPS to guarantee that they keep the correct relative positions safely, and so I am looking for another way.

Somehow the UAVs need to be able to measure their position and orientation in space relative to the other one. How can I do this? Is there some kind of sensor which can do this? It would need to have the following properties:

6 axes (position and orientation)

Range 0m - 5m, (from between plane centres, but planes won't actually ever touch wingtips)

Works in day or night and all weather conditions

Light weight (This is for 1.5m wingspan RC planes, so max extra weight of about 100g)

Probably need about 50Hz - 100Hz refresh rate, but might get away with less, using the IMU to fill in the gaps


问:
Quadcopter Throttle and PID mixing to Motor Speed
I've been writing some quad copter software and I am not sure what the best way is to map the throttle and PID inputs to ESC power.

My throttle range is 0-1 and my PID outputs are 0-1. My ESC's have a range of 1060us to 1860us.

I have mapped the motor speeds like this:

```<code>_motorPower[0] = map((_rcConstrainedCommands[3] + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 4.0, 1060, 1860);
_motorPower[1] = map((_rcConstrainedCommands[3] + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 4.0, 1060, 1860);
_motorPower[2] = map((_rcConstrainedCommands[3] - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 4.0, 1060, 1860);
_motorPower[3] = map((_rcConstrainedCommands[3] - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 4.0, 1060, 1860);
</code>```

This works but if my quad is perfectly level (i.e. the PID outputs are 0) and I apply full throttle (1.0) then map this to ESC power I will only get quarter power (1260us).

How should I be doing this so that if my throttle is on max then I get max power? If my throttle is half (0.5) then I should get half power plus the PID values etc.

Can anyone help me with this?

Thanks
Joe

答:
For each motor the output should be:

A value at which the power from the 4 motors roughly keeps the quadcopter airborne, for instance 1500ms.

Plus or minus the influence of the throttle. For instance with your throttle is in the 0:1 range you could apply 600 * (throttle - 0.5), which will put the motors output in the 1200:1800ms range.

Plus or minus the PIDs output. For instance with your PIDs in the 0:1 range you could add 200 * (PID - 0.5). The individual PID contribution would be in the -100:100 range. For a +4 configuration there are 2 PIDs to add for each motor, so the output would be in the 1000:2000ms range.

Here is my unmodified code for this. My throttle is in the -100:100 range, and my PIDs are already in the appropriate range. I'm sure you can translate to your own specifications easily.

```<code>uint16_t motor[4];
float base = MOTOR_IDLE + (THROTTLE_K * throttle);
motor[MOTOR_FRONT] = base + pitchRatePID.getControl() - yawRatePID.getControl();
motor[MOTOR_LEFT] = base + rollRatePID.getControl() + yawRatePID.getControl();
motor[MOTOR_BACK] = base - pitchRatePID.getControl() - yawRatePID.getControl();
motor[MOTOR_RIGHT] = base - rollRatePID.getControl() + yawRatePID.getControl();
</code>```

The THROTTLE_K constant would be what to modify if I experienced your problem.

As you realise the possible motors output exceeds the ESC range, which is evil. Bounding the output is not an elegant way to do it as it will trip up your PIDs computations. A better way which maintains the speed ratios between the motors is the add or remove a constant to all the motors to get all the outputs back within the bounds. Here is a sample from my own code, I hope it makes sense.

```<code>// Fix motor mix range
int16_t motorFix = 0;
uint16_t motorMin = motor[0], motorMax = motor[0];
for(int i=1 ; i<4 ; i++)
{
  if(motor[i] < motorMin) motorMin = motor[i];
  if(motor[i] > motorMax) motorMax = motor[i];
}
if(motorMin < MOTOR_MIN) motorFix = MOTOR_MIN - motorMin;
else if(motorMax > MOTOR_MAX) motorFix = MOTOR_MAX - motorMax;

// Refresh motors
for(int i=0 ; i<4 ; i++) Servo::setChannel(i, motor[i] + motorFix);
</code>```

I hope this helps, please let me know if you need further clarifications!

ContentLicense: CC BY-SA 3.0

Body:
For each motor the output should be:

A value at which the power from the 4 motors roughly keeps the quadcopter airborne, for instance 1500ms.

Plus or minus the influence of the throttle. For instance with your throttle is in the 0:1 range you could apply 600 * (throttle - 0.5), which will put the motors output in the 1200:1800ms range.

Plus or minus the PIDs output. For instance with your PIDs in the 0:1 range you could add 200 * (PID - 0.5). The individual PID contribution would be in the -100:100 range. For a +4 configuration there are 2 PIDs to add for each motor, so the output would be in the 1000:2000ms range.

Here is my unmodified code for this. My throttle is in the -100:100 range, and my PIDs are already in the appropriate range. I'm sure you can translate to your own specifications easily.

uint16_t motor[4];
float base = MOTOR_IDLE + (THROTTLE_K * throttle);
motor[MOTOR_FRONT] = base + pitchRatePID.getControl() - yawRatePID.getControl();
motor[MOTOR_LEFT] = base + rollRatePID.getControl() + yawRatePID.getControl();
motor[MOTOR_BACK] = base - pitchRatePID.getControl() - yawRatePID.getControl();
motor[MOTOR_RIGHT] = base - rollRatePID.getControl() + yawRatePID.getControl();

The THROTTLE_K constant would be what to modify if I experienced your problem.

As you realise the possible motors output exceeds the ESC range, which is evil. Bounding the output is not an elegant way to do it as it will trip up your PIDs computations. A better way which maintains the speed ratios between the motors is the add or remove a constant to all the motors to get all the outputs back within the bounds. Here is a sample from my own code, I hope it makes sense.

// Fix motor mix range
int16_t motorFix = 0;
uint16_t motorMin = motor[0], motorMax = motor[0];
for(int i=1 ; i<4 ; i++)
{
  if(motor[i] < motorMin) motorMin = motor[i];
  if(motor[i] > motorMax) motorMax = motor[i];
}
if(motorMin < MOTOR_MIN) motorFix = MOTOR_MIN - motorMin;
else if(motorMax > MOTOR_MAX) motorFix = MOTOR_MAX - motorMax;

// Refresh motors
for(int i=0 ; i<4 ; i++) Servo::setChannel(i, motor[i] + motorFix);

I hope this helps, please let me know if you need further clarifications!


问:
Role of Neuromorphic Computing and Quantum Computing in the field of Robotics and AI
I asked a similar kind of question some time ago (Neuromorphic Engineering and Robotics)

Since then, many things have come to the point of revelation. A road-map for neuromorphic computing was revealed recently; It proposes the analog way of computation, to solve advanced computer vision problems. IBM and Qualcomm are also working on the similar project though on the digital side. Memristor technology is slated to come very soon.

The question I am asking here is How is the Robotics community working to adopt the technology? This question opens the domain for other pressing questions which have been answered cryptically since the 1980s.

Are neuromorphic computers good for mission critical precise robots, like that on Mars? Can we use neuromorphic systems on Avionics systems? How is neuromorphic processing going to help us solve the problems on NLP, and knowledge processing? Aren't quantum computers very similar to neuromorphic computers in ideology? If neuromorphic robots gain traction, will digital hardware still be required?

It would be really nice if someone could explain all points, because answers in various but sparsely related research papers are very cryptic.

答:
You've asked more than one question, so I'll try to answer them in order.

The Robotics community has not yet hit the limits of current hardware, so very little work is being done on the exotic cutting edge like neuromorphic hardware. The exception to this is software neural nets, which have come in and out of fashion for decades, and the  Nv artificial neurons used successfully in BEAM robotics.

No. Neuromorphic systems are not ideal for precision. Most probes are remote-controlled, not Artificially Intelligent.

To some extent. A bird's brain could not be built in neuromorphic form, and then used to fly a fighter jet, as the control actuators and senses are completely different. If the algorithms for good flight are properly understood, they could be implemented on digital hardware more easily than trying to wire up a neuromorphic chip.

Its not. The hope was that if we could throw enough neurons at the problem , and put them in a learning circuit, then we would not have to understand NLP and similar problems, because the machine would magically solve them for us. But while that might work for a demo - look up Perceptrons, for instance - it does not get us any closer to the algorithms we need.

Not at all. Neuromorphic mimics the neural structures in living creatures. Quantum computers specialize in simultaneously parallel computing.

Most likely. Digital systems work very well for measurement and calculation. Consider how microcontrollers are often easier to understand, debug and use than operational amplifiers, for many control systems.

I am afraid that this is largely an opinion-based answer, as the questions were very general, and contained enough scope for an entire course.


问:
Visualizing kinect data on rviz
I am a beginner of ROS, Kinect and Ubuntu. What I want is to visualize Kinect's data on rviz environment then run object recognition on it.

I've tried a few tutorials but had no luck. All I got was an empty rviz world.

Since I am a beginner I would appreciate any step-by-step instructions (preferably for hydro or groovy).

I would also like to note that I've managed to get visual from Kinect so the device is working fine.

答:
You can  install the a package for kinect:

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

Then to make the package publish kinect's topics:

```<code>roslaunch openni_launch openni.launch
</code>```

To visualize the depth image run:

```<code>rosrun image_view image_view image:=/camera/depth/image
</code>```

To visualize for example the Point Cloud in rviz you can add a PointCloud2 Display ad choose the appropriate topic.

Title: Visualizing kinect data on rviz

Body:
You can  install the a package for kinect:

sudo apt-get install ros-hydro-openni-launch

Then to make the package publish kinect's topics:

roslaunch openni_launch openni.launch

To visualize the depth image run:

rosrun image_view image_view image:=/camera/depth/image

To visualize for example the Point Cloud in rviz you can add a PointCloud2 Display ad choose the appropriate topic.


问:
Quadcopter Localization Beacon
I want to use an RF beacon to localize my quadcopter for autolanding, when GPS is not precise enough, for example, when my driveway is only 10 feet wide, and the GPS is only showing 20-30 ft. accuracy (with a proverbial lake of lava on either side). The quadcopter would use the GPS to fly to the rough location until it had a strong enough signal off the beacon, when it would begin to use that signal to come to a landing in a precise location, referenced off said beacon. Can someone please explain to me the concepts and theories behind building the beacon and it's accompanying receiver (suitable for connection to an Arduino via any digital or analog method) and achieving, say, a 4" or better horizontal and vertical accuracy within a 50' sphere? Minimally, the quad should have range and altitude, i.e. "I am 10 feet away from the beacon and 2 feet above it". How much added complexity would it take to make the robot fully position aware about the beacon, i.e. "x ft. South, y ft. West and z ft. above it", where the coordinate system is determined by the beacon and not linked to any sort of geographic coordinate system? If the beacon is mounted on a, say, 10 ft pole, are there any changes to be made versus having it on the ground and presuming that all activity takes place above it's x-y plane?

Last note-
This thing would prefferably operate in the 72MHz band, please presume that where I'm operating, there are not other devices operating on the same band.

答:
As other people mentioned, an RF beacon will probably be difficult and vision is definitely a viable option. The common difficulty with vision-based solutions is that they are computationally expensive, making it difficult to be done on-board.

You can try using the PixArt IR tracking sensor from a Nintendo Wii remote, which communicates through I2C and can thus be easily connected to for example an Arduino and place a few active IR beacons on the ground which are then picked up by the sensor. Using a simple clever pose estimation algorithm will get you an accurate estimate of your position.

A regular colour camera could also be used but unless you have something like a Beagleboard or Gumstix on-board it would be difficult to process the images in real-time. (although there is nothing stopping you from doing calculations on the ground, of course).

Title: Quadcopter Localization Beacon


问:
Quadcopter forward speed
I am trying to better understand the dynamics of forward flight in multirotors.

Assuming I have a quadcopter with 4 motor/propeller combinations capable (each) of a propeller pitch speed of, say, SpeedMax= 100 mph.

In forward horizontal flight, the quadcopter will pitch down at a certain angle, let's say AlphaP, from horizontal. If AlphaP is, say, 45 degrees, and drag is neglected, wouldn't the quadcopter be capable of a max theoretical speed of sin (45)* SpeedMax ~ 70Mph?

Also, seems to me AlphaP cannot go all the way to 90 degree (quadcopter flying like a plane), as at that point the propellers would not produce any  upward thrust to maintain the copter aloft given there is no wing loading as available in a plane.  If drag was to be neglected, what factors would the optimum AlphaP be depended on, and what would that angle be, for maximum speed?

答:
As you said, as AlphaP increases, the amount of upward thrust decreases, and when AlphaP = Forget about speed. This idea that each propeller is capable of 100mph isn't that helpful. The problem is that the thrust produced by the propellers will be a function of speed. The faster they are traveling through the air, the less thrust they'll produce. Think instead about thrust. How much thrust are the propellers capable of?

90º, there's no upward thrust at all.
Your tilt angle, AplhaP will be limited by the upward thrust needed to keep the quadcopter aloft. the amount of upward thrust produced is proportional to cos(AlphaP).

If all 4 propellers together produce 20N, and your Quadcopter weighs 10N, then to stay aloft:

cos(AlphaP)*20N >= 10N

cos(AlphaP) >= 0.5

Alpha <= invcos(0.5)

Alpha <= 60º

Now the forward thrust produced will be sin(60º)*20N = 0.866*20N = 17.3N. What speed does this relate to? You can't neglect drag here. The quadcopter will accelerate until the drag equals 17.3N.

But of course, it's not that simple. What's confounding this is that the thrust produced at speed will be less than that produced when stationary. A very rough first approximation is to think of the power output of the propellers.

power = force * speed

Double the speed means half the thrust. But it's not that simple either. Generally, when you do these simple calculations, I would divide the results by 4 to get a more realistic estimate of the maximum speed.

Title: Quadcopter forward speed


问:
How much should I expect a Kalman filter to converge?
I am learning about Kalman filters, and implementing the examples from the paper Kalman Filter Applications - Cornell University.

I have implemented example 2, which models a simple water tank, filling at a constant rate. We only measure the tank level, and the Kalman filter is supposed to infer the fill rate.

According to the model, the fill rate is a constant, so I assumed that over time, the Kalman filter would converge more and more accurately (and with less and less noise) on the correct fill rate. However, the amount of noise in the fill rate never seems to reduce after the first few iterations:

This graph shows how the fill rate part of the state vector changes over the course of 1000 iterations of the simulation.

Adjusting the Measurement Variance Matrix seems to have very little effect on the fill rate noise.

Also, the Kalman gain vector and State Variance matrix seem to be constant throughout the simulation. I assumed that the State Variance would reduce as the filter became more and more confident in its state estimate.

Questions:
- Is this graph what I should expect to see?
- Should the Kalman Gain vector and State Variance matrix change over time in this situation?

答:
Your graph is actually indicative of what I would expect to see, but there might be some problems. For now, let us assume that you've implemented it perfectly.

the fill rate is a constant, so I assumed that over time, the Kalman filter would converge more and more accurately

Does your Kalman filter know that the fill rate is constant? It's actually pretty easy to estimate fill rate given lots of data, but one that can estimate it as fast as possible is very powerful.

It looks like your graph starts at zero and then rings three times and converges very fast. At this point, it just responding to injected noise.

However, the trick is to balance favoring your current estimate vs incoming data using the Kalman gain. The noisier the data, the more you'll have to favor your estimate. However, favoring it more means you won't converge that fast (because you're essentially telling the filter that the fill rate doesn't change that fast.)

Two items to test:

1.) The filter is receiving a measurement z with added noise. Take that signal and plot it's derivative on top of the estimated value. Look at how much noise is in the signal.

2.) Halfway through your simulation, increase the fill rate by 2x and see how the filter responses. Then play with the values in K (move them away from their calculated optimal values) and see what the behavior is.


问:
differential drive PID controller
I have a differential drive robot that works fine (good PD parameters) driving at say 1 m/s. Now, if it speeds up (to 1.2 m/s) it starts wobbling again. What would be a good strategy for a controller that is able to cope with the whole speed range of 0 - 4 m/s?

edit 14th of April:

The robot is a line follow robot but I do not see how this would be related to my question since a robot following a trajectory would have the same problem.

I recently talked to other developers of differential drive robots and they are facing similar issues e.g. they told me that they need to adjust PID parameters once the battery is not fully charged hence the robot drives at a different speed.

I do not know if you guys are into youtube, but if your are really interested in my robot this link would be helpful: https://www.youtube.com/watch?v=vMedNPhXlEo

PID parameters are: P 0.31, D 0.59, I 0.00

PID controller programmed using C:

```<code>  // note: the inner wheel turns backwards for narrow curves
  // cte is -128..128 depending on the robots position
  // relative to a trajectory / black line
  /** Execute the PID controller and update motor speeds */
  void PID()
  {
    int32_t steer;
    int32_t cte;
    cte = 128 - get_segment_center(0);
    // Compute PID equation
    steer = (int)(
      -P * (float)cte
      -D * (float)(cte - diff_cte) / (float)PERIOD_MS
      -I * (float)int_cte
    );
    if (steer < -5)
    {
      // turn left
      turn = -1;
      uXbot_move(MAX_SPEED + steer, MAX_SPEED);
    }
    else if (steer > 5)
    {
      // turn right
      turn = 1;
      uXbot_move(MAX_SPEED, MAX_SPEED - steer);
    }
    else
    {
      // go straight
      turn = 0;
      uXbot_move(MAX_SPEED, MAX_SPEED);
    }
    diff_cte = cte;
    int_cte += cte;
  }
</code>```

答:
After thinking about this problem a little more I had an important insight:

For the robots differential drive PID controller to be operational at different speeds, the turning angle R used for steering must be consistent over the whole speed range.

This post inspired my thinking and explains the turning angle of a differential drive robot:
Calculate position of differential drive robot

Thanks to Guy Sirton and Ian for discussing my PID controller and providing useful debugging tips.

Title: differential drive PID controller


问:
Servos power supply in Quadruped Robot
I'm facing a problem while building my quadruped robot which is figuring out the efficient power supply needed for the 12 servos. I'm using 12 MG995 tower pro servos powered by 2 lithium batteries 2x3.7v (about 8 volts) with 2200 mA . I really don't know if that enough for the servos or something else is needed to be added(i hardly fitted the 2 batteries into the robot's body)
any suggestions please?</p>

答: That should be enough power, although I would regulate the 8 volts to 6. You could get slightly better life out of the batteries on a single go if you pull some power out using a Polulu adjustable boost regulator. This will regulate the current to 6v and give you the ability to drop below 6v on the batteries and still have the 6v output to the servos (although  sometimes this can be bad for the batteries)</p>

Title: Servos power supply in Quadruped Robot


问:
What kind of motor control can I implement if I cannot use an Encoder?
Every time I see a PID control for a motor, it involves an Encoder, so the algorithm knows the real position of the motor or wheel.
But with the robot I have, I cannot use an encoder. I only have ONE optocoupler per wheel which can count how many degrees the wheel has moved. I can use it to increment a counter, but the counter ALWAYS increment (if the wheel moves forward or if the wheel moves backward).

The first moment I saw it as an inconvenient was when I studied the Arduino PID Autotune Library. In the first figure, I would not see decrements on the INPUT.

My objective is to move a little two-wheels robot small segments driven by simple trajectories separated over time by a complete stop (straight 10 cm, stop, move right 90 degrees, stop, straight until detect obstacle...)

Could you suggest me some kind of ideas?
The first idea I have is to transform the PID position control to a speed control (which is more convenient for the feedback loop I have) and keep a counter of the traveled distance to inform the speed control when to stop.

答:
I'm no guru (building a robot, learning as I go :) ), but I was facing similar problem (not with arduino, though).

For the one encoder only part: I'm counting ticks using a single reflective sensor. I assumed that motor + gearing are strong enough that they will never turn when I'm not sending commands. Then I keep track of the direction of rotation and when there is a request to change it, I force several milliseconds wait with the motor stopped to ensure that it's not turning in a wrong direction.
This adds some complications and state keeping to the code, but seems to work OK.

For the PID: I'm putting in speed difference (encoder ticks / timer interval) and using the output for PWM (actually I'm feeding it to a RC car ESC). This works really nice, it's fairly stable even when the motor has almost no torque during start up.

If some code will help you, here is my implementation for AVR:
https://github.com/bluecube/robomower2/blob/master/src_avr/drive/main.c


问:
Robot kit suggestions
I want to develop a toy project that will allow me to move object around the house. Because I am interested in the programming of the robot and not actually build it, I would like to use some sort of programmable "starter kits" (like lego mindstorm) to get started. While I do not have everything figured out yet, here is a list of specs that I expect my ideal kit should have:

The ability to lift object (Object as big as 4'' or 10 centimeters)

The ability to distinguish objects by their colors. It should have some stort of color sensors.

Obviously it should be able to move on a smooth surface.

Obstacle detection. Should have sensors for obstacle detection

Extra: Maybe remotely controllable.
Can someone please suggests the cheapest kit I should use for this?
Thanks

答:
There are a couple ways you could do this.  I'm gonna put down my recommendations in order of what I'd most recommend to least.

Mindstorms NXT flashed with Lejos for java.  You can follow simple instructions to build anything with these kits and then program them in java.  They have Bluetooth built in for simple remote control and no wiring necesary for tons of sensors. Plus, an NXT kit will cost you only $200 on ebay for near infinite possibilities.

Work with a simple material to build a very basic robot chassis.  My reccomendation would just be to buy a sheet of precut lexan and drill 8 holes for cheap motor mounts.  Boom, robot chassis.  Add an arduino with bluetooth shield, couple of motors and controllers and you're ready to go.  You would have infinite expansion opportunities, for example you could add any type of linear actuator you liked with minimal fuss.  Trust me, even if you think you're a software guy it's important to know a little about hardware.  Add a claw such as this, and you have a high powered solution for 1/2 the cost of a kit.

If you really do need a kit, I'd recommend going with the Rover 5 from
Sparkfun.  Add an arduino with bluetooth, a couple of sensors and a linear actuator and you're good to go.

All in all, there are many ways to get into robotics from a software perspective.  You may also be interested in software such as RobotC Virtual Worlds, which is essentially a robot simulator you program in C.  If you are feeling adventurous go with #2.   A kit such as the rover 5 would take you 10 minutes to build from scratch with just a drill and be so much more rewarding.  And if you need any help, you know where to ask.

Title: Robot kit suggestions


问:
Differential Drive Robot on uneven surfaces
So I am building a differential drive robot and I want it to autonomously drive in a straight line on an uneven surface. I know I need a position and velocity PID. As of now, I am deciding on which sensors to buy. Should I use optical encoders, accelerometers, or something else?

I wanted to go with accelerometers due to the error encoders would face due to slippage, but I am not sure.

Some enlightenment would help!

答:
Get a good IMU you can communicate with over i2c.  Make sure it has an accelerometer and a gyroscope.  Use these two sensors to calculate your heading, and then correct accordingly to make sure you are heading straight.

Here's an example of a nice IMU: https://www.sparkfun.com/products/10121

Sparkfun has created an arduino library for that particular sensor that will make calculating heading easy peasy.  Good luck!


答:
I found this treasure chest at the Hobbyking website today.
They feature Arduino based robot kits from 43 up to 150 USD.

A simple Arduino board is great for small robots, due to a low power consumption and thus smaller batteries and larger autonomy compared to other options.

Of course, having something like a Raspberry Pi with an operating system, access to standard frameworks and libraries (for computer vision etc.) mounted on your robot can be a huge asset, depending on what direction you would like to take with your project.

In either case, the above kits seem like a great start!

Title: Robot kit suggestions

Body:
I want to develop a toy project that will allow me to move object around the house. Because I am interested in the programming of the robot and not actually build it, I would like to use some sort of programmable "starter kits" (like lego mindstorm) to get started. While I do not have everything figured out yet, here is a list of specs that I expect my ideal kit should have:

The ability to lift object (Object as big as 4'' or 10 centimeters)

The ability to distinguish objects by their colors. It should have some stort of color sensors.

Obviously it should be able to move on a smooth surface.

Obstacle detection. Should have sensors for obstacle detection

Extra: Maybe remotely controllable.
Can someone please suggests the cheapest kit I should use for this?
Thanks


问:
What approaches should I consider to create rotating a turret
First a bit of background, I am planning to make a highly maneuverable airship controlled by four thrust vectored propellers. I don't want to rely on a rudder and forward momentum for turns but instead be able to maneuver with direct prop thrust. I want to be able to point each prop anywhere within a half sphere/dome. So two axis, 360 degrees traversal for the forward/back/up/down and 180 degrees for the left/right. The nearest thing I can think of is a ball turret similar to this ball turret but instead of a gun, have a motor and propeller. The turret can rotate infinitely through 360 degrees, but the gun rotates through 90 degrees.

My first thought was for a servo for both axis, but they are limited in range and I would like the 360 axis to be able to rotate continuously. This would allow for the turret to rotate to the desired angle using the shortest path.

My question is, What do I need to be able to rotate the turret and still know what angles the turret is currently pointing?

答:
Servos should be able to accomplish both the 90 degree and 360 degree (i.e. "continuous rotation") movements.

Servos operate on the concept of "steps", so you simply need to figure out how many servo steps represent the range of motion of each servo, then keep track of the current step (which can be used to calculate position) in your code.

Alternatively, you could directly measure the position with a separate sensor (like a wheel encoder).  This is traditionally used in cases where the servo could slip, and cause the steps and the position to become out of sync.  This will probably not be an issue in an airship, where the only load on the motor will be the turning of the turret in free space.


问:
Dead reckoning on a car-like robot with a gyro and only one encoder
Recently I began to build a car-like robot and I stumbled upon dead reckoning. I use one motor for steering and one for traction. I want to be able to get the position of the robot. From what I have read 2 encoders should be used. But I am curious if you can use only one encoder on the motor shaft to get distance  and a gyro + accelerometer to get the orientation of the robot.</p>

答:
Dead reckoning is not a measurement, it's a form of estimation.  It is by definition inaccurate, and accumulates error over time.  We use it when more precise forms of position measurement aren't available (because it's better than nothing) and only to the extent that the accumulated error stays below a desired bound.  In other words: after your error reaches a certain threshold, you're better off not using the estimate than relying on something that you know is wrong.

To keep the accumulated error as low as possible, you'll want to use as much sensor data as you can.  This is the opposite of what you're attempting to do (eliminating one of the encoders).  So the answer to your question is "yes", you can use only one encoder... but your estimates will suffer for it.


答:
The reason the filter stops converging on a solution is because of the Process Variance Matrix, Q.

This matrix tells the Kalman filter how imperfect the model itself is. If the matrix contains only zeros, you're telling the filter that the model is perfect, but if it contains non-zeros, you're saying that the model can't entirely be trusted, and that more weight should be given to the measurements. This will then limit the filter's convergence.

Title: How much should I expect a Kalman filter to converge?

Body:
I am learning about Kalman filters, and implementing the examples from the paper Kalman Filter Applications - Cornell University.

I have implemented example 2, which models a simple water tank, filling at a constant rate. We only measure the tank level, and the Kalman filter is supposed to infer the fill rate.

According to the model, the fill rate is a constant, so I assumed that over time, the Kalman filter would converge more and more accurately (and with less and less noise) on the correct fill rate. However, the amount of noise in the fill rate never seems to reduce after the first few iterations:

This graph shows how the fill rate part of the state vector changes over the course of 1000 iterations of the simulation.

Adjusting the Measurement Variance Matrix seems to have very little effect on the fill rate noise.

Also, the Kalman gain vector and State Variance matrix seem to be constant throughout the simulation. I assumed that the State Variance would reduce as the filter became more and more confident in its state estimate.

Questions:
- Is this graph what I should expect to see?
- Should the Kalman Gain vector and State Variance matrix change over time in this situation?


问:
Why do I need a Kalman filter?
I am designing an unmanned aerial vehicle, which will include several types of sensors:

3-axis accelerometer

3-axis gyroscope

3-axis magnetometer

horizon sensor

GPS

downward facing ultrasound.

A friend of mine told me that I will need to put all of this sensor data through a Kalman filter, but I don't understand why. Why can't I just put this straight into my micro controller. How does the Kalman filter help me about my sensor data?

答:
You do connect all these sensors directly to a microcontroller.
The Kalman filter is not an electronic filter like a LRC filter that goes between the sensors and the microcontroller. The Kalman filter is a mathematical filter implemented as software routine inside the microcontroller.

The sensors you have listed give the microcontroller 14 or 15 raw numbers each time they are all updated.

When I fly a little aircraft, what I really want to know is its position and orientation
and how far it is above the ground -- 7 numbers.

I need something that gives me those 7 numbers.

Ideally I want a new estimate of those 7 numbers every time through my control loop.
The once-per-second updates I get from my cheap GPS aren't nearly fast enough.
(People at What frequency does my quadcopter output-sense-calculate-output update loop need to stay stable? are telling me even 50-times-per-second isn't going to be fast enough).

Somehow I'm going to have to reduce those 14 or 15 raw numbers that I have, some of which only occasionally get updated, into (estimates of) the 7 numbers that I really want.

As Josh pointed out, there are many ad-hoc ways to convert those raw numbers into usable data.
Any routine that converts 15 numbers into 7 numbers can be described as a "filter".

You don't have to use the optimum filter.
But you will use some kind of filter -- i.e., something that converts from the 15 numbers of raw data you have into (estimates of) the 7 numbers you really want.

The Kalman Filter is, in some conditions, the "optimum" filter, the best way of converting that raw data into the 7 numbers I really want.

It may take less work on your part to use a Kalman filter that someone else has already written and debugged,
than to write some other filter from scratch, debug it, and keep adding stuff to it until it is usable -- a filter that will inevitably turn out to be sub-optimum.

Title: Why do I need a Kalman filter?


问:
Need specifications to operate this stepper motor with RPi or Arduino
Here is the disassembled stepper motor that I'm working with:

Contains the photo of the motor, and the label that's on the bottom of the motor.

I need to identify this stepper motor that was retrieved from scrap for a project. Budget constraints force us to use the scrap motor. I tried to drive this using a L298 H Bridge, but I couldn't find the right bit sequences to get this running smoothly. I also tried to search for a specifications sheet in the internet with the label, unsuccessfully.

I'm using either an RPi or Arduino board to run this.

I just need a pin diagram and the specifications of the motor, if you guys have seen this type before.

答:
The Marlin P. Jones Co. page for this item has a link to a pdf data sheet with the following diagram and stepping sequence.

The pdf file shows the following specifications:

17607-MS

  1.8 DEG. 3.84V NEMA 23 STEPMOTOR

  SHINANO KENSHI

  P/N: STP-57D211

  3.84VDC/4.7ohms/0.8A 2 Phase Uni-Polar 5 lead Step Motor with 6.5mm dia. X 18mm steel shaft with a 3/4 dia., 16 tooth, plastic cog belt gear attached by a steel roll pin. Ball bearings, 3" leads, 4 corner mounting holes, Removable anti vibration rubber/steel mtg. bracket.

  L: 2"(Body) SQ: 2-3/16"               WT: 1.5


问:
Our quadcopter goes forward instead of hovering in place. How to correct it?
I'm trying to do a quadcopter with some friends and we have a problem. It goes forward instead of hovering in place. We made a video to explain it, you can see it here.

As you can see, the quadcopter flight and go forward when I don't touch the controller. I need to correct it to go backward and it goes forward again.

We use the kk2.1.5.

The HobbyKing KK2.1.5 Multi-Rotor controller is a flight control board for multi-rotor aircraft (Tricopters, Quadcopters, Hexcopters etc). Its purpose is to stabilize the aircraft during flight. To do this it takes the signal from the 6050MPU gyro/acc (roll, pitch and yaw) then passes the signal to the Atmega644PA IC. The Atmega644PA IC unit then processes these signals according the users selected firmware and passes control signals to the installed Electronic Speed Controllers (ESCs). These signals instruct the ESCs to make fine adjustments to the motors rotational speed which in turn stabilizes your multi-rotor craft.

We made some test. As you can see in the video, we placed the battery backward to be sure there is no weight against. When we check values in the debug mode, all values are at 0 when nothing is pressed.

答:
There are quite a few possible explanations for the instability, I'll try to list 'em:

PID settings are off the mark. I'm not familiar with KK2.1.5 Multi-Rotor controller, but I'm sure there will be something to adjust the roll/pitch PIDs. Try tweaking them. Under-compensated integral error may be a possible culprit.

Some issue with ESC calibration. Try recalibrating the ESCs. In case you happen to have any spare ESCs, try replacing them.

Re-calibrating the 6050MPU might be a good idea.

Some thing wrong with the electrical connections inside PDB or the BLDC motors.

However, wrong PID tuning seems to be the most plausible reason.

Quite often, while hovering in low throttle, there will be an uncompensated roll/pitch integral error that would cause the quadrotor to translate sideways. Such translations would usually go away at a higher throttle.


问:
Denavit–Hartenberg parameters of a robot with spherical wrist
What are valid values for the Denavit-Hartenberg parameters $d$ and $a$ (sometimes called $r$) of the last 3 links of a robot with spherical wrist?

From this reference, "A spherical joint can be represented by three consecutive rotary joints with intersecting rotation axes."

So the retrictions should be:

$L_{n-2}$ ($d$ arbitrary, $a=0$)

$L_{n-1}$ ($d = 0$, $a=0$)

$L_{n}$ ($d = 0$, $a=0$)

But in this exam I have found on the internet, It says that the KUKA robot has spherical wrist, and $d$ of the last joint is different to $0$. Would $d\neq0$ in the last link still yield a spherical wrist?

答:
A spherical joint can be represented by three consecutive rotary
  joints with intersecting rotation axes.

This statement is true, your restrictions, I'm afraid are not. The position of spherical wrist can be determined based on what you said, but don't forget that there is another transformation from spherical wrist to the end-effector of the robot and that is the point with the last $d_i$. For example consider figure 4.2.A from the link you provided:

If you wanted to end kinematic chain at the red point, you could go with $d=0$, but to calculate the position of green point, how would you continue?

In this set of slides there some example DH for spherical joints. Furthermore, you can have a look at the slides of the same professor whom you have mentioned his exam (it can give you an idea of why he has answered the question that way).

Finally, you should remember that there are the following ambiguities in Denavit-Hartenberg convention i.e. two DH representation of the same robot, can be different, yet will result in the same forward kinematics. DH ambiguities:

Frame$_0$: origin and $x_0$ axis are arbitrary.

Frame$_n$: $z_n$ axis is not specified (but $x_n$ must be orthogonal to the intersect $z_{n-1}$

When $z_{i-1}$ and $z_i$ are parallel, the common normal is not uniquely defined ($O_i$ can be chosen arbitrarily along $z_i$)

When $z_{i-1}$ and $z_i$ are incident, the positive direction of $x_i$ can be chosen at will (however, we often take $x_i = z_{i-1} ! z_i$)


问:
Create set of drones to fly in patterns
How would one go about making a number of drones fly in a preset pattern or formation. for example have them rotating around a point.

something like this.
https://www.youtube.com/watch?v=ShGl5rQK3ew

答:
As a complete answer to this question would be very very long I'll attempt to answer it in generally high-level lines. If you wish to know specifics ask several more concise questions.

You need to create the drones. The drones need to be controllable and stable. Quadcopters are usually OK, octocopters are more stable/easier to control. Larger ones are more stable but have less maneuver capabilities.

You need a localization system so each drone knows it's position and orientation in a fixed coordinate system. This can be obtained in a quite large number of ways. Usually orientation is obtained using IMU and the position can be obtained via GPS (or dGPS for higher precision if possible), some sound-based system (using TDOA method or similar) or some visual systems for example. Another approach would be to fully localize only one drone and then use some system to find the relative positions of other drones with respect to that one.

After that, you need to produce control sequences to guide the drones along those paths. This can be done from the ground by some wireless control system or can be implemented directly onto the drones.

Given how wide your question is I hope this covers at least some portion of the answer you were looking for.

Title: Create set of drones to fly in patterns


问:
How to prevent twisting of cables
I am planning to create a motor turret described in this question. But to simplify the problem, I'm thinking of a wind turbine with a generator in the main head that can rotate freely through 360 degrees to face the wind. How would I prevent the power and sensor wires coming from the head and down the shaft from twisting? </p>

答:
Two approaches come to my mind.

The first one is brush connectors, these for example: http://www.mercotac.com/html/products.html

The second one would be to use something like audio connector for example http://www.trait-tech.com/uploads/details/T-PCA-7034-1__2-5-to-3-5-Adapter-Audio-Connector-Plug-Converter.jpg
If this isn't smooth enough for your application, there are conductive greases you can find appropriate. Be careful to separate plastic parts so that they don't make the connection rough.

Regards

ContentLicense: CC BY-SA 4.0

Title: How to prevent twisting of cables


答:
What you're looking for is generally referred to as a slip ring. Here's a cheaper slip ring sold by Adafruit along with a video demonstrating how it works: http://www.adafruit.com/products/736. They sell hobbyist parts. I've used slip rings before to manufacture my own rotating LiDAR using a Hokuyo.

Here's another company that sells what seem to be industrial grade slip rings: http://www.moflon.com/. I found them by doing a quick Google search for "slip ring connector", which you can use to find many many more options.

As a final note, here's the video of the rotating LiDAR I built which uses a slip ring...you will see the slip ring in the first 30 seconds of the video: https://www.youtube.com/watch?v=7PLcqhXVndI&list=UU0A5iVxngtVcHPjANH75z1A.

Title: How to prevent twisting of cables

Body: I am planning to create a motor turret described in this question. But to simplify the problem, I'm thinking of a wind turbine with a generator in the main head that can rotate freely through 360 degrees to face the wind. How would I prevent the power and sensor wires coming from the head and down the shaft from twisting? </p>


问:
Which type of actuator will be suitable for a very strong robot arm?
I wish to build a robotic arm that can lift a useful amount of weight (such as 3-6 kg on an arm that can extend to approx 1.25 meters). What actuators are available to accomplish this? The main factors and design points are:

Not Expensive

5 to 6 DOF

to be mounted on a yet to be designed mobile platform

battery powered

stronger than hobby servos (at least for the 'shoulder' and 'elbow' joints)

not slow to actuate

答:
Which actuators are suitable for your application depends very much on what kind of robot arm you want to build. Once you have decided on what kind of arm you want you can decide on a suitable actuator for each axis.

The Arm

Assuming from your description that a gantry robot wouldn't be viable, then depending on your specific application, you may want to consider a SCARA arm over an Articulated arm, which is what most people think of when they think Robot arm.

The big advantage of a SCARA arm is that most of it's lifting strength is in it's bearings. The main shoulder, elbow and wrist (yaw) joints are in a flat plane, which means that the motors only need to be strong enough to produce the lateral forces required, they don't need to support the weight of the remaining axes.

The Z axis, pitch and roll (and grip obviously) all have to work against gravity, but the Z axis is easy to gear highly enough to be able to support plenty of weight, and the pitch, roll and grip axes only have to support the payload weight, not the weight of other axes.

Compare this to an articulated arm, where many of the axes have to support the weight of all axes further down the kinematic chain.

The actuators

Gantry robots

Typically a gantry robot will use linear actuators for the main X, Y & Z axes. These could be low performance, low accuracy, high force actuators such as a lead screw with a servo or stepper drive (force and performance can be traded but accuracy will always be limited by backlash), all the way up to high performance, high accuracy direct drive linear motors with precision encoders.

The remaining 3DOF manipulator will usually require precision rotational motion for pitch, roll and yaw, so usually an electric motor (either stepper or servo), will be most suitable. Even a small motor with a reasonably high gearing can resist gravity against quite high loads.

An aside on servo motors vs. stepper motors

The difference between servo(1) and stepper is a trade-off between complexity and certainty in control.

A servo motor requires an encoder for position feedback, whereas a stepper doesn't. This means that a stepper is electrically much simpler, and from a control point of view simpler if you want low performance.

If you want to get the most out of your motor though (pushing it close to it's limit), then steppers get much more difficult to control predictably. With position feedback on a servo you can tune performance much more aggressively and since you know if it fails to reach it's target position or velocity then your servo loop will get to find out about it and correct it.

With a stepper you have to tune the system so that you can guarantee that it can always make the step, irrespective of the desired speed of move or weight of the payload. Note that some people will suggest adding an encoder to detect missed steps on a stepper motor, but if you are going to do that then you might as well have used a servo motor in the first place!

SCARA arm

With a SCARA arm, the Z axis is probably the only linear axis, while the remaining axes can all be done with rotational motor, so again stepper or servo motor. Sizing these motors is relatively easy because the weight carried is less important to many of them. The motor required to overcome the inertia of a load is rather less than sizing it to overcome gravity.

Articulated arm

With an articulated arm the calculations are more tricky, because most axes will need actuators sized depending on both moving the load and lifting it, but again an electric motor is the easiest to control and use.

The gripper

Finally there is the gripper. This is where I have seen the most variety in actuators. Depending on your applications you could easily use any number of different actuators.

I've used systems with traditional motor driven grippers, linear actuated grippers, piezo flexture grips, pneumatically actuated grippers, vacuum pick-ups and simple slots or hooks amongst others, many of which were specific to the application. What your typical payload is could significantly change the actuator which is best for you. (2)

Doing your calcs

As Rocketmagnet suggests ultimately you are going to have to break out your calculator.

You will need to take into account kinematics of your system, the maximum load on each motor (taking into account the worst case with arm fully extended if you are using an articulate arm design), the speed (a smaller motor with higher gearing might give the force you need without the speed, but a beefier motor might give you a higher torque with lower gearing and a higher speed etc.) and the positional accuracy you need.

In general, the more money you throw at the problem, the better performance (speed, accuracy, power consumption) you will get. But analysing specifications and making smart purchasing decisions can help optimise the price/performance of your robot.

(1) Note that my experience is with industrial servos, typically a brushed or brushless DC motors with a rotary encoder, so this may or may not apply with hobby RC servos.

(2) I would suggest posting another question on this.


问:
Accelerometer, gyro, and magnetometer sensor fusion in 2d
I have not yet built this so this is basically a theoretical question. I am still wrestling with some C code to manage i2c communication etc.

When I originally said "I have not built this", I meant that the robot is in what could be called a "design phase". For the sake of my question lets assume for a moment that the whole robot consists of just one IMU sensor. It moves magically (no motors that create a lot of noise in the sensor measurements). With theoretical I mean I am interested in the mathematics and algorithms involved in solving this problem. What I call IMU sensor provides raw accelerometer, gyro, and magnetometer measurements.

Lets say our tiny robot travels on a snooker table (3569 mm x 1778 mm). I believe this is sufficiently small to call it 2d. **Now, sensor fusion should be much easier (faster, consume less resources) than in 3d, right? **

I would like to measure the velocity and if possible the position.
With velocity I mean at a given point of time I need to know the current velocity of the robot moving over the snooker table. Velocity is in the range of 0 - 5 m/s.
With position I mean at a given point of time I need to know the current position of the robot on the snooker table (x, y, heading).

I hope this will be possible since the robot should be able to identify some landmarks and use this information to reduce position errors.

When I originally said "I hope this will be possible" I meant to express that I already am aware of the fact that real sensor data is noisy and errors accumulate quickly. Using landmarks I will / or will not be able to manage to reduce the error in the position estimates. But this is NOT part of my question.

I am about to improve my linear algebra knowledge. So I am confident to manage some matrix multiplications, inversions and such.

My question is for some ideas, references on measuring velocity and position in 2d using an IMU sensor like this one.

P.S. A little side question: I just figured that this question is probably too theoretical for robotics.SE. If you know any forum that is more focused on mathematical / algorithmic side of robotics please let me know.

答:
The gyrometer gives you angular velocity about each axis. You simply integrate these values to get the roll, pitch and yaw of the robot. Since this is 2D, all you care about is yaw, and you'll integrate one value. Of course, there are many different ways of integrating the value you read from the gyrometer. The easiest way is to sample the gyro, timestamp the sample, and assume this value holds until the next sample. This is similar to a Riemann's sum approximation of the integral, and will be more accurate the higher your sampling rate. You can use more advanced forms of interpolation between sets of samples like bilinear interpolation, but it's up to you to decide how far you'd like to go.

The accelerometer gives you linear acceleration along each axis. Since this is 2D, we only care about the acceleration along x & y (I assume x is pointing in the direction of positive velocity of the vehicle, y is pointing to the left, and z is the normal of the table). Again, we simply integrate these values to get velocity, and if you so choose integrate a second time to obtain position. Since you're integrating twice, you'll accumulate a significant amount of error. Error in the integration accumulates as a random walk (see: http://www.mit.edu/~kardar/teaching/projects/chemotaxis(AndreaSchmidt)/random.htm). This means error grows with time on the order of sqrt(t). Both the first and the second integration will have this random walk, which will compound your error further.

The magnetometer is there to provide a constant reference (magnetic north) which compensates for the drift of the gyroscope.

Essentially, you use the magnetometer and gyroscope to calculate your heading in the local frame (a static frame that has (0,0) located at the position where you started the robot). The accelerometer gives you changes in position in the vehicle frame (dynamic frame whose (0,0) origin is anchored somewhere on the robot). You can use the updated heading and changes in position in the vehicle frame to compute your location in the local frame.

Here's an article I think you will find very useful: https://forum.sparkfun.com/viewtopic.php?f=14&t=24226

Edit: I should mention that to do this, all you need are encoders on the wheels to compute position and velocity and a one axis gyro to measure heading. Then you simply use trigonometry with the change in distance computed from the encoders and the angle from the gyro to get the change in x & y. The best gyrometers can be used for hours upon hours without a significant amount of drift. Adding the magnetometer is a big help as well. Ideally you'd use the IMU as an "Attitude Heading & Reference System" and encoders to compute distance travelled and as another source of velocity. If you wanted the most accurate velocity, you'd fuse the data from the encoders with the integral of acceleration by adding the data together after weighting each data point by 1/variance of the data.


答:
Using an IMU you can only measure: acceleration, rate of rotation, and direction of magnetic field. You cannot measure velocity, you can only integrate the acceleration to infer velocity. As you can imagine, this leads to velocity drift, which in turn leads to a lot of unbounded position drift.

There are three parts to your problem:

Infer the robot's bearing from gyro and compass. If the robot is quite slow, you may be able to ignore the gyro, and just use the compass.

Infer the robots velocity and position from the accelerometer

Adjust your position estimate from 'landmarks'

The mathematics for 2. is very simple. if dT is the time between samples, then:

```<code>accelerationInWorldCoordinates = accelerationInRobotCoodinates.rotate(-heading)
velocity += accelerationInWorldCoordinates * dT
position += velocity * dT
</code>```

The mathematics for 1. is a little more involved, because we're fusing together the gyro and compass. Here's something to try:

```<code>k = 0.95
headingDegrees += gyroReadingDegreesPerSecond * dT
headingDegrees  = (headingDegrees*k) + (compassReadingDegrees*(1.0-k))
</code>```

What's happening here is that we're mainly using the gyro to update the heading, but mixing in a little of the compass reading to help keep it stable in the long term.

The mathematics for 3. (and possibly 1) is harder. What you are looking for is a Kalman Filter. This filter can take information from several sources and produce an optimal estimate of the state of the robot, along with a measure of its confidence in that estimate. It's not unlike our code for 1. In fact, you'll see the k and (1-k) in the Kalman filter too.

The problem with Kalman filters is that the only documentation you'll find on the subject tends to be heavy on the maths, and very low on practical advice for people actually implementing it.

The way I learned was by very slowly and carefully picking my way through Kalman Filter Tank Filling from Cornell University. I recommend implementing the examples given and trying to make them work. What's annoying about this paper (and most others), is that they often don't tell you the size and shape of the various matrices used in the algorithm, so you'll have to work these out by hand. They also don't tell you much about the meaning of the covariance matrix, or how to calculate it.

I have this dream that one day I'll get round to writing a genuinely helpful article on Kalman filters on my blog, but don't hold your breath.

Title: Accelerometer, gyro, and magnetometer sensor fusion in 2d

Body:
I have not yet built this so this is basically a theoretical question. I am still wrestling with some C code to manage i2c communication etc.

When I originally said "I have not built this", I meant that the robot is in what could be called a "design phase". For the sake of my question lets assume for a moment that the whole robot consists of just one IMU sensor. It moves magically (no motors that create a lot of noise in the sensor measurements). With theoretical I mean I am interested in the mathematics and algorithms involved in solving this problem. What I call IMU sensor provides raw accelerometer, gyro, and magnetometer measurements.

Lets say our tiny robot travels on a snooker table (3569 mm x 1778 mm). I believe this is sufficiently small to call it 2d. **Now, sensor fusion should be much easier (faster, consume less resources) than in 3d, right? **

I would like to measure the velocity and if possible the position.
With velocity I mean at a given point of time I need to know the current velocity of the robot moving over the snooker table. Velocity is in the range of 0 - 5 m/s.
With position I mean at a given point of time I need to know the current position of the robot on the snooker table (x, y, heading).

I hope this will be possible since the robot should be able to identify some landmarks and use this information to reduce position errors.

When I originally said "I hope this will be possible" I meant to express that I already am aware of the fact that real sensor data is noisy and errors accumulate quickly. Using landmarks I will / or will not be able to manage to reduce the error in the position estimates. But this is NOT part of my question.

I am about to improve my linear algebra knowledge. So I am confident to manage some matrix multiplications, inversions and such.

My question is for some ideas, references on measuring velocity and position in 2d using an IMU sensor like this one.

P.S. A little side question: I just figured that this question is probably too theoretical for robotics.SE. If you know any forum that is more focused on mathematical / algorithmic side of robotics please let me know.

Body:
Using an IMU you can only measure: acceleration, rate of rotation, and direction of magnetic field. You cannot measure velocity, you can only integrate the acceleration to infer velocity. As you can imagine, this leads to velocity drift, which in turn leads to a lot of unbounded position drift.

There are three parts to your problem:

Infer the robot's bearing from gyro and compass. If the robot is quite slow, you may be able to ignore the gyro, and just use the compass.

Infer the robots velocity and position from the accelerometer

Adjust your position estimate from 'landmarks'

The mathematics for 2. is very simple. if dT is the time between samples, then:

accelerationInWorldCoordinates = accelerationInRobotCoodinates.rotate(-heading)
velocity += accelerationInWorldCoordinates * dT
position += velocity * dT

The mathematics for 1. is a little more involved, because we're fusing together the gyro and compass. Here's something to try:

k = 0.95
headingDegrees += gyroReadingDegreesPerSecond * dT
headingDegrees  = (headingDegrees*k) + (compassReadingDegrees*(1.0-k))

What's happening here is that we're mainly using the gyro to update the heading, but mixing in a little of the compass reading to help keep it stable in the long term.

The mathematics for 3. (and possibly 1) is harder. What you are looking for is a Kalman Filter. This filter can take information from several sources and produce an optimal estimate of the state of the robot, along with a measure of its confidence in that estimate. It's not unlike our code for 1. In fact, you'll see the k and (1-k) in the Kalman filter too.

The problem with Kalman filters is that the only documentation you'll find on the subject tends to be heavy on the maths, and very low on practical advice for people actually implementing it.

The way I learned was by very slowly and carefully picking my way through Kalman Filter Tank Filling from Cornell University. I recommend implementing the examples given and trying to make them work. What's annoying about this paper (and most others), is that they often don't tell you the size and shape of the various matrices used in the algorithm, so you'll have to work these out by hand. They also don't tell you much about the meaning of the covariance matrix, or how to calculate it.

I have this dream that one day I'll get round to writing a genuinely helpful article on Kalman filters on my blog, but don't hold your breath.


问:
Multiple position estimates fusion
I have a system in which I have two separate subsystems for estimating robot positions.
First subsystem is composed of 3 cameras which are used for detecting markers the robot is carrying and which outputs 3 estimates of the robot's position and orientation.
The second subsystem is a system which is located on the robot and is measuring speed on the two points of the robot. By numerically integrating those two I can get an estimate on the robot's position and orientation (because I am tracking two points at once).

The first system is less accurate but the second system drifts. First system gives output about once a second while the second one gives output much more frequently (100-200 times per second).

I assume there must be a better approach than to just reset the position with the first system's estimate (as it is not 100% accurate), but to also use the accumulated position from the second sensor system and fuse that with the new data from the first system. Also, there is a question how to fuse 3 estimates of the first system? There must be a better way than pure average as it might happen that the two estimates are exactly the same and the third one is completely different (meaning that it is probably more wrong)?

Do you have any fusion algorithms to recommend to use in such a system? I know about Kalman filter, but I am having trouble figuring out how to use it as the two systems output data at different frequencies.

I hope the question is clear enough, what is the best approach to fuse the estimates into a more correct and accurate estimate?

Thanks

答:
What you are describing is essentially a textbook case for using a Kalman filter. First you need a prediction step. Let's assume you are predicting the pose of the robot $(x,y,\theta)$, given the previous pose estimate and your high-frequency velocity measurements $(v,\omega)$, where $v$ is the linear velocity and $\omega$ is the angular velocity.

$P$ is the 3x3 covariance matrix that represents the uncertainty of the robot pose. $Q$ is the covariance of your inputs (i.e., how noisy are those velocity measurements?) $F$ is the Jacobian of the motion model with respect to the state and $G$ is the Jacobian with respect to the inputs, i.e.,

Now you have your less frequent correction updates, which actually measure the full state, making this quite simple, i.e.,

where $z_k$ is your measurement (from the camera) and $R$ is the covariance matrix associated with that measurement (probably a diagonal matrix). This measurement is compared with the predicted measurement (which in your case is just the latest pose estimate). In this simple case, the Kalman gain is the proportion of the current pose covariance compared to the sum of the pose covariance and the measurement covariance.

To answer your question about the different rates, you can just run your motion update repeatedly until your prediction update arrives. For example, it might happen that the motion update occurs 100 times before your perform a correction.

You also asked about how to handle three cameras. The easiest way is to just process them sequentially; just apply three corrections in a row. Another way is to stack them and perform a single update. You would need to adjust the correction update step to do it this way.

Title: Multiple position estimates fusion


问:
How to calibrate differential drive?
I'm building a robot with differential drive. I've reached the point when I can drive it around on remote control and I'm trying to get the localization working. Now I would like to exactly measure parameters of the robot.

Model of the robot I'm using has two wheels spaced $b$ meters, each wheel has a distance per encoder tick $s_L$ and $s_R$ and variance standard deviation of the driven distance $\sigma_L$ and $\sigma_R$.
When moving, distances are random variables from the following distributions: $d_L \sim t_{L}s_{L}N(1, \sigma_L^2)$ and $d_R \sim t_{R}s_{R}N(1, \sigma_R^2)$. Later this model might expand a little bit.

What is a good way to measure the parameters?

I found a way to measure $b$, $d_L$ and $d_R$ (added that as an answer), but I have no idea how to measure the standard deviations.

The model will be used as a prediction input in MCL, so I don't need covariance matrix for localization.

答:
To estimate the first three parameters (distance between wheels and distance per encoder tick of both left and right wheels) I made a calibration pattern on the ground as shown in the image:

(this shape was chosen because it's easy to make using tape measure and a string)

Then I will manually drive the robot along the path in a ABCDBCA pattern as precisely as possible while recording encoder ticks. Then the calibration code will attempt to find the model parameters that minimize distance between output of the model (with variances set to 0) and the pattern.

My intuition is that since the path is quite long, maximal error between real robot and the pattern is bounded by a few centimeters and there are turns both left and right, the parameters found by this should be fairly close to the real values.

The only problem is that this won't give me the variances...

Title: How to calibrate differential drive?


问:
Connect sensors to beaglebone/arduino in a complex robot
I'm building the biggest robot I've ever done. The hardware I have so far is as follows:

HCR-Platform from DFRobot as a base

x2 12V 146Rpm DC motor with two phase hall encoder

x7 Sharp 2Y0A21 IR sensors

x6 URM37 ultrasonic sensor

x4 IR ground sensor

Microsoft Kinect

Right now I'm only using a RoMeo board (arduino 328 compatible) to drive the motors and process the PIDs for the wheels + steering and also access to all the sensors (except for the kinect). I have a BeagleBone Black running linux that is intended to be the main brain connected to the RoMeo using the RS232 port and do the processing of the Kinect + wifi access.

I started thinking about connecting the sensors to the Beagle board directly so I don't need to waste time sending commands to the arduino board to read the sensors and that yielded the first issue, beagle board works on 3.3V instead of 5V used on the sensors.

After this I thought to create a board with voltage divisors to connect the sensors there and then using a ribbon cable to connect this new board to the beaglebone. I couldn't find any 2x23 IDC male connector to create the "interface cable" between the two boards so the beaglebone option is out as I don't want to have tons of jumper cables all over the place.

This morning I thought about all this again and I researched about GPIO boards over USB and found three from Numato, only one works on TTL 5V and has 8 pins so I would need a few of them to use all the sensors so unless I design my own board this option is out too.

At this point I'm quite confused in terms of what's the best hardware I could use to drive this beast. Right now I think I should use a Intel NUC with linux for the Kinect, wifi and usb link to a custom made sensor board. This custom made board will work on TTL 5V, provide a power bus for the sensors and will interface all the "low level" sensors using USB as link. I also thought about an FPGA for the custom board but I'm not sure if this would help me or if it's worth the effort of learning how to use it.

What are your thoughts on this? Any idea on how this issues are solved on "complex" robots?

答: My reccomendation would be to do as much as possible arduino side, as you have much easier access to PWM and digital inputs.  Connect all of your sensors and motor drivers to the arduino.  Communicate over serial with your main robot controller (Beaglebone, Raspi, etc).  Send motor signals over serial and receive sensor values the same way.  Easy to program, neat to wire.</p>


答:
You're right to see this as a problem of complexity.  Whatever setup you decide to use, keep in mind that you will inevitably have to troubleshoot it.  It's already going to be complex -- but make it work in your favor.

In other words, let's say that you choose to make a subassembly for your sensors (like arduino + sensors, or GPIO board + sensors).  Make sure that this subassembly is completely self-contained: it can read all the sensor values, and without being connected to the beaglebone it can tell you whether all sensors are being reported properly.  What you don't want to do is make it so that the entire system has to be connected in order to check the low-level sensors.

As you compare options, figure out which options will support a modular system design and which options won't.

Title: Connect sensors to beaglebone/arduino in a complex robot

Body:
I'm building the biggest robot I've ever done. The hardware I have so far is as follows:

HCR-Platform from DFRobot as a base

x2 12V 146Rpm DC motor with two phase hall encoder

x7 Sharp 2Y0A21 IR sensors

x6 URM37 ultrasonic sensor

x4 IR ground sensor

Microsoft Kinect

Right now I'm only using a RoMeo board (arduino 328 compatible) to drive the motors and process the PIDs for the wheels + steering and also access to all the sensors (except for the kinect). I have a BeagleBone Black running linux that is intended to be the main brain connected to the RoMeo using the RS232 port and do the processing of the Kinect + wifi access.

I started thinking about connecting the sensors to the Beagle board directly so I don't need to waste time sending commands to the arduino board to read the sensors and that yielded the first issue, beagle board works on 3.3V instead of 5V used on the sensors.

After this I thought to create a board with voltage divisors to connect the sensors there and then using a ribbon cable to connect this new board to the beaglebone. I couldn't find any 2x23 IDC male connector to create the "interface cable" between the two boards so the beaglebone option is out as I don't want to have tons of jumper cables all over the place.

This morning I thought about all this again and I researched about GPIO boards over USB and found three from Numato, only one works on TTL 5V and has 8 pins so I would need a few of them to use all the sensors so unless I design my own board this option is out too.

At this point I'm quite confused in terms of what's the best hardware I could use to drive this beast. Right now I think I should use a Intel NUC with linux for the Kinect, wifi and usb link to a custom made sensor board. This custom made board will work on TTL 5V, provide a power bus for the sensors and will interface all the "low level" sensors using USB as link. I also thought about an FPGA for the custom board but I'm not sure if this would help me or if it's worth the effort of learning how to use it.

What are your thoughts on this? Any idea on how this issues are solved on "complex" robots?


问:
Programming Robots with JavaScript
As somebody who is spending the majority of his time programming in JavaScript, what's the best route to get into small-robotics without needing to deviate too much from my current language focus?

Are there any project kits or tools that make use of the JavaScript language that might make the field more approachable for developers like myself? I would even be interested in virtual environments where all code is executed in a simulation.

答:
Hmm, interesting question! I personally would suggest that you take a look at the Arduino platform. It's programmed in C, so the syntax is going to be... familiar, to a JavaScript writer. In my humble opinion, knowing how the syntax (while loops, if/than blocks, etc) is laid out is 70% of the battle, once you're familiar with that, you can plug and play. You can pick up a starter kit for around $30 online from the good folks at SparkFun.com or adafruit.com, you can program it on a PC, Mac or Linux machine, and it comes pre-loaded with enough examples to ease you into the language quickly. I'm not saying that you're just gonna drop some JavaScript code into it and be on your way, but I was a JavaScript programmer when I started out with Arduino, and I found it quite easy to pick up and run with. Good luck, as a part time teacher in robotics it's always awesome to see people getting into the field!

P.S. - you should also be familiar/comfortable with building electronic circuits (lights, motors and the like), but any good Arduino introduction kit worth its salt will get you started if you don't have any experience.

Title: Programming Robots with JavaScript


答:
Someone please check my math here, but the wheel encoder value $\theta$ is not a continuous random variable -- it can't have a variance.

You might get a more reasonable continuous random variable in the form of the estimated total linear distance.  In that case, the main source of variance in your encoder measurement is going to be the difference between your $r \theta$ calculation of distance travelled and the actual distance travelled -- if your radius measurement wasn't quite right, or the rubber on the tires compresses, or the wheels slip, etc.

Title: How to calibrate differential drive?

Body:
I'm building a robot with differential drive. I've reached the point when I can drive it around on remote control and I'm trying to get the localization working. Now I would like to exactly measure parameters of the robot.

Model of the robot I'm using has two wheels spaced $b$ meters, each wheel has a distance per encoder tick $s_L$ and $s_R$ and variance standard deviation of the driven distance $\sigma_L$ and $\sigma_R$.
When moving, distances are random variables from the following distributions: $d_L \sim t_{L}s_{L}N(1, \sigma_L^2)$ and $d_R \sim t_{R}s_{R}N(1, \sigma_R^2)$. Later this model might expand a little bit.

What is a good way to measure the parameters?

I found a way to measure $b$, $d_L$ and $d_R$ (added that as an answer), but I have no idea how to measure the standard deviations.

The model will be used as a prediction input in MCL, so I don't need covariance matrix for localization.


问:
Using vision for Monte-Carlo localization
From each step of my vision code I am able to get around 400 coordinates of where the robot thinks the walls are

I want to integrate this into Monte-Carlo observation step.

I'm storing the map of the maze as a set of Line segments. What would be a nice way to implement the sensor update, i.e. given the position (x,y) of the robot what is the probability that it is found there given the above described coordinates of the walls.

The main idea I currently have:

Transform points in polar coordinates. Then for each point (from vision output) compute a ray with this angle and find the first intersection with the maze. Now we have the predicted distance and real distance and we can compute the probability that this measurement is right.

The main drawback is that this is slow. For each point from vision output I have to iterate over all line segments to find the one with the closest intersection. The line segments number is around 50. So it gets to O(400*50*Particle number).

答: I assume you're doing this inside a loop. Make sure that you look at your computations and PRECOMPUTE everything you can before the loop starts. When you're multiplying matrices and doing other math operations 400*50*particle# times, it adds up very quickly. I remember I was doing a calculation for a transform between frames, which boiled down to multiplying 3 matrices together, and doing this inside the loop instead of precomputing made the difference of 3 seconds of computation time per iteration of my algorithm.</p>


问:
Can you identify the construction material/system used in the pic?
What's the name of the "big meccano" used in the photo below to construct all the cabinets and racks?

It appears to be an aluminium cut-to-length system of 4-way rails. I've seen it used many times and assume it has a well-known brand-name to those that know.

Photo taken from theverge.com and was a feature about how Audi are building a new car.

答:
It looks like 80/20 to me: http://www.8020.net/

I've used it many times...pretty useful stuff.


问:
What to use as electrovalve?
I want do make following installation (blowing bottle tops as music instrument):

```<code>V - valve
B - bottle

 ___________________
|mini air compressor|
 -------------------
         |
   ______|_______
  |  |  |  |  |  |
  V  V  V  V  V  V
  |  |  |  |  |  |
  B  B  B  B  B  B
</code>```

And I want to use and rc servo as electro valve (throttle) to control air flow for each bottle. Is there any other way to do that?

答:
Generally you won't need a servo.  The more common way to throttle a flow is to use pneumatic solenoid valves (which are either open or closed) and control them with PWM.  </p>

<p>See (for example) this link.  This is also how the boost controller in many cars works. </p>

Title: What to use as electrovalve?

Body:
Generally you won't need a servo.  The more common way to throttle a flow is to use pneumatic solenoid valves (which are either open or closed) and control them with PWM.

See (for example) this link.  This is also how the boost controller in many cars works.


问:
Outputting a precise voltage in millivolts on Arduino Mega
So I need to output a varying voltage off an Arduino Mega in a range of 17 to 32 millivolts, which I've attempted to do by sending a PWM signal off the board into a low-pass filter which steps down the voltage.

This works, but the problem is that Arduino's analogWrite function accepts a value of 0 - 255 to represent the duty cycle of the PWM which isn't precise enough. A value of 1 yields around 20 millivolts and a value of 2 yields around 40 millivolts. Is there some way to have a duty cycle that is more precise than the 0 - 255 range like 0 - 1023 (I think even this isn't really precise enough)? Or is there a better way to get precise voltage output?

The mega is running on and outputting a max voltage of 5 volts, and the low pass filter contains an 11 kiloohm resistor and a 1 microfarad capacitor.

答:
analogWrite() does not support a higher range of duty cycle.

However the timers on the AVR chip do support higher precision/resolution, so you can bang the registers directly, especially if you don't need to use the rest of the Arduino libraries. I recommend using avr-gcc with avr-libc and avrdude for programming, and let go of Arduino IDE/libraries entirely.

You have up to 16 bit timers, where the duty cycle can be up to 0-65535. However, at 16 MHz, a 16 bit timer only ticks over 244 times a second, so the filter has to be very low-frequency and thus the lag in the output will be long (although the default PWM frequency for 8-bit timers is only 490 Hz or so, so only twice as good.)

The second thing you can do is output to a resistor divider. If you output to a 9.9k:100ohm divider, the voltage at the 1ohm resistor will vary from 0 to 50 mV even though the voltage across the top will vary from 0 to 5V. Note that the source impedance is kind-of high here, so you might want to re-buffer with an opamp if the load is low-impedance.


答:
What kind of CPU or GPU power do you have available? A GPU might be able to do a large chunk of those calculations in parallel, and all of them in a single calculate call, which would complete in a few milliseconds.

Also, with proper spatial partitioning, SIMD implementation, and using coherency where you know that close rays are likely to have the same result, you should be able to optimize this even on a regular CPU.

Title: Using vision for Monte-Carlo localization

Body:
From each step of my vision code I am able to get around 400 coordinates of where the robot thinks the walls are

I want to integrate this into Monte-Carlo observation step.

I'm storing the map of the maze as a set of Line segments. What would be a nice way to implement the sensor update, i.e. given the position (x,y) of the robot what is the probability that it is found there given the above described coordinates of the walls.

The main idea I currently have:

Transform points in polar coordinates. Then for each point (from vision output) compute a ray with this angle and find the first intersection with the maze. Now we have the predicted distance and real distance and we can compute the probability that this measurement is right.

The main drawback is that this is slow. For each point from vision output I have to iterate over all line segments to find the one with the closest intersection. The line segments number is around 50. So it gets to O(400*50*Particle number).


问:
Static equilibrium for 7 dof manipulator
I have a 7 dof manipulator (Kuka LBR4+) and would like to calculate the joint torques needed to keep the arm in a static equilibrium. In most books the transposed jacobian is used to map the forces applying on the end effector to the joint torques.

$\tau = J^T\cdot F$

That however doesn't take the mass of the links into account. Is there a way to calculate the needed torques for a given configuration so that, assuming an ideal case, by setting these torques the arm will be in a static equilibrium?

cheers

EDIT:

For everybody interested, i found a solution to this problem in Introduction to Robotics - Third Edition by John J. Craig on Page 175-176. It is done with the aid of the iterative Newton-Euler dynamics algorithm. The actual trick is, to set all velocities and accelerations to zero except for the base acceleration. The base acceleration will be $^0 \dot v_0 = G$, where G has the magnitude of the gravity vector but points in opposite direction. This is equivalent to saying that the base of the robot is accelerating upwards with 1 g and this upward acceleration causes exactly the same effect on the link as gravity would.

答: If you represent the links by their center of mass you should be able to calculate the torque applied at each joint by the weight of the arm. Then your tau on the left hand side would be the tau_control that you're setting minus the tau_links which is the torques placed on each joint by the links, i.e. tau = tau_control - tau_links.</p>


答:
This is something that i have looked for in standard textbooks and have not found much information on it.  I am not sure if it is because they are geared towards industrial arms which are typically not backdrivable.  (meaning once you reach a position, you can essentially turn off your motors and the arm will continue to hold position).  Or if they don't care about this level of control.  (meaning they have low-level controllers that handle it and the arm will "know what to do").  Or if it is too simple of a problem to worry about.

nevertheless, i wrote some code to calculate these torques for a simulated robot.  the output seems correct, although it was never tested.  This is python code for OpenRAVE.

```<code># Calculate the center of mass past the current joint.
# This assumes the mass is specified properly for each link.
def get_COM_past_joint(joint):
    mass = 0.0
    com = np.array([0.0, 0.0, 0.0])
    for i in range(joint.GetDOFIndex(), robot.GetDOF()):
        j = robot.GetJointFromDOFIndex(i)
        link = j.GetHierarchyChildLink()
        mass += link.GetMass()
        com += link.GetMass() * link.GetGlobalCOM()
    # todo: add mass in robot's hand
    com /= mass
    return (mass, com)

# Calculate the torque required to hold joints in current configuration.
# Procedure:
#  1. calculate the center of mass "past" the joint
#  2. find the closest perpendicular distance between the COM and joint axis
#  3. cross the joint axis with gravity vector
#  4. cross the perpendicular distance vector with gravity vector
#  5. return mass * direction * magnitude of both crosses
def get_static_joint_torque(joint):
    (mass, com) = get_COM_past_joint(joint)
    anchor = joint.GetAnchor()
    axis = joint.GetAxis()
    joint_to_com_closest = (anchor - com) - np.dot((anchor - com), axis) * axis
    axis_cross_k = np.cross(axis, np.array([0, 0, -1]))
    joint_to_com_closest_cross_k = np.cross(joint_to_com_closest, np.array([0, 0, -1]))
    direction = np.sign(np.dot(axis, joint_to_com_closest_cross_k))
    return mass*direction*np.linalg.norm(axis_cross_k)*np.linalg.norm(joint_to_com_closest_cross_k)
</code>```

EDIT: I recently found out that OpenRAVE does a more advanced form of this calculation already.  (And their units are correct too).

```<code>robot.SetDOFValues(dofvalues)
robot.SetDOFVelocities(dofvelocities)
torques = robot.ComputeInverseDynamics(dofaccelerations)
</code>```

or

```<code>torqueconfiguration, torquecoriolis, torquegravity = robot.ComputeInverseDynamics(dofaccelerations,None,returncomponents=True)
</code>```

Title: Static equilibrium for 7 dof manipulator

Body:
I have a 7 dof manipulator (Kuka LBR4+) and would like to calculate the joint torques needed to keep the arm in a static equilibrium. In most books the transposed jacobian is used to map the forces applying on the end effector to the joint torques.

$\tau = J^T\cdot F$

That however doesn't take the mass of the links into account. Is there a way to calculate the needed torques for a given configuration so that, assuming an ideal case, by setting these torques the arm will be in a static equilibrium?

cheers

EDIT:

For everybody interested, i found a solution to this problem in Introduction to Robotics - Third Edition by John J. Craig on Page 175-176. It is done with the aid of the iterative Newton-Euler dynamics algorithm. The actual trick is, to set all velocities and accelerations to zero except for the base acceleration. The base acceleration will be $^0 \dot v_0 = G$, where G has the magnitude of the gravity vector but points in opposite direction. This is equivalent to saying that the base of the robot is accelerating upwards with 1 g and this upward acceleration causes exactly the same effect on the link as gravity would.

Body:
This is something that i have looked for in standard textbooks and have not found much information on it.  I am not sure if it is because they are geared towards industrial arms which are typically not backdrivable.  (meaning once you reach a position, you can essentially turn off your motors and the arm will continue to hold position).  Or if they don't care about this level of control.  (meaning they have low-level controllers that handle it and the arm will "know what to do").  Or if it is too simple of a problem to worry about.

nevertheless, i wrote some code to calculate these torques for a simulated robot.  the output seems correct, although it was never tested.  This is python code for OpenRAVE.

# Calculate the center of mass past the current joint.
# This assumes the mass is specified properly for each link.
def get_COM_past_joint(joint):
    mass = 0.0
    com = np.array([0.0, 0.0, 0.0])
    for i in range(joint.GetDOFIndex(), robot.GetDOF()):
        j = robot.GetJointFromDOFIndex(i)
        link = j.GetHierarchyChildLink()
        mass += link.GetMass()
        com += link.GetMass() * link.GetGlobalCOM()
    # todo: add mass in robot's hand
    com /= mass
    return (mass, com)

# Calculate the torque required to hold joints in current configuration.
# Procedure:
#  1. calculate the center of mass "past" the joint
#  2. find the closest perpendicular distance between the COM and joint axis
#  3. cross the joint axis with gravity vector
#  4. cross the perpendicular distance vector with gravity vector
#  5. return mass * direction * magnitude of both crosses
def get_static_joint_torque(joint):
    (mass, com) = get_COM_past_joint(joint)
    anchor = joint.GetAnchor()
    axis = joint.GetAxis()
    joint_to_com_closest = (anchor - com) - np.dot((anchor - com), axis) * axis
    axis_cross_k = np.cross(axis, np.array([0, 0, -1]))
    joint_to_com_closest_cross_k = np.cross(joint_to_com_closest, np.array([0, 0, -1]))
    direction = np.sign(np.dot(axis, joint_to_com_closest_cross_k))
    return mass*direction*np.linalg.norm(axis_cross_k)*np.linalg.norm(joint_to_com_closest_cross_k)

EDIT: I recently found out that OpenRAVE does a more advanced form of this calculation already.  (And their units are correct too).

robot.SetDOFValues(dofvalues)
robot.SetDOFVelocities(dofvelocities)
torques = robot.ComputeInverseDynamics(dofaccelerations)

or

torqueconfiguration, torquecoriolis, torquegravity = robot.ComputeInverseDynamics(dofaccelerations,None,returncomponents=True)


问:
Low-cost servo with digital control interfaces?
Some years ago, there where some projects that provided hardware and software to perform modifications on standard hobby servos to convert them to digital servos, with all the advantages that come with it.

OpenServo is a little outdated, and does not seem to be worked on anymore, and there is no hardware to buy.

Sparkfun has its own version of the OpenServo, which at least is available for buying.

Do you know if there are other mods, or even complete low cost digital servos? I am mostly interested in position feedback, and servo chaining.

答:
Yes, the v3 OpenServo is out of stock.

In fact we are working on a new version 4 of OpenServo that will modernise the codebase.

There are no timelines for the v4 as yet, but work has only just started.

I would recommend the SparkFun board for now.


问:
Simon K firmware when the IMU outputs at 50HZ
I am building a Quadcopter using the Sparkfun Razor IMU which outputs the Roll, Pitch and Yaw axes values at 50 Hz, which limits the operations of the controller(implemented on Arduino IMU), to 50 Hz mx itslef. Please tell me if flashing the ESCs(EMax 40A) with the Simon K firmware can do me any good.

I'll be grateful. :)

答:
Simonk K firmware can give you ESC update rate upto 400Hz. But your output rate is limited to 50Hz as your control loop cannot run at more than 50Hz.

High ESC update rate will only be hepful when you can run your control loop at higher frequency.


问:
What connectors are most reliable?
If you have used connectors for signal wiring for any length of time, you may find that they are unreliable.

Specifically, I find these to be unreliable when used for a long time, with a number of disconnections and re-connections:

This is due to the loss of springy-ness of the crimped metal end on the wire, which causes contact problems.

Which connectors (with rapid connection time) are reliable for multiple re-connections for simple signal wiring?

This excludes screw terminals and connectors with screws (eg. D-subminiature connectors), because they are not simple plug-in connectors.

答:
Maybe too expensive for a home project, but the MIL-DTL-38999 series connector (available from Glenair and many other suppliers) is the connectro of choice for military equipment

OK, it fails your "they are not simple plug-in connectors" requirement but are ideal for disconnect/reconnect in all kinds of environments.

ANd no I don't work for Glenair :)

Title: What connectors are most reliable?


问:
Solutions for Finding Position and Heading in a Multi-Level House
I was wondering if you could reccomend possible solutions for locating a robot within a multilevel house.  What seems obvious to me is that need an altitude sensor to derive the story the robot, and a compass sensor to derive the heading.  However I was wondering what I could use to locate the robots xy position in the house.  If this requirement is unclear, imagine that I have to map a dot representing my robot position to an image of the current floor from the top.

My original idea was to use GPS, however as I need submeter accuracy that would be incredibly expensive.  I also considered Monti-Carlo localization, however that requires no obstruction between sonar sensors and walls.  It is also a significant task programically.  I had an idea to place 3 wireless beacons of some sort on the vertexes of an equilateral triangle surrounding the house, then triangulate my position using distance from each beacon.  However, I have no idea how I would go about this hardware-wise.  Do any of these ideas seem viable, and if so do you have suggestions on how to implement them? Otherwise, can you reccomend an easier or cheaper alternative?  My platform is essentially an arduino hooked up to sensors and motor drivers connected to java on a laptop over serial.  Thanks.

答:
GPS won't give you the accuracy you are looking for. Outdoors it's good for around 1-3 m, indoors if you get a signal at all the multipath will increase your error. There are better GPS technologies out there such as RTK and PPP but their performance degrades significantly when the view of the sky is obstructed.

If you don't have line of sight to the landmarks, your options are a bit more limited.

RF trilateration is a good direction to investigate; there's a lot of interesting research these days into using COTS wifi access points for this purpose. You'll have to read some fairly recent research papers to find an algorithm.

This is actually a pretty difficult problem that you are trying to solve. To make it easier, I would suggest making the problem simpler. Try using a large number of artificial landmarks that are identifiable (such as QR codes or April tags), and use the landmark IDs to indicate the floor of the house you are on. (You can color code them by floor, for example.) From there you can implement a basic SLAM algorithm to find your position. It will be even easier if you map out the locations of the landmarks in advance, because then instead of SLAM you can just do triangulation.


问:
Grasshopper effect on a quadcopter with kk2.1.5
We have builded a quadcopter that use the flight manager kk2.1.5 with the latest firmware. When we increase the throttle, it flight. When we keep the hand on the stick we are able to maintain it but when we don't touch to the throttle, it goes up and down.

You can see an example on this video.

We have tried different values for PID but we don't know what is the best for us. We have a large quadcopter with medium propellers (may be too small).

Does the weight of the quadpcoter or the width of the propellers can be a factor? What can be the problem?

答:
You've correctly identified 2 out of the 3 possible sources for the problem you're having.  They are:

The propellers or motors may be inadequate for the weight of the quadcopter.  Since the propellers are able to lift the quadcopter off the ground, they are probably not causing the problem you see.

The PID settings may be incorrect.  Look at this question for some advice on tuning them.

The altitude sensor may be faulty. If the PID isn't getting good data in, then it won't be able to act appropriately.  If your quadcopter can't measure altitude much beyond zero, it could explain the behavior you're seeing.