问:
Kalman filter GPS + IMU fusion get accurate velocity with low cost sensors
I'm new to all this robotics stuff. Especially to Kalman filter.

My initial goal is to have velocity as accurate as possible

Here is my case:

I have a phone which is mounted, for example in the car. So it has low cost GPS and IMU sensors. 2D

GPS gives me:

position (longitude, latitude, <s>altitude</s>)

position accuracy (error can't be split into east, north, up directions)

speed

speed accuracy (error can't be split into east, north, up directions)

heading angle

heading angle accuracy

IMU: (separated accelerometer, gyroscope and magnetometer). I fuse them myself

Actually it's needed to be mentioned that I can't use magnetometer in my case. Since "car" is faraday cage. So I only can fuse accelerometer and gyroscope. Both of them are outputs from Madgwick AHRS (can get from here rotation matrix, quaternion if needed) and represented in North, East, Up dimensions.

What I've done so far:

I've implemented linear KF with position & velocity states. But I didn't achieve desired accuracy.

Get rid of IMU data from chart above.

It's IMU causes that drift. I have GPS updates every 1 second. And IMU with 13 Hz frequency. We can see here that every 13th iteration we have GPS updates and then IMU goes rogue.

Used approach:

Since I have GPS 1Hz and IMU upto 100Hz. But I took 13Hz in my case. Since I don't need to have so many updates.

predict when IMU fires event

When GPS fires event. I take latest IMU data. Do predict and then gps (position, velocity) update.

Since my primary goal is to have accurate velocity.

I don't care much about position and heading angle but... Since velocity correlates with them they can be added to Kalman Filter. Am I right?

So my Kalman states are position, velocity and heading angle.

Can I use something like?

$$
x = x_i + v_i\Delta t + \frac{a_i\Delta t}{2}
$$
$$
v = v_i + a_i\Delta t
$$
$$
\theta = \theta_i + w_i\Delta t
$$
Questions:

Could velocity benefit from adding position and heading angle as states to Kalman Filter. Since there is some correlation between them. (Angular velocity impacts on velocity itself).

Is it OK to use formulas from Linear motion? Because I have Curvilinear motion in my case.

Almost all papers describe (position, velocity) model with KF. Can I take advantage in using EKF? I found some papers that mention odometry word. And seems like they have the same model. (pos, velocity, angle)

What if after all manipulations velocity is still inaccurate? Should I apply additional instruments after KF?

Can I somehow take advantage of current location and prev. location points? (For example, calculate velocity from two points. Of course it means that my unit moves linear and not by curve). Then somehow correct my predicted KF result with this velocity.

Please help me with modeling Kalman Filter. And give an advice how to achieve best velocity accuracy.

Thanks!

答:
I would say, this site may helps you to understand where the power of the Kalman-filter lies.
http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

In my humble opinion:

I think basic useful Kalman-filter requires position and speed at least, because as you've said they correlate to each other.

There is an important thing to do before even implementing anything. Doing an analysis of the motion. I mean by that, what sort of motion does your system produce? A simple Kalman-filter is best at linear motion prediction. If it is non-linear, you have to be clever on how to set up the process noise Q parameter. Let me give you and example:
You have small vessel you are tracking with a radar. It is known, that your vessel has a non-linear speed (it is not known for the process, when the vessel slows down). In order to adjust the process noise to the situation, hence increasing accuracy, you have to have imply some knowledge in the algorithm, in what cases the speed turns more non-linear. Like, small vessel is usually more manouverable, than a 200 m long one, so Q shall set considering the size for example.

See four.

I would say, a 3 state (pos/speed/acc) KF, should do the job if you include angle, then it is even better. Although, if you motion is simple you could also consider an alpha-beta filter as well, which is less intelligent about the history of the motion, but quite fast.

Basically the first state of the KF shall make use of the previous and current position.

I hope this gives a little kick in your project.


问:
Calculation of inertia values of Ic and Im
In my dynamic model of two wheeled mobile robot, it has the total equivalent inertia, noted $I$, described by the equation below,

$I_c$ is the moment of inertia of the DDMR about the vertical
axis through the center of mass and $I_m$ is the moment
of inertia of each driving wheel with a motor about the wheel diameter.
May I know how to measure or calculate $I_c$ and $I_m$? My wheeled mobile robot is Arduino Robot

答:
You can find equations for the inertia of a cylinder on Wikipedia.

Using the equations for a solid cylinder will give similar results as if you calculated or measured the exact inertia of all the subcomponents.

The inertia of the wheels should include the inertia of the motor rotor and gears to be accurate. You can search for and find equations for this too.

You will need to disassemble the motor to get enough information.

Like mass, you can combine inertias by adding them together as long as they have the same reference axis (1D) or frame(3D).

ContentLicense: CC BY-SA 3.0


问:
IMU oriented wrongly. How to correct it to get correct values?
We have used an SMC-108 IMU sensor in a hydrographic survey. This was our first time using this sensor. While processing the data, we noticed that the roll, pitch, and heave values were not right. Turns out, this IMU sensor has to be aligned in a specific orientation to be giving the correct values. We had our IMU oriented in a different direction from what it should be (See image below).

Is there any way we can correct for this misalignment and get the correct roll, pitch and heave information?

Thanks in advance.

答:
It seems that only the orientation is not the desired one. Hence you get correct values but they are not the one you expected.

To make it systematic I would propose you to build the transformation matrix between your desired sensors location and the one what was used in the survey. This way you get a framework also working if the position of the sensor is off.
see http://www.diag.uniroma1.it/~deluca/rob1_en/08_EulerRPYHomogeneous.pdf for some theory

Then you can build the transformation matrix of your sensor reading, aka  a rotation matrix construct from the angles, then by multiplying the two transformation matrix you wold get the transformation matrix of your measurements at the give point. It is up to you to go back to roll, pitch, yaw angles. Note that RPY are ambiguous and subject to gimbals lock phenomena, hence rotation matrices are preferred even if they are less compact.

A comprehensive guide to go from one orientation representation to the next, https://www.swarthmore.edu/NatSci/mzucker1/papers/diebel2006attitude.pdf


问:
Angular velocity vector expression uncertain?
I have this problem of an excavator from a test I took a while ago.

Frame $\underline{\vec{e}}^0$ is the origin.

Frame $\underline{\vec{e}}^1$ rotates around $\underline{\vec{e}}_{3}^0$ with angle $\theta_1$

Frame $\underline{\vec{e}}^2$ rotates around $\underline{\vec{e}}_{2}^1$ with angle $\theta_2$

Frame $\underline{\vec{e}}^3$ rotates around $\underline{\vec{e}}_{2}^2$ with angle $\theta_3$

The question is:

I know that the velocity vector is expressed as:
$^{30}{\vec{\omega}}=\dot{\theta}_1\vec{e}_{3}^0+\dot{\theta}_2\vec{e}_{2}^1+\dot{\theta}_3\vec{e}_{2}^2$

But I have some trouble expressing it in frame $\underline{\vec{e}}^3$.
This is what I've come up with:

$^{30}{\vec{\omega}}=\begin{bmatrix} 0 & 0 & \dot{\theta}_1 \end{bmatrix} \underline{A}^{32} \underline{A}^{21} \underline{\vec{e}}^3+\begin{bmatrix} 0 & \dot{\theta}_2 & 0 \end{bmatrix} \underline{A}^{32} \underline{\vec{e}}^3+\begin{bmatrix} 0 & \dot{\theta}_3 & 0 \end{bmatrix}  \underline{\vec{e}}^3$

In which $\quad$$\underline{A}^{21}=\begin{bmatrix} cos(\theta_2) & 0 & -sin(\theta_2) \\ 0 & 1 & 0 \\ sin(\theta_2) & 0 & cos(\theta_2)\end{bmatrix}$,$\quad$ $\underline{A}^{32}=\begin{bmatrix} cos(\theta_3) & 0 & -sin(\theta_3) \\ 0 & 1 & 0 \\ sin(\theta_3) & 0 & cos(\theta_3)\end{bmatrix}$

I'm confused about the sequence:$\underline{A}^{32} \underline{A}^{21}$, does this mean you go from frame $1$ to $2$ and then from $2$ to $3$  or should the order be reversed? There's also an expresison for $\underline{A}^{10}$ but I don't think that has to be used here.

I'm confused and the reader that I have from my university isn't helping. Or I'm just to stupid to understand it.

Thanks in advance

答:
The following holds:

$\vec{\underline{e}}^1=\underline{A}^{10}\vec{\underline{e}}^0$

$\vec{\underline{e}}^2=\underline{A}^{21}\vec{\underline{e}}^1$

$\vec{\underline{e}}^3=\underline{A}^{32}\vec{\underline{e}}^2$

From frame $3$ to $0$ we get: $\vec{\underline{e}}^3=\underline{A}^{32}\underline{A}^{21}\underline{A}^{10}\vec{\underline{e}}^0$

From frame $0$ to $3$ we get: $\vec{\underline{e}}^0=\underline{A}^{01}\underline{A}^{12}\underline{A}^{23}\vec{\underline{e}}^3$

Using that $\underline{A}^{xy}=\big(\underline{A}^{yx}\big)^T$ we get:  $\vec{\underline{e}}^0=\big(\underline{A}^{10}\big)^T\big(\underline{A}^{21}\big)^T\big(\underline{A}^{32}\big)^T\vec{\underline{e}}^3$


问:
How often does a robot perform A* (A star) path planning in an unknown map?
I have read through lectures/ tutorials on A* but they have all been via computer simulations. I have an autonomous wheeled robot that is traversing an unknown map (essentially, it'll be a tabletop with no obstacles but has edges it can fall off of); it has an indoor GPS system, IMU, and cliff sensors. I'm trying input a desired waypoint.

Does the robot assume that everywhere is traversable, break up the map into a grid, calculate the best path, and go for it OR is it supposed to iterate this process? If the latter, how should the robot proceed in iterating? I'm thinking that it would travel in the "best path" until the cliff sensor is triggered and it will have to recalculate a new best path.

答: There is a dynamic version of A* (or of Dijkstra's algorithm) that was developed to address exactly this problem of trying to do planning on a map as you discover it. It is called D* or occasionally Stentz's algorithm after the originator, Tony Stentz. Have a look at this UIUC course page for a good description of the formulation, and the wikipedia entry has more info on variants that have been created over time.</p>


问:
Jacobian method for inverse kinematics
I have big problem. I have to solve inverse kinematics for a manipulator with 6-DOF using jacobian method. From what I know to do that I need to have matrix of transformation and Denavit–Hartenberg parameters, which both I have. But I am not a mathematician, and descriptions I find on the web are not even a bit understandable to me. So I would love if you could give me an example of how to solve my problem.

The Denavit-Hartenberg parameters are:
$$
        \begin{matrix}
        \alpha & l & \lambda & \theta\\
        90 & 150 & 0 & var(-69) \\
        0 & 610 & 0 & var(85) \\
        90 & 110 & 0 & var(-52) \\
        -90 & 0 & 610 & var(62) \\
        90 & 0 & 113 & var(-60) \\
        0 & 0 & 78 & var(-108) \\
        \end{matrix}
$$

The values in theta are values to get the following matrix of transformation, and values I want to get with this jacobian method. And those values are in degrees.

Matrix of transformation:
$$
        \begin{matrix}
        0.7225 & 0.0533 & 0.6893 & 199.1777\\
        -0.2557 & -0.9057 & 0.3381 & -500.4789\\
        0.6423 & -0.4206 & -0.6408 & 51.6795\\
        0 & 0 & 0 & 1 \\
        \end{matrix}
$$

I would be most greatful, if someone could walk me through how to solve it in simple language.

答:
enter link description hereYou can Use Jacobian Method of course;  couple of the techniques are

Jacobian Inverse Method

Jacobian Transpose Method

Jacobian Pseudo_Inverse Method.

It depends, whether computational cost or accuracy is at stake

The Procedure is as follows:
compute Transformation Matrix

Compute FKI (forward Kinematics)

i.e. (base to Tool Center Point(TCP) Transformation Matrix)

$$ T_{6}^{0} = T_{1}^{0}*T_{2}^{1}*T_{3}^{2}*T_{4}^{3}*T_{5}^{4}*T_{6}^{5}$$

Transformation matrix. In fact you can compute Transformation matrix from base to any point on any link of the robot arm.

Joint Velocity and end-effect velocity are related as $\qquad (1)$
$$V_{e} = J(q)\dot q $$

q-> joint position($\theta$),$\dot q$->joint velocity

J(q) is the Jacobian of the manipulator (6,dof) matrix

$J(q) = [z_{i} \times r_{6}^{i};z_{i}]$ for revolute joint

```<code>def  Matrix Jacobian(q):

 Matrix4d tempMat; //(4x4) matrix

 for i in range(0,dof):

    tempMat = get_T(i)  //transformation matrix from i-1 to i

    Ri    = R[i-1]*tempMat[0:3;0:3] //orientation Matrix
    Ti    = T[i-1]*tempMat

    zi  = Ri[:,2]
    Oi  = Ti[0:3,3]

    for i in range(0,dof):
    Vector3d tempVec = zi.cross( Oi[dof] - Oi[i] )
    Jmat[0:3,i] = tempVec
    Jmat[3:6,i] = zi

    return Jmat
</code>```

$z_{i}$->direction axis-i $3^{rd}$ column of $T_{1}^{0}*T_{2}^{1}..*T_{i-1}^{i}$ Matrix
$r_{i}^{6} = r_{0}^{6} - r_{0}^{i}$ note(this is vector operation)is position vector from joint i to TCP(end-effector)

Thus to solve Inverse Kinematics
do the inverse of velocity relation equation(1) as:
$$\dot q = J^{(-1)}(q)*V_{e} $$

$$q = q_{0} + \int \dot q dt $$

or do numerical integration in case of a discrete motion (which is mostly the case in real robot control)

$$q = q_{prev} + qd*\Delta t$$, $$\Delta t -> sampling \ time $$

for Better and clear understanding read
By these Authors

Title: Jacobian method for inverse kinematics

Body:
enter link description hereYou can Use Jacobian Method of course;  couple of the techniques are

Jacobian Inverse Method

Jacobian Transpose Method

Jacobian Pseudo_Inverse Method.

It depends, whether computational cost or accuracy is at stake

The Procedure is as follows:
compute Transformation Matrix

Compute FKI (forward Kinematics)

i.e. (base to Tool Center Point(TCP) Transformation Matrix)

$$ T_{6}^{0} = T_{1}^{0}*T_{2}^{1}*T_{3}^{2}*T_{4}^{3}*T_{5}^{4}*T_{6}^{5}$$

Transformation matrix. In fact you can compute Transformation matrix from base to any point on any link of the robot arm.

Joint Velocity and end-effect velocity are related as $\qquad (1)$
$$V_{e} = J(q)\dot q $$

q-> joint position($\theta$),$\dot q$->joint velocity

J(q) is the Jacobian of the manipulator (6,dof) matrix

$J(q) = [z_{i} \times r_{6}^{i};z_{i}]$ for revolute joint

def  Matrix Jacobian(q):

 Matrix4d tempMat; //(4x4) matrix

 for i in range(0,dof):

    tempMat = get_T(i)  //transformation matrix from i-1 to i

    Ri    = R[i-1]*tempMat[0:3;0:3] //orientation Matrix
    Ti    = T[i-1]*tempMat

    zi  = Ri[:,2]
    Oi  = Ti[0:3,3]

    for i in range(0,dof):
    Vector3d tempVec = zi.cross( Oi[dof] - Oi[i] )
    Jmat[0:3,i] = tempVec
    Jmat[3:6,i] = zi

    return Jmat

$z_{i}$->direction axis-i $3^{rd}$ column of $T_{1}^{0}*T_{2}^{1}..*T_{i-1}^{i}$ Matrix
$r_{i}^{6} = r_{0}^{6} - r_{0}^{i}$ note(this is vector operation)is position vector from joint i to TCP(end-effector)

Thus to solve Inverse Kinematics
do the inverse of velocity relation equation(1) as:
$$\dot q = J^{(-1)}(q)*V_{e} $$

$$q = q_{0} + \int \dot q dt $$

or do numerical integration in case of a discrete motion (which is mostly the case in real robot control)

$$q = q_{prev} + qd*\Delta t$$, $$\Delta t -> sampling \ time $$

for Better and clear understanding read
By these Authors


问:
Arduino C/C++ progamming tutorials
I have no problem in reading circuit schemes, connecting wires, resistors etc. (basing on for example instructables.com), but I've only tried to learn Java (a while ago) and it's difficult to me to find out what's going on with all this C-code-stuff.
Are there any tutorials that are focused on the programming part?
thanks</p>

答: Yes, under the learning section of the Arduino website there are a number of coding tutorials. They begin with explaining the minimum code necessary to how to use many of the peripherals. These tutorials explain fairly well what each line of the programs do.</p>

Title: Arduino C/C++ progamming tutorials


答:
Well, I totally understand your problem. You want to solve inverse kinematics problem using Jacobian inversion.

1) Firstly you need to compute Jacobian matrix. for that you may refer book "RObotics and control" by R K mittal and I J Nagrath or J Crag. you have 6 DOF manipulator, for that you will get 6 x 6 matrix.
You need to take note that which 3 rows are linar velocity and which are angular velocity. Generally first 3 rows are of liner and last 3 rows are of angular velocity(this is recommended).

2) Understand jacobian kinematics.

$Ve = J(q)*Vq $ .....(eq 1)

where Ve is end effector velocty and  Vq is the joint velocty.

so, for mapping joint velocty you need jacobian inverse

$Vq = inv(J(q))*Ve$ ..... (eq 2)

3) Now, how you can input Ve. So, for that you need to understand that the  that jacobian base IK is the iterative method to solve IK. You need large number of iteration to obtain the required goal position and orientation.

Ve can be inputted in form of error vector. Error vector is the difference between the target position vector and current or home position vector. In your case it will be 6 x 1 matrix.
Home position vector can be calculated by forward kinematcs.
And target position vector can be calculated by the matrix you provided. the vector is in form [position vector, orientation vector]. position vector is 3 x 1 vctor form right top of transformation matrix and orientatin matrix is 3 x3  matrix at left top corner, you need to convert in euler angle 3x1 matrix.

4) Now you can apply eq-2 to solve inverse kinematics. This will approximate your solution. At the end of each iteration you need to calculte error vector. you need to define least square error when you stop iterating.

