答:
The packages you've found don't estimate scale or 3d pose because that's not really feasible using just an imu. The only way to get 3d pose from an imu is to integrate acceleration (adjusting for attitude) but all real sensors have sensor bias so the error in the integration will grow unbounded.

To account for that, another sensor needs to be used to provide correction and bias estimation. In your case, the 3d pose estimation from the visual slam could perform this role. Other options include lidar based slam or gps.

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

Body:
I am implementing a 3D pose estimation algoriothm on mobile device (Android) which has Gyro, Accelerometer and Magnetometer sensors. I have already develeoped a Visual SLAM algoirthm to estimate full 3D camera pose. I want to estimate same pose just by using these sensors.

I have seen the code for EKF based sensor fusion techniques, Attitude estiamtor, etc. But none of these give full 3D Pose. Insted these give only orientation (and not scale and translation)

Could any one suggest an open source C++ implementation (Not using ROS) for the problem?

Few links which I have already found:

https://github.com/simondlevy/TinyEKF

https://github.com/AIS-Bonn/attitude_estimator


问:
Control method of large linear actuator
I have a Large linear actuator from Hiwin, the serial number is LAS3-1-1-500-24GE. the data sheet for the LAS3 can be seen on page 18 of the following link: http://www.hiwin.com/pdf/linear_actuators.pdf .

I would like to be able to use this to generate some small sinusoidal motion in the actuator. The speed of this does not matter.

I am looking to control this using an arduino uno and an H-bridge along with a power supply. For an example of the H-bridge: https://www.amazon.co.uk/gp/product/B00M1JZ7HY/ref=ox_sc_act_title_1?smid=AONS7HEF348I5&psc=1

What would be the most convenient method of generating this sinusoidal motion? is an arduino and the linked H-bridge appropriate?

答:
The LAS3 actuator draws a current of 2.5 Amps @ 24Volts and has a DC motor, so the H-Bridge in the link can be used to control it. It is even a bit overdimensioned. You can link the Ardunio to the H Bridge. The PWM pin should be a PWM output of the Arduino and the Dir pin should be a digital output of the arduino.

Having a sinusoidal motion on the actuator is as simple as giving a pwm with a sinusoidal duty cycle (in absolute value, no negative values) form the arduino  and switching the direction output at the end of each sinus half period. You can read more about the Arduino PWM here.

It seems that your actuator is equipped with a linear optical sensor. To make a closed loop control, you need to downstep the signal voltages to the TTL  (arduino 5V) level and decode those signals in an arduino sketch. You can find more information here about decoding an encoder signal here.

ContentLicense: CC BY-SA 3.0

Title: Control method of large linear actuator


问:
How can I use the Arduino PID library to drive a robot in a straight line?
I would like to create an Arduino based robot with 2 wheels, quadrature encoders on each wheel, a H-bridge driver chip (or motor controller) and a caster.  I want to use the PID library to ensure the speed is proportional to the distance to travel.  </p>

<p>At a conceptual level, (assuming the motors do not respond identically to PWM levels) how can I implement the PID control so that it travels in a straight line and at a speed proportional to the distance left to travel? </p>

答:
Specifics

Looking at the PID Basic Example I think that you just need to instantiate two copies of PID controller, one for each wheel, encoder and pwm:

```<code>PID leftPID(&InputLeft, &OutputLeft, &SetpointLeft,2,5,1, DIRECT);
PID rightPID(&InputRight, &OutputRight, &SetpointRight,2,5,1, DIRECT);
</code>```

Then, in your loop() equivalent, you just read both encoders, pass each encoder value to the relevant PID and finally write out both PWM values.

For now SetpointLeft and SetpointRight can actually point at the same value, but defining them separately like this allows you to add the capability to turn later.

Concepts

While this may work for the basic case, whether it is enough really depends on how accurate you need your straight line to be.

Dead reckoning

Given that you have encoders on each wheel, if you run two PID loops and compare each wheels' following error, then you can potentially calculate your maximum abbe error over the distance, assuming your wheels don't slip. If that error is smaller than your requirements, then dead reckoning is all you need.

If your wheels are prone to slipping however, then you may have hidden following error that your control system can't detect and you will need some way to detect a slip or calculate position independently of your wheel encoders and then use higher level software to correct demanded wheel positions/velocities to maintain a straight line.

Relative position determination

As John suggests, you may be able to use an accelerometer to determine position, but given their accuracy and the effect of accumulated errors over time, you may be better off using the accelerometer data to detect and correct for wheel slippage.

In mobile robotics, Kalman filtering techniques are commonly used to fuse the data from multiple sources, such as an accelerometer and the wheel encoders to better determine current position.

Whatever you do with relative position determination however, over time the position you believe yourself to be in will drift away from your actual physical position.

Absolute position determination

The only way to get around this is to have a reference point outside of your vehicles frame of reference.

A roomba for instance generally uses dead reckoning for moving around a room, but whenever it needs to dock, it looks for a beam of infra-red light sent out by the charging dock. As the roomba randomly moves through that beam, it detects it, locks onto the beam and follows it back to its source. In combination with it's bump sensors, it can position itself accurately on the charging contacts.

For your robot, it might have a home position, where it can move back to and detect that it is at that known location. At that point it knows exactly where it is and can report how far it's calculated position is from it's actual position.

Another option, if you need your robot to travel in a straight line over hundreds of meters would be to switch to a different technique, such as adding an Arduino GPS shield.

A combination of techniques

Ultimately, depending on your accuracy requirements, you may need to use a combination of these techniques.

If a guide beam is possible, you may be able to do what you want very simply with an invisible line following technique. If you need to move in any arbitrary straight line in an constrained area, then like the roomba you may be able to use a pair of guide beams (at right angles to each other) to allow you to correct your synthesised position in one Cartesian axis every time the robot passes one of the beams.

There are lots of options here, and what you chose will depend on what you need.

OwnerDisplayName: Robert

Body:
Specifics

Looking at the PID Basic Example I think that you just need to instantiate two copies of PID controller, one for each wheel, encoder and pwm:

PID leftPID(&InputLeft, &OutputLeft, &SetpointLeft,2,5,1, DIRECT);
PID rightPID(&InputRight, &OutputRight, &SetpointRight,2,5,1, DIRECT);

Then, in your loop() equivalent, you just read both encoders, pass each encoder value to the relevant PID and finally write out both PWM values.

For now SetpointLeft and SetpointRight can actually point at the same value, but defining them separately like this allows you to add the capability to turn later.

Concepts

While this may work for the basic case, whether it is enough really depends on how accurate you need your straight line to be.

Dead reckoning

Given that you have encoders on each wheel, if you run two PID loops and compare each wheels' following error, then you can potentially calculate your maximum abbe error over the distance, assuming your wheels don't slip. If that error is smaller than your requirements, then dead reckoning is all you need.

If your wheels are prone to slipping however, then you may have hidden following error that your control system can't detect and you will need some way to detect a slip or calculate position independently of your wheel encoders and then use higher level software to correct demanded wheel positions/velocities to maintain a straight line.

Relative position determination

As John suggests, you may be able to use an accelerometer to determine position, but given their accuracy and the effect of accumulated errors over time, you may be better off using the accelerometer data to detect and correct for wheel slippage.

In mobile robotics, Kalman filtering techniques are commonly used to fuse the data from multiple sources, such as an accelerometer and the wheel encoders to better determine current position.

Whatever you do with relative position determination however, over time the position you believe yourself to be in will drift away from your actual physical position.

Absolute position determination

The only way to get around this is to have a reference point outside of your vehicles frame of reference.

A roomba for instance generally uses dead reckoning for moving around a room, but whenever it needs to dock, it looks for a beam of infra-red light sent out by the charging dock. As the roomba randomly moves through that beam, it detects it, locks onto the beam and follows it back to its source. In combination with it's bump sensors, it can position itself accurately on the charging contacts.

For your robot, it might have a home position, where it can move back to and detect that it is at that known location. At that point it knows exactly where it is and can report how far it's calculated position is from it's actual position.

Another option, if you need your robot to travel in a straight line over hundreds of meters would be to switch to a different technique, such as adding an Arduino GPS shield.

A combination of techniques

Ultimately, depending on your accuracy requirements, you may need to use a combination of these techniques.

If a guide beam is possible, you may be able to do what you want very simply with an invisible line following technique. If you need to move in any arbitrary straight line in an constrained area, then like the roomba you may be able to use a pair of guide beams (at right angles to each other) to allow you to correct your synthesised position in one Cartesian axis every time the robot passes one of the beams.

There are lots of options here, and what you chose will depend on what you need.

OwnerDisplayName: Mark Booth


问:
Image Based Visual Servoing algorithm in MATLAB
I was trying to implement the IBVS algorithm (the one explained in the Introduction here) in MATLAB myself, but I am facing the following problem : The algorithm seems to work only for the cases that the camera does not have to change its orientation in respect to the world frame.For example, if I just try to make one vertex of the initial (almost) square go closer to its opposite vertex, the algorithm does not work, as can be seen in the following image

The red x are the desired projections, the blue circles are the initial ones and the green ones are the ones I get from my algorithm.

Also the errors are not exponentially dereasing as they should.

What am I doing wrong? I am attaching my MATLAB code which is fully runable. If anyone could take a look, I would be really grateful. I took out the code that was performing the plotting. I hope it is more readable now. Visual servoing has to be performed with at least 4 target points, because else the problem has no unique solution. If you are willing to help, I would suggest you take a look at the calc_Rotation_matrix() function to check that the rotation matrix is properly calculated, then verify that the line ds = vc; in euler_ode is correct. The camera orientation is expressed in Euler angles according to this convention. Finally, one could check if the interaction matrix L is properly calculated.

