问:
Quadcopter instability with simple takeoff in autonomous mode
I'm trying to get a quad rotor to fly. The on board controller is an Ardupilot Mega 2.6, being programmed by Arduino 1.0.5.

I'm trying to fly it in simple autonomous mode, no Radio controller involved. I've done a thorough static weight balancing of the assembly (somewhat like this: http://www.youtube.com/watch?v=3nEvTeB2nX4) and the propellers are balanced correctly.

I'm trying to get the quadcopter to lift using this code:

```<code>#include <Servo.h>

int maxspeed = 155;
int minspeed = 0;

Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;

int val = 0;
int throttleCurveInitialGradient = 1;

void setup()
{

val = minspeed;

motor1.attach(7);
motor2.attach(8);
motor3.attach(11);
motor4.attach(12);

}

void loop()
{
setAllMotors(val);
delay(200);
val>maxspeed?true:val+=throttleCurveInitialGradient;
}

void setAllMotors(int val)
  {
    motor1.write(val);
    motor2.write(val);
    motor3.write(val);
    motor4.write(val);
  }
</code>```

But the issue is, as soon as the quadcopter takes off, it tilts heavily in one direction and topples over.

It looks like one of the motor/propeller is not generating enough thrust for that arm to take-off. I've even tried offsetting the weight balance against the direction that fails to lift, but it doesn't work (and I snapped a few propellers in the process);

Is there something wrong with the way the ESCs are being fired using
the Servo library?

If everything else fails, am I to assume there is something wrong
with the motors?

Do I need to implement a PID controller for self-balancing the roll
and pitch just to get this quadrotor to take off?

Edit 1:    Thanks for all the replies.

I got the PID in place. Actually, it is still a PD controller with the integral gain set to zero.

Here's how I'm writing the angles to the servo:

```<code>motor1.write((int)(val + (kP * pError1) +(kI * iError1) +(kD * dError1)));  //front left
motor2.write((int)(val + (kP * pError2) +(kI * iError2) +(kD * dError2)));  //rear right
motor3.write((int)(val + (kP * pError3) +(kI * iError3) +(kD * dError3)));  //front right
motor4.write((int)(val + (kP * pError4) +(kI * iError4) +(kD * dError4)));  //rear left
</code>```

kI is zero, so I'll ignore that.

With the value of kP set somewhere between 0.00051 to 0.00070, I'm getting an oscillation of steady amplitude around a supposed mean value. But the problem is, the amplitude of oscillation is way too high. It is somewhere around +/- 160 degrees, which looks crazy even on a tightly constrained test rig.

[  Edit 2: How I calculate the term 'pError' - Simple linear thresholding.

I've a precomputed data of the average readings (mean and SD) coming out of the gyro when the IMU is steady. Based on the gyro reading, I classify any motion of the setup as left, right, forward or backward.

For each of these motion, I increase the pError term for two of the motors, i.e, for right tilt, I add pError terms to motors 2 & 3, for left tilt, I add pError term to motors 1 & 4 etc. (check the comment lines in the code snippet given above).

The magnitude of error I assign to the pError term is = abs(current gyro reading) - abs(mean steady-state gyro reading). This value is always positive, therefore the side that is dipping downwards will always have a positive increment in RPM.  ]

As I crank up the derivative gain to around 0.0010 to 0.0015, the oscillation dampens rapidly and the drone comes to a relatively stable attitude hold, but not on the horizontal plane. The oscillation dies down (considerably, but not completely) only to give me a stable quadrotor tilted at 90 - 100 degrees with horizontal.

I'm using only the gyros for calculating the error. The gyros were self calibrated, hence I do expect a fair amount of noise and inaccuracy associated with the error values.

Do you think that is the primary reason for the high amplitude
oscillation?

One other probable reason might be the low update frequency of the errors. I'm updating the errors 6 times a second.

Could that be a probable reason it is taking longer to stabilise the
error?

And, for the steady state error after the wild oscillations dampen, is it necessary to fine tune the integral gain to get rid of that?

Please help.

Edit 3:  I cranked up the frequency of operation to 150+ Hz and what I get now is a very controlled oscillation (within +/- 10 degrees).

I'm yet to tune the derivative gain, following which I plan to recompute the errors for the integral gain using a combination of gyro and accelerometer data.

Edit 4:  I've tuned the P and D gain, resulting in +/- 5 degrees oscillation(approx). I can't get it to any lower than this, no matter how much I try.

There are two challenges about which I'm deeply concerned:

After 5 to 8 seconds of flight, the quadcopter is leaning into one side, albeit slowly.

A) Can this drift be controlled by tuning the integral gain?

B) Can the drift be controlled by using accelerometer + gyro fused data?

C) Given that my drone still shows +/- 5 degrees oscillation, can I consider this the optimal set point for the proportional and derivative gains? Or do I need to search more? (In which case, I'm really at my wits end here!)

答:
Even very small errors can bother the balancing. Small errors such as:

Weight of the quadcopter is unbalanced.

One motor is rotating faster/slower than others due to manufacturing or your power-source.

Air resistance and wind.

Unbalanced propellers due to manufacturing.

Strong magnetic forces.

You simple can not send the same motor speed to all motors and expect it to hover.
You do need a sensor which can read the tilting of the robot and adjust the motor speeds based on the tilt angles. To do that you need to implement a PID controller (The simplest method).

As far as I know there are some PID controller for arudino. But you could always write your own simple version of a PID controller.


问:
How can I draw a line using rotation of two circles?
I’m building a drawbot with a self imposed constraint that it’s method of motion is just two rotating disks. One holding the paper and one holding the pen. However, the math to calculate the right rotations in order to move the pen along a given line (eventually an entire vector line art image) eludes me. Can someone point me in the right direction?

I’ve attached an image of the basic idea/layout of the discs since that should make things clearer than my description. :) (note that in order to allow the Center of the pen disk to be outside the paper disk’s area, the pen disk is intentionally larger in size so it can still reach the center of the paper)

答:
Made a quick diagram and a couple of calculations in matlab, let me know if it works for you.

First of all, I am assuming you are considering your piece of paper as your reference coordinate system (CS), in the sense that the coordinates $(x_2, y_2)$, are the input to your problem. Also, I will consider $\theta_1$ and $\theta_3$, as the positions of your stepper motors, both measured from a horizontal line.

The CS $X_1Y_1$ is fixed to the ground. The CS $X_2Y_2$ on the other hand, shares the same origin as $X_1Y_1$ but rotates with the paper. See Figure 1.

We will assume that you already have certain geometric parameters such as: $R$ - distance from the big disk to the pen and $D$ - distance between disk's centers.

So, let's say you want to place the pen in a position $x_2, y_2$. There will be two possible locations where you can rotate the sheet of paper, if you want to meet the pen there. These locations are at the intersections of the two circles with radii $R$ and $r$ (being $r$ the radius of the small disk). I marked them with red crosses in the Figure 2.

$r$ is obtained as follows:

$$ r = \sqrt{x^{2}_{2} + y^{2}_{2}}$$
now, knowing $x_2,y_2$ also allows us to calculate $\alpha_{3}$ as:
$$\alpha_{3} = \arctan\left(\frac{y_2}{x_2}\right)$$

If we know $r$, by using $R$ and $D$, we can then find out the angles $\theta_1$ and $\theta_2$, thus:

$$\theta_1 = \arccos\left(\frac{R^2 + D^2 - r^2}{2RD}\right)$$ and
$$\theta_2 = \arccos\left(\frac{r^2 + D^2 - R^2}{2rD}\right)$$

These pair $\theta_1, \theta_2$ should satisfy the condition that the points are located where both circles cross.

Now, to calculate $\theta_3$, you apply:

$$ \theta_3 = 180 - \theta_2 - \alpha_{3}$$

Being $\theta_3$ the angle you have to rotate your small disk to be able to meet the pen at the intended position (defined using coordinates $x_2,y_2$).

Now, these set of equations are basically your inverse kinematics.

Now what you need to do is to generate a vector of points in the CS $X_2Y_2$, that will represent the line (you choose the spacing between points).

This vector of points allows you to calculate a vector of $r$ and $\alpha_3$ values as well.

With $r$ and $\alpha_3$, you will now obtain the combinations of positions for $\theta_1$ and $\theta_3$ that will draw the line.

P.S.: This equations don't account for singularities, so you will have to take into consideration the checks that need to be made in order to know before hand if the point you want to reach, is in the reachable space of the robot.

ContentLicense: CC BY-SA 4.0


问:
How to calculate potential enegy of manipulator dynamics?
I'm trying to obtain dynamics of a 4 DOF robot. Firstly, I calculated all Transformation matrices and Jacobians. While solving Lagrangian there is two main things one is kinetic energy and the other is potential energy . The kinetic energy is simplified into D matrix and Coriolis's factor, which to find the gravitational factor in lagrangian method , we need to find the potential energy. My problem is in that potential energy which is equal to P=mgh . I don't know what does that h correspond to and how to find the value of h . Could some one help me solving this issue ?

答:
To calculate the potential energy you need to define where robot reaches a potential energy zero.

For example, if you have a single rotating arm, that moves up and down, the usual is to consider that the zero potential energy is achieved when the robot is horizontal (we can imagine that at this position it collides with the floor).

In your case it's the same. So what you have indicated as $r_{c,i}$, corresponds to the distance between the center of mass of each robot link and this zero potential energy plane. Generally, there is one potential energy associated with each actuator, and this helps you to calculate the torque that each actuator needs to apply to keep the robot in an static position, that doesn't coincide with the zero potential energy plane.


问:
Is Lidar SLAM with only lidar sensor and no loop closure detection just a odometry?
I'm a newbee to SLAM. I have a question that in Lidar SLAM(including 2D lidar and 3D lidar), if I only use a single lidar without any other sensors like IMU and no loop closure in the mapping process, is this just a laser odometry? Can I get a map?

Another question is what's the relationship between loop closure detection and backend optimization in laser SLAM? Does there exist backend optimization without other sensors and loop closure detection?

Thanks a lot!

答:
The problem with defining the terms SLAM, and odometry is that various people define them differently, and sometimes even use them interchangeably. It can also change depending on what your main sensor is(camera vs LIDAR), and even what sort of feature representation you use (scan-matching vs landmark based).

So for this question I am going to use the terms odometry/SLAM in regards to a LIDAR sensor with a scan matching implementation.

if I only use a single lidar without any other sensors like IMU and no
loop closure in the mapping process, is this jsut a laser odometry?

You are mostly right about this. SLAM is just odometry with loop closure. However, you can use multiple sensors(IMU,GPS,...) and still have it be odometry. Sensor fusion doesn't define whether it is odometry or SLAM.

Can I get a map?

Yes. Since you know your poses from odometry you can still combine the LIDAR scans into a map. However, since you don't have loop closure you can't refine the map which is the key part of SLAM(SLA-MAPPING).

Another question is what's the relationship between loop closure
detection and backend optimization in laser SLAM? Does there exist
backend optimization without other sensors and loop closure detection?

When we are doing backend optimization we are essentially taking the history of past poses and trying to optimize them all together. However, this is useless without knowing any additional information. It would just give us the same result as running the odometry algorithm. So somehow you need to introduce additional information/constraints.