Title: Jacobian method for inverse kinematics

Body:
I have big problem. I have to solve inverse kinematics for a manipulator with 6-DOF using jacobian method. From what I know to do that I need to have matrix of transformation and Denavit–Hartenberg parameters, which both I have. But I am not a mathematician, and descriptions I find on the web are not even a bit understandable to me. So I would love if you could give me an example of how to solve my problem.

The Denavit-Hartenberg parameters are:
$$
        \begin{matrix}
        \alpha & l & \lambda & \theta\\
        90 & 150 & 0 & var(-69) \\
        0 & 610 & 0 & var(85) \\
        90 & 110 & 0 & var(-52) \\
        -90 & 0 & 610 & var(62) \\
        90 & 0 & 113 & var(-60) \\
        0 & 0 & 78 & var(-108) \\
        \end{matrix}
$$

The values in theta are values to get the following matrix of transformation, and values I want to get with this jacobian method. And those values are in degrees.

Matrix of transformation:
$$
        \begin{matrix}
        0.7225 & 0.0533 & 0.6893 & 199.1777\\
        -0.2557 & -0.9057 & 0.3381 & -500.4789\\
        0.6423 & -0.4206 & -0.6408 & 51.6795\\
        0 & 0 & 0 & 1 \\
        \end{matrix}
$$

I would be most greatful, if someone could walk me through how to solve it in simple language.


问:
How to rotate a rotation quaternion in the body frame to a rotation quaternion in the world frame?
I have a sensor (3 axis gyroscope) which can rotate and measure angular velocity in 3 dimensions (aligned with the sensor).
I know what its current orientation is with respect to the world frame. Call this quaternion qs.
I take a reading from my gyroscope and integrate it to give me a rotation in the sensor frame. Call this quaternion qr.

I now want to apply the rotation qr to the current orientation qs to obtain the new orientation, qs'.
But I cannot use qr directly as it describes a rotation in the sensor body frame.

I need to transform my rotation quaternion into the world frame, and then I could just apply it to the orientation i.e. qs' = qs * qr_world.
But I am really struggling to understand how I can perform this transformation qr -> qr_world.

Does this even make sense? I wonder if I have fundamentally misunderstood some concepts here. If it does make sense, then I am specifically interested in understanding how to do this using quaternion operations (if that is possible) rather than rotation matrices or euler angles.

答:
As pointed out in my earlier comment, this is actually simpler than you may think.  Remember, $qs$ and $qr$ are fundamentally different, where the former represents orientation (in reference to the outside world) and the latter represents rotation (irrespective of any reference coordinate system).

You're right in saying that you don't need to do any additional transformations and that your new orientation $qs'$ will be given by:

$qs' = qs \times qr$

where;

$qs$ is the previous position, and

$qr$ is a single rotation by a given angle $\theta$ about a fixed axis (obtained from the gyroscope).

Example

Let's presume a simple analogy with straight line displacement;

$\delta s = v \times \delta t$

Given an initial position $s$, it can be said that moving at a velocity $v$ for a period of time $t$ sees us ending up at a final position $s'$:

$s' = s + \delta s$

Similarly, presume angular displacement (rotation in one axis);

$\delta \theta = \omega \times \delta t$

Given an initial angular position $\theta$, it can be said that moving at an angular velocity $\omega$ for a period of time $t$ sees us ending up at a final angular position $\theta'$:

$\theta' = \theta + \delta \theta$

Now, presume angular displacement in three-dimensional space (rotation in three axes) that, according to Euler's rotation theorem, is equivalent to a single rotation by a given angle $\theta$ about a fixed axis $\hat \omega$;

$qr_w = \cos \dfrac{\theta}{2}$

$qr_x = \omega_x \times \sin \dfrac{\theta}{2}$

$qr_y = \omega_y \times \sin \dfrac{\theta}{2}$

