问:
Denavit Hartenberg parameters - 3DOF articulated manipulator
I am trying to solve a forward kynematics problem for a 3DOF manipulator.

I am working with the Robotics Toolbox for MatLab created by Peter Corke and after calculte the DH parameters and introduce them into MatLab to compute the fordward kynematics the plotted robot is not what it should be.

I guess I made some mistakes calculating the DH parameters.

Attached is the file where you can see what are the DH frames calculated for each joint and the DH parameters for each frame.

Anyone could give me a clue whether this is the correct answer?

Here is the image with the frames calculated by me.

And here the robot I get from Matlab (using the Robotics Toolbox by P.Corke)

答:
I defined the robot using your (assumed standard) DH parameters:

```<code>>> robot = SerialLink([pi/2 1 0 pi/2; pi/2 1 0 -pi/2; pi/2 1 1 0])
robot =
noname:: 3 axis, RRR, stdDH, fastRNE
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          1|          0|     1.5708|          0|
|  2|         q2|          1|          0|    -1.5708|          0|
|  3|         q3|          1|          1|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+
</code>```

Note that for revolute joints the value of theta is ignored, not it is replaced with the values q1 to q3 in the printed table above.

To plot the robot at this configuration:

```<code>>> robot.plot([pi/2 pi/2 pi/2])
</code>```

shows a robot somewhat different to your diagram.  I think your DH parameters are not correct.

I really don't like DH parameters.  Looking at your diagram I can write the forward kinematics as a string of simple transformations expressed in the world coordinate frame

```<code>>> DHFactor('Rz(q1)Tz(L0)Rz(q2)Ty(L1)Tx(L2)Ry(q3)Tx(L3)')
  .
  .
  .
DH(q1, 0, 0, 0).DH(q2, L0, L2, -90).DH(q3, L1, L3, 90)
</code>```

where the parameters of the DH() functions are theta, d, a, alpha.

Body:
I defined the robot using your (assumed standard) DH parameters:

>> robot = SerialLink([pi/2 1 0 pi/2; pi/2 1 0 -pi/2; pi/2 1 1 0])
robot =
noname:: 3 axis, RRR, stdDH, fastRNE
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          1|          0|     1.5708|          0|
|  2|         q2|          1|          0|    -1.5708|          0|
|  3|         q3|          1|          1|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+

Note that for revolute joints the value of theta is ignored, not it is replaced with the values q1 to q3 in the printed table above.

To plot the robot at this configuration:

>> robot.plot([pi/2 pi/2 pi/2])

shows a robot somewhat different to your diagram.  I think your DH parameters are not correct.

I really don't like DH parameters.  Looking at your diagram I can write the forward kinematics as a string of simple transformations expressed in the world coordinate frame

>> DHFactor('Rz(q1)Tz(L0)Rz(q2)Ty(L1)Tx(L2)Ry(q3)Tx(L3)')
  .
  .
  .
DH(q1, 0, 0, 0).DH(q2, L0, L2, -90).DH(q3, L1, L3, 90)

where the parameters of the DH() functions are theta, d, a, alpha.


问:
Plotting SE(3) points in a 3D graph
I have a data-set containing points in SE(3), i.e the 4 $\times$ 4 transformation matrices and the corresponding dual quaternions. Now, I want to plot these points in a 3D graph. How to generate this plot?</p>

答: Or you can use the trplot() function from my Robotics Toolbox for MATLAB

ContentLicense: CC BY-SA 3.0

Title: Plotting SE(3) points in a 3D graph

ContentLicense: CC BY-SA 4.0


问:
Validation of dh parameters
I am working on a concept of robotic arm for which I have fixed DH frames and parameters. Is there any way to validate the correctness of DH parameters and frames ?

答:
I'm not a fan of DH parameters but they are entrenched in robot kinematics.  An approach I prefer is to just write the whole chain as a series of simple transformations, eg. Ry is a rotation about the y-axis, Tz is a translation along the z-axis. From your figure, starting at the base and applying the transformations consecutively, you could write it like

Ry(q1)Rz(q2)Tx(L1)Rz(q3)Ty(L2)Rz(q4)Tx(L3)...

but I get lost with what happens near the wrist in your diagram.  The Robotics Toolbox has a function, DHFactor, that will take a string like this and automatically compute it to DH parameters.

Title: Validation of dh parameters


问:
is it possible to get all possible solutions of inverse kinematics of a 6 DOF arm?
I would like to know if there is any way to get all the possible solutions of inverse kinematics of a 6 DOF robotic arm?
I have found some good Matlab codes but gives only one solution like in Peter corke's book .
Thank you in advance.

答: The Robotics Toolbox for MATLAB has an analytic solution, ikine6s, that works for most 6DOF arms with a spherical wrist.  It takes a configuration string.  If you call it with all 8 possible combinations then you will get all possible solutions, eg. 'run', 'lun', 'rdn' etc.


问:
How to manage interrupts on an AVR?
I have a number of interrupt service routines on an AVR. These include interrupts for USART serial communication, timers, and SPI communication.

For all of these, I use circular queues (using a start and end pointer, without boundary checking).

Some problems start to occur if the AVR starts to get overloaded. The circular queues will lose chunks of data. To solve this, I can reduce the load on the AVR (for example, by skipping some cycles in the timer). However, this is a manual process, where it is reduced if the AVR appears to have problems. This is partly because I do want relatively consistent timer periods.

However, even at 70% average processor load, the queues can fill up randomly by chance.

In case of spurious overloading at times, how can I make this more adaptive to avoid queue overflows?

答:
First, all processors have limited performance. So the effect could be that the performance is just not good enough for the amount of work you have to do.

The average load is pretty uninteresting for diagnosing lost interrupts. The interesting thing is how many interrupts and therefore how much load on the interrupt service routines are there in peak situations. If the interrupts cause too much calculation that is needed to be done in a fixed amount of time then you need a faster CPU.

With little computing in the ISR and a big buffer, these peaks can be flattened out, but this introduces a delay. If you can decrease the amount of data written into the buffer then a bit more processing in the ISR could pay off.

If your solution is to skip the unimportant(?) processing in the main routine, then you should put more processing into the ISRs. This way the CPU load will go to the ISRs and the main routine will get less. The problem here is that if the processing of one interrupt is ongoing, the next interrupt will be delayed, as its ISR can only run after finishing the other ISR. If this delay is too long then the interrupt will get lost.

So choose the way that is best fitted for your solution. But be aware that: "The only replacement for CPU power is CPU power."

Title: How to manage interrupts on an AVR?


问:
Kuka KR16L-2 robot simulation base and wrist rotation inconsistent with original robot
The issue is regarding simulation of Kuka robot KR16L6-2 in MATLAB using Robotics toolkit by Peter Corke. I wish to simulate  the kinematic before passing a command to real robot for motion.

I have attached the DH-Parameters. Apart from this I have also tried many other combination of orientations but to no useful effect.

The problems is that the robot base rotates counter-clockwise by default for positive increases in Joint1, while the original robot moves in the opposite direction. Similarly for the wrist roll, i.e. Joint 4, the direction of simulation is reversed.

In order to confirm that it's not my mistake only, I searched for similar ready made simulation software. Although it did not include the same KUKA robot, a similar variant (KUKA_KR_5_sixx_R650) was available. Hence, KUKA_KR_5_sixx_R650 had one set of motions for base and wrist in RoKiSim v1.7 for positive increases in joint angle and reverse motion in roboanalyzerv7  .

NOTE: Only the rotation of J1 (base) and J4 (Wrist Roll) are reversed
and I want to recreate the results of RoKiSim v1.7 in Matlab where rotations are similar to the real world robot spec provided by KUKA.

答:
This is a limitation of DH notation.  Axis rotation follows the right-convention about the z-axis.  For the first joint you are stuck with rotation about the world z-axis, hence your problem.

For the Robotics Toolbox joint rotation all follow the right-hand convention.  You could multiply your joint angle vector by a vector of rotation directions

```<code>dir = [-1 1 1 -1 1 1];

kuka.fkine(q .* dir)
</code>```

Body:
This is a limitation of DH notation.  Axis rotation follows the right-convention about the z-axis.  For the first joint you are stuck with rotation about the world z-axis, hence your problem.

For the Robotics Toolbox joint rotation all follow the right-hand convention.  You could multiply your joint angle vector by a vector of rotation directions

dir = [-1 1 1 -1 1 1];

kuka.fkine(q .* dir)


问:
Computing the Jacobian Matrix -- chain rule?
I am learning about robot kinematics and the Jacobian matrix, and I'm trying to understand how to compute the Jacobian matrix given a kinematic chain, such as a robot arm. I understand the theory behind the Jacobian matrix, but I'm not sure actually how it would be calculated in practice.

So, let's say that I have a 7 DoF robot arm, with 7 joints and 6 links between the joints. I know how to compute the transformation matrix between each joint, and by applying forward kinematics, I know the pose of the end effector for any configuration of joint angles. To calculate this, I have written some code which stores each transformation matrix, and then multiplies them in series to create the transformation matrix between the first joint and the end effector.

However, how do I now go about computing the Jacobian matrix? My solution so far, is to write down each transformation matrix by hand, then multiply them all by hand, to yield the overall transformation matrix, with respect to the joint angles. I could then differentiate this to create the Jacobian matrix. The problem with this though, is that the maths becomes very, very complicated as I move along the kinematic chain. By the end, there are so many terms as a result of the multiple matrix multiplications, that it just becomes so tedious doing this by hand.

Is there a better way to do this? In the case of calculating the forward kinematics, I didn't have to do it by hand, I just wrote some code to multiply the individual matrices. But when I want the Jacobian matrix, it seems like I need to compute the derivative of the overall transformation matrix after it has been computed, and so I need to do this by hand. What's the standard solution to this? Is it something to do with the chain rule for differentiation...? I'm not sure exactly how this applies here though...

Thank you!

答:
For a robot with a kinematic model expressed using standard DH parameters the classic reference is:

Differential Kinematic Control Equations for Simple Manipulators,
Paul, Shimano, Mayer,
IEEE SMC 11(6) 1981,
pp. 456-460

This is implemented in many software tools include the Robotics Toolbox for MATLAB as the method jacobe which gives the Jacobian for velocity expressed in the end-effector coordinate frame.  This method is used by jacob0 for velocity expressed in the world coordinate frame.

