问:
connecting MPU-9250 GY-9250 SENSOR MODULE to arduino uno
i am using this sensor to make self balancing robot.At first i have soldered the header(only to vcc,gnd,scl,sda ) on the imu borad at the opposite side where there is no component mounted.then connecting it to arduino uno r3(vcc to vcc 3.3v/5v,gnd to 1 of 3 gnd,scl to scl and sda to sda(first time at those next to AREF, second time A5,A4) ) i uploaded the sketch https://github.com/adafruit/Adafruit_ADXL345/blob/master/examples/sensortest/sensortest.pde then when i opened the serial monitor i got

Accelerometer Test

FF Ooops, no ADXL345 detected ... Check your wiring!

i thought may be i have soldered the header in wrong direction(as in picture and videos at internet,they are so) so i desolder(with solder iron,no other technique) the header,but there were still some solder around the hole which i could not remove.then while checking the continuity between pins with multimiter(in resistance mode) i found the resistance to be 20k(scl-sda),220k(scl-gnd),220k(sda-gnd),between vcc and 3 other pins multimieter shows 1(range 2000k). then i soldered it on opposit side(this time where other components are mounted).the serial monitor still shows same output,and so does the muiltimeter.so where is the problem? is it with soldering ?do i need to disolder the header again and clean left out solder(with Chip Quik type desoldering technique ) on the opposite side(no component mounted)?is there any hope that i won't need to buy it again?

picture of opposite side where no component is mounted and this is after desoldering and resoldering

答:
I don't know the details of the ADXL345 or the MPU-9250, but I wouldn't expect a MPU-9250 to respond to the Arduino running a program for the ADXL345.  So my first suggestion is to search for the right program for your board.

Secondly I think you are using I2C to connect to the board.  The SCL and SDA lines need pull up resistors, so place a 4.7K resistor between SDA and VCC and another between SCL and VCC.

Both the chips you mentioned are 3.3V chips, the boards might handle 5V but you need to check the data sheets. If they can't hand 5V then you will need a logic level shifter.

You might have problems getting a good connection on the two holes, but that's only a problem if you need them and I don't think with I2C you will.


问:
Transfer function of a quadrotor position controller
I'm trying to find the transfer function of a quadrotor with two controller loops, following next structure:

I know how to calculate the attitude stability controller, which relate rotor speed and desired angles. However, I have no clear at all how to implement the translational controller transfer function, whose output is the desired angle that the rotors must achieve considering the position I want to translate.

Considering that two controllers are PD, how can you calculate the translational controller transfer function and include it in the system? Time domain equations in the outer loop are next, where U terms relate to the thrust axis components. Thanks