This is where loop closure comes in. If we know these two poses distant in time are actually connected. Then we have introduced new information.

Here is the pose graph of just odometry, and with a red loop closure link.

You can see with the loop closure we are adding new information(the extra edge).

Using a pose graph is a good way to visualize problem, because if you can think of any new ways of adding edges and nodes to this graph then you are introducing new information.

Some other options are a localization constraint. We know a pose is connected to some landmark/map with known position.

.

Another popular one is that we know the start and end location of the robot/car. So we fix those and optimize the positions of the rest.

.


问:
When is the Quadcopter out of the control?
Assuming ideal environment/hardware flying conditions, is it possible that the autopilot loses the control of the quadcopter?
What is the maximum tilt in roll and pitch that is allowed for the quadcopter to be under the control? How to drive that from the quadcopter dynamics model?

I do not have a strong mathematical/physics background. I had followed this report to understand the quadcopter dynamics.
https://andrew.gibiansky.com/downloads/pdf/Quadcopter%20Dynamics,%20Simulation,%20and%20Control.pdf

Any insights would be appreciated!

答:
I suppose it depends on what you mean by "under control". If you mean the point where the flight controller will stop being able to correct for the error, then I'm not sure that it's well defined. It will depend a lot on how sophisticated the flight control software is and how much time it has to correct(how long it will take for it to crash).

If you mean the maximum roll/pitch that can be maintained while flying, then that will also vary widely based on weight and thrust. If the craft tilts more than 90 degrees then there will be no downward thrust and it will lose altitude, but the angle at which it can maintain its altitude will vary drastically. A lightweight high thrust racing drone might be able to maintain more than 45 degrees without dropping, whereas a larger and heavier craft might not be able to go past 20 degrees. If you know the maximum thrust then you can figure out how much downward thrust it will produce at a given angle using the equation:

If the downward thrust is greater than or equal to the weight of your craft then it will be able to maintain it's altitude.

I hope that helps even if I can't give a precise answer.


问:
How can I integrate a smart phone with my robotics project?
Smart phones these days typically come with a gyroscope, accelerometer, compass, camera, and GPS sensor all on board. They also usually have a connection to the internet with Wifi and mobile data networks. I've seen many cases of using a phone as a remote control for a robot, but to me, it seems like the phone itself is a perfect lightweight computing and sensing platform for an autonomous robot.

The main obstacle I see is interfacing with actuators. Being able to control motors to steer even a table-top robot, or control servos, for example. Connecting and communicating to a microcontroller could be an obstacle as well.

As a robot hobbyist, I'd like to know how I can overcome these and other obstacles to be able to harness the power of my smart phone with my robotics projects.

答: Yes, the lack of GPIO pins on smartphones is a shame. If you already have ROS running on the robot you could use this app on an Android phone to get access to the sensor suite on the phone.</p>

ContentLicense: CC BY-SA 3.0


答: Quadcopter is inherently unstable system. So you have to apply some feedback controller (eg. PID) to keep it airborne. Even if you apply some basic PID using angular rates and angles, you still have to provide manual correction for drift till PID gains are perfectly set. So using radio control for manual control is really helpful during initial development stage.</p>

Title: Quadcopter instability with simple takeoff in autonomous mode

Body:
I'm trying to get a quad rotor to fly. The on board controller is an Ardupilot Mega 2.6, being programmed by Arduino 1.0.5.

I'm trying to fly it in simple autonomous mode, no Radio controller involved. I've done a thorough static weight balancing of the assembly (somewhat like this: http://www.youtube.com/watch?v=3nEvTeB2nX4) and the propellers are balanced correctly.

I'm trying to get the quadcopter to lift using this code:

```<code>#include <Servo.h>

int maxspeed = 155;
int minspeed = 0;

Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;

int val = 0;
int throttleCurveInitialGradient = 1;

void setup()
{

val = minspeed;

motor1.attach(7);
motor2.attach(8);
motor3.attach(11);
motor4.attach(12);

}

void loop()
{
setAllMotors(val);
delay(200);
val>maxspeed?true:val+=throttleCurveInitialGradient;
}

void setAllMotors(int val)
  {
    motor1.write(val);
    motor2.write(val);
    motor3.write(val);
    motor4.write(val);
  }
</code>```

But the issue is, as soon as the quadcopter takes off, it tilts heavily in one direction and topples over.

It looks like one of the motor/propeller is not generating enough thrust for that arm to take-off. I've even tried offsetting the weight balance against the direction that fails to lift, but it doesn't work (and I snapped a few propellers in the process);

Is there something wrong with the way the ESCs are being fired using
the Servo library?

If everything else fails, am I to assume there is something wrong
with the motors?

Do I need to implement a PID controller for self-balancing the roll
and pitch just to get this quadrotor to take off?

Edit 1:    Thanks for all the replies.

I got the PID in place. Actually, it is still a PD controller with the integral gain set to zero.

Here's how I'm writing the angles to the servo:

```<code>motor1.write((int)(val + (kP * pError1) +(kI * iError1) +(kD * dError1)));  //front left
motor2.write((int)(val + (kP * pError2) +(kI * iError2) +(kD * dError2)));  //rear right
motor3.write((int)(val + (kP * pError3) +(kI * iError3) +(kD * dError3)));  //front right
motor4.write((int)(val + (kP * pError4) +(kI * iError4) +(kD * dError4)));  //rear left
</code>```

kI is zero, so I'll ignore that.

With the value of kP set somewhere between 0.00051 to 0.00070, I'm getting an oscillation of steady amplitude around a supposed mean value. But the problem is, the amplitude of oscillation is way too high. It is somewhere around +/- 160 degrees, which looks crazy even on a tightly constrained test rig.

[  Edit 2: How I calculate the term 'pError' - Simple linear thresholding.

I've a precomputed data of the average readings (mean and SD) coming out of the gyro when the IMU is steady. Based on the gyro reading, I classify any motion of the setup as left, right, forward or backward.

For each of these motion, I increase the pError term for two of the motors, i.e, for right tilt, I add pError terms to motors 2 & 3, for left tilt, I add pError term to motors 1 & 4 etc. (check the comment lines in the code snippet given above).

The magnitude of error I assign to the pError term is = abs(current gyro reading) - abs(mean steady-state gyro reading). This value is always positive, therefore the side that is dipping downwards will always have a positive increment in RPM.  ]

As I crank up the derivative gain to around 0.0010 to 0.0015, the oscillation dampens rapidly and the drone comes to a relatively stable attitude hold, but not on the horizontal plane. The oscillation dies down (considerably, but not completely) only to give me a stable quadrotor tilted at 90 - 100 degrees with horizontal.

I'm using only the gyros for calculating the error. The gyros were self calibrated, hence I do expect a fair amount of noise and inaccuracy associated with the error values.

Do you think that is the primary reason for the high amplitude
oscillation?

One other probable reason might be the low update frequency of the errors. I'm updating the errors 6 times a second.

Could that be a probable reason it is taking longer to stabilise the
error?

And, for the steady state error after the wild oscillations dampen, is it necessary to fine tune the integral gain to get rid of that?

Please help.

Edit 3:  I cranked up the frequency of operation to 150+ Hz and what I get now is a very controlled oscillation (within +/- 10 degrees).

I'm yet to tune the derivative gain, following which I plan to recompute the errors for the integral gain using a combination of gyro and accelerometer data.

Edit 4:  I've tuned the P and D gain, resulting in +/- 5 degrees oscillation(approx). I can't get it to any lower than this, no matter how much I try.

There are two challenges about which I'm deeply concerned:

After 5 to 8 seconds of flight, the quadcopter is leaning into one side, albeit slowly.

A) Can this drift be controlled by tuning the integral gain?

B) Can the drift be controlled by using accelerometer + gyro fused data?