To compute the forward kinematics as suggested above you can use the fkine method with symbolic joint variables and the result can then be differentiated symbolically.  MATLAB has pretty good symbolic support these days.

If you want to write the forward kinematics as a simple string of transformations (sidestepping the whole DH parameter thing) and do the chain rule then this article might be of interest.


问:
Screw based Jacobian
I am working on kinematics of 5-DOF robot. I already derived geometric Jacobian for position and orientation control.

But for singularity analysis, I require a simplified jacobian. And that can be obtained on base of screw theory called Screw Based Jacobian.

But I am not able to find the method to obtain screw based jacobian matrix; which completely explain the method to derive screw based jacobian.

Any advice on where I can get it? Please help me out.

答:
The geometric Jacobian provides all the information you need for singularity or manipulability analysis.  Linearly dependent columns correspond to joints with parallel axes.  More information about Jacobians for under-actuated manipulators (as is your case) can be found in my book Robotics, Vision & Control" section 8.4.1.

For information about computing the "screw based Jacobian" check out the book Modern Robotics by Park&Lynch.

Title: Screw based Jacobian


问:
Multivariable PID controller
I'm making a robot that uses two DC motors to move, and has two distance sensors on its right side. I want to make a PID controller to control the angle of the robot (using a wall as reference) and also its distance to the wall.

I made a PID controller that can minimize the difference between both sensors' measurements, making the robot go always parallel to the wall. I also made a controller for controlling the distance one sensor measures, for the robot to go always at the same distance to the wall.

What I want to do is combine those two controllers into one. I know there's something called MISO PID controller (multiple inputs single output) but I don't know how to use it or even design it. I thought of adding those two variables (the difference and one sensor's measurement) and using that as the controller's input. Is that right? How can I achieve this?

PS: I also thought of adding the difference between each sensor's measurement and the distance I want, or using those two differences as the variables of my MISO controller.

答:
The principle is called linguistic multivariable fuzzy pid control. The idea is to create a domain specific language and create with that a domain specific controller. The existing sensor variables and existing motor primitives for starting and stopping the servo motors are taken as a base for constructing a higher level behavior system on top. Rodney Brooks described this principle first. In terms of programming it means, to add new methods in the controller class. For example, “stayaway-fromwall”, “move-onthe-line”, “rotate-until” and so on. The motion primitives are forming together with the sensor variables a domain specific language and the controller is formulated in that language.

I know, it sounds very theoretically. A practical example can be tested out in the famous “Karel the robot” game. The user has the task to define new subfunction and combines them into a program. This gets started in the Karel simulator. Programming a real robot in a maze works similar.

Title: Multivariable PID controller


问:
What is the best SE3 library for python?
I am currently googling on this topic but I would like to hear your opinion.

What is the best self-contained SE3 library that offers interconversion between quaternion, rotation vector, transformation matrix?

Those are the functions that I often use for my projects and all implemented in Matlab. And I am looking for an alternative to avoid re-implementing them all in python.

Quaternion2RotVector and its inverse

Quaternion2RotMatrix and its inverse

RotMatrix2RotVector and its inverse

SE3 inverse, the inverse function of each rotation representation

答:
Writing your own package is always the best way to learn.  If you want to try something premade here are a few packages to choose from:

Spatial Math Toolbox for Python Python3+numpy+scipy, also available from PyPy, with classes and functions. This is a Python port of my Robotics/Spatial Math Toolbox for MATLAB

Sophus C++/Eigen with Python wrappers


问:
Control System Analysis - Block Diagram Basics

Why this relation is not right?

C(s) =  R(s) * [G(s).G(p)+D(s)] / [1+G(s).G(p)+D(s)]

答:
First step is the summing junction:

$$
R(s) - C(s) \\
$$

Then the controller:

$$
G_c(s)\left(R(s) - C(s)\right) \\
$$

Then the plant:

$$
G_p(s)\left(G_c(s)\left(R(s) - C(s)\right)\right) \\
$$

Then the addition of the disturbance input:

$$
D(s) + G_p(s)\left(G_c(s)\left(R(s) - C(s)\right)\right) \\
$$

Finally, all of the last step above is equal to the output, $C(s)$:

$$
C(s) = D(s) + G_p(s)\left(G_c(s)\left(R(s) - C(s)\right)\right) \\
$$

Multiply out the business on the right:

$$
C(s) = D(s) + G_p(s)\left(G_c(s)\left(R(s) - C(s)\right)\right) \\
C(s) = D(s) + G_p(s)G_c(s)R(s) - G_p(s)G_c(s)C(s) \\
$$

Then rearrange to get the $C(s)$ terms all on one side:

$$
C(s) + G_p(s)G_c(s)C(s) = D(s) + G_p(s)G_c(s)R(s) \\
$$

Factor out the $C(s)$ term and divide to get the answer:

$$
C(s)\left(1 + G_p(s)G_c(s)\right) = D(s) + G_p(s)G_c(s)R(s) \\
C(s) = R(s)\left(\frac{G_p(s)G_c(s)}{1 + G_p(s)G_c(s)}\right) + \frac{D(s)}{1 + G_p(s)G_c(s)} \\
$$

So it looks like your answer isn't correct because you have what distributes to be an $R(s)D(s)$ term, when the disturbance only sums with the result of the plant (and doesn't multiply), and you also have the disturbance term in the denominator, which is also incorrect.

There are all kinds of "thumbrules" on how you do block diagram reduction, but I always just multiply it out. It only takes a little bit longer, but I personally always get something wrong when I try the (N/(1+N)) method or something like that.


问:
Angle Random Walk vs. Rate Noise Density (MPU6050)
I’ve made a datalog from a MPU6050 (IMU: gyroscope and accelerometer) at 500Hz sample rate. Now I want to calculate the characteristics from the gyro to evaluate the sensor.

For the gyro I’ve found following values in the datasheet:

Total RMS Noise = 0.05 °/s

Low-frequency RMS noise = 0.033 °/s

Rate Noise Spectral Density = 0.005 °/s/sqrt(Hz)

Now I want to ask how I can calculate these values from my dataset?

At the moment I’ve the following values from the dataset:

Standard deviation = 0.0331 °/s

Variance = 0.0011

Angular Random Walk (ARW) = 0.003 °/sqrt(s) (From Allan deviation plot)

Bias Instability = 0.0012 °/s

Is the ARW equal to the Rate Noise Spectral Density mentioned in the datasheet? And also is the RMS Noise from the datasheet equal to the standard deviation?

edit:
I found following website: http://www.sensorsmag.com/sensors/acceleration-vibration/noise-measurement-8166
There is the statement:  "...Because the noise is approximately Gaussian, the standard deviation of the histogram is the RMS noise" So I guess the standard deviation is the RMS noise from the datasheet. But how about the ARW?

答: check page no: 11, eqn(8) from An introduction to inertial navigation by Oliver J. Woodman (Technical Report Number 696 from the University of Cambridge Computer Laboratory).


问:
Combining centres of mass and Inertia tensors
I'm trying to build the dynamic model of a 6DOF robot, and the company that has built it, kindly provides a document having the masses, centres of mass, principal axes of inertia and principal moments of inertia taken at the center of mass, taken at the center of mass and aligned with the output coordinate system, and taken at the output coordinate system (I've come to known that this was obtained from a tool in SolidWorks)

The robot has 6 actuators responsible for the motion of each one of the 6 links available. The problem that I have here is the way I should calculate the inertia matrix $M(q)$. Since the matrix has to have a 6x6 dimension, I know that I have to do some kind of "combining" between one link and the correspondent actuator.
The problem is that I don't really know how can I find the respective centre of mass between the two "objects" and the respective moment of inertia of the "multiobject". I've seen people saying that it is simply the summation of the respective moments of inertia but they need to be in respect to the same orientation and translation.

Can anyone shed some light into this? Suggestions would be greatly appreciated.

Thanks

答:
You are lucky to have the inertial parameters for the robot! This is something that manufacturers very rarely share and it is hard to reverse engineer.

Given that you have these inertial parameters, plus a kinematic model expressed in Denavit-Hartenberg parameters, then a very well known and efficient algorithm called recursive Newton-Euler (RNE) can compute the joint torquse required to achieve a particular acceleration, given joint angles and velocities.  Calling this algorithm 6 times (with zero velocity, unit acceleration, and zero gravity) will allow you to build up the joint-space inertia matrix column by column.

In practice the inertia of the motors, amplified by the gear ratio squared, is of a similar order to the link inertias and must be taken into consideration.  These terms are added to the diagonal of M.

The RNE algorithm can be found in most robotics textbooks and is implemented in various robotic software packages.  Using this algorithm to determine the joint-space inertia matrix is part of "Method 1" described in: Efficient dynamic computer simulation of robotic mechanisms, M. W. Walker and D. E. Orin, ASME Journal of Dynamic Systems, Measurement and Control, vol. 104, no. 3, pp. 205-211, 1982.  Other algorithmic approaches exist but given the efficiency and ubiquity of RNE this is a pretty good way to compute M for a 6-axis manipulator.

Both algorithms mentioned are implemented in the Robotics Toolbox for MATLAB. The Toolbox allows you to enter the kinematic and inertial parameters, as well as friction models and motor inertia, to investigate the dynamics of robot manipulator arms.


问:
Wrong forward-kinematic after calculating from DH-parameters
I'm working on an exercise about DH-parameters for my robotics course in university and I ran into a problem, because my solution is different from the one given by my professor. I calculated the DH-parameters $\theta=\theta_1-90˚, d=20m, a=0, \alpha=-90˚$. So far my solution is identical with the one of my professor. Now the next task is to calculate the transformation matrix $T_{0,1}$ using the calculated DH-parameters.  Now I calculated:
\begin{equation}
T_{0,1}=
\left(\begin{matrix}
\cos(\theta_1-90)  & 0 & -sin(\theta_1-90) & 0 \\
sin(\theta_1-90) & 0 & cos(\theta_1-90) & 0 \\
0 & -1 & 0 & 20\\
0 & 0 & 0 & 1
\end{matrix}\right)
\end{equation}
Now the solution of my professor states:
\begin{equation}
T_{0,1}=
\left(\begin{matrix}
\cos(\theta_1-90)  & 0 & -sin(\theta_1-90) & 0 \\
sin(\theta_1-90) & 1 & cos(\theta_1-90) & 0 \\
0 & -1 & 0 & 20\\
0 & 0 & 0 & 1
\end{matrix}\right)
\end{equation}
Which is almost what I got, but not exactly since they put a 1 where I put a 0 in position (2,2). After multiplicating the transformation matrixes of the single DH-parameters I had found that the position (2,2) evaluates to $\cos(\theta_i)\cdot\cos(\alpha_i)$ and since since $cos(-90˚)$ evaluates to 0 it should be 0, or not? Where am I wrong?