答: You have the equations (although I don't understand your $\ddot x$ term in the position equation).  You just need to take it into the frequency domain, where a time derivative is $s$ times the variable, and solve for output over input $\vec x \over \vec u$.    If you search for state space transfer functions it should be straightforward.  </p>

ContentLicense: CC BY-SA 3.0


答:
If your $u_x$, $u_y$, and $u_z$ terms are desired accelerations, and it looks like they are, then the quad would be commanded to an attitude where those accelerations can be achieved. So

$ \ddot x_{vehicle} = T_{vehicle}^T *F_{throttle} - g$

where $\ddot x_{vehicle}$ is the acceleration of the vehicle in the inertial frame and $T_{vehicle}$ is the transformation from the inertial frame to the vehicle frame. This is the equation relating the output of the position controller to the input of the attitude controller. It allows you to solve for the desired attitude and throttle (which you seem to be missing from your system).

-
the real answer to your question is that you will not be able to get a transfer function relating desired position to commanded attitude because the transformation in the equation above is nonlinear.

Title: Transfer function of a quadrotor position controller

Body:
I'm trying to find the transfer function of a quadrotor with two controller loops, following next structure:

I know how to calculate the attitude stability controller, which relate rotor speed and desired angles. However, I have no clear at all how to implement the translational controller transfer function, whose output is the desired angle that the rotors must achieve considering the position I want to translate.

Considering that two controllers are PD, how can you calculate the translational controller transfer function and include it in the system? Time domain equations in the outer loop are next, where U terms relate to the thrust axis components. Thanks


问:
What is the millisecond rate that robot-create can respond to two different drive commands?
I want to issue two slightly different drive commands what is the smallest loop rate that the robot-create accepts new commands?

I know from reading the documentation that it appears the sensors are read every 15ms.

Not sure what the command rate is?

答: The command rate should be the same as the sensors (so ~15 ms)</p>


问:
What's an efficient way to visit every reachable space on a grid with unknown obstacles?
I'm trying to create a map of the obstacles in a fairly coarse 2D grid space, using exploration.  I detect obstacles by attempting to move from one space to an adjacent space, and if that fails then there's an obstacle in the destination space (there is no concept of a rangefinding sensor in this problem).

example grid http://www.eriding.net/resources/general/prim_frmwrks/images/asses/asses_y3_5d_3.gif (for example)

The process is complete when all the reachable squares have been visited.  In other words, some spaces might be completely unreachable even if they don't have obstacles because they're surrounded.  This is expected.

In the simplest case, I could use a DFS algorithm, but I'm worried that this will take an excessively long time to complete — the robot will spend more time backtracking than exploring new territory.  I expect this to be especially problematic when attempting to reach the unreachable squares, because the robot will exhaust every option.

In the more sophisticated method, the proper thing to do seems to be Boustrophedon cell decomposition.

However, I can't seem to find a good description of the Boustrophedon cell decomposition algorithm (that is, a complete description in simple terms).  There are resources like this one, or this more general one on vertical cell decomposition but they don't offer much insight into the high-level algorithms nor the low-level data structures involved.

How can I visit (map) this grid efficiently?  If it exists, I would like an algorithm that performs better than $O(n^2)$ with respect to the total number of grid squares (i.e. better than $O(n^4)$ for an $n*n$ grid).

答:
Boustrophedon cell decomposition is simply sub-dividing an environment into areas which can be efficiently covered by a boustrophedon path. A trapezoidal decomposition will do, and can be accomplished using a line-sweep algorithm.
See [Choset 2000], This web site
, or (I recommend!) the excellent book "Computational Geometry" by Mark de Berg, et. al, for a complete description of the data structures and algorithms required.

Choset, Howie. "Coverage of Known Spaces: The Boustrophedon Cellular Decomposition" Autonomous Robots, 2000.

For example, consider the set of obstacles as edges and vertices. Let's say the environment is also bounded by a special polygon. We have something like the following. To decompose this space, we simply add vertical edges between every vertex and the nearest line or vertex.

To accomplish this in code, you need only a line-segment intersection test, a sorted list of edges, and a sorted list of vertices.

From every vertex $v_i$ in left-to-right order,

Create a vertical line $l_i$ at each $v_i$, extended until the first edge or vertex it intersects

At each intersection, create a new vertex.

When this is done, the set of new edges and vertices encloses only trapezoids. But I emphasize, you can't do this online (without prior knowledge of the obstacles). If you want to do robust coverage without prior knowledge, you might look at "bug algorithms." In particular, here's a simple algorithm, assuming the environment is bounded.

From the start position, move up and left until you reach the upper-left corner of the environment. If you encounter an obstacle first, you must travel around it. You know something is an obstacle if it can be circumnavigated (bump and move).

From the upper left, move right until you encounter the boundary. Then move down and left (We're doing a boustrophedon of the entire space).

When you are on a left-right line and encounter an obstacle, you have two options.  (i) We can circumnavigate until we reach the left-right line we are trying to cover, then continue.  (ii), We can turn around and cover a new left-right line until we find our way past the obstacle or end up in this situation again.  I'll illustrate.

On the left, we move around the obstacle until we can return to the "line" we were trying to follow. On the right, we continue covering the (smaller) area on the one side of the obstacle.

The advantage of the first method is you always map out the obstacle completely before you make a decision about how to get around it, thus you can take the shorter path. The advantage of the second method is you don't have to get around the obstacle at all, you can just proceed to cover the area you are in.

Note that this defines your boustrophedon decomposition in an online way: You cover the area between the obstacles or between the obstacles and the boundary.

However, as far as I know, the first method is easier to analyze. The more complicated algorithms (like BFS, etc), are chosen either because the environment is unbounded (you don't want to spend forever looking for a boundary), or there is a really nasty obstacle in the way that basically partitions the environment.  Why is this bad? look at this example:

Moving left-right, then circling each obstacle produces way too many covers of the small parts between each obstacle. In fact, without global path planning, you can make this as bad as the resolution of your grid by placing these columns 1 px wide, as tall as the entire environment and 1 px apart. Then you'd have to move around the obstacle each time you hit it.

This is why I asked if you had some idea of where you were in the environment or could do global path planning. But the online vs offline discussion and optimal algorithms for this is not what you really wanted.

Update: I had to remove the images (not https), and will post this which is often used in practical real-world applications. http://www.cs.cmu.edu/~motionplanning/papers/sbp_papers/integrated1/yamauchi_frontiers.pdf

ContentLicense: CC BY-SA 4.0


问:
Ultrasonic sensor range and shape
I have been looking for a cheap ultrasonic sensor that is not blind under +/-30 cm but the only sensors I could find use the following shape, which is not suitable for my project (because of the robot design that only has 1 hole, and not 2..) :

Is there any chance to find a sensor with that other shape with a range starting around 5cm ?

Actually I am wondering if that 2nd shape makes this constraint mandatory or if I just did not found the appropriate product.

答: A single transducer sensor has to both send out the sound and then sense the return sound. It takes time for the vibration to stop after making the sound and that is time it can't sense a return. So close distances can't be sensed by a single transducer sensor.</p>

Title: Ultrasonic sensor range and shape


问:
Create2 Serial. Canonical versus Number of Bytes interface
There is a message system which does not appear to be document in the OI spec. This appears to be a a canonical terminal type serial interface in which messages come back such as firmware version and stuff. I am not sure how to determine what the end of this type of message is? It is a fixed number of end lines? or Bytes. One message seems to indicate STR730 which would be a 730 byte string.

The open interface spec seems to indicate a non canonical interface spec in which you read a fixed number of bytes with no processing of end lines. Is this correct?

答:
You should not have to worry about those messages (or length of) since they are disabled once the OI is started (with the start command '128'). After OI is started, flush whatever data is on the line then request your desired sensor data.

As you noted, these are mainly just firmware/hardware version strings. There is no defined length of these messages, but they should always be terminated by "\r\n".


问:
Servo motors for large scale RC car
I want to convert an electric ATV (quad) for kids (like the HIGHPER ATV-6E) to radio control for a robotics project. These small ATVs are about a meter long and weigh about 40 kg. I need to choose servo motors for steering and braking. What grade servos do I need and how much torque do they need to have? Can I use the strongest RC servo I may find (like this 115kg/cm one or maybe even more, with metal gears of course) or do I need an "industrial grade" servo?

I plan to use one servo for steering and one for braking. For braking the ATV has mechanical disc brakes - two discs in the front and one common disc in the rear (there are two brake levers - front/rear). I plan to use only one servo and use it either for front or for rear. The plan is to mount the brake wire to the servo which would "simulate" the lever movement.

I guess I could also make a "weak" servo stronger by adding a proper gear, but I am not really into mechanical engineering much and would prefer an off-the-shelf component.

答:
What an interesting project!

For braking I was going to suggest a linear actuator, but then I realized you have an ATV already designed for the human hand to actuate the brakes.   Why don't you look into low cost electric robotic grippers, and mount one to the handlebar to take advantage of the existing brake lever design?  That would be pretty easy to integrate and control.

Steering is more of a challenge.  I agree with @hauptmech that you need to measure the required torques.   That could be difficult, though, since the range of torques needed to steer will vary widely through your operating scenarios.   My suggestion is to put the ATV on a downward incline, so that the steering wheels have the most load on them, and use a torque wrench to measure how much (worst-case) torque is needed to steer.   This should be sufficient to size that motor (or motor + gearbox).   You will have to keep in mind how fast you need the steering column to rotate, but I suspect if you can overcome the torque necessary to move the front wheels when standing still, you shouldn't have an issue with speed of rotation.

Title: Servo motors for large scale RC car


答: For the steering: I have seen boat "autopilots" which use a linear actuator that connects (via a bearing or universal joint) to the boat's steering tiller. I think a similar arrangement would work attached to one end of your handlebars. It would have to be robustly attached to prevent it shaking loose, and it would have high force but might be quite slow.</p>

Title: Servo motors for large scale RC car

Body:
I want to convert an electric ATV (quad) for kids (like the HIGHPER ATV-6E) to radio control for a robotics project. These small ATVs are about a meter long and weigh about 40 kg. I need to choose servo motors for steering and braking. What grade servos do I need and how much torque do they need to have? Can I use the strongest RC servo I may find (like this 115kg/cm one or maybe even more, with metal gears of course) or do I need an "industrial grade" servo?

I plan to use one servo for steering and one for braking. For braking the ATV has mechanical disc brakes - two discs in the front and one common disc in the rear (there are two brake levers - front/rear). I plan to use only one servo and use it either for front or for rear. The plan is to mount the brake wire to the servo which would "simulate" the lever movement.

I guess I could also make a "weak" servo stronger by adding a proper gear, but I am not really into mechanical engineering much and would prefer an off-the-shelf component.


问:
Robotic Arm analysis in Matlab/simulink
I am going through a paper, Kinematic Modelling and Simulation of a 2-R Robot Using SolidWorks and Verification by MATLAB/Simulink, which is about a 2-link revolute joint robotic arm. According to the paper, the trajectory analysis of the robot was done via simulations in MATLAB/Simulink.

It shows the following picture, Trajectory generation of 2‐R robot with MATLAB/Simulink:

and then, Simulink - Simulation block to calculate the trajectory:

I think this is done in SimMechanics, but I am not sure. Experienced users, can you please tell me what am I looking at and how can I reproduce this?

答:
The paper does not say anything about Subsystems. Does it mean that I can't implement it?

This question implies that you don't really know what a subsystem is in the context of matlab.

It's as if you got a new car and a friend of yours says "that's a nice yellow color" and your response would be "What does 'yellow' mean? Can it still drive?"

Please take a step back and think about your approach once again.

This matlab/simulink code is not ment to be used as an introduction tutorial for beginners. Try to familiarize yourself with the fundamental basics of the programming platform separately.

Otherwise you have to solve one equation for two unknowns: What parts of the code are the way they are due to the content of the paper and what due to how matlab/simulink works?

From the paper you can deduce what they implemented, but if you don't know how, it becomes hard (and more importantly: unreliable) to deduce any further information from the code.

Choose tools according to the problem at hand. If using the tool becomes a problem in of itself, you can get trapped in the need-new-tool-to-solve-the-problem-of-the-previous-tool loop.

Title: Robotic Arm analysis in Matlab/simulink


答:
It looks like the second image is a Simulink system. That doesn't mean it's not also SimMechanics, because there are ways to interface Simulink signals to Simscape.

The thing that is hard (impossible) for you is that those are subsystem blocks, meaning that you can't replicate the code contained inside of them without being able to see inside.

The thing that's really a pain is that it might be stock functionality, or a couple stock blocks strung together, but you can't tell because it's been encapsulated.

Title: Robotic Arm analysis in Matlab/simulink

Body:
I am going through a paper, Kinematic Modelling and Simulation of a 2-R Robot Using SolidWorks and Verification by MATLAB/Simulink, which is about a 2-link revolute joint robotic arm. According to the paper, the trajectory analysis of the robot was done via simulations in MATLAB/Simulink.

It shows the following picture, Trajectory generation of 2‐R robot with MATLAB/Simulink:

and then, Simulink - Simulation block to calculate the trajectory:

I think this is done in SimMechanics, but I am not sure. Experienced users, can you please tell me what am I looking at and how can I reproduce this?


问:
How can I modify a low cost hobby servo to run 'freely'?
I've got some hobby servos (Power HD 1501MGs) and I'd like to be able to control them (via an Arduino) so they will either go to the angle I set, or put them in a 'free running' mode, where the load will take them wherever it goes.

Is this even possible, or am I just going to end up stripping the gears?

My first thought is to simply kill the power to the servo, but the force required to move them in that state is more than I'd like.

If it is possible, am I looking at a hardware change, or could I do it in software?

答: Don't, instead get yourself some decent motors and drive them via an h-bridge like you'll find on this motor shield.


问:
Downgrade ROS from Jade to Indigo
Is it possible to downgrade from ROS Jade to Indigo?

For those who are not yet familiar with Robot Operating System (ROS), here: ROS

答:
You can have Jade and Indigo installed in parallel on the same system and use each one individually with different projects.

Ask on answers.ros.org. Don't forget to tell them what OS & OS version you are using. Installing Indigo in parallel via manual installation should not present any problems. apt style installation depends on your OS & version.

Title: Downgrade ROS from Jade to Indigo


问:
Meaning of s_last in D star Lite algorithm
In the D*Lite algorithm, described in line 21 of Figure 3, on page 4, in D* Lite, the main() starts with defining $s_{last}=s_{start}$. But value of $s_{last}$ is never updated in the entire algorithm.

So what is the purpose of defining this term and what does it mean?

答:
$s_{last}$ does change.

Looking at the pseudo code, $s_{last}$ is updated upon each iteration of the while loop in main(), in line 31, $s_{last}=s_{start}$, if the condition on line 29 is met:

```<code>if any edge costs changed
</code>```

Likewise, in figure 4 $s_{last}$ is updated on line 39:

Body:
$s_{last}$ does change.

Looking at the pseudo code, $s_{last}$ is updated upon each iteration of the while loop in main(), in line 31, $s_{last}=s_{start}$, if the condition on line 29 is met:

if any edge costs changed

Likewise, in figure 4 $s_{last}$ is updated on line 39:


问:
Mechanical odometer with digital output
I would like to mechanically measure the distance a kids electric ATV traveled. The ATV will not be used with kids but as a mobile robot instead. It has a common rear axle for both rear wheels which I think could be a good place to put an odometer on (since the chance both wheels will slip should be minimal). Regarding suspension it has a single shock for rear axle.

My plan is to put a bigger gear on the axle itself and then add a smaller gear to it on which some kind of sensor would measure number of its rotations. One rotation of the axle may be something like 20 rotations of the small gear. What kind of sensor can I use for sensing rotation?

Another way of making an odometer may be some kind of optical solution (disc with holes and an optical sensor) but this seems to be rather complicated and also the the direction of travel could not be easily estimated (unless the motor is running in some direction).

I just found a term called Wheel Speed Sensor which looks interesting and seems to employ primarily non-contact sensing (which is definitely better than mechanical gears). Rather then optical solution I like the Hall effect sensor solution which may be simple and mechanically robust. But still, my question is open on how to implement this...

I would like to use the odometer for both speed estimation and distance estimation. I need to read the sensor from C/C++ on a Linux box.

EDIT: The thing I am looking for is probably correctly called a rotary encoder or a wheel encoder.

The ATV may look like one of these:

答:
I don't think that an odometer is the right sensor for this task.

The wheels will definetly slip. The error will add up quickly and the measurement will not be very good. These types of sensors are not made to measure several kilometers of distance traveled.

Use GPS instead. It does not require any modification of the vehicle. The data is much more versatile, because you can put it onto a map, etc.

If you were to go for a walk yourself, would you attach some sensor to your feet or rather hit the record button on a Smartphone?

I want to use this mechanical odometer to supplement GPS. GPS (even with RTK corrections) is not reliable at all in areas where the signal is blocked (tall buildings etc.). So a properly setup mechanical odometer will be MUCH more accurate than GPS

To be properly setup, the odometry has to have it's own wheel independent from the motor driven ones that's in contact with the ground. This way you can guarantee that the influence of slip is minimized.

But looking at the ground in your images, I doubt that this will work well.

Instead, use computer vision to determine the position and speed of the vehicle. OpenCV provides functionality for such applications.

Title: Mechanical odometer with digital output


答: The automotive industry frequently uses Hall effect sensors to measure shaft and gear rotation.  The Hall effect has some beneficial properties:  it operates over a wide range of temperatures, is more immune to contaminants than, say, optical technologies, comes in a wide range of response profiles, and with multiple mounting options.  I'd suggest placing a magnet on the face of your smaller gear, near the circumference, and use a cylindrical, barrel mount Hall pickup to sense each rotation of the magnet.  If you also want to know direction of rotation, mount a second magnet/Hall sensor slightly offset angularly from the first (and at a different radius from the center of the gear).  The timing of the two signals will give rotation direction.</p>

Title: Mechanical odometer with digital output

Body:
I would like to mechanically measure the distance a kids electric ATV traveled. The ATV will not be used with kids but as a mobile robot instead. It has a common rear axle for both rear wheels which I think could be a good place to put an odometer on (since the chance both wheels will slip should be minimal). Regarding suspension it has a single shock for rear axle.

My plan is to put a bigger gear on the axle itself and then add a smaller gear to it on which some kind of sensor would measure number of its rotations. One rotation of the axle may be something like 20 rotations of the small gear. What kind of sensor can I use for sensing rotation?

Another way of making an odometer may be some kind of optical solution (disc with holes and an optical sensor) but this seems to be rather complicated and also the the direction of travel could not be easily estimated (unless the motor is running in some direction).

I just found a term called Wheel Speed Sensor which looks interesting and seems to employ primarily non-contact sensing (which is definitely better than mechanical gears). Rather then optical solution I like the Hall effect sensor solution which may be simple and mechanically robust. But still, my question is open on how to implement this...

I would like to use the odometer for both speed estimation and distance estimation. I need to read the sensor from C/C++ on a Linux box.

EDIT: The thing I am looking for is probably correctly called a rotary encoder or a wheel encoder.

The ATV may look like one of these:


问:
Hand-eye calibration?
I am having an issue with some hand-eye calibration.

So i am using a simple robot which at its tool point has an stereo camera mounted on it.

I want to perform some visual serving/tracking based stereo images extracted from the camera in the "hand". The camera provides me x,y,z coordinates of the object I want to track.

I can at all time extract an homogenous transformation matrix from base to tool (not cam) as (T_tool_base).

Firstly... I guess i would need perform some form of robot to (vice versa) camera calibration, My idea was that would consist of something like this

```<code>T_base_world = (T_base_tool) (T_tool_cam) (T_cam_world)
</code>```

Where the T_tool_cam would entail the calibration... since the camera is at the tool point, would that entail the T_tool_cam should entail information on how much the camera is displaced from the tool point, and how it is rotated according to the tool point? or is not like that?

secondly... How do i based purely x,y,z coordinate make an homogeneous transformation matrix, which includes an rotation matrix ?

thirdly.. Having a desired Transformation matrix which in theory this

```<code>T_base_world = (T_base_tool) (T_tool_cam) (T_cam_world)
</code>```

would provide me, would an inverse kinematics solution provide me with one or multiple solution?... In theory should this only provide me one, or what?

答:
If $x, y, z$ are the coordinates of the object in the camera frame, then the camera to object transform will be:
$$\begin{pmatrix}1 & 0 & 0 & 0\\ 0 & cos\theta & -sin\theta & 0\\
0 & sin\theta & cos\theta & 0\\
0 & 0 & 0 & 1\\ \end{pmatrix}
\begin{pmatrix}0 & 0 & 0 & x\\ 0 & 0 & 0 & y\\
0 & 0 & 0 & z\\
0 & 0 & 0 & 1\\ \end{pmatrix}
$$

The tool->cam transform includes a single rotation about the $x$ axis of the tool by $\theta$ from the tool frame to the camera frame, followed by the camera to object transform.

Rotation of the object is unaccounted for.

Title: Hand-eye calibration?


问:
What algorithm should I use for balancing a two wheeled robot using a gyroscope?
Is there a good, popular and reliable algorithm I can use by taking input from a gyroscope and using this to control two independant wheels to keep such a balanced robot reliably upright? I'm looking for an algorithm that will let me use it to drive a robot around as well as keep it upright when stationary. The ability to deal with inclines and people nudging it would also be a bonus, but not essential.

答:
The simplest controller is a linear state feedback controller. There are essentially 4 different states that you need a gain for. These are tilt angle, tilt rate, speed and position.

LQR (linear quadratic regulator) is a method to design these gains (after obtaining a linearized state-space representation of your system). If you do not have a state space representation (you probably don't), you can get equations of motion and measure the parameters. If you do not have a state space representation, you should just tune the gains manually (without LQR or other methods such as pole placement).

Tuning gains manually:

Assuming tilt angle, position/speed and wheel torques are all directed forwards (if positive), you want a positive gain on your tilt angle and tilt rate, as well as a positive gain on position and speed.

Start with a gain on tilt angle, and tilt rate. This will get it to balance initially. Once it remains balanced, you can control position and speed by adding a gain to them. If it is unstable, increase the gain on tilt rate (which helps to damp the system).

The position/speed control will control both states to zero. To control to some other value, you just need a reference tracking controller, by replacing the states with their errors before feeding it into your controller (eg. current speed - speed reference).

Yaw control can be done independently (with the differences in wheel torques added to the main balance/speed/position controller).


问:
How do I design for a target speed?
I need to make an omni wheeled robot platform (4 wheels), which should go at a minimum speed of 15 cm/s.  I have an idea for the design, but since this is my first time doing something like this I have made a lot of assumptions.

I decided to choose the TGY-S4505B servos as my motor system. I intend to attach these servos to FXA308B wheels. Finally, I intend to power my servos with one Turnigy LSD 6.0V 2300mAh Ni-MH Flat Receiver Packs (not sure if LiPo is a better choice). I need to be able to run the servos continuously for roughly 8 minutes. You can ignore the microcontroller and other stuff, relatively speaking they will consume much less power. The robot will have four wheels (thus, four servos).

The basic specifications of each servo is:

Type: Analog

Gear train: Plastic

Bearings: Dual

Motor Type: Carbon Brushed

Weight: 40g (1.41oz)

Lead: 30cm

Torque: 3.9kg.cm @ 4.8v / 4.8kg.cm @ 6v

Speed: 0.13sec 60°@ 4.8v / 0.10 60° @ 6v

So based on my battery pack, I will be running the servos at 6V. That gives me a speed of 60 degrees per 0.10 seconds. I plan on modifying these servos for continuous rotation, and connected them directly to the wheel. Since the wheel has a diameter of ~5 cm, it has a circumference of ~15 cm. Based on these specs, it seems to me that my robot can move at roughly 15 cm/0.6 seconds, or 25 cm/s (quite fast actually). I don't intend to run it constantly at that speed, so in the 8 minute run, assume my average speed to be 20 cm/s.

Are these assumptions reasonable, and are the calculations correct?  I would really appreciate any insight, advice, recommendations, and criticisms you may have.

答:
With all you're going to invest in your robot, it's a better idea to start off with a more solid drive train than a servo.  Take a look at a DC motor with an integrated gearbox and start from there.  Your servo approach won't be as energy efficient, performant, and it won't last as long.

A great reference to start with is Building Robot Drive Trains.

Title: How do I design for a target speed?


答:
The paper you mentioned in the comment clearly presents on SimMechanics model, but it also presnets the necessary equation to solve the inverse kinematics problem.

It seems that the screenshot you have submitted calculates sine and cosine functions of the angles. This would not be necessary if simmemchanics blocks would be used in the subsystems. It seems that the simmechanics model is used to verify the correctness of the inverse kinematics. It is a very convenient way to verify and validate the results. And the correctness of the equations.

From the point of view of the results the two approaches, the analytic solution and the numeric solution using simmechanics have to be the same (with a given tolerance of the numerical error).

From a performance (calculation time) point of view, the analytical solution should be superior to any other numerical solution.

Title: Robotic Arm analysis in Matlab/simulink


问:
Enable Bluetooth communication with iRobot Create 2
I just got a new iRobot Create 2. I used to use an Element Direct BAM (Bluetooth Adapter Module) for iRobot Create previously.

How can I communicate with a Create 2 using Bluetooth? What accessories do I need?

答:
The Create 2 projects page has a tutorial on how to make a Bluetooth serial module.

The Rootooth should work too.  Although it is no longer sold, but you might be able to find one somewhere.  Or make one yourself since SparkFun put all the designs online.

I couldn't find much information on the BAM, but I bet you could make it work with the Create2.  Consult the above mentioned projects page to determine the new Create pinout and if any level shifting is required.


问:
Quadcopter heading calculation
I'm working on an autonomous quad copter, I have two GPS co-ordinates (source and destination co-ordinates). I need to move my quad from the source to the destination, for this I need to calculate the heading and set the yaw value of my quad. How can I calculate the heading and make sure the quad is headed in the right direction as the target co-ordinates?

If I use magnetometer the declination angle will vary from place to place and so I will have to keep changing the declination angle. If I'm calculating based on just the GPS co-ordinates, it's not accurate.

What is the best way to do this?  How do I calculate the above?

答:
The habitual way to do this is to have an attitude and position loop for the quadrotor, as in next picture.

Attitude loop manages the quadrotor to achieve the desired angles (pitch, yaw, and roll). Position circuit on the other hand, calculates which are those desired angles depending on the path you want to follow or the point you want to reach.

One GPS is used for desired position, while your other GPS should be used for actual coordinates.

Have a look at this thread where formulas and expressions are furtherly explained

Title: Quadcopter heading calculation


问:
ComputeShortestPath() in Dstar lite algorithm
In optimized D*Lite algorithm as shown in the figure below (page 5, of the paper D*Lite), when the procedure ComputeShortestPath() is called for the first time in line 31, U(list of inconsistent vertices) contains only goal vertex ($s_{goal}$). Thus in the procedure ComputeShotestPath()(line 10-28), $u = s_{goal}$. And as, $k_{old}=k_{new}$ (because $k_m=0$), condition $k_{old}\leq k_{new}$ is satisfied and $u = s_{goal}$ is again inserted in U with same value of $k_{old}=k_{new}$. Thus, it seems that line(11-15) will run forever, and the algorithm will not be able to find the shortest path from goal to start.

I know that this algorithm has been widely used and I am failing to understand it. But where am I going wrong?

答:
The first relevant line is line 6:

$$U.Insert(s_{goal}, [h(s_{start}, s_{goal});0]);$$

Basically speaking, this is an association of the goal to some vector with two values in it. Now $U$ looks like this:

$$U:s_{goal}\rightarrow \left(\begin{matrix}h(s_{start}, s_{goal})\\0\end{matrix}\right)$$

Of course, $U$ is a list of such associations, but within the context of this question there's only one element in it.

Thus in the procedure ComputeShotestPath() (line 10-28), $u = s_{goal}$

That is correct.

And as, $k_{old}=k_{new}$ (because $k_m=0$)

That is not necessarily correct. The relevant lines are 12

$$k_{old}=U.TopKey();$$

Looking at what $U$ is above, this line is equivalent to:

$$k_{old}=\left(\begin{matrix}h(s_{start}, s_{goal})\\0\end{matrix}\right);$$

and line 13 for $k_{new}$:

$$k_{new}=CalculateKey(u)\color{red}{)};$$

I think the extra parenthesis is a typo. I will ignore it. This value actually comes from a function call and results in the following:

$$k_{new}=\left(\begin{matrix}\min\big(g(s), rhs(s)\big) + h(s_{start}, s) +k_m\\rhs(s)\end{matrix}\right);$$

Where $s$ is the parameter passed to the function, which is actually $u$ which in turn is actually $s_{goal}$, also as you stated $k_m=0$, thus:

$$k_{new}=\left(\begin{matrix}\min\big(g(s_{goal}), rhs(s_{goal})\big) + h(s_{start}, s_{goal})\\rhs(s_{goal})\end{matrix}\right);$$

I don't know what the functions $g()$ and $rhs()$ do (look them up in the paper) and what values they return, but if $rhs(s_{goal}) = 0$ and $g(s_{goal}) > rhs(s_{goal})$ then yes it's true that $k_{new}=k_{old}$.

condition $k_{old}\leq k_{new}$ is satisfied

I think there's your problem. I for one cannot see a condition $k_{old}\leq k_{new}$ being evaluated in the code. I guess you are referring to line 14, but that line actually looks like that:

$$if(k_{old} \color{red}{\dot\lt}k_{new})$$

This is not a less-than-or-equal sign. That is a les-than sign with a dot above it. I don't think they are the same thing and I wonder if the following is actually valid way to express that:
$$\le\neq\dot\lt$$

Anyway, what the heck is $\dot\lt$? I don't know. What I think it means is that it's an element wise less-than operator. In languages like Matlab or Octave it's common to add a dot to denote that an operator is applied element wise. The thing is that it doesn't make too much sense to apply a less-than operator to a vector. It's defined for scalar values, not vectors or matrices. I admit that I'm not sure about this one.

The condition in line 14 is not satisfied.

Now all of the above is under the assumption that the condition of the while loop in ComputeShotestPath() is true and the body of the loop is executed.

The (first part of the) condition looks like this:

$$U.TopKey()\dot\lt CalculateKey(s_{start})$$

With the assumptions from above this expands to:

$$\left(\begin{matrix}h(s_{start}, s_{goal})\\0\end{matrix}\right)
\dot\lt
\left(\begin{matrix}\min\big(g(s_{start}), rhs(s_{start})\big) + h(s_{start}, s_{start})\\rhs(s_{start})\end{matrix}\right)$$

Again, I don't know  how this algorithm works exactly, I had a quick look at the paper and from what I understood is that $h(s_{start}, s_{start}) = 0$ and $rhs(s_{start}) = 0$, which makes me doubt that the condition is true.

In other words: I don't think the while loop is even running at all and if it actually does, it's not because of that part of its condition.

tl,dr;
Holy cow, stop staring at nontextual pseudocode! Get yourself an implementation (or write it yourself) and use a debugger to step through the program.

Run the actual code on an actual machine, this is what matters in the end.


问:
Will connecting two servo motors double the torque?
For my robot, I am using two continuous rotation servos to spin a threaded rod. I am trying to make this project as cheap as possible. Here are the servos that I can find:

Servo #1: This is a very cheap option and it has half of the torque I need.

Servo #2: This has all of the torque my project requires, but it is much more expensive that two of servo #1.

Can I hook up two of servo #1 to each end of the rod and have them move synchronized? I can spare a few extra pins on my microprocessor that I am using; that isn't a issue. I know hooking two together will increase torque, but I don't want 75% of the torque I want in this situation. Also, I don't care if I only have 98% of my torque "goal" with the extra weight (which probably won't happen) but I don't want to, like I said earlier, have 70, 80, 90% of my "target goal" of torque if possible.

Any help appreciated. Thanks in advance.

答: Put some value on your time and on system reliability.  You're increasing design time & failure points with this approach.  Good small gear motors are pretty cheap to start with, it's a lot more valuable to have a working robot that's reliable than a broken or unfinished robot on the shelf.</p>


问:
Most accurate rotation representation for small angles
Assume that I have a rigid body for which I know that it can rotate with respect to a global reference frame (which is considered fixed and already given) for only a few degrees of angle, so I can describe its rotation by using the small angle approximation. For this system, I would like to know if there is a rotation representation that offers more accuracy when compared with other representation methods.

The main representation methods that I considered are the euler angles and the pitch-yaw-roll transformation. To my perception, I think that pitch-yaw-roll representation is expected to be more accurate, since all the angles are expressed with respect to the initial coordinate frame. On the other hand, euler angles are defined on different frames, so I am not sure if the resulting angles will be really small.

To sum up, I know that the body can rotate for only a few degrees and I would like to know which coordinate representation is much probable to deliver the smallest angles, such that the small angle approximation is more valid.

It could also be the case that there is not a general answer (so it depends on the specific configuration) but still I haven't found anything about this topic on the related literature!

Edit: This question is not related to numerical issues. Therefore, it is assumed that all the possible rotation matrix descriptions (Euler, PYR etc) result in the same, exact coordinate vector. Therefore, the question is if there exists one parametrization that is composed of the smallest possible angles.

Example (no small angle approx used): Assume I have a coordinate frame which describes a point in space by the following vector

$P2=\begin{bmatrix} 4 \\ 1 \\ 0.05 \end{bmatrix}$.

Given another coordinate frame which is rotated with respect to the previous one, the description of the same point is given by

$P1=\begin{bmatrix} 3.8933 \\
    1.3566 \\
   -0.0630 \end{bmatrix}$.

Using Euler angles, I can find that the rotation matrix $R_{euler}$ is characterized by the angles $0.1,0.2,0.1$ rads, which correspond to the angle of rotation around z axis, the rotation around the resulting y axis and the rotation around the resulting z axis, respectively (these are basic stuff, it is explained in many books.). So I have that $P1=R_{euler} P2$.

Now I want to find the corresponding rotation matrix if I use the pitch-yaw-roll representation. Here I have to solve an optimization problem and the solution that I get (maximum error between P1 and the estimated P1 is $3 \times 10^{-8}$) delivers me the following angles

$\begin{bmatrix}  -0.0103   \\ 0.0257  \\  0.0902\end{bmatrix}$,

which correspond to the rotation around the x,y and z axis of the initial coordinate frame.

答:
@jpro, I think you are not understanding something about kinematics.  Whether you use Euler angles, or homogeneous transforms, or rotation matrices, or quaternians, or any other kinematic representation, ALL of them relate the object's pose with respect to some coordinate frame.  If you refer back to a reference coordinate frame located at the end of the robot's forearm (or the wrist center if the wrist is spherical), you will ensure that any errors caused by the kinematics of the arm joints are not part of your equations.  But you can pick which type of kinematic model you use and you should find the same accuracy for them all.

Edited based in your update:

The Rotation Matrix is unique.  See the answers here:  https://math.stackexchange.com/questions/105264/3d-rotation-matrix-uniqueness

Each term of this 3x3 can be represented many ways, as you are trying to do.  A silly example:   If one of your angles is $\theta_1$, you could instead write it as the sum of ten smaller angles.  But $R$ would be the same.   There is probably a way to represent each of the nine terms with a minimum (least-squares) set of terms.  But $R$ will be the same.

One more edit (sorry):

$R$ is orthonormal.  That puts six constraints on the nine terms ($||R|| = 1$, the magnitude of each row (and column) is also 1, and the rows and columns are mutually orthogonal).  So you have three degrees of freedom to play with.   There is your optimization problem.   Pick any representation, and as you find different ways to represent those nine terms, it may be challenging to relate that back to the physical system.   I believe you will end up at screw coordinates.   You may want to research canonical kinematics for ideas.   But I suspect you will find different robot geometries, and different motion goals, will result in a change for which standard notation results in the smallest angles.


答:
A GPS unit doesn't give you heading, although it is possible to estimate the heading based on successive GPS readings. Unless you're looking to set and forget the heading, which you shouldn't be doing anyways because of wind and other disturbances, you should be using some form of a controller to do course correction along the path.

Say you don't correct for declination and just opt to accept the magnetometer as truth. This will put you on a (slightly) wrong heading for the destination. You should be filtering IMU output with the GPS signal to get a corrected position estimate that reduces drift from the IMU.

So eventually your corrected position updates and "tells" your controller that you're off course, so the flight controller corrects and alters the heading slightly to point at the destination.

This happens continuously, meaning that you'll take a sub-optimal arc instead of a straight line from origin to destination, but unless you're going to correct for declination angle, which you seem to not want to do, then there's nothing else really you can do about it.

Title: Quadcopter heading calculation

Body:
I'm working on an autonomous quad copter, I have two GPS co-ordinates (source and destination co-ordinates). I need to move my quad from the source to the destination, for this I need to calculate the heading and set the yaw value of my quad. How can I calculate the heading and make sure the quad is headed in the right direction as the target co-ordinates?

If I use magnetometer the declination angle will vary from place to place and so I will have to keep changing the declination angle. If I'm calculating based on just the GPS co-ordinates, it's not accurate.

What is the best way to do this?  How do I calculate the above?


答:
It sounds to me like you want something where you can (exaggerating) express 30 degrees as thirty 1 degree transforms, such that you can then do something where $\sin{(1)} \approx 1$ and "cheat" that transform to use $\sin{(30)}\approx 30$.

You can't do this, because as BendingUnit22 put it, "Truth is invariant under change of notation."

It's similar to saying, "Well, a mile is 5280 feet, and a foot is 12 inches, and each inch is 25.4mm, and a millimeter is basically nothing, so a mile is a 1.6 million times nothing, so a mile is nothing." It doesn't work because there is error with any approximation, and that error propagates with every successive step.

Title: Most accurate rotation representation for small angles

Body:
Assume that I have a rigid body for which I know that it can rotate with respect to a global reference frame (which is considered fixed and already given) for only a few degrees of angle, so I can describe its rotation by using the small angle approximation. For this system, I would like to know if there is a rotation representation that offers more accuracy when compared with other representation methods.

The main representation methods that I considered are the euler angles and the pitch-yaw-roll transformation. To my perception, I think that pitch-yaw-roll representation is expected to be more accurate, since all the angles are expressed with respect to the initial coordinate frame. On the other hand, euler angles are defined on different frames, so I am not sure if the resulting angles will be really small.

To sum up, I know that the body can rotate for only a few degrees and I would like to know which coordinate representation is much probable to deliver the smallest angles, such that the small angle approximation is more valid.

It could also be the case that there is not a general answer (so it depends on the specific configuration) but still I haven't found anything about this topic on the related literature!

Edit: This question is not related to numerical issues. Therefore, it is assumed that all the possible rotation matrix descriptions (Euler, PYR etc) result in the same, exact coordinate vector. Therefore, the question is if there exists one parametrization that is composed of the smallest possible angles.

Example (no small angle approx used): Assume I have a coordinate frame which describes a point in space by the following vector

$P2=\begin{bmatrix} 4 \\ 1 \\ 0.05 \end{bmatrix}$.

Given another coordinate frame which is rotated with respect to the previous one, the description of the same point is given by

$P1=\begin{bmatrix} 3.8933 \\
    1.3566 \\
   -0.0630 \end{bmatrix}$.

Using Euler angles, I can find that the rotation matrix $R_{euler}$ is characterized by the angles $0.1,0.2,0.1$ rads, which correspond to the angle of rotation around z axis, the rotation around the resulting y axis and the rotation around the resulting z axis, respectively (these are basic stuff, it is explained in many books.). So I have that $P1=R_{euler} P2$.

Now I want to find the corresponding rotation matrix if I use the pitch-yaw-roll representation. Here I have to solve an optimization problem and the solution that I get (maximum error between P1 and the estimated P1 is $3 \times 10^{-8}$) delivers me the following angles

$\begin{bmatrix}  -0.0103   \\ 0.0257  \\  0.0902\end{bmatrix}$,

which correspond to the rotation around the x,y and z axis of the initial coordinate frame.


答:
There is one option (I'm tempted to write trick) that might help you. in a limited set of conditions.

uC / uP systems are exceedingly good at measuring time.

So, if you can either assume that the speed is constant or that you can compensate for changes, you could convert your measurements of angles into measurements of time (i.e. how long does it take to reach a certain angular position).

This would enable faster fixed point math and you could even build calibration tables for the motors.

But it really depends on the specific application if anything of what I described can bring any benefit.


问:
Quaternion Kalman Filter Algorithm
I have been stuck on this for weeks, I really hope that someone can help me with this,thank you in advance.
I am trying to write an IMU attitude estimation algorithm using quaternion kalman filter. So based on this research paper: https://hal.archives-ouvertes.fr/hal-00968663/document, I have developed the following pseudo code algorithm:

Predict Stage:

Qk+1/k = Ak * Qk;  where Ak contains the gyro measurement.

Pk+1/k = Ak * Pk *Ak.transpose() + Q; where Q is assumed to be zero.

After prediction, we can use this formula to get the supposed gravity measurement of accelerometer Yg in body frame :

Yg = R * G;  // R is the rotation matrix generated from quaternion Qk+1/k and G = (0,0,0,9.81).

This equation then translates to the following equation which allows me to get measurement model matrix H.

H * Qk+1/k = 0; //where H stores value related to (Yg-G).

Update Stage:

K = P * H * (H * P * H.transpose()+R)^(-1); //R should be adaptively adjusted but right now initialized as identity matrix

Qk+1/k+1 = (I-KH)Qk+1/k;

Qk+1/K+1 = (Qk+1/K+1)/|Qk+1/k+1|; //Normalize quaternion

Pk+1/K+1 = (I - KH)Pk+1/k;

The following is the main part of my code. The complete C++ code is at here https://github.com/lyf44/fcu if you want to test.

```<code>Matrix3f skew_symmetric_matrix(float a, float b, float c, float d){
    Matrix3f matrix;
    matrix << a,d*(-1),c,
              d,a,b*(-1),
              c*(-1),b,a;
    return (matrix);
}

void Akf::state_transition_matrix(float dt,float gx,float gy, float gz){
    Vector3f tmp;
    tmp(0) = gx*PI/180;
    tmp(1) = gy*PI/180;
    tmp(2) = gz*PI/180;
    float magnitude = sqrt(pow((float)tmp(0),2)+pow((float)tmp(1),2)+pow((float)tmp(2),2));

    /*q(k+1) = |  cos(|w|*dt/2)       | quaternion_multiply q(k)
               |  w/|w|*sin(|w|*dt/2) |
    */
    //w/|w|*sin(|w|*dt/2)
    tmp = tmp/magnitude*sin(magnitude*dt/2);
    //quaternion multiplication
    A(0,0) = cos(magnitude*dt/2);
    A.block<3,1>(1,0) = tmp;
    A.block<1,3>(0,1) = tmp.transpose()*(-1);

    Matrix3f skew_symmetric;
    skew_symmetric = skew_symmetric_matrix((float)A(0,0),(float)tmp(0),(float)tmp(1),(float)tmp(2));
    A.block<3,3>(1,1) = skew_symmetric;
}

void Akf::observation_model_matrix(Vector3f meas){
    Vector3f G;
    Vector3f tmp;
    G << 0,0,9.81;
    /* H = | 0        -(acc-G).transpose     |
     *     | (acc-G)  -(acc+G).skewsymmetric |
     */
    tmp = meas-G;
    H(0,0) = 0;
    H.block<3,1>(1,0) = tmp;
    H.block<1,3>(0,1) = tmp.transpose()*(-1);
    tmp = tmp+G+G;
    Matrix3f matrix;
    matrix = skew_symmetric_matrix(0,(float)tmp(0),(float)tmp(1),(float)tmp(2));
    H.block<3,3>(1,1) = matrix*(-1);
    //H = H*(0.5);
    cout<<"H"<<endl;
    cout<<H<<endl;
    cout<<"H*X"<<endl;
    std::cout<<H*X<<std::endl;
}

void Akf::setup(){
    X_prev = Vector4f::Zero(4,1);
    X_prev(0) = 1;
    Q = Matrix4f::Zero(4,4);
    Z = Vector4f::Zero(4,1);
    R = Matrix4f::Identity(4,4);
    P_prev = Matrix4f::Identity(4,4);
    P_prev = P_prev*(0.1);
    I = Matrix4f::Identity(4,4);

    sum = Vector4f::Zero(4,1);
    noise_sum = Matrix4f::Zero(4,4);
    counter=1;
}

void Akf::predict_state(){
    cout<<(60*counter%360)<<endl;
    X = A*X_prev;
    A_T = A.transpose();
    P = A*P_prev*A_T+Q;
}

void Akf::update_state(){
  Matrix4f PH_T;
  Matrix4f tmp;

  PH_T = P*H.transpose();
  S = H*PH_T+R;
  if (S.determinant()!= 0 )
  {
      tmp = S.inverse();
      K = P*H*tmp;
      //std::cout<<"K"<<std::endl;
      //std::cout<<K<<std::endl;
      X_updated = (I-K*H)*X;
      X_updated = X_updated /(X_updated.norm());
      P_updated = (I-K*H)*P;
  }
  else{
      X_updated = X;
      std::cout<< "error-tmp not inversible!"<<std::endl;
  }
  X_prev = X_updated;
  P_prev = P_updated;
}

void rotation_matrix(Vector4f q,Matrix3f &rot_matrix){
    int i;
    for (i=1;i<4;i++){
        q(i) = q(i)*(-1);
    }
    Matrix3f matrix;
    matrix(0,0) = pow((float)q(0),2)+pow((float)q(1),2)-pow((float)q(2),2)-pow((float)q(3),2);
    matrix(0,1) = 2*(q(1)*q(2)-q(0)*q(3));
    matrix(0,2) = 2*(q(0)*q(2)+q(1)*q(3));
    matrix(1,0) = 2*(q(1)*q(2)+q(0)*q(3));
    matrix(1,1) = pow((float)q(0),2)-pow((float)q(1),2)+pow((float)q(2),2)-pow((float)q(3),2);
    matrix(1,2) = 2*(q(2)*q(3)-q(0)*q(1));
    matrix(2,0) = 2*(q(1)*q(3)-q(0)*q(2));
    matrix(2,1) = 2*(q(0)*q(1)+q(2)*q(3));
    matrix(2,2) = pow((float)q(0),2)-pow((float)q(1),2)-pow((float)q(2),2)+pow((float)q(3),2);
    rot_matrix = matrix;
}

Vector3f generate_akf_random_measurement(Vector4f state){
    int i;
    //compute quaternion rotation matrix
    Matrix3f rot_matrix;
    rotation_matrix(state,rot_matrix);
    //rot_matrix*acceleration in NED = acceleration in body-fixed frame
    Vector3f true_value = rot_matrix*G;
    std::cout<<"true value"<<std::endl;
    std::cout<<true_value<<std::endl;
    for (i=0;i<3;i++){
        noisy_value(i) = true_value(i) + (-1) + (float)(rand()/(float)(RAND_MAX/2));
    }
    return (noisy_value);
}

int main(){
      float gx,gy,gz,dt;
      gx =60; gy=0; gz =0; //for testing, let it rotate around x axis by 60 degree
      myakf.state_transition_matrix(dt,gx,gy,gz); // dt is elapsed time
      myakf.predict_state();
      Vector4f state = myakf.get_predicted_state();
      Vector3f meas = generate_akf_random_measurement(state);
      myakf.observation_model_matrix(meas);
      myakf.measurement_noise();
      myakf.update_state();
      q = myakf.get_updated_state();
</code>```

The problem that I face is that my code does not work.The prediction stage works fine but the updated quaternion state is only correct for the first few iterations and it starts to drift away from the correct value. I have checked my code against the research paper multiple times and ensured that it is in accordance with the algorithm proposed by the research paper.

In my test, I am rotating around x axis by 60 degree per iterations. The number below the started is the angle of rotation. state and updated state is the predicted and updated quaternion respectivly while true value, meas, result are acceleration due to gravity in body frame.As the test result indicates, everything is way off after rotating 360 degrees.
The following is my test result:

```<code>1
started
60
state
0.866025
0.5
0
0
true value
0
8.49571
4.905
meas
0.314533
7.97407
4.98588
updated state
0.866076
0.499913
-2.36755e-005
1.56256e-005
result
0.000555564
8.49472
4.90671

1
started
120
state
0.500087
0.865975
-2.83164e-005
1.69446e-006
true value
0.000306622
8.4967
-4.90329
meas
-0.532868
8.79841
-4.80453
updated state
0.485378
0.862257
-0.129439
-0.064549
result
0.140652
8.37531
-5.10594

1
started
180
state
-0.0107786
0.989425
-0.0798226
-0.12062
true value
-2.35843
-0.0203349
-9.52226
meas
-1.39627
-0.889284
-8.74243
updated state
-0.0195091
0.981985
-0.151695
-0.110965
result
-2.19598
-0.0456112
-9.56095

1
started
240
state
-0.507888
0.840669
-0.0758893
-0.171946
true value
-3.59229
-8.12105
-4.16894
meas
-4.52356
-7.73113
-4.98735
updated state
-0.53758
0.811101
-0.212643
-0.0889171
result
-3.65783
-8.18397
-3.98485

1
started
300
state
-0.871108
0.433644
-0.139696
-0.183326
true value
-3.94732
-6.909
5.73763
meas
-4.36385
-6.98853
5.39759
updated state
-0.86404
0.436764
-0.102296
-0.228487
result
-3.69216
-6.94565
5.86192
1
started
0
state
-0.966663
-0.0537713
 0.0256525
 -0.249024
true value
0.749243
0.894488
9.74036
meas
-0.194541
0.318586
10.1868
updated state
-0.78986
-0.0594022
0.0311688
-0.609607
result
1.1935
0.547764
9.72171

1
started
60
state
-0.654338
-0.446374
0.331797
-0.512351
true value
8.74674
2.39526
3.74078
meas
9.36079
2.96653
3.57115
updated state
-0.52697
-0.512048
0.221843
-0.64101
result
8.73351
2.50411
3.70018
</code>```

Can someone help me confirm that my understanding about the theory of this quaternion kalman filter and my pseudo code is correct? Also, if anyone has implemented attitude estimation using maybe a different version of quaternion kalman filter, I would greatly appreciate if you can provide a pseudo code and a little explanation.
Thank you guys very much!

答:
I hope this is / hope it isn't your problem, as it could be a face-palm moment, but I don't think you're calculating your Kalman gain correctly.

I've only used a Kalman filter a couple times in the past, so I'm by no means an expert on it, but I believe that the Kalman gains are calculated:

$$
K = P H^T (HP H^T + R)^{-1}
$$

You have, in your pseudo-code and in your pasted code:

$$
K = P H (HP H^T + R)^{-1}
$$

I believe what you're missing is that $H^T$, or H.transpose() in your code. You define

```<code>PH_T = P*H.transpose();
</code>```

but H.transpose() is used a couple times, so I think you would be better off defining

```<code>H_T = H.transpose();
</code>```

and using that in the two locations you need $H^T$.

That said, I hope this is/isn't your problem - I know how painful it is to waste so much time on a typo. If this doesn't solve your problem, please post more information on how you're getting from the gyro output to the quaternion. I believe this is where you will find your issues.

Title: Quaternion Kalman Filter Algorithm

Body:
I hope this is / hope it isn't your problem, as it could be a face-palm moment, but I don't think you're calculating your Kalman gain correctly.

I've only used a Kalman filter a couple times in the past, so I'm by no means an expert on it, but I believe that the Kalman gains are calculated:

$$
K = P H^T (HP H^T + R)^{-1}
$$

You have, in your pseudo-code and in your pasted code:

$$
K = P H (HP H^T + R)^{-1}
$$

I believe what you're missing is that $H^T$, or H.transpose() in your code. You define

PH_T = P*H.transpose();

but H.transpose() is used a couple times, so I think you would be better off defining

H_T = H.transpose();

and using that in the two locations you need $H^T$.

That said, I hope this is/isn't your problem - I know how painful it is to waste so much time on a typo. If this doesn't solve your problem, please post more information on how you're getting from the gyro output to the quaternion. I believe this is where you will find your issues.


问:
Motor upgrade to higher torque?
I have assembled a 4WD car using kits I bought on ebay.

I have 4 motors similar to this one: .

The description says:

Operating voltage: 3V~12VDC
  (recommended operating voltage of about 6 to 8V)

Maximum torque: 800gf cm min (3V)

No-load speed: 1:48 (3V time)

The load current: 70mA (250mA MAX) (3V)

This motor with EMC, anti-interference ability.
  The microcontroller without interference.

Size: 7x2.2x1.8cm(approx)

I am not too fond of the max speed I can reach, but I would be able to provide more power, because I have a 12V 2A battery onboard.

So far I have used 6V, because that seemed to be the safer voltage choice.

Has anybody tried successfully higher voltages, without wearing down the motor in few hours (I've read this can happen)?

Alternatively, can someone recommend replacement motors that would tolerate reliably a higher power envelope?

I would like to preserve the gearbox and replace only the motor, if possible.

I think I could fit a motor 2-4 mm longer (replacing the transparent strap which bonds it to the gearbox), if that makes any difference.

BTW, I'm making the assumption:

higher_voltage => higher_torque => higher_speed

but I'm not sure it's overall correct.

I expect that it would at least produce higher acceleration during the transients.

答:
Few points,

Voltage is like a reservoir with water and it doesn't matter how big it is if your tap only allows a thin stream of water, your current in this case. For motors, it is the current that is important as long as the voltage is in the operating range. Current determines how fast your motor can spin.

Torque is always inversely proportional to speed. The higher torque u need the slower the motor will have to be, you can see the torque vs speed graphs in the datasheets.

Cannot really suggest you a motor just like that since you haven't provided any requirements like what is the target torque required? Since a motor can be practically useful only if its torque can be specified since it relates a lot of things like the motor's size, power rating etc.

Edit:

To point you in the right direction, Information about where the motor is being used is required - linear drive units, rc cars, propellers etc. For starters, you can look at these links - Link1, Link2, Link3, Link4, Link5

Title: Motor upgrade to higher torque?


问:
PID tuning with methods like GA and PSO
I have recently started reading about PID tuning methods and algorithms, and I encountered the particle swarm optimization algorithm and genetic algorithm.

The problem is, that I don't understand how each particle/chromosome determines his fitness. On real physical system, each particle/chromosome checks his fitness on the system? Wouldn't it take a really long time? I think that I am missing something here... Can those algorithms be implemented on an actual physical system? If so, then how?

答:
GA and PSO methods are generally and more simply executed on a model of the plant you want to tune your PID for, not on the physical system. This way, you can of course converge much faster toward the solution and also you don't apply potentially disruptive gains to your PID controller.

The very first step is thus to come up with a good model of your process through identification.

Title: PID tuning with methods like GA and PSO


问:
High voltage motor control with arduino
I'm trying to control a higher voltage motor than an arduino can source with a pin, with an arduino. I am trying to hook it up to a transistor. The battery pack is not supposed to be 4.8V, it's 6V, 4 D batteries.

Here is the setup:

Here is the arduino code I'm trying to run to it:

```<code>int motorpin = 2;

void setup()
{
    pinMode(motorpin, OUTPUT);
}

void loop()
{
    digitalWrite(motorpin, HIGH);
    delay(500);
    digitalWrite(motorpin, LOW);
    delay(500);
}
</code>```

Code gives me no errors, but no motor movement happens. What would make this work? Thanks.

答:
And for the 'I'd rather be designing the sensors and grippers' of us, consider a ready made motor shield for Arduino such as:

Polulu Zumo Shield (1.2 amp motors) or the Arduino Motor Shield (2 amp motors) or the Polulu Dual Motor Driver Shield (3.0 amp motors).

Title: High voltage motor control with arduino


答: Thanks to the comment by Biscuits, in the end I realized that I have been adding random number to the predicted state instead of true state. Therefore, I am essentially accumulating my gyro errors which leads to the wrong results. I hope that other people won't make the same mistakes again.</p>

Title: Quaternion Kalman Filter Algorithm

Body:
I have been stuck on this for weeks, I really hope that someone can help me with this,thank you in advance.
I am trying to write an IMU attitude estimation algorithm using quaternion kalman filter. So based on this research paper: https://hal.archives-ouvertes.fr/hal-00968663/document, I have developed the following pseudo code algorithm:

Predict Stage:

Qk+1/k = Ak * Qk;  where Ak contains the gyro measurement.

Pk+1/k = Ak * Pk *Ak.transpose() + Q; where Q is assumed to be zero.

After prediction, we can use this formula to get the supposed gravity measurement of accelerometer Yg in body frame :

Yg = R * G;  // R is the rotation matrix generated from quaternion Qk+1/k and G = (0,0,0,9.81).

This equation then translates to the following equation which allows me to get measurement model matrix H.

H * Qk+1/k = 0; //where H stores value related to (Yg-G).

Update Stage:

K = P * H * (H * P * H.transpose()+R)^(-1); //R should be adaptively adjusted but right now initialized as identity matrix

Qk+1/k+1 = (I-KH)Qk+1/k;

Qk+1/K+1 = (Qk+1/K+1)/|Qk+1/k+1|; //Normalize quaternion

Pk+1/K+1 = (I - KH)Pk+1/k;

The following is the main part of my code. The complete C++ code is at here https://github.com/lyf44/fcu if you want to test.

```<code>Matrix3f skew_symmetric_matrix(float a, float b, float c, float d){
    Matrix3f matrix;
    matrix << a,d*(-1),c,
              d,a,b*(-1),
              c*(-1),b,a;
    return (matrix);
}

void Akf::state_transition_matrix(float dt,float gx,float gy, float gz){
    Vector3f tmp;
    tmp(0) = gx*PI/180;
    tmp(1) = gy*PI/180;
    tmp(2) = gz*PI/180;
    float magnitude = sqrt(pow((float)tmp(0),2)+pow((float)tmp(1),2)+pow((float)tmp(2),2));

    /*q(k+1) = |  cos(|w|*dt/2)       | quaternion_multiply q(k)
               |  w/|w|*sin(|w|*dt/2) |
    */
    //w/|w|*sin(|w|*dt/2)
    tmp = tmp/magnitude*sin(magnitude*dt/2);
    //quaternion multiplication
    A(0,0) = cos(magnitude*dt/2);
    A.block<3,1>(1,0) = tmp;
    A.block<1,3>(0,1) = tmp.transpose()*(-1);

    Matrix3f skew_symmetric;
    skew_symmetric = skew_symmetric_matrix((float)A(0,0),(float)tmp(0),(float)tmp(1),(float)tmp(2));
    A.block<3,3>(1,1) = skew_symmetric;
}

void Akf::observation_model_matrix(Vector3f meas){
    Vector3f G;
    Vector3f tmp;
    G << 0,0,9.81;
    /* H = | 0        -(acc-G).transpose     |
     *     | (acc-G)  -(acc+G).skewsymmetric |
     */
    tmp = meas-G;
    H(0,0) = 0;
    H.block<3,1>(1,0) = tmp;
    H.block<1,3>(0,1) = tmp.transpose()*(-1);
    tmp = tmp+G+G;
    Matrix3f matrix;
    matrix = skew_symmetric_matrix(0,(float)tmp(0),(float)tmp(1),(float)tmp(2));
    H.block<3,3>(1,1) = matrix*(-1);
    //H = H*(0.5);
    cout<<"H"<<endl;
    cout<<H<<endl;
    cout<<"H*X"<<endl;
    std::cout<<H*X<<std::endl;
}

void Akf::setup(){
    X_prev = Vector4f::Zero(4,1);
    X_prev(0) = 1;
    Q = Matrix4f::Zero(4,4);
    Z = Vector4f::Zero(4,1);
    R = Matrix4f::Identity(4,4);
    P_prev = Matrix4f::Identity(4,4);
    P_prev = P_prev*(0.1);
    I = Matrix4f::Identity(4,4);

    sum = Vector4f::Zero(4,1);
    noise_sum = Matrix4f::Zero(4,4);
    counter=1;
}

void Akf::predict_state(){
    cout<<(60*counter%360)<<endl;
    X = A*X_prev;
    A_T = A.transpose();
    P = A*P_prev*A_T+Q;
}

void Akf::update_state(){
  Matrix4f PH_T;
  Matrix4f tmp;

  PH_T = P*H.transpose();
  S = H*PH_T+R;
  if (S.determinant()!= 0 )
  {
      tmp = S.inverse();
      K = P*H*tmp;
      //std::cout<<"K"<<std::endl;
      //std::cout<<K<<std::endl;
      X_updated = (I-K*H)*X;
      X_updated = X_updated /(X_updated.norm());
      P_updated = (I-K*H)*P;
  }
  else{
      X_updated = X;
      std::cout<< "error-tmp not inversible!"<<std::endl;
  }
  X_prev = X_updated;
  P_prev = P_updated;
}

void rotation_matrix(Vector4f q,Matrix3f &rot_matrix){
    int i;
    for (i=1;i<4;i++){
        q(i) = q(i)*(-1);
    }
    Matrix3f matrix;
    matrix(0,0) = pow((float)q(0),2)+pow((float)q(1),2)-pow((float)q(2),2)-pow((float)q(3),2);
    matrix(0,1) = 2*(q(1)*q(2)-q(0)*q(3));
    matrix(0,2) = 2*(q(0)*q(2)+q(1)*q(3));
    matrix(1,0) = 2*(q(1)*q(2)+q(0)*q(3));
    matrix(1,1) = pow((float)q(0),2)-pow((float)q(1),2)+pow((float)q(2),2)-pow((float)q(3),2);
    matrix(1,2) = 2*(q(2)*q(3)-q(0)*q(1));
    matrix(2,0) = 2*(q(1)*q(3)-q(0)*q(2));
    matrix(2,1) = 2*(q(0)*q(1)+q(2)*q(3));
    matrix(2,2) = pow((float)q(0),2)-pow((float)q(1),2)-pow((float)q(2),2)+pow((float)q(3),2);
    rot_matrix = matrix;
}

Vector3f generate_akf_random_measurement(Vector4f state){
    int i;
    //compute quaternion rotation matrix
    Matrix3f rot_matrix;
    rotation_matrix(state,rot_matrix);
    //rot_matrix*acceleration in NED = acceleration in body-fixed frame
    Vector3f true_value = rot_matrix*G;
    std::cout<<"true value"<<std::endl;
    std::cout<<true_value<<std::endl;
    for (i=0;i<3;i++){
        noisy_value(i) = true_value(i) + (-1) + (float)(rand()/(float)(RAND_MAX/2));
    }
    return (noisy_value);
}

int main(){
      float gx,gy,gz,dt;
      gx =60; gy=0; gz =0; //for testing, let it rotate around x axis by 60 degree
      myakf.state_transition_matrix(dt,gx,gy,gz); // dt is elapsed time
      myakf.predict_state();
      Vector4f state = myakf.get_predicted_state();
      Vector3f meas = generate_akf_random_measurement(state);
      myakf.observation_model_matrix(meas);
      myakf.measurement_noise();
      myakf.update_state();
      q = myakf.get_updated_state();
</code>```

The problem that I face is that my code does not work.The prediction stage works fine but the updated quaternion state is only correct for the first few iterations and it starts to drift away from the correct value. I have checked my code against the research paper multiple times and ensured that it is in accordance with the algorithm proposed by the research paper.

In my test, I am rotating around x axis by 60 degree per iterations. The number below the started is the angle of rotation. state and updated state is the predicted and updated quaternion respectivly while true value, meas, result are acceleration due to gravity in body frame.As the test result indicates, everything is way off after rotating 360 degrees.
The following is my test result:

```<code>1
started
60
state
0.866025
0.5
0
0
true value
0
8.49571
4.905
meas
0.314533
7.97407
4.98588
updated state
0.866076
0.499913
-2.36755e-005
1.56256e-005
result
0.000555564
8.49472
4.90671

1
started
120
state
0.500087
0.865975
-2.83164e-005
1.69446e-006
true value
0.000306622
8.4967
-4.90329
meas
-0.532868
8.79841
-4.80453
updated state
0.485378
0.862257
-0.129439
-0.064549
result
0.140652
8.37531
-5.10594

1
started
180
state
-0.0107786
0.989425
-0.0798226
-0.12062
true value
-2.35843
-0.0203349
-9.52226
meas
-1.39627
-0.889284
-8.74243
updated state
-0.0195091
0.981985
-0.151695
-0.110965
result
-2.19598
-0.0456112
-9.56095

1
started
240
state
-0.507888
0.840669
-0.0758893
-0.171946
true value
-3.59229
-8.12105
-4.16894
meas
-4.52356
-7.73113
-4.98735
updated state
-0.53758
0.811101
-0.212643
-0.0889171
result
-3.65783
-8.18397
-3.98485

1
started
300
state
-0.871108
0.433644
-0.139696
-0.183326
true value
-3.94732
-6.909
5.73763
meas
-4.36385
-6.98853
5.39759
updated state
-0.86404
0.436764
-0.102296
-0.228487
result
-3.69216
-6.94565
5.86192
1
started
0
state
-0.966663
-0.0537713
 0.0256525
 -0.249024
true value
0.749243
0.894488
9.74036
meas
-0.194541
0.318586
10.1868
updated state
-0.78986
-0.0594022
0.0311688
-0.609607
result
1.1935
0.547764
9.72171

1
started
60
state
-0.654338
-0.446374
0.331797
-0.512351
true value
8.74674
2.39526
3.74078
meas
9.36079
2.96653
3.57115
updated state
-0.52697
-0.512048
0.221843
-0.64101
result
8.73351
2.50411
3.70018
</code>```

Can someone help me confirm that my understanding about the theory of this quaternion kalman filter and my pseudo code is correct? Also, if anyone has implemented attitude estimation using maybe a different version of quaternion kalman filter, I would greatly appreciate if you can provide a pseudo code and a little explanation.
Thank you guys very much!


问:
Electric vs. internal combustion engine for propulsion
What are the main differences between electric motor and internal combustion engine for an ATV-sized mobile robot platform in terms of functionality, implementation difficulty ("RC" conversion, "electronic" operation), durability and maintenance when used as an autonomous platform? A full sized ATV/UTV like Polaris Ranger (EV) is in question.

Are the advantages/disadvantages basically the same as the differences between electric and nitro RC cars or does the bigger scale adds something important to the game? I can think of the main differences like bigger range and faster "refueling" with IC and less maintenance with electric but I am interested in a detailed comparison.

The transmission for the IC engine is considered to be automatic.

EDIT: The fuel injection for IC is considered to be electronic (EFI) but I do not know whether that also means the "electronic" throttle (no mechanical wire as with carburetor?). Whatever the throttle may be I see the lag between its "actuation" and the engine running into higher RPM and giving more power/speed as the main disadvantage for IC control - however, it may probably be quite easy dealt with in software (by adding some timeout when checking desired RPM).

答:
Q: What are the main differences between electric motor and internal combustion engine for an ATV-sized mobile robot platform in terms of functionality, implementation difficulty ("RC" conversion, "electronic" operation), durability and maintenance when used as an autonomous platform? A full sized ATV/UTV like Polaris Ranger (EV) is in question.

A: Depending on the electric motor(s) chosen, you can have as much power as the IC yould provide you. You will also have the same amount of torque, no matter the rpm's. RC conversions and any other electronic operatios regarding the motor(s) will become significantly easier, since they can all be connected together. On an IC you have to create mechanisms for the throttle response, since it works mechanically (with wire for the polaris i think). Maintainance requires less efford and time in electronic motor(s), but it can become way more expensive and time-consuming if something breaks down and needs to be replaced.

Q: Are the advantages/disadvantages basically the same as the differences between electric and nitro RC cars or does the bigger scale adds something important to the game?

A: The basic differences are the weigh of the vehicle, the range they have to cover and their energy consumption. Using electronic motor(s) means batteries and/or other forms of power (solar power through panels). If thinking of covering long distances, you will need big and hence heavy batteries. You will also need space to fit all those. Tough one for a polaris, especially if any driver seats will be kept on. IC is factory bolt-on and uses gasoline, which allows covering greater distances.

Personal opinion: I would go for the IC and make the small modifications needed.


问:
Angle to a circle tangent line
I want to simulate the detection of a moving object by a unicycle type robot. The robot is modelled with position (x,y) and direction theta as the three states. The obstacle is represented as a circle of radius r1 (r_1 in my code). I want to find the angles alpha_1 and alpha_2from the robot's local coordinate frame to the circle, as shown here:

So what I am doing is trying to find the angle from the robot to the line joining the robot and the circle's centre (this angle is called aux_t in my code), then find the angle between the tangent and the same line (called phi_c). Finally I would find the angles I want by adding and subtracting phi_c from aux_t. The diagram I am thinking of is shown:

The problem is that I am getting trouble with my code when I try to find the alpha angles: It starts calculating the angles correctly (though in negative values, not sure if this is causing my trouble) but as both the car and the circle get closer, phi_c becomes larger than aux_t and one of the alphas suddenly change its sign. For example I am getting this:

$$\begin{array}{c c c c}
\text{aux_t} & \text{phi_c} & \text{alpha_1} & \text{alpha_2} \\ \hline
\text{-0.81} & \text{+0.52} & \text{-1.33} & \text{-0.29} \\
\text{-0.74} & \text{+0.61} & \text{-1.35} & \text{-0.12} \\
\text{-0.69} & \text{+0.67} & \text{-1.37} & \text{-0.02} \\
\text{-0.64} & \text{+0.74} & \text{-1.38} & \text{+0.1} \\
\end{array}$$

So basically, the alpha_2 gets wrong form here. I know I am doing something wrong but I'm not sure what, I don't know how to limit the angles from 0 to pi. Is there a better way to find the alpha angles?

答:
First, determine the angle $\phi$ between the robot $<\!a_{x},a_{y},\theta\!>$ and the target $<\!p_{x},p_{y}\!>$ as follows

$$
\phi = \tan^{-1} \left( \frac{ p_{y} - a_{y} }{ p_{x} - a_{x} } \right) - \theta
$$

See the below picture,

Based on $\phi$, you can determine the rest.

Title: Angle to a circle tangent line


问:
inverse kinematics for 6 jointed robots
I am a  uncertain about how to compute the right homogeneous transformation matrix to compute an inverse kinematic Q-configuration.

Looking at robot like this

Where at the end of this robot I have a camera mounted on to it.

The purpose of my application is to make the robot follow an object, so basically tracking it.  The camera provide me with an X,Y,Z coordinate, which the position i want place my robot arm.

First question - How do i set up the desired homogenous transformation matrix?

The way i see it, I have 2 transformation matrices being T_tool_base and T_world_tool which become T_world_base = (T_tool_base) (T_world_tool)

My question is that how do i compute my desired transformation matrix.
I think i know how i should setup the transformation matrix for the camera which would be like this

T_world_tool = 0 0 0 x
               0 0 0 y
               0 0 0 z
               0 0 0 1

(Second question is regarding the rotation matrix, how do prescribe such that rotation in arbitrary as long the endpoint has the desired position in the world frame?)

but what should t_tool_base entail? should it entail the transformation of its current state or the desired transformation, and if so how do i extract the desired t_tool_base transformation?...

答:
I would recommend changing the naming convention since it is a bit misleading. In robotics the world Coordinate system (CS) is usually your fixed, absolute coordinate system. Lets call the transformation matrix from your camera to your object $T_{Object,Tool}$ If it cannot include any rotation, then you are right is should have the form as you specified. You are also right with your multiplications, but I would advise keeping the two thing separate. I actually do not think that you need the $T_{Object,Tool}$ matrix

The $T_{Tool,Base}$ is a transformation matrix form your tool to your base. This is what you can use to solve the inverse kinematics problem.

Define your coordinate systems (with the Denavit Hartenberg convention)

Build your transformation matrix as
$T_{Tool,Base} = A1(q1) \cdot A2(q2) \cdot A3(q3)\cdot A4(q4)\cdot A5(q5)\cdot A6(q6)$

Build the Cartesian transformation Matrix as
$T_{TCP} = X_{trans}\cdot Y_{trans}\cdot Z_{trans}\cdot X_{rot}\cdot Y_{rot}\cdot Z_{rot}$

Solve the inverse kinematics problem from

$T_{TCP} = T_{Tool,Base}$

as presented in this description. After this step you should have a function that for a given Cartesian coordinates of the TCP as input $X = (x, y, z, a, b, c)$ calculates the corresponding joint position as output $Q = (q_1, q_2, q_3, q_4, q_5, q_6)$

Your camera will measure the relative position of the object. Lets define the vector of measurments as $X_{Object} = (x, y, z, 0, 0, 0)$. The current Cartesian position of the robot is known (after solving the IK problem). Lets call this $X_{TCP} = (x_r, y_r, z_r, a_r, b_r, c_r)$. If you only want to track positions then $X_{new} = X_{Object} + X_{TCP}$ will give you the coordinates you want to position your TCP. Use this as input for the inverse kinematic function and get the $Q$ angles which correspond to this position. If you want to orient the TCP toward the object calculate also the new realative angles geometrically then add those also (instead of the $0, 0, 0$). That is it!

Title: inverse kinematics for 6 jointed robots


问:
Extracting as many possible end configurations as possible
I am trying to implement a path planner to generate a path that moves the robot from q_start to q_goal.

Q_goal is extracted from a stereo camera mounted on the tool, from
which I extract x,y,z coordinates of the desired position, the rotation can be arbitrary.

The robot I am using is an industrial ur5 robot arm, the software I use is capable of performing Jacobian based inverse kinematics given a transformation matrix with rotation and translation.

my inverse kinematics provide me with only one solution, which is ok, but doesn't provide me flexibility for path planning...

How do I using inverse kinematics determine all possible q-configurations that fulfills my criteria of having the desired x,y,z coordinates?

答:
Your software library uses a gradient descent method for inverse kinematics.  The way this works is that it moves the Cartesian position of the end-effector towards the goal in small steps then gets the joint angles for each step with the relation:
$$
\Delta \theta= J^{-1}\Delta x
$$
This method has a number of nice properties, but as you found you only get one solution.

An analytical inverse kinematics solution is what you want.  In this method, you algebraically determine all possible solutions.  The only catch is if you are under-constrained, (like in your case), where you have more joints than degrees of freedom.  (You have 6 joints, but only want a 3 DOF position).  In this case, you pick some joints to "fix" and do the math as if that joint is rigid.  But in reality, you typically discretize that joint's range, and get a set of solutions for every fixed position.  (You can see how the curse of dimenionality will get you here.)

I would recommend investigating ikfast from OpenRave, which is an analytical IK solver.  This has a number of different IK types for different DOFs.  "Translation3D" is what you want.

But the way to trick your gradient decent IK solver into giving you different solutions is to give it different initial conditions.  You should start from drastically different arm configurations.  (Like joints flipped 180 degrees from each other).

As for the not needing rotation:  The gradient descent IK method can ignore rotation quite easily.  You would simply only use the top 3 rows of the Jacobian, and give it only the end-effector position (not rotation too).


问:
Robotic Arm analysis in Matlab/simulink
I am going through a paper, Kinematic Modelling and Simulation of a 2-R Robot Using SolidWorks and Verification by MATLAB/Simulink, which is about a 2-link revolute joint robotic arm. According to the paper, the trajectory analysis of the robot was done via simulations in MATLAB/Simulink.

It shows the following picture, Trajectory generation of 2‐R robot with MATLAB/Simulink:

and then, Simulink - Simulation block to calculate the trajectory:

I think this is done in SimMechanics, but I am not sure. Experienced users, can you please tell me what am I looking at and how can I reproduce this?

Title: Robotic Arm analysis in Matlab/simulink

Body:
The first step for reproducing the results is to utilize the equations. They didn't mention which controller they used but in my simulation I chose the PD controller, therefore,

$$
\begin{align*}
\ddot{\theta}(t)   &= M^{-1}( u - V - G) \\
              u(t)    &= M ( \ddot{\theta}_{d} + k_{d} \dot{e} + k_{p} e) + V + G \\
           M &=
              \begin{bmatrix}
              m_{11} & m_{12} \\
              m_{21} & m_{22}
\end{bmatrix} \\
V &=
              \begin{bmatrix}
              v_{11}  \\
              v_{21}
\end{bmatrix} \\
G &=
              \begin{bmatrix}
              g_{11}  \\
              g_{21}
\end{bmatrix} \\
              e(t)    &= \theta - \theta_{d} \\
          \dot{e}(t)  &= \dot{\theta} - \dot{\theta}_{d} \\
      \theta_{1d}(t)  & = 10t^{4} + 10t^{3} + 10t^{2} \\
\dot{\theta}_{1d}(t)  & = 40t^{3} + 30t^{2} + 20t \\
\ddot{\theta}_{1d}(t) & = 120t^{3} + 60t^{2} + 20t \\
             \theta_{2d}(t) &= 50t^{4} + 10t^{3}  \\
         \dot{\theta}_{2d}(t) &= 200t^{3} + 30t^{2}  \\
         \ddot{\theta}_{2d}(t) &= 600t^{2} + 60t  \\
\end{align*}
$$

where $k_{d} = 20, k_{p} = 100, m_{1} = m_{2} = 16.92, l_{1} = l_{2} = 1$. The results I got for the elbow down case via C++ code are

t     theta1  theta2 px        py
-----------------------------------------
0     0       0      2         0
0.1   0.111   0.015  2         0.00349066
0.2   0.496   0.160  1.99992   0.0177943
0.3   1.251   0.675  1.9993    0.0518948
0.4   2.496   1.920  1.99642   0.115588
0.5   4.375   4.375  1.98633   0.221951
0.6   7.056   8.640  1.95706   0.385653
0.7  10.731  15.435  1.88376   0.618508
0.8  15.616  25.6    1.7218    0.919393
0.9  21.951  40.095  1.40621   1.25027
1.0  30.0    60      0.878878  1.49793

They are very close to the results stated in the paper. Visualizing the movement of the manipulator is not big deal. As a matter of fact, people care much about the results not the visualization. This is can be done by OpenGL as well. No one cares about the software you are using.


问:
How do we write a STOP to a continuous Servo?
I'm using processing to send strings to Arduino, using functions like

```<code>else {
    int u=90;
    port.write(u+"z");
}
</code>```

on the processing side and in the Arduino side I'm using calls like

```<code>  case 'z':
    z.write(v);
    v = 0;
    break;
  case 'L':
    z.write(0);
    //v = 0;
    break;
}
</code>```

yet I can't get the servo to stop at all. How do I make it shut off?

If it was a regular servo I wouldn't even ask because that's easy but I write 0 or 90 or LOW and nothing, it just keeps spinning in one direction but when it meets one of the conditions in my statements it switches polarity/direction and that's good - I want that but I made this function to make it stop and it is not doing so, does anyone have any ideas ?

I am using a Parallax Continuous Rotation Servo.

答:
There should be some intermediate value of degrees or microseconds that minimizes rotation speed.  You can run calibration tests to find the best value, for example by sweeping the angle or time through a range of values, while writing the current value to LCD or Serial; or by setting an initial value then using two buttons or an analog control voltage to tell the sketch to raise or lower the value, jogging up or down until the servo stops or runs at very slow speed.

Note, you will get finer control (hence, closer to the proper value) using  Servo.writeMicroseconds() rather than Servo.write().  In some Servo library versions, Servo.writeMicroseconds() resolution is half-microsecond, vs about 10 μs/degree Servo.write() resolution.

Also note, in some Servo library versions, Servo.write() accepts microsecond values as well as degree values, distinguishing the former from the latter by number size; ie, numbers less than 544 are clipped to the range 0 to 180 and treated as degrees, while numbers in the range 544 to 2400 are treated as microsecond values. [See example code in related answer at arduino.stackexchange.]


问:
Regarding Kalman filter and estimating heading with magnetic compass
I have trouble estimating the heading when close to the "pivot" point of the compass, and could use some input on how to solve it. I have set up my angles to be from 0-360 degrees during the testing but will be using radians (-pi, pi) from now on.

The setup is a differential robot with IMU, wheel encoders and a magnetic compass.

A complementary filter is used for fusing gyroZ and odo measurements to estimate the heading, and then correct it with a Kalman filter using the magnetic compass.

My problem occurs when the robot heading is close to -pi/pi .

The estimated heading is useless even though the robot is not even moving.

I am thinking this must be a very common problem and probably has a better solution than what I came up with, which was either re-initializing the integrator when crossing zero, adding 180 degrees each time the error is larger, or just ignoring the compass if the error is too large...

It's my first Kalman filter so I may have made a poor implementation if this is not a common issue...

Edit: trudesagen's solution solved my problem.

答:
I encountered this problem myself when making an extended Kalman filter for a quadrotor.

You have to check if the estimate goes above or below +/- pi, and then correct it if it does.

You can do this with a simple if statement:

```<code>if (angle > pi)
    angle = angle - 2*pi;
else if (angle < -pi)
    angle = angle + 2*pi;
</code>```

You could also throw in a while loop over that, if the value ever becomes larger than 2*pi for some reason, due to spikes etc. This looks like so:

```<code>while (abs(angle) > pi) {
    if (angle > pi)
        angle = angle - 2*pi;
    else if (angle< -pi)
        angle = angle + 2*pi;
}
</code>```

Remember to have this check after your prediction, as well as on the difference between your measured compass angle and your prediction, that you use to calculate the corrected estimate.

After this you also check the corrected estimate for the same, which should fix your problem.

Body:
I encountered this problem myself when making an extended Kalman filter for a quadrotor.

You have to check if the estimate goes above or below +/- pi, and then correct it if it does.

You can do this with a simple if statement:

if (angle > pi)
    angle = angle - 2*pi;
else if (angle < -pi)
    angle = angle + 2*pi;

You could also throw in a while loop over that, if the value ever becomes larger than 2*pi for some reason, due to spikes etc. This looks like so:

while (abs(angle) > pi) {
    if (angle > pi)
        angle = angle - 2*pi;
    else if (angle< -pi)
        angle = angle + 2*pi;
}

Remember to have this check after your prediction, as well as on the difference between your measured compass angle and your prediction, that you use to calculate the corrected estimate.

After this you also check the corrected estimate for the same, which should fix your problem.


问:
Bayesian filter for 2-D grid localizaton
I have some data obtained from an experiment in terms of movements and observations with odometry and sensor data. My task is to find the probability mass on each of the grid cells after each set of motion and observation. I'm a bit lost in figuring out how to compute probability mass for each of the grid cell.
My odometry information is in terms of rotation, translation and rotation and my sensor information is in terms of range and bearing angle.
How do I calculate the probability of robot present in each of the grid cell?

I have the formula for belief after motion as summation(P(x|u, x')xBel(x'))
How do I compute the motion model with noise?

答:
I can give you some leads and you can probably take it from there. Since you mention calculating "the probability of the robot present in each of the grid cell", what you want to do is essentially a histogram filter, i.e a discrete bayes filter applied to a continuous state space.
If you have the Probabilistic Robotics book by Thrun, you will find the algorithm you are looking for in chapter 8 as "Grid Localization":

which is a variant of Markov localization:

The Grid Localization algorithm precisely calculates at each time step the probability of each cell to correspond to the actual robot's state. If you only care about estimating the position (x,y), your grid is in 2D. If you also want to estimate the orientation (x,y,theta), your grid is in 3D, and each plane of the grid corresponds to an orientation:

As for the motion/measurement models, you can find them in chapter 5 and 6 of the same book.
If you don't have the book, you can find some excerpts here for motion models and here for sensor models

As for your second question, P(x|u,x') in your example should be your motion model, so any model of odometry noise should be included inside, but the exact form will depend on the motion model that you choose, cf the links above.


问:
What's the difference between a holonomic and a nonholonomic system?
I was wondering if a 1D point mass (a mass which can only move on a line, accelerated by an external time-varying force, see Wikipedia - Double integrator) is a holonomic or a nonholonomic system? Why?

I think that it is nonholonomic since it cannot move in any direction in its configuration space (which is 1D, just the $x$ axis). E.g. if the point mass is moving at $$x=10$$ with a velocity of 100 m/s in positive $x$-direction it cannot immediately go to $$x=9.9$$ due to its inertia. However, I have the feeling that my thoughts are wrong...

The background is the following:

I am trying to understand what holonomic and nonholonomic systems are. What I found so far:

Mathematically:

Holonomic system are systems for which all constraints are integrable into positional constraints.

Nonholonomic systems are systems which have constraints that are nonintegrable into positional constraints.

Intuitively:

Holonomic system where a robot can move in any direction in the configuration space.

Nonholonomic systems are systems where the velocities (magnitude and or direction) and other derivatives of the position are constraint.

答:
For a nonholonomic system, you can at best determine a differential relationship between state and inputs.   You cannot determine a closed-form geometric relationship.  This means that the history of states is needed in order to determine the current state.   Vehicles are a good example because you can intuitively see that turning the right wheel 100 rotations and turning the left wheel 100 rotations does not provide enough information to describe the change in position.   If the wheels are turned synchronously, the vehicle will follow a straight line.   If they are coordinated in another sequence, the robot will turn, and follow some other path.   This is nonholonomic:  knowing the total change in state variables is insufficient to describe the motion, because you cannot write a geometric relationship between input and output.  It is differential at best.  </p>

<p>The system you describe seems holonomic to me.   If the total motion of your point mass is 1 meter forward, doesn't that hold regardless of the history of motions that resulted in the net 1m path?   I did not dive into the paper to look at the equations yet so I could be wrong.  But intuitively, I think there would be a closed-form, non-differential, equation for that mass' motion profile.  </p>

Body:
For a nonholonomic system, you can at best determine a differential relationship between state and inputs.   You cannot determine a closed-form geometric relationship.  This means that the history of states is needed in order to determine the current state.   Vehicles are a good example because you can intuitively see that turning the right wheel 100 rotations and turning the left wheel 100 rotations does not provide enough information to describe the change in position.   If the wheels are turned synchronously, the vehicle will follow a straight line.   If they are coordinated in another sequence, the robot will turn, and follow some other path.   This is nonholonomic:  knowing the total change in state variables is insufficient to describe the motion, because you cannot write a geometric relationship between input and output.  It is differential at best.

The system you describe seems holonomic to me.   If the total motion of your point mass is 1 meter forward, doesn't that hold regardless of the history of motions that resulted in the net 1m path?   I did not dive into the paper to look at the equations yet so I could be wrong.  But intuitively, I think there would be a closed-form, non-differential, equation for that mass' motion profile.