$qr_z = \omega_z \times \sin \dfrac{\theta}{2}$

Given initial three-dimensional angular positions (orientation) $qs$, it can be said that moving at angular velocities $\omega_x$, $\omega_y$ and $\omega_z$ for a period of time $t$ sees us ending up at final three-dimensional angular positions (orientation) $qs'$:

$qs' = qs \times qr$

Therefore, as straight line displacement $\delta s$ can be accumulated to track the position along a straight line $s$, so can angular displacements about three axes (or, rather, their single equivalent angular displacement about the Euler axis $qr$) be accumulated to track angular position in three-dimensional space (orientation) $qs$.


答:
```<code> A great way to learn C/C++ for arduino is by going to the website: ardx.org/CIRC01
</code>```

This website is completely and gives great mini projects that involve electronics and then gives an example code and a great explanation of how the code works. This site consists of 11 projects and explains the basics of C/C++ for arduino, it also teaches you how to read schematics and data sheets. After you are done with project #1 change the CIRC01 at the end of url to CIRC02, and each time you complete a project continue to go to the next one by changing the # on the end of the CIRC to a higher # until you get to 11. By the time you get to project #11 you should have a good understanding of programming for arduino and a better understanding of electronics as well. Another way that I learned C++ is by reading the book "C++ for Dummies by Stephen R. Davis". This book teaches C++ in a simple yet affective fashion, however it is quite tedious to completely read this book, if it were me I would start by using the projects on ardx.org and then once I have finished all the projects and if I wanted even more knowledge on programming c/C++ then read the book. Good luck!

Title: Arduino C/C++ progamming tutorials

Body:
I have no problem in reading circuit schemes, connecting wires, resistors etc. (basing on for example instructables.com), but I've only tried to learn Java (a while ago) and it's difficult to me to find out what's going on with all this C-code-stuff.
Are there any tutorials that are focused on the programming part?
thanks</p>

Body:
 A great way to learn C/C++ for arduino is by going to the website: ardx.org/CIRC01

This website is completely and gives great mini projects that involve electronics and then gives an example code and a great explanation of how the code works. This site consists of 11 projects and explains the basics of C/C++ for arduino, it also teaches you how to read schematics and data sheets. After you are done with project #1 change the CIRC01 at the end of url to CIRC02, and each time you complete a project continue to go to the next one by changing the # on the end of the CIRC to a higher # until you get to 11. By the time you get to project #11 you should have a good understanding of programming for arduino and a better understanding of electronics as well. Another way that I learned C++ is by reading the book "C++ for Dummies by Stephen R. Davis". This book teaches C++ in a simple yet affective fashion, however it is quite tedious to completely read this book, if it were me I would start by using the projects on ardx.org and then once I have finished all the projects and if I wanted even more knowledge on programming c/C++ then read the book. Good luck!


问:
Estimation of Focus of expansion
I am trying to estimate focus of expansion for a moving camera mounted in a mobile robot. I am using the method described in this method page 13-14. My aim is to use it to avoid obstacle using optical flow. I have attached the code below. Calculating the terms when I display the calculated point it keeps jumping around and isnt correct. Any advice would be appreciated.</p>

```<code>void calculate_FOE(vector<Point2f> prev_pts, vector<Point2f> next_pts)

    MatrixXf A(next_pts.size(),2);
    MatrixXf b(next_pts.size(),1);
    Point2f tmp;

    for(int i=0;i<next_pts.size();i++)
    {

        tmp= prev_pts[i]-next_pts[i];
        A.row(i)<<prev_pts[i].x-next_pts[i].x,prev_pts[i].y-next_pts[i].y;
        b.row(i)<<(prev_pts[i].x*tmp.x)-(prev_pts[i].y*tmp.y);

    }

    Matrix<float,2,1> FOE;
    FOE=((A.transpose()*A).inverse())*A.transpose()*b;
</code>```

答:
Have you tried reversing your derivatives so that velocities are specified forward in time? i.e., do next_pts - prev_pts instead of prev_pts - next_pts. Negating all the constraint equations could result in the crazy instability you describe.

It also looks like A.row(i) can be simplified to just tmp.

If you haven't already, you should also try visualizing the optical flow results that are feeding into this estimation, to make sure that they look good enough for you, as a human, to identify FOE from them.

Title: Estimation of Focus of expansion


问:
Do we need a backend graph optimizer for OctoMap?
As far as I know, OctoMap or any other grid occupancy maps are only for mapping. How do they handle a loop closure problem?

The first method came to my mind is optimizing a pose graph in the backend and send the corrected map to grid map library to fuse and build the map again from the beginning. But I dought that they do this. What's the secret?

答:
There is no secret. OctoMap was not really designed with pose refinement in mind. You can even see one of the authors of RGBD-SLAM mention it here.

If you did want to try and still use octomap with loop closure then the approach that you mention could work. You could probably make it a bit more efficient by only deleting the poses in Octomap that were affected by the loop closure.

One possibility you could also use is to utilize the probabilistic aspect of OctoMap. On your first run set the pointclouds to have a very low probability. With the loop closure update you then add another set of pointclouds with a much higher probability. Octomap will take care of the fusing for you so this idea could work.

If you do ,however, want mapping software that is built to deal with changing poses due to loop closure and bundle adjustment then I would advise you to look into Thomas Wheelans work on deformable pose graphs specifically with Bundlefusion and Elasticfusion.


问:
Main not defined package python issue
My apologies but I've run out of ideas. I'm very new to python and stuck trying to execute the following code from this tutorial:

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

import rospy
from geometry_msgs.msg import Twist

import RPi.GPIO as GPIO

# Set the GPIO modes
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)

_FREQUENCY = 20

def _clip(value, minimum, maximum):
    # """Ensure value is between minimum and maximum."""

    if value < minimum:
        return minimum
    elif value > maximum:
        return maximum
    return value

class Motor:
    def __init__(self, forward_pin, backward_pin):
        self._forward_pin = forward_pin
        self._backward_pin = backward_pin

        GPIO.setup(forward_pin, GPIO.OUT)
        GPIO.setup(backward_pin, GPIO.OUT)

        self._forward_pwm = GPIO.PWM(forward_pin, _FREQUENCY)
        self._backward_pwm = GPIO.PWM(backward_pin, _FREQUENCY)

    def move(self, speed_percent):
        speed = _clip(abs(speed_percent), 0, 100)

        # Positive speeds move wheels forward, negative speeds
        # move wheels backward
        if speed_percent < 0:
            self._backward_pwm.start(speed)
            self._forward_pwm.start(0)
        else:
            self._forward_pwm.start(speed)
            self._backward_pwm.start(0)

class Driver:
    def __init__(self):
        rospy.init_node('driver')

        self._last_received = rospy.get_time()
        self._timeout = rospy.get_param('~timeout', 2)
        self._rate = rospy.get_param('~rate', 10)
        self._max_speed = rospy.get_param('~max_speed', 0.5)
        self._wheel_base = rospy.get_param('~wheel_base', 0.091)

        # Assign pins to motors. These may be distributed
        # differently depending on how you've built your robot
        self._left_motor = Motor(13, 15)
        self._right_motor = Motor(36, 32)
        self._left_speed_percent = 0
        self._right_speed_percent = 0

        # Setup subscriber for velocity twist message
        rospy.Subscriber(
            'cmd_vel', Twist, self.velocity_received_callback)

    def velocity_received_callback(self, message):
        # """Handle new velocity command message."""

        self._last_received = rospy.get_time()

        # Extract linear and angular velocities from the message
        linear = message.linear.x
        angular = message.angular.z

        # Calculate wheel speeds in m/s
        left_speed = linear - angular*self._wheel_base/2
        right_speed = linear + angular*self._wheel_base/2

        # Ideally we'd now use the desired wheel speeds along
        # with data from wheel speed sensors to come up with the
        # power we need to apply to the wheels, but we don't have
        # wheel speed sensors. Instead, we'll simply convert m/s
        # into percent of maximum wheel speed, which gives us a
        # duty cycle that we can apply to each motor.
        self._left_speed_percent = (100 * left_speed/self._max_speed)
        self._right_speed_percent = (100 * right_speed/self._max_speed)

    def run(self):
        # """The control loop of the driver."""

        rate = rospy.Rate(self._rate)

        while not rospy.is_shutdown():
            # If we haven't received new commands for a while, we
            # may have lost contact with the commander-- stop
            # moving
            delay = rospy.get_time() - self._last_received
            if delay < self._timeout:
                self._left_motor.move(self._left_speed_percent)
                self._right_motor.move(self._right_speed_percent)
            else:
                self._left_motor.move(0)
                self._right_motor.move(0)

            rate.sleep()

    def main():
        driver = Driver()

        # Run driver. This will block
        driver.run()

if __name__ == '__main__':
    main()
</code>```

I get get the following error:

```<code>(classic)adeoduye@localhost:~/catkin_ws$ rosrun simple_robot_navigation driver_node_controller
    Traceback (most recent call last):
      File "/home/adeoduye/catkin_ws/src/simple_robot_navigation/src/driver_node_controller", line 117, in <module>
        main()
    NameError: name 'main' is not defined
</code>```

BTW, it's ROS code which I have implemented in my custom catkin_ws/src/simple_robot_navigation package. I suspect it's a python issue but I very new at python and not sure how to fix it.

I would appreciate any help.

答:
I suspect that there are some indentation error in the tutorial.

In the example code, the function main is defined as a function of the class Driver. However, as the actual code of main goes, it creates an instance of the class Driver and run it. So I think the function definition should be at the same indentation level as the class definition. What you need to do is to remove the indentation of the function definition of main. That is, remove four white spaces such that the function definition would be

```<code>def main():
    driver = Driver()
    driver.run()

if __name__ == '__main__':
    main()
</code>```

Title: Main not defined package python issue

Body:
I suspect that there are some indentation error in the tutorial.

In the example code, the function main is defined as a function of the class Driver. However, as the actual code of main goes, it creates an instance of the class Driver and run it. So I think the function definition should be at the same indentation level as the class definition. What you need to do is to remove the indentation of the function definition of main. That is, remove four white spaces such that the function definition would be

def main():
    driver = Driver()
    driver.run()

if __name__ == '__main__':
    main()


答:
The focus of expansion (FOE) is the point in the image that has the smallest cross product with all of the optical flow vectors. Each optical flow vector has a previous point and a delta. Let $p_x$ and $p_y$ be the x and y positions of the previous position of an optical flow vector. Let $dx$ and $dy$ be the change in x and y position between the previous location and the current location.

The cross product between a single optical flow vector and the FOE is
\begin{equation}
M =
\begin{bmatrix}
\hat i & \hat j & \hat k \\
p_x-foe_x & p_y - foe_y &0\\
dx & dy &0
\end{bmatrix}
\end{equation}

\begin{equation}
M = \hat k \left( (p_x - foe_x)dy - (p_y - foe_y)dx \right)
\end{equation}

The moment, $M$, is what we want to minimize. So setting $M$ equal to zero we can solve for the FOE.

\begin{equation}
\begin{bmatrix}
-dy & dx \\
... & ... \\
\end{bmatrix}
\begin{bmatrix}
foe_x \\ foe_y\\
\end{bmatrix} =
\begin{bmatrix}
p_ydx-p_x dy \\
...\\
\end{bmatrix}
\end{equation}

Note that my matrices do not match the matrices in the paper; their $v$ term is positive while I believe it should be negative.

Title: Estimation of Focus of expansion

Body:
I am trying to estimate focus of expansion for a moving camera mounted in a mobile robot. I am using the method described in this method page 13-14. My aim is to use it to avoid obstacle using optical flow. I have attached the code below. Calculating the terms when I display the calculated point it keeps jumping around and isnt correct. Any advice would be appreciated.</p>

```<code>void calculate_FOE(vector<Point2f> prev_pts, vector<Point2f> next_pts)

    MatrixXf A(next_pts.size(),2);
    MatrixXf b(next_pts.size(),1);
    Point2f tmp;

    for(int i=0;i<next_pts.size();i++)
    {

        tmp= prev_pts[i]-next_pts[i];
        A.row(i)<<prev_pts[i].x-next_pts[i].x,prev_pts[i].y-next_pts[i].y;
        b.row(i)<<(prev_pts[i].x*tmp.x)-(prev_pts[i].y*tmp.y);

    }

    Matrix<float,2,1> FOE;
    FOE=((A.transpose()*A).inverse())*A.transpose()*b;