答:
Your professor has made an error, but he or she is only human.

The upper-left 3x3 matrix must be an orthonormal rotation matrix.  Every column of that must have a unit norm.  The second column $[0, 1, -1]^T$ has a norm of $\sqrt{2}$ which makes the rotation matrix invalid.


问:
How can I determine the Transition Matrix of a Kalman-Filter ?
I am trying to set up a Kalman-Filter to filter position-measurements of a self-driving car. To do so, I consider a state-vector with 5 elements and am now trying to set up the Transition Matrix.

As you can see in the following picture, I do not have a clear understanding of the car dynamics. How does my steering for example contribute to my x-position? Since steering is the rate of change of the vehicle heading, it must somehow be linked to acceleration. Can anyone help me to figure out A ?

I would be so grateful!

答:
When localizing a robot we usually use the world coordinate frame. Since it's static it is easier to represent the position of the robot. As an example you may use the North and East directions as the x and y axis of the world coordinate frame. But if you are travelling on the robot you can notice that the heading direction always changes. Let's take 2 cases to understand this.
1. Robot is travelling to the absolute north with a speed of 1m per second.
2. Robot is travelling to the absolute north-west with a speed of 1m per second.

In both cases robot travels towards its' heading direction but in world coordinate frame the heading is different.

We can calculate the A matrix by assuming that there is no control inputs to the system.


问:
Question about Lens distortion
Suppose I have a 240x180 camera with the following distortion co-efficients,

$$k_1 = -0.138592767408$$
$$k_2=0.0933736664192$$
$$k_3=0.0$$
$$p_1=k_4=-0.000335586987532$$
$$p_2=k_5=0.000173720158228$$

The camera center is given to be

$$u_0 = 129.924663379$$
$$v_0 = 99.1864303447$$

And the distortion formulae are

$$r^2=(u-u_0)^2+(v-v_0)^2$$

$$
\begin{align}
  \begin{bmatrix}
    u_d\\
    v_d
  \end{bmatrix} = & (1+k_1r^2+k_2r^4+k_3r^6)
  \begin{bmatrix}
    u-u_0\\
    v-v_0
  \end{bmatrix} + \\