```<code>function VisualServo()

    global A3D B3D C3D D3D A B C D Ad Bd Cd Dd

    %coordinates of the 4 points wrt camera frame
    A3D = [-0.2633;0.27547;0.8956];
    B3D = [0.2863;-0.2749;0.8937];
    C3D = [-0.2637;-0.2746;0.8977];
    D3D = [0.2866;0.2751;0.8916];

    %initial projections (computed here only to show their relation with the desired ones)
    A=A3D(1:2)/A3D(3);
    B=B3D(1:2)/B3D(3);
    C=C3D(1:2)/C3D(3);
    D=D3D(1:2)/D3D(3);

    %initial camera position and orientation
    %orientation is expressed in Euler angles (X-Y-Z around the inertial frame
    %of reference)
    cam=[0;0;0;0;0;0];

    %desired projections
    Ad=A+[0.1;0];
    Bd=B;
    Cd=C+[0.1;0];
    Dd=D;

    t0 = 0;
    tf = 50;

    s0 = cam;

    %time step
    dt=0.01;
    t = euler_ode(t0, tf, dt, s0);

end

function ts = euler_ode(t0,tf,dt,s0)

    global A3D B3D C3D D3D Ad Bd Cd Dd

    s = s0;
    ts=[];
    for t=t0:dt:tf
        ts(end+1)=t;
        cam = s;

        % rotation matrix R_WCS_CCS
        R = calc_Rotation_matrix(cam(4),cam(5),cam(6));
        r = cam(1:3);

        % 3D coordinates of the 4 points wrt the NEW camera frame
        A3D_cam = R'*(A3D-r);
        B3D_cam = R'*(B3D-r);
        C3D_cam = R'*(C3D-r);
        D3D_cam = R'*(D3D-r);

        % NEW projections
        A=A3D_cam(1:2)/A3D_cam(3);
        B=B3D_cam(1:2)/B3D_cam(3);
        C=C3D_cam(1:2)/C3D_cam(3);
        D=D3D_cam(1:2)/D3D_cam(3);

        % computing the L matrices
        L1 = L_matrix(A(1),A(2),A3D_cam(3));
        L2 = L_matrix(B(1),B(2),B3D_cam(3));
        L3 = L_matrix(C(1),C(2),C3D_cam(3));
        L4 = L_matrix(D(1),D(2),D3D_cam(3));
        L = [L1;L2;L3;L4];

        %updating the projection errors
        e = [A-Ad;B-Bd;C-Cd;D-Dd];

        %compute camera velocity
        vc = -0.5*pinv(L)*e;

        %change of the camera position and orientation
        ds = vc;

        %update camera position and orientation
        s = s + ds*dt;

    end
    ts(end+1)=tf+dt;
end

function R = calc_Rotation_matrix(theta_x, theta_y, theta_z)

    Rx = [1 0 0; 0 cos(theta_x) -sin(theta_x); 0 sin(theta_x) cos(theta_x)];
    Ry = [cos(theta_y) 0 sin(theta_y); 0 1 0; -sin(theta_y) 0 cos(theta_y)];
    Rz = [cos(theta_z) -sin(theta_z) 0; sin(theta_z) cos(theta_z) 0; 0 0 1];

    R = Rx*Ry*Rz;

end

function L = L_matrix(x,y,z)

    L = [-1/z,0,x/z,x*y,-(1+x^2),y;
       0,-1/z,y/z,1+y^2,-x*y,-x];
end
</code>```

Cases that work:

```<code>A2=2*A;
B2=2*B;
C2=2*C;
D2=2*D;

A2=A+1;
B2=B+1;
C2=C+1;
D2=D+1;

A2=2*A+1;
B2=2*B+1;
C2=2*C+1;
D2=2*D+1;
</code>```

Cases that do NOT work:
Rotation by 90 degrees and zoom out (zoom out alone works, but I am doing it here for better visualization)

```<code>A2=2*D;
B2=2*C;
C2=2*A;
D2=2*B;
</code>```

答:
There is a mistake in calculating ds; it should be:
 ds = L*vc instead of ds = vc</p>


问:
How to create push and pull mechanism using Servo
How do I create a push and pull mechanism using a standard hobby servo? Eg. SG-5010

Preferably without the need of 3D printing.

答:
Really simply - suppose you want to push-pull a switch.

You fix the servo and the switch (or any other manipulateable object) to some base.

Then set the servo to center position, put the plastic cross/handle/other connecting element of servo in place such way, that its end point is in right angle to the switch (and fix it with a little screw to servo). The switch should be oriented such way, that it moves toward/from the end point.

Then connect the switch and the endpoint with some hard rod (you can use wood or some piece of hard plastics for this. Cut the right shape/lenght with sharp knive).

Then you loosely screw the rod to the endpoint of servo on one side and the switch (moved to central position) on other side.

then if you rotate the servo toward the switch, it is pushed away by the rod, if you rotate the servo on the othe side, the switch is pulled back.

(It is wonderfull how many mechanical pieces you can do fast with simple piece of wood and sharp knife. Much faster, that to print them in 3D (design included). I do a lot of parts from wood, usually I do not need to re-make them later, when the machine is finished, from any "better material", as the wood is good enought.)

ContentLicense: CC BY-SA 4.0


问:
Wheel Odometry Covariance Matrix for Custom Robot
I have been looking around in the forums, but could not find any answer for how to go about getting the wheel odometry covariance matrix for a custom built planar robot (I found some posts related to EKF, but could not find any clear solution). I need this in order to for e.g. fuse wheel odometry with other types of odometry, etc... I really would only need the covariance for the global planar velocities (which are directly related to the encoders/inputs), and determining the position or acceleration covariances seem to be able to be derived from there.

I will try posting the answers to this as I find them, and perhaps this could also help future roboticists (hobbysts!?) that want to build a new mobile robot but may be confused about this.

Let's say I have a custom planar robot with N number of motors/wheels/encoders, and a defined kinematic model.

That is, I have a mapping:

(Vx, Vy, AngVel) -> ( W1, W2, ..., Wn)

where W's are each motor's angular velocities. I am not sure if the inverse mapping always exists, but I could assume that it does just for now.

By reading around the forums, I found that first we should calibrate for the systematic errors (e.g., due to unequal wheel diameters, etc..). For differential wheeled robots, this can be done with the UMBMark algorithm. This still does not give any specific information on how to get the covariance matrix tho.