C) Given that my drone still shows +/- 5 degrees oscillation, can I consider this the optimal set point for the proportional and derivative gains? Or do I need to search more? (In which case, I'm really at my wits end here!)


问:
Kalman Filter with multiple inputs
Let's say I have one laser scanner and a radar device, which I should use to measure a distance to a wall (Fig. 1). Both devices are place on the same support, so... they should measure the same distance (+/- the tolerances of the single device). It is an ideal scenary, so both devices are measuring the distance perfect perpendicular to the wall. Both tolerances of the devices are taken from the datasheet of the device. So the values of $\delta r_{1}$ and $\delta r_{2}$ are known. We consider the case, where those values are normal distributed.

Now... I need to fuse both measures to one value using a Kalman filter.
Reading here and here I can imagine two way to achieve this goal:

From both values and measures I calculate the mean value and the new variance by simply computing:
$$\mu_{new} = \frac{\mu_{1}\delta r_{2}^{2} + \mu_{2}\delta r_{1}^{2}}{\delta r_{1}^{2} + \delta r_{2}^{2}}
$$
and
$$\delta_{new} = \frac{1}{\frac{1}{\delta r_{1}^{2}} + \frac{1}{\delta r_{2}^{2}}}
$$
where $\mu_{1}$ and $\mu_{2}$ are the mean values of the measures from device 1 and 2 respectively (a serie of measurements is undergone).
After calculating those values I apply the Kalman Filter algorithm, as I would handle a single input, single output system. So there is nothing new and or special in this case. By calculating the mean value and the mean variance of both devices together I can use any kalman filter implementation I want.

I would like to keep the measures and the variances separated and I don't want to calculate the mean values ($\mu_{new}, \delta_{new}$) like (1) above, for istance because they are taken with different time intervalls or for some other reasons...
Anyway... I need to rewrite my time update equations:
$$ \hat{\mathbf{x}}_{n+1,n} = \mathbf{F}\hat{\mathbf{x}}_{n,n} + \mathbf{G}\hat{\mathbf{u}}_{n,n} $$
and $$ \mathbf{P}_{n+1,n} = \mathbf{F}\mathbf{P}\mathbf{F}^{T} + \mathbf{Q}$$
in such a way, that I can just provide both measures $\mathbf{z}_{n}$ from both devices and then the new state prediction is calculated at every step.
But since my state vector is a position $\mathbf{x} = [x, y, z]$ I can't figure out, how I can treat the 2 measures independently at every iteration step (of course we consider in this case only the $x$ axis, since $y$ and $z$ can be put 0. Just for simplification).

Any idea?

答:
In fact you can update the filter's state using the input from each sensor separately, you just need to use a different covariance matrix $\mathbf{R}$ and observation matrix $\mathbf{H}$ depending on the sensor. This will only affect the update step — the prediction step equations (the ones you referred to) are computed the same way regardless of the reading's source.

Years ago I wrote a simple program for this exact scenario of running an EKF on LiDAR and RADAR data, you can find it here if you'd like to take a look.

Title: Kalman Filter with multiple inputs


问:
Controlling more than 12 Servos with the Arduino Servo library
I'm using Teensy hardware specifically.  I have a Teensy 2.0 and a Teensy 3.0, and from the documentation it seems like there are two 16 bit timers available, and each should be able to control 12 servos.  However, I've attached a logic analyzer and have confirmed that only the first 12 servos attached ever function.

Is there anything special I have to do with my sketch in order to convince the Servo library to allocate the second timer for servos attached beyond number 12?

This works:

```<code>#define NUM_SERVOS 12

Servo servos[NUM_SERVOS];

// teensy 2.0 pins
int pin_assignments[NUM_SERVOS] = {0, 1, 2, 3, 4, 5, 20, 19, 18, 17, 16, 15};

void setup() {
  for(int i = 0; i < NUM_SERVOS; i++) {
    servos[i].attach(pin_assignments[i]) ;
  }
}
</code>```

But this, below, only ever shows activity on the first twelve pins attached:

```<code>#define NUM_SERVOS 24

Servo servos[NUM_SERVOS];

// teensy 3.0 pins
int pin_assignments[NUM_SERVOS] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23};

void setup() {
  for(int i = 0; i < NUM_SERVOS; i++) {
    servos[i].attach(pin_assignments[i]) ;
  }
}
</code>```

答:
You only need to chage the value of:

#define SERVOS_PER_TIMER       12     // the maximum number of servos controlled by one timer

Change 12 for your value.


答: Instead of driving the servos with the Teensy, you could use a servo driver. The one in the link has an I2C servodriver that handles 16 servos and can be chained together to address more servos. This will take the load off the microcontroller and leave processing power for other tasks.</p>

Title: Controlling more than 12 Servos with the Arduino Servo library

Body:
I'm using Teensy hardware specifically.  I have a Teensy 2.0 and a Teensy 3.0, and from the documentation it seems like there are two 16 bit timers available, and each should be able to control 12 servos.  However, I've attached a logic analyzer and have confirmed that only the first 12 servos attached ever function.

Is there anything special I have to do with my sketch in order to convince the Servo library to allocate the second timer for servos attached beyond number 12?

This works:

```<code>#define NUM_SERVOS 12

Servo servos[NUM_SERVOS];

// teensy 2.0 pins
int pin_assignments[NUM_SERVOS] = {0, 1, 2, 3, 4, 5, 20, 19, 18, 17, 16, 15};

void setup() {
  for(int i = 0; i < NUM_SERVOS; i++) {
    servos[i].attach(pin_assignments[i]) ;
  }
}
</code>```

But this, below, only ever shows activity on the first twelve pins attached:

```<code>#define NUM_SERVOS 24

Servo servos[NUM_SERVOS];

// teensy 3.0 pins
int pin_assignments[NUM_SERVOS] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23};

void setup() {
  for(int i = 0; i < NUM_SERVOS; i++) {
    servos[i].attach(pin_assignments[i]) ;
  }
}
</code>```


问:
What is the Point of Monocular SLAM
I am not a V-SLAM expert yet, but as far as I understand with monocular V-SLAM there is a scale ambiguity introduced by the fact that a camera essentially is an azimuth sensor that maps the 3D world to the 2D world via a projectivity.

In particular I was reading/using orb_slam_2_ros and came across the following:

This is the ROS implementation of the ORB-SLAM2 real-time SLAM library for Monocular, Stereo and RGB-D cameras that computes the camera trajectory and a sparse 3D reconstruction (in the stereo and RGB-D case with true scale).

So my question is, from a practical perspective, if with monocular SLAM you don't have "true scale" then what is the point, what useful information would monocular V-SLAM give to a mobile robot trying to localize in a space?

Thanks!
Cheers

答:
I would say true scale actually doesn't really matter. A good way to reason about this is actually video games. If you play a racing game(or really any kind of 3D game) do you care that the world has the proper scale? No you don't. If I went in and modified the video game to double the size/scale of all the models you would still be able to play it.

So it is possible to setup your system to navigate a world without knowing the true scale. It is what you do when you play a video game. A SLAM map still has a use in this world as it can be used to localize your position/build a map.

The reason true scale is used in the real world is that it unifys the units of your other sensors, and devices like motors,encoders,... . E.g. if your motors makes you move 1 m/s you know exactly how it will interact with some other sensor.

The are other tasks you can do that also don't require knowing the scale e.g. 3D reconstruction or structure from motion(computer vision terminology for monocular SLAM). We only care about the quality of the 3D reconstruction. The scale doesn't really matter. This scaleless 3D model can then be used in things like video games, 3D printing, and more which all don't care about the final scale of the model. If you do need true scale, you can also always scale it later to the correct size. E.g I know this object is 1 meter tall so scale the whole scene by the appropriate amount.

If you do need scale then another alternative is to is to fuse the monocular SLAM results in a loosely coupled fashion with an IMU or other sensor. So your SLAM algorithm may not be aware of the true scale, but your fused estimator algorithm is. A lot of of visual SLAM work on drones, was implemented this way before tightly coupled implementation became the standard.

Finally I would argue the lack of true scale is not the main problem of monocular SLAM algorithms, it is scale drift. As long as your map has the same consistent scale then everything works out. Problems start to occur when different parts of the map have different scales. This means reasoning in one area doesn't work in another.

Title: What is the Point of Monocular SLAM


问:
how to power extra usb devices for beaglebone black
I'm building a robot that uses a beaglebone black, however I have several different usb devices that I want to connect to it (microphone, usb sound device and some other things). Now I have heard that the usb output of the beaglebone doesn't power more then 0.1A. So the combined draw of these usb devices is likely to exceed this by a fair margin. So I started looking for powered usb hubs to use instead. However these tend to be powered by 220V and my robot currently only has a 12V power supply and a converter to 5V for the beaglebone. Which given the size expense and inefficiency of converting power to 220 from 12V and then back again doesn't seem very good. Is there a good method for fixing this? </p>

答: The USB1 host port can deliver 500mA if powered from 5V 2A</p>


问:
Finding the DH parameters from Transformation Matrices
My questions is the following: How do I find the DH values of a given transformation matrix? Or a set of transformation matrices if one is not enough?
I can assume that the angle theta is known for each of them and all other DH parameters are the same accross transformation matrices.

thank you guys,
Alex

答:
Consider the expression at the bottom as the transformation between one system and the next.

If you have the numeric values of this transformation (from coordinate system $i$, to coordinate system $i+1$) you could use the elements of the matrix in equation 3.10 as follows:

$$\theta_i = \arctan \left(\frac{A_{2,1}}{A_{1,1}} \right)$$

$$\alpha_i = \arctan \left(\frac{A_{3,2}}{A_{3,3}} \right)$$
Knowing $\theta_i$, you can then find $a_i$ easily as:

$$a_i=\frac{A_{1,4}}{\cos(\theta_i)}$$

Finally, $d_i=A_{3,4}$

equation taken from here


问:
Differential Drive using shopping cart-style casters
I thought I would be clever and use TWO shopping-cart-style casters (more stable) instead of the ONE usual omnidirectional ball caster. Now that I am running it I am almost positive that the kinematics are totally different and in fact very hard to model.

Can someone confirm or deny this belief, and give any context (links etc.) so I can dig deeper?

答: You might take a look at this paper dealing with a powered wheelchair, which has two caster wheels.  https://hal.inria.fr/hal-02124781/document</p>


问:
Does it matter how I mount an IMU board?
I am going through the board's calibration process and it is declaring success. Do I still need to pay attention to how I mount the board, that is, match the x,y,z directions printed on the board?</p>

答:
Not necessarily, so long you're aware how the IMU's mounting orientation may affect readings.

I don't know what IMU model you're using or how, but assuming it integrates a gyroscope and accelerometer, and you're using readings from both to compute roll, pitch and yaw, you must be aware how results will be affected by the device's orientation relative to its host device (e.g. a robot) and the Earth's surface.

For example, if you mount the IMU at an angle, this will result in non-zero roll and pitch readings; if it's upside-down, this will reverse the direction of yaw changes. Depending on the intended application, you'll have to account for that in code.

In general it's worth the trouble avoiding these complications and just mounting the IMU in a way that agrees with the host device's orientation conventions. However you're free to do otherwise, so long you know what you're doing.


问:
GPS Course vs IMU Course
Im currently working with Kalman Filter for position and velocity, one of the important parameters that im using is the heading that the sensor fusion of the imu gives me, but i have seen that the GPS also provides me one heading called course over the ground, so i think that i will put the heading information as a state variable as well and compare both heading to produce a better estimate, but i want to know how the GPS calculate this information and have not found some valuable information, is this angle with reference to magnetic north? Can i compare these two headings? or they are distinct information. The figure below in orange is the course that i get with gps and in blue is the heading with sensor fusion that IMU provides me, i collected this data with a displacement around my building and recorded it into a sd card. With these data, it seems a good idea to fuse them to get a better estimate of the actual heading.

Now im working with a linear kalman filter,and keep north, east coordinate and north, east velocity. The IMU BNO085 makes the sensor fusion for me, with this i use the heading to get the north and east velocity, using the velocity that is coming from the GPS rmc sentence. And with the R-P-Y angles i rotate the acceleration from body frame to NED frame. With the Lat lon that is coming from the GPS i get the ned coordinates. Im using the equations below to use as the state transition matrix, where $N_k$ is the north coordinate, $E_k$ is the east coordinate, $Vn_k$ is the north velocity abd $Ve_k$ is the east velocity:

$N_k = N_{k-1} + Vn_k * dt +\frac{A_e * dt^2}{2}  $

$Vn_k = Vn_{k-1} + A_n * dt   $

$E_k =  E_{k-1} + Ve_k * dt +\frac{A_e * dt^2}{2}  $

$Ve_k = Ve_{k-1} + A_e * dt $

Since the velocity that is coming from the GPS isn't in north and east coordinate i use the heading to get this information as below:

$V_n = V_{GPS} * cos(heading) $

$V_e = V_{GPS} * sin(heading) $

This velocity goes to measurement vector and its used at update step of kalman filter together with GPS LAT-LON converted to ned coordinate.

I have GPS and IMU as the sensors, now im trying to increase the accuracy of the results, so im learning about unscented kalman filter and trying to increase the number of state variables. Any suggestions are welcome.
Below is the last result i got walking with the device. In red are the gps data and in blue the filter data.

答:
So this probably won't work as course over ground and heading are often 2 different things. Heading is the direction your vehicle is facing, while course over ground is the direction your vehicle is traveling.

Imagine a boat facing north, and it is in a drift current flowing east to west. Your heading is north, but your course over ground is west, as the current is moving the boat westward.

As for how it is calculated it is simply the difference between a start and end position. So the GPS simply continuously records a start and end position over a time interval(e.g 5 seconds). So simply the vector $v_{cog}=v_{end}-v_{start}=[x_{end}-x_{start},y_{end}-y_{start}]$. If it reports an angle rather than a vector then they probably just use arctan2 to convert it.

Title: GPS Course vs IMU Course


答:
If GPS course heading angle is computed as edwinem described — that is, as the slope of a line crossing the sensor's current and previous coordinates in $(x, y)$ notation — and the robot's always headed towards the direction of movement, then IMU heading and GPS heading readings are relative to the same reference frame, and they can be directly fed to the Kalman filter in the update step.

Even if course heading doesn't exactly agree with the direction the robot is pointed to, so long it's never completely off, that can still be accounted for in the variance attributed to the measurement.

Title: GPS Course vs IMU Course

Body:
Im currently working with Kalman Filter for position and velocity, one of the important parameters that im using is the heading that the sensor fusion of the imu gives me, but i have seen that the GPS also provides me one heading called course over the ground, so i think that i will put the heading information as a state variable as well and compare both heading to produce a better estimate, but i want to know how the GPS calculate this information and have not found some valuable information, is this angle with reference to magnetic north? Can i compare these two headings? or they are distinct information. The figure below in orange is the course that i get with gps and in blue is the heading with sensor fusion that IMU provides me, i collected this data with a displacement around my building and recorded it into a sd card. With these data, it seems a good idea to fuse them to get a better estimate of the actual heading.

Now im working with a linear kalman filter,and keep north, east coordinate and north, east velocity. The IMU BNO085 makes the sensor fusion for me, with this i use the heading to get the north and east velocity, using the velocity that is coming from the GPS rmc sentence. And with the R-P-Y angles i rotate the acceleration from body frame to NED frame. With the Lat lon that is coming from the GPS i get the ned coordinates. Im using the equations below to use as the state transition matrix, where $N_k$ is the north coordinate, $E_k$ is the east coordinate, $Vn_k$ is the north velocity abd $Ve_k$ is the east velocity:

$N_k = N_{k-1} + Vn_k * dt +\frac{A_e * dt^2}{2}  $

$Vn_k = Vn_{k-1} + A_n * dt   $

$E_k =  E_{k-1} + Ve_k * dt +\frac{A_e * dt^2}{2}  $

$Ve_k = Ve_{k-1} + A_e * dt $

Since the velocity that is coming from the GPS isn't in north and east coordinate i use the heading to get this information as below:

$V_n = V_{GPS} * cos(heading) $

$V_e = V_{GPS} * sin(heading) $

This velocity goes to measurement vector and its used at update step of kalman filter together with GPS LAT-LON converted to ned coordinate.

I have GPS and IMU as the sensors, now im trying to increase the accuracy of the results, so im learning about unscented kalman filter and trying to increase the number of state variables. Any suggestions are welcome.
Below is the last result i got walking with the device. In red are the gps data and in blue the filter data.


问:
Multi-Agent simulation causing trouble in Rviz
I'm trying multi-agent simulations using ROSBOT 2.0 by husarion. I'm comfortable with single agent but when it comes to multiple agents I'm able to spawn them and control them independently but the problem comes with Rviz. I'm unable to visualize the robot in Rviz. It spwans upside down, i'm unable to see the camera feed and lidar output but the topics are present in the rostopic list.

The below is the launch file which I run.

```<code><launch>
  <arg name="world_name" default="worlds/empty.world"/>

  <include file="<span class="math-container">$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$</span>(arg world_name)"/> <!-- world_name is wrt GAZEBO_RESOURCE_PATH environment variable -->
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="true"/>
    <arg name="verbose" value="true"/>
  </include>
  <include file="$(find rosbot_gazebo)/launch/multi_rosbot.launch"/>

</launch>
</code>```

Below is multi_rosbot.launch file.

```<code><launch>
  <param name="robot_description" command="<span class="math-container">$(find xacro)/xacro.py '$</span>(find rosbot_description)/urdf/rosbot.xacro'"/>

  <!-- BEGIN ROBOT 1-->
  <group ns="rover1">
    <param name="tf_prefix" value="rover1" />
    <include file="$(find rosbot_gazebo)/launch/rosbot_empty_world.launch" >
      <arg name="init_pose" value="-x 0 -y 0 -z 0" />
      <arg name="robot_name"  value="rover1" />
    </include>
  </group>

  <!-- BEGIN ROBOT 2-->
  <group ns="rover2">
    <param name="tf_prefix" value="rover2" />
    <include file="$(find rosbot_gazebo)/launch/rosbot_empty_world.launch" >
      <arg name="init_pose" value="-x -2 -y 0 -z 0" />
      <arg name="robot_name"  value="rover2" />
    </include>
  </group>

  <!-- BEGIN ROBOT 3 -->
  <group ns="rover3">
    <param name="tf_prefix" value="rover3" />
    <include file="$(find rosbot_gazebo)/launch/rosbot_empty_world.launch" >
      <arg name="init_pose" value="-x -4 -y 0 -z 0" />
      <arg name="robot_name"  value="rover3" />
    </include>
  </group>
</launch>
</code>```

Below is the rosbot_empty_world.launch

```<code><launch>

  <arg name="init_pose"/>
  <arg name="robot_name"/>
  <param name="robot_description" command="<span class="math-container">$(find xacro)/xacro.py '$</span>(find rosbot_description)/urdf/rosbot.xacro'"/>
  <arg name="model" default="<span class="math-container">$(find xacro)/xacro.py '$</span>(find rosbot_description)/urdf/rosbot.xacro'"/>

  <node name="rosbot_spawn" pkg="gazebo_ros" type="spawn_model" ns="/<span class="math-container">$(arg robot_name)"
        args="$</span>(arg init_pose) -unpause -urdf -model $(arg robot_name) -param /robot_description " respawn="false" output="screen"/>

  <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher" ns="/<span class="math-container">$(arg robot_name)" args="--namespace=/$</span>(arg robot_name)">
    <param name="publish_frequency" type="double" value="30.0" />
    <param name="tf_prefix" value="/<span class="math-container">$(arg robot_name)" type="str"/>
  </node>
  <rosparam command="load" file="$</span>(find joint_state_controller)/joint_state_controller.yaml" ns="/$(arg robot_name)"/>

  <node name="controller_spawner_<span class="math-container">$(arg robot_name)" pkg="controller_manager" type="spawner" ns="/$</span>(arg robot_name)"
        respawn="false" args=" joint_state_controller --namespace=/$(arg robot_name)" output="screen"/>

</launch>
</code>```

I cannot visualize anything in Rviz. Therobot spawns upside down, the camera is blank.

Here's the rostopic list output.

```<code>wally1002@wally:~$ rostopic list
    /clock
    /gazebo/link_states
    /gazebo/model_states
    /gazebo/parameter_descriptions
    /gazebo/parameter_updates
    /gazebo/set_link_state
    /gazebo/set_model_state
    /rosout
    /rosout_agg
    /rover1/camera/depth/camera_info
    /rover1/camera/depth/image_raw
    /rover1/camera/depth/points
    /rover1/camera/parameter_descriptions
    /rover1/camera/parameter_updates
    /rover1/camera/rgb/camera_info
    /rover1/camera/rgb/image_raw
    /rover1/camera/rgb/image_raw/compressed
    /rover1/camera/rgb/image_raw/compressed/parameter_descriptions
    /rover1/camera/rgb/image_raw/compressed/parameter_updates
    /rover1/camera/rgb/image_raw/compressedDepth
    /rover1/camera/rgb/image_raw/compressedDepth/parameter_descriptions
    /rover1/camera/rgb/image_raw/compressedDepth/parameter_updates
    /rover1/camera/rgb/image_raw/theora
    /rover1/camera/rgb/image_raw/theora/parameter_descriptions
    /rover1/camera/rgb/image_raw/theora/parameter_updates
    /rover1/cmd_vel
    /rover1/imu
    /rover1/joint_states
    /rover1/odom
    /rover1/range/fl
    /rover1/range/fr
    /rover1/range/rl
    /rover1/range/rr
    /rover1/scan
    /rover2/camera/depth/camera_info
    /rover2/camera/depth/image_raw
    /rover2/camera/depth/points
    /rover2/camera/parameter_descriptions
    /rover2/camera/parameter_updates
    /rover2/camera/rgb/camera_info
    /rover2/camera/rgb/image_raw
    /rover2/camera/rgb/image_raw/compressed
    /rover2/camera/rgb/image_raw/compressed/parameter_descriptions
    /rover2/camera/rgb/image_raw/compressed/parameter_updates
    /rover2/camera/rgb/image_raw/compressedDepth
    /rover2/camera/rgb/image_raw/compressedDepth/parameter_descriptions
    /rover2/camera/rgb/image_raw/compressedDepth/parameter_updates
    /rover2/camera/rgb/image_raw/theora
    /rover2/camera/rgb/image_raw/theora/parameter_descriptions
    /rover2/camera/rgb/image_raw/theora/parameter_updates
    /rover2/cmd_vel
    /rover2/imu
    /rover2/joint_states
    /rover2/odom
    /rover2/range/fl
    /rover2/range/fr
    /rover2/range/rl
    /rover2/range/rr
    /rover2/scan
    /rover3/camera/depth/camera_info
    /rover3/camera/depth/image_raw
    /rover3/camera/depth/points
    /rover3/camera/parameter_descriptions
    /rover3/camera/parameter_updates
    /rover3/camera/rgb/camera_info
    /rover3/camera/rgb/image_raw
    /rover3/camera/rgb/image_raw/compressed
    /rover3/camera/rgb/image_raw/compressed/parameter_descriptions
    /rover3/camera/rgb/image_raw/compressed/parameter_updates
    /rover3/camera/rgb/image_raw/compressedDepth
    /rover3/camera/rgb/image_raw/compressedDepth/parameter_descriptions
    /rover3/camera/rgb/image_raw/compressedDepth/parameter_updates
    /rover3/camera/rgb/image_raw/theora
    /rover3/camera/rgb/image_raw/theora/parameter_descriptions
    /rover3/camera/rgb/image_raw/theora/parameter_updates
    /rover3/cmd_vel
    /rover3/imu
    /rover3/joint_states
    /rover3/odom
    /rover3/range/fl
    /rover3/range/fr
    /rover3/range/rl
    /rover3/range/rr
    /rover3/scan
    /tf
    /tf_static
</code>```

I'm unable to solve this. Any help is appreciated. Thank You.

答:
I have managed to solve the problem.

It was a problem of Fixed Frame used in the visualization

tf_prefix was not used correctly.

You can look at this answer for more details. Thanks to anyone who tried.


问:
How to solve this differential equation using ode23?

Hello! :) What I have here is a gear system I need to simulate using
MATLAB and Simulink.

