问:
What is the best way to compute the probabilistic belief of a robot equipped with a vision sensor?
I am trying to implement 'belief space' planning for a robot that has a camera as its main sensor. Similar to SLAM, the robot has a map of 3D points, and it localizes by performing 2D-3D matching with the environment at every step. For the purpose of this question, I am assuming the map does not change.

As part of belief space planning, I want to plan paths for the robot that take it from start to goal, but in a way that its localization accuracy is always maximized. Hence, I would have to sample possible states of the robot, without actually moving there, and the observations the robot would make if it were at those states, which together (correct me if I am wrong) form the 'belief' of the robot, subsequently encoding its localization uncertainty at those points. And then my planner would try to connect the nodes which give me the least uncertainty (covariance).

As my localization uncertainty for this camera-based robot depends entirely on things like how many feature points are visible from a given locations, the heading angle of the robot etc.: I need an estimate of how 'bad' my localization at a certain sample would be, to determine if I should discard it. To get there, how do I define the measurement model for this, would it be the camera's measurement model or would it be something relating to the position of the robot? How do I 'guess' my measurements beforehand, and how do I compute the covariance of the robot through those guessed measurements?

EDIT: The main reference for me is the idea of Rapidly exploring Random Belief Trees, which is an extension of the method Belief Road Maps. Another relevant paper uses RRBTs for constrained planning. In this paper, states are sampled similar to conventional RRTs, represented as vertices as a graph, but when the vertices are to be connected, the algorithm propagates the belief from the current vertex to the new, (PROPAGATE function in section V of 1), and here is where I am stuck: I don't fully understand how I can propagate the belief along an edge without actually traversing it and obtaining new measurements, thereby new covariances from the localization. The RRBT paper says "the covariance prediction and cost expectation equations are implemented in the PROPAGATE function": but if only the prediction is used, how does it know, say, whether there are enough features at the future position that could enhance/degrade the localization accuracy?

答:
Use bearings-only localization to model camera informativeness, and simulate measurements with zero noise (e.g., no innovation).

For a variety of reasons, this is actually theoretically sound way of estimating the informativeness of a path.

There are many "measurement free" informative-ness metrics, like the Fisher Information Matrix. All you need are the positions of the robot and the positions of the landmarks in the map to determine how much information about the robot's position would be obtained by measuring the landmark locations. (Or visa-versa, the innovation from measurements is applied to both target and robot (it's SLAM right?), so the same metric works for both).

I'd start with a bearing sensor, as that's a good, well accepted model of a vision sensor. Figure out the "noise" on the bearing measurements by assuming a few pixels of error in locating features in the world.  Let the state of the system be the position of the robot plus its uncertainty, and then sample paths (as you suggest). From each position in the sampled path, I'd recompute the predicted uncertainty using the FIM. This is not hard to do, just assume no error in the measurements (i.e., there will be no "innovation" about the robot's belief, but you will still experience a drop in uncertainty represented by shrinking covariance in the robot's position estimate. I would not update the locations or uncertainties of the landmarks, just to simplify the problem.

This is a fairly well understood approach from what I recall in my last review of this literature, but don't take my word for it (review yourself!). At least this should form a baseline approach that is easy to simulate. Let's use the power of literature. You might peruse this thesis for the set up and equations.

Summarizing

Form the state vector $x$ which is the position (and orientation if you care) of the robot, and uncertainty in those position elements $\Sigma$.

Use an RRT, discrete graph planner (dijkstra / A*), or grid search (A* probably), but at every "step" of the sampled trajectory, recompute $\Sigma_i$ using the standard EKF update equations.

Let the "cost" of the trajectory be some convex combination of progress towards goal and the inverse of the covariance (e.g., the information matrix)

Some subtleties

Use the smallest state vector that makes sense. If you can assume the robot can point the camera independently of motion, or has multiple cameras, then ignore orientation and just track position. I'll proceed in 2D positions only.

