问:
How to derive the Time-Update equation of SLAM
I was going through the tutorial on SLAM by Hugh Durrant-Whyte and Tim Bailey† and found this Time-Update equation (equation 4 in the paper):

$
P\left(\mathrm{x}_{k},\mathrm{m} | \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k},\mathrm{x}_{0}\right) = \int P\left(\mathrm{x}_{k}|\mathrm{x}_{k-1},\mathrm{u}_{k}\right) \times P\left(\mathrm{x}_{k-1},\mathrm{m}|\mathrm{Z}_{0:k-1},\mathrm{U}_{0:k-1},\mathrm{x}_{0}\right)\mathrm{dx}_{k-1}$

where $\mathrm{m}$ is the position of landmarks or one can call it a map and other symbols have usually meanings like $\mathrm{x}_{k}$ is robot location, $\mathrm{u}_{k}$ is control inputs and $\mathrm{z}_{k}$ world observations made by robot. Is the right hand side derived using the total probability concept. I am aware of the Markov facts that $\mathrm{x}_{k}$ depends on $\mathrm{u}_{k}$ and $\mathrm{x}_{k-1}$ and $\mathrm{z}_{k}$ depends on $\mathrm{x}_{k}$ and $\mathrm{m}$. Still I am not able to derive the right hand side from the left hand side specially the fact that $\mathrm{m}$ was occurring with $\mathrm{x}_{k}$ in the joint probability on left hand side but it gets attached with $\mathrm{x}_{k-1}$ on the right hand side if I think we are using the fact

$P(a,b|c) = \int_{a'} P(a,b|a',c)P(a'|c) da'$

†Simultaneous Localisation and Mapping (SLAM): Part I The Essential Algorithms

答:
Our goal is to find a recursive expression for $ P\left(\mathrm{x}_{k},\mathrm{m} | \mathrm{Z}_{0:k}, \mathrm{U}_{0:k},\mathrm{x}_{0}\right)$. This expression is called the belief for the robot's state $\mathrm{x}_{k}$ and the map $\mathrm{m}$ given all the measurements $\mathrm{Z}_{0:k} = (\mathrm{z}_{0},..., \mathrm{z}_{k})$, the control actions $\mathrm{U}_{0:k} = (\mathrm{u}_{0},..., \mathrm{u}_{k})$ and the robot's initial state $\mathrm{x}_{0}$.

First, let us write the down the Markov assumptions:

Motion model: $P\left( \mathrm{x}_{k} | \mathrm{x}_{k-1}, \mathrm{m}, \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right) = P\left(\mathrm{x}_{k} | \mathrm{x}_{k-1}, \mathrm{u}_{k} \right)$

Observation model: $P\left( \mathrm{z}_{k} | \mathrm{x}_{k}, \mathrm{m}, \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right) = P\left( \mathrm{z}_{k} | \mathrm{x}_{k}, \mathrm{m} \right) $

By using the Bayes' rule, we can pull $z_{k}$ to the left of "$|$":

$$ P\left(\mathrm{x}_{k},\mathrm{m} | \mathrm{Z}_{0:k}, \mathrm{U}_{0:k},\mathrm{x}_{0}\right) = \eta P\left( \mathrm{z}_{k} | \mathrm{x}_{k}, \mathrm{m}, \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right) P\left( \mathrm{x}_{k}, \mathrm{m}, \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right) $$

By using the Markov assumption regarding the observation model, we can simplify the term $P\left( \mathrm{z}_{k} | \mathrm{x}_{k}, \mathrm{m}, \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right)$:

$$ P\left(\mathrm{x}_{k},\mathrm{m} | \mathrm{Z}_{0:k}, \mathrm{U}_{0:k},\mathrm{x}_{0}\right) = \eta P\left( \mathrm{z}_{k} | \mathrm{x}_{k}, \mathrm{m} \right) P\left( \mathrm{x}_{k}, \mathrm{m}, \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right) $$

By using the total probability theorem, we can rewrite the term $P\left( \mathrm{x}_{k}, \mathrm{m}, \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right) $ as a marginalization integral over $\mathrm{x}_{k-1}$:

$$ P\left(\mathrm{x}_{k},\mathrm{m} | \mathrm{Z}_{0:k}, \mathrm{U}_{0:k},\mathrm{x}_{0}\right) = \eta P\left( \mathrm{z}_{k} | \mathrm{x}_{k}, \mathrm{m} \right) \int P\left( \mathrm{x}_{k}, \mathrm{m}, \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0}, \mathrm{x}_{k-1} \right) \mathrm{dx}_{k-1} $$

By using the Bayes' rule, we can rewrite the term $P\left( \mathrm{x}_{k}, \mathrm{m}, \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0}, \mathrm{x}_{k-1} \right)$ in terms of conditional probability:

$$ P\left(\mathrm{x}_{k},\mathrm{m} | \mathrm{Z}_{0:k}, \mathrm{U}_{0:k},\mathrm{x}_{0}\right) = \eta P\left( \mathrm{z}_{k} | \mathrm{x}_{k}, \mathrm{m} \right) \int P\left( \mathrm{x}_{k} | \mathrm{x}_{k-1}, \mathrm{m}, \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right) P\left( \mathrm{x}_{k-1}, \mathrm{m}, \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right)  \mathrm{dx}_{k-1} $$

By using the Markov assumption regarding the motion model, we can simplify the term $P\left( \mathrm{x}_{k} | \mathrm{x}_{k-1}, \mathrm{m}, \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right)$:

$$ P\left(\mathrm{x}_{k},\mathrm{m} | \mathrm{Z}_{0:k}, \mathrm{U}_{0:k},\mathrm{x}_{0}\right) = \eta P\left( \mathrm{z}_{k} | \mathrm{x}_{k}, \mathrm{m} \right) \int P\left( \mathrm{x}_{k} | \mathrm{x}_{k-1}, \mathrm{u}_{k} \right) P\left( \mathrm{x}_{k-1}, \mathrm{m}, \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right)  \mathrm{dx}_{k-1} $$

By using the Bayes' rule, we can rewrite the term $P\left( \mathrm{x}_{k-1}, \mathrm{m}, \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right) $ in terms of conditional probability:

$$ P\left(\mathrm{x}_{k},\mathrm{m} | \mathrm{Z}_{0:k}, \mathrm{U}_{0:k},\mathrm{x}_{0}\right) = \eta P\left( \mathrm{z}_{k} | \mathrm{x}_{k}, \mathrm{m} \right) \int P\left( \mathrm{x}_{k} | \mathrm{x}_{k-1}, \mathrm{u}_{k} \right) P\left( \mathrm{x}_{k-1}, \mathrm{m} | \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right) P\left( \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right)\mathrm{dx}_{k-1} $$

By using the fact that $(\mathrm{x}_{k-1}, \mathrm{m})$ is independent from the future action $\mathrm{u}_{k}$, we can simplify the term $P\left( \mathrm{x}_{k-1}, \mathrm{m} | \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right)$:

$$ P\left(\mathrm{x}_{k},\mathrm{m} | \mathrm{Z}_{0:k}, \mathrm{U}_{0:k},\mathrm{x}_{0}\right) = \eta P\left( \mathrm{z}_{k} | \mathrm{x}_{k}, \mathrm{m} \right) \int P\left( \mathrm{x}_{k} | \mathrm{x}_{k-1}, \mathrm{u}_{k} \right) P\left( \mathrm{x}_{k-1}, \mathrm{m} | \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k-1}, \mathrm{x}_{0} \right) P\left( \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right) \mathrm{dx}_{k-1} $$

The term $P\left( \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k}, \mathrm{x}_{0} \right)$ is a constant within the integral. It can be taken out and fused into $\eta$ coefficient:

$$ P\left(\mathrm{x}_{k},\mathrm{m} | \mathrm{Z}_{0:k}, \mathrm{U}_{0:k},\mathrm{x}_{0}\right) = \eta P\left( \mathrm{z}_{k} | \mathrm{x}_{k}, \mathrm{m} \right) \int P\left( \mathrm{x}_{k} | \mathrm{x}_{k-1}, \mathrm{u}_{k} \right) P\left( \mathrm{x}_{k-1}, \mathrm{m} | \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k-1}, \mathrm{x}_{0} \right) \mathrm{dx}_{k-1} $$

Finally, the integral expression, that computes the belief before incorporating the measurement $\mathrm{z}_{k}$, is the time update equation (aka prediction step):

$$ P\left(\mathrm{x}_{k},\mathrm{m} | \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k},\mathrm{x}_{0}\right) = \int P\left( \mathrm{x}_{k} | \mathrm{x}_{k-1}, \mathrm{u}_{k} \right) \times P\left( \mathrm{x}_{k-1}, \mathrm{m} | \mathrm{Z}_{0:k-1}, \mathrm{U}_{0:k-1}, \mathrm{x}_{0} \right) \mathrm{dx}_{k-1} $$


问:
Reward Function for q learning on a robot
I have 2 wheeled differential drive robot which I use pid for low level control to follow line. I implemented q learning which uses samples for 16 iterations then uses them to decide the best position to be on the line so car takes the turn from there. This allows PID to setup and smooth fast following. My question is how can I setup a reward function that improves the performance i.e. lets the q learning to find the best

Edit

What it tries to learn is this, it has 16 inputs which contains the line positions for the last 15 iterations and this iteration. Line position is between -1 and 1 which -1 means only left most sensor sees the line and 0 means the line is in the center. I want it to learn a line position that when it faces this input again it will set that line position like its the center and take the curve according to that line position. For example error is required position - line position so let say I had 16 0 as input then I calculated the required as 0.4. So after that the car will center itself at 0.4 I hope this helps :)

You asked for my source code i post it below

```<code>void MainController::Control(void){

    float linePosition = sensors->ReadSensors();

    if(linePosition == -2.0f){
      lost_line->FindLine(lastPos[1] - lastPos[0]);
    }
    else{
        line_follower->Follow(linePosition);
        lastPos.push_back(linePosition);
        lastPos.erase(lastPos.begin());
    }
}
</code>```

My Sensor reading returns a value between -1.0f and 1.0f. 1.0f means Outer Sensor on the right is only the line. I have 8 sensors.

```<code>void LineFollower::Follow(float LinePosition){

    float requiredPos = Qpredictor.Process(LinePosition,CurrentSpeed);
    float error = requiredPos - LinePosition;

    float ErrorDer = error -LastError;

    float diffSpeed = (KpTerm * error + (KdTerm * ErrorDer));

    float RightMotorSpeed = CurrentSpeed - diffSpeed;
    float LeftMotorSpeed = CurrentSpeed + diffSpeed;

    LastError = error;

    driver->Drive(LeftMotorSpeed,RightMotorSpeed);
}
</code>```

Here is the logic for the value for QPredictor(I call the learning part as this). And Finally QPredictor