To do that, firstly I need to get the differential equation for this
rotation (gear) system. I think I managed to do so and it is as
follows iT1 = J * theta2''+D * theta2'+K * theta2

, where J = i^2 * Jn1 + Jn2 + J2 (also, i is the gear ratio; i = theta1/theta2 = N2/N1 = Tn2 / Tn1 = T2/T1).

Now I need to solve this second-order differential equation using
ode23 in MATLAB. The problem is, I don't know any initial conditions,
nor do I know how to get them from this particular system e.g. y(0) = 0, y'(0) = 1 Can I still solve this particular problem without the
initial conditions, or do I make the initial conditions up...? In case
anyone needs it the parameters of my problem are as follows:

N2 = 90, N1 = 36, Jn1 = 0.5 Nms^2/rad, Jn2 = 0.8 Nms^2/rad, J2 = 2 Nms^2/rad, D = 8 Nms/rad, K = 5 Nm/rad

The differential equation should look like this for y=theta2 and
x=T1

```<code>y''+1.3502y'+0.8439y = 0.4219x
</code>```

Any help would be really appreciated! :)

答:
You can certainly employ ode23 to solve this ordinary differential equation, but it's much simpler if you recruit the state space approach via ss and then lsim.

The response of a linear system is always the aggregate of its forced (with a null initial state) and unforced evolutions (with $T_1=0$ and a given initial state $\mathbf{x}_0$).