& \begin{bmatrix}
    2k_4(u-u_0)(v-v_0)+k_5(r^2+2(u-u_0)^2)\\
    k_4(r^2+2(v-v_0)^2+2k_5(u-u_0)(v-v_0)
  \end{bmatrix} +
  \begin{bmatrix}
    u_0\\
    v_0
  \end{bmatrix}
\end{align}
$$

In the above formulae, $p_1=k_4$ and $p_2=k_5$, $u$ and $v$ are the undistorted co-ordinates and $u_d$, $v_d$ the corresponding distorted ones.

My understanding of the undistorted co-ordinates is that it is a mapping of every undistorted co-ordinate to a distorted one. Therefore, the ranges of both undistorted and distorted co-ordinates should be between (0-240, 0-180). However, going by the above formula, I get extremely high numbers for distorted co-ordinates if I input undistorted co-ordinates in the range(0-240, 0-180) which goes against this understanding. Have I done something wrong here?

答:
You can only say that the distorted image coordinates are in the range (0-240, 0-180), since that's the image you are starting with.  Typically you assume the dimensions of the undistorted image as being the same as the distorted image, and for every pixel in the undistorted image work out the corresponding coordinate in the distorted (input) image.  It will have non-integer coordinates so often an interpolation is done to find the pixel value to drop into the undistorted image.  It's possible that some values in the undistorted image map to points outside the distorted image, you should check for that, and maybe need to crop the result to eliminate pixels of indeterminate value.

A quick sanity check indicates that there's something badly wrong with your distortion coefficients, they are just too big.  Take a value of r=100 which is possible towards the edge of your image, $k_1 r^2 \approx 1300$ which is a massive shift.   The $k_2 r^4$ term is of the order of millions.

Do you know what the units of the distortion parameters are?  They might be in units of pixels or units of metres on the image plane.

Title: Question about Lens distortion


问:
Quadcopter motors vibration problem
I built an Arduino Due based quadcopter, with MPU-6050 for auto levelling and BMP-085 for Altitude.

I am using Kalman filter code from here for the MPU-6050 to take the Roll and Pitch angels from the sensor.

The Roll and Pitch angels are correctly read while the quadcopter motors are off, but when the motor speed increased the angels start to be more inaccurate.

As shown in the following photos (where I took different speed motors to show the effects of the motor vibration on the MPU readings). These tests were taken while the quadcopter on the ground.

Motor speeds vary from 0 to 180 so I choose (50,100, and 150).

My sensors are fixed on a breadboard over a plastic surface with anti-vibrator rubber balls as shown in my quadcopter pictures.

My problem is that the controller I designed is very sensitive to these vibrations, is there any way to reduce these vibrations as minimum as possible.

Thank you

答:
First of all, check that your propellers are in good condition and balanced.

You mentioned that your controller design is sensitive to vibrations. Would you mind elaborating? Perhaps you will have to trade off some of your design qualities.

Have you tried a low-pass filter on your signals before feeding it into the controller?

You can also try using a different vibration damper. The optimal damper depends on the mass that the damper is carrying.

Title: Quadcopter motors vibration problem


问:
Power systems design for a vehicle
I'm designing a system to power up my vehicle with 12vDC and I have 8 motors each draws 25A at max and they are not expected to work always at the same time, meaning that in the maximum case 6 of them working 70% and 2 of them works 100%, so my question is that what power supply should I choose to be enough to power the vehicle up like what power rating should it be ? 1kw or 2kw ? And why ? I mean if there is any equation or calculation method to use ?
Please help me with that.</p>

答:
Power equals voltage times current. One motor therefore has maximum power of 12V * 25A = 300W

6 x 0.7 x 300W = 1260W (1.26kW)

2 x 1.0 x 300W = 600W (0.6kW)

Total: 1860W (1.86kW)

So the maximum power is equal to 1.86kW meaning you need to have a supply power being able to output at least that much (though in reality, at least a bit more).

Title: Power systems design for a vehicle


问:
How to control a brushless motor?
I consider using a brushless outrunner motor, designed for helicopters, in my driving roboter. How can I control such a brushless motor with my micro controller? Of course I'll have a separate power source.

The roboter should be able to move forwards and backwards, so I need to control the motor in a way to determine direction of rotation, too. I think this isn't related to the question, but I need to ensure high acceleration.

Specially, I am talking about this motor which is listed in a German shop.

答:
You can control brushless motors 2 ways

control with a hall effect sensor http://scholar.lib.vt.edu/theses/available/etd-09152003-171904/unrestricted/T.pdf

sensorless(back emf) control http://www.pmdcorp.com/downloads/app_notes/BrushlessSensorConfig.pdf
or you can buy an esc (elcetronic speed control) My advice If you are not knowledgeable about electronic you can buy esc

Title: How to control a brushless motor?


问:
Path planning of wheeled robot
let's say the robot Looks like this (an usual robot arm with 4 wheels) :

In this case as far as I know, the idea of path planning is just to compare the actual position x with the desired position x_goal. At the end, the robot would be at the position x_goal.

But the Question is how to set this x_goal. If the robot doesnt have an arm, then x_goal is equal to the position that the robot has to be there. But since the robot has an arm, x_goal must takes account the reachability and singularity of the arm.

What would be the most common/efficient way to estimate this x_goal in this case?

答:
Pathplanning for a wheeled robot consists of transferring domain specific knowledge into the software. Requirements like determine the xgoal position and take care of reachability of the robotarm are usually not given as a machine readable description but as expert knowledge of how to manual control the robot with a joystick. What the system engineer has to do is to convert weak defined knowledge into a pathplanner which gets executed autonomously. One option in doing so is a fuzzy control system. In contrast to the famous myth, these systems can be used in complex domains, for example pathplanning.

Quote: “This paper treats the autonomous navigation problem of robotic
  systems in a dynamic and uncertain environment. [...] The fuzzy logic
  is certainly one of the most adopted approaches in industry.“

Boufera, Fatma, et al. "Fuzzy control system for autonomous navigation
  of thymio II mobile robots." Journal of Emerging Technologies in Web
  Intelligence 6.1 (2014): 101-105.

Before the pathplanner can be realized with fuzzy-rules and linguistic variables, the requirements have to be written down in natural language. This is part of the software engineering process and is done in expert-interviews. That are humans, how have controlled the robot platform in the past with a joystick and are able to explain which details are important for reaching a position.

Title: Path planning of wheeled robot


答:
First try adding some dampers to the electronic modules. The best way to reduce the vibration is balancing the propellers. You can mount one propeller at a time and measure the amplitude of the vibration first. Then use a piece of sticky tape on one blade and measure the amplitude again. If the amplitude is less try adding sticky tapes on the same blade to reduce the vibration as much as possible or try the other blade.

You need to adjust some of the parameters before using the kalman filter for your quadcopter since variances of the readings depends on the sensor that you use. Further make sure that your sampling rate and the update frequency is matching.

Title: Quadcopter motors vibration problem

Body:
I built an Arduino Due based quadcopter, with MPU-6050 for auto levelling and BMP-085 for Altitude.

I am using Kalman filter code from here for the MPU-6050 to take the Roll and Pitch angels from the sensor.

The Roll and Pitch angels are correctly read while the quadcopter motors are off, but when the motor speed increased the angels start to be more inaccurate.

As shown in the following photos (where I took different speed motors to show the effects of the motor vibration on the MPU readings). These tests were taken while the quadcopter on the ground.

Motor speeds vary from 0 to 180 so I choose (50,100, and 150).

My sensors are fixed on a breadboard over a plastic surface with anti-vibrator rubber balls as shown in my quadcopter pictures.

My problem is that the controller I designed is very sensitive to these vibrations, is there any way to reduce these vibrations as minimum as possible.

Thank you


问:
Rotating two things on the same axis
I'm working on a project where I want two separate shafts to be in the same place. Think of it like a clock where the hour and minute hands move independently. Also like a clock, I want the motors and gears tucked away in back.

With clocks, they use a kind of tube that slides over a shaft. Then the shaft and the tube can both be driven from the back and rotate independently.

Is there a name for this tube part? If I wanted it to be low-friction, is there a name for a part that includes bearings?

答:
It's called a sleeve bearing if you intend for the shafts to rotate relative to each other or a bushing if you intend for the shafts to move axially relative to each other. They generally look pretty much the same, and usually are, and people generally just use the term "bushing" for both, but there is technically a distinction.

You're probably going to have trouble getting power on and off the bushing unless you machine it to have some kind of gear teeth or something, and if you want the outer "shaft" to rotate relative to the chassis then you'll need a bearing for the outer shaft, too, so you'll wind up with bearing-sleeve bearing-inner shaft.

Getting power to or from the shaft is an open-ended question that is driven by your task, so please save any questions like that for Robotics Chat when you have sufficient reputation.

Title: Rotating two things on the same axis


问:
How to determine the Process- and Measurement Noise Covariance Matrix of a Kalman Filter
I'm stuck with the following problem:
In a Kalman Filter, we assume the following holds:

The state vector can be filled with values which are returned by actual sensors. The Transition matrix needs to be set up according to a motion model. Same holds for the control input matrix. But how can I determine w_t and v_t ? How do I know which values to choose for the covariance matrices Q and R ?

I hope someone can help

答:
All noises are assumed as Gaussian models in KF and EKF. To determine the values for the measurement noise you need to collect a sample data sets from the sensors that you are using. For each data set you can calculate the variance value put it to the corresponding position of the measurement noise covariance matrix. </p>

<p>Usually process noise covariances are experimental values. You can use a trial and error method to find the values which reduce the error.  </p>

Body:
All noises are assumed as Gaussian models in KF and EKF. To determine the values for the measurement noise you need to collect a sample data sets from the sensors that you are using. For each data set you can calculate the variance value put it to the corresponding position of the measurement noise covariance matrix.

Usually process noise covariances are experimental values. You can use a trial and error method to find the values which reduce the error.


问:
Euler Angles Order For Quadrotor Modelling
I am modelling a quadrotor and I need to choose an order for the rotations that transfer vectors which are represented in Earth Frame to the Body Frame.

what is the most logical order for these rotations?

which order is likely used?

does the order have a big effect on the control of the quadrotor?

Thanks in advance for any answers

答:
For a flying vehicle an intuitive representation of orientation is roll-pitch-yaw angles aka Tait-Bryan angles.

For a quadrotor it is common to assume a left-handed world coordinate frame with the z-axis down and the x-axis to the north (a so called NED frame).  Attach a similar frame to the vehicle (the body frame) with its z-axis down and its x-axis in forward direction (choose one rotor to be the front).  Now the rotation matrix from world frame to body frame is the result of a sequence of canonical rotation matrices, multiplied in this order:

Yaw rotation about the z-axis, which describes the vehicle's heading direction.

Pitch rotation about the y-axis, which describes the "angle" of attack.  A positive rotation causes the nose to pitch down which is commonly considered a negative pitch angle (nose is pitched down).  Since a quad rotor is under-actuated, it is pitch motion that is used to generate forward thrust for forward flight.

Roll rotation about the x-axis.


问:
If you can create pure yaw motion with a quadcoptor then why won't this work with a tricoptor?
An answer to the question Why are quadcopters more common in robotics than other configurations?  said:

You need 4 degrees of freedom to control yaw, pitch, roll and thrust.

Four props is therefore the minimum number of actuators required. Tricoptors require a servo to tilt one or more rotors which is more mechanically complicated.

In a comment, I asked:

How do you get pure yaw motion with a quadcoptor and if that's possible why won't this work with a tricoptor? I don't understand how can you get yaw motion with any system where all rotors are in a plane without first tilting and moving. I would have thought that the main difference between quadcopters and tricoptors would be the kinematic calculations would be more complex.

Another answer explained:

you get pure yaw in the following way:

North and South motors rotating the same speed but collectively at a higher (or lower) speed than East and West Motors which are also at the same speed.

This explains why it works with a quadcopter, but doesn't explain why it won't work with a tricopter.

Is it simply the fact that the asymmetry means that you can't imbalance the torque effects to provide yaw movement while still keeping the thrusts balanced to keep pitch and roll constant?

答:
Here's an intuitive way to think about this.  A quadrotor has 4 motor speeds that you can adjust to control 4 things: vertical lift, pitch angle, roll angle, heading angle.

A trirotor has 3 motor speeds and therefore only let's you control 3 things, one of the four previously listed has to go.  Using @srim's notation, and if A is the front, then thrust from A+B+C is vertical lift.  The differential thrust between A and (B+C) controls pitch angle.  The differential thrust between B and C controls roll angle.  Once you have set the speeds of the 3 rotors to control these 3 things, then you can compute the reaction/drag torques that will arise, but you have no degrees of freedom left to control it.  In general these torques will not be balanced, so the tri rotor would spin around its vertical axis, maybe a big tail fin could keep that in check...


问:
Tackling loops and discontinuations in PID line follower
We are making a PID line follower using arduino for a competition. According to the rules of the competition they will have loops and discontinuations in their track. Can anyone give some idea how we can tackle them using code.</p>

答:
What you need is a hierarchical fuzzy controller for crossing detection. The situation, that the black line on the ground has complicated shapes like zigzags and loops is a good opportunity for implementing a program flow which contains of if-then-statements and subfunctions. In contrast, a normal line following robot will need only a simple table, which produces for an input signal of the sensor an output action of the motor, in case of a crossing layout of the path, a more complicated information processing is needed, namely a dedicated program flow which is done in C, Java or Python.

In the easiest implementation, the robot stops if he detects an unusual pattern on the ground and waits for new commands from the human operator. He can drive to the left and follows the path1 or he can stay on the main line and follows path2.

```<code>class Detectcrossring:
  def __init__():
    if getsensor()=="crossing":
      askhumanoperator()
    else driveforward()
  def getsensor():
    result ="normallane"
    result ="crossing"
    result ="zigzag"
    return result
  def driveforward():
  def askhumanoperator()
    print "crossing detected."
    print "Should i move forward, left, right?"
</code>```

Body:
What you need is a hierarchical fuzzy controller for crossing detection. The situation, that the black line on the ground has complicated shapes like zigzags and loops is a good opportunity for implementing a program flow which contains of if-then-statements and subfunctions. In contrast, a normal line following robot will need only a simple table, which produces for an input signal of the sensor an output action of the motor, in case of a crossing layout of the path, a more complicated information processing is needed, namely a dedicated program flow which is done in C, Java or Python.

In the easiest implementation, the robot stops if he detects an unusual pattern on the ground and waits for new commands from the human operator. He can drive to the left and follows the path1 or he can stay on the main line and follows path2.

class Detectcrossring:
  def __init__():
    if getsensor()=="crossing":
      askhumanoperator()
    else driveforward()
  def getsensor():
    result ="normallane"
    result ="crossing"
    result ="zigzag"
    return result
  def driveforward():
  def askhumanoperator()
    print "crossing detected."
    print "Should i move forward, left, right?"


问:
Multivariable PID controller
I'm making a robot that uses two DC motors to move, and has two distance sensors on its right side. I want to make a PID controller to control the angle of the robot (using a wall as reference) and also its distance to the wall.

I made a PID controller that can minimize the difference between both sensors' measurements, making the robot go always parallel to the wall. I also made a controller for controlling the distance one sensor measures, for the robot to go always at the same distance to the wall.

What I want to do is combine those two controllers into one. I know there's something called MISO PID controller (multiple inputs single output) but I don't know how to use it or even design it. I thought of adding those two variables (the difference and one sensor's measurement) and using that as the controller's input. Is that right? How can I achieve this?

PS: I also thought of adding the difference between each sensor's measurement and the distance I want, or using those two differences as the variables of my MISO controller.

答:
A (possible) way that I could think of is to use a (resource-aware) switching PID controller in your feedback loop. The operation is as follows:

[1] The controller is of the class single-input-single-output.

[2] The input to the controller is the (desired) error i.e., the error measued between the reference position and the location of the wall-1/wall-2 that is being measued along the motion. I omit the discussion on the control output which is redundant.

[3] The fun part starts here: In short the controller will act as a switching PID controller which switches between its two error inputs. Thereby, reducing the error when certain limit exceeds. This limit can be in terms of performance, for instance, how the ideall the robot should maneuver parallel to the wall.

[4] This switching action can be based on algorithims like roll-out policy. Thereby, switching between the inputs whenever it is necessary. Of course, this depends upon your performance specificationsn and your processor's capabilities. Nevertheless, you can dig into topics such as resource aware controller design per se to look more in detail about this.

Disclaimer: Following this procedure might have impact on the stability, hence always verify the stability argument before going forward with these techniques.

Title: Multivariable PID controller


问:
Hand eye calibration of robot arm using a 2D camera and marker of known size
If my understanding is correct, one flavor of hand eye calibration requires the following pairs of data for different robot arm positions:

The transform from the robot base to the robot hand.

The transform from the marker to the robot camera.

By transform, I mean the 4x4 homogeneous matrix that contains both rotation and translation information.

The part I am stuck on is getting the transform from the marker to the robot camera, which is a 2D camera. The marker can be checkerboard, or Aruco, etc. for which opencv can give you the pose of the marker with respect to the camera.

The question I have is fundamental: Is it even possible to get the pose of the camera from images of a marker of known size? Won't there be a scale ambiguity specifically in the z direction of the camera? Is this z ambiguity resolved by using a marker of known size? Finally, if this scale ambiguity is always there, is it possible to do hand eye calibration using a 2D camera and a marker of known size?

答:
I'm tied up at the moment, so I'll flesh this more out tomorrow morning, but I believe the answer to your question is that the pose estimation algorithms utilize the camera parameters of the camera you're using. Field of view, resolution, focal point, and a known object's dimensions all come together to form similar triangles. The focal point and resolution form the triangle between the CCD or image receptor and lens of the camera, then there's another triangle between the lens and the actual object.

If you know the focal distance and "height" on the image receptor, then you can get the angular width of the object. Knowing its real height and the angular width then lets you calculate the distance or depth of the object.


答:
Is it even possible to get the pose of the camera from images of a marker of known size?

-> Yes. From the camera image that is containing a calibration board, you can estimate a 3 by 3 homography matrix H. Then, if you know a camera intrinsic parameters then you can inversely recover the scale known transformation matrix using the intrinsic and homography H. Here is a good reference for this (have a look at 3.1 Closed-form solution). Translation scale will be unknown when the board size is unknown.

Won't there be a scale ambiguity specifically in the z direction of the camera?

-> Not at all if the camera intrinsic and board size is known.

Is this z ambiguity resolved by using a marker of known size?

-> Yes

Finally, if this scale ambiguity is always there, is it possible to do hand eye calibration using a 2D camera and a marker of known size?

-> As your statement "scale ambiguity is always there" is not necessary, the calibration is fairly simple now with the scale known transformations above. You just need to find a robot base to the board transformation and hand to eye transformation.

You have 12 unknowns and each constraint will give you 3 equations. 4 different set data will be enough for a closed-form solution. What you need to solve is

$${^RT_{E_i}}{^ET_{C}}{^{C_i}T_{B}}={^RT_{B}} $$

where R is robot base, E is end-effector, C is a camera, B is the board coordinate.

Refers to the following paper for a detailed solution.

Title: Hand eye calibration of robot arm using a 2D camera and marker of known size

Body:
If my understanding is correct, one flavor of hand eye calibration requires the following pairs of data for different robot arm positions:

The transform from the robot base to the robot hand.

The transform from the marker to the robot camera.

By transform, I mean the 4x4 homogeneous matrix that contains both rotation and translation information.

The part I am stuck on is getting the transform from the marker to the robot camera, which is a 2D camera. The marker can be checkerboard, or Aruco, etc. for which opencv can give you the pose of the marker with respect to the camera.

The question I have is fundamental: Is it even possible to get the pose of the camera from images of a marker of known size? Won't there be a scale ambiguity specifically in the z direction of the camera? Is this z ambiguity resolved by using a marker of known size? Finally, if this scale ambiguity is always there, is it possible to do hand eye calibration using a 2D camera and a marker of known size?


问:
provide world map for ekf global localization problem
I am trying to implement the ekf_localization algorithm in page 217 (Table 7.3) of the probabilistic robotics book by Thrun.

From my previous post, I understand that I need to extract observed features on step 9 of the algorithm given in the book. So I am planning to use a line extraction algorithm (https://github.com/kam3k/laser_line_extraction) to extract lines, then find the center point of the line and use that point as my observed feature in step 9.

Click part1 part2 to see table 7.3.

Now, I am having trouble understanding what is the map (m) input.

Since, the ekf_localization algorithm assumes that the map is already giving, and let’s say figure 1 is the actual map that my robot will navigate in. Does this mean, that m consist of points in the world coordinate frame and that I can manually choose them? For example, the dots in figure one are my point landmarks that I provide for the algorithm (m = {(2,2), (2,4), 5,1), (5,2), (5,3), (6,2)}). If so, how many points should I provide?

Be great if you could help C.O Park.

答:
In EKF slam generally we include landmarks which can be marked as points: Corners, columns, trees etc. The uncertainties for landmarks are calculated by assuming that those are point landmarks. (1) Since it's not straightforward to include a line in EKF SLAM you need to come up with a solution to convert them to feature points. Using the center point of the line will not help to track the entire line. Adding sampled points from a line as landmarks will increase the matrix size as well as the computational cost. Following are some methods that I have tried in this case.

1) Convert the line to $y = mx + c$ form and include m and c values in the state matrix. However this doesn't provide the limits of the line.

2) Include the starting points and end points of the line. This is the most computationally efficient way to include a line in EKF SLAM.