</code>```


问:
directly tapping Create2 battery
Since the mini DIN power is limited to 250ma, has anyone found a way to tap the battery to supply power to added electronics?

I'd prefer this over adding a secondary battery and separate charger

Thanks,
Frank

答:
We have tried it and have a stably working system using a DC DC voltage converter directly off the battery. We have seen issues though where it will try to run the converter even when the 14-18v battery is at 4 volts so can extremely deep discharge the battery.

If you do this, I would suggest calculating your max current load you want to run and validating it won't over-tax the battery in an un-safe way or have significant spikes in power that will be a problem across the system.

Buffer voltage + UPS battery

Therefore, we introduced a buffer inbetween (OpenUPS or battery backup battery) to buffer it, clean things up better and allow the computer to function without the create being powered.

Compute platform shutdown on low power

We also set up an automatic shutdown of the laptop if voltage becomes too low and "return to dock" commands after 15 minutes to make sure it re-charges itself as regularly as possible if not being actively used.

Check charge regularly on charger

Additionally, we found that on the charger, when in trickle charge mode, the system does not actually test the voltage (Expects it to always be full), so the computer will discharge the battery and it won't turn the charger back on. You therefore have to pull it out of passive mode and then trigger it again (Back out then dock and charge) and it will correctly check the voltage and charge.

State

This is currently not a fully tested system, but have have tried it on multiple create 2's / NUC's / Laptops and have had the system recharging itself for 10+ days in a row while powering lidar, camera, NUC, create, etc.

If you try this, would love to hear how it goes for you and improvements you see as valuable.

Please also just be careful and have a fire extinguisher near-by. ;-)

Title: directly tapping Create2 battery


问:
iRobot Roomba 620 (similar to create 2) enters power saving mode although the is a pulse at BRC pin and wakes up at next pulse
Sorry for asking again, but there is still no solution and I am not able to comment this question from Aaron: Cannot disable sleep in passive mode for iRobot Create 2

Create 2 spec says:

In Passive mode, Roomba will go into power saving mode to conserve battery power after five minutes of inactivity. To disable sleep, pulse the BRC pin low periodically before these five minutes expire. Each pulse resets this five minute counter. (One example that would not cause the baud rate to inadvertently change is to pulse the pin low for one second, every minute, but there are other periods and duty cycles that would work, as well.)

Here you see the signal at the BRC pin at the connector:

Is there a newer firmware? Roomba output:

```<code>bl-start
STR730
bootloader id: #x47135160 6BFA3FFF
bootloader info rev: #xF000
bootloader rev: #x0001
2007-05-14-1715-L
Roomba by iRobot!
str730
2012-03-22-1549-L
battery-current-zero 258
2012-03-22-1549-L
r3_robot/tags/release-3.2.6:4975 CLEAN
bootloader id: 4713 5160 6BFA 3FFF
assembly: 3.3
revision: 0
flash version: 10
flash info crc passed: 1
processor-sleep
</code>```

If Roomba is already sleeping it wakes up from the pulse. So there is no chance for not receiving it.

Any idea?

答:
We are also seeing the same problem and have identified a few things which specifically help.

We are trying to get it to stay awake on the charger so we can monitor it and therefore tried toggling into full then passive modes, however encountered a crash where it freezes communication when it toggles back in to passive mode afterwards. stack overflow link

A few things to verify:

Change Interval

It looks like you are sending it out once a minute. Have you tried every 30 seconds? See the link below but this could be the issue where it goes to sleep every minute. Given you have older firmware this could be an issue. (Guessing it isn't, but need to note it)

One link I found states that there may be a bug in the firmware for the 600 series which actually makes it fall asleep after 1 minute and also not wake up: "The latest Roomba 600 models will go to sleep after 1 minute on the dock. Unfortunately iRobot has disabled the serial communication when Roomba is asleep on the dock. We think this is a bug in iRobots firmware and we hope they will fix this soon." link

Double number of pulses

Have you tried sending out 2 pulses in a row, a few seconds apart to see if a fast interval would work and replicate the second pulse, but faster? A solution like sending out those pulses every minute could possible make it work.

Send pulse when communication stops

If you are monitoring the packet rate, you can send a pulse when the packet rate goes to 0, restarting it immediately if it responds while asleep.

Charger vs not

Are you trying this on the charger and having it fail? I would suggest trying to see if the behavior is different while on the charger and off as we have seen a fully responsive robot off the charger for toggling all states but are having trouble with stability of communications while charging where it freezes and then re-appears minutes later which confused our team a lot.

Note: We have firmware version: r3-robot/tags/release-stm32-3.7.5:6216 so it is significantly newer (3.7 vs 3.2 than yours).


问:
How to convert vertical motion to horizontal?
I am interested in using this miniature motor Squiggle Micro Motor to create very tiny horizontal movements. However, due to very limited space, I can only place it vertically within my project.

Assuming this motor is placed as follows, how can one adapt it to simultaneous movement at a right angle? (Ideally with the X-axis movement matched to the Y-axis movement as well as possible.)

答:
The mechanism suggested in the previous answer is a form of
four-bar linkage.  A bell crank is a slightly simpler form of basically the same thing.  You could push on one side of a bell crank with the end of the motor shaft, and use a spring return for the other direction if it is difficult to attach to the shaft.  (The shaft apparently rotates, but the Squiggle motors site doesn't say if it does or doesn't, and the pictures, videos, and technical drawing PDF files are not clear on this point.)

Also consider using a flexible-wire push-pull control cable, or a push cable with  spring return; or push a wedge up to produce cam action.

ContentLicense: CC BY-SA 4.0


问:
Orientation estimation using gyro and accelerometer when sensor platform has high acceleration
I am trying to estimate the orientation of a sensor platform using gyroscope and accelerometer. I am using a Kalman filter based approach. I integrate the readings from gyro to obtain the orientation about x, y and z axis. This is will act as the process model. I am planning to use accelerometer readings as observation. I know how to estimate the angles of orientation when the accelerometer is reading only gravity, by using acceleration components in x, y and z.

However, when the sensor platform is moving or accelerating (i.e. gravity is not the only acceleration the sensor is reading), can accelerometer provide orientation/ tilt estimation without the help of any other sensor? Can this approach work without using a third sensor when the sensor platform has high acceleration?

答:
General movement is not a problem even at high velocity but, as you already described, acceleration is. There are a couple mitigating factors that allow many platforms to still (mostly) assume the accelerometer is a gravity measurement

Short bursts of acceleration can be filtered out (e.g. if $||a|| >> ||a_g||$ ignore the reading)

High frequency acceleration oscillations average out to zero over time. This just means you have to increase the measurement noise parameter of the filter

If you happen to know the maneuver acceleration you can simply adjust the accelerometer measurement accordingly. This case is rather unlikely though (there's a reason we need sensors in the first place)

When this is not the case, such as for a missile which has long periods of sustained non-gravity acceleration, you simply need a different sensor.


问:
Why do we need a marginalization in Bundle Adjustment?
A common practice in Bundle Adjustment is to reduce the state dimension by marginalizing structure or pose states to improve the optimization speed.

In case 3d points(structure) $\textbf{p}_i$ are marginalized out as follows, $\textbf{p}_i$ are triangulated to calculate residual $\textbf{e} $.

$\textbf{e} = \textbf{z}_{ij} - \pi(\textbf{T}_j\textbf{p}_i)$

where $\textbf{T}_j\in SE(3), \textbf{p}_i\in R^3$ are the states we want to estimate and $\textbf{z}_{ij}$ is the observed feature in $R^2$.

And just optimize the pose related terms only.

$\begin{bmatrix}
 \textbf{H}_{cc}& \textbf{H}_{cs} \\
\textbf{H}_{sc} & \textbf{H}_{ss}
\end{bmatrix}
\begin{bmatrix}
 \mathbf{\xi}_c \\
\textbf{p}_s
\end{bmatrix}=
\begin{bmatrix}
 \textbf{g}_{c} \\
\textbf{g}_{s}
\end{bmatrix}$

$\bar{\textbf{H}}_{cc}=\textbf{H}_{cc}-\textbf{H}_{cs}{\textbf{H}_{ss}}^{-1}\textbf{H}_{sc}$

$\bar{\textbf{g}}_{c}=\textbf{g}_{c}-\textbf{H}_{cs}{\textbf{H}_{ss}}^{-1}\textbf{g}_{s}$

$\bar{\textbf{H}}_{cc}\mathbf{\xi}_c =\bar{\textbf{g}}_{c}$

Here my question arises. If we can calculate 3d points $\textbf{p}_i$ by the triangulation, only $\textbf{T}_j$ are the state variable to be estimated.

Then, why are we bothered to calculate marginalization related terms $-\textbf{H}_{cs}{\textbf{H}_{ss}}^{-1}\textbf{H}_{sc}$ and $-\textbf{H}_{cs}{\textbf{H}_{ss}}^{-1}\textbf{g}_{s}$ instead of optimizing only poses by ${\textbf{H}}_{cc}\mathbf{\xi}_c ={\textbf{g}}_{c}$ (note that H and g are without bar).

I guess  ${\textbf{H}}_{cc}\mathbf{\xi}_c ={\textbf{g}}_{c}$ is enough to find the optimal poses $\textbf{T}_j$.

So, my question is why do we use $\bar{\textbf{H}}_{cc}\mathbf{\xi}_c =\bar{\textbf{g}}_{c}$ instead of ${\textbf{H}}_{cc}\mathbf{\xi}_c ={\textbf{g}}_{c}$?

答:
I am still not entirely sure what you are asking, but I believe this may answer your question. Also note I myself don't fully understand all the details behind the marginalization trick, but I will attempt to explain why it is used.

Lets first setup how the optimization problem looks for your two different possibilities with just 2 cameras. So we are only solving for 1 pose.

Here is the actual bundle adjustment reprojection error.

$ \displaystyle \min_{T,p} \sum_{i=1}^{p}  (P_i-T(\pi(p_i))^2 $ $\quad Eq1$

You solve for $T$(pose/transform) and $p$(points) in one optimization problem.

And here is the 2nd version to only find the poses.

$ \displaystyle \min_{T} \sum_{i=1}^{p}  (P_i-T(\pi(p_i))^2 \quad Eq2$

After solving for the pose($T$) with $Eq2$ you would then apply some sort of triangulation method to solve for the 3d points.

Lets agree on the fact that $Eq1$ will be more accurate then applying the 2nd method when it comes to finding the position of the poses. This is because it allows for error in the 3d points. The 2nd method assumes that the positions of the 3d points is correct. However, $Eq1$ is also a lot slower than $Eq2$ cause you are also optimizing for the points($p$).

Now for why we use the marginalization trick. We would apply the marginalization to Eq1. You do all the math you have written above, and are able to solve for the $T$ without also optimizing the points($p$). Remember how I said $Eq1$ was more accurate, but slower. Well with the marginalization we are able to reduce the problem into only solving for the pose($T$), but still keep most of the accuracy. So with marginalization we can almost do the exact same as $Eq2$, but with the accuracy of $Eq1$.

I typically know this trick as the Schur complement, however it also appears in the EKF formulations as some sort of null space trick.

References:

GTSAM Smart Factors

MSCEKF

Bundle Adjustment


答:
I believe you can solve this using a least squares approach since all the math in equation is linear. Rearrange the equation so
\begin{equation}
\bf RR_1 -R_2R = 0
\end{equation}

Set up the relation
\begin{equation}
\bf Ax = b
\end{equation}

where the vector $\bf x$ contains all values of $\bf R$, $\bf B$ is a $[9,1]$ zero vector, and $\bf A$ is a $[9,9]$ matrix which contains the $\bf R_1$ and $R_2$ terms.

Then solve for $\bf x$
\begin{equation}
\bf x = A^{-1} b
\end{equation}

You'll then have to restructure the $\bf x$ vector back into $\bf R$.

I know this may not be exactly what you're looking for but it is a closed form solution.

Title: A closed-form solution of $\textbf{R}\textbf{R}_1=\textbf{R}_2\textbf{R}$ w.r.t $\textbf{R}$

Body:
Is there a closed-form solution of $\textbf{R}\textbf{R}_1=\textbf{R}_2\textbf{R}$ with respect to $\textbf{R}\in SO(3)$?

$\textbf{R}_1$, $\textbf{R}_2 \in SO(3)$ are given.

Added:

I tried holmeski's solution but it fails because of rank deficiency in A matrix.(why?) The following code simulates holmeski's solution in matlab. (please correct me if the code is incorrect)

```<code>clear all
cTl=rotx(rand*100)*roty(rand*100)*rotz(rand*100)
lTc = inv(cTl)