The unforced evolution can be determined by means of initial.

So, to answer your direct question, the initial conditions (read the initial state) are crucial as they establish part of the transient response of the system.

% parameters
N2  = 90;
N1  = 36;
Jn1 = 0.5;
Jn2 = 0.8;
J2  = 2;
D   = 8;
K   = 5;
J   = (N2/N1)^2 * Jn1 + Jn2 + J2;

% define the system
sys = ss([0 1; -K/J -D/J], [0; N2/(N1*J)], [1 0], 0);

% initial state: (position, velocity) [rad; rad/s]
x0 = [0.1; 0];

% define the time span
t = linspace(0, 15, 10000)';

% define the input step
T1 = zeros(length(t), 1);
T1(t>=1) = 1;

% compute the system step response at once
theta1 = lsim(sys, T1, t, x0);

% compute the system response as aggregate of the forced and unforced
% temporal evolutions
theta2 = lsim(sys, T1, t, [0; 0]) + initial(sys, x0, t);

% plot results
figure('color', 'white');
hold on;
yyaxis left;
plot(t, T1, '-.', 'linewidth', 2);
ylabel('[N]');
yyaxis right;
plot(t, theta1, 'linewidth', 3);
plot(t, theta2, 'k--');
xlabel('t [s]');
ylabel('[rad]');
grid minor;
legend({'$T_1$', '$\theta_1$', '$\theta_2$'}, 'Interpreter', 'latex',...
       'location', 'southeast');
hold off;
```
<p>Here's the resulting plot:</p>
<p><img src="https://i.stack.imgur.com/gdUJ4.png" alt="enter image description here" /></p>
<hr />
<p>If you really want to use <code>ode23</code>, then you can run the following snippet:</p>
<pre class="lang-matlab prettyprint-override"><code>% initial state: (position, velocity) [rad; rad/s]
x0 = [0.1; 0];

% define the time span
t = linspace(0, 15, 10000)';

% integrate the differential equation
[t, x] = ode23(@fun, t, x0);

% plot results
figure('color', 'white');
plot(t, x(:, 1));
xlabel('t [s]');
ylabel('[rad]');
grid minor;
legend({'<span class="math-container">$\theta$</span>'}, 'Interpreter', 'latex', 'location', 'southeast');

function g = fun(t, x)
% parameters
N2  = 90;
N1  = 36;
Jn1 = 0.5;
Jn2 = 0.8;
J2  = 2;
D   = 8;
K   = 5;
J   = (N2/N1)^2 * Jn1 + Jn2 + J2;

% compute gradient
g = zeros(2, 1);
g(1) = x(2);
g(2) = (-K/J)*x(1) + (-D/J)*x(2) + (N2/(N1*J)*(t>=1));
end
</code>```

yielding the graph below:

Finally, if you aim to integrate a generic input as a function of time $t$, you could resort to the code below:

function x = integrate(t, u, x0)
    % parameters
    N2  = 90;
    N1  = 36;
    Jn1 = 0.5;
    Jn2 = 0.8;
    J2  = 2;
    D   = 8;
    K   = 5;
    J   = (N2/N1)^2 * Jn1 + Jn2 + J2;

    % integrate the differential equation
    [t, x] = ode23(@fun, t, x0);

    % plot results
    figure('color', 'white');

    % plot position
    yyaxis left;
    plot(t, x(:, 1));
    ylabel('$x$ [rad]', 'Interpreter', 'latex');

    % plot velocity
    yyaxis right;
    plot(t, x(:, 2));
    ylabel('$\dot{x}$ [rad/s]', 'Interpreter', 'latex');

    grid minor;
    xlabel('$t$ [s]', 'Interpreter', 'latex');

    function g = fun(t, x)
        g = zeros(2, 1);
        g(1) = x(2);
        g(2) = (-K/J)*x(1) + (-D/J)*x(2) + (N2/(N1*J)*u(t));
    end
end
```
<p>You can now employ anonymous functions to specify the temporal evolution of the input. For example, if the input is sinusoidal with pulse frequency <span class="math-container">$\omega = 0.5 \; \text{rad/s}$</span>, you'll need to run:</p>
<pre class="lang-matlab prettyprint-override"><code>t = linspace(0, 15, 10000)';
x0 = [0.1; 0];
integrate(t, @(t)(sin(0.5*t)), x0);
</code>```

to get:

Body:
You can certainly employ ode23 to solve this ordinary differential equation, but it's much simpler if you recruit the state space approach via ss and then lsim.

The response of a linear system is always the aggregate of its forced (with a null initial state) and unforced evolutions (with $T_1=0$ and a given initial state $\mathbf{x}_0$).

The unforced evolution can be determined by means of initial.

So, to answer your direct question, the initial conditions (read the initial state) are crucial as they establish part of the transient response of the system.

% parameters
N2  = 90;
N1  = 36;
Jn1 = 0.5;
Jn2 = 0.8;
J2  = 2;
D   = 8;
K   = 5;
J   = (N2/N1)^2 * Jn1 + Jn2 + J2;

% define the system
sys = ss([0 1; -K/J -D/J], [0; N2/(N1*J)], [1 0], 0);

% initial state: (position, velocity) [rad; rad/s]
x0 = [0.1; 0];

% define the time span
t = linspace(0, 15, 10000)';

% define the input step
T1 = zeros(length(t), 1);
T1(t>=1) = 1;

% compute the system step response at once
theta1 = lsim(sys, T1, t, x0);

% compute the system response as aggregate of the forced and unforced
% temporal evolutions
theta2 = lsim(sys, T1, t, [0; 0]) + initial(sys, x0, t);

% plot results
figure('color', 'white');
hold on;
yyaxis left;
plot(t, T1, '-.', 'linewidth', 2);
ylabel('[N]');
yyaxis right;
plot(t, theta1, 'linewidth', 3);
plot(t, theta2, 'k--');
xlabel('t [s]');
ylabel('[rad]');
grid minor;
legend({'$T_1$', '$\theta_1$', '$\theta_2$'}, 'Interpreter', 'latex',...
       'location', 'southeast');
hold off;

Here's the resulting plot:

If you really want to use ode23, then you can run the following snippet:

% initial state: (position, velocity) [rad; rad/s]
x0 = [0.1; 0];

% define the time span
t = linspace(0, 15, 10000)';

% integrate the differential equation
[t, x] = ode23(@fun, t, x0);

% plot results
figure('color', 'white');
plot(t, x(:, 1));
xlabel('t [s]');
ylabel('[rad]');
grid minor;
legend({'$\theta$'}, 'Interpreter', 'latex', 'location', 'southeast');

function g = fun(t, x)
% parameters
N2  = 90;
N1  = 36;
Jn1 = 0.5;
Jn2 = 0.8;
J2  = 2;
D   = 8;
K   = 5;
J   = (N2/N1)^2 * Jn1 + Jn2 + J2;

% compute gradient
g = zeros(2, 1);
g(1) = x(2);
g(2) = (-K/J)*x(1) + (-D/J)*x(2) + (N2/(N1*J)*(t>=1));
end

yielding the graph below:

Finally, if you aim to integrate a generic input as a function of time $t$, you could resort to the code below:

function x = integrate(t, u, x0)
    % parameters
    N2  = 90;
    N1  = 36;
    Jn1 = 0.5;
    Jn2 = 0.8;
    J2  = 2;
    D   = 8;
    K   = 5;
    J   = (N2/N1)^2 * Jn1 + Jn2 + J2;

    % integrate the differential equation
    [t, x] = ode23(@fun, t, x0);

    % plot results
    figure('color', 'white');

    % plot position
    yyaxis left;
    plot(t, x(:, 1));
    ylabel('$x$ [rad]', 'Interpreter', 'latex');

    % plot velocity
    yyaxis right;
    plot(t, x(:, 2));
    ylabel('$\dot{x}$ [rad/s]', 'Interpreter', 'latex');

    grid minor;
    xlabel('$t$ [s]', 'Interpreter', 'latex');

    function g = fun(t, x)
        g = zeros(2, 1);
        g(1) = x(2);
        g(2) = (-K/J)*x(1) + (-D/J)*x(2) + (N2/(N1*J)*u(t));
    end
end

You can now employ anonymous functions to specify the temporal evolution of the input. For example, if the input is sinusoidal with pulse frequency $\omega = 0.5 \; \text{rad/s}$, you'll need to run:

t = linspace(0, 15, 10000)';
x0 = [0.1; 0];
integrate(t, @(t)(sin(0.5*t)), x0);

to get:


问:
Looking for resources regarding conveyor object allignment / object orientation
Im currently on the hunt for resources / examples / words to use in a search engine in regards to orienting (aligning, funneling) an object on a conveyor.

I am looking for techniques / utilities used to 'line up' an object on a standard belt conveyor.  Essentially I have objects, boxes of the same size, coming in at some x,y position.  I would like to funnel (either passively with bars or objects, or actively with actuation) objects such that they are lined up in a specific position and orientations (straight at a single x,y position).

I have been looking for examples of such funneling however a lot of my queries conveyor object funneling conveyor object orientation conveyor object alignment are not yielding results withing the funneling realm.

Im curious if there are keywords that I am missing in my research to find more examples, papers, vidoes, resources of such a task being completed

If such a question is not encouraged in this community or if I am asking at the wrong stack exchange my apologies in advance.

答:
The best option will be to use Google Scholar(direct link)  about item orientation on conveyer belts.

General TIP

If "normal" Google doesn't return your results, try it via scholar.google.com


问:
Speeding Up UR5e Joint Movement
I am working with a Universal Robots UR5e. I am attempting to do a simple back and forth movement of 60 Degrees with Wrist 2, operating at 2 Hz (back and forth in 0.5s). Looking at the Joint Limits, the maximum Joint Speed is listed at 191 deg/sec, which should be plenty fast for this. However, I can't seem to get it to move faster than about 1 Hz. Even if I set the Robot Limits to Least Restricted, I max out at this speed.

I am using a movej command and setting t=0.100 to get the max speed. If I set it to  t=0.250 I will still see the movement take over a second to go back and forth.

Does anyoneone have any suggestions for how to speed up the movement?

Thanks!

答:
Sounds like it could also be voltage sag in the power source that powers the servos.

Check the servo specs for the peak instantaneous current they draw under load, and multiply by 6 (or whatever concurrency you need).

If your power source can't robustly supply that total demand, its voltage will sag and the servos will operate slowly (at best), erratically (at worst), or not at all.  Depending on the speed of the transients, an ordinary voltmeter might not be fast enough to measure the dropoff reliably.

Title: Speeding Up UR5e Joint Movement


问:
What algorithm should I implement to program a room cleaning robot?
For this question assume that the following things are unknown:

The size and shape of the room

The location of the robot

The presence of any obstacles

Also assume that the following things are constant:

The size and shape of the room

The number, shape and location of all (if any) obstacles

And assume that the robot has the following properties:

It can only move forward in increments of absolute units and turn in degrees. Also the operation that moves will return true if it succeeded or false if it failed to move due to an obstruction