You will have to derive the linearized system, but can borrow it from the thesis above. make sure to not bother with simulating measurements (e.g., if you only do EKF updates with "simulated measurements", then assume the measurements are true and without noise.

Critically, this simplifies the planning problem as your state doesn't change except for the uncertainty and planned motion. Check the equations here for EKF, and note that the only two equations that are relevant to calculate the new covariance are
$$P_{i|i-1}=F^T_iP_{i-1|i-1}F_i+Q$$
and $$P=P-PH^T  (H P H^T + R)^{-1}  HP$$ ... (I expanded the kalman gain into the last equation).

If we apply Woodbury matrix identity

we simplify that second one to $$P^{-1}=P^{-1}+H^TR^{-1}H$$.

We're done. You have a nice informativeness measure over the $n$ steps of the trajectory as,

$$I=\sum_{i=1}^n H_i^TR^{-1}H_i$$

where $R$ is the measurement noise (e.g., the bearing error of measurement to each landmark), for each time step in your trajectory. Since we are looking at information (inverse of covariance) we want to maximize the trace, determinant, or something else of $\sum_{i=1}^n H_i^TR^{-1}H_i$

So, what's $H$? Well it's the Jacobian of the measurement function with respect to the robot's location. That is, $H$ is of size $nx2$, since you have $n$ landmarks ($n$ measurement functions), and $2$ state elements of the robot: x,y. $R$ is of size $n\times n$, where I would set it to $\sigma I_{n\times n}$ for some pretend value of $\sigma$,(the actual uncertainty doesn't matter, it's all the same constant.

What's the measurement equation? It's

$$\tan^{-1}\frac{y_t-y_r}{x_t-x_r}$$

for target $t$ and robot position $r$.

Unwinding the recursion. I'd proceed as follows:

Write a path search algorithm that finds its way without considering uncertainty.

Derive $H$, or look it up in any bearings localization paper.

To determine the "goodness" of a path for use in the search algorithm, add in $trace(H^TRH)$ for each pose along the path.

Notice that the result matches the FIM of the trajectory (exercise left to the reader), and you have correctly and in a theoretically sound way determined the most informative trajectory.


问:
Forward Kinematics with matrices

From the image, I would like to find out how joint 1, 2 and 3 can be computed by using forward kinematics? (Assuming a theta value was given)

I understand that to get point 3 to origin (P3 -> o) you can do the following matrix multiplications:

```<code>[P3->o] = [P2->P1][P1->o]
</code>```

Where each P matrix is:

```<code>P = |Scale*Rotation  0|
    |Translation     1|
</code>```

If the first Theta was 50 degrees and the second theta was 100 degrees, what would the [P2->P1] and [P1->o] matrices be?

答:
While the other answers have already provided the analytical solution and numerical approach thereafter, I would mention that you need to have coordinates for point 1 with respect to the origin. </p>

<p>Theta 1 won't suffice to know the transformation to the origin. Always remember that a joint angle only affects the position/orientation of the child links and thereafter. You need to know the relation between the origin and point 1 to obtain transforms with respect to the former.  </p>

ContentLicense: CC BY-SA 4.0

Title: Forward Kinematics with matrices

Body:
While the other answers have already provided the analytical solution and numerical approach thereafter, I would mention that you need to have coordinates for point 1 with respect to the origin.

Theta 1 won't suffice to know the transformation to the origin. Always remember that a joint angle only affects the position/orientation of the child links and thereafter. You need to know the relation between the origin and point 1 to obtain transforms with respect to the former.


问:
Maximum value of norm of mass matrix M
How do I compute the maximum values for ||M(q)|| and ||M_dot(q)||, the induced 2-norms, where M(q) is the mass matrix from the standard dynamical model of a robotic manipulator. I have the numerical values but not the analytical symbolic representation. </p>

答:
I think this is an eigenvalue problem because:

The mass matrix can be assumed to be PSD

The induced norm of a PSD matrix is see here $\sqrt{\lambda_{max}(A^TA)}$

So for anything but the smallest matrices, you should use iterative methods to determine the norm (i.e., there is no closed-form solution).

Title: Maximum value of norm of mass matrix M


问:
Making a robot go in a straight rate - sampling rate for motor PID
I'm trying to build a robot/rover based on Raspberry Pi 3
At the moment I'm straggling with a basic driving in a straight line.

I'm in a process of tuning my PID code andI'm not sure what sampling rate should I choose.

These are the specs of the motors:

Voltage: DC 6V

  Speed: 210 rpm

  Encoder motor end: 11 signals

  Rated voltage: DC 6V

  No-load speed: 210RPM 0.13A

  Max efficiency: 2.0kg.cm/170rpm/2.0W/0.60A

  Max power: 5.2kg.cm/110rpm/3.1W/1.10A

  Stall torque: 10kg.cm 3.2A

  Retarder reduction ratio: 1 : 34

  Hall resolution: hall x ratio 34.02 = 341.2PPR

When the robot moves at the desired speed I get 20 encoder signals if I take a sample every 100ms and I get 10 if I sample every 50ms.

My question is what is the reasonable frequency to collect the encoder counts?

At 50ms I get higher frequency, so my PID adjustments can occur more frequently, but I get only 10 signals, so the resolution seems to be a bit low.

At 100ms I get 20 ticks which should provide more consistent stats, but then maybe adjustments are not happening fast enough.

Is there any rule of thumb for choosing a sampling rate for PID when driving a robot rover?

答:
There are probably some very strong theoretical results about the sampling frequency required to control a system. In the absence of a better answer, I have:

When considering sample rate from a scientific or mathematical perspective, the guidelines are a bit more involved and beyond the scope of this blog post. Still, it’s generally accepted best-practice for the sample rate to be based on frequency or time-based components of the process data. Basic formulas propose the use of data that is either 5-10 times faster than the Process Time Constant or 3-5 times faster than the Process Dead-Time.

There's a couple of links in the quote to define process time constant and process dead time.

In general, faster is probably better. If you state significantly changes on the order of milliseconds, then you should provide control updates faster than that. In my experience (ground and maritime robotics), 10Hz has usually been sufficient.


问:
Subscriber to array type overwriting data from different publishers!
I have been witnessing some bizarre behaviour with my very simple node.

I have a custom message that contains uint8[] type (it's 8 elements long). I have a subscriber that subscribes to a topic of the custom message and stores it in a global variable:

```<code>uint16* leddar_distance_cm_1 = NULL;

void leddarCallback(const perception_msgs::Leddar8::ConstPtr& msg) {
  if (msg->leddar_id == 1) {
      leddar_distance_cm_1 = const_cast<uint16_t*>(&(msg->distance[0]));
  }

  for (uint8_t i = 0; i < 8; i++) {
      ROS_INFO("VALUE: [%d]", leddar_distance_cm_1[i]);
  }
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "collision_avoidance_node");
  ros::NodeHandle n;

  ros::Subscriber leddar_sub = n.subscribe("perception/leddars", 1000, leddarCallback);

  ros::spin();

  return 0;
}
</code>```

I then have 2 publishers publishing onto that topic using rostopic pub. When I try to print the stored array from the callback I get this (bottom right window):

As you can see the two publishers seem to be interfering with each other and somehow overwritting leddar_distance_cm_1   pointer repeatedly. This doesn't happen with any non-pointer types from the message such as char or int.

I even tried creating 2 different subscribers for each publisher, same issue! As if both publishers are overwriting the same memory address (which they in fact are). Does this have anything to do with queues? I have a feeling that I am not reading the array correctly. I have spent over 8 hours looking at this and I can't figure out what the problem is, if there is an obvious mistake could someone please tell me what it is?

答:
The problem was that I was pointing a pointer to a memory address that gets free'd after the callback is executed.

Instead of using C style arrays I moved onto vectors, where leddar_distance_cm1 is std::vector<uint16_t leddar_distance_cm1(array_size, starting_value).

When using callbacks always copy rather use references or pointers, unless you know your object's lifecycle.

If you still want to use C array, use std::copy or memcpy.


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

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

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

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

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

答:
There are lots of very reliable connectors out there. Here are a few of my favorites:

Harwin Datamate These come in a few varieties. The ones shown here are fantastic and have both high current power connections, and many signal connections. They are extremely reliable, and I know that they are used on several robots I know of, including the Shadow Robot Hand, Robonaut and the latest DLR space qualified hand.

The ones you see here are a little on the expensive side, up to $30 each! But the simpler connectors with only signal pins are much more reasonable, just a couple of dollars each. They are extremely reliable. The pins are gold plated and have several contact points. The latching and screw mechanisms on them mean they can even survive the vibration of a rocket launch.

Check out Harwin's other connectors, especially the M30 range. They're pretty small and, if you crimp the wires properly, then they are extremely reliable. I have never seen one of the M30 connectors fail due to repeated mating.

Consumer Connectors: Generally, connectors intended for the consumer market should be able to survive quite a while. These include USB, Firewire, HDMI, VGA, D-Sub.

The other good thing about these connectors is that they are suitable for high speed data, like EtherCAT's LVDS physical layer. The downside of USB, HDMI and Firewire is that they aren't vibration proof, and may fall out if not strapped down. Make sure you get gold plated ones.

Omnetics: The cream of the crop come from Omnetics. They have the double benefits of being both super reliable, and also very small.

They have all kinds of wire-wire, and wire-board connectors. But be warned, they are very expensive.

ContentLicense: CC BY-SA 3.0

Title: What connectors are most reliable?


问:
Localizing a swarm of robots
I have a 300cm x 300cm room with a 25cm high ceiling (yes twenty five centimeters). It contains 50 small wheeled robots (about 20cm x 20cm). A central computer will orchestrate the movements of the robots, using a wireless network to send position commands to them. The robots will perform their own closed loop position control to save wireless bandwidth. The robots have 32-bit ARM microcontrollers. They have wheel position sensors, and the ability to do accurate wheel control.

Problem: The robots can't actually do this yet because they have no way to measure their position in the room.

Question: How can the robots be given the ability measure their position and orientation to an accuracy of better than ±5mm? I am looking for an accurate and robust solution, which is not affected by occlusions, and doesn't require a high power PC on each robot. Whatever sensors are necessary for localisation can easily be added to the robots. The microcontrollers can easily be changed for more powerful ones if the localisation solution demands it.

答:
If the ceiling is a flat surface that is visible from the tops of the robots, you could place marker stripes (or some other known fiducial pattern) on the ceiling at regular intervals.   The stripes might be white or black lines or narrow reflective tape, detected using photosensors on top of robots.

If wheel position sensors and accurate wheel control are good enough to maintain desired location data accuracy when a robot has moved no further than distance d from an accurately known position, the stripes (which might be white stripes, black stripes, or narrow reflective tape) might need to be placed no further than about d/√2 apart.

The motion-planning software probably would need to adapt. If a robot's position is consistently wrong when it crosses a stripe, adjust its ratio of wheel encoder counts to distance traveled; or if a stripe would be seen if a travel leg were just slightly longer, extend the leg to cross the stripe; or move to calibrate against several stripes just before any position-sensitive operations; or perform position-sensitive operations at a stripes crossing-point.

There is quite a variety of fiducial patterns possible.  Orthogonal stripes laid out parallel to x and y axes are probably simplest to create and work with.  But bulls-eyes, cross-hairs, wedges,  bar codes, and other patterns as well are worth considering.

Title: Localizing a swarm of robots


问:
Are robotics competitions for beginners available?
The micromouse challenge was done since the 1980 in the context of university driven robotics research. The Robocup challenge has a similar approach and provides a competition for university students. Today, Robocup soccer is a regularly event which brings robotics to the mainstream. But there is small problem. Both competitions are not attended by amateurs, that means by non-students. The main reason is, that the task of controlling a team of Nao humanoid robots is difficult and costly at the same time. That means, the Nao robot is too expensive for amateurs and they are not overwhelmed by implementing vision, motor control and self-localization at the same time.

On the other a hand, a robotics competition is a great opportunity to learn from mistakes. The same task is done by others and if strategy A is working it can be copied. A robot competition is a great opportunity to learn something and in most cases it helps to form a community.

My question is: are some entry level robotics competitions available which have to do with reaching objective goals against other participants which are open for amateurs? Open means, that the robot is cheap and the task isn't very hard.

Location

Are you looking for competitions anywhere in particular?

A competition can be held in real life, for example in a city. Or it can run online-only, that means the participants have to send their submission over a website from anywhere. I'm not sure, what the better idea is. offline competitions have the advantage, that a video can made from it, while online challenges have a lower price tag because the travel to the location isn't necessary.

答:
There are some competitions for smaller teams (or individuals), mostly centered around a small RPi-Based robot:
FormulaPi if you are interested in Computer Vision and autonomous cars, or
PiWars for a larger variety of tasks (including teleoperation, so you don't need a fully autonomous robot).

So searching for "RPi robot competition" could be a good start for you.


问:
State-vector for distance measurement between two autonomous cars
I hope someone can help me:
Given two autonomously driving cars, I want to make sure they keep a constant distance to each other. For this purpose, I want to design a Kalmanfilter. Typically, the first step for designing such a filter is to set up a state vector. I am given two robots already. They have an ultrasonic-sensor to measure distance and an encoder to measure velocity. However, what is not clear to me is: What would be the proper state-vector for my Kalmanfilter equations?

I have troubles understanding this, because, from the tutorials I have read, I got the impression a Kalmanfilter always combines at least two Gaussian distributed measurements. In order to compute a new distance for example, I would have to compute ... I don't know, maybe this:

Does somebody know?

答:
Kalman filter can be used to estimate the position of each car independently since there is no communication between two cars. In that case the most suitable parameters for the state vector is [Px Py $\theta$]. $\theta$ represents the heading direction. You can also include the X and Y velocities according to your requirement. In order to maintain a constant distance you can get the euclidean distance between the X and Y states of the two kalman filters.

I'm not sure what distance you are trying measure using the ultrasonic sensor.


答:
This is more of a hobbyists' competition, but you could take a look at it.
https://www.sparkfun.com/avc_2018</p>

Title: Are robotics competitions for beginners available?

Body:
The micromouse challenge was done since the 1980 in the context of university driven robotics research. The Robocup challenge has a similar approach and provides a competition for university students. Today, Robocup soccer is a regularly event which brings robotics to the mainstream. But there is small problem. Both competitions are not attended by amateurs, that means by non-students. The main reason is, that the task of controlling a team of Nao humanoid robots is difficult and costly at the same time. That means, the Nao robot is too expensive for amateurs and they are not overwhelmed by implementing vision, motor control and self-localization at the same time.

On the other a hand, a robotics competition is a great opportunity to learn from mistakes. The same task is done by others and if strategy A is working it can be copied. A robot competition is a great opportunity to learn something and in most cases it helps to form a community.

My question is: are some entry level robotics competitions available which have to do with reaching objective goals against other participants which are open for amateurs? Open means, that the robot is cheap and the task isn't very hard.

Location

Are you looking for competitions anywhere in particular?

A competition can be held in real life, for example in a city. Or it can run online-only, that means the participants have to send their submission over a website from anywhere. I'm not sure, what the better idea is. offline competitions have the advantage, that a video can made from it, while online challenges have a lower price tag because the travel to the location isn't necessary.


问:
Why no cmd_vel topic when running Turtlebot simulation on ROS + Gazebo?
I have been having a very difficult time trying to understand why my system does not have the cmd_vel topic.  I am trying to get a ROS system running with Gazebo so that I can work through tutorials and learn ROS.  I started with the ROS Development Studio (online) and it was great!  But sooner or later one must be able to get ROS going on a local system.  So I built a Ubuntu system and installed ROS + Gazebo, then installed Turtlebot including the simulator.  It all works!  I can launch a Turtlebot simulation then launch the Turtlebot_teleop package and I am able to move the robot around.

But when I list the running topics cmd_vel is not one of them.  These are the list of topics grep'd on "cmd":

```<code>scott@Ubvm:~/catkin_ws/devel$ rostopic list | grep cmd
/cmd_vel_mux/active
/cmd_vel_mux/input/navi
/cmd_vel_mux/input/safety_controller
/cmd_vel_mux/input/switch
/cmd_vel_mux/input/teleop
/cmd_vel_mux/parameter_descriptions
/cmd_vel_mux/parameter_updates
</code>```

No /cmd_vel!  So I can't work through tutorials to drive the robot around.  Besides, I just don't understand why it is not there but teleop works.

Could someone tell me why I do not see the cmd_vel topic?

System info:

Ubuntu 16.04

ROS Kinetic

Gazebo 7 (not sure exactly which version but it launches and runs)

Installed Turtlebot based on Indigo install but replaced with kinetic.

```<code>sudo apt-get install
ros-kinetic-turtlebot
ros-kinetic-turtlebot-apps
ros-kinetic-turtlebot-interactions
ros-kinetic-turtlebot-simulator
ros-kinetic-kobuki-ftdi
ros-kinetic-rocon-remocon
ros-kinetic-rocon-qt-library
ros-kinetic-ar-track-alvar-msgs
</code>```

答:
/cmd_vel and /cm_vel_mux are very often interchangeably used where the latter is a multiplexer superimposed to enforce priority in execution. Obviously, if the robot is moving, there has to be a navigation topic with data being published.

You should check the /cmd/vel_mux/input/navi topic - it should be the one that has a Twist message to it and makes the Turtlebot move.


问:
Maximum Distance using Ultrasonic sensor Arduino
What is the maximum distance (of say , a car ) you could measure using an ultrasonic sensor that would be compatible with arduino? Is there any sensor(ultrasonic or not) that could measure the distance of a car , say upto 50 meters that can be used with arduino?</p>

答:
Let's take a look at the physics side of things. Ultrasonic (acoustic) and lidar (laser) rangefinder sensors calculate the distance based on time-of-flight (tof). The time taken for a burst of sound waves to hit an obstacle and reflect back to the source indicates how far away the obstacle is from the source. The propagation speed of sound is $340$ m/s whereas for light it is $3 \times 10^8$ m/s. In other words, the time it takes for an ultrasound wave to travel $1$ m is approximately $3$ ms, as opposed to $3.3$ ns for light and radio waves.

The equation to calculate the tof duration is: $$ Duration (s) = Distance (m) \times \frac{2}{Propagation Speed (m/s)}$$

and the directivity pattern of $40$ kHz sound waves looks like this:

with this kind of directivity pattern, if the sensor is mounted on a car, there will be a lot of reflection from the ground, which will cancel out the original signal.

The good news is as the frequency increases the directivity pattern shrinks, increasing the range. But this is no way going to give you $50$ m. Even if you could do that, due to the relatively slow propagation speed of sound, you had to take the car velocity into account while calculating the distance.

This is why autonomous vehicles use ultrasonic sensor for park assistance and lidar sensors for collision avoidance / pedestrian detection and long-range radar for adaptive cruise control.


问:
Building a non rotating persistence of vision device
I'm looking for a way to create a non-rotating persistence of vision device. I have all the electronics set up but I'm stumped with the mechanical design.
I tried these two designs:

But these didn't work so well. Everything shakes violently and it doesn't go nearly as fast as I need (about 20 swipes per second)
Any ideas on how I can build this?

答:
I think Zuu's answer was very good, but I'd like to add some more detail to it.

The idea of using a spring is very good. From the start this creates the oscillating movement you want. I would suggest using a steel leaf spring. This is good for two reasons:

It restricts the movement to just left-right.

It allows you to add solenoids to drive the movement.

Firstly, set up your spring system. The PCB with LEDs is mounted somehow on the end of the leaf spring. As well as the LEDs, the PCB should also have an accelerometer to measure the movement of the PCB.

Pull back the PCB with LEDs and let it oscillate. Look at the output of the accelerometer to measure the frequency of oscillation. Adjust the length of the spring until the oscillation frequency is about 20 swipes per second.

You're almost there. Now you need to drive the oscillation using the solenoids. Use the accelerometer to synchronise energising the solenoids.

Added:

The reason this is an improvement over Zuzu's answer is that, while solenoids are great for short sharp movements, they are terrible for long smooth movements. The travel you get from them will be too short to make an interesting display.

What you need is some mechanism that allows the solenoids to work over a short distance, while the LEDs move a large distance. Hence allowing the stick to bend at the leaf spring. Whether you consider this to be a 'rotating' mechanism depends on whether you consider bending to be a type of rotating.


问:
Is pose estimation using images necessary in visual-inertial SLAM
In visual SLAM, pose estimation is performed between two adjacent frames or keyframes using feature detection and pose estitmation from corresponding features.

However in visual-inertial SLAM, pose information can be calculated from acceleroemeter and gyro. Is visual information still used to estimation poses?
I have read several papers about SLAM, but authors don't mention this explicitly.

Edit:

From the link:

Full 3D Pose (Scale, Rotation and Translation) Estimation using Gyro and Acceleromter sensors fusion

Pose estimation from IMU sensor is incorrect due to bias. Then IMU sensor will not help in typical situation, but only in the situation where few textures are found and feature matching won't work?

答:
No, nothing is "necessary".  You can estimate the pose of the robot perfectly legitimately using only IMU data. You can also estimate it perfectly legitimately using only image data. But it won't be that good, there will be errors in any sensor.

Localization is not a decision problem. It can be tempting to "switch" between sensors and think really hard about when one sensor is better than another. The community has determined that is the wrong way to do business. It's much better (the story goes) to use all sensors merged together. The community has agreed since the days of Apollo that an average of all position estimates, weighted by uncertainty, is best.

Almost any practitioner will tell you the best way to localize a robot using IMU and vision is V+INS SLAM, of which there are several open-source implementations available. Currently ORB-SLAM or MAPLAB is very popular (I'm told).

You may also want to peruse this q/a post or  (disclaimer, I said a similar thing there).


问:
RGB-D SLAM - Compute Information Matrix
currently im working on a RGB-D SLAM with a Kinect v1 Camera. In the front-end the SLAM estimates the pose with Ransac as an initial guess for the ICP. With the pose estimation i transform the pointcloud to a pointcloud-scene which represents my map.

To smooth the map im trying to implement a graph optimizing algorithm (g2o).
Until now, there is no graph representation in my frontend, so i started to integrate that.

Im trying to build a .g2o file with the following fromat:

VERTEX_SE3 i x y z qx qy qz qw

where x, y, z is the translation and qx, qy, qz, qw ist the Rotation in respect to the initial coordinate system. And,

EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx, qy, qz, qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66

Translation and rotation for the edge is the pose estimate that i compute with Ransac and ICP (visual odometry).

Now im getting stuck with the information matrix.
I read the chapter 3.4 THE INFORMATION FILTER in Thrun's Probabolistic Robotics and several threads in this forum, such as:

The relationship between point cloud maps and graph maps

and

information filter instead of kalman filter approach

From the second link, i got this here.

The covariance update
  $$P_{+} = (I-KH)P$$
  can be expanded by the definition of K to be

$$ P_{+} = P - KHP$$
  $$ P_{+} = P - PH^T (HPH^T+R)^{-1} HP$$

Now apply the matrix inversion lemma, and we have:

$$P_{+} = P - PH^T (HPH^T+R)^{-1} HP$$
  $$ P_{+} = (P^{-1} + H^TR^{-1}H)^{-1}$$

Which implies:
  $$ P_{+}^{-1} = P^{-1} + H^TR^{-1}H$$

The term $P^{-1}$ is called the prior information,$$H^TR^{-1}H$$
  is the sensor information (inverse of sensor variance), and this gives us  $P^{-1}_+$, which is the posterior information.

Could you please point this out for me.
What data do i need to compute the information matrix?

答:
Your sensor does not measure state

Kinect does not provide any "measurements" of state. It scans the environment and provides a point cloud. To use this to localize a robot, you must compare currently-sensed point clouds to past-sensed point clouds to provide a "correction" term. ICP (as mentioned) will provide such a "correction" term. That correction is the "innovation" in the EKF/EIF. To calculate the covariance / information matrix, you need, at minimum, the variance (if one-dimensional) or covariance (if 2-or-more-dimensional) of the innovation.

But you don't want to do that, actually. You want to do pose-graph optimization to smooth the maps. For that, you simply need to provide a state estimate of the position of the kinect sensor at the time of each scan. Imagine an interative process like this:

Take the first scan, save it with sensor position $x_0=0,0$

Take another scan, and use ICP to figure out how the sensor has moved between scans $\delta x$

Save that scan, and set $x_1=\delta x+x_{0}$

Now, repeat 2 and 3, taking scans, calculating $\delta_x$ and setting $x_i=\delta x + x_{i-1}$.

You have a pose graph / set of key frames. The positions are the poses, and the features in the environment are the ... features. Pose graph optimization requires at minimum these two things plus some measure of uncertainty of the poses. You can easily make up these values to get a working implementation (e.g., $\Sigma_i=c\cdot I$). Then, the information matrix is .... $\frac{1}{c}I$. Is this OK? well, the only better way to do it is to calculate the variance of the innovation above, and somehow "calibrate" the "position sensor" that is given by the ICP scan registration process.

Repeating again

The optimization problem is to find the correct set of $\delta x$ values for all $i=1...n$, but, you already did that in 2-3. So your pose graph optimization will not do anything new unless you iteratively match all pairwise scans and update the correction factors. This will only work if subsequent scans overlap scans from more than one step in the prior set of scans ... e.g., if you have a loop closure.

Repeating another way

Ultimately, the "right" way to do this is to form a giant optimization problem where you find the set of all $\delta x$ to minimize the error in the scan matches, subject to not straying "too far" from the initial pose estimates. Here "too far" is encoded by your "made up" uncertainty on the poses, and you'll have to tune how much you weight priors.

you may find this restatement of the above details better

Title: RGB-D SLAM - Compute Information Matrix


问:
Trajectory following- What if the robot joint motor are not able to keep up with the generated trajectory?
I want my robot manipulator to follow a user-defined path. Based on the waypoints given as input and the velocity and acceleration limits of the joint motors, I generate a trajectory.
At every time step (say 2ms), I send the joint motor controller the reference position, velocity and acceleration. But my robot's joint motors are not able to keep up with the reference command.
How can I update my generated trajectory, so as to ensure the robot moves along the user-defined path?
Thanks!</p>

答:
Great question!

In practice, you don't specify a dense trajectory for the arm necessarily

Instead, you should move the arm between "safe" positions before it moves to the point where it interacts with parts. Most robotic arms are designed to be controlled in this way. If you are trying to control the exact path of the end effector, then you are trying to do essentially velocity control, which requires force-torque control of the arm (usually). That's a harder optimization problem and still requires a good model as described below.

But trajectory control is more fun

We can add a lot of similar situations to this: What if I'm slightly wrong about the model of how the manipulator moves between waypoints, and the trajectory is not follow-able? Or what if the world changes slightly and my trajectory is now invalid? etc, etc.

At a very low technical level, you should change the time step in your trajectory planning to accommodate motions the arm can actually follow. This means allowing sufficient dwell time at each waypoint to ensure the arm can arrive at the points. For "dense" trajectories with lots of waypoints, this is hard. Without a near-perfect model of how the arm converges to a point, how can you predict how long it will take to reach a point when planning a trajectory?

Usually, at the highest level, you can just replan the trajectory based on the current state of the world. Then, you issue the "next" waypoint as the carrot for the arm to follow. This is called receding horizon control, which is a common way to implement model-predictive control


答:
A Kalman filter can combine multiple sensors, but can also recover information of other states you do not measure directly. For example if you only measure the position the Kalman filter can also obtain a good estimate of the velocity. A naive way for this would be to differentiate the position. However, when the position is subjected to noise, differentiating it will amplify this noise a lot. In such a case a Kalman filter would be able to get a much better estimate of the full state. It does this by trusting more on the model you have of your system.

I do have to note that in order for the Kalman filter to be able to estimate the full state does require that the sensors you have make your system observable.

Title: State-vector for distance measurement between two autonomous cars

Body:
I hope someone can help me:
Given two autonomously driving cars, I want to make sure they keep a constant distance to each other. For this purpose, I want to design a Kalmanfilter. Typically, the first step for designing such a filter is to set up a state vector. I am given two robots already. They have an ultrasonic-sensor to measure distance and an encoder to measure velocity. However, what is not clear to me is: What would be the proper state-vector for my Kalmanfilter equations?

I have troubles understanding this, because, from the tutorials I have read, I got the impression a Kalmanfilter always combines at least two Gaussian distributed measurements. In order to compute a new distance for example, I would have to compute ... I don't know, maybe this:

Does somebody know?


问:
Does the red object act as a fulcrum (as explained bellow) to make picking up easier?
In picture one I have shown a robot arm (green) rotating to pick up a box, using a fulcrum (red). The arm after rotating is shown with black rectangle. The vertical arm on the left is attached to the horizontal green arm and goes down to make the green arm rotate.

In picture two, I have shown a robot arm which goes up and down to pick up the box, connected to a Revolute shoulder on the left which is not shown in the picture.

I have tried to modify the second picture, so I have created the first picture. I wanted to use fulcrum and related physics theorems to make the picking up easier for the robot.

Can you please tell me if I am making mistake and this is not a way to modify the arm and to make picking up easier? Does the red object act as a fulcrum?

答:
The red object looks like a fulcrum in that it provides the pivot point for the green bar.

However, the point of a lever + fulcrum set is to generate a mechanical advantage. Mechanical advantage is essentially "trading" a displacement for a force. If the output of the lever moves half as far as the input when rotated then it generates twice the force/torque.

In your diagram, it looks like the middle of the green bar is on your fulcrum, though, so it's basically the same motion on either end, which means you're looking at a 1:1 force difference.

That is, you're not getting any more force out of your lever than you're putting in. This might be advantageous if you can only orient your actuator in a particular way, but otherwise this doesn't seem to be doing much for you. You'd want more of the green bar on the actuator side and less above the load to get a force advantage.


问:
How to get starting location of robot using IMU Sensor (without using gps)
i have one robot, i have to get the initial location of that robot. then when we move the robot from that location, and when we give the command like "HOME" then it should go that initial location. but i don't understand how to get initial location   of that robot.   </p>

答: An IMU alone is not sufficient to get position and is exactly why you use it in combination with a sensor that measures position, like a GPS. Even if you manually give it an initial position, the IMU is going to drift in position unless calibrated often. </p>


答:
An IMU gives you linear acceleration and rotational speed. It doesn't give a position.

You can integrate the output of an IMU to get a linear position and angular orientation (the pose), but you need to choose initial speeds and positions.

Typically, these choices are zero. If you decided to choose zero for your initial/default pose, then returning "home" just means trying to get those values to go back to zero again.

Title: How to get starting location of robot using IMU Sensor (without using gps)

Body: i have one robot, i have to get the initial location of that robot. then when we move the robot from that location, and when we give the command like "HOME" then it should go that initial location. but i don't understand how to get initial location   of that robot.   </p>


问:
What is the difference between Obstacle Avoidance and Dynamic Path Planning?
I always assumed that obstacle avoidance refers to general prevention of collisions with any kind of obstacles, even without considering a goal for the decision how to execute an avoidance maneuver. Is that correct?</p>

答: In my perusion of a few scholarly articles, most, if not all, use them in direct conjunction with each other (such as "Dynamic obstacle avoidance path planning"). Dynamic path planning is defined as "...process of breaking down a desired movement task into discrete motions that satisfy movement constraints and possibly optimize some aspect of the movement." (Wikipedia). Based on these two, its safe to say that obstacle avoidance is part of dynamic path planning.


问:
Why won't my PID line follower work?
I'm trying to implement a PID based line follower. The robot has 2 line sensors with ranges from 0-200, with 0 being black and 200 being white. I'm trying to follow a black line.

I'm struggling to get the robot to do anything but erratically turn and confuse itself on the spot, getting wildly out of control. I've tried adjusting the setpoint value, the operators in the equations and the kp/kd values to no avail.

Is there something wrong with the algorithm I'm using? (I'm a beginner to this so it's likely)

Thank You

My code is below:

答:
You seem to only be using a PD controller, but that should be sufficient. The biggest typo I could spot is that you multiple instead of add the proportional and derivative terms. Namely it should be:

```<code>motorSpeed = kp*error + kd*(error - lastError)
</code>```

Usually one would also divide the error difference by the time that has passed, however this would only change the effective kd gain by a constant factor, assuming that the time between samples is constant. Also normally the error would defined as setpoint minus position, which effectively would multiply motorSpeed by minus one.

Title: Why won't my PID line follower work?

Body:
You seem to only be using a PD controller, but that should be sufficient. The biggest typo I could spot is that you multiple instead of add the proportional and derivative terms. Namely it should be:

motorSpeed = kp*error + kd*(error - lastError)

Usually one would also divide the error difference by the time that has passed, however this would only change the effective kd gain by a constant factor, assuming that the time between samples is constant. Also normally the error would defined as setpoint minus position, which effectively would multiply motorSpeed by minus one.


答:
I have lots of experience with this that I won't bother you with. Most vehicle planners have multiple layers, with different requirements. In short, you're right in that they are very closely related and often overlap. There is some general consensus from what I've seen for a "layered" approach as follows.

example three layers

Goal planner

Path planner

Hazard avoidance

Goal planner

The point of this layer is to decide where the robot should be going. Usually this is done from a strategic view, like "Get to that hilltop to charge solar panels" or "try to get into this lane because you need to exit soon" or "the human wants pictures from this location". You can even choose multiple goals and plan a route through them, so sometimes this layer is called route planning. This is the purview of operations research usually, and involves problems often called vehicle routing, facility location or TSP-like problems. You might run this layer once every few seconds to once every minute. (notice how long it takes google maps to replan your route).

Usually, this level of planning considers the general direction of travel over minutes to hours range, and does not include anything but a cursory approximation of terrain or obstacles.

Path planning

Once you know you are at location A and want to get to location B next, you usually plan a path (or trajectory) to get to that location (or at least closer if you are using receding horizon control). At this layer, you can often afford (computation-wise) to include static obstacles like no-go zones, terrain, or other vehicles that you can see and trust your predictions about. At this stage you often do not consider dynamic obstacles that are hard to predict, or are very far away in terms of time to collision etc.  You usually optimize some metric like time to arrival or energy cost or safety (e.g., likelihood of using hazard avoidance during trajectory) or comfort of operator, etc.

In short, you do plan to avoid collisions that you can anticipate at the 10s of seconds to (maybe) minutes range, depending on the density of the environment.

Basically, RRT and MPC dominate this layer, from my experience, and are confusingly called trajectory optimization, trajectory generation, path planning, and feed-forward controllers. Sometimes this is broken up into two layers, as well. You might run this layer at 1-100Hz. Due to the continuing improvement of onboard computation, and the continuing R&D at this layer, sometimes you can do all your planning (including obstacle avoidance) at this layer. Usually, you still want the following layer running just in case...

Hazard avoidance

This layer is the most "reactive". It is the quick reflexes to slam on the brakes or veer when something unexpected happens. Usually this takes as input a general velocity or heading, and tries to keep the vehicle safe while trying to reach that velocity. If a car veers in front of you, you don't want to spend a second replanning your whole trajectory for the next minute (or replanning your strategic objectives), you just want to avoid a collision and let the higher planners take over once the vehicle is safe again.

This layer operates on a millisecond reaction time and only looks over the next few seconds usually. It also does the crudest approximation (to stay fast) of the dynamic obstacle's trajectories, usually linear projection. You might run this layer at 100Hz in some cases.

Technologies fitting in this area are usually something like artificial potential fields or velocity obstacles and are usually classified as obstacle avoidance algorithms.

Title: What is the difference between Obstacle Avoidance and Dynamic Path Planning?

Body: I always assumed that obstacle avoidance refers to general prevention of collisions with any kind of obstacles, even without considering a goal for the decision how to execute an avoidance maneuver. Is that correct?</p>


问:
What wavelengths are most commonly used in laser-scanners and LIDAR systems?
In Photography SE I've just asked LIDAR burnout; ways to check for damaging infrared lasers before shooting besides looking for posted warnings? and discussed a camera that was damaged by a self-driving car's LIDAR system at the recent CES show.

The article linked there mentions the powerful "fiber laser" used by that car's LIDAR system. For those I normally think of infrared wavelengths around 1500 or 1600 nm used in communications, but for simple semiconductor lasers based on III-V material the wavelengths are often around 800 or 900 nm.

This is just from personal memory. I'm wondering if I've got it right, and/or if there are other sources for common laser wavelengths used in obstacle detection and ranging.

答:
You are correct. Most Velodyne LIDARs (very commonly used on autonomous vehicles), use 905 nm diode lasers, and so do many Hokuyo laser scanners and SICK LIDARs.

A new company, Ouster, uses 850 nm lasers, claiming that their choice of lower wavelength is because the "atmospheric water absorption is orders of magnitude lower than at 905nm".

Some more info: https://velodynelidar.com/newsroom/guide-to-lidar-wavelengths/


答:
Both 1550nm and 900nm ranges are used in different lidars.

Velodyne and majority others use 905nm lasers because these diodes are really cheap.

1550nm are used when there is a need for very long distance measurement range, such as Luminar, as the eye safety threshould is several orders of magnitude higher for this wavelength than 905nm.

In principle, the laser has to be very very high power to distroy a camera. Unfortunately most of these lidar start-ups are still in developing phase, and the so called Class 1 is the goal to achieve. Be careful not to look at the start-up's lidars.

Title: What wavelengths are most commonly used in laser-scanners and LIDAR systems?

Body:
In Photography SE I've just asked LIDAR burnout; ways to check for damaging infrared lasers before shooting besides looking for posted warnings? and discussed a camera that was damaged by a self-driving car's LIDAR system at the recent CES show.

The article linked there mentions the powerful "fiber laser" used by that car's LIDAR system. For those I normally think of infrared wavelengths around 1500 or 1600 nm used in communications, but for simple semiconductor lasers based on III-V material the wavelengths are often around 800 or 900 nm.

This is just from personal memory. I'm wondering if I've got it right, and/or if there are other sources for common laser wavelengths used in obstacle detection and ranging.


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

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

答:
It was really interesting to check out the Livox lidar site! The order can be placed from DJI site.

The lidar scans 3D point clouds with 200m detection range outdoors. Comparable with velodyne 32 and priced at 600 bucks.


问:
Why do robots need rangefinders while animals don't?
Will vision based system eventually prevail rangefinders based systems given that the most successful autonomous agents in the world like animals use primarily visions to navigate?</p>

答:
Vision is actually one kind of rangefinder, although passive. Earlier camera use parallax to find the distance and focus.

Nowadays, active rangefinders are developed such as ultrasonic sensors, lidar sensors, which emit a signal and checks the returned time, the time of flight. This is more accurate and could adapt to low light conditions.


问:
Controlling YuMi robot by serial port
I'm trying to control the YuMi robot arm by a serial port. The serial port will give the information on what direction the end-effector should move and what is the velocity. What I would like to achieve is that the robotic arm will keep moving in the certain direction and velocity until the serial command changes. For example, if the serial port tells the robotic arm to move at velocity 1 in the x-direction, the robotic arm will not stop until the serial port changes the velocity to 0.

We are currently using the 'yumipy' python library (https://github.com/BerkeleyAutomation/yumipy) for this purpose, but it might not be suitable for real-time smooth motions in our application. Because it can only generate consecutive discrete pose commands by reading the serial commands, and there will be small time gaps between each pose command. Also, the RAPID server has to execute each command in full before execute the next command (which can cause a time delay as well).

Will External Guided Motion (EGM) be suitable for this application? Are there any other possible approaches and what would be the optimal one?

答:
There is an open source ROS library, which implements controlling an ABB robot using the EGM. You can find the library here.

The library allows the definition of a trajectory and adding points to the trajectory. So might be what you want to do. The exact code which achieves this is the addTrajectoryPointFront() and addTrajectoryPointBack() function in the egm_trajectory_interface.h file. It is unclear if the currently executed trajectory can be altered using the functions above or not.

If the above method is not suitable, you can try using velocity goals. Velocity goals can be added using a fast_transition option which seems to enable what you are looking for.

Please note that I have not used the EGM Interface, I am also trying to implement a similar application, real-time trajectory control with EGM, but did not progress to actual implementation yet.

Title: Controlling YuMi robot by serial port


问:
Localizing a swarm of robots
I have a 300cm x 300cm room with a 25cm high ceiling (yes twenty five centimeters). It contains 50 small wheeled robots (about 20cm x 20cm). A central computer will orchestrate the movements of the robots, using a wireless network to send position commands to them. The robots will perform their own closed loop position control to save wireless bandwidth. The robots have 32-bit ARM microcontrollers. They have wheel position sensors, and the ability to do accurate wheel control.

Problem: The robots can't actually do this yet because they have no way to measure their position in the room.

Question: How can the robots be given the ability measure their position and orientation to an accuracy of better than ±5mm? I am looking for an accurate and robust solution, which is not affected by occlusions, and doesn't require a high power PC on each robot. Whatever sensors are necessary for localisation can easily be added to the robots. The microcontrollers can easily be changed for more powerful ones if the localisation solution demands it.

答:
Some ideas off the top of my head...  Generally you can either have each robot sense it's own position or have some sort of system find the robots and send them information about their position (or a combination).  Possibly using other robot positions to locate yourself is another option if they can communicate.  You could also combine sensor information from the robots and other sources.

Optical

You can encode either absolute position or relative position on the floor or ceiling.  A sensor on the robot can look at those.  A sensor like the one in your optical mouse can get relative motion off most surfaces even without any pattern.  There were older style mice that used a patterned grid.

You could have two (or more) cameras on every robot looking at different directions, if the walls of the room have the right kind of pattern you could determine your location based on the images.  A spinning camera is another option.  Obstruction of the camera by other robots may be a concern.

A sensor array or camera in the floor or ceiling could locate the robots and then you can send the robots their location.

Some sort of spinning optical sensor that can locate the direction of an optical beacon (e.g. a LED).

Sound

You could have a few beacons around emitting ultrasound chirps.  If they are all synchronized (e.g. fixed delay between them) then given their location you could use a time of flight calculation to determine the position of the robot.  Many years ago I've worked with an ultrasound digitizer that was accurate to about a mm over a distance of about a meter so it seems in the ballpark.  Depending on the shape of your robot and the configuration of the swarm reflections and obstructions may or may not be a problem.  You would need to experiment but my gut feeling is that with enough beacons you could good performance.

Ultrasound range finders on every robot.  (spinning?)  Could map the distance to other robots or the walls.

If any of these sound interesting I can try develop those ideas a little further.

Title: Localizing a swarm of robots


问:
Humanoid Robot Arm
I have a modeling design for Humanoid robot arm in simscape-SimMechanics in Matlab, I am trying to use the inverse kinematic to make a reaching movement for the arm to the target position; however, I want to know if there is a possibility to coding the forward and inverse kinematic using a block function in Simulink becuase all most of the users try to exploit the Robotic Toolbox to get the Forward and Inverse Kinematics and implement to the robot. ( I mean i want to control my own design in SimMechanics rather than using Robotic Toolbox) </p>

答: You could use the MATLAB function block. Write a script that contains a single function for doing forward/inverse kinematics, then reference it in the MATLAB function block. Similarly, you could use the S-function block. This would let you make a more complex set of functions contained in a block. Within either of these functions that you write you can use your favorite numerical method to solve the inverse kinematics.</p>

Title: Humanoid Robot Arm


答:
Short answer: They don't need. Cameras can be used as range finders, and an easy example is the Xbox Kinect.

Computer vision is an area that is being studied a lot.
You can find a lot on authonomous cars, and the problem of SLAM.

Title: Why do robots need rangefinders while animals don't?

Body: Will vision based system eventually prevail rangefinders based systems given that the most successful autonomous agents in the world like animals use primarily visions to navigate?</p>


问:
Bullet vs SimMechanics: Inverse Dynamics and sensed forces/torques
This question may be a little broad, but I could not find any other information.

How accurate are the inverse dynamics functions in SimMechanics compared to Bullet?  How accurate are the sensed/measured forces/torques that will occur in joints? Can I rely on the results, that Bullet finds or is SimMechanics "the better way" to go? Do you have any experience? In my case, an accurate simulation for scientific purpose is needed, but I do not know how reliable or advanced bullet-physics is compared to tools like SimMechanics from mathworks.

Thanks.

答:
While SimMechanics and Bullet are both very reliable resources, in my experience, they serve different purposes. SimMechanics is more of a scientific environment as compared to Bullet which is a real-time high-performance Physics engine.

You can rely on both of their measurements and observations but the system modeling step determines the reliability better. SimMechanics is optimized for accuracy while Bullet is optimized for performance with some allowed approximations.

In SimMechanics, you can import CAD models and all sorts of design parameters and constraints, making the simulation very well defined. While the same can also be done in Bullet, it is more cumbersome and involves manual modifications which can be difficult to handle. The ability to import CAD models through SimMechanics and have more granular control on all factors gets it the upper hand. Sensors and physical world to electrical world and vice-versa conversions are also supported, giving you more options to replicate real world conditions.

While SimMechanics trumps Bullet for your application (assuming you only need for a prototyping kind of application), the latter is the best one for production environment.


问:
How to determine the rotation matrix and translation vector of a fixed LiDAR from the sensor frame to an aircraft's vehicle frame?
I am using a fixed 3D LiDAR sensor on a small aircraft. The vehicle has a GPS/INU to calculate its own position and orientation. I do not know the exact offset of the LIDAR from the center of the vehicle frame. I have an initial estimate of the rotation matrix and translation vector for my transformation matrix from the sensor frame to the vehicle frame. Is there an iterative method using my initial estimates to converge to a more accurate solution for roll, pitch, yaw angles and position vector (x,y,z)? Or is there a Least Squares approach? If yes, can you show/describe the method? Otherwise, if there is another method, please explain.</p>

答:
It sounds to me like you have a system with an unknown parameter that you want to estimate. That is the purview of parameter estimation literature. If this parameter to be estimated is not changing over time (as yours is not), then you can use old-school methods like LS or similar, as you suggest. This particular brand of parameter estimation is known as extrinsic calibration.

Most methods I'm familiar with (and I'm not familiar with most), follow an iterative (or batch) process as follows:

Estimate translation and rotation between two sensors

Observe features common to both sensors (e.g., estimate ego-motion from the lidar scan matching and integrate IMU measurements)

Use the features to estimate the position of each sensor relative to the features

You now know the location of the sensors w.r.t. common features, and therefore you can estimate the translation and rotation between them.

Use that estimate to update the original estimate (usually by taking a weighted average)

So, google scholar provides some hits on "imu lidar calibration" such as:

https://infoscience.epfl.ch/record/116145

http://www.isprs.org/proceedings/xxxiv/3-w13/papers/Katzenbeisser_ALSDD2003.pdf


问:
Assigning Serial number and GUID to a microcontroller
This might be a out of league question and may seems to be very odd.I am using multiple Arduino UNO boards over network and want to assign a GUID and Serial number to each board so that when ever they send any data to a central server, server is able to find which Device it is if we have assign name for each device.

first way to do this is to assign GUID and serial number of device before each message that is send to central server manually while programming and then burn that hex to arduino.

Now is there any way out that we can burn a program that always give a output as a string (GUID+Serial number of device) like we program EEPROM for this and then burn our main code in Arduino which pick the GUID+Serial ID combo from EEPROM and write it before every message which arduino is pushing to central server.

Or my another way of asking is can we program EEPROM with a different program and our Arduino separately like 2 files running in parallel or is it not possible?

Is there any other way of doing this?

答:
I would create a small sketch that allows you to communicate over serial port that will let you create a GUID on the desktop computer and write it to the EEPROM of your uno to a known location.

Then later on in your networked arduino code you can use that saved value.  I've done this before and it's pretty simple to implement.  You can even copy / paste guid values from the web like this Online GUID Generator.

See this for simple usage.  Arduino EEPROMWrite Command

Another option if you have networking already active is to setup a server on your network and have the machines request a GUID from a network server if they don't have one already present on the device, then write that to the EEPROM.

Either way it should be a small step you perform when first programming your devices.


问:
How do I find the inverse kinematics of a 5-DOF manipulator having the following DH parameters?
```<code>alpha = [ -90,   0,   0, 90,   0]
a     = [   0, 400, 200,  0,   0]
d     = [63.5,   0,   0, 90, 100]
</code>```

I am trying to calculate the joint angles but I am stuck after I got the values of Theta1 and Theta2.

答: If you say you're are stuck after values for the first two angles, you are probably trying an analytical approach. Try solving for IK using standard techniques Jacobian Pseudo Inverse or Cyclic Coordinate Descent. If you are looking for immediate solutions, use Peter Corke's Robotic Toolbox for MATLAB or KDL/TracIK in C++/Python to obtain the values.</p>


答:
A general IK solution is not possible for an arm with less than 6 joints.  You can move the gripper to any point (within reach) but you can't completely control the orientation.  If you fix the orientation, say fingers aligned with vertical axis (normal to the work surface), this should simplify the analytic solution.

Using my Toolbox, and starting with the DH kinematic parameters you provide (and assuming they are standard DH, you didn't specify)

```<code>>> robot = SerialLink( [zeros(1,5); d; a; alpha*pi/180]')
robot =
noname:: 5 axis, RRRRR, stdDH, fastRNE
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|       63.5|          0|    -1.5708|          0|
|  2|         q2|          0|        400|          0|          0|
|  3|         q3|          0|        200|          0|          0|
|  4|         q4|         90|          0|     1.5708|          0|
|  5|         q5|        100|          0|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+
</code>```

To get a feel for the robot I drive a graphical model around interactively and choose a reachable point with the gripper fingers downward

```<code>>> robot.teach
>> T = transl([550 90 0]) * trotx(180, 'deg')
T =
     1     0     0   550
     0    -1     0    90
     0     0    -1     0
     0     0     0     1
</code>```

Now I can solve for the inverse kinematics

```<code>>> robot.ikine(T, 'mask', [1 1 1 1 1 0])
ans =
   -0.0000    0.2694   -0.0017   -0.5561         0
</code>```

where the mask has only 5 ones in it, the Cartesian degrees of freedom we want to control.  In this case we accept that we cannot independently control rotation about the z-axis.  We can verify the IK solution

```<code>>> robot.fkine(ans)
ans =
    0.9587         0   -0.2845       550
         0         1         0        90
    0.2845         0    0.9587 1.916e-11
         0         0         0         1
</code>```

and see that it does give us the desired position, rightmost column, but the rotation matrix is not what we asked for, because rotation about z-axis is chosen for us.

Title: How do I find the inverse kinematics of a 5-DOF manipulator having the following DH parameters?

Body:
```<code>alpha = [ -90,   0,   0, 90,   0]
a     = [   0, 400, 200,  0,   0]
d     = [63.5,   0,   0, 90, 100]
</code>```

I am trying to calculate the joint angles but I am stuck after I got the values of Theta1 and Theta2.

Body:
A general IK solution is not possible for an arm with less than 6 joints.  You can move the gripper to any point (within reach) but you can't completely control the orientation.  If you fix the orientation, say fingers aligned with vertical axis (normal to the work surface), this should simplify the analytic solution.

Using my Toolbox, and starting with the DH kinematic parameters you provide (and assuming they are standard DH, you didn't specify)

>> robot = SerialLink( [zeros(1,5); d; a; alpha*pi/180]')
robot =
noname:: 5 axis, RRRRR, stdDH, fastRNE
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|       63.5|          0|    -1.5708|          0|
|  2|         q2|          0|        400|          0|          0|
|  3|         q3|          0|        200|          0|          0|
|  4|         q4|         90|          0|     1.5708|          0|
|  5|         q5|        100|          0|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+

To get a feel for the robot I drive a graphical model around interactively and choose a reachable point with the gripper fingers downward

>> robot.teach
>> T = transl([550 90 0]) * trotx(180, 'deg')
T =
     1     0     0   550
     0    -1     0    90
     0     0    -1     0
     0     0     0     1

Now I can solve for the inverse kinematics

>> robot.ikine(T, 'mask', [1 1 1 1 1 0])
ans =
   -0.0000    0.2694   -0.0017   -0.5561         0

where the mask has only 5 ones in it, the Cartesian degrees of freedom we want to control.  In this case we accept that we cannot independently control rotation about the z-axis.  We can verify the IK solution

>> robot.fkine(ans)
ans =
    0.9587         0   -0.2845       550
         0         1         0        90
    0.2845         0    0.9587 1.916e-11
         0         0         0         1

and see that it does give us the desired position, rightmost column, but the rotation matrix is not what we asked for, because rotation about z-axis is chosen for us.


答:
Animals and robots both need to understand something about the 3D structure of the world in order to thrive. Because it's so important, animals have evolved a huge number of strategies to estimate depth based on camera-like projective sensors (eyes).  Many make use of binocular disparity -- the fact that the distance between the same scene point in two different views is inversely proportional to depth.  A mantis moves its head sideways to do this, a dragonfly makes very precise forward motion, birds move their heads forward and backwards, we use the fact that our eyes are widely spaced on our heads and have overlapping fields of view.  Stereo vision is a computer vision technique that mimics this.

However disparity is not the only trick we use, there are lots of others including proprioceptive signals from the muscles in our eyes (focus and vergence), observing how things in the world move as we move (optical flow), our knowledge about the world (the tree looks small, we know trees are big, therefore it must be far away), haziness, texture density, occlusion (thing A blocks thing B, therefore A must be closer). Each technique (or cue) is imperfect, but mostly in a different way, and each operates across a different range of distances.  Our brain stitches (fuses) all that together to give a result that is mostly pretty robust.

Checkout the wikipedia page for more details. I like this (old) article

How the eye measures reality and virtual reality.
JAMES E. CUTTING, Cornell University, Ithaca, New York.
Behavior Research Methods, Instruments, & Computers 1997, 29 (1), 27-36

which gives a good insight into this issue.

There are now deep-learnt networks now that can take a monocular (single) image and output estimated depth.  We can never know for sure but my theory is that they are encoding some of the cues that we use for depth estimation such as texture and apparent size.


问:
Running ROS on Digital Ocean?
Has anyone done this? Create a "droplet" or whatever they call it and set up ROS, and be able to actually run roscore, gazebo, rviz, etc. from there?

I can imagine various complications, like having a static IP, figuring out what to with graphics apps like gazebo. Is there any way? Thanks!

答:
I did install a YARP based environment on my droplet along with graphics and it works great. I use it for testing purpose.

The installation in my case was very standard and when it came to graphics I simply searched for online guidance, which is usually super useful on Digital Ocean (DO). This is why I tend to prefer DO over other online premises: it's easy and highly configurable.

An example of how to deal with graphics installation can be found here.

Compared with ROS, YARP is a gentler beast, but I can imagine the installation is still doable, even though you would require definitely a droplet with more resources.

Title: Running ROS on Digital Ocean?


问:
How can i find and install (acspid) python PID library
How can i find and  install (acspid) python PID library in the folowing code </p>

```<code>import acspid
pid=acspid.pidcont(1.2,0.02,0.01,5,-5)
</code>```

答: You can find it on github:  ASC-PID</p>

Body: You can find it on github:  ASC-PID


问:
Libraries to simulate path planning in a dynamic environment for a 2D space
I need to simulate a path planning approach for a 2D space where the obstacles are moving. Initially, I need to implement probabilistic road map, D* lite, A* and Bug2 algorithm. The robot needs to be able to follow the wall of the obstacles.

I do not know which library is most suitable for these algorithms. I have a tight schedule, I need to finish the simulation within 1 month. I found ROS, matlab and Player. Which one can handle the simulation of dynamic obstacles and has the algorithms that I need. Any other suggestions are welcome.

答:
The most compact pathplanner in a 2d world is “rrt.py lavalle” The code which can be found in the Internet, includes the documentation and has only 64 LoC. It draws a wonderful randomized tree to the screen with the help of pygame. It works under all operating systems and can be adapted to new constraints.

What i can't recommend is to combine RRT with Fuzzy logic to inject expert knowledge in the pathplanner. In theory, this would allow to handle dynamic obstacles and classifies different maps. The problem here is, that it is not sure, how to falsify this kind of things, so the question is, if it's still mathematics or something else.


答:
The low distance between the top of the robot and the ceiling really restricts your options. It seems pretty much impossible to get a centralised overview of the whole room and work from there.

I'm not sure what kind of 'room' you are talking about and how much you can instrument it, but it might be an option to place markers on the ceiling rather than the robots. Given the short distance, you'd have to pretty much completely fill the ceiling with tiny markers that can be completely observed by an upwards pointing camera on every single robot, though you might be able to position this camera lower on the robot, say between the front and back wheels on either side, to give you a wider viewing angle. But the biggest challenge would be to print enough distinct markers to instrument the whole ceiling.

Alternatively, it might be conceivable to instrument the floor with lots of RFID tags, provided you can find readers that have a small enough range (AFAIK, RFID readers will only tell you that a certain tag is in range, not where it is). The Phidgets RFID reader already has a range of approx. 3 inches, so unless you localise by seeing which group of tags you can observe (if it's possible to observe multiple tags at the same time - can you tell I have no actual working experience with RFID?), you'd have to experiment with getting smaller tags and 'shielding' them to a degree from the reader, so that they can't be read other than at very close range.

All in all it seems a tough but very interesting challenge. If it's for work, I assume you can't tell us the purpose of the project, but it sure sounds intriguing :)

Title: Localizing a swarm of robots

Body:
I have a 300cm x 300cm room with a 25cm high ceiling (yes twenty five centimeters). It contains 50 small wheeled robots (about 20cm x 20cm). A central computer will orchestrate the movements of the robots, using a wireless network to send position commands to them. The robots will perform their own closed loop position control to save wireless bandwidth. The robots have 32-bit ARM microcontrollers. They have wheel position sensors, and the ability to do accurate wheel control.

Problem: The robots can't actually do this yet because they have no way to measure their position in the room.

Question: How can the robots be given the ability measure their position and orientation to an accuracy of better than ±5mm? I am looking for an accurate and robust solution, which is not affected by occlusions, and doesn't require a high power PC on each robot. Whatever sensors are necessary for localisation can easily be added to the robots. The microcontrollers can easily be changed for more powerful ones if the localisation solution demands it.


问:
Arm path planning with obstacle avoidance in Unity
My application is not specifically in Robotics, but the problem that I'm trying to solve seems to have been studied primarily from the viewpoint of Robotics, so I am hoping to get an answer here. Apologies if it's not the correct place.

Problem description

I am creating a virtual human agent in Unity. The agent will primarily interact with blocks on a table by sliding or carrying them from one point to another in 3D. Apart from the constraints that accompany human arms, the planned path should not result in arm colliding with other blocks, nor should the blocks collide with one another. The former case is a possibility since blocks could possibly be stacked on top of one another to form towers.

My solution

I don't have any experience in Robotics, so my solution may not be the best: First, use an IK solver to find initial and goal arm orientations, so I have initial and goal state in configuration space. This could be done by an IK solver. I'm thinking to use Cyclic Coordinate Descent, since it feels very easy to implement. Next, I'd have to implement RRT-Connect to find the path between those configurations. My knowledge on this is limited to what I've read in the RRT-Connect paper.

Questions

My questions are three-fold:

Does the stated approach seem to be reasonable? Lacking background in Robotics, I'm afraid I might be missing something obvious.

Is there a way to find initial and goal configurations for arm that are also collision free?

Finally, in the tradition of being lazy as a programmer, I've been looking at OMPL to use an existing implementation for a path planner, especially since there are so many, and they'll also be well optimized. Problem is I'm not sure if there's a way to build it for .NET (C#) since that's what is used in Unity for scripting.

Any help would be greatly appreciated!

答:
The steps you have identified are correct.

You will need an IK solver if you want to plan in Cartesian space. Cyclic Coordinate Descent is one OK option if you always solve for points which are close to eachother.  However, I would advise to look at Unity IK solvers. It will probably save you some time in implementation.

RRT Connect is one option which is fairly easy to implement. Instead trying to include OMPL in a Unity3D Project, you might want to search for Unity3D implementations of RRT Connect or similar algorithms. There is TRRT open source for example, which might serve as in inspiration for implementation. As I know, Unity3D offers only 2D navigation algorithms "out of the box", however there are some path planning assets available in the assets store. One free asset which support 3D path planning is this, but you can look for others, maybe non-free ones also.


问:
PID loop to control motor velocity with disturbances
I am trying to use a PID loop to control a motor. This is my first attempt at creating a PID loop (really just a PI loop). There doesn't exist a model for this system, and I don't know how to create one. All that I mention below may be an incorrect approach, so please guide me on the correct one.

My goal, is to set a percentage velocity, and have the motor run at the velocity.

My only feedback is degrees of movement, which I have correlated to an acceptable input -> output ratio. Meaning, without any resistance, if I have moved 50 degrees in 250mS that is considered 100% velocity. 25 degrees = 50% etc.

As mentioned in another thread here, I've experimented with setting waypoints for the PID controller. That is, if I were moving at 100% velocity, I set a waypoint that is 50 degrees away from the last one, every 250mS. This appears to be working fine under non problematic angles.

The problem is, if I am at a lower speed, such as the 50% velocity. There are certain angles where the proportional gain is not enough, and the motor is stuck. I've tried experimenting with how I modify the waypoints, and modifying Ki and Kp. I could enumerate them here, though I don't think it's worth it, as I think I have some fundamental misunderstanding on how to do this.

So to give an overview of my current structure in pseudo code:

```<code>while (1)
{
    if firstRun or 250ms elapsed
        targetPosition = currentPosition + scaledVelocityValue

    if 10mS elapsed
        calculatePID()
}
</code>```

In this case, the currentPosition is not be the previous targetPosition, and therefore the speed is always constant. I've tried using the previous targetPosition, but that creates an unstable system because it's such a large increase.

I hope this makes some sense. Some guidance please.

答:
You undertook a dead-end road. You must use a velocity setpoint instead.
This way, the problem boils down to the estimation of the velocity from position feedback, which is usually quite tough since it entails to deal with a derivative of a physical thus noisy signal, yielding spikes in the outcome.

Computing the standard derivative of a signal is something a good engineer should always avoid. To get around this, one could either make the derivative more robust hardware-wise by using a very resolute encoder or resort to control techniques such as Kalman filtering (where the derivative is replaced by integration) or Savitzky–Golay filtering that instead solves a linear least-squares optimization on the fly.

Related Q&A.


问:
Inverse kinematics for differential robot knowing linear and angular velocities
I have a differential robot and I am using ROS to run it. When I need to move the robot, I can send commands to it, specifically linear X and angular Z velocities. Linear X is in m/s whereas angular Z is in rad/s.

Know I need to compute what velocities should I set to both wheels in order to meet the linear and angular velocities. I have found this formula:

```<code>velocity_left_cmd = (linear_velocity – angular_velocity * WHEEL_BASE / 2.0)/WHEEL_RADIUS;

velocity_right_cmd = (linear_velocity + angular_velocity * WHEEL_BASE / 2.0)/WHEEL_RADIUS;
</code>```

But it does not seem to be correct. If I put the linear velocity to be 0.5 m/s and angular to be 0.6 rad/s, then having my WHEEL_BASE of 0.14 meters and WHEEL_RADIUS of 0.03 meters, I get very large values, like 15 m/s on one of the wheels. That does not seem to be correct.

What is the right way to get the individual velocities on each wheel?

答:
Your linear velocity should be the average of both wheel values. Assuming there's some wheel radius of WHEEL_RADIUS, as you've stated, then you should get each wheel speed as:

```<code>left_velocity = left_rpm * (RPM_TO_RAD_PER_S * DIST_PER_RAD);
right_velocity = right_rpm * (RPM_TO_RAD_PER_S * DIST_PER_RAD);
linear_velocity = 0.5f * (left_velocity + right_velocity);
</code>```

Units matter and they're important! I define RPM_TO_RAD_PER_S here because you need to convert from motor RPM to radians per second; that conversion is:

$$
\left(\frac{1 \mbox{ rev}}{1 \mbox{ minute}}\right) * \left( \frac{1 \mbox{ minute}}{60 \mbox{ seconds}} \right) * \left(\frac{2\pi \mbox{ radians}}{1 \mbox{ rev}}\right) \\
\left(\frac{1 \require{cancel}\bcancel{\mbox{ rev}}}{1 \require{cancel}\cancel{\mbox{ minute}}}\right) * \left( \frac{1 \require{cancel}\cancel{\mbox{ minute}}}{60 \boxed{\mbox{ seconds}}} \right) * \left(\frac{2\pi \boxed{\mbox{ radians}}}{1 \require{cancel}\bcancel{\mbox{ rev}}}\right) \\
\mbox{RPM_TO_RAD_PER_S} = \frac{2\pi}{60} \ = 0.1047
$$

The DIST_PER_RAD is just the circumference, 2*pi*WHEEL_RADIUS, divided by the number of radians in a complete revolution, which is just 2*pi. This means that DIST_PER_RAD is the same as the WHEEL_RADIUS, but I think it's important to point out why the equation works out the way that it does. Convert RPM to rad/s, and then use that value with the wheel radius to get speed.

Important to note - left_rpm and right_rpm actually need to be in RPM, and DIST_PER_RAD and/or WHEEL_RADIUS needs to be in meters in order to get the output velocities in meters per second.

Moving on to the rotation rate -

Assume for a moment that rotation is positive CCW, viewed from above. If, hypothetically, the robot is rotating CCW, then it would mean that the right wheel is moving some distance further than the left wheel. The difference in distances traveled becomes the arc length scribed by the right wheel (again, relative to the left), and the wheel base, or distance between the wheels, becomes the hypotenuse or radius for the rotation.

Continuing with the assumption, positive is CCW, you would wind up with an equation like:

```<code>angular_rotation = (right_distance - left_distance)/WHEEL_BASE;
</code>```

You can divide through by time to get:

```<code>angular_rate = (right_velocity - left_velocity)/WHEEL_BASE;
</code>```

This is coupled with the linear velocity equation, and you wind up with:

```<code>angular_rate = (right_velocity - left_velocity)/WHEEL_BASE;
linear_velocity = 0.5f * (left_velocity + right_velocity);
</code>```

You can solve for the right_velocity in the angular_rate equation:

```<code>angular_rate = (right_velocity - left_velocity)/WHEEL_BASE;
angular_rate*WHEEL_BASE + left_velocity = right_velocity;
</code>```

And then plug that into the linear velocity equation:

```<code>linear_velocity = 0.5f * (left_velocity + angular_rate*WHEEL_BASE + left_velocity);
</code>```

Then solve for the left_velocity:

```<code>linear_velocity = 0.5f*left_velocity + 0.5f*angular_rate*WHEEL_BASE + 0.5f*left_velocity;
linear_velocity = 0.5f*left_velocity + 0.5f*left_velocity + 0.5f*angular_rate*WHEEL_BASE;
linear_velocity = left_velocity + 0.5f*angular_rate*WHEEL_BASE;
linear_velocity - 0.5f*angular_rate*WHEEL_BASE = left_velocity;

left_velocity = linear_velocity - 0.5f*angular_rate*WHEEL_BASE;
</code>```

And then plug this back into the expression for the right_velocity:

```<code>angular_rate*WHEEL_BASE + left_velocity = right_velocity;
right_velocity = angular_rate*WHEEL_BASE + left_velocity;
right_velocity = angular_rate*WHEEL_BASE + linear_velocity - 0.5f*angular_rate*WHEEL_BASE;
right_velocity = angular_rate*WHEEL_BASE - 0.5f*angular_rate*WHEEL_BASE + linear_velocity;

right_velocity = 0.5f*angular_rate*WHEEL_BASE + linear_velocity;
</code>```

This then gives the equations for left_ and right_velocities in terms of linear velocity and angular rate:

```<code>left_velocity =  linear_velocity - 0.5f*angular_rate*WHEEL_BASE;
right_velocity = linear_velocity + 0.5f*angular_rate*WHEEL_BASE;
</code>```

Now, recall the equations for the conversion from _rpm to _velocity:

```<code>left_velocity = left_rpm * (RPM_TO_RAD_PER_S * DIST_PER_RAD);
right_velocity = right_rpm * (RPM_TO_RAD_PER_S * DIST_PER_RAD);
</code>```

And you can see that:

```<code>left_rpm  = left_velocity/(RPM_TO_RAD_PER_S * DIST_PER_RAD);
right_rpm = right_velocity/(RPM_TO_RAD_PER_S * DIST_PER_RAD);
</code>```

Which, substituting in the _velocity terms, gives:

```<code>left_rpm  = (linear_velocity - 0.5f*angular_rate*WHEEL_BASE)/(RPM_TO_RAD_PER_S * DIST_PER_RAD);
right_rpm = (linear_velocity + 0.5f*angular_rate*WHEEL_BASE)/(RPM_TO_RAD_PER_S * DIST_PER_RAD);
</code>```

This looks similar to what you state from the start, but there's a conversion here from RPM to rad/s. If you wanted this in percentage, then you should use the percentage formula:

```<code>percentage = 100.0f * (value/maximum);
</code>```

I would define a value to be something like:

```<code>RPM_TO_PERCENT = 100.0f/TOP_SPEED_RPM;
</code>```

and then you can do:

```<code>left_percent  = left_rpm * RPM_TO_PERCENT;
right_percent = right_rpm * RPM_TO_PERCENT;
</code>```

You can define any other values you want, too, such as a value that would get you to a PWM output on a 0-255 scale, like:

```<code>RPM_TO_PWM = 255.0f/TOP_SPEED_RPM;
</code>```

and then cast to int on the equation:

```<code>left_pwm =  <int>(left_rpm * RPM*PWM);
right_pwm = <int>(right_rpm * RPM*PWM);
</code>```

You haven't posted any sample data, so I'm assuming this is the answer to your question, at least - that the source of your trouble is a units problem. Don't worry about defining too many variables! I don't think you would ever run into a problem where you're at a limit on how many variables you have defined. Defining the conversion constants like this makes things easier to read, debug, maintain/update, etc.

Body:
Your linear velocity should be the average of both wheel values. Assuming there's some wheel radius of WHEEL_RADIUS, as you've stated, then you should get each wheel speed as:

left_velocity = left_rpm * (RPM_TO_RAD_PER_S * DIST_PER_RAD);
right_velocity = right_rpm * (RPM_TO_RAD_PER_S * DIST_PER_RAD);
linear_velocity = 0.5f * (left_velocity + right_velocity);

Units matter and they're important! I define RPM_TO_RAD_PER_S here because you need to convert from motor RPM to radians per second; that conversion is:

$$
\left(\frac{1 \mbox{ rev}}{1 \mbox{ minute}}\right) * \left( \frac{1 \mbox{ minute}}{60 \mbox{ seconds}} \right) * \left(\frac{2\pi \mbox{ radians}}{1 \mbox{ rev}}\right) \\
\left(\frac{1 \require{cancel}\bcancel{\mbox{ rev}}}{1 \require{cancel}\cancel{\mbox{ minute}}}\right) * \left( \frac{1 \require{cancel}\cancel{\mbox{ minute}}}{60 \boxed{\mbox{ seconds}}} \right) * \left(\frac{2\pi \boxed{\mbox{ radians}}}{1 \require{cancel}\bcancel{\mbox{ rev}}}\right) \\
\mbox{RPM_TO_RAD_PER_S} = \frac{2\pi}{60} \ = 0.1047
$$

The DIST_PER_RAD is just the circumference, 2*pi*WHEEL_RADIUS, divided by the number of radians in a complete revolution, which is just 2*pi. This means that DIST_PER_RAD is the same as the WHEEL_RADIUS, but I think it's important to point out why the equation works out the way that it does. Convert RPM to rad/s, and then use that value with the wheel radius to get speed.

Important to note - left_rpm and right_rpm actually need to be in RPM, and DIST_PER_RAD and/or WHEEL_RADIUS needs to be in meters in order to get the output velocities in meters per second.

Moving on to the rotation rate -

Assume for a moment that rotation is positive CCW, viewed from above. If, hypothetically, the robot is rotating CCW, then it would mean that the right wheel is moving some distance further than the left wheel. The difference in distances traveled becomes the arc length scribed by the right wheel (again, relative to the left), and the wheel base, or distance between the wheels, becomes the hypotenuse or radius for the rotation.

Continuing with the assumption, positive is CCW, you would wind up with an equation like:

angular_rotation = (right_distance - left_distance)/WHEEL_BASE;

You can divide through by time to get:

angular_rate = (right_velocity - left_velocity)/WHEEL_BASE;

This is coupled with the linear velocity equation, and you wind up with:

angular_rate = (right_velocity - left_velocity)/WHEEL_BASE;
linear_velocity = 0.5f * (left_velocity + right_velocity);

You can solve for the right_velocity in the angular_rate equation:

angular_rate = (right_velocity - left_velocity)/WHEEL_BASE;
angular_rate*WHEEL_BASE + left_velocity = right_velocity;

And then plug that into the linear velocity equation:

linear_velocity = 0.5f * (left_velocity + angular_rate*WHEEL_BASE + left_velocity);

Then solve for the left_velocity:

linear_velocity = 0.5f*left_velocity + 0.5f*angular_rate*WHEEL_BASE + 0.5f*left_velocity;
linear_velocity = 0.5f*left_velocity + 0.5f*left_velocity + 0.5f*angular_rate*WHEEL_BASE;
linear_velocity = left_velocity + 0.5f*angular_rate*WHEEL_BASE;
linear_velocity - 0.5f*angular_rate*WHEEL_BASE = left_velocity;

left_velocity = linear_velocity - 0.5f*angular_rate*WHEEL_BASE;

And then plug this back into the expression for the right_velocity:

angular_rate*WHEEL_BASE + left_velocity = right_velocity;
right_velocity = angular_rate*WHEEL_BASE + left_velocity;
right_velocity = angular_rate*WHEEL_BASE + linear_velocity - 0.5f*angular_rate*WHEEL_BASE;
right_velocity = angular_rate*WHEEL_BASE - 0.5f*angular_rate*WHEEL_BASE + linear_velocity;

right_velocity = 0.5f*angular_rate*WHEEL_BASE + linear_velocity;

This then gives the equations for left_ and right_velocities in terms of linear velocity and angular rate:

left_velocity =  linear_velocity - 0.5f*angular_rate*WHEEL_BASE;
right_velocity = linear_velocity + 0.5f*angular_rate*WHEEL_BASE;

Now, recall the equations for the conversion from _rpm to _velocity:

left_velocity = left_rpm * (RPM_TO_RAD_PER_S * DIST_PER_RAD);
right_velocity = right_rpm * (RPM_TO_RAD_PER_S * DIST_PER_RAD);

And you can see that:

left_rpm  = left_velocity/(RPM_TO_RAD_PER_S * DIST_PER_RAD);
right_rpm = right_velocity/(RPM_TO_RAD_PER_S * DIST_PER_RAD);

Which, substituting in the _velocity terms, gives:

left_rpm  = (linear_velocity - 0.5f*angular_rate*WHEEL_BASE)/(RPM_TO_RAD_PER_S * DIST_PER_RAD);
right_rpm = (linear_velocity + 0.5f*angular_rate*WHEEL_BASE)/(RPM_TO_RAD_PER_S * DIST_PER_RAD);

This looks similar to what you state from the start, but there's a conversion here from RPM to rad/s. If you wanted this in percentage, then you should use the percentage formula:

percentage = 100.0f * (value/maximum);

I would define a value to be something like:

RPM_TO_PERCENT = 100.0f/TOP_SPEED_RPM;

and then you can do:

left_percent  = left_rpm * RPM_TO_PERCENT;
right_percent = right_rpm * RPM_TO_PERCENT;

You can define any other values you want, too, such as a value that would get you to a PWM output on a 0-255 scale, like:

RPM_TO_PWM = 255.0f/TOP_SPEED_RPM;

and then cast to int on the equation:

left_pwm =  <int>(left_rpm * RPM*PWM);
right_pwm = <int>(right_rpm * RPM*PWM);

You haven't posted any sample data, so I'm assuming this is the answer to your question, at least - that the source of your trouble is a units problem. Don't worry about defining too many variables! I don't think you would ever run into a problem where you're at a limit on how many variables you have defined. Defining the conversion constants like this makes things easier to read, debug, maintain/update, etc.


问:
How to approach any advanced control problem in robotics
I have worked on a couple of projects like control of a quadrotor/ self balancing robot/ 6 dof robotic arm using nonlinear, optimal and adaptive control. So, in all these projects I have first basically tried to understand the dynamics of the system, then I modeled it. Next I applied different control strategies and compared their responses and chose the best one.

My question is when approaching problems in industry or in research, is the approach for these simple bots fine or are there other things that I should consider.

EDIT:
Can someone who has worked/ has experience in this area, put a step by step approach that should be done to approach any control problem in any field. With the necessary concepts and skills needed.

答:
What all control problems have in common is, that they can be solved with expert knowledge. A quadrotor needs a human expert for flight-dynamics while a pick&place robot needs somebody who is familiar with grasping objects. If it's possible to transfer domain knowledge into software the control problem is solved. The best way in doing so are domain specific languages grounded in a domain. At first, a mindmap is created which contains all the words needed in a domain, then they will be aggregated to a grammar and then a robot-controller can be build on top of this grammar.

The common method in doing so is to use a 3d simulator. Different control problems can be made visible with the same physics engine. On top of the physics engine a motion controller can be tested and improved interactively. The overall framework in doing so is called a distributed version control system. That means, an engineer can commit to the repository his newly created domain specific knowledge, and another engineer can run a test on it. Such projects are driven by requirements, formulated from the outside of the system, for example a robot competition about a UAV which has to avoid obstacles.