```<code>float Memory[MemorySize][DataVectorLength] =
{
  {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
  {0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3},
  {0.6, 0.6, 0.6, 0.6, 0.6, 0.6, 0.6, 0.6, 0.6, 0.6, 0.6, 0.6, 0.6, 0.6, 0.6, 0.6},
  {0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8},

  {0.000, 0.012, 0.050, 0.113, 0.200, 0.312, 0.450, 0.613, 0.800, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.012, 0.050, 0.113, 0.200, 0.312, 0.450, 0.613, 0.800, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.012, 0.050, 0.113, 0.200, 0.312, 0.450, 0.613, 0.800, 1.000, 1.000, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.000, 0.012, 0.050, 0.113, 0.200, 0.312, 0.450, 0.613, 0.800, 1.000, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.000, 0.000, 0.012, 0.050, 0.113, 0.200, 0.312, 0.450, 0.613, 0.800, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.012, 0.050, 0.113, 0.200, 0.312, 0.450, 0.613, 0.800, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.012, 0.050, 0.113, 0.200, 0.312, 0.450, 0.613, 0.800, 1.000},

  {0.000, 0.025, 0.100, 0.225, 0.400, 0.625, 0.900, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000},

  {0.000, 0.050, 0.200, 0.450, 0.800, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000},

  {0.000, 0.100, 0.400, 0.900, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.100, 0.400, 0.900, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.100, 0.400, 0.900, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.000, 0.100, 0.400, 0.900, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.000, 0.000, 0.100, 0.400, 0.900, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.100, 0.400, 0.900, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.100, 0.400, 0.900, 1.000, 1.000, 1.000, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.100, 0.400, 0.900, 1.000, 1.000, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.100, 0.400, 0.900, 1.000, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.100, 0.400, 0.900, 1.000, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.100, 0.400, 0.900, 1.000, 1.000},
  {0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.100, 0.400, 0.900, 1.000}
};

QPredictor::QPredictor(){
    for(int i=0;i<MemorySize;i++){
        output[i]=0.0f;
        input[i]=0.0f;
    }

    state = 0;
    PrevState = 0;
}

float QPredictor::Process(float linePosition,float currentBaseSpeed){
    for(int i=1;i<DataVectorLength;i++){
        input[i] = input[i-1];
    }

    input[0] = m_abs(linePosition);

    int MinIndex = 0;
    float Distance = 10000.0f;

    float sum = 0.0f;

    for(int i=0;i<MemorySize;i++){
        sum = 0.0f;
        for(int j=0;j<DataVectorLength;j++){
            sum +=m_abs(input[j] - Memory[i][j]);
        }

        if(sum <= Distance){
            MinIndex = i;
            Distance = sum;
       }
    }

    sum = 0.0f;
    for(int i=0;i<DataVectorLength;i++){
        sum += input[i];
    }

    float eta = 0.95f;

    output[MinIndex] = eta * output[MinIndex] + (1 - eta) * sum;

    return -m_sgn(linePosition) * output[MinIndex];
}

float QPredictor::rewardFunction(float *inputData,float currentBaseSpeed){
    float sum = 0.0f;

    for(int i=0;i<DataVectorLength;i++){
        sum += inputData[i];
    }

    sum /= DataVectorLength;

    return sum;
}
</code>```

I now only have average Error and currently not using learning because it's not complete without reward function. How can I adjust it according to the dimensions of my Robot?

答:
I think Q-learning is an overkill for such a simple tracking problem. I highly recommend you using a simpler method, namely pure pursuit tracking controller. Here are some links that you can read about the method. Matlab's page on the subject is pretty neat.

Matlab's Pure Pursuit Controller

Implementation of the Pure Pursuit Path Tracking Algorithm

ContentLicense: CC BY-SA 3.0

ContentLicense: CC BY-SA 4.0


问:
360 Continuous Rotation Servo only goes 180 degrees
I bought the following servo: D DOLITY High Speed 360 Degree Continuous Rotation Servo for Robotic(15kg/cm) Metal Gears Servo

I'm trying to get it to spin in one direction continuously, but it looks like it can only go 180 degrees and then stops.

Is there something wrong with my code that is making it stop at 180 degrees or is the product falsely advertised as a 360° continuous rotation servo?

This is the Arduino code I'm using:

```<code>Servo myservo;  //Servo name is myservo

void setup() {
  Serial.begin(9600);
  myservo.attach(9);  // attaches the servo signal pin on pin D6
}

void loop() {
  Serial.println("0");// You can display on the serial the signal value
  myservo.write(0); //Turn clockwise at high speed
}
</code>```

答: Since you already determined that the servo acts as a positional servo, and not as a continuous rotation servo, then it is not a continuous rotation servo.</p>


问:
Understanding the SICK S300 RS422 data output
I'm using a SICK S300 safety scanner connected via RS422 -> USB and I'm trying to decode the lines of data into meaningful values, however looking at the data compared to the Telegram Listing it seems almost impossible due to the many differing values. Has anyone done this before?

I've tried using ROS and found difficulties doing so, especially as the package for the S300 doesn't support reflectors, which would be preffered.

答: According to the SICK 300 driver in ROS, your endpoint data will be a sensor_msgs/LaserScan ROS message, which you could reach to its values such as ranges[] and intensities[] by the specified topic.


问:
What exactly is a nodelet?
I am new to ROS. I tried to read the documentation regarding nodelets on ROS wiki. But I couldn't understand it very well. I would like to know what is a nodelets in simple and intuitive terms</p>

答:
TL;DR

nodelet is a package for fast communication (based on shared memory heap) instead of using ROS TCP communication.

This image demonstrates their differences:

Title: What exactly is a nodelet?


答:
The source code for decoding the telegram coming from the serial interface can be found here.

The actual decoding is in the getScan() function at line 160. after identifying the required part of the telegram (which as it seems, comes form multiple readings of the serial port), the scan data is converted to Polar coordinates using the convertScanToPolar() function in line 196

Title: Understanding the SICK S300 RS422 data output

Body:
I'm using a SICK S300 safety scanner connected via RS422 -> USB and I'm trying to decode the lines of data into meaningful values, however looking at the data compared to the Telegram Listing it seems almost impossible due to the many differing values. Has anyone done this before?

I've tried using ROS and found difficulties doing so, especially as the package for the S300 doesn't support reflectors, which would be preffered.


答:
To answer your question. Yes, myself and others have done this before.

In practice you only need a subset of the telegrams for a given application.

I think you would be surprised how quickly you can write what you need if you sit down and start coding.

If you really get into it and handle all the telegrams, and are able to open the source code, I'm sure the community would appreciate it.


问:
Unlabelled pins on this L293D dual DC motor driver
I've got this board from a local store. It's an L293D based dual DC motor driver. All the pins are labelled except the 5 on the bottom as shown in the picture below. Couldn't find any datasheet or manual for using this board.

I've connected the board's Vcc to Arduino's 5V and board's ground to Arduio's GND. Then put the IN1 to HIGH and IN2 to low, and got a voltage of 4.3 across the motor A's terminals.

Now what I want to do is use an external power source (11.1V battery) for the motor terminals, NOT the Arduino's 5V.

Are there any datasheet/manual for this board?

How do I use external power source with this board?

What are the pin-out/labels for those 5 pins annotated with a red
box in the picture above?

答:
A Google Image search on L293D board suggests that this is a widely available board manufactured by (or at least designed by) Deek-Robot.

The 5 pin connector mostly duplicates pins on the 6 pin green terminal block:

I suspect it is mostly useful for a quick motor connection when testing, whereas the green connector is used for a final motor connection.

Note that the Deek-Robot product page includes a link to the (very simple) circuit diagram and a link to the L293D datasheet.

Also, note that this chip can get very hot when pushed close to it's limits. 4 of the 16 pins are designed to be heat sinks to the ground plane, but it looks like this board doesn't have a very good thermal connection, so you may want to consider attaching a chip heatsink to the top side of the chip with thermal glue if it gets too hot. I've even known people to solder the heatsink to the chip via the heatsink pins as the plastic chip packaging is quite thermally insulating.


问:
How to suck using vacuum only first sheet of paper from pack of paper?
We are designing robot that will pick and place sheets of paper. We have found an issue that it is hard to suck only first sheet of paper of paper pack. Issue occurs because paper sheet passes vacuum through it and vacuum sucks additional (2 - 3) sheets of paper.

How to resolve this? Maybe we are using too small suction cups or incorrect design?

Current pneumatic system contains following:
1) Vacuum pump that produces 120 kpa vacuum
2) SGPN 20 FPM-65 suction cups 20 mm in diameter for paper handling
3) Need to hold standard office paper A4 80 gsm (210 x 297 mm)
4) Distance between suction cups is 100 x 125 mm
5) Paper pack could contain at least 30 sheets of paper.

Current experiments results.
1) 1 suction cup easely holds 5 sheets of paper because vacuum is flowing through paper
2) if pressure is decreased to minimum its stops sucking or sucks 3 sheets

Solution which we are trying to get is shown on this video:
https://www.youtube.com/watch?v=FT7NWbXyM7Q

Similar issue that we are getting is shown on this video:
https://www.youtube.com/watch?v=LKBDzkm3moo

Maybe somebody have other ideas how to make it possible to suck only first sheet of paper without mechanical holding of it?

答:
I think the primary problem you are seeing here is that you are trying to pick up ordinary paper, which is relatively porous. When I was working with suction cup systems we were dealing primarily with labels, so non porous, waxed backing paper. Our page turning systems used rubberised rollers rather than suction cups, even though we used pneumatics elsewhere in the page turning mechanism.

As the two videos show, with the waxed paper only one sheet at once is usually picked up, whereas with the more porous tissue, several are usually picked up. In fact the vacuum is so strong that with only a single tissue, the demonstrator was having trouble separating the suction cup from the table below.

I agree with morbo that your system has way more suction than it needs, but I think it's probably easier to find the right balance experimentally than to try to work out from first principals, with all of the variables that aren't accounted for by the math. I think you need to get a stack of paper and your actuator (and possibly some alternatives) and find the limits of your system.

You need to be able to control the vacuum

Without being able to control the vacuum you are going to have a hard time controlling the behaviour of the system.

As in the videos, you need to be able to see what the vacuum pressure is when picking up and holding the paper, so you need a relatively accurate vacuum gauge.

Depending on your characterisation of the system, you may only need variable vacuum control while performing that characterisation, but it may be that you need to vary the vacuum dynamically during different phases of your pick & place operation. Rather than just on/off you may need low vacuum to pick and lift a single sheet, followed by a switch to high vacuum befor a fast move to the place location without dropping the paper.

How a variable vacuum is achieved is beyond the scope of this answer, but may involve a variable vacuum source, a variable regulator, a bypass or some king of pulsed source or bypass. The specifics are beyond my expertise. What I do know is that you need to characterise the behaviour of your system.

Characterise the behaviour of your suction cups

Starting from a pressure too low to pick up any paper steadily increase the vacuum until it picks up one sheet. Then keep increasing the vacuum until it picks up two sheets. The vacuum at the mid point between the two should give you a good quick compromise between holding a single sheet securely and the risk of picking up a second sheet. Depending on the reliability you need though (99%, 99.9%, 5 nines) you may need to characterise your system more thoroughly.

Repeat a pick-up attempt at each pressure enough times to build up confidence in how much variation there is in results. If vacuum $x$ picks up 1 sheet 99% of the time and never picks up 2 sheets, while vacuum $y$ picks up 1 sheet every time, and 2 sheets 1% of the time you can surmise that you have a well characterised system and can probably rely on the midpoint to pretty reliably only pick up 1 sheet.

If vacuum $x$ picks up 1 sheet 80% of the time and 2 sheets 20% of the time, while vacuum $y$ picks up 1 sheet 20% of the time, and 2 sheets 80% of the time, then you may have to consider whether this is the right solution for you.

Check it behaves the same at the extremes