for k = 1: 9
    l1Tl2{k}=rotx(rand*100)*roty(rand*100)*rotz(rand*100)
    c1Tc2{k}= cTl*l1Tl2{k}*lTc;
end
R = calib_RR1_R2R_closedform(l1Tl2,c1Tc2)

function R = calib_RR1_R2R_closedform(l1Tl2,c1Tc2_klt)

Astack=[]
for k = 1:length(l1Tl2)
    R1 = l1Tl2{k}(1:3,1:3);
    R2 = c1Tc2_klt{k}(1:3,1:3);
    A = [ R1(1,1) + R1(1,2) + R1(1,3) - R2(1,1) - R2(1,2) - R2(1,3), R1(2,1) + R1(2,2) + R1(2,3) - R2(2,1) - R2(2,2) - R2(2,3), R1(3,1) + R1(3,2) + R1(3,3) - R2(3,1) - R2(3,2) - R2(3,3), R1(1,1) + R1(1,2) + R1(1,3) - R2(1,1) - R2(1,2) - R2(1,3), R1(2,1) + R1(2,2) + R1(2,3) - R2(2,1) - R2(2,2) - R2(2,3), R1(3,1) + R1(3,2) + R1(3,3) - R2(3,1) - R2(3,2) - R2(3,3), R1(1,1) + R1(1,2) + R1(1,3) - R2(1,1) - R2(1,2) - R2(1,3), R1(2,1) + R1(2,2) + R1(2,3) - R2(2,1) - R2(2,2) - R2(2,3), R1(3,1) + R1(3,2) + R1(3,3) - R2(3,1) - R2(3,2) - R2(3,3)]
    Astack = [Astack; A];
end

det(Astack)

[U S V] = svd(Astack);
x=V(:,end);
R=reshape(x,3,3);
</code>```


问:
How many stages necessary to stabilize position while controlling torque?
I'm assuming that since torque translates into acceleration, the basic transfer function from torque to position becomes

$1/s ^ 3$

Does this mean that 3 pid controllers are required to properly control the process? ie acceleration, velocity and finally position? Perhaps a two stage position+velocity controller can decently aproximate the solution, but from mathematical standpoint, how many stages are actually needed for optimal control?

答:
If you are trying to implement torque control, you only need a control loop around torque. The challenge isn't in trying to integrate the torque (or acceleration) controller, and passing it to a velocity control loop, then finally passing it to a position control loop.  The challenge is in finding the torque setpoints for your torque control loop so that the manipulator achieves the desired performance.  For this you need a model of your robot + environment if you want to use torque control to follow position profiles.  That model will be where you incorporate $s^2$ differentiation to get to desired accelerations.

But I have to ask - why are you implementing torque control to achieve positional stability?  This gets very complicated if the impedance of your robot's environment changes.  For example, of the system is lifting a box, controlling the torque can result in the desired positional profile.  But what if the robot encounters an immovable object - then you will never get the commanded positional changes.  Or, what if you have multiple heavy and light boxes?  The position-to-torque model must account for these things.

See @MarkBooth's answer to this SE question about position, velocity, and torque control.


答:
After long hours of search, I realized that it is not a simple problem at all. There are many related papers exists published back in the 1980s 1, 2. The short summary is as follows

\begin{equation}
^L\textbf{R} \textbf{R}=\textbf{R} ^C\textbf{R}
\end{equation}

$^L\textbf{R}_{i}=\boldsymbol{e}^{[^L\textbf{r}_i]_\times}
, ^C\textbf{R}_i=\boldsymbol{e}^{[^C\textbf{r}_i]_\times}$

$\textbf{M} = \sum_{i=1}^{I}{^L\textbf{r}_i {^C\textbf{r}_i^\top}}$

$\textbf{R} = (\textbf{M}^\top\textbf{M})^{-1/2}\textbf{M}^\top$

Single pair of rotation is not enough to decide the unique solution.

Also, the following post is actually asking the same question. here

If scale translation should be considered as well, Eq 8.27 of this book has the answer.

Here is matlab implementation of this problem.here


问:
SE3 composition operator gives unexpected results

Given Data and Algorithm

I have a stream of SE3 poses supplied by a basic wheel encoder odometry through ROS message passing system. Odometry publishes data in traditional to ROS ENU coordinate frame (X - forward, Y - left, Z - up) with right chirality.

I present this trajectory on graphs below (TX, TY, RZ):

It should be obvious that other 3 dimensions have all zeros in them, as the wheel odometry poses have only 3DoF.

Then I rotate this stream of poses to another coordinate frame, customary for Visual SLAM (Z - forward, X - right, Y - down). The acquired result is shown below and is not what I have expected.

More Formally:

Let's say agent's camera is aligned with principal axes of agent and relative translation from camera to agent center is neglected. In other words: $P_{C_i}^{B_i} = \text{Id}$.

I have a stream of SE3 transformations from current place of agent's body to Odometry World Coordinate Frame: $$P_{B_0}^{W_O}, P_{B_1}^{W_O}, P_{B_2}^{W_O}, \dots$$
I need to observe this motion from standpoint of a Visual Slam:

$$P_{C_0}^{W_V}, P_{C_1}^{W_V}, P_{C_2}^{W_V}, \dots$$

So I do this in a following manner: $$P_{C_i}^{W_V} = P_{W_O}^{W_V} \cdot  P_{B_i}^{W_O} \cdot P_{C_i}^{B_i} = P_{W_O}^{W_V} \cdot  P_{B_i}^{W_O}$$
I define $P_{W_O}^{W_V} = (\text{Quaternion}(\text{Euler}(-90, -90, 0)), (0, 0, 0))$, where Euler is intrinsic and active, and order is $(z,x,y)$.
The resulting quaternion $\text{qxyz}$ is $(\frac 1 2, \frac 1 2, -\frac 1 2, \frac 1 2)$ and Euler angles choice can be verified geometrically with the help of the following picture:

Results Interpretation and Question

I have plenty of positive X and positive Y translational movement in the start of dataset, and some yaw rotation along Z axis later on.

So, when I look at this motion from Visual World coordinate frame, I
expect a plenty of positive Z and negative X translational movement in the start of dataset, and some yaw rotation along -Y axis later on.

While the linear part behaves exactly as I describe, the rotation does something else. And I am bewildered by results and count on your help.

It's terrible to realize that these group operations on SO3 are still a mystery to me.

Comparison graphs are shown below (TX,TY,TZ,RX,RY,RZ):

Sorry for the longpost!

Meta:

I have long pondered where to post: in Math or Robotics. I have finally decided for the latter, for the question seems very "applied".

答:
The mystery has been solved. In reality, the equality does not hold in the last part of the proposition, stated in the question:

$$P_{C_i}^{W_V} = P_{W_O}^{W_V} \cdot  P_{B_i}^{W_O} \cdot P_{C_i}^{B_i} \ne P_{W_O}^{W_V} \cdot  P_{B_i}^{W_O}$$

More specifically: $$P_{C_i}^{B_i} \ne \text{Id}$$
But In reality: $$ P_{C_i}^{B_i} = \big[ P_{W_O}^{W_V} \big]^{-1} = P_{W_V}^{W_O}$$
Hence:

$$P_{C_i}^{W_V} = P_{W_O}^{W_V} \cdot  P_{B_i}^{W_O} \cdot P_{C_i}^{B_i} = P_{W_O}^{W_V} \cdot  P_{B_i}^{W_O} \cdot P_{W_V}^{W_O} $$

Such a transformation yields correct rotation of agent in Visual World coordinate frame:


问:
Direct vs semi-direct methods for visual inertial odometry
I was reading these papers on visual inertial odometry from IROS 15:

Semi-Direct EKF-based Monocular Visual-Inertial Odometry

Robust Visual Inertial Odometry Using a Direct EKF-Based Approach

I would appreciate if someone could explain how semi-direct and direct methods vary exactly? As far as I understand, direct methods use pixel intensities in their framework. However, both these papers listed above use photometric intensities/pixel intensity values and yet one is semi direct and the other's direct.

答:
So there are actually 2 different definitions of semi direct. I will refer to the one used in the paper you linked.

The difference is actual trivial and explained in your first link "Semi-Direct EKF-based Monocular Visual-Inertial Odometry". From the paper "proposed  to estimate  depth  only  for  pixels  in  textured  image  areas".

So basically in semi direct approaches you only run your direct algorithm over selected portions in the image. So rather then run your photometric algorithm over every single point in the image you only run it over points you find may be valuable. This cuts down on computation time as you are working on a smaller subset of the image.

How you select those portions is up to you. LSD-SLAM uses patches that have a high gradient value. SVO which the other user mentioned uses patches around FAST keypoints.


问:
How can I simulate a changing environment with non-rigid objects?
Many robot applications (actually - the most practical and appealing ones) include the robot's reaction to (and impact on) the evironment, e.g. due to stochastic nature of the environment (e.g. when robot should handle non-rigid objects like clothes) or due to the variability of environment (e.g. harvesting robots should be prepared to pick fruits of different sizes and shapes).

The question is - is there a robotics simulator that can simulate not only robot but also the environment as well? E.g. that can simulate the response of robots action on cloth folding or fruit picking and so on. I guess that such simulator is really non-trivial but maybe there is some ongoing project for it?

答:
Gazebo, the most popular open-source robotics simulator at the moment, is adding support for the Bullet physics library and that should eventually add simulation of contact with deformable objects. Check out this question: http://answers.ros.org/question/57900/deformation-on-gazebo-and-ros-possible/

Also, OpenGRASP, built on top of OpenRAVE, includes soft contact simulation.


问:
Appropriate distance sensing technology for specified conditions
I have been considering different distance sensing technologies for the following circumstance:

Range needs to be about 2.5 meters

The surface being measured is a wall that is painted black

The angle between the wall and the sensor would be at worst around 10 degrees, usually around 45 degrees (so reflective technologies will have trouble, to add to the already black wall which will absorb most light).

Preferably I would like to avoid IR because the environment is already going to have a lot of IR noise.

Essentially I want to be able to triangulate the position and angle of an object placed in a rectangular box (about 2.5m x 1.8m) with black walls, with a lot of IR noise.

All the technologies I have considered have issues:

From what I understand ultrasonic will fare very poorly with the angled walls

TOF / lidar modules will do poorly with the black (low reflection) walls.

Infrared sensors will be interfered with by the IR noise.

I could solve the angle issue by placing 6 or so of these sensors around the object so there are sufficient sensors with a mild angle to get accurate measurements, but the trick is eliminating the sensor readings that are inacurate due to sharp angles.

Any thoughts?

答:
If you can instrument the object that you need to detect, consider an electromagnetic tracker, such as the Aurora.

It is not susceptible to the types of challenges you've listed, but it has some of its own: a field generator produces an EM field that allows tracking of sensors that would need to be placed on objects to be tracked. I believe sensors normally require a wire, but perhaps not for all systems. And accuracy will be compromised by any conductive or ferromagnetic materials (i.e., metal) in the box.


问:
iRobot Create2+Arduino: Make the robot stop when facing an obstacle
I have been working on writing a code to control the iRobot Create movements (forward, backward, right, left and stop) from serial monitor and finally I got the correct code. I was trying to understand how to make stop moving when it face an obstacle but I couldn't get it. Also, I didn't know how to make move for a specific distance. Could anyone help me with this?

Here is my code:

```<code>#include "Arduino.h"
#include "Morse.h"
#include <SoftwareSerial.h>
#define rxPin 10
#define txPin 11
SoftwareSerial softSerial = SoftwareSerial(rxPin, txPin);
char inByte = 0; // incoming serial byte