You need to modify your measurement model to recover the line from the state matrix and generate measurements.


问:
Robot moving on a flat floor - 2D or 3D SLAM?
What is the best way to model the SLAM problem (offline) for a wheeled robot moving on a flat floor in 3D space (the pose has only 3 degrees of freedom)?

In terms of sensor data, we have the wheel odometry and 3D measurements of landmarks (coming from a Kinect v2).

The bit that I'm confused about is that this is a very standard setting, yet all the examples I've seen for 2D SLAM use 2D features and don't mention anything about what to do with 3D sensor measurements, and vice-versa, I haven't seen a 3D SLAM solution where we could easily incorporate known constraints on the robot pose (has to lie on a plane). It would be great to know what is common practice in this setting. An explanation or a link to an example implemented in some SLAM framework would be of much help.

答:
Since you are using Kinect my advise is to use Octomap ROS package to build a 3D map. You can extract the 2D occupancy grid map of the ground floor from the Octomap for path planning and navigation. </p>

<p>One disadvantage of using Octomap is, it doesn't have a pose correction method. It just build the map based on the poses provided by the odometry.  </p>

Body:
Since you are using Kinect my advise is to use Octomap ROS package to build a 3D map. You can extract the 2D occupancy grid map of the ground floor from the Octomap for path planning and navigation.

One disadvantage of using Octomap is, it doesn't have a pose correction method. It just build the map based on the poses provided by the odometry.


问:
How many independent control inputs does a hexarotor have?

6, because a rigid body has six degrees of freedom

4, since it is similar to a quadrotor, except with more motors

6, because there are six motors

I was taking an online quiz, and I thought option 3 is the correct one, only to find out I got the answer wrong. I know that I could change the answer 2 times and by chance I finally got it right, but I want to understand why option 3 is wrong?

答: The 4 independent inputs directly correspond to the transmitter stick inputs, $u = [\delta_{thr}, \delta_{ele}, \delta_{ail}, \delta_{rud}]^T$


问:
Swarm Robot Simulation
Both V-rep and Webots simulator says that they can simulate multiple robots. Is these simulators are capable for simulate swarm robots? Can I simulate the swarm intelligence algorithms like ACO, BeeClust algorithm, Stick pulling Algorithm, Occlusion Based Collective pushing etc. on these simulators? Which robot I have to select for implementing above algorithms? For implementing these algorithms which simulator will be better? Any suggestions is appreciated.

答:
It's important to separate between simulation and artificial intelligence. An environment for simulating hundreds of robots on the screen is called a game engine. It can be realized with standard programming languages for example Python/pygame. The user creates an array of objects and each objects has a position and a speed value. The graphics engine displays all the objects on the screen. This is called a realtime simulation.

The other part is the artificial intelligence to control the objects. Without an AI, the robots in the simulation has to be controlled manually or by a random generator. They are displayed at the screen with smoothly 60 frames per second, but it's unknown how to move them right so that robots are solving a task. In general, a simulator for displaying hundreds of objects is easy to realize. The more advanced part is to create the AI which drives the objects autonomously.

Simulators like Webots, V-Rep, Gazebo, pygame or Unity3D are the environment in which a robotics controller is developed. They are providing the graphics and the underlying physics engine. They are working as a game construction set in which the user can script a game.

Title: Swarm Robot Simulation


问:
Vertically moving robot
I want to build a 4WD robot that can travel vertically by pushing against opposite rails. The wheels push against the rails and generate friction, the robot moves upwards as the wheels rotate. This is the side view that I am thinking of:

If one side moves faster than the other then the robot would loose grip and fall down. I have to ensure that the wheels on both sides rotate at the exact same speed. How to maintain the same speed on both sides?

答: The most popular method is to use wheel encoders. Since it counts the rpm value, we can maintain the same speed in both sides. But as I understand this is not a reliable way to keep the robot in exact horizontal position. You can use an IMU to measure the robots orientation and the speed of the wheels can be adjusted through a PID controller.</p>

Title: Vertically moving robot


问:
What is the difference between a Robot and a Machine?
What is the difference between a Robot and a Machine? At what point does a machine begin to be called a robot?

Is it at a certain level of complexity? Is it when it has software etc?.

For instance: A desktop printer has mechanics, electronics and firmware but it is not considered a robot (or is it). A Roomba has the same stuff but we call it a robot. So what is the difference.

I have always believed that a robot is a robot when it takes input from it's environment and uses it to make decisions on how to affect it's environment; i.e. a robot has a feedback loop.

答:
When I was contributing to the Robot Wikipedia article, this was a question I spent a long time struggling with. What could you possibly write in the first sentence to basically define 'robot'. I spent days searching for people's personal definitions of robot, and reading forum discussions, until I found the 'agent' definition, and I realised this was the closest I was going to get to a definition.

A robot is a mechanical or virtual artificial agent

The sense of agency seems to be key whether or not people generally call a machine a robot. A machine must give a sense that it has its own agency:

it's active in the environment

it can take in data

it can affect the environment

it has its own internal state

This is why software agents get called robots too, even though they have no body. Software agents are active in the environment inside computers.

A long time ago, the Wikipedia article went into much more depth describing how, for example, Kitt would be considered a robot, while a radio controlled humanoid would generally not be. See the Robot page from July 2009. (Sadly, since then it has rotted quite a lot.)

I think that the reason the 'agency' definition works well is that it replaces the poorly defined and poorly understood concept of 'robot' and replaces it with the equally poorly defined, but much better understood concept of 'agency'. Even if we don't explicitly think of animals and robots as agents, we are wired to recognise agents, and categorise objects into things that are and aren't agents. We can easily tell the difference between animals and plants (well, for the types of animals and plants we usually encounter).

Animal -> Robot

Plant -> Machine


答:
You should not follow this approach. It is nice in theory but is not really suitable for real life.
If you use a single motor, you are not able to recover from any tilting due to a slipping wheel. If you have separate motors for the sides, it will be rather difficult to synchronize the motors e.g. while turning on the robot.

You always have to think about a cable or motor failure. Your robot has to be able to hold its position even if you cut some cables or a motor burns out. In your configuration, you have no safety or fallback method.

The easiest fix is to add some more (passive) wheels at one rail beneath your current wheels (distance maybe half the rail distance). This stops the robot from tilting and falling. You can then add encoders to this passive wheel and use this data to control the motor that drives the robot up and down. As its a passive wheel you will have less problems with slip.

(The encoder data will also be an important input for the emergency braking system that your robot has)

Title: Vertically moving robot

Body:
I want to build a 4WD robot that can travel vertically by pushing against opposite rails. The wheels push against the rails and generate friction, the robot moves upwards as the wheels rotate. This is the side view that I am thinking of:

If one side moves faster than the other then the robot would loose grip and fall down. I have to ensure that the wheels on both sides rotate at the exact same speed. How to maintain the same speed on both sides?


问:
Turning a 2 wheeled robot 90°
i got a task for which i need to drive a certain distance and move around an object. I finally managed that my robot goes almost straight but i think its enough. But now i gotta tackle the problem how to turn my robot by 90° at certain points.
Can someone give me some advise how i can program the turning of the robot?
For measurements i got wheel encoders and an IMU but i dont have any experience using the IMU.
Thanks you for reading :)</p>

答:
Usually IMUs have only an accelerometer and a Gyro. Gyro gives the angular rates around x,y and z axes. The gyro can help to measure the rotated angle around z axis. But this is not very accurate method since gyro gives the angular rate to calculate the rotated angle by multiplying from the sample time.

$\theta = \omega_z*\Delta t$

Calculating the absolute rotation is not possible unless you use an magnetometer. My personal advice is to use the wheel encoders to calculate the rotated angle using the following method.

Title: Turning a 2 wheeled robot 90°


问:
Open source robotics projects that have complex software structure?
I've built some robotic systems that rely on handcrafted hierarchical FSM, I'm hoping to learn more by studying codes from some open source system that have more complex structures. Can anyone point out some open source robotics projects that have complex software structure? For example, a service humannoid robot might need to handle different situations, so if it's a handcrafted system then it should have a complex structure storing and retrieving instances of interaction. It shouldn't just be a system like an end-to-end deep reinforcement learning based humanoid walking because this system encapsulates too much its working functions in a blackbox but still pretty straightforward in terms of input-output relationships in control and perception. </p>

答:
Robotics software can be separated into environments and the motion controller itself. Notable example for robotics environments are ROS or Lejos. They are providing hardware capabilities to control the robot on a physical level and they have built in message passing features. On top of ROS and similar software so called robotics controllers are realized. That's a cognitive layer which is able to control the robot autonomously. A motion controller is a replacement for a manual driven remote control. They are replacing a human operator.

The robot controller can be realized with the mentioned hierarchical FSM, deeplearning or with behavior trees. In most cases, domain knowledge has to be given in a machine readable language. More simpler controller are able to pathplanning the trajectory between two points. They are working with searching in the state space. The lejos project provides a built-in package for finding the shortest path between points which is helpful for implementing an obstacle avoidance system.


问:
constraining rotation in jacobian IK algorithm
i'm implementing a damped least squares IK algorithm for an articulated arm, based on the work here. I have the algorithm working well, but it occasionally produces solutions that have segments intersecting, and doesn' account for the realities of cable management etc. that prevents full 360 rotation of any given joint.

can the constraints be incorporated into the composition of the Jacobian, or is there some other approach for solving for these?

edit

I have a solution that kind of works, but it gets stuck sometimes and feels inelegant. In effect, as I perform an iterative IK, at each step I check to see if the solution self-collides, or violates an angle constraint. If it violates an angle constraint, I clamp the angle. If it self collides, I take the offending segment and rotate it by a random amount. This seems to work, but in addition to feeling wrong, it occasionally "gets stuck." What I mean by this is that as I try to run the solver, it's unable to find a solution from a particular configuration to get to what is otherwise a reachable area until I move it into some other configuration. Code below. Any advice appreciated.

```<code>while (iterations++ < 100) {
  SimpleMatrix j = computeJacobian(segments);
  SimpleMatrix dTheta = calcDeltaThetasDLS(j, target);

  for (int i=0; i<segments.size(); i++) {
    Segment s = segments.get(i);
    s.angle += dTheta.get(i, 0);
    if (s.angle < s.minAngle)
      s.angle = s.minAngle;
    else if (s.angle > s.maxAngle)
      s.angle = s.maxAngle;
  }

  segments.get(0).recompute();

  for (int i=0; i<segments.size(); i++) {
    Segment s = segments.get(i);

    if (s.isOverlapping(segments)) {
      s.angle += PI * Math.random();
      segments.get(0).recompute();
      dThetaLast = null;
    }
  }

  if (dThetaLast != null && dTheta.minus(dThetaLast).elementMaxAbs() < 0.0001) {
    break;
  } else
    dThetaLast = dTheta;
}
</code>```

答:
If you want to stick to Jacobian-based IK techniques, such as DLS, you ought to take barrier functions into the game and couple the resulting primary and secondary tasks with the null-space operator.

The barrier functions can generate a repulsive potential for the joints when they approach their physical constraints.

The primary task (reaching for the target) and the secondary task (joint bounds avoidance) can be then aggregated together using the null-space projection of the Jacobian.

Here's an enlighting paper on these topics:

A Realistic Joint Limit Algorithm for Kinematically Redundant Manipulators, H.-Y. Lee, B.-J. Yi, and Y. Choi, International Conference on Control Automation and Systems, Seoul, 2007.

In particular, the null-space projection of the DLS version $J^\#$ of the Jacobian $J$ is $I-J^\#\cdot J$.


问:
How to tell if a stepper motor position is moved by external force?
I'm designing a round knob positioned by a stepper. The knob is intended move by an external force while positioned by the stepper. I'd like the stepper to release (power off) when the knob is manually turned.

Is there a way to sense the application of external force via the stepper drivers ie a current sense ?

I know there is a current spike when the stepper position stalls. I'm unsure whether the current spikes when already positioned and the position manually moved.

答: I am not sure how you are driving your stepper motor. In case you developed your own way, you could connect an analog pin to the wires that drive the stepper motor. These should be able to detect an induced voltage if the stepper is turned without being driven.</p>


答:
Any general purpose robot simulator like V-REP or Webots can get your task done, though with not the best support for swarm robots in particular. ARGoS is a good platform for swarm robot simulation, maintained well enough with active contributors and shall allow you to inherently have support for centralized/de-centralized control. </p>

<p>The most common swarm robots are the Kilobots and the Khepera and ARGoS supports both of them. </p>

Title: Swarm Robot Simulation

Body: Both V-rep and Webots simulator says that they can simulate multiple robots. Is these simulators are capable for simulate swarm robots? Can I simulate the swarm intelligence algorithms like ACO, BeeClust algorithm, Stick pulling Algorithm, Occlusion Based Collective pushing etc. on these simulators? Which robot I have to select for implementing above algorithms? For implementing these algorithms which simulator will be better? Any suggestions is appreciated.

Body:
Any general purpose robot simulator like V-REP or Webots can get your task done, though with not the best support for swarm robots in particular. ARGoS is a good platform for swarm robot simulation, maintained well enough with active contributors and shall allow you to inherently have support for centralized/de-centralized control.

The most common swarm robots are the Kilobots and the Khepera and ARGoS supports both of them.


问:
How is the IMU used in the PixHawk?
Looking at 3DR's Pixhawk hardware on GitHub the FMU and IMU are treated as different pieces of hardware. On the schematics for the FMUs (example) there is limited reference to the IMU.

So, is the IMU a separate board inside the Pixhawk? Is there information somewhere that shows which IMUs come with which FMUs?

答:
Take a read here and here.
The Flight Management Unit (FMU) board and the IO board combined make a Pixhawk autopilot.

The FMU is contains the control logic and the sensors while the IO takes care of the IO (duh) and some failsafe functionality which is best kept separate from the main loop.

Recent Pixhawk versions contain more advanced versions of the FMU board which hosts more than one IMU sets. To be more specific, they are populated with 2 or 3 separate accelerometers, gyroscopes and magnetometers.
I'm not 100% clear if they use 3 9DOF chips or 9 3DOF chips to achieve this; it may vary from FMU version to FMU version.

That said, the FMU hosts one (or more FMUs) and not vice versa.

Title: How is the IMU used in the PixHawk?


问:
How do I Calibrate Analog Thumb stick?
Outline:

I'm trying to work with an Arduino and Analog thumb stick to get values for a simple differential drive robot I'm working on. The Keyes_Sjoys Arduino Joystick Module I have in my possession is giving me some strange numbers.

Following axises Data I have:

X-axis range of 0 to a shaky 470-520 with a center value of 40.

Y-axis range of a solid 4 to solid 1023 with a center value of 605.

Problem

I haven't used analog sensors before but it seems pretty obvious that my X-axis ranges should feel somewhat similar to the Y-axis but they don't. In addition, the X-axis hits zero way way before even coming close to the edge for its operating range.