It may also be worth checking if behaviour is different when the stack of paper is full compared to when it is empty. If not, then you may have to chose a different compromise point to maximise the reliability over the whole paper refill lifetime. Alternatively, you may have to calculate vacuum pressures at the pick location depending on the number of sheets in the stack (if you can't mechanically compensate for the stack height, as printers do).

Finally, you may want to try running the same characterisation with 70gsm and 90gsm paper as well as at 80gsm. While there is little variation in commercially bought paper, you can never rule out customers choosing to buy cheaper thinner paper, or decide they need to use heavier paper stock for some jobs.

In my experience it's better to know about likely problems before they occur than have to deal with them in the middle of the night.

Frantic Customer: We filled the machine & it started picking up 2 sheets at once!

Bleery eyed Engineer: What did you change?

Apologetic customer: Oh, this job needed 70gsm paper

Resigned engineer: Ok, go into the .ini file and reduce the vacuum value by 20% then on Monday put in a change request to implement 70gsm paper support in the GUI

Conclusion

Reliability only comes from testing, and testing again after every adjustment you make. Data gives you the ability to reason about the behaviour of your system, predict what changes will improve or impact on reliability and test hypotheses until you get what behaviour you need, or decide you need to a different solution.


问:
Free-floating sphere dynamics using Roy Featherstone's spatial_v2 toolbox
I am using Roy Featherstone's spatial_v2 to model a mobile robot. I encountered a problem so I went back to modeling a free-floating sphere to familiarize myself with the library and the algorithms.

I have an error when I model the sphere in free fall in the zy plane while spinning around x. I am using the FDfb function for the forward dynamics as the sphere is a free-floating object.

The spatial velocity, in fixed based coordinates, is set to :
$$v = \begin{bmatrix} -5\\ 0\\ 0\\ 0\\ -1 \\-1 \end{bmatrix}$$
And being in free fall and with no centrifugal force, the spatial acceleration should be, in fixed based coordinates :

$$a = \begin{bmatrix} 0\\ 0\\ 0\\ 0\\ 0 \\-9.8066 \end{bmatrix}$$
But the one returned by the FDfb function is, in fixed based coordinates :
$$a = \begin{bmatrix} 0\\ 0\\ 0\\ 0\\ 5\\-14.8066 \end{bmatrix}$$

I guess Featherstone's library has been sufficiently tested so it must be my inputs. So what is wrong with my inputs ? Could someone else test it and post the results ?

sphere.NB     = 1;
sphere.parent = 0;
sphere.jtype  = {'R'};
sphere.Xtree  = {eye(6)};
% Inertia
M = 0.2;
C = [0 0 0];
J = diag([0.0002 0.0002 0.0002]);
sphere.I = {mcI( M, C, J )};
sphere.gravity = [0 0 -9.8066]';
% To Floating base
sphere = floatbase(sphere);
% Simulation parameters
vv_0  = [-5 0 0 0 -1 -1]';
xfb = [rq(eye(3)); [0 0 0]'; vv_0];

% Forward Dynamic
[xdfb] = FDfb(sphere, xfb, [], [], [])
```

I hope one of you can help me understand. Thanks !

Titouan

答:
You still haven't posted the (full) code that gives the results you've presented; when I run your snippet I don't the results you posted. Instead, I get:

```<code>[xdfb] = FDfb(sphere, xfb, [], [], [])

xdfb =

         0
   -2.5000
         0
         0
         0
   -1.0000
   -1.0000
         0
         0
         0
         0
    5.0000
  -14.8066
</code>```

Here xdfb is the time-derivative of your input system sphere and the initial conditions xfb. It's a 13-element vector made up of the derivative of the things that comprise xfb, which means:

You have a quaternion derivative,

```<code>             0
       -2.5000
             0
             0
</code>```

You have a Cartesian velocity, and

```<code>             0
       -1.0000
       -1.0000
</code>```

You have a spatial acceleration.

```<code>             0
             0
             0
             0
        5.0000
      -14.8066
</code>```

Note that here the spatial acceleration I have is not what you've presented in your question, so I'm not positive that we're doing the same thing.

You can use constant-acceleration equations to calculate future positions. For your y-values, there's no acceleration, so:

$$
y = y_0 + v_y \Delta t + \frac{1}{2} a_y \Delta t^2 \\
$$

becomes

$$
y = y_0 + v_y \Delta t \\
$$

For your starting position of $y_0 = 0$ and starting speed of $v_y = -1$, after 30 seconds you should have:

$$
y = 0 + (-1)*30 \\
$$

for the trivial result of $y = -30$.

For z, you have a constant acceleration of gravity and a starting speed of -1, so you should have:

$$
z = 0 + (-1)*30 + (0.5)*(-9.8066)*(30^2) \\
$$

for a result of $z = -4,443$.

If I run the following after running your code:

```<code>t0 = 0;
tMax = 30;
dT = 0.001;
time = t0:dT:tMax;

nSamples = numel(time);

pos = zeros(3,nSamples);

for i=1:nSamples
    % Forward Dynamic
    [xdfb] = FDfb(sphere, xfb, [], [], []);
    q = xfb(1:4);
    dq = xdfb(1:4);

    r = xfb(5:7);
    v = xdfb(5:7);

    v_old = xfb(8:13);
    a = xdfb(8:13);

    q = q + dq*dT;
    q = q./(norm(q)); % re-normalize the unit quaternion
    r = r + v*dT;
    v = v_old + a*dT;

    pos(:,i) = r;
    xfb = [q;r;v];
end
plot3(pos(1,:),pos(2,:),pos(3,:));
</code>```

Then I get:

```<code>>> pos(:,end)

ans =

   1.0e+03 *

         0
   -0.0300
   -4.4431
</code>```

There's a slight difference in the z-value from integration/rounding errors, but the y-value is correct. There's no problem in the output.

I think the issue for you is kind of the same issue I had with the spatial vector formulation - it was really hard for me to conceptualize. I studied Featherstone's method for a while on my own and eventually gave up because of difficulties I had in trying to implement anything based on his work. I didn't (don't) have any formal classroom training on his spatial maths and so didn't have anyone to ask for help and didn't have the ability to get feedback on whether anything I was doing was correct or not.

You've given the sphere a rotational speed of -5 about the x-axis, and this is how you're getting a y-axis term in the spatial acceleration.

Frankly speaking, I've never had any education with regards to screw theory, Plücker coordinates, etc., so again I don't have any intuition here to be able to illuminate anything more for you. I'd like to say it's Coriolis forces or some other fictional force, but it's all a guess for me.

You can look at the source code for FDfb and see the conversion from your spatial velocities to local floating-body velocities. When I plot them, everything looks as expected - the sphere is spinning, so I see the speed oscillating between y- and z-axes, and it's in free-fall, so I see the magnitude of speed (on both axes) increasing.

So, tl;dr - I can't explain how the spatial velocities or accelerations are working, but the orientations and positions should all be coming out correct.

Body:
You still haven't posted the (full) code that gives the results you've presented; when I run your snippet I don't the results you posted. Instead, I get:

[xdfb] = FDfb(sphere, xfb, [], [], [])

xdfb =

         0
   -2.5000
         0
         0
         0
   -1.0000
   -1.0000
         0
         0
         0
         0
    5.0000
  -14.8066

Here xdfb is the time-derivative of your input system sphere and the initial conditions xfb. It's a 13-element vector made up of the derivative of the things that comprise xfb, which means:

You have a quaternion derivative,

             0
       -2.5000
             0
             0

You have a Cartesian velocity, and

             0
       -1.0000
       -1.0000

You have a spatial acceleration.

             0
             0
             0
             0
        5.0000
      -14.8066

Note that here the spatial acceleration I have is not what you've presented in your question, so I'm not positive that we're doing the same thing.

You can use constant-acceleration equations to calculate future positions. For your y-values, there's no acceleration, so:

$$
y = y_0 + v_y \Delta t + \frac{1}{2} a_y \Delta t^2 \\
$$

becomes

$$
y = y_0 + v_y \Delta t \\
$$

For your starting position of $y_0 = 0$ and starting speed of $v_y = -1$, after 30 seconds you should have:

$$
y = 0 + (-1)*30 \\
$$

for the trivial result of $y = -30$.

For z, you have a constant acceleration of gravity and a starting speed of -1, so you should have:

$$
z = 0 + (-1)*30 + (0.5)*(-9.8066)*(30^2) \\
$$

for a result of $z = -4,443$.

If I run the following after running your code:

t0 = 0;
tMax = 30;
dT = 0.001;
time = t0:dT:tMax;

nSamples = numel(time);

pos = zeros(3,nSamples);

for i=1:nSamples
    % Forward Dynamic
    [xdfb] = FDfb(sphere, xfb, [], [], []);
    q = xfb(1:4);
    dq = xdfb(1:4);

    r = xfb(5:7);
    v = xdfb(5:7);

    v_old = xfb(8:13);
    a = xdfb(8:13);

    q = q + dq*dT;
    q = q./(norm(q)); % re-normalize the unit quaternion
    r = r + v*dT;
    v = v_old + a*dT;

    pos(:,i) = r;
    xfb = [q;r;v];
end
plot3(pos(1,:),pos(2,:),pos(3,:));

Then I get:

>> pos(:,end)

ans =

   1.0e+03 *

         0
   -0.0300
   -4.4431

There's a slight difference in the z-value from integration/rounding errors, but the y-value is correct. There's no problem in the output.

I think the issue for you is kind of the same issue I had with the spatial vector formulation - it was really hard for me to conceptualize. I studied Featherstone's method for a while on my own and eventually gave up because of difficulties I had in trying to implement anything based on his work. I didn't (don't) have any formal classroom training on his spatial maths and so didn't have anyone to ask for help and didn't have the ability to get feedback on whether anything I was doing was correct or not.

You've given the sphere a rotational speed of -5 about the x-axis, and this is how you're getting a y-axis term in the spatial acceleration.

Frankly speaking, I've never had any education with regards to screw theory, Plücker coordinates, etc., so again I don't have any intuition here to be able to illuminate anything more for you. I'd like to say it's Coriolis forces or some other fictional force, but it's all a guess for me.

You can look at the source code for FDfb and see the conversion from your spatial velocities to local floating-body velocities. When I plot them, everything looks as expected - the sphere is spinning, so I see the speed oscillating between y- and z-axes, and it's in free-fall, so I see the magnitude of speed (on both axes) increasing.

So, tl;dr - I can't explain how the spatial velocities or accelerations are working, but the orientations and positions should all be coming out correct.


问:
Calculate ticks with three encoders?
I have a question regarding calculating the ticks in a sensored BLDC. The BLDC has three hall effect sensors with slightly differing phasing and there are 30 magnets in 360 degrees of wheel rotation. I have seen coding examples where two hall effect sensors are used to calculate the ticks by incrementing or decrementing depending of the value of both signals. For example this:

https://github.com/1988kramer/motor_control/blob/master/Encoder.cpp

Right now, I’m just using the signal of two hall effect sensors to calculate the ticks and seems to be working fine.

My question is: Should I use the three signals? Or it is enough with two to know when to decrement or increment the ticks.

答: To measure position more accurately you need to use all three. They are used for choosing which motor phase to energise, which is called commutation. Searching for "6 step commutation" will give you lots of diagrams and info on how to estimate position this way.</p>

Title: Calculate ticks with three encoders?


问:
How does sensor fusion related to SLAM?
How does sensor fusion affect the SLAM process? If we have a stereo camera with GPS and IMU, how does sensor fusion benefit the SLAM?
Do we use sensor fusion to enhance the localization phase of SLAM, and then the map generation becomes more accurate?</p>

答:
Sensor fusion is a process where you are gathering information from one or more sensors and inputting that data to your processor to process the data to make sense whether it is useful or not.

In your case, you have a stereo camera, IMU and GPS. If your question is whether all of these sensors altogether will increase the accuracy of the map, then the answer is no. The stereo camera alone can produce good quality 3D point-clouds/maps, adding the IMU and GPS is not going to increase the accuracy of the quality of the data gathered from the stereo cam. But the GPS and IMU along with the stereo cam will help you increase the accuracy of the localisation.

It also depends on what sensors you are using and their quality.

TIP: If you are using a stereo camera on your robot/drone and planning to put in dark environments, or places with low light, add a few lights to light up the area which is being scanned by the stereo camera.

Title: How does sensor fusion related to SLAM?


问:
Is a thrift store a good place to get a servo motor?
I'm trying to learn about a very basic motor, the servo motor.  Can these be found a thrift stores like Goodwill in old toys?  Are these "robotic" quality?  What toys or other kinds of things would I scavenge?  All I want to do is get a motor.  After that I want an Arduino and make it "work."  Nothing complex.</p>

答:
A servo is not a basic motor - it's a motor plus some means to control its position. If you want DC motors. A small servo costs around £1.75/$2 new for 1-10 quantities.  Servos have three wires going to them - ground, power, and a pulse width modulation signal to control them, which is fairly easy to generate with a microcontroller.

Plain DC motors are electrically simpler - you provide power as DC and they move one way or another depending on polarity - but require more driving electronics as a microcontroller cannot provide enough current. So you would need either an H-bridge to drive it both ways, or a power transistor to drive it one direction only. A small low-quality DC motor costs about a pound, a higher quality motor (such as you'd get in a model railway locomotive) ten or a few tens of pounds. An H-bridge to drive it, such as L298 costs a couple of pounds, so if you want bidirectional drive it's probably worth buying a better motor.

The prices above are for hobby kit; industrial robotics motors are typically ten times more expensive, so if you mean industrial by 'robotic quality' then it's unlikely you'll find something in a second hand store.

Buying scrap equipment and salvaging the motors from goodwill is unlikely to give you better quality or consistency than buying cheap motors new. You may get lucky and be able to buy a toy with a few hobby quality servos in it for less than $$2 per servo, or you may have a project in mind which needs a higher power motor and so buy something like a sewing machine for less than the cost of an equivalent motor (~$20), but unless you're lucky it's probably not worth the candle.

So be lucky!


问:
Understanding the kinetic energy expressions for a 1 link vs 2 link robot arm
I'm studying these lecture notes on dynamic models of robot arm links, slides 33-36, where two examples are given for the kinetic energies for a single link and two link robot arm.

In the single link case, the kinetic energy is posed as:

$$K=\frac{1}{2}I\dot{\theta}^2$$

This appears to account for the rotational kinetic energy of the single link around the joint.

In the two link case, the kinetic energy of the first link is posed as:

$$K=\frac{1}{2}I_1\dot{\theta_1}^2 + \frac{1}{2}m_1a_{c1}^2\dot{\theta_1}^2$$

This appears to mean that the total kinetic energy in the first link of the two link chain contains a contribution from rotation around the first joint ($m_1a_{c1}^2$ term) and from rotation around the center of mass ($I_1$ term).

Comparing the two link and single link cases, shouldn't $I$ from the single link case be decomposed similarly into energies from joint rotation and center of mass rotation as well?  Or is the single link case fundamentally different in that one only needs to consider energy from rotation around the joint?

It's my intuition that if the parameters of the single link case are equal to the parameters of the first link (of the two-link case), then we could conceivably write $I=I_1+m_1a_{c1}^2$.  Is this correct?  If not, why not?

答: Yes, your intuition is correct. In the first case, $I$ is computed around the joint point. In the second case, $I_1$ is computed around the center of mass of the link. The Parallel Axis Theorem can be used to yield different inertia values around different axes, yielding to your intuition.


问:
Libraries to calculate kinematics using Screw Theory
Most kinematics libraries/packages use the conventional transformation matrices (DH parameters). However, I'm looking for something that uses Screw Theory principles to set up the Product of Exponentials formula.

For eg., the KDL library from Orocos uses homogeneous transformation matrices, quaternions, Euler angles, etc. under the hood. But are there libraries that make use of twists and exponentials to derive the kinematics?

答:
Lynch and Park's Modern Robotics book uses the product of exponentials formula and screw axes to describe manipulators, and they have a well-documented library available in Python, MATLAB, and Mathematica. Plus there is a community-released C++ port using CMake/Eigen.

Book is available on this site (for free): http://modernrobotics.org/

Original library is here: https://github.com/NxRLab/ModernRobotics

C++ port is here: https://github.com/Le0nX/ModernRoboticsCpp

Full disclosure: I contributed some small elements to the book, and helped organize, build-out, and maintain the software library.


答:
From my understanding it is not correct as in the case of the single-link $I$ is directly computed around w.r.t. the joint point. However, in the two-link case, each link's inertia is firstly computed w.r.t. the center of mass of the link ($I_1$) and then translated to a reference frame with the origin coinciding to the joint-point via the Huygens-Steiner Theorem.

By the way, this structure is given by the Euler-Lagrange formulation of the robot model and the definition of the inertia matrix $M(q)$.
The slides you linked from Prof. Claudio Melchiorri (my former supervisor at the university of Bologna) explains this in the part preceding the example.

Title: Understanding the kinetic energy expressions for a 1 link vs 2 link robot arm

Body:
I'm studying these lecture notes on dynamic models of robot arm links, slides 33-36, where two examples are given for the kinetic energies for a single link and two link robot arm.

In the single link case, the kinetic energy is posed as:

$$K=\frac{1}{2}I\dot{\theta}^2$$

This appears to account for the rotational kinetic energy of the single link around the joint.

In the two link case, the kinetic energy of the first link is posed as:

$$K=\frac{1}{2}I_1\dot{\theta_1}^2 + \frac{1}{2}m_1a_{c1}^2\dot{\theta_1}^2$$

This appears to mean that the total kinetic energy in the first link of the two link chain contains a contribution from rotation around the first joint ($m_1a_{c1}^2$ term) and from rotation around the center of mass ($I_1$ term).

Comparing the two link and single link cases, shouldn't $I$ from the single link case be decomposed similarly into energies from joint rotation and center of mass rotation as well?  Or is the single link case fundamentally different in that one only needs to consider energy from rotation around the joint?

It's my intuition that if the parameters of the single link case are equal to the parameters of the first link (of the two-link case), then we could conceivably write $I=I_1+m_1a_{c1}^2$.  Is this correct?  If not, why not?


问:
Is there a specific way or module I can use to stream video data from a camera hooked to an Arduino Uno board?
I'm currently thinking about the feasibility of creating a robot which streams live video footage and other data such as temperature, humidity, etc. to a smartphone. Is it possible to do this on an Arduino Uno board? Or is a Raspberry Pi board needed?</p>

答: The arduino is physically incapable of doing video streams. The atmega 32u can only handle gpio operations and working with sensors and some math and serial comms. The Raspi CAN take a video image stream and then publish it where you want. There is a camers module that plugs directly into the pi for this sort of purpose. Usually the arduino acts as an I/O expander for the pi. The pi then can connect to the internet and publish data to a webserver or to a server machine. Does this make sense?</p>


答: Not servos explicitly, but I've obtained low end nema 17 stepper motors on occasion from electronic typewriters at goodwill. The typewriters I've seen there usually have 1 such motor in them (in addition to some other neat pin mount components) and fetch for $1 each. In my case the only other source for parts is radio shack and online outlets, so for me it's a viable option if I don't want sit around waiting for orders. This is however not a reliable method at all, especially if you want parts in large numbers or with certain specifications.</p>

Title: Is a thrift store a good place to get a servo motor?

Body: I'm trying to learn about a very basic motor, the servo motor.  Can these be found a thrift stores like Goodwill in old toys?  Are these "robotic" quality?  What toys or other kinds of things would I scavenge?  All I want to do is get a motor.  After that I want an Arduino and make it "work."  Nothing complex.</p>


问:
How does sensor fusion help in robot localization
Good day...
Sorry for bigner quesion, but really I need some friendly explanation rather than long reference
I have 9 dof mpu9250 imu and gps. I found some source that calculate euler angle in ardunio
How much euler angle help me in localization?
Can I use the mpu9250 raw data for localization?
If i fuse gps and imu what is the out put of filter?
Can i determine x y z position using imu and gps?</p>

答:
As an advice it's better to separate your questions and make them more precise.

In the end, if I understand correctly, your question is: how can the GPS and the IMU be used together for localization?

The IMU measures angles and angular speed but can be used as a position tracking sensor given that you know where the robot starts. You could use an extended Kalman filter to merge the sensor measurements.

I won't go in much more details because this questions has been asked before on this site and I think this answer is already an excellent read.


问:
What's the difference between the term "pose estimation" and "visual odometry"?
I'm reading a paper:

Choi C, Trevor A J B, Christensen H I. RGB-D edge detection and
  edge-based registration[C]//Intelligent Robots and Systems (IROS),
  2013 IEEE/RSJ International Conference on. IEEE, 2013: 1568-1575.

which refers:

Visual features such as corners, keypoints, edges, and color are
  widely used in computer vision and robotic perception for applications
  such as object recognition and pose estimation, visual odometry, and SLAM

I previously assume pose estimation to be roughly equal to visual odometry, yet the text above seems to deny.

So what's their difference? I didn't find much info from google. IMHO, it seems pose estimation is estimating the pose of moving object with the camera static, while visual odometry is estimating the pose of camera in a static(mostly) scene, is that precise enough?

答: Visual odometry is the ego-motion of the camera. It can be obtained by combining camera poses at different time instances. On the other hand, camera pose is defined just for consecutive frames. The full trajectory of the camera is obtained by concatenating all the camera poses. </p>


问:
question about spatial velocity in the book <modern robotics>

I think the equation(3.73) should be:

$\dot p-w_s$x$p=\dot p-\dot p=0$

have no idea how it can be wrong

and don't know the difference between infinitely large body and a body just include the fix-frame origin and body-frame origin

答:
A coordinate transformation of a point P from Frame 1 to Frame 0 is given by:
$$
\mathbf{p}^0=\mathbf{o}^0_1+\mathbf{R}^0_1\mathbf{p}^1.
$$
Differentiating with respect to time gives:
$$
\dot{\mathbf{p}}^0=\dot{\mathbf{o}}^0_1+\mathbf{R}^0_1\dot{\mathbf{p}}^1+\dot{\mathbf{R}}^0_1\mathbf{p}^1.
$$
Considering that $\dot{\mathbf{p}}^1=0$ as $\mathbf{p}^1$ is fixed in Frame 1, we come up with:
$$
\dot{\mathbf{p}}^0=\dot{\mathbf{o}}^0_1+\mathbf{S}\left(\mathbf{\omega}^0_1\right)\mathbf{R}^0_1\mathbf{p}^1,
$$
being $\mathbf{S}$ the skew matrix. This expression can be further reduced noting that $\mathbf{R}^0_1\mathbf{p}^1=\mathbf{r}^0_1$:
$$
\dot{\mathbf{p}}^0=\dot{\mathbf{o}}^0_1+\mathbf{\omega}^0_1 \times \mathbf{r}^0_1,
$$
which is eventually the known form of the velocity composition rule.

To come back to your statement, $\dot{\mathbf{p}}^0=\mathbf{\omega}^0_1 \times \mathbf{r}^0_1$ holds only when $\dot{\mathbf{o}}^0_1=0$, that is when there exists a pure rotational velocity without translational velocity.

This result can be obtained also by observing that a vector $\mathbf{p}$ can be expressed in terms of its magnitude $p$ and unit vector $\mathbf{\hat{p}}$ as:
$$
\mathbf{p}=p \cdot \mathbf{\hat{p}}.
$$
Thereby, the time derivative is:
$$
\dot{\mathbf{p}}=\dot{p}\cdot\mathbf{\hat{p}}+p\cdot\dot{\mathbf{\hat{p}}}=\dot{p}\cdot\mathbf{\hat{p}}+\mathbf{\omega} \times \mathbf{p}.
$$
Only in case there is a pure rotational velocity (i.e. $\dot{p}=0$), then you'll get:
$$
\dot{\mathbf{p}}=\mathbf{\omega} \times \mathbf{p}.
$$


问:
Calculating Error of Arm Manipulator
How can we calculate the error position of arm manipulator?

Let's say we have a Transformation Matrix:

$$\begin{bmatrix}
R_{00} & R_{01} & R_{02} & x\\
R_{10} & R_{11} & R_{12} & y\\
R_{20} & R_{21} & R_{22} & z\\
0 & 0 & 0 & 1
\end{bmatrix}$$

and target Position: x', y', z'.

I've found out the error for x, y, z is:

$$\epsilon_x = x-x' \\
\epsilon_y = y-y' \\
\epsilon_z = z-z'$$

From a reference I found, the position error is:

$$\epsilon = \sqrt{(x-x')^2 + (y-y')^2 + (z-z')^2}$$

Is this the correct formula to find position error? I may be confusing it with Root Mean Square Error/Standard Deviation from statistical methods.

答:
The interpretation of error for a robot manipulator pose is subjective in terms of what is the error being used for. A robot manipulator pose is combination of both position (x, y, z) and the orientation (quaternions or euler angles) and thus, designing the error could partially or completely include all terms.

Your representation of error is essentially scalar Euclidean distance for robot pose, acceptable for tasks where the orientation of the end-effector is not important. These applications are very few and a general example would be planar robots like the SCARA. It could also be useful in cases where the error is to be decoupled(position and orientation errors are differently analyzed/).

In a lot of cases, the error is left as a vector representation instead. That allows for tracking attempts are made for all individual directions.

Error = [(x-x') (y-y') (z-z')]'

The current scalar representation fails to convey the actual information. It could be misleading where a deviation of 1 cm along all individual axes would result in a error of 1.73205081 cm which would be equivalent to the error that a deviation of 3 cm along the one of the axes would result in. The former sounds acceptable for pick and place task because there would be some tolerance in all directions while the latter means there needs to be considerably larger tolerance.

I would propose decoupled vector representation of position and orientation error in the robot pose.

Title: Calculating Error of Arm Manipulator


答: A standard arduino (like the Uno) is not powerful enough, but if you go a bit higher, like the ESP32 (with WIFI), it's definitely possible to stream an image with a tiny Chip: ESP32 with camera</p>

Title: Is there a specific way or module I can use to stream video data from a camera hooked to an Arduino Uno board?

Body: I'm currently thinking about the feasibility of creating a robot which streams live video footage and other data such as temperature, humidity, etc. to a smartphone. Is it possible to do this on an Arduino Uno board? Or is a Raspberry Pi board needed?</p>


答:
When the manipulator's tool position $p_\text{tool} = (x_\text{tool}, y_\text{tool}, z_\text{tool})$ is different from the point $p_\text{target} = (x_\text{target}, y_\text{target}, z_\text{target})$ where the manipulator should go to, we say there is position error.

Intuitively, position error $e$ should tell us how far the current tool point $p_\text{tool}$ is from the expected point $p_\text{target}$. So a natural choice of the formula to use to calculate position error given $p_\text{tool}$ and $p_\text{target}$ is how we would measure the distance between two points in a 3D space, i.e.
$$
e = \Vert p_\text{tool} - p_\text{target} \Vert_2 = \sqrt{(x_\text{tool} - x_\text{target})^2 + (y_\text{tool} - y_\text{target})^2 + (z_\text{tool} - z_\text{target})^2}.
$$

However, in general, the function used to calculate position errors, does not have to be the square root of squared differences as above but can be any distance metric.

The root-mean-square error is kind of similar to the position error above (in case $L_2$-norm is used as the distance metric), but used in a different context.

While the position error tells us how much a single measurement ($p_\text{tool}$) deviates from the expected value ($p_\text{target}$), the root-mean-square shows how much a given set of measurements (for example, $\{p_\text{tool}^{1}, p_\text{tool}^{2}, \ldots, p_\text{tool}^{N}\}$) deviates from expected values ($\{p_\text{target}^{1}, p_\text{target}^{2}, \ldots, p_\text{target}^{N}\}$). It might be used when, say, you control the manipulator to go along a path of $10$ points and you want to see how well the manipulator tracks those $10$ points, for example.

While generally a position error is a single number as mentioned above, at times we might want to express a position error as a vector (as @Akshay mentioned in this answer) as it gives us which direction the measured point deviates from the expected one, which could be useful for further use.

Title: Calculating Error of Arm Manipulator

Body:
How can we calculate the error position of arm manipulator?

Let's say we have a Transformation Matrix:

$$\begin{bmatrix}
R_{00} & R_{01} & R_{02} & x\\
R_{10} & R_{11} & R_{12} & y\\
R_{20} & R_{21} & R_{22} & z\\
0 & 0 & 0 & 1
\end{bmatrix}$$

and target Position: x', y', z'.

I've found out the error for x, y, z is:

$$\epsilon_x = x-x' \\
\epsilon_y = y-y' \\
\epsilon_z = z-z'$$

From a reference I found, the position error is:

$$\epsilon = \sqrt{(x-x')^2 + (y-y')^2 + (z-z')^2}$$

Is this the correct formula to find position error? I may be confusing it with Root Mean Square Error/Standard Deviation from statistical methods.


问:
IR sensor spacing in sensor array relative to line width
I'm trying to build a line follower bot that follows a 3cm wide black line. I have a line sensor array with 6 analog sensors spaced 1cm apart.

This means that in an ideal situation, i.e bot going straight on track, the center 2 sensors are on black line, the ones immediately to the side of each sensor are on the border of the black line and the farthest sensors are on the white surface as shown in the image below.

I've used PID control in the bot but it's oscillating like crazy. Even on a straight line it's following the whole line by snaking. It has much more sideways movement than forwards.

If I try to smooth it, then it fails to turn at bends. Tried several combinations of PID parameters.

What I think is happening

It's almost impossible to position the 2nd and 5th sensors right on the border. If the bot moves slightly to the right, the 2nd sensor is on black and 5th on white, so bot thinks it's too far right, and so on.

My questions

Is it practically possible to follow 3cm line with 1cm spaced sensors?

Am I doing something wrong in my PID?

PS:
I'm using rear 2 wheel differential drive with an inter-wheel distance of around 13cm. My line sensor array is placed around 8cm in front of the rear wheel axle line. I'm going to post a question regarding this shortly. I'll post a link in the comment.

Edit:

This is the loop of my Arduino sketch (minus the Serial.prints).

Kp = 2.5

Ki = 0 (yes I'm not using integral part of PID for now)

Kd = 0.05

```<code>void loop() {
  int position = qtr.readLineBlack(sensors); // get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position.

  int error = position - set_point;
  integral += error;

  last_error = error;
  int motorSpeed = Kp * error + Ki * integral + Kd * (error - last_error);

  int rightMotorSpeed = BASE + motorSpeed;
  int leftMotorSpeed = BASE - motorSpeed;
  drive(leftMotorSpeed, rightMotorSpeed);
  delay(30);
}
</code>```

BASE speed value is 55

According to Pololu QTR library, readLine returns a value in the range 0 to 100 * (n_sensors - 1) with 0 indicating line is too far left. I have 6 sensors so I can get a max of 5000. So I set the set_point value to 2500. The function retusn the last recorded value in case the sensors have lost the line completely. I've tested the function by sliding a black ribbon under the sensors and observed the motors, so I know that my position reading is not an issue

Now, I'm driving the motors with this function:

```<code>void drive(int left_speed, int right_speed) {
  if (left_speed < 0) {
    digitalWrite(LF, LOW);
    digitalWrite(LB, HIGH);
  } else {
    digitalWrite(LF, HIGH);
    digitalWrite(LB, LOW);
  }
  if (right_speed < 0) {
    digitalWrite(RF, LOW);
    digitalWrite(RB, HIGH);
  } else {
    digitalWrite(RF, HIGH);
    digitalWrite(RB, LOW);
  }
  analogWrite(LPWM, min(PWM_LIMIT, abs(left_speed)));
  analogWrite(RPWM, min(PWM_LIMIT, abs(right_speed)));
}
</code>```

This function basically reverses the corresponding motor direction of the one that has negative speed. I'm using a BASE pwm value of 55 and a PWM_LIMIT of 80 because I'm using a high voltage battery for the motors and my L293D motor driver can only handle 600mA.

I can understand that with my current Kp value of 2.5, even a slight error is causing a big change in motor drive.

答:
The PID controller is best if you have a range of error from the ideal position to the extreme one. When i say extreme i am talking about the hard turn positions as compared to the ideal position. Now in your case the 2 and 5 sensor are never going to be on the edge and hence in best case scenario the bot will oscillate between 2 and 5. Now lets talk about the extreme position. According to the configuration if sensor 1 or 6 are active then the bot is assumed to be in the extreme position. So we only have 2 levels of error from ideal to extreme (for eg. Ideally 1 is inactive and in case of hard left 1 gets active). This range is too limited for an effective PID controller.
Coming to your questions.

Yes it is possible. But it is not preferred. Specially with the PID control. With if else coding it can be implemented but that defeats the purpose of using so many sensors.

Probably not. Although I am sure the parameters can be optimised but in this architecture there is not much scope of optimization.

I have implemented PID control with 5 sensors, ideal position was: sensor 3 on the line and 2 and 4 just beyond the edge of the line so that both are inactive during ideal operation. Sensor 1 and 5 even farther away.

So my advice will be, if there are hard turns then try to change the array. If that's not possible you can try something hybrid involving if else coding and PID control as well.


问:
Why would I use three state variables for a second order system? [state space modeling]
I'm currently being asked to write a state space model of a falling object (equation attached).

I had taken a linear systems course five years ago in which I did a lot of conversion between high order differential equations and systems of first order linear equations. However, this task seems odd for a few reasons:

Most nth ordered differential equations that I had converted only required n state variables. For example, if I had a second order differential equation, I needed two state variables. In this problem, it's asking for 3 state variable which seems odd. Is the third state variable even necessary?

This differential equation directly includes the independent variable (time) itself.  It also includes an initial condition p0 which seems odd.

I'm able to derive a solution to this problem, however it just seems very odd in how I'm doing it. This is my solution:

答:
Initial conditions are important, let's assume this is a rigid body falling from 10m height at time 't_0', then to know its position at any time 't' I must know its initial position. Also, the third state (acceleration) is redundant information as it has been already described that body is falling at a constant acceleration, which simply means jerk (rate of change of accel.) is zero.

P.S: I have not seen any real system so far where we need to have acceleration as a system state. In system dynamics we are mostly concerned with forces/torques, which lead to accelerations, so typical state vectors include positions and velocities as system states.


问:
Constant Offset matrix
I've been reading about controlling systems via momentum wheels.

Such as in Satellites, or fancy inverse pendulums.

In a particular paper, the writer tries to explain away an offset and settling of the system angle and reaction wheel angular velocity at non-demand conditions because of the dynamics of the control scheme will give to a constant motor angular velocity when there is a constant offset. Given the matricies A and B and the Kd gains aquired from the LQR method from a SSM such as:

$A=\left(
\begin{array}{ccc}
 0 & 1 & 0 \\
 \frac{g \left(l_b m_b+l m_w\right)}{\Theta _b+l^2 m_w} & -\frac{R_b}{\Theta _b+l^2 m_w} & \frac{R_w}{\Theta _b+l^2 m_w} \\
 -\frac{g \left(l_b m_b+l m_w\right)}{\Theta _b+l^2 m_w} & \frac{R_b}{\Theta _b+l^2 m_w} & \frac{R_w \left(\Theta _b+l^2 m_w+\Theta _w\right)}{\Theta _w \left(\Theta _b+l^2 m_w\right)} \\
\end{array}
\right)$

$B=\left(
\begin{array}{c}
 0 \\
 -\frac{\text{Km}}{\Theta _b+l^2 m_w} \\
 \frac{R_w \left(\Theta _b+l^2 m_w+\Theta _w\right)}{\Theta _w \left(\Theta _b+l^2 m_w\right)} \\
\end{array}
\right)$

$Kd=\left(
\begin{array}{c}
 \text{K1} \\
 \text{K2} \\
 \text{K3} \\
\end{array}
\right)$

They explain

As it can be seen from the plot, the estimated state of the system converges to ($\phi$b , $\phi'$b , $\phi'$w ) = (−0.058, 0.0, 37.0). This behavior can be explained by the following property of the system:

$$V=(\text{IdentityMatrix}[3]-A+B.\text{Kd})^{-1}.B=\left(
\begin{array}{c}
 0 \\
 0 \\
 \frac{\text{Km}}{\text{K3} \text{Km}+R_w} \\
\end{array}
\right)$$

However, when working out their maths...I get anything but their solution, where the first two components or $V$ are equal to 0.

Is this particular equation a known one assumed to give zeros in the first two components? Or is there a mistake of some kind? Unfortunately there are no references given to their assumption.

The exact text for context:

答:
I actually spent a while trying to figure this out, and I realized that they're talking about Final value theorem.

Here's a paper (warning: PDF) that gives the equation you've provided in your paper with a correction:

$$
\left(sI - \left(A-BK\right)\right)^{-1}B \\
$$

Note the Laplace operator $s$! The final value theorem says that you can get $\lim_{t\rightarrow\infty} x(t)$ by taking the limit $\lim_{s\rightarrow0}sX(s)$.

Now, I couldn't get the right answer with the matrices you provided, but I think I found your source material ("The Cubli: A Cube that can Jump Up and Balance", Gajamohan et al 2012), which gives:

When I use these values and use $Kd = [k1, k2, k3]$ (Note that this is a row vector, not column vector as in your question), I get the following:

```<code>clear all;
clc;
syms L Lb mw mb g Km K1 K2 K3 Ib Iw Cb Cw;
syms s;
A = [0, 1, 0; ...
    (mb*Lb+mw*L)*g/(Ib+mw*L*L), -Cb/(Ib+mw*L*L), Cw/(Ib+mw*L*L); ...
    -(mb*Lb+mw*L)*g/(Ib+mw*L*L), Cb/(Ib+mw*L*L), -Cw*(Ib+Iw+mw*L*L)/(Iw*(Ib+mw*L*L))];
B = [0; ...
    -Km/(Ib+mw*L*L); ...
    Km*(Ib+Iw+mw*L*L)/(Iw*(Ib+mw*L*L))];
Kd = [K1, K2, K3];

eqn = ((s*eye(3) - (A-B*Kd))^(-1))*B;

solution = limit(eqn, s, 0);

simplify(expand(solution))

ans =

               0
               0
 Km/(Cw + K3*Km)
</code>```

Hope this helps!!

Title: Constant Offset matrix

Body:
I actually spent a while trying to figure this out, and I realized that they're talking about Final value theorem.

Here's a paper (warning: PDF) that gives the equation you've provided in your paper with a correction:

$$
\left(sI - \left(A-BK\right)\right)^{-1}B \\
$$

Note the Laplace operator $s$! The final value theorem says that you can get $\lim_{t\rightarrow\infty} x(t)$ by taking the limit $\lim_{s\rightarrow0}sX(s)$.

Now, I couldn't get the right answer with the matrices you provided, but I think I found your source material ("The Cubli: A Cube that can Jump Up and Balance", Gajamohan et al 2012), which gives:

When I use these values and use $Kd = [k1, k2, k3]$ (Note that this is a row vector, not column vector as in your question), I get the following:

clear all;
clc;
syms L Lb mw mb g Km K1 K2 K3 Ib Iw Cb Cw;
syms s;
A = [0, 1, 0; ...
    (mb*Lb+mw*L)*g/(Ib+mw*L*L), -Cb/(Ib+mw*L*L), Cw/(Ib+mw*L*L); ...
    -(mb*Lb+mw*L)*g/(Ib+mw*L*L), Cb/(Ib+mw*L*L), -Cw*(Ib+Iw+mw*L*L)/(Iw*(Ib+mw*L*L))];
B = [0; ...
    -Km/(Ib+mw*L*L); ...
    Km*(Ib+Iw+mw*L*L)/(Iw*(Ib+mw*L*L))];
Kd = [K1, K2, K3];

eqn = ((s*eye(3) - (A-B*Kd))^(-1))*B;

solution = limit(eqn, s, 0);

simplify(expand(solution))

ans =

               0
               0
 Km/(Cw + K3*Km)

Hope this helps!!


问:
How timer interrupt is handled in the presence of other interrupt services?
When timer interrupt is received, if there is already some ISR being executed, doesn't it delay (elongate) the time interval measured? If yes, how is it handled? For the sake of precise time measurements.</p>

答:
No. The actual time is calculated from counter registers that automatically increment. The timer interrupt will be delayed, and things the timer interrupt is triggering will be delayed. This is called jitter. But the measurement of the interval will not change in accuracy.

Counters roll over to zero automatically at the hardware level. So there are never lost counts. Counters are large enough that they either don't roll over or that the software has lots of time to count the number of roll overs.

The error in time measurement one sees in digital devices is called drift and is due to the oscillator driving the time counter at a non-exact rate.

All microcontrollers have oscillator driven counters for keeping time. Some microcontrollers and most processors have a realtime clock subsystem. Real-time clocks have the same counter at their core and some convenience hardware for handling time and date.

The only difference between counters and real-time clocks is that attached to real-time clocks there is usually an oscillator that has a frequency that is a power of two factor of 1 second, and sometimes that oscillator with have less drift (more accuracy).

One way to think of it is how you behave with a wrist watch. The watch is always keeping time, with some error from true time (drift). If you get an alarm from the watch, start an action, get distracted, and then complete the action, there is error in the timing of your action, but the watch continues to measure time accurately.


答:
I'll point out that your solution:
$$
\dot{X} = \left[\begin{matrix}
\dot{p} \\
\ddot{p} \\
\dddot{p}\end{matrix}\right] = \left[\begin{matrix}
x_2 \\
x_3 \\
0 \end{matrix}\right]= \left[\begin{matrix}
0 & 1 & 0 \\
0 & 0 & 1 \\
0 & 0 & 0 \end{matrix}\right] \left[\begin{matrix}
x_1 \\
x_2 \\
x_3 \end{matrix}\right]\\
$$

multiplies out to:

$$
\dot{p} = \dot{p} \\
\ddot{p} = \ddot{p} \\
$$

which is a trivial statement and doesn't express the system you presented.

I am not a fan of this question (not you, but the question you were asked to solve.) As others have mentioned, there's no reason to have acceleration $\ddot{p}$ as a state, because you get acceleration when you take the derivative of your state vector if your state vector includes speed. That is, if $x = \left[\begin{matrix}
p \\
\dot{p} \end{matrix}\right]$, then $\dot{x} = \left[\begin{matrix}
\dot{p} \\
\ddot{p}\end{matrix}\right]$.

Further, in order to get the solution here, I believe the question is wanting you to rearrange the equation in the form of $\ddot{x} = \text{<stuff>}$, but you wind up with an expression that implies that acceleration is a function of position and speed, which is not the case. Position is a function of speed and acceleration.

Title: Why would I use three state variables for a second order system? [state space modeling]

Body:
I'm currently being asked to write a state space model of a falling object (equation attached).

I had taken a linear systems course five years ago in which I did a lot of conversion between high order differential equations and systems of first order linear equations. However, this task seems odd for a few reasons:

Most nth ordered differential equations that I had converted only required n state variables. For example, if I had a second order differential equation, I needed two state variables. In this problem, it's asking for 3 state variable which seems odd. Is the third state variable even necessary?

This differential equation directly includes the independent variable (time) itself.  It also includes an initial condition p0 which seems odd.

I'm able to derive a solution to this problem, however it just seems very odd in how I'm doing it. This is my solution:


问:
Arc welder for 3d printing
Has anybody experimented with GMAW for additive manufacturing? The thing is, welding wire is so much cheaper than ABS or PLA filaments and, well, it is steel you are printing in, not some flimsy plastic! I imagine the arc deposition printhead would be constructed similarly to one used in plastic filament printers, except there is no need for the heating element (so, even simpler). Welding often requires fast Z speed (to finely control the arc) so i think Delta (DeltaMaker) chassis would work best. GMAW calls for some sort of inert gas to insulate heated metal from oxygen. It would make sense to seal off most of the interior of the printer and fill it with heavier than air inert gas during printing.

I would highly appreciate any pointers on existing 3d printer designs employing this deposition method as well as flaws in design i outlined here.

答:
I believe this is in very active development.  Here's an article with a few videos on that subject:

3D printing with metal: The final frontier of additive manufacturing

The real breakthrough that has enabled 3D printing for the masses has been the laser. Spray welding is a technique that has been used for decades to build up worn motor shafts, but it is far too crude for controlled additive printing. Spray welding uses a gravity-fed powdered metal dispenser integrated into a special oxygen-acetylene torch head which melts the powder as it is dispensed. Swapping the torch for a laser gave us the powerful construction tool we have today. A powdered metal feedstream, confined and protected against oxidation with a surrounding jet of inert shielding gas, fused by a laser piped through a central bore in the head is now the state of the art technology.

There is also a room-temperature extrusion method that seems to work, using gallium:

3D printing with liquid metals

The medium the team is using for their metallic 3D prints is an alloy of 75% gallium and 25% indium. This alloy is liquid at room temperatures, but when exposed to an oxygen atmosphere, a very thin layer of oxide forms on a small metal bead squeezed out of a syringe. Tiny metal sphere by tiny metal sphere, the team can build up metallic objects out of this alloy, stacking the beads into just about any shape imaginable.

Title: Arc welder for 3d printing


问:
What are wheel ticks and wheel impulses?
I'm tracking the position of a vehicle along a certain trajectory using the Kalman filter and we have odometry data provided to us which gives the x-position of the vehicle, y-position of the vehicle and orientation of the vehicle. This is calculated based on the wheelticks and is relative to the inertial origin of coordinates. We also have data which tells us about the number of received wheel impulses from each of the wheels.

What exactly are the differences between wheel ticks and wheel impulses? Does wheel impulse mean the wheel speed (rpm)?

If steering angle data (Averaged steering wheel angle) is additionally available, can this also be used to calculate velocity and position?

答:
Regarding question 2, you can use steering angle for velocity (directional speed) estimation, but Ackermann steering results in a heading that is a function of starting orientation, steering angle, speed, and time.

Ackermann-steered vehicles are always turning along the tangent of a circle, where the circle's radius is set by the steering angle. As the vehicle moves, the steering angle sets the radius of curvature and then the vehicle speed causes you to traverse some arc length, which results in your new heading.

It's not enough to just know what the steering angle is currently, you've got to integrate the entire path. If the radius of curvature is such that the circle being traversed has a circumference of 1 meter, and you drive 1 meter, then the current heading is equal to the starting heading. If the circumference is 2 meters and you drive 1 meter then your current heading is the opposite of your starting heading, etc.


问:
SLAM and navigation
I have a question about SLAM. Is it common for SLAM to be used for mere map generation? For instance, have a map generated using ORB-SLAM and then freeze the map for future path planning where future updates are prohibited. I suppose, the map becomes a static map. However, is the common application to constantly update the map?</p>

答:
Yes, if you mean "incorporate new information".

Not really, if you mean "re-run maplab with new point clouds".

There isn't really a "common" application for SLAM, per se. However, one emerging industry is self-driving cars (or trucks). The need to regularly update maps to account for construction, new roads, closures, and so on is fairly obvious. (note, most maps don't include mobile cars or pedestrians, so sensing them is not quite updating the map).

Another common use of SLAM (by another name) is photogrammetric inspection of infastructure. This is the use of drones or cameras to construct 3D models of guardrails, buildings, bridges, etc. Here, we directly compare two maps to see what changed.

So, it may seem the answer is Yes, we often update maps in practice. However, I want to point out a subtlety here. By update in SLAM we mean incorporate more sensor readings to better pinpoint landmarks or expand the map into new areas. This is usually not done. We don't take new measurements and update the old position of the old guardrails after an accident. Instead, we take new measurements, and discard the old location of guardrails because they have in fact moved. The same is true for all the other cases I've mentioned.

The problem is the colloquial definition of update is correct the map to incorporate all new information, including movement of landmarks. In this sense, yes, we update it. I want to point out that the automatic updating of maps in this sense is not possible using ORBSLAM or other methods currently in literature that have found popularity through "drones flying through buildings" type videos. This should be considered an active and fruitful future area of research.

Title: SLAM and navigation


问:
Block algebra and normal algebra inconsistent
I've been playing around with some block maths, mostly trying to remember how it actually works, and I've come to a simple example that should be reduced, however when doing the normal maths, compared to the block maths, I get different answers.

The solution, when doing regular maths give me:
$$y=e S_1 S_2 G_r-d S_2$$
$$e = w - y$$
$$y+ y G_r S_1 S_2 =w G_r S_1 S_2-d S_2$$
$$y=\frac{ w G_rS_1 S_2 -d S_2}{1+G_r S_1 S_2}$$

doing Block algebra, moving $S_2$ before $d$ (which would make it now, $S_2 d$

Then going under the rule for Feedback, $\frac{G_s}{G_s+1}$

I would expect to get as a result:

$$y=\frac{ w G_rS_1 S_2 -d S_2}{1+S_1 S_2 G_r-d S_2}$$

However, it appears this is the wrong solution.

is there something I'm missing on my block algebra, or is this correct, except one assumes $d S_2$ in the denominator is always zero?

答:
I think your math is correct, but it's a little easier to look at if you rearrange it as:

$$
y = w\frac{S_1 S_2 G_r}{1 + S_1 S_2 G_r} - d\frac{ S_2}{1 + S_1 S_2 G_r}\\
$$

The output is a combination of the closed-loop response and the disturbance input.

I think the problem with your block diagram work is that you're treating everything, including the disturbance input, as though it's a "plant," but it's not, because the input $y$ doesn't route through $d$.

This is a superposition (summing) problem; I think the "easy" way to solve this problem with block diagram work is to look at each input as though the other input doesn't exist.

Consider:

This is easy; this is the method you were going for and reduces to:

$$
y = w\frac{S_1 S_2 G_r}{1 + S_1 S_2 G_r} \\
$$

Then, consider the disturbance input:

This is a little harder to look at, so consider instead:

You can combine the terms on the feedback branch:

And then reduce the loop (Line 4 on that page) and get:

It's important (as always) to keep up with the negative signs. Here they cancel in the denominator and you're left with:

$$
y = - d\frac{ S_2}{1 + S_1 S_2 G_r}\\
$$

The complete solution is the summation (superposition) of the response from the input $w$ and the disturbance $d$:

$$
y = w\frac{S_1 S_2 G_r}{1 + S_1 S_2 G_r} - d\frac{ S_2}{1 + S_1 S_2 G_r}\\
$$


答:
Because of the Superposition principle, you can write $y$ as:

$$
y = H_1 w + H_2 d,
$$

where $H_1$ is the system transfer function when $d=0$, whereas $H_2$ is the corresponding transfer function for $w=0$.

It turns that

$$
H_1 = \frac{G_rS_1S_2}{1+G_rS_1S_2},
$$

while

$$
H_2 = \frac{-S_2}{1+G_rS_1S_2}.
$$

$H_2$ can be readily obtained using block simplification by observing that it holds:

Title: Block algebra and normal algebra inconsistent

Body:
I've been playing around with some block maths, mostly trying to remember how it actually works, and I've come to a simple example that should be reduced, however when doing the normal maths, compared to the block maths, I get different answers.

The solution, when doing regular maths give me:
$$y=e S_1 S_2 G_r-d S_2$$
$$e = w - y$$
$$y+ y G_r S_1 S_2 =w G_r S_1 S_2-d S_2$$
$$y=\frac{ w G_rS_1 S_2 -d S_2}{1+G_r S_1 S_2}$$

doing Block algebra, moving $S_2$ before $d$ (which would make it now, $S_2 d$

Then going under the rule for Feedback, $\frac{G_s}{G_s+1}$

I would expect to get as a result:

$$y=\frac{ w G_rS_1 S_2 -d S_2}{1+S_1 S_2 G_r-d S_2}$$

However, it appears this is the wrong solution.

is there something I'm missing on my block algebra, or is this correct, except one assumes $d S_2$ in the denominator is always zero?


问:
What is the degree of freedom of the following drone?
This might be a stupid question to ask here .i am confused whether the dof of this drone is 3 or 4?

答: Assuming that it is a planar robot (only moving in XY-plane), then from the diagram one can see it has 3-DoF (x,y,pitch) along with 2 additional DoF for the arm links. </p>


问:
How to get transformation between a camera and a CNC plotter?
I have a question about robot and camera calibration.

I have experience to calibrate btw 6DOF robot arm and hand-eye camera.
But this time I should use a machine like CNC plotter. This machine can move only x and y directions and no rotation. I will set a camera under the machine.
In this case, can I do calibration btw a cam and the machine? And also can I calculate transformation matrix btw them?

If anyone knows how to solve it please let me know. And if there are some papers about this, also let me know.

答:
In this case, the Z value and rotation value are constant, so it's a 2DOF system. You can do the same calibration as with the 6DOF and you'll find a homogeneous matrix as usual.

The difference here is that instead of a direct kinematics approach from a manipulator you'll have to determine the position (0, 0) for the CNC Plotter and the constant Z and rotation. Depending on the method (if you're using a least-squares method), you can find the Z and rotation value as well, otherwise, one approach is to determine them previously and find the X and Y values for the matrix.


问:
Using a ROS custom message type across machines/hosts
I am unable to use a custom ROS message type on machine B that I have created on machine A.

Machine A:

runs roscore.

has the custom message code as a ROS package.

has sourced the setup.bash of the package.

runs rostopic pub /test_topic my_custom_msg test_value

Machine B:

when I run: rostopic list, I do see the topic I am publishing to on machine A ("/test_topic").

when I run: runmsg list, I DO NOT see the custom message type I am looking for (the one, my topic is using: "my_custom_msg")

When I do these things locally, means all on machine A, everything works as expected: I am publishing a test message using my custom message type and with rostopic echo I can even successfully subscribe to it with getting all the messages that are sent displayed correctly.

So, what am I missing in order to use my custom message from machine A also on machine B?

Please, I really need help on that!

答:
I solved the problem!

The solution is to simply have the custom message definition on all machines where you'd want to use it.

This might be most comfortable if the custom message is realized as a separate ROS package, because then it's easier to distribute it among different machines.


问:
ROS Gazebo Odometry Issue
I am setting up a Gazebo model for use with the navigation stack. I have been reading the Navigation Tuning Guide and am confused about the lidar data in the odom frame. I would think that the tuning guide, when it says:

"The first test checks how reasonable the odometry is for rotation. I open up rviz, set the frame to "odom," display the laser scan the robot provides, set the decay time on that topic high (something like 20 seconds), and perform an in-place rotation. Then, I look at how closely the scans match each other on subsequent rotations. Ideally, the scans will fall right on top of each other, but some rotational drift is expected, so I just make sure that the scans aren't off by more than a degree or two. (Nav Stack Tuning)"

it means that the lidar data is supposed to be in approximately the same place before, during, and after the rotation. I have been reading and it seems that these sweeping swirls that I see are correct? I have been trying to use gmapping in my simulation and whenever I rotate the map gets horridly disfigured - I believe that odometry is to blame.

I have recorded what the lidar data looks like in the odom frame. Is this correct or should it look differently?

I followed this tutorial to build the initial model and simulate it. I created a (visually) crude model with two wheels (left and right) that move and two frictionless casters (front and back) using their general framework. I changed the shape of the robot but just followed their procedure and tried to reproduce it. I have tried to flip the x rotation for the left and right wheels from -pi/2 to pi/2 and that just reversed the direction of motion, which I expected, but does not change the issue of streaky lidar from the odom frame. I am puzzled because the straight odometry data keeps the laser scans in the same position (as one would expect) but when I rotate the robot I get the streaks. I don't really know the mechanism behind calculating the odometry data in Gazebo so I am stuck as to fixing this issue.

The urdf file is shown:

```<code><?xml version="1.0"?>
<robot name="bender" xmlns:xacro="http://ros.org/wiki/xacro">

    <xacro:property name="pi" value="3.141592653589794" />
    <xacro:property name="base_len" value="0.89" />
    <xacro:property name="base_wid" value="0.80" />
    <xacro:property name="base_height" value="0.20" />
    <xacro:property name="caster_length" value="0.10" />
    <xacro:property name="caster_radius" value="0.15" />
    <xacro:property name="wheel_length" value="0.10" />
    <xacro:property name="wheel_radius" value="0.15" />
    <xacro:property name="update_rate" value="50"/>
    <xacro:property name="hokuyo_size" value="0.05"/>

    <xacro:macro name="default_inertial" params="mass">
        <inertial>
          <mass value="${mass}" />
          <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
        </inertial>
    </xacro:macro>

    <material name="white">
       <color rgba="1 1 1 1.5"/>
    </material>

    <link name="base_link">
        <visual>
          <geometry>
            <box size="${base_len} ${base_wid} ${base_height}"/>
          </geometry>
          <material name="white"/>
        </visual>
        <collision>
            <geometry>
              <box size="${base_len} ${base_wid} ${base_height}"/>
            </geometry>
        </collision>
        <xacro:default_inertial mass="1"/>
    </link>

    <xacro:macro name="caster" params="position reflect">
        <joint name="${position}_wheel_joint" type="fixed">
            <parent link="base_link"/>
            <child link="${position}_wheel"/>
            <axis xyz="0 0 1"/>
            <origin xyz="${reflect*(base_wid/2)} 0 ${-1 * base_height}" rpy="${pi/2} 0 0"/>
        </joint>
        <link name="${position}_wheel">
            <visual>
                <geometry>
                    <sphere radius="${caster_radius}"/>
                </geometry>
                <material name="white"/>
            </visual>
            <collision>
                <geometry>
                  <sphere radius="${caster_radius}"/>
                </geometry>
            </collision>
            <xacro:default_inertial mass="0.5"/>
        </link>

<!-- This block provides the simulator (Gazebo) with information on a few additional
physical properties. See http://gazebosim.org/tutorials/?tut=ros_urdf for more-->
        <gazebo reference="${position}_wheel">
            <mu1 value = "0.0"/>
            <mu2 value = "0.0"/>
            <kp  value = "10000000.0"/>
            <kd  value = "1.0"/>
            <material>Gazebo/Grey/</material>
        </gazebo>
    </xacro:macro>

    <xacro:macro name="wheel" params="position reflect">
        <joint name="${position}_wheel_joint" type="continuous">
            <parent link="base_link"/>
            <child link="${position}_wheel"/>
            <axis xyz="0 0 1"/>
            <origin xyz="0 ${reflect*(base_wid/2)} ${-1*base_height}" rpy="${-pi/2} 0 0"/>
        </joint>
        <link name="${position}_wheel">
            <visual>
                <geometry>
                    <cylinder length = "${wheel_length}" radius="${wheel_radius}"/>
                </geometry>
                <material name="white"/>
            </visual>
            <collision>
                <geometry>
                  <cylinder length = "${wheel_length}" radius="${wheel_radius}"/>
                </geometry>
            </collision>
            <xacro:default_inertial mass="0.5"/>
        </link>

<!-- This block provides the simulator (Gazebo) with information on a few additional
physical properties. See http://gazebosim.org/tutorials/?tut=ros_urdf for more-->
        <gazebo reference="${position}_wheel">
            <mu1 value = "200.0"/>
            <mu2 value = "100.0"/>
            <kp value="1000000.0" />
            <kd value="1.0" />
            <fdir1 value="1 0 0"/>
            <material>Gazebo/Grey/</material>
        </gazebo>

<!-- This block connects the wheel joint to an actuator (motor), which informs both
   simulation and visualization of the robot -->
       <transmission name="${position}_wheel_trans">
        <type>transmission_interface/SimpleTransmission</type>
             <actuator name="${position}_wheel_motor">
                  <mechanicalReduction>1</mechanicalReduction>
             </actuator>
         <joint name="${position}_wheel_joint">
              <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
         </joint>
        </transmission>
    </xacro:macro>

<!-- Creating the actual robot -->

    <xacro:caster position="front" reflect="1"/>
    <xacro:caster position="back"  reflect="-1"/>
    <xacro:wheel  position="right" reflect="1"/>
    <xacro:wheel  position="left"  reflect="-1"/>

 <!-- Gazebo plugin for ROS Control -->

    <gazebo>
      <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
        <robotNamespace>/</robotNamespace>
      </plugin>
    </gazebo>

<!-- Hokuyo Laser Senso -->
    <link name="hokuyo_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="${hokuyo_size} ${hokuyo_size} ${1.5*hokuyo_size}"/>
        </geometry>
        <material name="Blue" />
      </visual>
    </link>
    <joint name="hokuyo_joint" type="fixed">
      <origin xyz="${base_wid/2} 0 ${base_height+hokuyo_size/4}" rpy="0 0 0" />
      <parent link="base_link"/>
      <child link="hokuyo_link" />
    </joint>
    <gazebo reference="hokuyo_link">
      <material>Gazebo/Blue</material>
      <turnGravityOff>false</turnGravityOff>
      <sensor type="ray" name="head_hokuyo_sensor">
        <pose>${hokuyo_size/2} 0 0 0 0 0</pose>
        <visualize>false</visualize>
        <update_rate>40</update_rate>
        <ray>
          <scan>
            <horizontal>
              <samples>720</samples>
              <resolution>1</resolution>
              <min_angle>-1.570796</min_angle>
              <max_angle>1.570796</max_angle>
            </horizontal>
          </scan>
          <range>
            <min>0.10</min>
            <max>10.0</max>
            <resolution>0.001</resolution>
          </range>
        </ray>
        <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
          <topicName>/scan</topicName>
          <frameName>hokuyo_link</frameName>
        </plugin>
      </sensor>
    </gazebo>

</robot>
</code>```

I set up my differential drive controller as shown in the tutorial, with my own values, here:

```<code>type: "diff_drive_controller/DiffDriveController"
publish_rate: 50

left_wheel: ['left_wheel_joint']
right_wheel: ['right_wheel_joint']

# Odometry covariances for the encoder output of the robot. These values should
# be tuned to your robot's sample odometry data, but these values are a good place
# to start
pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]

# Wheel separation and radius multipliers
wheel_separation_multiplier: 1.0 # default: 1.0
wheel_radius_multiplier    : 1.0 # default: 1.0

# Top level frame (link) of the robot description
base_frame_id: base_link

# Velocity and acceleration limits for the robot
# default min = -max
linear:
  x:
    has_velocity_limits    : true
    max_velocity           : 5.0   # m/s
    has_acceleration_limits: true
    max_acceleration       : 0.6   # m/s^2
angular:
  z:
    has_velocity_limits    : true
    max_velocity           : 1.0   # rad/s
    has_acceleration_limits: true
    max_acceleration       : 0.5 # rad/s^2
    has_jerk_limits        : true
    max_jerk               : 2.5  # rad/s^3
</code>```

I leave the wheel_separation and wheel_radius out because I wish for them to be generated automatically. I have put in different values, including what I think they are, and it does not change the streaking in lidar data when viewed from the odom frame.

I have also posed the question here.

Edit:
I have compiled and ran code from the Book ROS Robotics Projects. Chapter 9 code has a simulated robot. I put fixed frame to odom and when I move forward there is 'streaking' in the x direction but not when I rotate. This makes sense to me but I am wondering why my simulated robot does not behave like this.

答:
this may be a bit late to answer, but i hope it may be of help for people learning ros in the future.

First, in the URDF model,

mu1 and mu2 is friction coefficient was set as 0.
If the wheel has no friction coefficient, it has no friction.
Hence, the wheel will not move, since it has no friction, set it as non-zero and it will move.

Second, in the diff_drive (yaml) file
pose_covariance_diagonal and twist_covariance diagonal is required to be set up.

Refer to https://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovariance.html
and TwistWithCovariance.

How to set this matrix up?
The higher the number, the more it effects that particular matrix.
1.1 1.2 1.3
2.1 2.2 2.3

1.1 = x in relation to x.
etc...
Covariance matrix explained -> https://manialabs.wordpress.com/2012/08/06/covariance-matrices-with-a-practical-example/

Title: ROS Gazebo Odometry Issue


问:
Does Vehicle Yaw angle measures same at every point on it? If not, how to translate yaw rotation to COG of the vehicle from different point?
I am using an IMU to measure the yaw rotation angle of a car. I need to translate the rotation measured to COG of the vehicle. I can ignore roll and pitch as I am driving it on a flat surface. So, does yaw rotation is same at origin of IMU and at COG? If not, how to translate the measured rotation to COG of vehicle?

I went through some research papers, I could not quite follow things. Any help is appreciated.

答:
The best way to solve this is to position your IMU onto the Car's COG, so you can associate directly the IMU information to the COG.

In case that's not possible (design choices, space, etc), two things need to be done:

1) Find the transformation from the IMU to the COG. You need to find the translation and rotation from the IMU to the COG so you can compute any information given by the IMU in relation to the COG. If you're using ROS, TF2 solves that for you once you have the URDF.

2) Using the transformation, relate the changes in the IMU with changes in the COG. Every time you read information from the IMU (Force, acceleration, etc.), you have to transform that information in relation to the COG. The angular rate will change accordingly to the transform, for example. It's a challenging problem where there is not one single solution. Most solutions I found use an EKF for it.

Some papers I found:

https://www.ncbi.nlm.nih.gov/pmc/articles/PMC4208239/

https://ieeexplore.ieee.org/document/6728637

https://journals.sagepub.com/doi/abs/10.1177/0954410014550052


问:
Depth images with undetectable outlines
I'm trying to capture depth images with a RGBD camera, but all of the images have undetectable outlines around objects:

Is this expected from RGBD cameras or is mine a dud? It's an Astra Pro.

答:
That's perfectly normal and expected. Most RGBD camera work by projecting a point pattern into the world and capturing it with an IR-camera.

Camera and projector are positioned some cm apart (called baseline) to make triangulation possible. So there is always the chance that some parts of the world cannot be seen by the camera even though the projector put some pattern on them (and vice versa).

The camera also needs to see a small patch of the pattern and at depth jumps, some part of the pattern is missing.

Title: Depth images with undetectable outlines


问:
Robot detection in motion
In the environment, I have two robots and a couple of fixed obstacles. In order to detect obstacles I am using ultrasonic sensors. For robots, they need to detect each other and from which side other robot is coming (front, left, right or back), and do this during the motion. For this purpose, I cannot use PIR sensors, because robots are constantly moving. Also, I need to differentiate between moving robots and stationary obstacles, so ultrasonic sensors are also not helpful.

So I came up with idea to somehow mark the robots with some property unique to environment, so when we detect object with that property, we know that it is a robot, and not another obstacle. One of ideas might be to put lasers on one robot, and on other robot put four laser sensors, one for each side, so we precisely can say from which side the other robot came from. Another option might be to use IR transmitters on one robot and four IR receivers on other robot? What do you suggest, is there any other type of sensors that might help?

答: I would mark each robot with an AprilTag or anyone of the fiducial markers available. Than with an "easy" openCV program and a camera observer you can track the movements and the orientation for each robot distinguishing each one form the others relying on the tag id detected.</p>

Title: Robot detection in motion


问:
MPU-6050 as a compass not affected by magnet!
I am using an MPU-6050 as a compass connected to an Arduino Uno and using the AccZ register presumably from the accelerometer. So, first question, is the magnetometer signal processed internally with the magnetometer output to give true angle in relation to horizontal? Secondly, I placed a powerful magnet as close as I could to the chip and the output appeared to be unaffected. This is a perfect result for my application as my design required several magnets more than 7 cm away from the Sensor. Any clues?</p>

答:
So, first question, is the magnetometer signal processed internally with the magnetometer output to give true angle in relation to horizontal?

The MPU-6050 does not have a magnetometer.

Secondly, I placed a powerful magnet as close as I could to the chip and the output appeared to be unaffected. This is a perfect result for my application as my design required several magnets more than 7 cm away from the Sensor. Any clues?

There's no magnetometer, so on the one hand it's (probably) not going to be affected by magnets, but on the other hand there's nothing to give you a heading fix. Any heading you're getting from the device is based on numeric integration of the gyro outputs. The gyro output is not perfect, so you're going to have drift in the "heading" you get from the sensor output.