irobot::irobot(int pin)
{
  pinMode(pin, OUTPUT);
  _pin = pin;
}

void irobot::Begin()
{
   delay(2000); // Needed to let the robot initialize
// define pin modes for software tx, rx pins:
 pinMode(rxPin, INPUT);
 pinMode(txPin, OUTPUT);
// start the the SoftwareSerial port 19200 bps (robotís default)
 softSerial.begin(19200);
// start hardware serial port
Serial.begin(19200);
softSerial.write(128); // This command starts the OI.
softSerial.write(131); // set mode to safe (see p.7 of OI manual)
delay(2000);
}

void irobot::runIt()
{
    softSerial.write(142); // requests the OI to send a packet of
    // sensor data bytes
    softSerial.write(9); // request cliff sensor value specifically
    delay(250); // poll sensor 4 times a second
    if (Serial.available() > 0) {
        inByte = Serial.read();
        if(inByte == '8')
        {
            goForward();
        }
        if(inByte == '2'){
            goBackward();
        }
        if(inByte == '4')
            goLeft();
        if(inByte == '6')
            goRight();
        if(inByte == '5')
            stop();
    }
    Serial.println(inByte);
}

void irobot::goForward()
{
   // Drive op code
    softSerial.write(137);
    // Velocity (-500 ñ 500 mm/s)
    softSerial.write((byte)0);
    softSerial.write((byte)200);
    //Radius (-2000 - 2000 mm)
    softSerial.write((byte)128);
    softSerial.write((byte)0);
}

void irobot::goBackward()
{
    // Drive op code
    softSerial.write(137);
    // Velocity (-500 ñ 500 mm/s)
    softSerial.write(255);
    softSerial.write(56);
    //Radius (-2000 - 2000 mm)
    softSerial.write((byte)128);
    softSerial.write((byte)0);
}

void irobot::goLeft()
{
    // Drive op code
    softSerial.write(137);
    // Velocity (-500 ñ 500 mm/s)
    softSerial.write((byte)0);
    softSerial.write((byte)200);
    //Radius (-2000 - 2000 mm)
    softSerial.write((byte)0);
    softSerial.write((byte)1);
}

void irobot::goRight()
{
    // Drive op code
    softSerial.write(137);
    // Velocity (-500 ñ 500 mm/s)
    softSerial.write((byte)0);
    softSerial.write((byte)200);
    //Radius (-2000 - 2000 mm)
    softSerial.write((byte)255);
    softSerial.write((byte)255);
}

void irobot::stop()
{
    softSerial.write(137);
    softSerial.write((byte)0);
    softSerial.write((byte)0);
    //Radius (-2000 - 2000 mm)
    softSerial.write((byte)0);
    softSerial.write((byte)0);
}
</code>```

答: You can use the roombas' bumper and light bumper(infrared) sensors to slow down or completely avoid an object. Note: It won't work on surfaces that don't reflect light. Edit: You can use its wheel encoders to know how far it has gone.</p>


问:
Why does a Bayesian Filter require random controls?
As stated in Probabalistic Robotics, the proof for correctness of a Bayesian Filter relies on the fact that

$$p(x_{t-1}|z_{1:t-1},\ u_{1:t}) = p(x_{t-1}|z_{1:t-1},\ u_{1:t-1})$$

In order to justify this, they say

$u_t$ can be safely omitted ... for randomly chosen controls

Why is that required? Isn't that true because the control input at time step t cannot possibly effect the state at time t-1?

答:
I think it is easy to see, when you take a look at the bayesian network:

Now, we eliminate all the variables not given in your equation:

Based on this baesian network, you can see that $u_t$ has no effect on the other variables and thus can be omitted.

This however would not work for $u_t$s that are not random. If for example $u_t$ would be dependent on $x_{t-1}$. Which means, based on the last position, the actual control is selected:

Here $u_t$ can not be omitted.


问:
Maths in Robotics
I'm interested in Robotics but not very skilled in Maths.

Since I always come across some very intense looking mathematical formulae in robotic reports, books etc I just wonder what branch of Mathematics I should be learning to better understand the material I'm trying to read?

Is there a specific branch of maths relevant to robotics OR do I need expertise across multiple branches of maths?

Is expertise in Maths necessary at all to get into robotics?

Please help.

答:
I cannot give you a definite answer since - as C.O.Park said - it depends so much on what you do. From my experience (doing visual SLAM and machine learning) linear algebra is (to a varying degree) an indispensable foundation in many fields like (2d) computer vision, 3d perception, robotic manipulation, path planning, optimization algorithms in machine learning, dimensionality reduction, deep learning, ...

By the way, I found the video lectures on linear algebra from MIT by Gilbert Strang https://ocw.mit.edu/courses/mathematics/18-06-linear-algebra-spring-2010/ very enjoyable. The provided intuitions made many things click for me.

Second, since we are often dealing with probabilistic problems, good knowledge of (multivariate) statistics and probability are required, particularly when you directly or indirectly deal with sensor data.

For optimization problems including most of machine learning, Calculus comes up very often, because you need gradients for optimizations.

In my opinion, you can also get into robotics without rigorous math knowledge, when you focus more on systems. Robots are complex beasts and companies build robots from hardware and software components, so there is a lot of integration work to be done. A high-level overview of robotics, programming skills and maybe system administration knowledge should get you quite far in this field.

Also I am sure there are some fields, where math is not that important. Human-machine-interfacing comes to mind, though I lack the experience in that field.

Again, this is based on personal experience and most probably, there are various fields where these skills may not be relevant at all while other skills are required.

Title: Maths in Robotics


问:
how to write a uav teleop code?
I'm currently working on a teleoperation code for crazyflie. I'm not using a joystick, but I'm using leapmotion and I'm trying to understand how they code for a joystick in order to implement the same idea in my code with leapmotion.

I understand the general idea of what I'm supposed to be doing; 1) subscribing to the leapmotion data; 2) writing a transformation function and then 3) publishing the goal position to be sent to my controller.

Coding is not my forte and having to deal with ROS is adding to the fun of all of this. So I'm going to ask some questions regarding this code because I don't understand it well.

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

import rospy
import tf
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import Joy
from math import fabs

lastData = None

def joyChanged(data):
    global lastData
    lastData = data
    # print(data)

if __name__ == '__main__':
    rospy.init_node('publish_pose', anonymous=True)
    worldFrame = rospy.get_param("~worldFrame", "/world")
    name = rospy.get_param("~name")
    r = rospy.get_param("~rate")
    joy_topic = rospy.get_param("~joy_topic", "joy")
</code>```

Why is it necessary to get the joy_topic param?

Also, I'm not sure if this is correct but I believe that the x, y, z values are the position values for the joystick, so since this values are always the same they are probably using them as initial values for the position values.

```<code>    x = rospy.get_param("~x")
    y = rospy.get_param("~y")
    z = rospy.get_param("~z")

    rate = rospy.Rate(r)

    msg = PoseStamped()
    msg.header.seq = 0
    msg.header.stamp = rospy.Time.now()
    msg.header.frame_id = worldFrame
    msg.pose.position.x = x
    msg.pose.position.y = y
    msg.pose.position.z = z
    yaw = 0
    quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
    msg.pose.orientation.x = quaternion[0]
    msg.pose.orientation.y = quaternion[1]
    msg.pose.orientation.z = quaternion[2]
    msg.pose.orientation.w = quaternion[3]

    pub = rospy.Publisher(name, PoseStamped, queue_size=1)
    rospy.Subscriber(joy_topic, Joy, joyChanged)

    while not rospy.is_shutdown():
        global lastData
        if lastData != None:
            if fabs(lastData.axes[1]) > 0.1:
                msg.pose.position.z += lastData.axes[1] / r / 2
            if fabs(lastData.axes[4]) > 0.1:
                msg.pose.position.x += lastData.axes[4] / r * 1
            if fabs(lastData.axes[3]) > 0.1:
                msg.pose.position.y += lastData.axes[3] / r * 1
            if fabs(lastData.axes[0]) > 0.1:
                yaw += lastData.axes[0] / r * 2
</code>```