Is my sensor broken (it's new), or is there some way I can recalibrate the potentiometer?

Note, I also asked this over on Electrical Engineering Stack Exchange.

答:
It certainly sounds like the joystick is broken, but in Robotics, nothing is certain. Some things to try:

Swap over the X and Y wires going to the Arduino. Which axis exhibits the problem now?

Disconnect the joystick from the Arduino, and power it up on its own. Using a multimeter, measure the voltage on the X axis wire as you move the joystick. Does this make more sense? If so, then there's something wrong with the way it's connected to the Arduino, or something wrong in software.

Disconnect the joystick completely from its power, and use the multimeter to measure the resistance between the X axis signal wire and the GND wire. If this doesn't make sense, then there's likely a fault in the joystick. Possibly a failed solder joint. This could be fixed by simply re-heating the solder joints on that pot.

Title: How do I Calibrate Analog Thumb stick?


问:
Self balancing robot: PID Control Algorithm
I am working on a hobby project for self balancing robot. My robot balances quiet well, so my next step is to start moving it. I saw in some blogs that they used a cascade control algorithm as shown below

So I have the following question which is giving me a hard time

When I see some videos about segway like systems (like boston dynamics here ), I see that before moving forward, the robot moves a bit backward (to generate a tilt) which I think is justified because it must first get an error to get started in the forward direction. So will this kind of control system be able to handle that or do I need to code it seperately?

答: If you for example want to move in the positive direction the speed error will become positive. The first PID controller will translate that into a (most likely) positive angle reference as well. So the inner PID controller will try to bring the measured angle to the positive reference, which initially only can be achieved by driving backwards. Once that positive angle is achieved the wheels will need to continuously drive forwards in order to exert a torque to keep the robot at that angle. So you do not need to do anything additionally to achieve this behavior.</p>


答:
The fastest way in tuning the pid controller for a self-balancing is to put a remote control into the loop. That is a knob which adjusts pid parameters manual, similar to what musicians are using for fine tuning a bass-guitar. What exactly the knob modifies depends on the control system. It can be the sinus waveform, the amplitude or the delay until the feedback is send back into the system. The important thing is, that the knob can't be controlled by the pid system itself but the input is given from the outside. That's a well informed human operator who is able to detect if the balancing works or not.

The knob will transform a closed loop system into a semi-open system which depends on it's environment. The resulting problem “which position needs the knob?” is a sign for progress because it helps to reduce a complicated control problem to parameter finding.

Title: Self balancing robot: PID Control Algorithm

Body:
I am working on a hobby project for self balancing robot. My robot balances quiet well, so my next step is to start moving it. I saw in some blogs that they used a cascade control algorithm as shown below

So I have the following question which is giving me a hard time

When I see some videos about segway like systems (like boston dynamics here ), I see that before moving forward, the robot moves a bit backward (to generate a tilt) which I think is justified because it must first get an error to get started in the forward direction. So will this kind of control system be able to handle that or do I need to code it seperately?


问:
Difference between Collision Cone and Velocity Obstacle
I'm trying to understand these two classic approaches to path planning in dynamic environments/with moving obstacles.
According to some review papers, there seems to be quite a difference between them, e.g. so that the collision cone approach (as presented in [1]) can be used for non-circular objects as well. But to me they seem very analogous:

In my understanding, given a point robot and a circle obstacle, both moving at constant velocities $v_A$ and $v_B$, the collision cone $CC$ is defined by two rays through the robot, tangentially containing the whole obstacle. If the relative velocity $v_r = v_A - v_B$ is contained in this cone, a collision will eventually happen, otherwise they will pass without collision. Now, the velocity obstacle seems to be just the collision cone moved by $v_B$, i.e. $VO = CC \bigoplus v_B$ where $\bigoplus$ is the Minkowski sum. Therefore, any velocity of the robot in $VO$ will result in an collision, all the others won't.

Is there something I'm misunderstanding here or is it that simple?
Here is a figure from [2] that seems to fit to my understandings.

[1]: Chakravarthy, A., & Ghose, D. (1998). Obstacle avoidance in a dynamic environment: a collision cone approach. IEEE Transactions on Systems, Man, and Cybernetics - Part A: Systems and Humans, 28(5), 562–574. https://doi.org/10.1109/3468.709600

[2]: Wilkie, D., van den Berg, J., & Manocha, D. (2009). Generalized velocity obstacles. In 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems (S. 5573–5578). IEEE. https://doi.org/10.1109/IROS.2009.5354175

答:
After reading through the original collision cones paper and many others I realized that my understanding of the collision cone was wrong. The "collision cone" mentioned in the Velocity Obstacle papers refers to something completely different.

Simply said, given a point robot and an obstacle both moving at (given) constant velocities, the collision cone is the set of angles or directions, that will result in a collision if the robot chooses one of them. In the most cases it is just one range of angles, and visualized in the work- or configuration space (which are equivalent here) resembles a cone. Note that the vertex of the cone is located at the robots position, and is generally speaking not diverging exactly in the direction of the obstacle, but roughly in the direction to the path the obstacle is following. See this figure as example, where O is the robot and the CC is hatched:

Source: [1]

Now for the Velocity Obstacle the explanation given in my question seems still correct to me, and the differences are quite noticeable now. In the VO case, the "collision cone" mentioned there is just defined by the two tangential straights, and refers to the relative velocities, unlike the CC that regards the robots direction.

Another difference is the meaning of the cones in these two approaches. CCs describe forbidden directions for the robot, where both velocities are already fixed. VOs contain all forbidden velocity vectors, which may have different directions and magnitudes.

[1]: Chakravarthy, A., & Ghose, D. (1998). Obstacle avoidance in a dynamic environment: a collision cone approach. IEEE Transactions on Systems, Man, and Cybernetics - Part A: Systems and Humans, 28(5), 562–574. https://doi.org/10.1109/3468.709600


问:
Setting up a Structured Light Stereo system
I am trying to get started with a stereo experiment and wanted to know the mistakes I might have made during the experiment. The goal is to calculate the depth map of an object using one light projector and one camera mounted in a stereo configuration.

I have a DLP 4500 lightcrafter of resolution 912x1140 made by Texas Instruments and a camera of resolution 600x800. I am planning to use triangulation methods to estimate the depth. This is how I have setup the components.

1) The experiment is conducted in an enclosure to eliminate effects of ambient light.

2) The structured light pattern that has to be projected onto the object  is loaded as an image. The image is projected onto one of the walls of the enclosure. The projector lens is focused until the projection is clear and the projector is moved backwards or forwards until the projection fits within the boundaries of the face of the enclosure that the projector projects on.

3) The camera is mounted on an adjustable stand with all six degrees of freedom. It is adjusted facing the projection. I am unsure about this part-The camera is placed in the same vertical line as the lens of projector.  The camera is placed above the projector at a distance of about 15 cm. This would be the baseline b for the triangulation formula.

4) The lens of the camera is adjusted until the image observed in the camera is sharp and the entire object and some part of the projected image is made to appear in the frame of the camera.

5) The camera is calibrated using a checkerboard in ROS. The camera intrinsics matrix is generated and thus the focal length f required for the triangulation formula is obtained.

Is this setup okay for the experiment? Have I made a mistake at some stage. I am new to computer vision and may have overlooked some aspects. Thanks.

答:
Sounds good to me but there are some missing stages.

camera to the projector extrinsic calibration

projector intrinsic calibration
-> don't need this stage if you are not interested in the accuracy of your result.

Also, you need to make sure to calibrate camera after adjusting focus. Extrinsic as well if you have made a change in camera-to-projector distance or angle.


问:
Image data fusion
I have three cameras separated by some distance, looking at a scene. I have calibrated them using checkerboard and I have their respective intrinsic parameters and their pose and rotation matrix, I am stuck as to how to do data/image fusion.

I think I have to do feature detection and feature matching and then do data fusion but I am not sure if I am on the right path.

Would be very helpful if anyone can point me to some answer or help me with it.

Or if I am wrong then what is the right method or steps to follow?

Would be really helpful even if someone can point to a resource or just guide. Thank you, good people

答:
What do you mean by data fusion

Here's some definitions I can think of:

Determine from the features in the scene the positions of the camera

Determine from the positions of the camera the geometry of the scene

Determine from the features of the scene the intrinsic parameters of the camera.

Since you say you have fully calibrated the three cameras jointly using a checkerboard, then 1 and 3 are done. That must leave 2.

Estimating positions of features, or structure from motion

To estimate the position of a feature in the world, you first define what features. All pixels? That would be called disparity mapping Just a few features? That you can do with triangulation.

Title: Image data fusion


问:
Forward Kinematics with matrices

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

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

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

Where each P matrix is:

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

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

答:
Since I'm late to the game, see this question

But you want the numerical values.

The transformation matrices you describe are .. strange.
here is a great resource on how to set up and use transformation matrices

Which matches my understanding of:

$$^jT_i=\begin{bmatrix}
^jR(\theta_i)&^jp_i\\
0&1
\end{bmatrix}$$

So, for example,

$$
^1T_2=\begin{bmatrix}
\cos(\theta_2')&-\sin(\theta_2')&\ell_1\\
\sin(\theta_2')&\cos(\theta_2')&0\\
0&0&1
\end{bmatrix}
$$

Just sub in the values for the angles and lengths to find the matrices.

The position of the point is:

$$x=\ell_1\cos\theta_1 + \ell_2\cos(\pi/2-\theta_2+\theta_1)+x_1$$
$$y=\ell_1\sin\theta_1 + \ell_2\sin(\pi/2-\theta_2+\theta_1)+y_1$$

Where $\ell$ is the length of each arm, and $\theta$ is the angles you describe.

You can see this by setting the arm to be fully outstretched along the x-axis, and noting that the position of the end point must be $(\ell_1+\ell_2,0)$. Also, $\theta_2'$ can be temporarily set to $\pi-\theta_2$ to make this arrangement correspond to the "laying along the x-axis" one for ease of derivation.

Or:

$$^{o}p_3=^oT_1\cdot ^1T_2\cdot ^2p_3 +p_1$$

read as "point three with respect to origin equals point of joint three with respect to joint 2, transformed from coordinate frame two to coordinate frame 1, then from coordinate frame 1 to original frame".

And then,

$$^2p_3=(\ell_2,0)$$

When solving the full system, remember to use homogeneous coordinates then, so you have to express the end point as

$$^2p_3=(\ell_2,0,1)$$

Title: Forward Kinematics with matrices


问:
How to prioritize target position over target angle in Inverse Kinematics?
I have succesfully gotten my inverse kinematics method working using damped least squares and it has some really good results.

My target position also includes a target angle for the end effector, which i calculate in in forward kinematics by adding the angles of the three elevation joints. The problem that occurs though is that when i set an unrealistic target angle it tries to accomplish it before actually getting to the target.

So for instance, if i tell it to go to the a point (for example (2,-2,0) as showing below) with a Z height of zero and also tell it that it needs to come in at an angle of pi/2 (facing +Z direction) it has a ton of trouble because it's not physically possible for it to do so. Animation.

But if i tell it to go to the same point and come in at a more reasonable angle of -pi/3 (facing downward towards -Z with some angle) it does a lot better because thats a physically realizable angle. Animation.

I've tested the same algorithm without the target angle (which changes the jacobian because the target angle is no longer a part of the forward kinematics) and it works great, but i want to be able to have some control over the target angle.

How do i implement logic that tells it to focus on getting to the target position first and THEN attempt to get as close to the target angle as possible without compromising the position?

答:
You have to enforce a prioritization between the two tasks at hand:

the primary task is reaching in position;

the secondary task is reaching in orientation.

Finally, you can aggregate the two tasks using the null-space projection of the Jacobian.

In formulas:
$$
\dot{\theta}=J^\#\cdot\left(x_d-x\left(\theta\right)\right)+\left(I_n-J^\#\cdot J\right)\cdot g,
$$
where:

$\theta \in \mathbf{R}^n$ is the vector of the $n$ joints;

$\dot{\theta} \in \mathbf{R}^n$ is the resulting vector of joint velocities to be sent to the manipulator's actuators;

$x\left(\theta\right)$ is the Cartesian position of the tip of the manipulator in the joint configuration given by $\theta$;