I imagine there are two options, using a static covariance matrix (predetermined by calibration), or dynamically adjusting them (let's say through a Kalman filter).

A static covariance matrix is probably less accurate, but simpler to determine. However, I have no idea how to go about choosing these values (should I make the robot move back and forth several times and use the error as the Vx variance?). Are there any basic guidelines for filling up the covariance matrix statically?

Another (more difficult) option seems to be to use a Kalman filter, and update the covariance matrices dynamically. But I am unsure what to choose as the inputs, nor white gaussian noise values for process/observation.

Imagine there's some sort of local controller that I just give desired angular velocities and it tries to produce them. Should I go as low level as defining my inputs as the currents, and then go through the motor model? Or should I just choose the inputs to be my commanded angular velocities?

But if my inputs are the desired angular velocities, then the state doesn't seem to depend on the previous state, and doesn't follow the kalman filter convention for the process (i.e. the new state would only depend on the input, as I am controlling the planar velocities directly from the wheels, and it is not affected by the previous state)!?
At least the sensor model seems to be quite easy to derive from the kinematic model.

As you can probably see, I am extremely confused for something that most likely has to be determined for most mobile robots out there. I am finding little to no clear documentation, which is weird for such a (very common!?) problem. If anyone could point me in the right direction I'd be extremely happy!

Thanks!!

答:
Josh, thank you for your help!

From my understanding it seems that a basic process for determining the velocity covariance matrices of a 3D (X,Y, Theta) robot with N wheels would then be (please correct me if I am wrong!):

Define the state of the robot. For example, this could be (x Velocity $\dot{x}$, y velocity $\dot{y}$, theta velocity $\dot{\theta}$).

Define a mapping function $x=f_x(u), y=f_y(u), \theta=f_{\theta}(u)$ of the control inputs $u$ to the state variables. This would depend on the kinematic model of the robot. For example, the control inputs could be the wheel velocities $w_1, w_2, ..., w_n$ for $N$ wheels.

Place the robot at a reference position on the floor. Give control inputs to move it in the X direction (for e.g. 0.5 m/s), measure the real velocity the robot moved in X (e.g. 0.45 m/s). Repeat this several times for the forward direction, and obtain the standard deviation in x velocity based on statistical measurements. Do the same process for y velocity and Theta velocity.
This constant covariance matrix we will call Q:

$$
Q = \begin{bmatrix}
\sigma_{\dot{x}}^2 &0  &0 \\
 0&  \sigma_{\dot{y}}^2&0 \\
 0&  0& \sigma_{\dot{\theta}^2}
\end{bmatrix}
$$

Compute the transition matrix $F$, which is constructed from the jacobian of the state function $f$ with respect to the states. Refer to Wikipedia for the matrix equations.

Define the sensor model. This maps the state variables to sensor values. For wheeled robots with $N$ encoders $p_1, p_2, ..., p_N$, this would be a map of the form $p_1 = h_1(\dot{x},\dot{y},\dot{\theta});p_2 = h_2(\dot{x},\dot{y},\dot{\theta});... $.

Define the sensor error covariance matrix $R$. A wheel can be turned to a specific value, and the real measurements compared with the encoder outputs. When done several times, its statistics can be determined (similar to above):

$$
R = \begin{bmatrix}
\sigma_{p_1}^2 &0  &0 \\
 0&  \sigma_{p_2}^2&0 \\
 0&  0& ...
\end{bmatrix}
$$

Compute the matrix $H$, as the jacobian of the sensor model function $h$ to the state variables.

Now you can start the kalman filter loop. Choose matrix $P_0$ as an initial guess (you can choose all 0 for the beginning). Follow the equations in Wikipedia and solve for the state and odometry covariance matrix $P$ at every frame! This result (state, covariance) can be read by for example, the robot_localization package in ROS to fuse several sets of data and get a better odometry approximation!

P.S. I am not really sure if this is correct, but I hope someone can tell me if it is (or any mistakes that I may have done along the way!)!


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

答: This site about robotics walks you through how you can define the link inertias in the local frame, then use transformations to come up with $M$.  It is pretty straightforward.  Just remember to define link parameters locally, then premultipy by the appropriate transformation. </p>


问:
Real-time TCP position/pose control on a robotic arm such as YuMi: recommendation to calculate IK/Inverse Jacobian?
I plan to make a mouse or a gesture control robot like this video on YouTube : ABB Externally guided motion.

For a 6-axes robot, I could implement it by using ABB’s EGM (Externally Guided Motion) option, which allows to send a Cartesian position and pose of TCP, and all the tedious calculation was handled by its controller.

However, when I started to work on YuMi, I noticed that EGM’s position guide control cannot be applied to the 7-axes robot (For YuMi, a joint control mode in EGM is only available). Are there any recommendations to implement what I described above?

Also, I'm guessing that I need to implement IK class to get the correct joints' angles from a desirable TCP position. Maybe using OpenRAVE or OMPL? If you have any recommendation to calculate IK / Inverse Jacobian, please let me know too.

答:
You have to differ between motion planning and control approaches:
What you want is control (no fancy planning algorithms in the loop).

Regarding the IK problem, this is the lib that you want to use:
https://bitbucket.org/traclabs/trac_ik
It is amazingly fast and super easy to use (given you have a URDF description of your robot). And it combines different IK approaches that run concurrently under the hood.
It is also available as a ROS package.

BUT: You have to take care about pose changes! So with a six / seven axis robot you can reach TCP poses with a variety of joint configurations. And if you just query the IK algorithm, it could spit out altering configurations, which would mean huge motions for your robot.
So thats why TracIK must be initialized with the current joint angles, so that it can chose a solution that is similar to the current one.

On top I would suggest a sanity check (accumulated joint distance threshold), so that you do not execute jumps, if Trac delivers a solution that implies a configuration change.


问:
Different methods to determine DOF: Chebychev-Kutzbach-Grubler method vs. Screw method
I'm familiar with Chebychev-Kutzbach-Grubler method to determine degree of freedom of a robot arm. But it seems this method fails to calculate the mobility of some parallel robots, as explained here.

However I cannot understand screw theory well and I do not know how to apply it to determine DOF.
So I wanna know what is the idea behind screw method to determine DOF?
And could anyone explain with a simple example how Screw method works?

EDIT:

Could you please explain how we can determine total DOF of ,for example, SCARA Arm via screw method?

答:
The most popular introductory text for screw theory on robotics is "A mathematical introduction to robotic manipulation" which can be found here for free.  A more mathematically advanced text (perhaps too advanced) is "Geometric Fundamentals of robotics" but it is not free. Another text discussing screw theory is "Rigid Body Dynamics algorithms" by Roy Featherstone. You might want to check his website, he has some introductory material before he goes to the full-blown dynamics.

I am probably oversimplifying things, but a screw is a 6D vector that has the linear and the rotational velocity of movement and is accompanied by a wrench which is a 6D vector with the linear forces and the torques. They might look like they are two 3D vectors stacked together, and is some cases behave like that, but they have their own vector properties.

The paper you are referencing should make more sense once you read Chapter 2 and 3 of the book I am linking.

Hope this will give you a good starting point.


问:
Use of universal humanoid robots for lean and agile manufacturing and services?
How robot industry perceives the idea about the use of universal humanoid robots for the agile, rapidly reconfigurable manufacturing and services?

Are there examples of such use? E.g. are there examples of use of Nao robots or similar robots in food industriy (where manual work is required) and in hotel services?

And do the developers of humanoid robots take into account the potential use of their products in the manufacturing and services?

Aparently the manufacturing worflow rapidly evolves and universal, multi-functional robots can be especially suitable for such use.

答:
There is the problem, that human-like robots are not yet able to move as reliably as humans. Not far ago was there content for robots to get out of car, walk to doors, open the handle and walk inside - a lot of participants failed miserably at such "simple" work.

There is so much possibilities for movements, lot of inertia, not well defined ground and not perfect visual recognition in real time.

On the other hand machine-like robots can move on rails, have perfect information about its position, only few axes, all programmed for optimal movement and stable envirnment. They are able to do limited number of action much faster and with a lot higher precision, than humans can.

So for ongoing couple of years the non-human robots would be used more, as it is much simple to get much better result from them.

The only field, where human looking robots can be better is interraction with people, for psychological reasons. But while from X-Y-Z laser you expect getting some king of high-tech program and then cut material fast and effectively, doing many times the same movements, form human looking robot you would expect human-like behavior, mainly ability to recognize gestures and face-expression, native language and vague defined commands like "I would like something sweet, but without too much suggar and rather mildly flavoured as I am on the diet" - which is a too much more difficult task to respond as expected from human like inteligent being.

And the reconfigurability is higher if you can just change some robotic arm ot drilling tool, in contrast to difficulty to teach effectively manage tool designed for human hands (which cannot be hold as firm and in perfect position, as something, where you screw two linear plates together to make standardized connection and wire the motor to the driver, instead of pressing some oddly shaped switch with hand full of servos.


问:
Symbolic Representaion of Links and Joints: How to sketch complex manipulators using simple symbols?
In some papers and books we can see that authors using symbols to represent robot arms. My question is, is there a convention for such sketches? If so could you provide a reference which shows how these symbols should be used?

答:
There are usually only two kinds of joints you would use in a robot - a revolute (rotational) joint and a prismatic (translational) joint. Every reference I've seen uses a cylinder to represent revolute joints and a cube or prism to represent a prismatic joint.

You might be able to argue that a cylinder should represent a cylindrical joint, but in reality a cylindrical joint is a combination revolute and prismatic joint.

There are spherical (ball) joints, but I have never seen an actuated spherical joint before. They're more used as a kinematic constraint in shoulder joints or suspension design, in which cases the joint is a pivot and the actuation is still done by prismatic joints.


问:
Determining the speed of each pole for a transfer function

I have a transfer function and i want to calculate the speed of each pole and i do not know how to do that

答:
What are you doing that you're being asked to find the speed of a pole? If this is a course (online or actual), then I would expect the material would have been covered already. Any time I've heard of "speed" mention with regards to a pole, it's how "fast" or "slow" the pole is, which means how far or close the pole is to the imaginary axis (respectively). A pole that is very close to the imaginary axis will take a long time to settle; one that is very far from the imaginary axis will settle very quickly.

This is how you get into the "dominant pole(s)" or "dominant response" - the "faster" dynamics (poles) settle quickly and the "slower" dynamics (poles) are left, and so at the medium- to long-term, the only observable dynamics are those of the "slower" poles.

I remember learning to compare pole speed by looking at their natural frequencies, but in researching for your question I've seen several posts stating it's the distance from the imaginary axis (-$\xi \omega_n$), which I believe makes more sense than just the natural frequency ($\omega_n$).

From this site

In this system, as the poles begin to move from the open loop poles, the "slow" pole (It doesn't move slower, but because it is closer to the imaginary axis the response due to that pole is slower.) at s = -1 initially moves to the left, indicating a faster response in that component of the response.  The "fast" pole starts from s = -2, and move to the right, indicating that that portion of the response slows down.  Still, the overall effect will be a speedup in the system, at least until the poles become complex.  You can see that in this clip that shows the unit step response of the system with the root locus above.

Here is a PDF on control engineering that makes numerous references to how fast the system responds with respect to damping ratios and natural frequencies.

So again, I would imagine you didn't ask yourself to do this problem, so talk to the person that gave you the problem and ask them what they mean when they say speed. If I had to guess, I would say to find $-\xi \omega_n$ for each pole.


答:
I will try not to skip too many steps. Assuming a Global coordinate frame at the base and the arm is fully extended along the Y-axis of the base frame.

Since SCARA has four joints, we will create four 6D spatial vectors (screws) ${ξ}_{i}$ with respect to a global coordinate frame. Keep in mind that all spatial vectors are described with respect to the global frame. Also ${ξ}_{i}$ will describe the axis around which rotation takes place and along which translation takes place. Basically, we assume a screw motion for every joint along its Z-axis.

The first three elements of vector ${ξ}_{1}$ (for joint 1) will the linear velocity on the screw axis, let's call it ${v}_{i}$. The last three elements are the rotation of the joint with respect to the global coordinate frame, regardless if there are other joints preceding it. Let's call it ${ω}_{i}$ and it will always be a unit vector.$${ξ}_{ι}=\left[{\begin{matrix}{v}_{i}\\{ω}_{i} \end{matrix}}\right]$$

For the first joint the axis of rotation coincides with the global Z-axis. Therefore ${ω}_{1}$ is $${ω}_{1}=\left[{\begin{matrix}0\\0\\1 \end{matrix}}\right]$$

If you are not sure how I came up with it consider the following. if the z axis of the joint was along the y axis of the global frame then the ${ω}_{1}$ would be $${ω}_{y}=\left[{\begin{matrix}0\\1\\0 \end{matrix}}\right]$$. if it was along the x-axis then it would be $${ω}_{x}=\left[{\begin{matrix}1\\0\\0 \end{matrix}}\right]$$. And of course it was along the z axis but pointing downwards then: $${ω}_{-z}=\left[{\begin{matrix}0\\0\\-1 \end{matrix}}\right]$$.

Now let's focus on ${v}_{1}$. We need a point ${q}_{1}$ that will give us the distance of the joint from the origin. The distance is L1 along the z-axis, therefore: $${q}_{1}=\left[{\begin{matrix}0\\0\\L1 \end{matrix}}\right]$$

We rotate ${q}_{1}$ with the unit angular velocity ${ω}_{1}$ (cross product) and we have $${v}_{1}=\left[-{ω}_{1} \times {q}_{1} \right] =- \left[{\begin{matrix}0\\0\\1 \end{matrix}}\right] \times \left[{\begin{matrix}0\\0\\L1 \end{matrix}}\right] = \left[{\begin{matrix}0\\0\\0 \end{matrix}}\right] $$

Therefore $${ξ}_{1}=\left[{\begin{matrix}{v}_{1}\\{ω}_{1} \end{matrix}}\right]=\left[{\begin{matrix}0\\0\\0\\0\\0\\1 \end{matrix}}\right]$$.

Simillarly we have $${ξ}_{2}=\left[{\begin{matrix}{v}_{2}\\{ω}_{2} \end{matrix}}\right]=\left[{\begin{matrix}L2\\0\\0\\0\\0\\1 \end{matrix}}\right]$$

For the third joint the ${ω}_{3}$ is 0 because it is prismatic and ${v}_{3}$ is the unit vector $${v}_{3}=\left[{\begin{matrix}0\\0\\1 \end{matrix}}\right]$$ making ${ξ}_{3}$ $${ξ}_{3}=\left[{\begin{matrix}{v}_{3}\\0 \end{matrix}}\right]=\left[{\begin{matrix}0\\0\\1\\0\\0\\0 \end{matrix}}\right]$$.

For joint 4 $${v}_{4}=-\left[{\begin{matrix} 0\\0\\1\end{matrix}}\right] \times \left[{\begin{matrix}0\\L2+L3\\L1-d1\end{matrix}}\right] = \left[{\begin{matrix}0\\L2+L3\\0\end{matrix}}\right]$$

$${ξ}_{4}=\left[{\begin{matrix}0\\L2+L3\\0\\0\\0\\1\end{matrix}}\right]$$

Now the spatial vectors ${ξ}_{i}$ describe the axes of rotation of all the joints. Keep in mind that some authors have $v$ and $ω$ switch places. For example the paper you are mentioning. This is fine as long as you are consistent throughout.

This example is being worked out in "A mathematical introduction to robotic manipulation" I have linked in my previous answer on page 88.

These are the basics of the screw theory description. Similarly we can create a wrench 6D vector φ with the linear and the angular (torques) forces. As you can tell by now, screw theory is a method to describe the manipulator (like the DH parameters) and not a mobility formula like the Chebychev-Kutzbach-Grubler method. The paper you are referencing stacks the spatial vectors (it calls them twists) ${ξ}_{i}$ and the wrenches ${φ}{i}$ to create a twist matrix and then analyze their constraint using the union and the reciprocal of the twists. Those are vector functions that the screw theory can take advantage of.

I am not sure if my answer helps you but the paper you are mentioning requires a relatively strong understanding of screw theory and vector algebra to make sense.

Please let me know if you need more clarifications.

Cheers.

Title: Different methods to determine DOF: Chebychev-Kutzbach-Grubler method vs. Screw method

Body:
I'm familiar with Chebychev-Kutzbach-Grubler method to determine degree of freedom of a robot arm. But it seems this method fails to calculate the mobility of some parallel robots, as explained here.

However I cannot understand screw theory well and I do not know how to apply it to determine DOF.
So I wanna know what is the idea behind screw method to determine DOF?
And could anyone explain with a simple example how Screw method works?

EDIT:

Could you please explain how we can determine total DOF of ,for example, SCARA Arm via screw method?


问:
Drone targeting
Imagine a "drone" and a target point on a 2d plane. Assuming the target is stationary, there are eight parameters:

```<code>P = my position
Q = target's position
V = my velocity
I = my moment of inertia
w = my angular velocity
s = my angular position
T = max thrust
U = max torque
</code>```

The drone's job is to get to the target as fast as possible, obeying max torque and max thrust. There are only two ways to apply the torque, since this is only in a 2d plane. Thrust is restricted to only go in one direction relative to the orientation of the craft, and cannot be aimed without rotating the drone. Neglect any resistance, you can just pretend it is floating around in 2d outer space. Let's say the drone checks an equation at time interval t (maybe something like every .01 seconds), plugs in the parameters, and adjusts its torque and thrust accordingly.

What should the equations for thrust and torque be?

What have we tried?

We know that the time it takes for the drone to reach the target in the x-direction has to be the same for the same time in the y-direction. There is going to have to be some integral over time in each dimension to account for the changing thrust based on total thrust, and total thrust in each direction given the changing angular position. I have no idea how to tie the torque and thrust together in a practical way where a function can just be called to give what thrust and torque should be applied over the interval t unless there is some other technique.

答:
You need to do a bit of Calculus.

First a note about your input parameters:

Actually acceleration depends on Force and mass. You don't specify what units your max. thrust is in so let's assume your max. thrust is your acceleration. We can do the same thing with your max. torque then and assume that it is also your acceleration (angular) and forget about moment of inertia. Actually you would divide torque by moment of inertia to get your angular acceleration,

So you can simplify by giving your drone two static properties:

```<code> thrust as max acceleration
 and torque as max angular acceleration
</code>```

instead of giving it four properties

```<code>Force and mass
and Torque and MoI.
</code>```

And now crunch the numbers:

Now you have your drone's Position: Px, Py, S -- it's first derivative, velocity: Vx, Vy, W -- and it's second derivative, acceleration: Ax, Ay, U

Actually Ax, Ay will be calculated from S (angular position) and T (acceleration) by:

```<code>Ax = T ×  cos(S)
Ay = T × -sin(S)
</code>```

That should get you a good way towards generating your formulas. Now you want to project a line from P that intersects Q in the least amount of time. Of course, it'll be a curve, not a straight line, so you'll have to use Calculus.

You could try asking the same question on math.stackexchange.com for specific formulas.

Title: Drone targeting

Body:
You need to do a bit of Calculus.

First a note about your input parameters:

Actually acceleration depends on Force and mass. You don't specify what units your max. thrust is in so let's assume your max. thrust is your acceleration. We can do the same thing with your max. torque then and assume that it is also your acceleration (angular) and forget about moment of inertia. Actually you would divide torque by moment of inertia to get your angular acceleration,

So you can simplify by giving your drone two static properties:

 thrust as max acceleration
 and torque as max angular acceleration

instead of giving it four properties

Force and mass
and Torque and MoI.

And now crunch the numbers:

Now you have your drone's Position: Px, Py, S -- it's first derivative, velocity: Vx, Vy, W -- and it's second derivative, acceleration: Ax, Ay, U

Actually Ax, Ay will be calculated from S (angular position) and T (acceleration) by:

Ax = T ×  cos(S)
Ay = T × -sin(S)

That should get you a good way towards generating your formulas. Now you want to project a line from P that intersects Q in the least amount of time. Of course, it'll be a curve, not a straight line, so you'll have to use Calculus.

You could try asking the same question on math.stackexchange.com for specific formulas.


问:
Will this have more or less lifting strength
Bit of an engineering question. If a snake robot is 100 cm long, and every 10 cm is another motor. Would the robot lift more weight, the longer/more motors attached to it.. or is lifting ability solely based on a single motor lifting.</p>

答:
Every action has an equal and opposite reaction. Each motor/joint in a linear chain of actuators (snake) needs to be capable of supplying the appropriate reaction forces.

This mean that, if you have a 100cm long snake robot with a motor every 10cm, the first/"neck" joint (at 10cm) needs to support 8 other motors and 90cm of snake body. The second joint needs to support 7 other motors and 80cm of snake. This chain continues.

So no, making it longer and/or adding more motors doesn't really help anything because you'll need to make the existing motors bigger to be able to handle extra length and new motor(s). The new motors don't "help" because they provide a reaction force/torque which just gets passed off to the next actuator.

The existing actuators wind up still "seeing" all the load forces that the new motors are trying to interact with, but now they have to carry the bonus motors in addition to the load.


问:
Can you detect if encoder wire is connected on a motor?
I am writing some logic for a PID controlled catapult (In order to improve precision). That is all fine and well. However, if, for some reason, the encoder wire disconnects, the motor spins continuously in the opposite direction, which breaks my catapult. To solve this, I would like to write a function to catch the failure of the wire, and use that to switch the runmode to not using the encoder. I have both control functions working properly. My issue is the transition. How can I detect when an encoder disconnects.

Note:
I have thought about writing a function to checks the return value of the encoder to see if it is disconnected, but I no not know what is returned by the getPosition function when the encoder is disconnected. Is it 0, is it null, or is it something else entirely?

答:
If you have a situation where your motor can break your system, for a small DC motor then rather than relying on a solution which requires the software to detect the condition, instead add limit switches to prevent it running beyond a point by inserting a diode in series with the motor when the switch is activated. For BLDC motors or more powerful motors then have the limit switches feed into the controller instead. If the limit switches are not in series with the motor, then use normally closed ones so a disconnection also trips the fault condition.

If it is a open collector optical encoder, then you will already be using pull-up resistors and still get consistent values when fully disconnected, so detect the case that the motor is running for more than a given timeout with the encoder state not changing. An intermittent disconnection is harder to detect as it may produce valid sequences of pulses ( you don't state whether it's a single or dual pulse encoder; a dual one will have invalid sequences that can be detected), in which case limit switches would again be a reliable answer.

If you don't have a suitable mechanism to use a mechanical limit switch, then optical detectors can be used instead.


问:
How to work with arduino softwareserial communication?
This is my first project using Bluetooth module HC-05. I am using two of these modules. One is connected to an Arduino Nano(slave) and another one is connected to an Arduino Uno(master). I have paired them through AT commands.

For testing, I was giving a pulse in pin 7 on the Nano. If there is a pulse, then slave will send character "1". If not, then it will send "0".

In the Uno there is an LED connected to pin 13. If the master receives '1', then the LED will on and if receives '0', the LED will remain off.

here is my source code ----------

slave code=>

master code=>

The Arduino IDE isn't showing any bug, but the code is not working at all. Although I am giving a pulse in pin 7 of the Nano, the LED is remaining off in the Uno. I am at a loss now. I have a lot to do after this and my project submission is knocking at the door. Please help me as soon as possible.

答:
I had connected some pins the wrong way.

I had connected the "EN" pin with 5V even after pairing. I was out of my brain. As soon as it came to my sight I disconnected the "EN" pin from 5V. After a few seconds it started to work....


问:
How can I turn a bowl/glass upside down?
I want to dispense water/cut-vegetables from a glass/bowl(240 ml) by turning it upside down. The screencast shows the idea:

I can directly mount the clamp on a servo motor, but I think that will be a lot put a lot of downward force on the shaft. What will be a good mechanical arrangement to do this? Thanks!

Ref:

Servo MG996R with Metal Gears

Cup:

答:
You can take the shear force onto a bearing housing. And the servo can only deal with the rotation of the bowl.

To do so, you need a bearing which carries the mass of the load (bowl). And through this bearing a shaft is attached onto the bowl. And the servo drives this shaft.

Two bearings at each side would make the system more stable, and possibly decrease any extra friction due to the bending moments.


问:
Quadrotor control using ArduIMU
We are using ArduIMU (V3) as our Quadrotor's inertial measurement unit. (we have a separate board to control all motors, not with ArduIMU itself).

As mentioned here , the output rate of this module is only at about 8hz.

Isn't it super slow to control a quadrotor ? I'm asking because as mentioned in this answer a quadrotor needs at least 200hz of control frequency to easily stay in one spot, and our ESCs is configured to work with 450hz of refresh rate. Any working PID controller I saw before for Quadrotors used at least 200-400hz of control frequency.

I asked similar question before from Ahmad Byagowi (one of the developers of ArduIMU ) and he answered:

The arduimu calculates the dcm matrices and that makes it so slow. If
  you disable the dcm output, you can get up to 100 hz gyro, acc and so
  on.

So, what will happen if I disable DCM from the firmware ? Is it really important ? We did a simulation before and our PID controller works pretty well without DCM.

答:
I updated ArduIMU's firmware and successfully got 100hz of output without disabling normalization.

Update:

Thanks to Kalman filter firmware of ArduIMU I got up to 180hz of output plus removing all noises.

Title: Quadrotor control using ArduIMU


问:
Converting Image Coordinates to (x, y) Position for Robotic Arm
I have a Robotic Arm with a camera mounted above it looking down at a slight angle.

Assuming I know the height of the camera, the angle of tilt and the small distance from the center of the robot which is considered (0, 0) what else would I need to convert the image coordinates to the distance from the center of the robotic arm?

I am also assuming all the objects will have a z=0 because they will be sitting on the same platform as the arm.

I have the inverse kinematics worked out to control the arm, I just need to give it coordinates to move to.

If it helps I am using Python and OpenCV.

Edit: Clarification

答:
A quite simple math is involved in this task, but you need to be aware of the pinhole camera model as well as of the homographic projection.

The pinhole camera model gives you the 3D position $(x,y,z)$ of a point in the space whose projection in the image plane corresponds to the $(u,v)$ pixel at a distance $\lambda$ from the image plane (considered infinite):

$$
\left( \begin{array}{c} x \\ y \\ z \\ 1 \end{array} \right) = \Pi^\dagger \cdot \left( \begin{array}{c} \lambda \cdot u \\ \lambda \cdot v \\ \lambda \end{array} \right),
$$

where $\Pi^\dagger$ is the pseudoinverse of the matrix $\Pi \in \mathbb{R}^{3 \times 4}$ that incorporates both the intrinsic parameters of your camera (i.e. the focal length, the pixel ratio and the position of the principal point) and the extrinsic parameters accounting for the position of the camera reference frame expressed in the world frame (i.e. a $\mathbb{R}^{4 \times 4}$ roto-translation matrix).

You have now to use the concept of homography, because you know that the object of interest lies on the plane $z=0$.

To do so, let's define the following quantities:

$\mathbf{p}_0$, a point in the plane $z=0$; in particular, $\mathbf{p}_0=(0,0,0)$ is fine.

$\mathbf{z}$, the normal to the plane $z=0$, that is $\mathbf{z}=(0,0,1)$.

$\mathbf{c}$, the 3D position of the camera frame expressed in the world frame.

$\mathbf{p}_1$, a 3D point that corresponds to the pixel $(u,v)$ at a distance $\lambda=1$.

Your final point $\mathbf{p}^*$ is therefore:

$$
\mathbf{p}^*=\mathbf{c}+\frac{(\mathbf{p}_0-\mathbf{c}) \cdot \mathbf{z}}{(\mathbf{p}_1-\mathbf{c}) \cdot \mathbf{z}} (\mathbf{p}_1-\mathbf{c}).
$$

That said, the strong assumption we're making here is that the object of interest is squashed on the plane $z=0$, that is it has a null height, which is not reasonable. To compensate for that, you could include such an information in the above process and replace the plane $z=0$ with $z=h$, where $h$ represents a suitable height.


问:
DH Parameters of RRP planar manipulator
I have been learning forward kinematics and having some trouble with coordinate systems and dh parameters with prismatic joints. Trying to work through this question. Trying to work this out I ended up with this system.
 However running through matlab it appears to be wrong. If anyone is able to point out my mistake, or help point me in the right direction, would be appreciated!

答:
Frame 3 (transform from 2 to 3) does not match the parameters in column 2. DH parameters cannot describe the transform you sketched.

See if the frames below help. Remember that DH is a mathematical way to reduce the number of parameters for when you need to manipulate the equations by hand. Frames that fit the DH parameterization for a given kinematic topology don't necessarily match the frames that humans might choose.

Pro-tip. Don't use DH if you are doing numerical analysis. People (designers) tend to think of robot kinematic neutral positions along orthogonal directions. As such, writing the transform matrix directly, augmented with a parameterization along Z, is easy to do, and lets you put the link frames wherever you like without problem.

Title: DH Parameters of RRP planar manipulator


问:
Can ROS run on a Raspberry Pi?
Can ROS run on a Raspberry Pi?

ROS is resigned to run on a network of machines, with different machines, even different cores on the same machine doing different jobs. Can one of those machines be a Raspberry Pi?

I am considering using an R-Pi as the EtherCAT master on a mobile robot, communicating with the main PC over WiFi, using a dongle.

Can an R-Pi even run ROS at all?

Would an R-Pi have enough processing power to do some 1kHz servoing?

Would it be possible to run some servoing on the host through the WiFi connecion?

答:
Yes. Sure. We've been doing this long time.

ROS is not an "OS" and it's not big. It's just "middleware" which runs well in "Raspbian".

We just burn with the official Raspbian(so we hope it can make us work on a "stable" environment) and compile the ROS with source. Here's the step-by-step link(for ROS Kinetic):
http://wiki.ros.org/ROSberryPi/Installing%20ROS%20Kinetic%20on%20the%20Raspberry%20Pi

you can try ros_comm or desktop.

I tried ros_comm and then add other packages later.

Title: Can ROS run on a Raspberry Pi?


问:
How can I implement tremaux algo in arduino line follower to navigate and create map?
The Map1

The task is divided into 2 runs, In first run, We have to navigate through whole map and reach the end mark taking all checkpoints.
In second run, using the mapping from first run we have to follow the shortest path.

I am reading Algos and codes from days, I know the steps that should be taken,
Navigate the maze using Tremaux algo and turning right at each possble junction.
then Save that map in memory and use djikstra algo or A* algo to find the shortest path.
But how to implement it practically?

Also since all  the dictances and angles are known and also the wheel dia, motor rpm, bot width then Can I skip wheel encoders and use calculation instead?

答:
A good way to work once you have your map : turn your map in a weighted graph, were where the vertices are only at crosses and the weight is the length beetween the points represented by the vertices.
Then running an A* or a djikstra will be simple.</p>


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

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

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

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

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

答:
Depends a lot on what the connectors are being used for.  Do they need to be panel mounted? pcb mounted?  Just hanging out?

I see anderson power poles all over the place in robotics because they lock reliably and can be snapped together to make connectors of any size.  Normally they are used for power, not signals.

IDC connectors are also very common for signals/low power.  They also lock reliably and use standard .1 header spacing which is nice for prototyping/testing.

The locking headers you mentioned are also very common, but never where repeated disconnects/reconnects are expected.  Mostly just indicator lights and such.

In weight/space restricted applications RC connectors are great.  A lot of the RC connectors have extremely high current capacity and are very reliable under vibration.  My favourite is the XT60.

Title: What connectors are most reliable?


问:
Connecting More Than Six Analog Input Pins to arduino
I'm in the planning stages for a project using the Arduino Uno to control 8 distance sensors, and have run into a little road block, the Uno only has six input pins. So I'm wondering, is there any way for this to work? If so, how?</p>

答:
There's more than one way to do it (TMTOWTDI).

There are a several ways to connect 8 analog inputs to an Arduino.

Add an analog multiplexer, as georgebrindeiro suggested. Such as: (a), (b), (c), (d), etc.

Replace the Arduino with one that has enough analog inputs already built-in. Such as the Arduino Mini with 8 analog inputs, the Arduino Due with 12 analog inputs (b), (a) the Arduino Mega with 16 analog inputs (b), the Teensy 3.0 with 14 analog inputs (c), etc.

Add one or more external ADCs, and connect it to digital pins of your CPU. Such as: Arduino 4-20mA Shield with 16-bit ADC (a); ADS1115 16-Bit ADC -- with 4 of these boards, 2 digital pins from the Arduino are used to read 16 analog inputs (b); MCP3208 8 channel 12 bit SPI ADC (c); 8-channel 16-bit Raspi analog board (d); Arduino and the LTC2440 24bit ADC (e) (f); "The Brick" 8 Single Ended 16 Bit Analog Inputs (g); etc.

p.s.: Multiplexing analog inputs requires an "analog multiplexer" -- most multiplexers are "digital muxes" which won't work with analog inputs.

p.p.s.:
All the ultrasonic distance sensors I've used only require digital I/O pins.
What kind of distance sensors are you using?


问:
Low-cost centimeter accurate satellite positioning (GNSS/GPS)
I am looking for a cheapest possible GPS setup with a centimeter precision without much HW hacking. I am not able to produce my PCB or do any soldering (though I would do that if there is no other way) so a kind of a easy-to-assemble setup would be welcome. I know about the $900 Piksi thing but that is still too expensive for me. It seems like cm precision should be possible for much less - like employing a 50 USD raw GPS sensor with an antenna and ordinary PC with RTKLIB software.

I am not sure if it is better to use two GPS sensor setup for RTK (one base station and one for rover) or whether I can get the corrective DGPS data elsewhere (my region is Czech Republic - there seems to be national grid here allowing to stream correction data for reasonable cost).

My application will be in a passenger car so I will not be limited with power source - no low power needed although that would be nice. I will be using the position readings within OpenCV - so I need to get the data into C/C++ code. The application is data collection so I can use raw GPS post-processing.

答: Ublox sell an RTK solution which provides 2cm accuracy. The evaluation kit is €359 and it works beautifully.</p>


问:
What is the cheapest / easiest way of detecting a person?
I'd like to know if anyone has had success detecting a warm-bodied mammal (ie. Human) using standard off the shelf, inexpensive sensors?

Ideally, I'd like to use an inexpensive sensor or combination of sensors to detect a person within a room and localize that person. I would like the robot to enter a room, detect if a human(s) is/are present and then move to the detected human. The accuracy does not need to be 100%, as cost is more of a factor. I'd like the computational requirements of such a sensor to be such that it can run on an Arduino, although if it's impossible, I'd be willing to utilize something with more horespower, such as a Raspberry Pi or a BeagleBone Black. I have a few thoughts; however, none of them are ideal:

PIR Sensor - Can detect movement within a large field of vision (ie. usually 120 degrees or more).  Might be the closest thing to a "human" detector that I'm aware of; however, it requires movement and localizing/triangulating where a person is would be very difficult (impossible?) with such a large field of vision.

Ultrasound - Can detect objects with good precision.  Has a much narrower field of view; however, is unable to differentiate between a static non-living object and a human.

IR detectors - (ie. Sharp range sensors) Can again detect objects with great precision, very narrow field of view; however, it is again unable to differentiate objects.

Webcam + OpenCV - Possibly use face detection to detect human(s) in a room.  This may be the best option; however, OpenCV is computationally expensive and would require much more than an arduino to run.  Even on a Raspberry Pi, it can be slow.

Kinect - Using the feature detection capabilities of Kinect, it would be relatively easy to identify humans in an area; however, the Kinect is too expensive and I would not consider it a "cheap" solution.

Perhaps someone is aware of a inexpensive "heat-detector" tuned to body heat and/or has had success with some combination of (#1-4) above and would like to share their results?

答:
I tried using PIR but it has problems with handling and delays. It is not an efficient choice for human detection to be honest.
You can use Capacitive Sensing Technique as it is the cheapest and easiest way for human detection(A smart choice) and it is less complex too. You can make a sensor for yourself at very low cost and it is good for small projects. I have used one in my "Human Detection Robot" Project. You can watch my video at: Capacitive-Based Human Detection </p>


问:
How does rocker bogie keep the body almost flat?
How does rocker-bogie mechanism keep the body flat / keep the solar panel almost flat all the time? I know there is an differential system that connect both rocker bogie (left and right) together. But how does it actually work?

Edited: Please provide relevant references.

答:
I was looking for something similar, and I found Mars Rover Rocker-Bogie Differential to be really helpful.

With my level of understanding it took me a while. But the link my professor provided me with really helped me, it has decent animations to help understanding the concept.

Okay so here's my understanding of the mechanism.
The differential system essentially could consist of either of these two mechanisms:
The Differential Gearbox and the Differential Bar.

The differential Gearbox consists of three gears. The one in the middle (2) is connected to the body, while the ones on the side (1) rockers of the system.

If you were to pick the rocker bogie up and hold the body intact, and tilt one of the side rocker downwards, the gears in the gearbox would make the rocker on the other side tilt upward. If you tilt it upwards, the other one would tilt it downwards. More complex system use more gears to make the whole system more sensitive to the movements.

As for the Differential bar, it's not used on the rover as it interferes with the solar panel, but it works in a similar way. Except its rod that connects the two rockers. And this rod is pivoted in the middle onto the body.

So, as far as my understanding goes, look at the picture and consider the gears on either side (Gears numbered 1). If one of the gear(let's say the top one) is turning clockwise, it would make Gear no. 2 (which sits in the grove of both of the other gears.) move anti-clockwise. Which in turn would make the other No.1 Gear turn anti-clock wise.

NOTE: ALL THE GEARS FIT INTO THE GROOVE OF EACH OTHER. So any rotation that takes place in this system affects the other two gears. And the gears are attached or arranged in such a way that No.1's are running in the opposite direction of each other.

Here you can see the differential Bar on Curiosity.


问:
How to compute a path from a Frame A to a Frame B
the problem I have is the following (see picture below), I am able to compute a starting frame $F_S (X_S, Y_S, Z_S)$, an ending frame $F_E (X_E, Y_E, Z_E)$ and a path from $F_S$ to $F_T$ and what I want to do it to compute a serie of transformations that will transform $F_S$ into $F_E$ along the path.

Naively I computed the Euler angles for $F_E$ end $F_S$, compute the differences and incrementally built the transformations but it does not work. Does somebody can give me some hints or pointers towards existing solutions?

The application is related to the computation of a path for an arm and so the frames are associated with the end effector.

Thank you

答: Quaternions and SLERP is what you want.</p>

Body: Quaternions and SLERP is what you want.


问:
Gyroscope - How can I remove low frequency component with a high pass filter only?
I'm using Matlab to suppress low frequency components with a high pass filter.

Objective

Filter angular velocity measurements affected by high frequency noise and bias in order to get the best estimate of the angular position.

The output when the gyroscope is still looks like this.

First Approach

The easiest way to remove baseline is to remove the average and can be achieved with Matlab using one line of code.

```<code>yFilt = y - mean(y)
</code>```

Second Approach

We can design a high pass filter to attenuate low frequency components. If we analyze the frequency components of the signal we will see one peak at low frequency and "infinite" small components in all frequencies due to Noise. With a second order ButterWorth filter with normalized cutoff freq Wn = 0.2 we will get what we are looking for.

Filtered data

Tilting the Gyro

When we tilt the gyroscope the situation changes. With a sampling frequency of 300Hz we get the following plot.

The first half of the dft is shown below in a normalized scale.

You can find the sample.mat file here

The first approach works great. I would like to apply the second one to this particular case but here there are other low frequency components that make to job harder.

How can I apply the second approach based on the High Pass filter to remove the bias?

EDIT 1
You can find more information here

EDIT 2
How can we filter this signal to remove bias while keeping the angular velocity information (from 110-th to 300-th sample) intact?

If gyroscopes have the bias problem only when they are not experiencing any rotation, then the offset is present only in the first ~110 samples.

If the above hypothesis is correct, maybe if we apply high pass filtering only in the first 110 samples and desactivate the filter during rotations of the gyro, the estimated angular position will be more accurate.

答:
Please refer to this Article "Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs".

They are using low-pass filter at stationary gyro to estimate the gyro bias and then subtract it from the original signal. They are doing that only if the gyro is stationary. However, I recommend to remove the bias when the gyro is stationary by means of subtracting the mean value.

The method of subtracting the bias only when the gyro is stationary based on low pass filter is called online gyro compensation.


答:
There are a number of ways to do so. Using quaternions has already been mentioned so I will go with (4x4) transformation matrices.

Let a transformation matrix $T$ be
$$
T = \begin{bmatrix}R & p\\ 0 & 1\end{bmatrix},
$$
where $R$ is a rotation matrix and $p$ is a position vector. I will write $T = (R, p)$ for short.
You want to generate a path connecting $T_1= (R_1, p_1)$ and $T_2= (R_2, p_2)$. The problem then boils down to generating a rotational path and a translational path, separately. For translation, a path can be generated pretty easily using any method like polynomial interpolation or using splines. For rotation, it's a bit more complicated but in the end it can also be done using methods like polynomial interpolation. So I will talk a bit about rotation here.

The thing is that any rotation matrix can be written as
$$R = e^{[r]},$$
where $r \in \mathbf{R}^3$, $[\cdot]$ maps a vector to a unique skew symmetric matrix, and $e^{[r]}$ is the matrix exponential
$$
e^{[r]} = \sum_{k = 0}^{\infty}\frac{1}{k!}[r]^k.
$$
(Of course, there is a formula for that without evaluating that infinite sum.) So actually a rotational path may be generated as
$$R(t) = R_0e^{[r(t)]},$$
where $r(t)$ is a polynomial vector and $R_0$ is some constant rotation matrix. Then you just need to find $r(t)$ such that $R_0e^{[r(0)]} = R_1$ and $R_0e^{[r(T)]} = R_2$, so as to meet your boundary conditions. For more details, you may have a look at this paper, for example.

Title: How to compute a path from a Frame A to a Frame B

Body:
the problem I have is the following (see picture below), I am able to compute a starting frame $F_S (X_S, Y_S, Z_S)$, an ending frame $F_E (X_E, Y_E, Z_E)$ and a path from $F_S$ to $F_T$ and what I want to do it to compute a serie of transformations that will transform $F_S$ into $F_E$ along the path.

Naively I computed the Euler angles for $F_E$ end $F_S$, compute the differences and incrementally built the transformations but it does not work. Does somebody can give me some hints or pointers towards existing solutions?

The application is related to the computation of a path for an arm and so the frames are associated with the end effector.

Thank you


问:
Is it possible to control joint torque using the position input and torque feedback?
So I am working with a UR10 manipulator which doesn't have a direct torque interface. However, it provides torque/velocity/position feedback for each joint as well as position/velocity interfaces for joint control.

I have a feeling the answer is "yes", but I've been having trouble finding examples and comments on the feasibility of this approach.

Thanks!

答:
If you're trying to do torque control, then you'll probably get best results if you could work with joint accelerations, because:

$$
\tau = I \alpha \\
$$

Where $\tau$ is joint torque, $I$ is the moment of inertia, and $\alpha$ is the joint angular acceleration. It's a linear relationship and should be pretty straightforward to control with a PID controller.

You don't have an acceleration input for your joint, though. What you have is a speed input for your joint. SO, what you could do is to setup a PID controller with torque error as your input, joint acceleration reference as your output, and then perform a numeric integration of the output to get a speed reference. This would look like:

$$
\tau_{\mbox{err}} = \tau_{\mbox{ref}} - \tau_{\mbox{fbk}} \\
e_P = \tau_{\mbox{err}} \\
e_I = e_I + e_P*\Delta t \\
e_D = \frac{e_P - e_{P_{\mbox{prev}}}}{\Delta t} \\
e_{P_{\mbox{prev}}} = e_P \\
a_{\mbox{ref}} = (K_P e_P) + (K_I e_I) + (K_D e_D) \\
v_{\mbox{ref}} = v_{\mbox{ref}} + a_{\mbox{ref}} * \Delta t \\
$$


问:
Math behind trajectory planning
Let's assume the very simple case of a particle and a control system in one dimensional space therefore our particle can move only in a straight line and dynamics of system is described by:

$m\vec{a} = u$.

Now the problem: we would like to make our particle move from point $A$ to point $B$ in time $t$ and constrain our acceleration with some  value $a_{m}$ i.e. $a$ can not exceed $a_{m}$ at any moment.

How would one do this assuming that our control system allows us to control either velocity or acceleration?

The most important things here are names of mathematical methods behind this task and explanation of how to apply them.
Also consider that

$x(0) = A = 0\\ x(t) = B\\ v(0)=0\\a(0)=0\\ v(t)=0\\ a(t)=0$

答:
Since the problem is one dimensional, you are actually asking to compute a velocity profile. (A velocity profile is the information of how a path is traversed with respect to time.) Now the problem is "How to travel for $B$ units within time $T$?" (Let's call the duration $T$ instead.)

A velocity profile can be viewed as a curve in the $v$-$t$ (velocity vs time) plane. And as we all know, the area under a curve in that $v$-$t$ plane is the displacement. So any curve which passes through points $(v_0, t_0) = (0, 0)$ and $(v_2, t_2) = (0, T)$ with the area $B$ will be what you are looking for.

One velocity profile which solves the problem is as shown below.

It contains two segments. From time $0$ to $t_s$, you travel with a constant positive acceleration. Then after that you travel with a constant negative acceleration (of equal magnitude). The peak velocity $v_p$ can be easily computed since we know that
$$
\frac{1}{2}v_{p}(T) = B.
$$
This is fine as long as the magnitude of acceleration of both parts, $|a| = v_p/t_s = 2v_p/T$, does not exceed $a_m$.

Normally, people would ask how to get from $x_0 = A$ to $x_1 = B$ the fastest possible. When there are only velocity and acceleration limits, the time-optimal velocity profile can be computed analytically (see, e.g., this paper ) using polynomial interpolation. People may also be interested in other higher-order constraints such as jerk limits (i.e., limits on the rate of change of acceleration).

I don't think there is any more specific name for these things than something like trajectory generation using polynomials or splines, etc.

Title: Math behind trajectory planning


问:
bounded deviation for straight line motion
I came across the paper(link given below) which discusses about bounded deviation joint path for straight line motion.

Planning and Execution of Straight Line Manipulator Trajectories (RH Taylor)
https://pdfs.semanticscholar.org/e01a/58608f4e68f31c7b9e7cdbddceae645727bb.pdf

In this method, the assumption is that the maximum deviation happens at or near the midpoint between the start and end point.

1) Is this assumption true in all cases?
2) Even if the assumption may not be true, will resulting trajectory be a straight line if this method is used for trajectory planning?

I hope someone shed some light on this. Thank you.

答:
1) Is this assumption true in all cases?

I think it is pretty difficult to conclude that the assumption is true in all cases, as it depends on so many factors such as robot geometries. However, as the sampled points are closer to one another, it is reasonable to assume that the maximum deviation happens near the midpoint (as can be seen from Figure 3 in the paper you mentioned).

2) Even if the assumption may not be true, will resulting trajectory be a straight line if this method is used for trajectory planning?

If you are referring to the second method discussed in the paper (bounded deviation joint paths), the resulting trajectory is not a straight line in the Cartesian space and that does not really depend on the validity of the assumption. The assumption is stated just for them to have a reasonable method to select more intermediate points if the particular trajectory segment exceeds the deviation bound. The generated trajectory will (only) follow the straight line closely enough that it does not deviates from the real straight line more than the specified maximum deviation allowed.


问:
Connecting two servo motors to drive one shaft
I need to connect two servo motors (with internal position control) to rotate one shaft (to control its position), is that possible? and how to avoid synchronization problems?

the servo motor

I am making a robotic arm, my problem here is that the servos are internally controlling position, which means that if only one of them arrived to the target and the other one haven't yet, it will still try to get to it (even if it is just 0.5 degree away).. i am afraid that this will make the system vibrate around the required position , or make one motor to continuously drag current, or it might hurt the servo shafts and gears,
 I am thinking of using pulleys and a synchronous belts to connect the servos to the shaft, this could absorb the bad effects.
so please let me know if you consider this to be safe, or if there is something i can do to improve the performance and the reliability of the system.

答:
One bigger servo is better.

You can just hook them both to the same shaft. If you give them the same command, they will fight each other a bit and waste some energy but should work. To improve performance you can calibrate one motor to the other. For each command value of motor A find the command that moves servo B to the same position.


答:
I think you are talking about velocity profiles.. as I know if you have a limit for the acceleration and a limit for the velocity then the fastest way to move from A to B is called Trapezoidal velocity model, which uses max acceleration until it reaches the max velocity as this image shows and the inverse when it stops,

all the equations can be found if you consider that the space under the velocity curve equals to the total distance (Xf - Xi), thus you can find the minimum T if the acceleration limit and velocity limit are known

Other common profiles are bang-bang acceleration, Smoothed trapezoidal velocity model, third or fifth degree trajectory, They are well explained in Modelling identification and control of Robots By W Khalil & E Dombre

Title: Math behind trajectory planning

Body:
Let's assume the very simple case of a particle and a control system in one dimensional space therefore our particle can move only in a straight line and dynamics of system is described by:

$m\vec{a} = u$.

Now the problem: we would like to make our particle move from point $A$ to point $B$ in time $t$ and constrain our acceleration with some  value $a_{m}$ i.e. $a$ can not exceed $a_{m}$ at any moment.

How would one do this assuming that our control system allows us to control either velocity or acceleration?

The most important things here are names of mathematical methods behind this task and explanation of how to apply them.
Also consider that

$x(0) = A = 0\\ x(t) = B\\ v(0)=0\\a(0)=0\\ v(t)=0\\ a(t)=0$


问:
DH parameters of a PUMA-type manipulator
I'm struggling to find the DH parameters for this PUMA-type manipulator that yield the same results as the author (1):

The way I'm checking if the parameters I have are correct is by comparing the resulting J11, J21 & J22 matrices with the author. These sub-matrices are the constituents of the wrist Jacobian matrix (Jw).

I tried many different combinations of the DH parameters including:

α
=[0,90,0,-90,90,-90]

 θ
=[0,0,0,0,0,0]

a=[0,0,a2,-a3,0,0]

d=[d1,-d2,0,-d4,0,0]

Which result in the same matrices as the author except for some minor differences. The general wrist Jacobian matrix and the sub-matrices obtained by the author are given by:

Whereas the result I got for J11 was:

$$
\left[
\begin{array}{ccc}
 -d_2 c_1-s_1 (a_2 c_2-a_3 c_{23}+d_4 s_{23}) & c_1 (d_4 c_{23}-a_2 s_2+a_3 s_{23}) & c_1 (d_4 c_{23}+a_3 s_{23}) \\
 c_1 (a_2 c_2-a_3 c_{23}+d_4 s_{23})-d_2 s_1 & s_1 (d_4 c_{23}-a_2 s_2+a_3 s_{23}) & s_1 (d_4 c_{23}+a_3 s_{23}) \\
 0 & a_2 c_2-a_3 c_{23}+d_4 s_{23} & d_4 s_{23}-a_3 c_{23} \\
\end{array}\right]
$$

And for the J22 matrix I got:

$$
\left[
\begin{array}{ccc}
 -c_1 s_{23} & c_4 s_1+c_1 c_{23} s_4 & s_1 s_4 s_5-c_1 (c_3 (c_5 s_2+c_2 c_4 s_5)+s_3 (c_2 c_5-c_4 s_2 s_5)) \\
 -s_1 s_{23} & c_{23} s_1 s_4-c_1 c_4 & -c_5 s_1 s_{23}-(c_2 c_3 c_4 s_1-c_4 s_2 s_3 s_1+c_1 s_4) s_5 \\
 c_{23} & s_{23} s_4 & c_{23} c_5-c_4 s_{23} s_5 \\
\end{array}\right]
$$
And the same J12 matrix as the author.

Perhaps the most pronounced difference here is that every Sin
  [

   θ2
   +
   θ3

  ]

 is replaced with  Cos
  [

   θ2
   +
   θ3

  ]

 and vice versa, in addition to some sign differences.

Where am I going wrong here?

(1) Wenfu Xu, Bin Liang, Yangsheng Xu, "Practical approaches to handle the singularities of a wrist-partitioned space manipulator".

答:
In looking at your matrices, I would guess that you (or the author!) have made a mistake somewhere in your alpha $\alpha$ terms. Recall the period shift identity that states:

$$
\sin{\left(\theta + \pi/2 \right)} = +\cos{\theta} \\
\cos{\left(\theta + \pi/2 \right)} = -\sin{\theta} \\
$$
So, if you (or again, the author) were off by 90 degrees ($\pi/2$), then that would explain the fact that you appear to have some sines and cosines swapped and some sign errors, too.

I don't have the time at the moment to go through each step by hand and try to evaluate where you've gone wrong, but I would imagine that you should be able to compare individual joint transforms and find the ones that don't match. Try changing the $\alpha$ values for that joint by $\pm \pi/2$ and see if what you have matches what the author has.

Authors can and do make mistakes, and if you're positive the author made a mistake then the professional thing to do would be to contact that author and/or the journal that published the paper and alert them to the mistake. I would just double- and triple-check that the author was incorrect before taking that step, though (run it by your professors, etc.)


问:
What is the difference between motion planning and trajectory generation?
What are the major differences between motion planning and trajectory generation in robotics? Can the terms be used interchangeably?</p>

答:
Sometimes the two words are used interchangeably. However, there are some subtle nuances between motion planning and trajectory generation.

When talking about motion planning, it is planning motions for robots to move from point $A$ to point $B$ (such as for mobile robots, etc.) or pose $A$ to pose $B$ (such as for manipulators, etc.). In order to do so, a number of constraints need to be taken into account: collision avoidance, joint limits, velocity/acceleration limits, jerk limits, dynamic balance, torque bounds, and many more. In this sense, not only the  the robots is considered but also its environment (e.g., to avoid collision, to how remain balanced). Considering this, motion planning is kind of trajectory generation with lots of constraints. They may be considered the same thing. So, yes, you can use them interchangeably.

When talking about trajectory generation, the scope can be narrower than that of motion planning. Often time, in trajectory generation, people really focus on generating a trajectories---with joint limits, velocity and acceleration constraints---and just that. One case I can think of which is called trajectory generation but not motion planning (or at least, that's what I perceive) is as follows. Think of when we want to execute a trajectory on a manipulator. We send commands to the controller as a set of waypoints, i.e., discrete points (supposedly closed to one another) spread across the trajectory, often at a fixed interval equal to the controller's sampling time. The controller then has to regenerate trajectory segments between two consecutive waypoints, such that manipulator reaches the next waypoint within the fixed time interval while staying within joint limits, velocity limits, and acceleration limits. However, the controller does not really consider even collision avoidance or anything else. That is called trajectory generation.

Sometimes, people may think of motion planning as path planning. That's another thing since, strictly speaking, a path is not equal to a trajectory. A trajectory is a path and information of how to traverse the path with respect to time, a.k.a a velocity profile. Considering this, trajectory generation is kind of a bigger thing.

tl;dr

Generally, motion planning and trajectory generation are kind of interchangeable.

Sometimes, trajectory generation can be viewed as a subclass of motion planning.

If motion planning is viewed as path planning, then trajectory generation is a bigger thing.


问:
Stabilising a quadcopter using YPR
I'm using the MPU-6050 accelerometer + gyro with the library I2Cdev which outputs: quaternion, euler angles and YPR angles. The equations used for calculating the YPR are:

```<code>uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
  // yaw: (about Z axis)
  data[0] = atan2(2 * q -> x * q -> y - 2 * q -> w * q -> z, 2 * q -> w * q -> w + 2 * q -> x * q -> x - 1);
  // pitch: (nose up/down, about Y axis)
  data[1] = atan(gravity -> x / sqrt(gravity -> y * gravity -> y + gravity -> z * gravity -> z));
  // roll: (tilt left/right, about X axis)
  data[2] = atan(gravity -> y / sqrt(gravity -> x * gravity -> x + gravity -> z * gravity -> z));
  return 0;
}
</code>```

I want to stabilize a quadcopter with these values and 3 PID regulators like this:

FL = Throttle + (-PitchPID) + (-RollPID) + (+YawPID)

FR = Throttle + (-PitchPID) + (+RollPID) + (-YawPID)

RL = Throttle + (+PitchPID) + (-RollPID) + (+YawPID)

RR = Throttle + (+PitchPID) + (+RollPID) + (-YawPID)

The pitch and roll values are between -90 and +90 degrees (0 degrees is horizontal and +-90 is vertical). The problem is that when the quad starts tipping over, the error will start decreasing and will stabilize upside down.

答:
You can't use an absolute value for the feedback in a PID controller.

Integral error accumulates the control error, which is the difference between what you want and what you have. So, consider a basic case where you want level flight (reference = 0).

If you take the absolute value of the feedback, then in ALL cases your feedback is either zero or positive. That means that the control error (reference - feedback), is ALWAYS NEGATIVE.

The integral error term can ONLY accumulate negative values. Typically you would overshoot the reference, at which point the sign would flip on the feedback and you could begin accumulating POSITIVE values, which would "bleed" the integral error term.

You have no means to reduce integral error. It will be a large negative number that will only ever get more negative (until you reset it or it overflows).

You can't just flip the sign on the output of the PID controller when a feedback signal changes signs because that doesn't do anything to correct the error terms that drive the controller.

Contributing to the problems you're having with the IMU are the facts that it looks like you're not converting from the quaternion to Euler angles correctly, and maybe also that you're not even getting a quaternion.

Any time you see an atan function your first thought should be, "Something's wrong." The atan function cannot differentiate between $(-y/x)$ and $(y/-x)$, so it can't always give you a correct solution.

I can see where you got the calculations and I understand that a sqrt() function will always return a positive value, but that's a problem too, isn't it? If your denominator is always positive then you know that you're never going to get a value of $-x$ and thus you know that you're not going to get a full range of angles from the calculation.

I would suggest using the code provided on Wikipedia if you're going to do this conversion yourself:

```<code>static void toEulerianAngle(const Quaterniond& q, double& roll, double& pitch, double& yaw)
{
  double ysqr = q.y() * q.y();

  // roll (x-axis rotation)
  double t0 = +2.0 * (q.w() * q.x() + q.y() * q.z());
  double t1 = +1.0 - 2.0 * (q.x() * q.x() + ysqr);
  roll = std::atan2(t0, t1);

  // pitch (y-axis rotation)
  double t2 = +2.0 * (q.w() * q.y() - q.z() * q.x());
  t2 = t2 > 1.0 ? 1.0 : t2;
  t2 = t2 < -1.0 ? -1.0 : t2;
  pitch = std::asin(t2);

  // yaw (z-axis rotation)
  double t3 = +2.0 * (q.w() * q.z() + q.x() * q.y());
  double t4 = +1.0 - 2.0 * (ysqr + q.z() * q.z());
  yaw = std::atan2(t3, t4);
}
</code>```

But again, back to the source of the quaternion - how are you getting that value? From the documentation page you linked:

Currently, the source code available will only provide basic device configuration and raw accel/gryo readings.

When I searched the PDF files at the bottom of the product's website, there were NO mentions of "quaternion" or "Euler" or "angle," so I'm not sure what the "motion processing" feature of the chip is or what it's supposed to do.

So, in summary:

Double check how/where/if you are actually getting a quaternion,

Check the math on how you are converting a quaternion to an Euler angle and verify that math checks out for all angles, and

Always use signed feedback for a PID controller.

Title: Stabilising a quadcopter using YPR

Body:
You can't use an absolute value for the feedback in a PID controller.

Integral error accumulates the control error, which is the difference between what you want and what you have. So, consider a basic case where you want level flight (reference = 0).

If you take the absolute value of the feedback, then in ALL cases your feedback is either zero or positive. That means that the control error (reference - feedback), is ALWAYS NEGATIVE.

The integral error term can ONLY accumulate negative values. Typically you would overshoot the reference, at which point the sign would flip on the feedback and you could begin accumulating POSITIVE values, which would "bleed" the integral error term.

You have no means to reduce integral error. It will be a large negative number that will only ever get more negative (until you reset it or it overflows).

You can't just flip the sign on the output of the PID controller when a feedback signal changes signs because that doesn't do anything to correct the error terms that drive the controller.

Contributing to the problems you're having with the IMU are the facts that it looks like you're not converting from the quaternion to Euler angles correctly, and maybe also that you're not even getting a quaternion.

Any time you see an atan function your first thought should be, "Something's wrong." The atan function cannot differentiate between $(-y/x)$ and $(y/-x)$, so it can't always give you a correct solution.

I can see where you got the calculations and I understand that a sqrt() function will always return a positive value, but that's a problem too, isn't it? If your denominator is always positive then you know that you're never going to get a value of $-x$ and thus you know that you're not going to get a full range of angles from the calculation.

I would suggest using the code provided on Wikipedia if you're going to do this conversion yourself:

static void toEulerianAngle(const Quaterniond& q, double& roll, double& pitch, double& yaw)
{
  double ysqr = q.y() * q.y();

  // roll (x-axis rotation)
  double t0 = +2.0 * (q.w() * q.x() + q.y() * q.z());
  double t1 = +1.0 - 2.0 * (q.x() * q.x() + ysqr);
  roll = std::atan2(t0, t1);

  // pitch (y-axis rotation)
  double t2 = +2.0 * (q.w() * q.y() - q.z() * q.x());
  t2 = t2 > 1.0 ? 1.0 : t2;
  t2 = t2 < -1.0 ? -1.0 : t2;
  pitch = std::asin(t2);

  // yaw (z-axis rotation)
  double t3 = +2.0 * (q.w() * q.z() + q.x() * q.y());
  double t4 = +1.0 - 2.0 * (ysqr + q.z() * q.z());
  yaw = std::atan2(t3, t4);
}

But again, back to the source of the quaternion - how are you getting that value? From the documentation page you linked:

Currently, the source code available will only provide basic device configuration and raw accel/gryo readings.

When I searched the PDF files at the bottom of the product's website, there were NO mentions of "quaternion" or "Euler" or "angle," so I'm not sure what the "motion processing" feature of the chip is or what it's supposed to do.

So, in summary:

Double check how/where/if you are actually getting a quaternion,

Check the math on how you are converting a quaternion to an Euler angle and verify that math checks out for all angles, and

Always use signed feedback for a PID controller.


问:
function of PIDControl #pragma config() directive in robotC
I am trying to sync motors on a VEX Cortex based robot and have had mixed success using the encoders with position control.  I noticed that the motor setup directive

```<code>#pragma config(Motor, port2, motorA, tmotorVex393, PIDControl, encoder, encoderPort, I2C_1, 1000)
</code>```

has a parameter "PIDControl" but I cannot find any documentation as to what it actually does.

I see on the encoder documentation page here that the encoder provides velocity output, but it is not apparently built into the API.  So my question is two fold:

1) What does the "PIDControl" directive actually do?

2) How can I use the encoder to control the speed of the motors?

答:
"PIDControl" directive allows the internal to do closed loop feedback control on the motors. You do want this on. Without going into greater detail, you should leave it there so that you can have use more robust motor motions.

You do not use encoders to control speed. You use encoders to control distance.  Now, having said that, if you want to get higher performance of motor control, such as eliminate overshooting, you will need to use the encoder to tell the maximum distance (equivalent to the encoder value, you need simple gear math work here, of course). Use the setMotorTarget(...).


问:
Confusion about order of homogeneous transform multiplication
This is my first post here, so if I unknowingly vioated any rules, mods are welcome to edit my post accordingly.

Ok, so my problem is that following Craig's conventions, I can't seem to find the expected homogeneous transform after a series of transformations.

I have included the image for clarity.

We are given the initial frame {0} as usual and then:

-$\{A\}$ is the frame under rotating {0} $90^\circ$ around $z$ and translating, with $OA = \begin{bmatrix}
1
\\ 1
\\ 1
\end{bmatrix}$

-$\{B\}$ is obtained after translating $A$ by $AB = \begin{bmatrix}
-2
\\ -2
\\ 0
\end{bmatrix}$

What I found is:

$$ {}_A^OT = \left[ {\begin{array}{*{20}{c}}
0&-1&0&1\\
1&0&0&1\\
0&0&1&{1}\\
0&0&0&1
\end{array}} \right], \;\;{}_B^AT=\left[ {\begin{array}{*{20}{c}}
1&0&0&-2\\
0&1&0&-2\\
0&0&1&{0}\\
0&0&0&1
\end{array}} \right],\\ {}_B^OT = {}_A^OT  {}_B^AT=\left[ {\begin{array}{*{20}{c}}
0&-1&0&3\\
1&0&0&-1\\
0&0&1&{1}\\
0&0&0&1
\end{array}} \right] $$
This is wrong, since the last column should obviously have the coordinates $-1,-1,1$, the origin of B

What am I missing?

答: In order to find the correct translation you need to rotate the origin of frame B by $^{A}_{B}{R}$ and then add the origin of frame A. Check eq. 2.41 page 35 of Craig's Introduction to Robotics</p>


问:
using Li-Ion batteries in parallel
Looking at the Li-Ion battery packs from Servocity, part #605056, 12V, 6000maH battery pack. Any reason I shouldn't put these in parallel with each other? Any idea on what these might weigh? I've got a robot project going, currently running on a very heavy 12V lead acid RV battery, essentially a small car battery.</p>

答:
Hoo boy, let's tackle this one part at a time.

Weight

You're in luck! Lithium-based batteries are some of the lightest in the business! Obviously you could go lighter if you had Lithium Polymer instead of Lithium Ion but that's beside the point. This would certainly weight a fraction of what your RV battery weighs. Unfortunately, because your listed part doesn't have very good datasheets I can't tell if it actually is a Li-Ion since there isn't much to go on and the voltages are a little odd (more on that later).

Parallel

Again, easy as cake. Simply connect any group of batteries of the same values and you'll be golden. Though those odd voltages... anyways see below for a lil' demonstration.
As you can see, if we add together a bunch of batteries in parallel, they're voltage will remain the same, and the total capacity for this would become 8000 mAh. However, try to keep the specifications for the batteries exactly the same, else you could have problems (see here for an explanation). Though I will warn you, be mindful of the difficulties of charging an array of Li-Ion batteries. There are specialized chargers but it might be safest to charge them all seperately.

The battery itself

Ah yes, now we must look at the problems I see with your choice of battery. First, the "datasheet":
That is, first of all, not a datasheet. That is a user manual. Why is this a problem? We don't know how many batteries are in there, what kind actually are in there, and what their characteristics are. In reality, it could be a NiMH battery and that would ruin everybody's day (also, they're heavier). Normally, LiPo and Li-Ion batteries have a nominal voltage of 3.7v. However, these listed voltages are not divisible by the nominal voltage values slapped onto Lithium-based batteries (multiples would give us the number of cells). Say we take nominal voltage with it's 3.7v, 7.4v, 11.1v, or 14.8v. Neither 12.6v or 8.4v fits. Let's try the maximum voltage at 4.2v, 8.4v, and 12.6v. Ah, a match. It would appear that this might be listing the range, which would mean that 8.4v is the voltage you would be able to pull from the battery before permanently damaging it. So it's definitely a 3-cell 6Ah Li-Ion. HOWEVER, its nominal voltage is 11.1v, which means that 90% of the time you will be getting less than 12v, which I assume is not what you want.

In summary

No problem, it'll be lighter; make sure you account for possible issues charging arrays, and you'll be fine. However, this battery may not be quite what you are looking for with a nominal voltage of 11.1v compared to your SLA's 12v.

Sorry some of it got long, I got carried away. Credit to SparkFun for the parallel battery image.

Title: using Li-Ion batteries in parallel


答:
It is the order of multiplication.

Suppose you have a frame $A$ and you want to apply the transformation $T_B$ to $A$:

If $T_B$ is described in the global frame, you pre-multiply $T_A$ with $T_B$. This is your desired solution: you want to translate $A$ by $(-2, -2, 0)$, i.e., $-2$ in the world's $x$-direction and $-2$ in the world's $y$-direction.

If $T_B$ is described in the $A$ frame, you post-multiply $T_A$ with $T_B$. This is what you did.

Title: Confusion about order of homogeneous transform multiplication

Body:
This is my first post here, so if I unknowingly vioated any rules, mods are welcome to edit my post accordingly.

Ok, so my problem is that following Craig's conventions, I can't seem to find the expected homogeneous transform after a series of transformations.

I have included the image for clarity.

We are given the initial frame {0} as usual and then:

-$\{A\}$ is the frame under rotating {0} $90^\circ$ around $z$ and translating, with $OA = \begin{bmatrix}
1
\\ 1
\\ 1
\end{bmatrix}$

-$\{B\}$ is obtained after translating $A$ by $AB = \begin{bmatrix}
-2
\\ -2
\\ 0
\end{bmatrix}$

What I found is:

$$ {}_A^OT = \left[ {\begin{array}{*{20}{c}}
0&-1&0&1\\
1&0&0&1\\
0&0&1&{1}\\
0&0&0&1
\end{array}} \right], \;\;{}_B^AT=\left[ {\begin{array}{*{20}{c}}
1&0&0&-2\\
0&1&0&-2\\
0&0&1&{0}\\
0&0&0&1
\end{array}} \right],\\ {}_B^OT = {}_A^OT  {}_B^AT=\left[ {\begin{array}{*{20}{c}}
0&-1&0&3\\
1&0&0&-1\\
0&0&1&{1}\\
0&0&0&1
\end{array}} \right] $$
This is wrong, since the last column should obviously have the coordinates $-1,-1,1$, the origin of B

What am I missing?


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

答:
I don't understand what you mean by order. I have always seen the quadrotor modelled like this, for instance.

For your question of how to represent the body-frame vectors into inertial frames, no quadrotor knowledge is necessary. Just Applied Mechanics, where the rotation matrix R showed in previous link is necessary.

It has no influence on the control, as the control is usually modelled with body-frame angles of the quadrotor.