Why is he dividing the values from the joystick with the rate?

This is where I get lost because I don't know how I would translate this leapmotion yaw, pitch and roll values. I guess I could write an if statement saying if the pitch is within this values then hover at 1 meter.

```<code>            quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
            msg.pose.orientation.x = quaternion[0]
            msg.pose.orientation.y = quaternion[1]
            msg.pose.orientation.z = quaternion[2]
            msg.pose.orientation.w = quaternion[3]
            # print(pose)
        msg.header.seq += 1
        msg.header.stamp = rospy.Time.now()
        pub.publish(msg)
        rate.sleep()
</code>```

答:
It would be clearer if you explain were you got the source code you use as an example and what it's suppose to do.

Concerning the first question, the code is taking initialization parameters.
See http://wiki.ros.org/rospy/Overview/Parameter%20Server
The x,y,z do not come from the joypad but from an init file, the joy_topic parameter is the actual name of the topic you want to subscribe, also taken from init file (it is cleaner than hardcoded values in the code because if you change topic name you don't need to recompile)

Concerning the second question, it seems unclear what you want to use as commands for your drone. In the given example, position and yaw is used which is a flat output for quadrotors. But  the leapmotion will give you access to, the orientation of your hand and to the angular rate of your hand, which could also be used to control the quadrotor.
Lastly note that from your input device motion (Joypad or leapmotion) you need as scaling factor for the real world displacement. For example value of a joypad between 0 and 1024 need to be converted to meter some how.

Title: how to write a uav teleop code?


问:
"modern contact dynamics" - Why are spring-damper contact models not being used anymore for dynamic simulations in robotics?
While researching different physics engines for dynamic simulations in robotics I found the following statement on the MoJoCo documentation website:

Physics engines have traditionally separated in two categories. Robotics and biomechanics engines (MATLAB Robotics Toolbox, SD/FAST, OpenSim) use efficient and accurate recursive algorithms in generalized or joint coordinates. However they either leave out contact dynamics, or rely on the earlier spring-damper approach which has fallen out of favor for good reason. Gaming engines (ODE, Bullet, PhysX, Havoc) use the modern approach where contact forces are found by solving an optimization problem at each time step.

Link: http://www.mujoco.org/book/index.html

What is the "good reason" for spring-damper not commonly being used anymore to model contacts in dynamic simulations?

答:
The author appears to be writting in a self promotional style, where the details listed about other work are simplified or ignored so that their claim, that their approach is better, can be established.

I suspect the author is making the statement 'for good reasons' because they don't really know.

I would ignore their claims and go read their calculations document to see if it was useful to me.

Contact is difficult to model and one needs to choose the approach that works for their assumptions, goals, and computational resources.

For all simulation, you should understand the phenomenon you are trying to predict and choose appropriate math for that prediction. If a spring-damper works, use it. If it does not, don't.

You (and the author) need to specify the context in which the spring damper model is being used before we can talk about why it is a good or bad choice.


问:
How to convert/transform robot local frame to global frame to use in Kalman Filter?
I'm currently working on the iRobot Create 2 platform using a RaspberryPi (Python) and ROS. I have an indoor navigation/ GPS system, which can provide me with x,y coordinates within its coordinate system. I have thus far implemented a Kalman filter in MATLAB for practice and now am trying to implement on the Create. I also have an IMU but haven't yet implemented that.

I'm currently trying to figure out how to subscribe to the topics from the Marvelmind indoor nav system (but that's a different issue).

My Kalman filter is using [x, y, xdot, ydot], and I believe those should be in the global frame (which I'm taking to be the coordinate system provided by the Marvelmind indoor nav system). That being the case, I can easily get my x and y position from that system; however, I'm not sure what to do about the xdot and ydot. Currently, I have that information from the Create odometry (Twist msgs), but those are in the local frame (since the robot can only go in the x (forward) direction and can't go in y (side to side)). Do I need to transform the local to the global? If yes, do I need to use the IMU to get the angle to use for the transformation?

I feel like I have many pieces, but I'm just not sure how to piece them together. Any advice would be appreciated. Thank you!

答:
It sounds like you are saying that the navigation system tells you the robot's x and y position within a global reference frame and that the IMU tells you the robot's speed (but not direction) within the same global reference frame. Let's say that these are your two types of measurement.

The kalman filter requires that you provide a transformation from the robot's state space [x,y,xdot,ydot] into the measurement spaces for each of your types of measurements. Here are the two transformations:

GPS: [x,y,xdot,ydot] -> [x,y]

IMU: [x,y,xdot,ydot] -> sqrt(xdot^2 + ydot^2)

Kalman filter also requires a motion model. I think you already have that part, but I'll include it just in case.

Motion: [x,y,xdot,ydot] -> [x + xdot * deltat, y + ydot * deltat, xdot, ydot]

It's okay if some parameters in your state, such as xdot and ydot, are not directly measurable. The kalman filter would still be able to make use of those parameters and narrow down their likely values based on how they affect x and y over time, even if you didn't have speed measurements. Just let the kalman filter work its magic. :-)


问:
Turtlebot 2 dimensions
I am trying to drive the differential drive kinematics of Turtlebot 2. However, I couldn't find the distance between 2 wheels on the datasheet. I'd appreciate if anyone could provide me this dimension.

Thanks.

答:
The specs are listed here:

https://www.active-robots.com/fileuploader/download/download/?d=0&file=custom%2Fupload%2FFile-1434995349.pdf

While they don't directly list the distance between wheels, they do list everything that you need for a kinematics model. I would especially focus on these two numbers:

Maximum translational velocity: 70 cm/s

Maximum rotational velocity: 180 deg/s

Title: Turtlebot 2 dimensions


问:
Mobile robot identification
The problem: line follower robots - non linear systems - use linear PID regulation algorithm in order to bring error to zero. However, using linear regulator is not the best way to drive non linear system.

There is something like global linearization of non linear systems - an algorithm that can bring regulation error to zero. In order to use it, one has to know kinematics of robot: Coriolis, inertion, gravity and friction matrixes. Those were once measured in EDDA manipulator and are now used in science, and that is how I learned about global linearization.

The question: I'd like to identify <s>kinematical</s> dynamical parameters of my line follower robot. I already have kinematical model, since it is simple (2,0) platform. Has anyone got information about good sources on physical parameters identification of mobile robot like this?

答:
If you have a parameterized motion model for your robot, you can include each of those parameters as part of the state space for a kalman filter. Use the parameterized motion model as the kalman filter's motion model and treat the robot's position over time as measurements.

I took this approach in a recent project because I didn't have direct access to the robot and needed the details of its motion model to be calculated automatically. In the following links, kalman.py is a general kalman filter implementation (unscented) and gain_controller.py uses that kalman filter to determine the motion model and choose appropriate values of throttle and brake to achieve a desired acceleration.

https://github.com/ericlavigne/CarND-Capstone-Wolf-Pack/blob/gain_kalman_2018-03-18/ros/src/twist_controller/kalman.py

https://github.com/ericlavigne/CarND-Capstone-Wolf-Pack/blob/gain_kalman_2018-03-18/ros/src/twist_controller/gain_controller.py

Title: Mobile robot identification


问:
Use of automated tricopters instead of quadcopters?
Hy, I just found useful to post my idea here.

I've seen videos about automated quadcopters: http://www.ted.com/talks/raffaello_d_andrea_the_astounding_athletic_power_of_quadcopters.html and http://www.ted.com/talks/vijay_kumar_robots_that_fly_and_cooperate.html.

I surfed pages from the companies presenting this research and other information on the internet, but I haven't found why they use quadcopters specifically.
I understand, how accelerating, rotating and rolling works in those quadcopters - it's simple, but they claim that quadcopters have minimum number of working parts to fulfill their needs, which I don't agree and I think that tricopters are better in this (duocopters can't rotate horizontally, but tricopters can by inclining and then powering the remaining left or right propeller).

I rode forums, calculated and draw drafts of both tri and quad and found, that tri is much more efficient just in everything than quad with same props and battery when taken in account that best properties has the smallest copter with the largest props so:
3:4 moving parts (no vectored yaw in tri), 9.5:16 size, building Y instead of X construction take far less material 1.5:2.82, lesser maximum power input 3:4, better power efficiency makes longer flight time and tricopters have also improved agility over quadcopters.

The only disadvantage I see is a bit complicated horizontal rotating in tricopter without vectored yaw, which can be problem in man controlled machines but easily solved by simple algorithms in automated machines -> it's not a real disadvantage, just a small work to be done. I was thinking about doing that in my bachelor thesis, but for now I am looking for your opinions, thanks!

EDIT: Maybe the torque is the problem, because on tricopters you can can have all 3 props in 1 direction or 2 in 1 direction and 1 in the opposite and it's symmetrical in neither way, but I'm not sure if this is the main problem...

答:
If you want to maximise efficiency, you go with two rotors: a main rotor, and a tail rotor. I.e. a traditional helicopter. With the common CCPM setup (Cyclic Collective Pitch Mixing), you have three servos controlling the main rotor. Together they can tilt it any direction and control the pitch on the blades. As you can imagine, this is mechanically complicated.

The advantage of a quadcopter is that it's very simple; you have no servos. Adjacent arms are counter-rotating to control yaw.

Tricopters have a yaw imbalance that is corrected by dynamically tilting one of the motors.

So, with a tricopter you lose the "no servo" advantage that made people switch from traditional 2-rotor helis to quads.


答:
As you mentioned that SLAM is your interest, the following are the courses that you need to understand a modern SLAM algorithm.

Very important courses:
Linear Algebra,
Optimization,
State Estimation,
Dynamics,
Computer Vision,
Random Process

Optional:
Lie algebra,
Optics,
Signal Processing,
Pattern Recognition,
Nemerical Analysis

But it is not necessary to learn all of the courses above because they contain too many unnecessary things for SLAM (except Linear Algebra). I recommend you take SLAM courses on youtube first.

Title: Maths in Robotics

Body:
I'm interested in Robotics but not very skilled in Maths.

Since I always come across some very intense looking mathematical formulae in robotic reports, books etc I just wonder what branch of Mathematics I should be learning to better understand the material I'm trying to read?

Is there a specific branch of maths relevant to robotics OR do I need expertise across multiple branches of maths?

Is expertise in Maths necessary at all to get into robotics?

Please help.


问:
Compatibility of my quadcopter build
I am building my first quadcopter and have multiple questions. I am using a Crius MWC Multiwii SE 2.6 flight controller which will get input from a Raspberry Pi Zero.
I have this 4in1 ESC and this PDB. The battery is LiPo 4S 45C 1550mAh.

1.) Since the 4in1 ESC has one output for each of the 4 motors. Can I connect these to the corresponding inputs on the MultiWii that would normally be used if I had 4 separate ESCs? When using 4 separate ESCs each would have additional Ground and Voltage which would be missing when using the 4in1 ESC.

My idea was to connect the 5V input and GND of the MultiWii to the PDB but I am not sure if this will suffice in powering the entire MultiWii.
I want to connect the Raspberry to the MultiWii via TX/RX Pins, same question as as before: Do I need additional GND and 5V here if I connect the entire Multiwii to the PDB?

2.) I wanted to connect the Pi Zero also to the PDB. I read that its more secure to use PWR USB port. I am thinking of sacrificing an USB cable and soldering the ends to the PDB. Is this really more secure than using the GPIO Pins and can I power both, Raspberry and MultiWii through the PDB?