A reasonably unlimited source of power (let's say it is a solar powered robot placed on a space station that faces the sun at all times with no ceiling)

Every movement and rotation is carried out with absolute precision every time (don't worry about unreliable data)

Finally please consider the following properties of the robot's environment:

Being on a ceiling-less space station the room is a safe but frustratingly close distance to passing comets, so the dust (and ice) are constantly littering the environment.

I was asked a much simpler version of this question (room is a rectangle and there are no obstacles, how would you move over it guaranteeing you could over every part at least once) and after I started wondering how you would approach this if you couldn't guarantee the shape or the presence of obstacles. I've started looking at this with Dijkstra's algorithm, but I'm fascinated to hear how others approach this (or if there is a well accepted answer to this? (How does Roomba do it?)

答:
This question is probably old but here's my code to attempt to answer this question:

https://codereview.stackexchange.com/questions/255545/implement-an-algorithm-for-a-room-cleaning-robot


问:
How to simulate a system output with a sine wave input?
I wish to simulate the output of a certain gear system I have. How the gear system looks isn't particularly important to the problem, I managed to get the differential equation needed from the mechanical system. Here is the code I have (big thanks to the person who helped me with this)

```<code>% parameters
N2  = 90;
N1  = 36;
Jn1 = 0.5;
Jn2 = 0.8;
J2  = 2;
D   = 8;
K   = 5;
J   = (N2/N1)^2 * Jn1 + Jn2 + J2;

% define the system
sys = ss([0 1; -K/J -D/J], [0; N2/(N1*J)], [1 0], 0);

% initial state: (position, velocity) [rad; rad/s]
x0 = [0; 0];

% define the time span
t = linspace(0, 15, 10000)';

% define the input step
T1 = zeros(length(t), 1);
T1(t>=0) = 1;

% compute the system step response at once
theta1 = lsim(sys, T1, t, x0);

% compute the system response as aggregate of the forced and unforced
% temporal evolutions
theta2 = lsim(sys, T1, t, [0; 0]) + initial(sys, x0, t);

% plot results
figure('color', 'white');
hold on;
yyaxis left;
plot(t, T1, '-.', 'linewidth', 2);
ylabel('[N]');
yyaxis right;
plot(t, theta1, 'linewidth', 3);
plot(t, theta2, 'k--');
xlabel('t [s]');
ylabel('[rad]');
grid minor;
legend({'<span class="math-container">$T_1$</span>', '<span class="math-container">$\theta_1$</span>', '<span class="math-container">$\theta_2$</span>'}, 'Interpreter', 'latex',...
       'location', 'southeast');
hold off;
</code>```

This should work in generating a graph that shows the positions, my outputs, for a Heaviside/step input. My question is, how would I do this for a sine wave input. I figure I should have sin(w*t) instead of (t>=0), where w is my pulse frequency. Still, I can't seem to make this work. Any help would be really appreciated! :)

答:
Marx, you already asked me this and I've provided the answer to that in the bottom line of https://robotics.stackexchange.com/a/21740/6941.

Let me recall the function here as well:

function x = integrate(t, u, x0)
    % parameters
    N2  = 90;
    N1  = 36;
    Jn1 = 0.5;
    Jn2 = 0.8;
    J2  = 2;
    D   = 8;
    K   = 5;
    J   = (N2/N1)^2 * Jn1 + Jn2 + J2;

    % integrate the differential equation
    [t, x] = ode23(@fun, t, x0);

    % plot results
    figure('color', 'white');

    % plot position
    yyaxis left;
    plot(t, x(:, 1));
    ylabel('$x$ [rad]', 'Interpreter', 'latex');

    % plot velocity
    yyaxis right;
    plot(t, x(:, 2));
    ylabel('$\dot{x}$ [rad/s]', 'Interpreter', 'latex');

    grid minor;
    xlabel('$t$ [s]', 'Interpreter', 'latex');

    function g = fun(t, x)
        g = zeros(2, 1);
        g(1) = x(2);
        g(2) = (-K/J)*x(1) + (-D/J)*x(2) + (N2/(N1*J)*u(t));
    end
end
```
<p>Example call:</p>
<pre class="lang-matlab prettyprint-override"><code>t = linspace(0, 15, 10000)';
x0 = [0.1; 0];
x = integrate(t, @(t)(sin(0.5*t)), x0);
</code>```

Body:
Marx, you already asked me this and I've provided the answer to that in the bottom line of https://robotics.stackexchange.com/a/21740/6941.

Let me recall the function here as well:

function x = integrate(t, u, x0)
    % parameters
    N2  = 90;
    N1  = 36;
    Jn1 = 0.5;
    Jn2 = 0.8;
    J2  = 2;
    D   = 8;
    K   = 5;
    J   = (N2/N1)^2 * Jn1 + Jn2 + J2;

    % integrate the differential equation
    [t, x] = ode23(@fun, t, x0);

    % plot results
    figure('color', 'white');

    % plot position
    yyaxis left;
    plot(t, x(:, 1));
    ylabel('$x$ [rad]', 'Interpreter', 'latex');

    % plot velocity
    yyaxis right;
    plot(t, x(:, 2));
    ylabel('$\dot{x}$ [rad/s]', 'Interpreter', 'latex');

    grid minor;
    xlabel('$t$ [s]', 'Interpreter', 'latex');

    function g = fun(t, x)
        g = zeros(2, 1);
        g(1) = x(2);
        g(2) = (-K/J)*x(1) + (-D/J)*x(2) + (N2/(N1*J)*u(t));
    end
end

Example call:

t = linspace(0, 15, 10000)';
x0 = [0.1; 0];
x = integrate(t, @(t)(sin(0.5*t)), x0);


问:
Have bloodstream nanobots been approved in any countries?
A Google search on "bloodstream nanobots" yields thousands of results and just on the first page, many results of blog posts that date back to 2009. It is nearly 4 years later.

I've had no luck in finding any information on actual APPROVAL of these bots.

Are there any countries at all who have approved this? People seem to have talked about it like crazy 4 years ago, yet, we're still not seeing anything.

答:
Short Answer: No.

While there is a lot of media hype, and patent applications, there isn't any nanobots being commercially manufactured. Its unlikely that any legislative or FDA approval would happen before there are working demonstration models of actual utility.
There are several significant problems that have to be solved before we see any kind of production models: a lack of nanoscale processors and dependable nanoscale power supplies probably being the two biggest problems.

I would estimate 20+ years before they become mainstream.


问:
Visual Odometry terminology: Scale, Relative scale, absolute scale
I'm watching a MOOC about Machine Vision for Robotics, but I really do not know the meaning of some vocabulary of this domain.

For example, What does the professor mean when he says this:

However, we usually cannot estimate the absolute scale from a single
camera. What this means is that estimation of motion produced from one
camera can be stretched or compressed in the 3D world without
affecting the pixel feature locations by adjusting the relative motion
estimate between the two images.

1- What does the absolute scale mean? In this context, scale refers to what property related to an image?

2- What is the meaning of the statement stretching and compressing the estimation of motion in the 3D world?

3- What does a scaled trajectory mean?

答:
What does the absolute scale mean? In this context, scale refers to
what property related to an image?

Essentially scale refers to the size of the object/scene that the camera sees.
As a projective sensor the camera can't know the depth/size of the object it is viewing. Look at the below image. While we might know that the big and small boxes are different sizes the camera/image can't tell the difference.

The same reason is why people are always doing the optical illusion of holding up the leaning tower of pisa. A purely projective sensor can't tell the actual size of the object.

Note that the actual equation for using scale is typically something like this.

$$size_{true} = scale_{absolute} * size$$

In your case absolute scale is just the true size/ or the scalar multiple needed to get the true size of the object.

2- What is the meaning of the statement stretching and compressing the
estimation of motion in the 3D world?

3- What does a scaled trajectory mean?

These can be answered with the same example.

In the image below you have 3 cases of the robot trajectory generated from a monocular visual odometry algorithm. In reality they are just stretched(bigger) or compressed(smaller) versions of the same trajectory. This is because we can't tell the size of the world. These trajectories would be equally valid for a robot the size of an ant, or one as large as a truck.

A scaled trajectory generally means a trajectory that is scaled to the correct size. Generally used for evaluation purposes. As we want to compare our visual odometry algorithm against something like GPS poses.


问:
Interfacing HMC5883l Compass with Arduino
I'm using an HMC5883l compass module to find the angle at which my module is pointed from magnetic North. I found this code as an example with "HMC5883L_Simple.h" library.

And the results I'm getting right now are like this. Is it the angle at which my module is pointed from magnetic North in degrees? If not, How can find the pointing angle

答:
In answer to your question the results do appear to be in degrees and unless you’ve entered a declination value then the results should point to magnetic north. If you’ve entered a declination offset for your local area then the magnetometer should point to true north.

In answer to your other question a magnetometer will be distorted by its local environment so if you have any local magnetic disturbances which dominate the earths magnetic field then this will distort the results and the magnetometer will point to this local source. If however the magnitude of the local disturbances are less than the earths magnetic field then there net effect will be to distort the results. These type of effects can be removed by calibrating for what are called hard iron (bias values) and soft iron (scaling values) which should account for the local distortions and ensure the magnetometer operates correctly (providing its local environment doesn’t change).  To calibrate the magnetometer, data needs to be recorded whilst rotating the magnetometer (and the platform to which it’s attached) so that the collected data looks like an ellipse (data from all orientations). The collected data then needs to be transformed to a unit sphere. Various algorithms exist for performing this transformation to a unit sphere but a typical approach is a least squares estimation.


问:
Path planning using nodes
I want to run my bot over a map that has nodes and I am just stuck about how to begin the path planning, I literally searched over the internet and found so many algorithms Like A* , Dijkstra's, etc.
But after reading that all I am more frustrated, as I am a beginner in this domain.

Let me explain what I want to do with the help of Example:-

In the above provide map Node 1 is the start position and the blue highlighted box are the area where bot has to go.

From the start position, the bot has to go at node 27 and after he finds node 27 he has to go at node 18 with the shortest part every time.

After the bot reaches 18 he again travels to node 26 and returns to node 18.

After completing all the runs bot has to come back to node 1.

I want to perform the same like this but how to calculate the shortest path and even how to plan the LEFT and RIGHT turning.

I can code like the counting of the node and turning according to node number but this doesn't seem fun. Fun is in coding the bot for finding its own way to a particular node.

I just need a kickstart to perform this any help from your side is really appreciated.

答:
You have presented a map of the robot. This is the correct first step. Now this map has to be reformulated as a sequence of decision which path to take.

If you do this, you can build a tree. In this tree, each stage (level) of the tree you advance from one note to the other. So, if your start node is 1 this is the first node in your tree.

From the first node, you can have move to 5 paths which lead to adjacent nodes. Let's indicate that these are reachable within one step from the starting node by adding S1 as prefix. Now if we add these 5 choices to the starting node we get:

We can continue expanding these nodes, by listing their adjacent nodes. Usually it is bes t to omit the node you came from into this nodes again and again increasing the depth of the tree. (The tree might not match exactly your map but it is close)

Now if you are in a uniform  (and unitary) cost environment the cost of taking a decision, or moving form a parent node to a child node is 1. If you actually calculate distances each edge (line) in this tree will have a different cost which will be the distance metric between the nodes.

Any graph base path planning algorithm will try to build such a tree and the goal is the find the minimum cost path from the start until one of the nodes (leafs) in this tree will be your goal node. The difference is how they get to this path.

Breadth First Search: expands all nodes in the tree going level by level.  Starting at the S1 level all S1 nodes will be expanded. If none of them contains the target node, the next level will be extended...and so on

Depth First Search: Similar to BFS, but goes downwards, not sideways. It expands the nodes downwards until there the maximum depth is reached than goes to the next branch.

Dijkstra: It is more efficient then the two above because it uses an internal rule to select the nodes that  is more application oriented. For each node expanded is ads cost information about the cost to get to that node. The cost is the edges traversed to get to the node (if the cost it uniform 1) or the sum of costs (e.g. cartesian distance between the nodes). This cost is what selects the next node to be extended. Regardless of the level in the tree, you always select the node for expansion, which has minimal cost.

The algorithm performs better then BFS if the cost is not uniform. It is more ore less the same as BFS in a uniform unitary cost map.

For implementation, usually there are two lists, closed and open. The closed list contains all nodes which have been expanded. The open list contains all nodes which have not yet been expanded.

First step is to add the first node to the open list.

Select the node with minimal cost in the open list

Check if this node is the target node, if yes, recreate the path using the pointers to its parents set below. if not:

Expand the selected node, this is where we get the adjacent nodes

Add all newly obtained nodes and add them to the open list

Calculate the cost to get to the expanded nodes (parent cost + edge cost)

Connect a pointer from the children nodes to the parent node.

Add delete the expanded node from the open list and add it to the closed list
Go to 2

Please note that some implementations and explanations consider the presence of the target node in the open list a stop condition. This is wrong, it has to be the in the open list and has to have the minimum cost in the open list for the algorithm to complete. Otherwise it is not guaranteed that the shortest path is found

A:* it is based on Dijkstra, but has one difference. The cost calculated in step 6  has one further component. It is an approximated cost of how much would it take to get from the current node to the target node. If this approximation is less or equal then the actual real cost to get to the target node then the algorithm will function correctly. E.g. in a uniform cost environment this approximation can be 1. On a map, this approximation could be the Cartesian distance between the current node and the target node. Please note, that the approximation plays a role only is the selection of the node, the actual cost component is calculated based on the actual cost only, not the approximated part.

Title: Path planning using nodes


问:
The modulus of each columns of the rotation matrix is
The modulus of each columns of the rotation matrix is

1, 0.5, 0.866 or 1.366

which one is correct?

答: The modulus of each column vector is 1. The column vectors of a rotation matrix are expected to be orthonormal.</p>


问:
How to implement the Wavefront algorithm?
I am thinking of creating a robot that can navigate using a map. It is controlled from a PC. An 8-bit controller performs low level tasks and the PC is doing the image processing. I plan to implement it in a single room where the robot is placed and the robot and environment are tracked by a camera from a height or from the ceiling of the room. First, the robot needs to be mapped, like this http://www.societyofrobots.com/programming_wavefront.shtml

To do:

Track the robot from some height using a camera Following the
Wavefront algorithm to locate the robot and obstacles.

Procedure:(just my idea)

The camera will give an image of the robot surrounded by obstacles in random places.
using some OpenCV technique draws some grind over the image.

Locating the grid which contains robot (by having some colored symbol over the robot) and locating the grids containing the obstacle.

Now, the grids with an obstacle are thought of as wall and the remaining is the free space for the robot to navigate.

The robot is going to get the goal place which should be reached is given from the pc(maybe like point the place to reach in the image by mouse click).

Unknowns :

Mapping the room and locating the robot

How to do that? The robot should know where it is in the map or the image. We cannot believe only the camera is enough to locate the robot. So I thought of adding triangulation mapping like placing two IRs in the room and a receiver in the robot.

The doubt I have in this is how an IR receiver can know from which direction it is receiving the IR signal (from left or right ). I think it knows only that it receives IR, not the direction. Then how is the triangulation going to happen if I don't know the angle and direction?

coming to the image processing, how can I implement the Wavefront
algorithm (that captures the live video and draw grids over it to
find the robot and the obstacles)?

I have HC-05 Bluetooth module, Arduino, Bluetooth dongle, chassis with dc motors and driver, and a dc supply.

答:
The camera is enough to locate the robot if its mounted high enough,unless the robot is hidden by furniture. If the furniture hides the robot from the camera, it will hide it from a
IR Beacon.
If you start the robot in a visible position, don't have too much cover, and use dead reckoning when under cover, you should be okay.The dead reckoning will add error to the robot's known position, but really, how much time is it going to spend underneath a table?
The position can be corrected when the robot comes back into view.</p>


问:
How to handle robot movement close to coordinates x=0 with atan2
I am working on controlling a robot to various points on an x,y grid. The robot starts at random locations and I can control it's velocity and angular velocity.

Currently, I am using the technique of rotate the robot until the atan2 of the distance between the robot and the target goal like so:

```<code>dx = target_position.x - robot_position.x
dy = target_position.y - robot_position.y
angle = atan2(dy, dx)
angle = wrap_to_pi(angle) # Wrap the angles to the range [-π, π] radians.

if angle - robot_angle <= some_error_factor:
   increase_linear_velocity() # note this is pseudo code
</code>```

This works pretty well to get me to target positions when the robot never has to go near x=0 on the coordinate system. I know there is a discontinuity with atan2 near pi, but my question is how can I wrap the angle or control the robot because currently when x is close to 0 the robot just spins around unable to get under the error factor to say you're pointing at the position

答:
First off, your code may have some confusion about atan2, which already returns an angle between -pi and +pi.  That's one reason to use atan2(dy, dx) instead of atan(dy/dx) -- it figures out the correct quadrant so we don't have to.  (The other reason to use atan2 is that it works fine when dx is near zero, where atan(dy/dx) will suffer loss of precision, overflow, or even division by zero.)

Secondly, I think your code is doing the pi-wrap in the wrong place.  Try something more like this:

```<code>dx, dy = target_position - robot_position
target_angle = atan2(dy, dx)
d_angle = target_angle - robot_angle
d_angle = wrap_to_p(d_angle)    # wrap the angle here
if abs(d_angle) < acceptable_error
   see_about_moving_forward
elif d_angle < 0:
   turn left some
else:
   turn right some
</code>```

Body:
First off, your code may have some confusion about atan2, which already returns an angle between -pi and +pi.  That's one reason to use atan2(dy, dx) instead of atan(dy/dx) -- it figures out the correct quadrant so we don't have to.  (The other reason to use atan2 is that it works fine when dx is near zero, where atan(dy/dx) will suffer loss of precision, overflow, or even division by zero.)

Secondly, I think your code is doing the pi-wrap in the wrong place.  Try something more like this:

dx, dy = target_position - robot_position
target_angle = atan2(dy, dx)
d_angle = target_angle - robot_angle
d_angle = wrap_to_p(d_angle)    # wrap the angle here
if abs(d_angle) < acceptable_error
   see_about_moving_forward
elif d_angle < 0:
   turn left some
else:
   turn right some


问:
How to set initial heading
I'm confused how to set up an initial heading for my mobile robot project. I've determined heading from gyro integration and also using magnetometer readings. Gyro initializes at 0 and magnetometer initializes to a random value between -180 and 180 based on direction of the imu.

My question is how should I set up this parameter initially. I plan keep the range between -pi and pi, but do i want to use the global magnetic field as my reference and navigate based upon this, or do i want to offset that value to agree with the gyro at startup?

Thanks for any help!

答:
magnetometer initializes to a random value between -180 and 180

It's not a random value, it's

based on direction of the imu.

If your question is

how should I set up this parameter initially.

Then you need to decide what you want it to be. Do you want zero to be whatever direction the robot is pointing when it's powered on, or do you want zero to be magnetic North?

Either way, you need to grab the magnetometer output on startup (or maybe average the first five or ten samples.) Store it in a variable called absoluteInitialHeading.

Then, if you want zero to be magnetic North, you set the initial gyro angle to be that absolute initial heading and you're done.

If you want zero to be initial vehicle heading then you can subtract the absolute initial heading from all subsequent magnetometer readings.

Title: How to set initial heading


问:
Extract laser scan from a map and known location
If we have the map of an environment (an occupancy grid for instance) and precisely know the pose of the robot within it. How can one reconstruct what the laser scan should look like at a given location ?</p>

答:
This would depend on how your map is formed in the first place. Is it saved by aggregating several scans into one map? If so, it'd be very hard to extract individual scans since this information is lost in the registration and subsequent aggregation. You can however, use a filter (eg: radius filter, crop box, etc.) to query the map based on your current pose and extract a portion of the map. This wont give you an individual scan but will be close.

The other option would be to skip the aggregation step and save individual scans with their poses. You will have to write a script to merge them at runtime and view/show a complete map, but no information is lost in the process. Extracting a scan would then be easy, as you just find the scan with the lowest distance between the current pose and one saved in the files.


问:
Yaw from IMU acceleration
When IMU is gravity aligned, yaw is not observable from linear acceleration data.

However, when IMU has non null pitch, the sensor is not gravity aligned anymore and the gravity acceleration gets remapped on other axes.

Question: how do you compute the yaw angle from linear acceleration data when the pitch is not null (and therefore yaw is observable)?

Thank you in advance

答:
If you assume the IMU lies on a fixed incline plane with pitch $\theta$, and you define yaw $\psi$ as the rotation between the IMU and the downhill direction, then the answer is

$$\psi = \text{atan2}(-a_y, a_x)$$

where $a_x$ and $a_y$ are the X and Y accelerometer readings respectively. Note that this is independent of the inclined plane pitch $\theta$ (which is not equal to the pitch as measured by the IMU; the IMU may be facing perpendicularly to the descent direction).

There are many definitions of yaw but the one we are using here is with respect to the downhill direction of the inclined plane. If that inclined plane were to rotate around the $z$ axis, the yaw as defined using e.g. Euler ZYX angles would change, but our yaw would stay the same. I suspect this is the disconnect between the question and the other answers. To be clear, this computation for $\psi$ is only the "local" yaw with respect to the incline. Getting the angle with respect to a fixed north or world $x$-axis given just an accelerometer reading and no other information is not possible. This is because gravity points down, and rotations around that axis (the down axis) will not cause a change in the accelerometer reading.

To derive this expression for $\psi$, note that the orientation of the IMU, $R$, in this inclined plane scenario only really has one degree of freedom: the yaw.
The orientation can be described by first pitching downwards by an angle of $\theta$, then rotating around the new body $z$-axis by $\psi$.
We can compute an expression for this orientation given $\theta$ and $\psi$ by multiplying an elementary rotation around the $y$ axis and an elementary rotation about the $z$ axis. See https://en.wikipedia.org/wiki/Rotation_matrix for their definitions. Since the rotation around $z$ is intrinsic (w.r.t. the new $z$-axis), we post multiply $R_y(\theta)$ with $R_z(\psi)$.

$$
R = R_y(\theta)R_z(\psi) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix} \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1\end{bmatrix} = \begin{bmatrix} \cos\theta \cos\psi & - \cos\theta \sin\psi & \sin\theta \\ \sin\psi & \cos\psi & 0 \\ -\sin\theta \cos\psi & \sin\theta \sin\psi & \cos\theta \end{bmatrix}
$$

Now what does the accelerometer measure? It measures the gravity vector in the body frame*. The gravity vector in the world frame is $\begin{pmatrix} 0 & 0 & g \end{pmatrix}^\top$, so in the body frame, it is
$$ R^\top \begin{pmatrix} 0 \\ 0 \\ g \end{pmatrix} = \begin{pmatrix} -g\sin\theta \cos\psi \\ g\sin\theta \sin\psi \\  g\cos\theta \end{pmatrix} = \begin{pmatrix} a_x \\ a_y \\ a_z \end{pmatrix} $$

Now note that $$ -\frac{a_y}{a_x} = \tan \psi $$
and we have a simple way to compute $\psi$.

Note that when both $a_x$ and $a_y$ are zero, i.e. $\theta = 0$, we are not able to get any information about $\psi$. This corresponds to the intuition that on flat ground, the accelerometer cannot tell you about yaw.

*I am assuming there is no linear acceleration, which is a very common assumption. If the linear acceleration is known, it can be subtracted. For example, if this is a mobile robot, linear acceleration is typically close to zero or it can be estimated using the control inputs.

Title: Yaw from IMU acceleration


问:
Inverse Kinematics does not give the desired joint angles
I have a location of a tool with respect to the base. That location is described by an x,y,z coordinates and three rotations. I call that $Target$. I want to make a geometrical approach on the first three angles. I first have to remove some parts of my target to get it to the wrist and also remove the base. This. First I remove the base, then the wrist to tool(TWT) and lastly the frame 6 wrist(T6T). Then you end up with transformation from 0 to 6 (T60). There are 6 frames.

Essentially we did this

$T06=TB0^{-1}\cdot target \cdot TWT^{-1} \cdot T6W^{-1}$

When I as shown in the figure, when theta 1 is calculated for it makes athis equation.

$\theta_1=tan^{-1}(y/x)$

This gave the wrong answer. Since the trigonometry was straightforward, my hunch is that the above statement where we changed the transformation was at fault. Did we do it correctly is my question.

答:
Your atan probably returns radians. Were you expecting degrees?

Forgive me if this seems so basic as to be insulting, but just yesterday it snagged two engineers pair programming (one was me)!


问:
IR pattern projector
I am currently working on a robotics project in which I wish to integrated a depth/object scanner. Now my focus is currently on an IR camera in combination with some sort of IR pattern projector.

Now I am having a hard time finding any sort of IR projector which can project a pattern. I have found several topics suggesting that I use an IR-LED and create a pattern in front of it, but that would mean the projector would take up significant space.

Are there any projectors out on the market in a sensible form-factor for consumers? I came across the Structure Core, but that's only available to OEMs.

答:
You can find a wide selection of pattern projectors here,
some of them have eye-safety features, other are better than AMS projectors in the sense that the pattern is much more random.</p>

Title: IR pattern projector


问:
Roslaunch include file remotely
I am trying to launch a file from remote computer but I could not success. Actually I can connect to remote computer but I think the problem is with including a file from remote computer. In other words, I am looking for a machine tag for include. Here is the my code:</p>

```<code><launch>

    <group >
      <machine name="marvin-1" address="tek-marvin-1" user="blabla" password="blabla" env-loader="/home/blabla/.rosLaunchScript.sh"/>

      <include file="$(find openni_launch_marvin)/launch/kinect_left.launch"/>
    </group>

</launch>
</code>```

答:
The following hacky method is only valid if you have packages / launch files that are ONLY available on remote.  If the packages / launch files are available on both host and remote machine, you can simply use the <machine> tag from roslaunch which is much cleaner.

(cross posting from https://answers.ros.org/question/371480/roslaunch-nodes-launch-files-only-available-on-remote-machine/?answer=371491#post-id-371491)

Assuming host has ssh key access to remote, I accomplished this with the following:

Host machine launch file:

```<code><launch>
    <node pkg="local_pkg" type="remote_bringup.sh" name="remote_bringup"/>
</launch>
</code>```

Host machine remote_bringup.sh

```<code>#!/usr/bin/env bash

trap 'ssh remote@remote "pkill roslaunch"' INT TERM
ssh remote@remote "~/bringup.sh"
wait
</code>```

Remote machine bringup.sh

```<code>#!/bin/bash

source /opt/ros/melodic/setup.bash
source /opt/ros/install/setup.bash
export ROS_MASTER_URI=http://host_machine:11311
export ROS_IP=remote_machine
nohup roslaunch remote_pkg normal.launch &
</code>```

The script properly catches SIGINT, SIGTERM and forwards it to the remote machine's roslaunch

Title: Roslaunch include file remotely

Body:
The following hacky method is only valid if you have packages / launch files that are ONLY available on remote.  If the packages / launch files are available on both host and remote machine, you can simply use the <machine> tag from roslaunch which is much cleaner.

(cross posting from https://answers.ros.org/question/371480/roslaunch-nodes-launch-files-only-available-on-remote-machine/?answer=371491#post-id-371491)

Assuming host has ssh key access to remote, I accomplished this with the following:

Host machine launch file:

<launch>
    <node pkg="local_pkg" type="remote_bringup.sh" name="remote_bringup"/>
</launch>

Host machine remote_bringup.sh

#!/usr/bin/env bash

trap 'ssh remote@remote "pkill roslaunch"' INT TERM
ssh remote@remote "~/bringup.sh"
wait

Remote machine bringup.sh

#!/bin/bash

source /opt/ros/melodic/setup.bash
source /opt/ros/install/setup.bash
export ROS_MASTER_URI=http://host_machine:11311
export ROS_IP=remote_machine
nohup roslaunch remote_pkg normal.launch &

The script properly catches SIGINT, SIGTERM and forwards it to the remote machine's roslaunch


问:
Calculating rotation matrix efficiently
I'm trying to efficiently calculate the vertical offset of each corner of a rectangular base. I have an accelerometer mounted in the middle of the base, ADXL345. The steps I have taken, and seems to work are below. Can anyone advise if this is the most efficient way of doing this. I'm using the raspberry pi pico, with MicroPython:

convert the accelerometer into a unit vector

use the dot product and cross products ( between X =0, y= 0, z= 1 and the accelerometer unit vector) to calculate the quaternion

calculate the quaternion rotation matrix

calculate the rotation of the four corners using the rotation matrix, but only for the z component, as I'm only interested in vertical offset of the corners

答:
The accelerometer measures the gravity vector in the body frame, call it $a$. If you normalize that, say $\hat a$, it's the third row of the rotation matrix $R$ that represents the rotation of the body. i.e.
$$
R^\top \begin{pmatrix}0 \\ 0 \\ 1\end{pmatrix} = \frac{a}{||a||} = \hat a =  z_{\mathcal{W}}^{\mathcal{B}}
$$

So $$ R = \begin{pmatrix} ? \\ ? \\ \hat a \end{pmatrix} $$

Now to get the world coordinates of a corner point $p^{\mathcal{B}}$ expressed in the body frame,
$$
p^{\mathcal{W}} = R p^{\mathcal{B}} = \begin{pmatrix} ? \\ ? \\ \hat a \end{pmatrix} p^{\mathcal{B}} = \begin{pmatrix} ? \\ ? \\ \hat a ^\top p^{\mathcal{B}} \end{pmatrix}
$$

Thus the world $z$ coordinate of the corner point is the dot product between the normalized accelerometer vector and the corner point expressed in the body frame.

If you just need the $z$ coordinate, you do not need to construct the rotation matrix.

Title: Calculating rotation matrix efficiently


问:
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>

答: My robot project uses an Esp32Cam to control the robot and stream video to a web ui over a websocket connection.  https://github.com/Ezward/Esp32CameraRover2.  It is also possible to do this with an Arduino Uno and a OV7670 camera module, but it is way easier with an Esp32Cam.  Cheap and easy.</p>


问:
Is it possible to control the speed of a hoverboard motor[BLDC motor] with a raspberry pi? If so, how?
In my project, I'm making a robot using two hover board motors, a motor control board such as this one:
https://www.ebay.com/p/2242827374?iid=162674407600
and a raspberry pi. I've connected the motor to the motor controller board, and the motor controller board to the motor. The motors work fine, I can run a function that makes the motors spin forward or backward. The one problem is the speed. I read that a pin on the motor controller board[VR pin] controls the speed. When running the motor, it is meant to simply take a parameter that controls the speed of the motor. However the motor only runs at max speed regardless of what number I use in the parameter. Thank you in advance and let me know if you need any more details.</p>

答:
You're looking for an electronic speed controller. Be careful to check that the one you purchase supports forward AND reverse if you need both directions, and that they're rated for the voltage and current that you need.

The electronic speed controllers should take a PWM signal, but also you can check the datasheet for specifics.


问:
ROS Webcam Stream Really Slow
I am trying to stream the images from my camera to a VM via ROS. Here is my setup:

Raspberry Pi 3B+ with Picamera v2. It is running ROS Noetic with cv_camera_node

An Ubuntu 20.04.1 VM with ROS Noetic. I am reviewing the live stream using rosrun rqt_image_view rqt_image_view

When I tried this, the stream is really laggy and slow. I am barely getting 10 fps. I was able to do camera calibration with it but mapping with it will be really challenging. Is there any way to speed it up. Can I decrease the quality of the camera capture? Any help is truly appreciated.

Here is my launch file:

```<code><launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video0" />
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="uyvy" />
    <param name="camera_frame_id" value="camera" />
    <param name="io_method" value="mmap"/>
    <param name="framerate" value="30" />
    <param name="autofocus" value="true" />
    <param name="camera_name" value="camera" />

  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/camera/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>
<span class="math-container">```
```

答:
What camera are you using? It might be helpful to know since the driver might be at fault here.

Also, any particular reason you are working with the cv_camera_node? usb_cam has worked much better for me in the past. It works with libusb rather than opencv which could also be a culprit here. In the end, you just get a sensor_msgs::Image with both packages. So it'd be worth a shot. You can also set a bunch of different parameters in the launch file itself. Maybe play around with those? Specifically the framerate and image width/height.

Title: ROS Webcam Stream Really Slow


答: If you are using the Raspberry Pi camera and not a USB camera, it is better to use this node: https://github.com/UbiquityRobotics/raspicam_node. This gave me really good speed.</p>

Title: ROS Webcam Stream Really Slow

Body:
I am trying to stream the images from my camera to a VM via ROS. Here is my setup:

Raspberry Pi 3B+ with Picamera v2. It is running ROS Noetic with cv_camera_node

An Ubuntu 20.04.1 VM with ROS Noetic. I am reviewing the live stream using rosrun rqt_image_view rqt_image_view

When I tried this, the stream is really laggy and slow. I am barely getting 10 fps. I was able to do camera calibration with it but mapping with it will be really challenging. Is there any way to speed it up. Can I decrease the quality of the camera capture? Any help is truly appreciated.

Here is my launch file:

```<code><launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video0" />
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="uyvy" />
    <param name="camera_frame_id" value="camera" />
    <param name="io_method" value="mmap"/>
    <param name="framerate" value="30" />
    <param name="autofocus" value="true" />
    <param name="camera_name" value="camera" />

  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/camera/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>
<span class="math-container">```
```