$x_d$ is the target tip position;

$J \in \mathbf{R}^{3\times n}$ is the Jacobian operator for what concerns only the position part of the kinematic map;

$J^\# \in \mathbf{R}^{n\times 3}$ is the DLS-inverse of $J$;

$I_n-J^\#\cdot J$ is the null-space operator;

$g \in \mathbf{R}^n$ is the gradient of a suitable cost function that determines how far we are from the target orientation.

In this setting, the manipulator will be instructed to reach for the target position $x_d$ while employing its redundancy (if there is any) to also comply with the secondary task without disrupting the primary task.

A very similar Q&A can be found here.

Interestingly, we came up with an assignment on this exact IK problem for a course we gave on Robotics, whose solution is given below in form of a patch:

```<code>diff --git a/src/main.cpp b/src/main.cpp
index 7bccd6a..44330c3 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -33,6 +33,35 @@ class Controller : public RFModule
     Vector encoders;
     Vector target;

+    // compute the 2D position of the tip of the manipulator
+    Vector forward_kinematics_position(const Vector &j) const
+    {
+        Vector ee(2);
+        ee[0]=link_length*(cos(j[0])+cos(j[0]+j[1])+cos(j[0]+j[1]+j[2]));
+        ee[1]=link_length*(sin(j[0])+sin(j[0]+j[1])+sin(j[0]+j[1]+j[2]));
+        return ee;
+    }
+
+    // compute the scalar orientation of the tip of the manipulator
+    double forward_kinematics_orientation(const Vector &j) const
+    {
+        return (j[0]+j[1]+j[2]);
+    }
+
+    // compute the Jacobian of the tip's position
+    Matrix jacobian_position(const Vector &j) const
+    {
+        Matrix J(2,3);
+        J(0,0)=-link_length*(sin(j[0])+sin(j[0]+j[1])+sin(j[0]+j[1]+j[2]));
+        J(0,1)=-link_length*(sin(j[0]+j[1])+sin(j[0]+j[1]+j[2]));
+        J(0,2)=-link_length*sin(j[0]+j[1]+j[2]);
+
+        J(1,0)=link_length*(cos(j[0])+cos(j[0]+j[1])+cos(j[0]+j[1]+j[2]));
+        J(1,1)=link_length*(cos(j[0]+j[1])+cos(j[0]+j[1]+j[2]));
+        J(1,2)=link_length*cos(j[0]+j[1]+j[2]);
+        return J;
+    }
+
 public:
     bool configure(ResourceFinder &rf)override
     {
@@ -45,8 +74,9 @@ public:
         // init the encoder values of the 3 joints
         encoders=zeros(3);

-        // init the target
-        target=zeros(3);
+        // init the target at the tip of the manipulator
+        target=forward_kinematics_position(encoders);
+        target.push_back(forward_kinematics_orientation(encoders));

         return true;
     }
@@ -81,10 +111,23 @@ public:
         // retrieve the current target orientation
         double phi_d=target[2];

+        // compute quantities for differential kinematics
+        Vector ee=forward_kinematics_position(encoders);
+        double phi=forward_kinematics_orientation(encoders);
+        Matrix J=jacobian_position(encoders);
+        Vector err=ee_d-ee;
+
         Vector &vel=portMotors.prepare();
-        vel=zeros(3);

-        // FILL IN THE CODE
+        // resolve the primary task
+        double k1=10.0;
+        Matrix Jsharp=J.transposed()*pinv(J*J.transposed()+k1*k1*eye(2,2));
+
+        // resolve the secondary task
+        Vector g=2.0*(phi_d-phi)*ones(3);
+
+        // aggregate tasks using null-space projection
+        vel=Jsharp*err+(eye(3,3)-Jsharp*J)*g;

         // deliver the computed velocities to the actuators
         portMotors.writeStrict();
</code>```

Body:
You have to enforce a prioritization between the two tasks at hand:

the primary task is reaching in position;

the secondary task is reaching in orientation.

Finally, you can aggregate the two tasks using the null-space projection of the Jacobian.

In formulas:
$$
\dot{\theta}=J^\#\cdot\left(x_d-x\left(\theta\right)\right)+\left(I_n-J^\#\cdot J\right)\cdot g,
$$
where:

$\theta \in \mathbf{R}^n$ is the vector of the $n$ joints;

$\dot{\theta} \in \mathbf{R}^n$ is the resulting vector of joint velocities to be sent to the manipulator's actuators;

$x\left(\theta\right)$ is the Cartesian position of the tip of the manipulator in the joint configuration given by $\theta$;

$x_d$ is the target tip position;

$J \in \mathbf{R}^{3\times n}$ is the Jacobian operator for what concerns only the position part of the kinematic map;

$J^\# \in \mathbf{R}^{n\times 3}$ is the DLS-inverse of $J$;

$I_n-J^\#\cdot J$ is the null-space operator;

$g \in \mathbf{R}^n$ is the gradient of a suitable cost function that determines how far we are from the target orientation.

In this setting, the manipulator will be instructed to reach for the target position $x_d$ while employing its redundancy (if there is any) to also comply with the secondary task without disrupting the primary task.

A very similar Q&A can be found here.

Interestingly, we came up with an assignment on this exact IK problem for a course we gave on Robotics, whose solution is given below in form of a patch:

diff --git a/src/main.cpp b/src/main.cpp
index 7bccd6a..44330c3 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -33,6 +33,35 @@ class Controller : public RFModule
     Vector encoders;
     Vector target;

+    // compute the 2D position of the tip of the manipulator
+    Vector forward_kinematics_position(const Vector &j) const
+    {
+        Vector ee(2);
+        ee[0]=link_length*(cos(j[0])+cos(j[0]+j[1])+cos(j[0]+j[1]+j[2]));
+        ee[1]=link_length*(sin(j[0])+sin(j[0]+j[1])+sin(j[0]+j[1]+j[2]));
+        return ee;
+    }
+
+    // compute the scalar orientation of the tip of the manipulator
+    double forward_kinematics_orientation(const Vector &j) const
+    {
+        return (j[0]+j[1]+j[2]);
+    }
+
+    // compute the Jacobian of the tip's position
+    Matrix jacobian_position(const Vector &j) const
+    {
+        Matrix J(2,3);
+        J(0,0)=-link_length*(sin(j[0])+sin(j[0]+j[1])+sin(j[0]+j[1]+j[2]));
+        J(0,1)=-link_length*(sin(j[0]+j[1])+sin(j[0]+j[1]+j[2]));
+        J(0,2)=-link_length*sin(j[0]+j[1]+j[2]);
+
+        J(1,0)=link_length*(cos(j[0])+cos(j[0]+j[1])+cos(j[0]+j[1]+j[2]));
+        J(1,1)=link_length*(cos(j[0]+j[1])+cos(j[0]+j[1]+j[2]));
+        J(1,2)=link_length*cos(j[0]+j[1]+j[2]);
+        return J;
+    }
+
 public:
     bool configure(ResourceFinder &rf)override
     {
@@ -45,8 +74,9 @@ public:
         // init the encoder values of the 3 joints
         encoders=zeros(3);

-        // init the target
-        target=zeros(3);
+        // init the target at the tip of the manipulator
+        target=forward_kinematics_position(encoders);
+        target.push_back(forward_kinematics_orientation(encoders));

         return true;
     }
@@ -81,10 +111,23 @@ public:
         // retrieve the current target orientation
         double phi_d=target[2];

+        // compute quantities for differential kinematics
+        Vector ee=forward_kinematics_position(encoders);
+        double phi=forward_kinematics_orientation(encoders);
+        Matrix J=jacobian_position(encoders);
+        Vector err=ee_d-ee;
+
         Vector &vel=portMotors.prepare();
-        vel=zeros(3);

-        // FILL IN THE CODE
+        // resolve the primary task
+        double k1=10.0;
+        Matrix Jsharp=J.transposed()*pinv(J*J.transposed()+k1*k1*eye(2,2));
+
+        // resolve the secondary task
+        Vector g=2.0*(phi_d-phi)*ones(3);
+
+        // aggregate tasks using null-space projection
+        vel=Jsharp*err+(eye(3,3)-Jsharp*J)*g;

         // deliver the computed velocities to the actuators
         portMotors.writeStrict();


答:
The general principle of multiplying matrices together is correct, but the matrices themselves are you use are not correct.  They appear to be transposed and for kinematics there is no "Scale" parameter.

Another resource you might find useful is this masterclass on robot arm kinematics.  There's also a master class on 3D geometry which might help with your understanding of homogeneous transformation matrices.

Title: Forward Kinematics with matrices

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

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

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

Where each P matrix is:

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

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


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

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

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

答:
If a vicon system is out of the question, then you can use a system of calibrated cameras to read markers placed on top of the robots.

Place the best cameras you can get around the environment. I use $20 web cams from amazon.com

Use OpenCV's calibration tools to calibrate the cameras.

Place markers on top of the robots

The cameras know their relative positions in the environment, so by triangulating the marker positions, they can accurately output the position and orientation of the markers.

Sort of like what these researchers did in this paper (see page 16)

However, as others have noted, if the ceiling is very low, you won't have a good line of sight to the top of the robots, and you'll have a difficult time using this method.

Title: Localizing a swarm of robots


问:
How many independent control inputs does a hexarotor have?

6, because a rigid body has six degrees of freedom

4, since it is similar to a quadrotor, except with more motors

6, because there are six motors

I was taking an online quiz, and I thought option 3 is the correct one, only to find out I got the answer wrong. I know that I could change the answer 2 times and by chance I finally got it right, but I want to understand why option 3 is wrong?

答:
I have no problem with your answer #3, and it could be argued that the question is not sufficiently precise.  It depends what you consider the "inputs" to be.  At the motor control level, the hex rotor has 6 motors so there are 6 motor speeds that can be independently set.  However hexrotors often use the same kind of control handset as a quad rotor, with sticks that control motion:up/down, roll, pitch, yaw.  Considered at this level there are 4 "inputs" which are mapped to the 6 motor speeds.  This introduces constraints and the 6 motor speeds are no longer independent.

The vehicle itself is a rigid-body moving in 3D space so it has 6 degrees of freedom.  Assuming that all propellor axes are parallel, only 4 of those degrees of freedom are directly accessible, just like for a quad rotor.   Translational motion is only accessible as a consequence of roll/pitch motion.