Thanks in advance

答:
1) you 4in1 ESC as 3 outputs per motor and 1 input per motor...
Your description of the brushless motor seems very confused. Brushless motor are commanded with 3 phases, theirs switching patterns makes the motor turn.

1b)Concerning power you need to check what the PDB specification are for its 5V line, if the current supported is high enough you can power both our multiWii and the Pi.

2) the usb line as some kind of regulation and fuse that the GPIO doesn't have, see https://raspberrypi.stackexchange.com/questions/1617/how-do-i-supply-power-through-the-gpio

Title: Compatibility of my quadcopter build


问:
Are robot's codes usually compiled or interpreted?
I would bet that usually these are compiled for better performance, and of lawful reasons (make it harder for any user to save code without decompiling), though I'm not sure.

In any case, I believe performance is the major consideration (I'm very new to programming and cannot tell how data processing of robots is different, say, than that of high-end PC games or heavy PC software and if performance considerations will be usually similar or how different).

Are robot's codes usually installed when compiled from source code in an external computer or as pure source codes interpreted by some engine installed on the robots operating system?

(please example on JavaScript and Raspberry pi if possible at the moment).

答:
Many people use Python to control robots. While it does use more computing resources to interpret the python code, it may take less time for the programmer to develop in an interpreted language.

Advantages of Python:

Many programmers are familiar with Python because it is often taught as a first language in schools.

It can be faster to program in Python than in c/c++ because you don't constantly have the compile/test cycles.

There are even versions of python that will run on microcontrollers similar to the Arduino. The one I'm thinking of is MicroPython, but there is also WiringPi.

There are often Python API's for compiled robot software. For example, ROS is usable through Python.

The main disadvantage is that you need something like a Raspberry Pi (at a minimum) to the full version of Python. Even the Raspberry Pi Zero is a powerful brain for a robot.


问:
How to link ends of timing belt into loop
When I buy some length of timing belt I don't know how to link ends of timing belt into loop.

So far I've found one way to do that (thanks to http://www.lasersaur.com/):
http://www.flickr.com/photos/stfnix/8697962319/in/set-72157624491114826

Any other ideas?

答:
The point of a loop would be that it can, well, loop around. The belt never stops. You can't link the ends into a seamless loop. Better buy a looped belt of approximate size and add something to adjust your construct to match the loop size. Leading the belt around 2 wheels in a // shape can help you tighten the loop.

But if you don't really need an endless loop, if you have something that is connected to the "loop" at a fixed point, like in your picture, then that is a different question. In both your picture and the one in the comments the belt isn't looped. Instead one end is simply folded over itself so the teeth bite and prevent it from slipping.

An alternative to this I like is to take both ends of the belt and turn them by 90° with the tooth facing inwards. Place a worm gear between those two ends and a hollow cylinder around it so both belt ends are pressed into the worm gear with their teeth. Now you can turn the worm gear to tighten or loosen the belt "loop". You can 3D print the worm gear and enclosing cylinder to match your belt tooth profile and thickness.


问:
DH parameters for offseted elbow joint
Below is the axis setup for the elbow joint manipulator. I am trying to workout the DH parameters for the manipulator. Green frame represents the base and frame1. Starting from there, I am able to get the correct values up to joint3 (purple frame). If I rotate the frame about Z, so the blue frame moves, it gives me incorrect values. Below is the DH parameter table.

My question is, is the DH table setup correctly? If I move the frame about q3  I am getting invalid results. You will have to set q2 to -90 to achieve the configuration in picture.

```<code>+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|         d1|          0|        -90|          0|
|  2|         q2|          0|         a2|          0|          0|
|  3|         q3|          0|         a3|        -90|          0|
|  4|          0|         d4|          0|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+
</code>```

答:
You can simplify the robot configuration by connecting the Purple frame to the Blue frame with a link.
The length of the new link can be easily calculated with the Pythagorean theorem.
Now you can obtain the DH parameters more easily.

This setup will reach the same results as the original setup, but it is easier to obtain the DH parameters in this case.

Title: DH parameters for offseted elbow joint


问:
How to turn direction of 4 planar wheels into up/down motion?
For the head in a 2D plotter I have 4 gears in an X configuration in the same plane. The gears are driven by belts from the outside in different combinations. The top and bottom gear can be turned in opposite direction as indicated by green arrows. Or the left and right wheels can be turned in opposite directions as indicated by the red arrows. Both of those can happen simultaneously. The gears can also be driven all in the same direction as indicated by the blue arrows. Or all directions can be reversed. This is part of a larger construct and the belt movement already has effects there so they can't be changed.

Now what I want to build is some mechanical contraption that turns the blue arrow movement into an upward (or downward if reversed) movement to rise (or lower) the pen of the plotter.

If all wheels turn anti clockwise the pen should rise. If all wheels turn clockwise the pen should lower (or vice versa).

答:
A- Imagine a hollow cylinder with the following design,

The outside surface of the cylinder can mesh with the four gears in the image and it will be placed among them.

the inside of the cylinder has a helical groove spanning the length of the cylinder (from top to bottom)

the cylinder can only rotate (no upward or downward motion)

the cylinder is open from top and bottom.

B- Now for the part that would hold the pen,

this part will be placed inside the cylinder.

this part will have pins that are meshing the inside grooves of the cylinder.

this part cannot rotate. it can only move up or down.

When the gears rotate according to the blue arrows >>> the cylinder will rotate (meaning that the groves will rotate) >>> and since the pen holder cannot rotate, the groves will force it to move up or down.
The below figure shown the same idea but with a cylinder that has an outside groove.


问:
Delayed Kalman navigation based on two way ranges
I am interested to know the basic principle of the "Delayed KF" when considering an underwater robot aiming to localize it self using a LBL system.

More practically, in order to calculate the distance to each anchor,   the robot transmits a PING to and waits for the RESPONSE from the anchor. This is time consuming as the sound velocity is very  low (1500m/s). Lets say it will take order of 5/10 seconds to complete such iteration (large distances).

If the robot is static (during this operation, or the anchors are very close), I can recover the two travel time,  TWTT, and after that the distance (knowing the sound velocity, divide by two etc). In that case i can include the range to my EKF navigation filter.

Assuming that the robots moves (1 m/s) this assumption does not hold anymore as I can not divide the TWTT by a factor of 2.

Searching online i found talking about the delayed EKF in which (in my understanding) having a  measurements timestamped (the real time and the time when i got it) i re-run the filter at the right time when the measurements was/or should have been done.

Does this hold also for this case and how can be integrated with the navigation filter of the robot?

Geometrically to find the right distance based on the TWTT and considering the movement done during that time seems hard to me to model. At least i have not found any paper in the literature.

Regards,

答:
I will answer with respect to the unscented kalman filter (ukf) instead of the extended kalman filter (ekf) because I think the ukf is easier to understand. You can either switch to using ukf yourself (as I have) or translate the idea back into ekf. They are similar enough that the same idea would work.

In ukf the probability distribution of your state space is represented by a swarm of "sigma points." When receiving a new measurement, we need to translate each of the sigma points into measurement space so that so that they can be compared with the actual measurement.

Let's say your state includes variables x, y, vx, and vy to represent both position and velocity. Each sigma point would have these same variables. The anchor has position anx, any. The main challenge is to calculate the expected return time (rt) of a sonar ping that reaches you now and starts at your previous position at rt time ago. If you can do that for each individual sigma point, then combining those results into an overall state update is just following the usual ekf recipe. We'll assume that velocity was constant during the period of time that this measurement took place.

Current position: x, y

Previous position: px = x - vx * rt, py = y - vy * rt

Expected rt: rt = ((distance from x,y to anx,any) + (distance from px,py to anx,any)) / (speed of sound)

Ideallly you would treat these as a system of equations with rt completely unknown, but that's probably more trouble than it's worth. You can get away with using the measured value of rt for estimating the previous position part. The rt that you calculate in that last equation is what you'll compare with the measured rt.


问:
Power delivery issue for our human following robot project
We are very new in the game of robotics and we are dealing with a power delivery issue for our motors.

We are using two 12V DC motors to give motion to our robot.
We are using the L293D motor driver to control the  motors.

But motors are not receiving enough power so that they may give enough torque. I measured the power output to the motors and it is 3.7V.

For power we are using an 9V DC battery. We are using an Adriano UNO board as our microcontroller.

Full list of parts used:-

Ardriuno UNO

9V battery

L293D motor driver

UV sensor

Now how can we deliver addition power direct to the motors using the L293D motor drivers. Please help

Code: https://github.com/pantharshit00/robo

答:
It would be nice to know more about your robot. I've built robots in that range, and can make some suggestions.

First, if you're using a 9v battery such as the ones used in smoke detectors, you need a better battery. These just don't put out enough current to run everything a robot requires. I'd suggest a set of 6 AA batteries in series. This will provide you with the same voltage, but much more current.

Second, test the L293D motor driver by connecting the Arduino to the motor driver and motor and the power batter. Program the Arduino such that the pins will set the motor running at full speed with absolutely no PWM. Then edit your question to show the voltage across the motor.

Third, I believe that the Arduino Uno uses 5V logic, but I'm not absolutely sure that there isn't a 3v3 version of it.

Fourth, when putting together a robot, it's best to do one system at a time, get that working, and then add a new system. In this case, I'd suggest getting the motors working, and only then add the UV sensor.

Fifth, when asking a question, it's a good thing to provide links to everything necessary. It's even better to include these things in the question. It would be nice to have a schematic of the circuit to see if perhaps the pins are hooked up correctly. Not all the Arduino pins work the same.

I may have given too much information. I think that the main problem is the 9v battery.


问:
What other equipment or hardwares I should buy along with raspberry pi
I am planning to build some primary level of project like home made robot, which I need to explore an image recognition and path recognition(using four wheels vehicle). So I am looking to buy a device like Raspberry pi.

What other equipment or hardwares I should buy along with raspberry pi.

Which one is better Arduino or raspberry pi for robotics using AI.

答:
First, it is important to understand the differences between the Arduino and the Raspberry Pi.  You can read more here and here.

The Arduino is a microcontroller.  It is a specialized board that handles low level, often repetitive tasks like reading a sensor or controlling a motor with a motor driver.

The Raspberry Pi is a complete Linux computer that typically runs on the Raspbian OS.  While the Raspberry Pi has a set of GPIO ports that can interface with sensors and motor drivers, the Pi can also process higher level tasks.  The Pi is capable of running python and C/C++ libraries such as OpenCV and other AI frameworks.

Using the two boards in a project are not mutually exclusive!  You can use the Arduino for controlling motors and processing sensors while using the Raspberry Pi for your AI libraries and decision making steps.