答:
a robot that could get items out of the dishwasher and put them away

This is an active research topic. Toyota has a whole division working on assistive robotics, close to Stanford in Palo Alto. It is by no means a solved problem.

That being said, scissors are a fine way to guide motion in a single dimension, when you can afford the additional size perpendicular to the axis of motion. Scissors expand sideways a lot when they are close to the minimum extension point.
Also, the backlash in the scissor joints adds up, so a four-joint scissors has four times as much backlash as a single-joint mechanism. Backlash in turn adds to flex and imprecision in movement.

If you're running this in your home, also consider the pinching hazard for pet tails, child fingers, and the occasional forgetful owner.

Another option, that is often used, and that uses less space, is a linear actuator. This can be as simple as a worm screw driven by a gear motor or stepper, and some kind of sensor. Open loop stepper counts, encoder motor counts, potentiometer, distance sensor, ... plus a homing / zero sensor / limit switch.
Add parallel linear rails/rods with appropriate bearings and you get a high-precision, high-strength assembly.

Title: Are scissors what I want for moving straight up and out?

Body:
I'm a programmer. I had a tiny amount of experience building robots in college a few years ago, but haven't done anything since.

I'd like to build a robot that can move around my house, pick items up and put them down... IE, a robot that could get items out of the dishwasher and put them away.

I was thinking it would have a square base with a wheel at each corner for moving around the floor, a scissor jack so it can adjust its height (I'm hoping to be able to be able to move between 1 foot tall and 7 feet tall), and then a scissor jack at the top for moving a gripper towards or away from it (between a couple of inches and 3 feet away).

Are two scissor jacks actually what I want? It seems like 90+% of robots with grippers go with arms instead, but it seems to me that those are more complicated and would be less precise. I have no robotic experience beyond little car like things - I've never built one with any sort of gripper or actuator, so advise would be much appreciated. (Huh - there's not even a tag for scissor or jack... what do you guys call them?)

Also... do people normally build these parts themselves, or do they buy them? I've searched around but I can't find any scissor jack kit or anything like that. If I need to build it myself... how would I do that? What would I build it out of?


问:
Which sensors will best suited for indoor/outdoor SLAM system with high accuracy of localization?
We need a SLAM system for mobile platform which will be used in on-site construction.

We are to build a 3D map from clouds of points and need localization in it.

We don’t need fast localization, but we need high accuracy of localization, error of localization should be around 1mm at a distance up to 5 meters.

Which sensor will suit for this - maybe it will be enough only a stereo vision (can it give us required accuracy?), or only LiDAR sensor (what’s about accuracy and usage in direct sunlight?) or we need to use something different and combine several type of sensors?

Can you advise the best solution for this task?

答:
A high-performance outdoors LIDAR is almost certainly going to be part of the solution. You still won't get 1mm out to 5m, though.

Another sensor I would add would be stereo vision. Kinects are kind-of bad in direct sunlight; they are indoor devices; the later RealSense sensors from Intel are better in that regard but certainly not perfect.

Also, consider that dust, sparks, and smoke in a construction site will impede the ability to localize. If you need really precise localization of the robot (rather than other objects) then you can put a number of beacons around the construction site, and let the robot localize using millimeter radio instead. Look for example at https://www.pozyx.io/

ContentLicense: CC BY-SA 3.0


问:
EKF-SLAM Update step, Kalman Gain becomes singular
I'm using an EKF for SLAM and I'm having some problem with the update step.  I'm getting a warning that K is singular, rcond evaluates to near eps or NaN. I think I've traced the problem to the inversion of Z.  Is there a way to calculate the Kalman Gain without inverting the last term?

I'm not 100% positive this is the cause of the problem, so I've also put my entire code here.  The main entry point is slam2d.

```<code>function [ x, P ] = expectation( x, P, lmk_idx, observation)
    % expectation
    r_idx = [1;2;3];
    rl = [r_idx; lmk_idx];

    [e, E_r, E_l] = project(x(r), x(lmk_idx));
    E_rl = [E_r E_l];
    E = E_rl * P(rl,rl) * E_rl';

    % innovation
    z = observation - e;
    Z = E;

    % Kalman gain
    K = P(:, rl) * E_rl' * Z^-1;

    % update
    x = x + K * z;
    P = P - K * Z * K';
end

function [y, Y_r, Y_p] = project(r, p)
    [p_r, PR_r, PR_p] = toFrame2D(r, p);
    [y, Y_pr]   = scan(p_r);
    Y_r = Y_pr * PR_r;
    Y_p = Y_pr * PR_p;
end

function [p_r, PR_r, PR_p] = toFrame2D(r , p)
    t = r(1:2);
    a = r(3);
    R = [cos(a) -sin(a) ; sin(a) cos(a)];
    p_r = R' * (p - t);
    px = p(1);
    py = p(2);
    x = t(1);
    y = t(2);
    PR_r = [...
        [ -cos(a), -sin(a),   cos(a)*(py - y) - sin(a)*(px - x)]
        [  sin(a), -cos(a), - cos(a)*(px - x) - sin(a)*(py - y)]];
    PR_p = R';
end

function [y, Y_x] = scan(x)
    px = x(1);
    py = x(2);
    d = sqrt(px^2 + py^2);
    a = atan2(py, px);
    y = [d;a];
    Y_x =[...
    [     px/(px^2 + py^2)^(1/2), py/(px^2 + py^2)^(1/2)]
    [ -py/(px^2*(py^2/px^2 + 1)), 1/(px*(py^2/px^2 + 1))]];
end
</code>```

Edits:
project(x(r), x(lmk)) should have been project(x(r), x(lmk_idx)) and is now corrected above.

K goes singular after a little while, but not immediately.  I think it's around 20 seconds or so.  I'll try the changes @josh suggested when I get home tonight and post the results.

Update 1:

My simulation first observes 2 landmarks, so K is $7\ x\ 2$.  (P(rl,rl) * E_rl') * inv( Z ) results in a $5\ x\ 2$ matrix, so it can't be added to x in the next line.

K becomes singular after 4.82 seconds, with measurements at 50Hz (241 steps).  Following the advice here, I tried K = (P(:, rl) * E_rl')/Z which results in 250 steps before a warning about K being singular is produced.

This tells me the problem isn't with matrix inversion, but it's somewhere else that's causing the problem.

Update 2

My main loop is (with a robot object to store x,P and landmark pointers):

```<code>for t = 0:sample_time:max_time
    P = robot.P;
    x = robot.x;
    lmks = robot.lmks;
    mapspace = robot.mapspace;

    u = robot.control(t);
    m = robot.measure(t);

    % Added to show eigenvalues at each step
    [val, vec] = eig(P);
    disp('***')
    disp(val)

    %%% Motion/Prediction
    [x, P] = predict(x, P, u, dt);

    %%% Correction
    lids = intersect(m(1,:), lmks(1,:));  % find all observed landmarks
    lids_new = setdiff(m(1,:), lmks(1,:));
    for lid = lids
        % expectation
        idx = find (lmks(1,:) == lid, 1);
        lmk = lmks(2:3, idx);
        mid = m(1,:) == lid;
        yi = m(2:3, mid);

        [x, P] = expectation(x, P, lmk, yi);
    end  %end correction

    %%% New Landmarks

    for id = 1:length(lids_new)
    % if id ~= 0
        lid = lids_new(id);
        lmk = find(lmks(1,:)==false, 1);
        s = find(mapspace, 2);
        if ~isempty(s)
            mapspace(s) = 0;
            lmks(:,lmk) = [lid; s'];
            yi = m(2:3,m(1,:) == lid);

            [x(s), L_r, L_y] = backProject(x(r), yi);

            P(s,:) = L_r * P(r,:);
            P(:,s) = [P(s,:)'; eye(2)];
            P(s,s) = L_r * P(r,r) * L_r';
        end
    end  % end new landmarks

    %%% Save State
    robot.save_state(x, P, mapspace, lmks)
    end
end
</code>```

At the end of this loop, I save x and P back to the robot, so I believe I'm propagating the covariance through each iteration.

More edits
The (hopefully) correct eigenvalues are now here. There are a number of eigenvalues that are negative. Although their magnitude is never very large, $10^{-2}$ at most, it happens on the iteration immediately after the first landmark is observed and added to the map (in the "new landmarks" section of the main loop).

答:
I just see your post now and it is maybe too late to really help you... but in case you are still interested in this: I think that I identified your problem.

You write the innovation covariance matrix in the following way

E = jacobian measure * P * jacobian measure

It might be alright in theory but what happens is that if your algorithm is effective and especially if you are working on a simulation: the uncertainties will decrease, especially in the directions of your measurement. So E will tend to [[0,0][0,0]].

To avoid this problem what you can do is to add a measurement noise corresponding to the uncertainties in the measurement and your innovation covariance becomes

E= Jac*P*Jac'+R

where R is the covariance of the measurement noise (diagonal matrix where the terms on diagonal are the squares of the standard deviation of the noise). If you actually don't want to consider noise you can make it as small as you want.

I also add that your covariance update seems strange to me the classical formula is:

P=P - K * jacobian measure * P

I never saw your formula written anywhere else, I might be correct but if you are not sure of it you should check it.

ContentLicense: CC BY-SA 4.0


问:
Forward kinematics equations
I'm stuck at computing forward kinematics equations.

I have configuration of the first two joints like on the following image:

Transformation from the origin to the first joint basis is trivial: just translation by $\vec{OO_{1}}$.

The second transform from joint 1 to joint 2 basis makes me nervous throughout this day. First of all it is a rotation around $Z$ axis. So rotational part will look like this:

$
R_{12}=
\begin{pmatrix}
cos(q_{1}) & -sin(q_{1}) & 0\\
sin(q_{1}) & cos(q_{1}) & 0\\
0 & 0 & 1
\end{pmatrix}
$

Problems are all about the translation part. I see two approaches here.
Since angle between $\vec{O_{1}O_{2}}$ and plane $X_{1}O_{1}Y_{1}$ is constant because rotation is performed around $Z$ axis, length of projection of $\vec{O_{1}O_{2}}$ onto $X_{1}O_{1}Y_{1}$ is constant. Here it is:

$\vec{v} = O_{2} - O_{1} = \begin{pmatrix} v_{x}\\ v_{y}\\ v_{z} \end{pmatrix}
$
Its' projection onto $X_{1}O_{1}Y_{1}$ is $\vec{v_{p}} = \begin{pmatrix} v_{x}\\ v_{y}\\ 0 \end{pmatrix}$ and it's magnitude is $m=\sqrt{v_{x}^2 + v_{y}^2}=const$.

Now let's look at what happens after rotation:

So the translation matrix looks like:
$
S_{12}=
\begin{pmatrix}
m\cdot cos(\alpha+q_{1})\\ m\cdot sin(\alpha+q_{1})\\ v_{z}
\end{pmatrix}
$

And full transformation matrix from joint 1 to joint 2 basis is:

$
T_{12}=
\begin{pmatrix}
R_{12} & S_{12}\\
0 & 1
\end{pmatrix}
$

Unfortunately it gives me wrong results even when $q_{1}=0$. Can not see where my reasoning is wrong.

Second approach is more straightforward. Being able to calculate $\vec{O_{1}O_{2}}$ in initial configuration makes it possible just to rotate this vector by $q_{1}$ around $Z$ axis and this has to be our translation vector. Nevertheless I can't make it work.

$R_{z}=
\begin{pmatrix}
cos(q_{1}) & -sin(q_{1}) & 0\\
sin(q_{1}) & cos(q_{1}) & 0\\
0 & 0 & 1
\end{pmatrix}\\
\vec{v} = O_{2} - O_{1} = \begin{pmatrix} v_{x}\\ v_{y}\\ v_{z} \end{pmatrix}\\
R_{z}\vec{v}=
\begin{pmatrix}
v_{x}cos(q_{1})-v_{y}sin(q_{1})\\
v_{x}sin(q_{1})+v_{y}cos(q_{1})\\
v_{z}
\end{pmatrix}\\
T_{12}=
\begin{pmatrix}
R_{12} & R_{z}\vec{v}\\
0 & 1
\end{pmatrix}
$

It works until I rotate the first joint(i.e. only when $q_{1}=0$).

Under works I mean "calculates position of joint 2 origin right". This is done by multiplying transformation matrix $T_{02} = T_{01}T_{12}$ by $\begin{pmatrix} 0 & 0 & 0 & 1\end{pmatrix}^{T}$

答:
In general, when you apply "rotation around a point," you construct that matrix by doing:

translate the center of rotation to world origin

apply rotation around world origin

translate the world origin back to the center of rotation

Also note that there are two matrix conventions, and nobody agrees which one is "standard": row vectors on the left, with translation in the bottom row of the matrix, or column vectors on the right, with translation in the right column of the matrix. As long as you know which one you're using, and rigorously stick to it, and know to translate if you import data/formulas from another source, you'll do fine. (The translation of a matrix between conventions is a simple transpose.)

Separately, there's the "left handed" versus "right handed" world coordinate interpretation conventions that may also vary, but luckily, the math is the same for both, so it doesn't change the matrices. A 90 degree rotation of unit X around the Z axis yields the unit Y vector, in either convention; the different only happens on translation to/from world coordinate space.

Title: Forward kinematics equations


问:
what controller do we use in robots that has forward and inverse kinematics?
i used ATmega328p chip to make a car, but its the car is always controlled by me.
my question is in robotic arms , the arm moves using the kinematics that i put, what chip and programming language should i use? i know of matlab which works with matrices, but what chip works with it?</p>

答:
Matlab doesn't run on the Atmega328p. You have to use a separate computer, which runs Matlab, and then use some kind of data transmission from the computer to the chip to tell it where to go. I e, the chip would receive solved joint angles from the transmission, and drive the actuators to those angles.

If you need to run the Matlab on your particular robot, then you need a computer on the robot that can run Matlab. An Intel NUC is a good choice for this. You'll still need some way to get the data from Matlab to the actuators; either use actuators with a built-in serial bus (such as Robotis Dynamixel servos and the USB2Dynamixel serial interface) or use your Atmega328p as a translator from Matlab commands to actuator signals.

Another option is to write the forward kinematics directly in C++ code. This requires that you're OK with linear algebra details and with C++ programming. Most microcontrollers can do well in this situation, but I'd recommend something reasonably modern an powerful, such as the Teensy 3.5 from PJRC: https://www.pjrc.com/store/teensy35.html
It has a hardware floating point unit, which is great, and a good set of libraries and integration into the Arduino IDE, so it will look similar to what you're used to.


问:
Very slow calculations of link orientation for 6DOF IK
I have written a matlab code to solve IK for 6 DOF robotic arm. I use Newton method to numerically solve IK. Also i use Tikhonov regularization to hand bad conditioned Jacobians. It works fast and reliable when i want just to move the last link in certain position, when i use difference between X,Y,Z coordinates as condition to interrupt loop of Newtoon method. But when i want also to get into the right orientation (use difference between Euler angles as interrupt condition) it takes a very long time 2, 5, 10 minutes even more, regardless i want to get to the right coordinates also or not. So there are questions:

How can i accelerate calculations, or why is it so slow?

Can i use quternions instead of Euler angles? Quternions will increase dimension of Jacobian so it will not be a square matrix anymore and it will not be possible to use Tikhonov regularization that works so good.

How often people use numerical methods to solve such things? I saw may examples of using analitycal solutions but not numerical.

How to get sure that programm will find solution using Newton method and programm will find it in finite number of interations?

UPD: here is my matlab code, that was rewritten using damped least squares method and quaternion. But still i have the same problem.
In this code we move along trajectory, but we can remove it and try to jump directly to the destination point.

```<code>%Derivative step for Jacobian composing
step = 0.01;

%Generalized coordinates for start position
q_prev = [34; 89; 1; 1; 89; 0];

%Generalized coordinates for end position. To be sure we can reach it
q_fin = [170; 150; 120; 156; 9; 158];

%get_coordinates() function returns 4x4 matrix of homogeneous transformations. It contains forward kinematics equations
%Coordinates we are at
A_forward = get_coordinates( q_prev );

%Coordinates we need to reach
Dest = get_coordinates( q_fin );

%Getting rotation matrices for start and finish positions
rotmat_curr = A_forward(1:3, 1:3);
rotmat_dest = Dest(1:3, 1:3);

%Matrix_to_quat() - is my analog of rotm2quat() function
%Getting quternions for start and finish positions
quat_curr = matrix_to_quat(rotmat_curr);
quat_dest = matrix_to_quat(rotmat_dest);

%Next steps are not inmportant, but i still comment them

%Here i make a trajectory, and move along it with small steps. It was
%needed for Newton's method but also useful if it is needed to move along a
%real trajectory
                       %X coordinate    Y coordinate    Z coordinate    Quaternion
coordinates_current = [ A_forward(1,4); A_forward(2,4); A_forward(3,4); quat_curr ];
coordinates_destination = [ Dest(1,4); Dest(2,4); Dest(3,4); quat_dest ];

%Coordinate step
step_coord = 5;

%Create table - trajectory
distance = sqrt( (coordinates_destination(1) - coordinates_current(1)).^2 + (coordinates_destination(2) - coordinates_current(2)).^2 +(coordinates_destination(3) - coordinates_current(3)).^2 );

%Find out the number of trajectory points
num_of_steps = floor(distance / step_coord);

%Initialize trajectory table
table_traj = zeros(7,(5*num_of_steps));

%Calculate steps size for each coordinate
step_x = (coordinates_destination(1) - coordinates_current(1)) / num_of_steps;
step_y = (coordinates_destination(2) - coordinates_current(2)) / num_of_steps;
step_z = (coordinates_destination(3) - coordinates_current(3)) / num_of_steps;
step_qw = (coordinates_destination(4) - coordinates_current(4)) / num_of_steps;
step_qx = (coordinates_destination(5) - coordinates_current(5)) / num_of_steps;
step_qy = (coordinates_destination(6) - coordinates_current(6)) / num_of_steps;
step_qz = (coordinates_destination(7) - coordinates_current(7)) / num_of_steps;

new_coord = coordinates_current;
%Fill trajectory table
for ind = 1:num_of_steps
    new_coord = new_coord + [step_x; step_y; step_z; step_qw; step_qx; step_qy; step_qz];
    table_traj(:,ind) = new_coord;
end;

%Set lambda size. I found out that algorithm works better when lambda is
%small
lambda = 0.1;

%In next steps i inialize Jacobian, build new destination matrix, calculate
%orientation error at the first step. As orientation error i use max
%element of quaternions difference.
for ind = 1:num_of_steps
    J = zeros(7, 6);

    %quat_to_matrix() - analog of quat2rotm() function
    rot_matr = quat_to_matrix(table_traj(4:7, ind));

    Dest = [ rot_matr, table_traj(1:3, ind);
             0, 0, 0, 1  ];

    %mat_to_coord_quat() function takes matrix of homogeneous
    %transformations and returns 7x1 vector of coorditaes
    %X Y Z and quaternion
    differ = mat_to_coord_quat(Dest) - mat_to_coord_quat(A_forward);
    error = max(abs(differ(4:6)));

    %Here is the algorithm. It works until we have coordinates and
    %orientation error less that was set
     while (abs(differ(1)) > 0.05) || (abs(differ(2)) > 0.05) || (abs(differ(3)) > 0.05) || error > 0.01

         %first - calculating Jacobian
         for ind2 = 1:6  %for every coordinate
                %Calculating of partial derivatives:
                q_prev_m1 = q_prev;
                q_prev_m1(ind2) = q_prev_m1(ind2) - step;
                q_prev_p1 = q_prev;
                q_prev_p1(ind2) = q_prev_p1(ind2) + step;
                Fn1 = mat_to_coord_quat(get_coordinates(q_prev_m1)); % in q_prev vector ind1 element is one step smaller than in original q_prev
                Fn2 = mat_to_coord_quat(get_coordinates(q_prev_p1)); % in q_prev vector ind1 element is one step bigger than in original q_prev
                deltaF = Fn2 - Fn1; %delta functions vector
                deltaF = deltaF/(2*step); %devide by step to get partial derivatives for every function
                %composing Jacobian from column of partian derivatives
                J(:,ind2) = deltaF;
        end;
        %Next according to damped least squares method
        %calculate velosities along all coordinates
            A_forward = get_coordinates( q_prev );
            differ = mat_to_coord_quat(Dest) - mat_to_coord_quat(A_forward);

            %calculating generalized coordinates velosities
            dq = (J.'*J + lambda * eye(6))\ J.' * differ;

            %integrate generalized coordinates velosities
            q_prev = q_prev + dq;

            %calculate max orientation error
            error = max(abs(differ(4:7)));
    end;
end;
</code>```

答:
If you have 6 DOF in your joints, and you want a particular 6 DOF pose, and you have some kind of preferred movement envelope, then there is at most one solution. There is also a closed-form solution to this that you can calculate. Typically, you'll want to design your armature to make this solution easy.

If you have a known desired XYW position and UVW orientation of the end effector, and each axis of rotation is individually controlled, then you can start by posing the endmost three axes (presumably with short translations) to get you to an "ideal plane" for positioning, and then post the other three axes (presumably with longer translations) to get to the point of wherever the third-outmost actuator ended up when getting orientation right.

Another option, which is useful when you have a "desired shape" and "desired end goal" incrementally from the current position, is to simply minimize the amount of joint movement (or some other penalty function, such as desired shape) to move between the current state and the desired end state. As long as the end state varies smoothly, the incremental movement between each solution step is small, and the solution converges quickly.

That being said, 1 minutes to solve a 6DOF system sounds wrong. Either you're running into numerical problems (denormalized numbers?) or you're implementing the solution wrong, or you're running on some piece of hardware that is not suitable for the task (like, an 8-bit microcontroller without floating point support.)


问:
unstable quad with naza m lite
I almost finished my first quad and first time it just flipped(the back over the front 360 degrees)...the motors and  props are installed correctly..i triple checked but i'm still a  noob in this...my quad specs are q330 frame with naza m lite (with gps),hobbywing 30a esc,rs 2205 motors ,flysky fs i6 with ia6b,3s battery ... maybe someone can help with my problem...i don't want to loose to many props or even worse...thanks</p>

答:
There are multiple possibilities for the flip of quadrotor.

It can be because of incorrect prop directions (in X or + configuration, the diagonally opposite props must have same rotational direction and adjacent props must have opposite rotational direction; This is all about compensation of angular momentum).

It can be because of a faulty ESC or improperly tuned ESC. There are several ways to tune ESCs properly. (Like this: https://librepilot.atlassian.net/wiki/display/LPDOC/ESC+Calibration )

It can be an issue of PID tuning. (http://myfirstdrone.com/tutorials/how-to-tune-a-quadcopter/ https://oscarliang.com/quadcopter-pid-explained-tuning/ )

IMU may not be calibrated properly and the EKF estimates may be wrong. (https://fpv-flightclub.com/quadcopter-flips-takeoff/ Accelerometer and Gyro orientation along with Compass need to be properly set up)

(Rare) GPS functionality might be malfunctioning ending that up with GPS locks for inaccurate location estimates.

BTW, there are many forums that address this issue and simple Google search is ending up with results like:

https://oscarliang.com/quadcopter-flies-unstable-flips-troubleshoot/

http://diydrones.com/forum/topics/quadcopter-flips-on-liftoff

https://www.rcgroups.com/forums/showthread.php?2284434-My-x-quadcopter-flips-on-takeoff

Please consider mentioning what you have tried and what hasn't worked (otherwise this question is 'too broad' with many Google results popping up for same).

Title: unstable quad with naza m lite


问:
Dji Wookong-M - To unstable to take off
I've built a quadcopter using the Dji Wookong-M. As of a couple of weeks ago I have been able to get everything to work except for one small thing. When I throttle up the Drone tends to flip to the side. I have tested all the motors over and over again and I know that they are spinning the right direction and that I have the right props on the right motors. I tested on both grass and concrete but both times it flipped. It starts to flip once the throttle is past 50%. I don't know if it is catching or if something is off balance although I don't think this is the problem since the quadcopter tips different directions almost every time. If any one could tell me what is wrong I would appreciate it a lot since my project is due in 2 1/2 weeks.

Thanks in Advance

答:
Referencing my answer from https://robotics.stackexchange.com/a/12086/15014 ,

There are multiple possibilities for the flip of quadrotor.

It can be because of incorrect prop directions (in X or + configuration, the diagonally opposite props must have same rotational direction and adjacent props must have opposite rotational direction; This is all about compensation of angular momentum).

It can be because of a faulty ESC or improperly tuned ESC. There are several ways to tune ESCs properly. (Like this: https://librepilot.atlassian.net/wiki/display/LPDOC/ESC+Calibration )

It can be an issue of PID tuning. (http://myfirstdrone.com/tutorials/how-to-tune-a-quadcopter/ https://oscarliang.com/quadcopter-pid-explained-tuning/ )

IMU may not be calibrated properly and the EKF estimates may be wrong. (https://fpv-flightclub.com/quadcopter-flips-takeoff/ Accelerometer and Gyro orientation along with Compass need to be properly set up)

(Rare) GPS functionality might be malfunctioning ending that up with GPS locks for inaccurate location estimates.

BTW, there are many forums that address this issue and simple Google search is ending up with results like:

https://oscarliang.com/quadcopter-flies-unstable-flips-troubleshoot/

http://diydrones.com/forum/topics/quadcopter-flips-on-liftoff

https://www.rcgroups.com/forums/showthread.php?2284434-My-x-quadcopter-flips-on-takeoff

Please consider mentioning what you have tried and what hasn't worked (otherwise this question is 'too broad' with many Google results popping up for same).

+1 for @Chuck 's comment:

Without any data, anyone that answers will be guessing at what could
  be wrong. Can you provide any data? Pictures of your quadcopter, your
  test setup, data from the flight controller, etc.

Title: Dji Wookong-M - To unstable to take off


问:
Stereo camera vs. one camera for player detection & ball tracking in basketball scenes?
There are a number of similar questions such as Monocular vs. stereo computer vision robustness for object detection, but none that address my question specifically.

My weekend project is to build a little robot that can detect and track players and shots made in a basketball game.

Detection and tracking need not be real-time (though it would be ideal).

The goal is to understand if a stereo camera would help improve speed and accuracy of tracking players and shots made, or if a solo camera is sufficient.

Would the depth information of a stereo camera simplify the task? Or would a solo camera, because of assumptions you can make about the basketball scene, be equally as accurate (and therefore preferable since less hardware is required)?

Assume the camera must track activity at both baskets, and is 100 feet from the furthest basket (i.e., at the other end of the court).

Specific Questions:

Could a stereo system let you more quickly detect human bodies and basketballs (i.e., spheres with ~9" diameter) because you could detect volumetric shapes whereas you can't with one camera?

Could shot detection be more accurate and faster because you can measure depth of the ball (i.e., only trigger analysis when ball is around same depth as hoop)?

Would hoop detection be easier because of depth information?

Obviously, stereo cameras require higher computational load at a nominal level, but could algorithm simplifications (e.g., ignore non-spheres for ball detection) allowed by depth information actually reduce overall computational load?

Argument for solo camera: since the robot only operates against basketball scenes, you can make assumptions like there will be at least one 10-foot basketball hoop. Since you know the height of the hoop, would that allow you to perform depth measurements as if you had a stereo camera?

The paper "Real-Time Tracking of Multiple People Using Continuous Detection" by David Beymer and Kurt Konolige suggests a stereo camera would offer advantages over a solo camera, confirming some of the hypotheses here, but the paper is also very old (1999). Is player & shot tracking better with a stereo camera, or are solo cameras equally as effective?

答:
100 feet seems too far given the occlusions (like player's hands masking or player's body covering the ball while dribbling). So, I would suggest multiple cameras with linear Kalman tracking (According to my experience this works really well when the problem involves practical limits; Like humans' running velocity is fairly predictable; Dribbling rate is predictable etc) and then adding sophisticated trackers like deep-nets if necessary.

If two cameras are used near two hoops, programming will be significantly simplified regarding the scoring. However, for tracking (3D positional information of the ball), we have two sources of information: Players' movements and ball's movement. We can use multiple cameras like this https://pdfs.semanticscholar.org/fb69/2c04d7720a68c49efb1cc4608b6fc90e4ca0.pdf and detect people like this http://onlinepresent.org/proceedings/vol16_2012/6.pdf .

We can use detection and tracking of people (with second Kalman filter and other sophisticated algorithms can be added later if necessary) for two reasons:

We can detect all the elements on the ground and subtract people; This gives us ball in most cases.

We can use conditional state-estimate probability to figure out the probability of the states of the ball when it is occluded. (For example, players who are near the ball are generally frenzied and they tend to be crowded near the ball and they will generally be running at a faster rate)

+1 for awesome question.

EDIT: 100 feet with stereo vision is difficult; I am referencing my previous answer here https://robotics.stackexchange.com/a/11856/15014

(Basically, off-the-shelf cameras do not provide such information and associative learning can give out some information for upto 50 metre. So, there must be a great training information as the machine 'learns' just that)


答:
There are a number of way to integrate code written in Matlab to a number of platforms. For a robot arm, you would most likely need a processor with a floating point unit.

Probably the most cost effective solution is to use a Raspberry Pi. It is officially supported as a platform by Matlab (Simulink), so you may be able to transfer matlab code directly to the Pi (please look up the limitations here)

Arduino is also supported by Matlab (however computational performance is more limited then the rPi).

If you want to use different chips, Matlab can generare C/C++ code from Matlab or Simulink applications. It require some coding knowledge, but you can incorporate these to an IDE using the same language and use them even if Matlab does not officially support the integration to that IDE.

Title: what controller do we use in robots that has forward and inverse kinematics?

Body:
i used ATmega328p chip to make a car, but its the car is always controlled by me.
my question is in robotic arms , the arm moves using the kinematics that i put, what chip and programming language should i use? i know of matlab which works with matrices, but what chip works with it?</p>


问:
Detect road surface in a traffic scene point cloud
I want to analyze a traffic scene. My source data is a point cloud like this one (see images at the bottom of that post). I want to be able to detect objects that are on the road (cars, cyclists etc.). So first of all I need know where the road surface is so that I can remove or ignore these points or simply just run a detection above the surface level.

What are the ways to detect such road surface? The easiest scenario is a straight and flat road - I guess I could try to registrate a simple plane to the approximate position of the surface (I quite surely know it begins just in front of the car) and because the road surface is not a perfect plane I have to allow some tolerance around the plane.

More difficult scenario would be a curvy and wavy (undulated?) road surface that would form some kind of a 3D curve... I will appreciate any inputs.

答:
There are couple of ways to detect road surface. One is to use planar gradients (like linear gradients in 3D) as mentioned in @ArminMeisterhirn 's answer. (http://www.pointclouds.org/assets/uploads/3DRP-PCL14_Bellone.pdf)

Other one is to use associative learning; We can identify objects in image (like cars, roads etc some hints can be found in this video https://www.youtube.com/watch?v=j8zj5lBpFTY ) and use corresponding point cloud data and train the second stage with corresponding 3D objects. (Like using CNN for object classification/ training as per ImageNet) (Example attempt: https://people.csail.mit.edu/fisher/publications/papers/mastin09cvpr.pdf)

I prefer second option and usage of AI. There is a database containing both images and other sensors' data timestamped by software automatically as available http://grandchallenge.mit.edu/wiki/index.php?title=PublicData .


问:
How to check lane departure in OpenCV?
I'm using OpenCV 3 in Python 2.7 on a Raspberry Pi 3. My project's aim is to build an autonomous lane departing robot that can detect the two lanes on its sides and continuously correct itself to remain within them. I want to achieve something like this project: https://www.youtube.com/watch?v=R_5XhnmDNz4

So far I've done the line detection part from the live video feed using both HoughLines and HoughLinesP. Here is a screenshot from my video feed and the outputs I'm getting so far:

Till now my logic for detecting if the robot is going left or right is based on the (rho,theta) output of the HoughLines function. What I want to achieve is a more robust way of tracking how the robot is departing from the lanes. Some sort of central line marker that can be used to detect if the robot has moved away from the center. I'm still new to OpenCV and python and the part where I'm stuck at is converting the logic of detecting the lane departure of the robot.

My understanding is that averaging the lines on the lanes into two lines (left and right lanes) and then working with their slopes should give some result. However, I've not been able to transform this into code. I'd appreciate any suggestions on ways to detect lane departure of the robot. Thanks!! :)

答:
One of the best ways is to use a combination of hough-transform (what you have achieved is nice) and inverse perspective transform (as in http://www.vision.caltech.edu/malaa/publications/aly08realtime.pdf). This is because, inverse perspective transform fails at 'very near' and 'very far' distances and hough transform can be compensated for that.

Now, once we get parallel lines, we can use that information for localisation; if one of the two edges is missing, then, it means that the robot is not on lane most probably; We can figure out mid-line (average of two edges after inverse perspective transform and filtered by hough-line fusion) and use an evaluation function to figure out how distant the robot is from the centre of the lane.

This solution definitely fails in cases of intersection and parallel lanes (one lane next to another and both lanes are detected simultaneously). In such cases, some sort of mapping or path planning is mandatory (with costs associated for change of lane and wrong routes at intersections). Thus, this solution only applies locally.

For some more learning, auto-encoders can be used for steering angle estimation (as in https://arxiv.org/pdf/1608.01230.pdf ; But, too much blurring can mean simple averaging and very precise auto-encoding reproduction means over-fitting; Dataset can be found here https://github.com/commaai/research).

Title: How to check lane departure in OpenCV?


答:
Firstly, I have to say that all the problems was because of my inattentiveness. Nothing was wrong with rotating $O_{1}O_{2}$ by $q_{1}$. The problem was about rotating first joint about global coordinates $Z$ axis instead of its' axis. Therefore I got an error between calculation and position in my modelling environment.

Several times I was told that I have not to include $q$ in the translation part. Not only here but also in some other sources. However it is not true and I wonder why so many people have told me about that. Let us look at what actually happens here. For simplicity we will look at the planar case.

Assume initial configuration like pictured below:

Let's describe two transformation matrices: one which includes $q_{1}$ in translation part and one that does not.

$
T_{01}=\begin{pmatrix}
cos(q_{1}) & -sin(q_{1}) & a_{1}\\
sin(q_{1}) & cos(q_{1}) & 0\\
0 & 0 & 1
\end{pmatrix}\\
T_{01}'=\begin{pmatrix}
cos(q_{1}) & -sin(q_{1}) & a_{1}cos(q_{1})\\
sin(q_{1}) & cos(q_{1}) & a_{1}sin(q_{1})\\
0 & 0 & 1
\end{pmatrix}\\
$

Vector $\vec{a_{2}}$ in $O_{1}$ frame is $\vec{a_{2}} = \begin{pmatrix}
x_{0}'\\y_{0}'\\1\end{pmatrix}$

Now let's rotate our $\vec{a_{1}}$ by some angle $q_{1}$

What coordinates in origin frame vector $\vec{a_{2}}$ has? Geometrically easy to find that $\vec{a_{2}^{0}} = \begin{pmatrix}
cos(q_{1})x_{0}'-sin(q_{1})y_{0}'+a_{1}cos(q_{1})\\
sin(q_{1})x_{0}'+cos(q_{1})y_{0}'+a_{1}sin(q_{1})\\
1
\end{pmatrix}
$

Now let's multiply our transformation matrices by $a_{2}$ and we should get the same result.

$
T_{01}\vec{a_{2}}=\begin{pmatrix}
cos(q_{1})x_{0}'-sin(q_{1})y_{0}'+a_{1}\\
sin(q_{1})x_{0}'+cos(q_{1})y_{0}'\\
1
\end{pmatrix}\\\\
T_{01}'\vec{a_{2}}=\begin{pmatrix}
cos(q_{1})x_{0}'-sin(q_{1})y_{0}'+a_{1}cos(q_{1})\\
sin(q_{1})x_{0}'+cos(q_{1})y_{0}'+a_{1}sin(q_{1})\\
1
\end{pmatrix}
$

And you see the difference: first transformation does not consider $O_{1}$ coordinate changes introduced by $q_{1}$ and it shifts our vector by the whole $a_{1}$ along $X$ and by 0 along $Y$. Therefore IF next joint coordinates are changed by current joint angle, you have to find projections in your translation part or if you know initial vector between their origins, you can rotate it by $q_{1}$. However the latter approach is more computationally expensive. Otherwise if coordinates are not going to be changed, you can throw out unnecessary calculations.

Title: Forward kinematics equations

Body:
I'm stuck at computing forward kinematics equations.

I have configuration of the first two joints like on the following image:

Transformation from the origin to the first joint basis is trivial: just translation by $\vec{OO_{1}}$.

The second transform from joint 1 to joint 2 basis makes me nervous throughout this day. First of all it is a rotation around $Z$ axis. So rotational part will look like this:

$
R_{12}=
\begin{pmatrix}
cos(q_{1}) & -sin(q_{1}) & 0\\
sin(q_{1}) & cos(q_{1}) & 0\\
0 & 0 & 1
\end{pmatrix}
$

Problems are all about the translation part. I see two approaches here.
Since angle between $\vec{O_{1}O_{2}}$ and plane $X_{1}O_{1}Y_{1}$ is constant because rotation is performed around $Z$ axis, length of projection of $\vec{O_{1}O_{2}}$ onto $X_{1}O_{1}Y_{1}$ is constant. Here it is:

$\vec{v} = O_{2} - O_{1} = \begin{pmatrix} v_{x}\\ v_{y}\\ v_{z} \end{pmatrix}
$
Its' projection onto $X_{1}O_{1}Y_{1}$ is $\vec{v_{p}} = \begin{pmatrix} v_{x}\\ v_{y}\\ 0 \end{pmatrix}$ and it's magnitude is $m=\sqrt{v_{x}^2 + v_{y}^2}=const$.

Now let's look at what happens after rotation:

So the translation matrix looks like:
$
S_{12}=
\begin{pmatrix}
m\cdot cos(\alpha+q_{1})\\ m\cdot sin(\alpha+q_{1})\\ v_{z}
\end{pmatrix}
$

And full transformation matrix from joint 1 to joint 2 basis is:

$
T_{12}=
\begin{pmatrix}
R_{12} & S_{12}\\
0 & 1
\end{pmatrix}
$

Unfortunately it gives me wrong results even when $q_{1}=0$. Can not see where my reasoning is wrong.

Second approach is more straightforward. Being able to calculate $\vec{O_{1}O_{2}}$ in initial configuration makes it possible just to rotate this vector by $q_{1}$ around $Z$ axis and this has to be our translation vector. Nevertheless I can't make it work.

$R_{z}=
\begin{pmatrix}
cos(q_{1}) & -sin(q_{1}) & 0\\
sin(q_{1}) & cos(q_{1}) & 0\\
0 & 0 & 1
\end{pmatrix}\\
\vec{v} = O_{2} - O_{1} = \begin{pmatrix} v_{x}\\ v_{y}\\ v_{z} \end{pmatrix}\\
R_{z}\vec{v}=
\begin{pmatrix}
v_{x}cos(q_{1})-v_{y}sin(q_{1})\\
v_{x}sin(q_{1})+v_{y}cos(q_{1})\\
v_{z}
\end{pmatrix}\\
T_{12}=
\begin{pmatrix}
R_{12} & R_{z}\vec{v}\\
0 & 1
\end{pmatrix}
$

It works until I rotate the first joint(i.e. only when $q_{1}=0$).

Under works I mean "calculates position of joint 2 origin right". This is done by multiplying transformation matrix $T_{02} = T_{01}T_{12}$ by $\begin{pmatrix} 0 & 0 & 0 & 1\end{pmatrix}^{T}$


问:
For the second Paden-Kahan Sub-problem, how is 'r' determined?
When using Paden-Kahan Sub-problems to solve the inverse kinematics of manipulators, 'r' is described as the intersection point between the first and second twist axes. But how is this r actually found?

Referencing Murray (here), on page 122.

答:
The subproblem starts with the assumption that $\xi_1$ (axis one) and $\xi_2$ (axis two) intersect.

Each axis has a unit normal, $\omega_1$ and $\omega_2$, that points in the direction of their respective axes.

You are told that the two axes intersect, but you can verify that for yourself by checking that the cross product of the two axes is nonzero.

If $\omega_1 \times \omega_2 \neq 0$, then there is an intersection.

From there, you can find the intersection of the two axes using an intersection test.


问:
What is the difference between 4-point and 8-point connectivity in graph based planning?
In graph-based planning (say, A*), states are connected to their neighbors.  How should one decide whether to connect to the 4 neighbors or the 8 neighbors?  What are the pros and cons of each approach?</p>

答:
Assuming that (1) you are representing your environment as a grid (which is the only case where the 4-neighbor vs. 8-neighbor question makes sense) and (2) you are planning for navigation (which is the most common use case for grid-based representations)

I personally would not make much a case for 4-neighbour makes not much sense. What you would do by connecting only 4 neighbour per cell is force the robot to go only in "straight" directions. It is already a problem as it is with 8 neighbours, as one usually already has to "smooth" the result or use other approaches to interpret the resulting path.


问:
Angular Velocity from dual tri axial accelerometers
Can anyone throw some light on using accelerometers to measure  angular acceleration and hence angular velocity. This approach is to avoid gyroscopes due to drifting errors. Any links for this also would be very helpful.

Thank you.

答: Gyros measure angular velocity without the insane drift that comes from integrating accelerometer data. Integrating accelerometers for velocity or position is not a good idea because any noise gets integrated. In theory if the noise was perfectly random this wouldn't be a problem but the noise almost never is.</p>


问:
Convert Twist from frame B to frame A
(full disclosure: this is homework)

I have a twist expressed in frame B:

$\zeta_b = \begin{bmatrix}1\\3\\-2\\0\\-2\\4\end{bmatrix}$

And a general transformation matrix:

$g_{ab} = \begin{bmatrix}-0.4749 & 0.8160 & 0.3294 & -1.5\\-0.2261 & -0.4749 & -0.8505 & -1\\-0.8505 & -0.3294 & 0.4100 & 2\\0 & 0 & 0 & 1\end{bmatrix}$

How would I go about converting my twist into frame A?

I suspect I would break $\zeta_b$ into its component $\omega$ and $v$ vectors using the knowledge that:

$\zeta = \begin{bmatrix}v \\ \omega\end{bmatrix} = \begin{bmatrix}-\omega \times q \\ \omega\end{bmatrix}$ (where $q$ is a point on $\omega$)

But I am unsure.

答:
Hint: First, write the transformation matrix as
$$
T = \begin{bmatrix}
R &p\\0_{1\times3} &1
\end{bmatrix}.
$$

Now we use the relations $\omega_a = R\omega_b$ and $q_a = Rq_b + p$. Then since $v_a = -\omega_a \times q_a$, we can derive $v_a$ in terms of all the known quantities.

In fact, two twists representing the same screw motion described in different reference frames are related to each other by an adjoint map. That is given $S_a = (\omega_a, v_a)$ and $S_b = (\omega_b, v_b)$, we have
$$
S_a = [Ad_{T_{ab}}](S_b)\\
S_b = [Ad_{T_{ba}}](S_a),
$$
where $[Ad_T] = \begin{bmatrix}
R & 0\\
[p]R & R
\end{bmatrix}$ and $[p]$ is the skew-symmetric matrix constructed from $p$.

I found the material of this edX course really helpful for me to understand this topic. Maybe you want to check it out.

Title: Convert Twist from frame B to frame A


问:
Is it possible that a quadcopter hold height for long time stable only using IMU (without GPS) and barometer?
As we know, we can calculate altitude from barometer readings, and a UAV can hold height by referring to these data. However the real air pressure is varying with many conditions. These variance will cause the UAV's height unstable. Is it possible to avoid height drifting due to pressure changes using sensor fusion without GPS?</p>

答: You would probably get better altitude hold using an IMU barometer sensor fusion scheme because IMU is accurate short term and you can prevent drift with the barometric reading. Also the barometric pressure doesn't change that much if you temperature compensate.</p>


问:
How path following works analytically for way points?
First of all sorry for the confusing question title. I am also confused about the concept.

I have implemented a quadcopter and its controller. Controller finds the rotor speeds based on the position and yaw angle references.

The thing that I don't understand is, let's say I want the vehicle to climb 5m up and then 5m left. At this point, I think I need to create a vector containing the reference values. By the way, the model is discretized by some deltaT time interval so the reference vector. This does not coincide and well behave according to the dynamics of the vehicle. Let's say, the reference input for altitude is 5m until 5sec and 0 for [5,10]sec. But it is not guaranteed that the vehicle will reach to 5m altitude in 5sec. Thus, my intention is, that reference vector shouldn't rely on the time. Therefore, my perception is to use some if condition to check if the vehicle is reached for the first waypoint and then, register the next one.

Which is a simple if-else statement? This makes me think what is the mathematical or analytical background in this. Is it just the following of the line between two waypoints by geometrical analysis like Line of Sight guidance law things.

Can you give me some insight about the concept which makes me confused?

答: Most people just create a queue of waypoints on the path and go through one at a time. There are more complex path execution schemes that are more continuous but your application I doubt this is required. If your interested in looking for more complex schemes you can use B-Splines(https://en.wikipedia.org/wiki/B-spline) between waypoints. You hit the nail on the head when you brought up that you might not be at the target location when you think you will be there. Usually people get around that by having a good controller and recomputing the path as needed.</p>


问:
Inverse dynamics for robotic arm using Matlab Simmechanics
I am learning Simmechanics Matlab to do inverse dynamics for 4 DOF robotic arm. I read many examples to input motion to revolute joints like through PID, slider gain, sine waves, signal Builder etc. But these are not fulfilling my purpose as I have to rotate angles within limits and automatically. For example when I used sine wave signal, it continuously rotate until simulation time is not over. So, basically what I have is angles to rotate (through inverse kinematics) and now I want to find out torque required to reach that pose. How I can do this? How to create signal which fit in this scenario.

Thanks.

答:
You can use Simscape Multibody (the new name for what was SimMechanics) as a two-step process.

Joints in the package generally allow for one of two inputs to be used - force/torque or position. If you choose to provide joint torque, then the resulting output motion is calculated by the dynamics of the system. If you choose to provide joint position, then Simscape calculates the force/torque required to achieve that motion, then applies that to the system dynamics.

The process I would use for designing would go something like this:

Create a motion profile that you deem is representative of the tasks the robot is supposed to perform.

Calculate the inverse kinematics to get the required joint angles to achieve the motion profile.

Use the joint sensors 50k4 mentioned to trend/plot the joint forces and torques required to achieve your motion profile.

Choose the peak force/torque for each axis of motion, add some performance margin, and choose actuators that at least provide that performance.

Generate a model for the actuators you selected. This model includes whatever actuator dynamics there are along with whatever input conditioning you're expecting to use. For example, how are you expecting to have the actuators in real life track joint angle? PID control of angle error? That's what you model.

Replace your ideal multibody joints with your detailed actuator models.

Re-run your simulation and verify you're getting the performance you want.

Important to note - try to model weight and bulk as accurately as you can. Masses and inertias can, in some instances, have a very dramatic and negative impact on performance.


问:
Supplying power from a power bank to an arduino and DC Motors
I have a two port powerbank capable of supplying a maximum of 5V 2.1A

I'm using it to power an Arduino and a L293D IC connected to two DC motors which have the following specifications:

Working voltage : 3V to 9V

No-load current = 60 mA, Stall current = 700 mA

The setup is not working and I have made the following observations

1) The voltage across the output terminals of the powerbank is read as 4.9V (when the arduino is not powered either from the other port of the power bank or another power supply all together)

2) The voltage across the output terminals of the powerbank is read as 4.0V when the Arduino is powered either from the other port of the power bank or my laptop

This voltage is given to the L293D Pin 8 (That is meant to be given 12V). A 12V to 5V buck provides the 5V to the IC itself (this is part of the motor driver board)

3) When only one motor is switched on, the voltage provided to that motor is 2.8V. The motor rotates only when given some manual force on the axle.

4) When both motors are switched on, the voltage provided to each motor is around 0.5V. Both the motors don't rotate at all.

5) When only one motor is switched on, the voltage reads around 0.2V until given manually rotating the axle after which it picks up speed.

I couldn't measure the current (or rather measured current to be 0A) anywhere as the motors don't rotate at all if I connect the ammeter. I understand that the power bank supplies power only when the load connected to it demands the power (due to smart sensing).

What should I do so that the power bank continually provides 5V and this 5V is delivered to each motor?

答:
Are you sure the problem is about smart sensing ?

You should look about your 12V to 5V converter. Most of these devices require a minimum voltage above the desired voltage.

Typically, many 5V regulators requires 7V input.

Also, is this regulator necessary ? Since your powerbank supply 5V, you don't need to lower the voltage.


答:
A powerbank is a battery. It sounds like your battery is not charged. When was the last time you charged it? What happens when you try to charge it now?

I would suggest putting it on to charge for an hour at least and checking if performance is better or worse.

Title: Supplying power from a power bank to an arduino and DC Motors

Body:
I have a two port powerbank capable of supplying a maximum of 5V 2.1A

I'm using it to power an Arduino and a L293D IC connected to two DC motors which have the following specifications:

Working voltage : 3V to 9V

No-load current = 60 mA, Stall current = 700 mA

The setup is not working and I have made the following observations

1) The voltage across the output terminals of the powerbank is read as 4.9V (when the arduino is not powered either from the other port of the power bank or another power supply all together)

2) The voltage across the output terminals of the powerbank is read as 4.0V when the Arduino is powered either from the other port of the power bank or my laptop

This voltage is given to the L293D Pin 8 (That is meant to be given 12V). A 12V to 5V buck provides the 5V to the IC itself (this is part of the motor driver board)

3) When only one motor is switched on, the voltage provided to that motor is 2.8V. The motor rotates only when given some manual force on the axle.

4) When both motors are switched on, the voltage provided to each motor is around 0.5V. Both the motors don't rotate at all.

5) When only one motor is switched on, the voltage reads around 0.2V until given manually rotating the axle after which it picks up speed.

I couldn't measure the current (or rather measured current to be 0A) anywhere as the motors don't rotate at all if I connect the ammeter. I understand that the power bank supplies power only when the load connected to it demands the power (due to smart sensing).

What should I do so that the power bank continually provides 5V and this 5V is delivered to each motor?


答:
You have a waypoint, which is some $<x,y,z>_\mbox{waypoint}$ coordinate.

You should have some internal position estimate, $<x,y,z>_\mbox{quadcopter}$.

If the waypoint coordinates are $x_w,y_w,z_w$, and the quadcopter coordinates are $x_q,y_q,z_q$, then you can calculate your distance to the waypoint as:

$$
r = \sqrt{(x_q-x_w)^2 + (y_q-y_w)^2 + (z_q-z_w)^2}\\
$$

Now you have a singular value that indicates how close you are to the waypoint. You can also check speeds by looking at the quadcopter's speed on each axis -

$$
\dot{s} = \sqrt{\dot{x}_q^2 + \dot{y}_q^2 + \dot{z}_q^2} \\
$$

You can check now that you're at target along two criteria. For example, maybe your position tracking has some degree of overshoot, and you'd like to be sure that the quadcopter has stabilized at the waypoint before continuing on to the next point. Maybe the first waypoint is at the entrance to a duct or pipe, and the second waypoint is in the duct/pipe.

So, then you'd want to make sure that your distance to the waypoint, $r$, is small, and that your vehicle speed $\dot{s}$ is also small. If your position error $r$ is big but your speed $\dot{s}$ is small, then maybe you're at the previous waypoint and about to start moving.

If your position error $r$ is small but your speed $\dot{s}$ is big, then you're close to your waypoint but you're moving quickly and are about to overshoot it. You might not care, or you might care a lot. It depends on the type of maneuver you're trying to complete and the allowable deviation from your waypoint-to-waypoint path is. If you overshoot the first waypoint by a lot and then immediately proceed to the second, then you're not actually taking the desired path to the second waypoint.

Last piece of advice I could offer is to make sure that your waypoint coordinates and quadcopter coordinates are in the same frame of reference. You wouldn't want a quadcopter in real GPS coordinates trying to track a relative waypoint coordinate - your quadcopter might autonomously take off for the intersection of the prime meridian and the equator.

Title: How path following works analytically for way points?

Body:
First of all sorry for the confusing question title. I am also confused about the concept.

I have implemented a quadcopter and its controller. Controller finds the rotor speeds based on the position and yaw angle references.

The thing that I don't understand is, let's say I want the vehicle to climb 5m up and then 5m left. At this point, I think I need to create a vector containing the reference values. By the way, the model is discretized by some deltaT time interval so the reference vector. This does not coincide and well behave according to the dynamics of the vehicle. Let's say, the reference input for altitude is 5m until 5sec and 0 for [5,10]sec. But it is not guaranteed that the vehicle will reach to 5m altitude in 5sec. Thus, my intention is, that reference vector shouldn't rely on the time. Therefore, my perception is to use some if condition to check if the vehicle is reached for the first waypoint and then, register the next one.

Which is a simple if-else statement? This makes me think what is the mathematical or analytical background in this. Is it just the following of the line between two waypoints by geometrical analysis like Line of Sight guidance law things.

Can you give me some insight about the concept which makes me confused?


答:
You could not prevent long term barometer offset just by using the barometer and IMU, as both are accurate in the fairly short term but both are susceptible to drifting over time, as pressures change over the course of the day or error builds in your IMU position solution.

Over a Baro only solution a Baro + IMU will perform better, but again will not provide long term stability.

If you are looking for a long term solution I would suggest an Ultrasonic sensor or 1D LIDAR sensor to provide an absolute altitude reading.

Title: Is it possible that a quadcopter hold height for long time stable only using IMU (without GPS) and barometer?

Body: As we know, we can calculate altitude from barometer readings, and a UAV can hold height by referring to these data. However the real air pressure is varying with many conditions. These variance will cause the UAV's height unstable. Is it possible to avoid height drifting due to pressure changes using sensor fusion without GPS?</p>


问:
Robot Graphical Representation in Real Time
I'm working with a robot intended to be placed in a tele-echography environment. To control the robot I'm using a 6D space mouse that control each degree of freedom of the robot. However, since the rotation is made in the end effector, the end user would have difficulties in understanding where to move the mouse in order to do the desired motion, since the end effector's reference is constantly changing.

So, I'm thinking of doing a graphical representation of the motion of the robot in real time while the user controls the robot. The robot comes andith many API's to control it and to get sensor data. I'm currently using Qt Creator (C/C++) in order to send the mouse's commands to the robot, so I would like to integrate in my program some kind of simulator. What do you recommend as a C++ package / program in order to accomplish this?

Thanks

答:
If you are already using Qt, then Q3 3D would be an obviuos choice for 3D representations. Gaming engines like Unity (C#/JavaScript) or the Unreal Engine (C++) are also a suitable choice for representing robots in 3D. You will find plenty of exmples like this.

Using directly OpenGL is also an option, but if you want anything else then just simple 3D rerpesentations, probably a gaming engine would be a better option.


问:
What kind of sensor do I need for knowing that something is placed at a position?
I have a small device that's picking up small rocks from a pile and moving them to another place. Its a kind of crude way of trying to push the whole pile onto a bigger gear and hoping one of them is pushed to one of the spaces between gears and taken around and falls off on the other side of the spinning gear. Here I want to know if the machine successfully got a rock here, if not it should spin the gear until it turns up a single rock on the other side of it. If a rock is present at the spot, the gear should stop spinning until the rock is taken care of by the rest of the machine.

What kind of device can I use to sense if I successfully succeeded in getting a rock on the other side of the gear?

This is just a part of a bigger system. To sum up, I need the sensor to signal when a rock is signaled out and separated from the rest so it can continue work on that single rock.

I am building this using an Arduino to move the gear around, so the sensor needs to be something that can be controlled by an Arduino.

答:
Try an IR sender and receiver, facing each other, and the connection is obstructed by the rock.

For example:


问:
What's the name of a robot manipulator construction with multiple thin arms joined together?
I am seeing on some videos robots picking items and putting them somewhere in order.

Here are some examples:

https://www.youtube.com/watch?v=wg8YYuLLoM0.

https://youtu.be/ggFdvUlp8YU?t=38

How are these types of manipulators called and where can I find schematic illustration explaining principles of its work?

答:
They are parallel manipulators.

You may find chapters on parallel manipulators in robotics textbooks, for example, A Mathematical Introduction to
Robotic Manipulation, useful. There are also a number of articles related to kinematics analysis of those manipulators (such as this article).


问:
What mechanical parts can be attached to a 28BYJ-48 stepper motor?
Sorry I am asking a mechanical question here, but, after all, where else people have experience with using motors? If there is a better forum for this, please do guide me.

Everywhere I've seen online, the stepper motor 28BYJ-48 is used in tutorials, to rotate on its own, or, at most, to spin a clothes pin attached to it. I am trying to get Arduino to work for my 10 year old kid. He's got the motor rotating, now what? How does he attach anything to it?

Don't laugh, I made him a wheel out of a raw potato. He is happy with it now. Where can I find any guidance as to what to do next?

答: It is amazing that there are so few solutions for power transfer for such a common motor. I have looked at ebay and amazon. Gears are available in a wide range of prices, none that are in the same scale as the price of this cheap motor. There is a gear generator at http://woodgears.ca/gear/help3/index.html for wood gears. This or 3d printing gives you the flexibility of gear ratios and shaft sizes. I just wanted to be able to connect a programmable motor to a drive that would actually do something.  </p>

OwnerDisplayName: Таня Т.


问:
difference between sabertooth motor controller and rc esc?
i am building a rc/robot mower.

most of the youtube videos show the sabertooth motor controllers being used to connect the rc receiver to the dc motors.

But here in Australia, the sabertooth i need cost about 200 dollars. RC esc sell for about 10 dollars.

What is the difference between the esc and the sabertooth and can i use an esc instead?

the specs i have on the motor and battery are 12v 10amp normal, 35amp stall. and my rc is a flysky

答:
The Sabertooth Motor Controller is for driving DC motors, where most RC escs are for driving AC (brushless) motors. If the motor you have is a DC motor there are plenty of other motor drivers out there that can supply the 35A stall current, that you might find cheaper in your country.

I have listed some below

Victor

RoboClaw 2x45A

Cytron 40A


答:
There are numerous options that could/should work here.

As mentioned by Elias, an IR sender/receiver is a good choice. This is similar to a "break beam" sensor. Essentially, when the beam of light between the transmitter and receiver is broken, the controller knows to do something about it. Similar to this would be an IR distance sensor, which records the distance from the sensor to an object by way of the angle of the light reflections. The problem with any sort of sensor based around light (such as IR) is that it can be "corrupted" easily by ambient light: the sun, light bulbs, camera flash, etc. The way to get around this is to pulse the light, only looking for light of a specific frequency (this is how IR TV remotes work).

Example sensors: http://www.acroname.com/robotics/info/articles/sharp/sharp.html

There are also things such as ultrasonic rangefinders which use sound instead of light:
http://www.acroname.com/robotics/parts/R335-SRF06.html

At the most complicated level, you could have a camera mounted that detects when a rock has been moved.

Alternatively, you could use a physical sensor to tell when a rock has been moved. This can be as simple as a push button platform - the button is pushed down when a rock is placed on top of it. Of course, this would only work if the rocks weighed enough to counteract the spring inside of a push button switch.

As far as the type of sensor that can be used: Arduino is based around an AVR microcontroller. Microcontrollers can be used with pretty much any kind of sensor you can imagine, although some may be too fast to be handled by the slower clock of an micro-controller versus a micro-processor or require more processing power than is available.

Title: What kind of sensor do I need for knowing that something is placed at a position?

Body:
I have a small device that's picking up small rocks from a pile and moving them to another place. Its a kind of crude way of trying to push the whole pile onto a bigger gear and hoping one of them is pushed to one of the spaces between gears and taken around and falls off on the other side of the spinning gear. Here I want to know if the machine successfully got a rock here, if not it should spin the gear until it turns up a single rock on the other side of it. If a rock is present at the spot, the gear should stop spinning until the rock is taken care of by the rest of the machine.

What kind of device can I use to sense if I successfully succeeded in getting a rock on the other side of the gear?

This is just a part of a bigger system. To sum up, I need the sensor to signal when a rock is signaled out and separated from the rest so it can continue work on that single rock.

I am building this using an Arduino to move the gear around, so the sensor needs to be something that can be controlled by an Arduino.


问:
Robot Veering to the Left
I'm currently programming in RobotC, for a Vex 2.0 Cortex. I'm using encoders to make my robot go straight.

This is my code:

```<code>#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl2,  ,               sensorDigitalIn)
#pragma config(Sensor, dgtl7,  ,               sensorDigitalOut)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           RM,            tmotorVex393_HBridge, openLoop, reversed, encoderPort, I2C_2)
#pragma config(Motor,  port10,          LM,            tmotorVex393_HBridge, openLoop, encoderPort, I2C_1)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/* Port 1 is right motor*/

//all functions expect no reverse motors (in port menu)
//**GLOBAL VARIABLES**
int buttonSTATE = 0;

//**MOVE FUNCTIONS**
void goforwards(int time)
{
    int Tcount = 0;
    int speed1 = 30;
    int speed2 = 30;
    int difference = 5;

    motor[LM] = speed1;
    motor[RM] = speed2;
    while (Tcount < time)
    {
        nMotorEncoder[RM] = 0;
        nMotorEncoder[LM] = 0;

        while(nMotorEncoder[RM]<3000)
        {
            int REncoder = -nMotorEncoder[RM];
            int LEncoder = -nMotorEncoder[LM];

            if (LEncoder > REncoder)
            {
                motor[LM] = speed1 - difference;
                motor[RM] = speed2 + difference;
            }
            if (LEncoder < REncoder)
            {
                motor[LM] = speed1 + difference;
                motor[RM] = speed2 - difference;
            }
            wait1Msec(100);
        }
        Tcount ++;
    }
}

//**CONTROL STRUCTURE:**
task main()
{

    goforwards(1);
}
</code>```

When I execute the code, the Robot's encoder values are very close, but the robot quickly starts to veer to the left. What are possible causes of this? Is it something in the code?

答:
Lets suppose, that left wheel is blocked, so nMotorEncoder[LM] is constantly zero, while nMotorEncoder[RM] counts something. Correct behavior is for right motor to effectively stop (or jumping little back and forth the same lenght).

Now LEncoder is 0, REncoder is negative, LEncoder > REncoder so

```<code>            motor[LM] = speed1 - difference;
            motor[RM] = speed2 + difference;
</code>```

left motor slowes, right motor goes faster. This is not, what you want, in next step nMotorEncoder[RM] goes positive again (nMotorEncoder[LM] is still zero), right motor tries to move faster, while left motor tries to be even slower. Soon left motor is going backward full speed, right motor going forward full speed, no stabilizing.

Usually the motor are not totally equal (and usually they turn one way better, than the opposite way), so it is possible, that your left motor is a little slower/weaker, than right. Even if it is not much and can be compensated for, the way you are doing it is making it more and more visible.

Try to remove the minuses on

```<code>        int REncoder = -nMotorEncoder[RM];
        int LEncoder = -nMotorEncoder[LM];
</code>```

of change the direction of comparing LEncoder > REncoder

BTW: I am not sure, but you probably also should limit the maximal/minimal speed

Title: Robot Veering to the Left

Body:
Lets suppose, that left wheel is blocked, so nMotorEncoder[LM] is constantly zero, while nMotorEncoder[RM] counts something. Correct behavior is for right motor to effectively stop (or jumping little back and forth the same lenght).

Now LEncoder is 0, REncoder is negative, LEncoder > REncoder so

            motor[LM] = speed1 - difference;
            motor[RM] = speed2 + difference;

left motor slowes, right motor goes faster. This is not, what you want, in next step nMotorEncoder[RM] goes positive again (nMotorEncoder[LM] is still zero), right motor tries to move faster, while left motor tries to be even slower. Soon left motor is going backward full speed, right motor going forward full speed, no stabilizing.

Usually the motor are not totally equal (and usually they turn one way better, than the opposite way), so it is possible, that your left motor is a little slower/weaker, than right. Even if it is not much and can be compensated for, the way you are doing it is making it more and more visible.

Try to remove the minuses on

        int REncoder = -nMotorEncoder[RM];
        int LEncoder = -nMotorEncoder[LM];

of change the direction of comparing LEncoder > REncoder

BTW: I am not sure, but you probably also should limit the maximal/minimal speed


问:
How to control 6v micro solenoid from arduino
I want to control this solenoid from Arduino, but I am confused which transistors, resistors and diodes to choose. I have seen a lot of tutorials about controlling solenoid from Arduino, but all of them are for 12 volt solenoid or are using some relays which I don't want to use. I will be using 6 of these solenoids for my project.  </p>

答:
Most of 12V solenoid tutorials would work here too, if you use just 6V instead of 12V. Post here links to some 12V tutorial, I will look at that and say you, if they are easy to adjust and how.

Or as last resort - your selenoid is rated 6V/0.35A - that is 17.14 Ohm, you can buy power resistor 17-20 Ohm with minimal of 7 Watt (can be much more Watt, no problem - probably big ceramic one) and just connet it in serie with your solenoid and then follow ANY 12V solenoid tutorial verbatim (with this combination in place for 12V solenoid - as it is de-facto 12V / 0.35A solenoid together)


问:
Pose-graph-slam:using only a camera
I'd like to know how to form my pose graph if the only information I have available is that from a camera,

(1) What are my poses? Are they just the accumulated transformations from pairwise matching?

(2) And then what are my edges?

(3) If I have already detected the loops, how would I loop correct?

Thank you.

答:
(1) Usually, the poses are just your absolute camera poses, i.e $[R,T]$ where $R$ is a rotation matrix and $T$ is a translation, expressed in world coordinates. You can add a scale factor to that if you want.

(2) Usually, the edges are the relative displacements between your cameras. That is , if you have cameras $C_1=[R_1,T_1]$ and $C_2=[R_2,T_2]$, then, noting $X_w$ a point in world coordinates and $X_1$, $X_2$ its representations in (respectively) cameras $C_1$ and $C_2$, we will have

$X_w=R_1X_1+T_1$

$X_w=R_2X_2+T_2$

$\Rightarrow X_1=R_1^{t}R_2X_2+R_1^t(T_2-T_1)\triangleq R'X_2+T'.$

Now, your edges are just defined as the relative transformation $[R',T']$. Once again, you can add a scale factor.

(3)  Let $P_1,...,P_n$ be the relative poses computed between cameras $1,...n,n+1$ by your SLAM algorithm. Now, you detect a loop between cameras $n$ and $n+1$, and this says that your relative transformation should be $T_n$, not $P_n$ .  The difference between $T_n$ and $R_n$ is often important, and you want to propagate this large error to previous cameras. Thus, what you want to do is this: you want to change all absolute camera poses in a way that does not significantly change the relative poses $P_1,...,P_{n-1}$, all the while making the transformation between cameras $n$ and $n+1$ as close to $T_n$ as possible. Let us write $A_i$ the absolute pose of camera $i$, and assume that $A_i^{-1}A_j$ gives us the relative transformation between $i$ and $j$. Now, we can state our goal clearly:

$\{P_1,...,P_{n-1}\}\cup{T_n}   \ \ \ \text{these are our constants}$

$\{A_1,...,A_n,A_{n+1}\} \ \ \ \ \text{These are the variable we optimize}$

$argmin_{A_i}\Sigma_{i=1}^{n-1}d(A_i^{-1}A_{i+1},P_i)+d(A_{n}^{-1}A_{n+1},T_n) \ \ \ \text{what we want to solve}$

where $d$ is a function measuring the distance between its two arguments. Its actual definition will depend on how you actually parametrize your transformations (e.g Lie algera, quaternions, etc...). Usually, any iterative minimization algorithm will do.

A really great resource to understand loop closure is Hauke Strasdat's phd thesis.

Title: Pose-graph-slam:using only a camera


问:
Regarding Pose Graph Slam using Lidar
So I'm trying to incorporate pose graph optimization into a mapping framework using a Lidar. So I basically have all the relative transformations between the pointclouds and I have pairs of pointclouds which satisfy my place recognition algorithm so I know which poses to complete the loop with, now the question I have is given that I only have these relative transformations (1) how do I calculate the error

where $\hat{z}$ is the ground truth since I only have one set of measurements which are my R,t estimate from consecutive pointclouds?

(2) How do I loop close using g2o?

(3) What will my information matrix be isn't it supposed to be a property of the sensor itself?

Thank you.

答:
In the context of a pose graph / factor graph SLAM backend, $e\left(i,j\right)$ actually reduces to the difference between the current estimate of pose $x_j$ and the predicted pose $\hat{x}_j$ given the current estimate of pose $x_i$ and the constraint / pose graph edge $c_{ij}$:
$$
e\left(i,j\right)=x_j-\hat{x}_j=x_j-x_i\cdot c_{ij}=x_j-\left(R_{ij}\cdot x_i+t_{ij}\right)
$$
where $R_{ij},t_{ij}$ are the parametrization of the transformation between poses $x_i$ and $x_j$ as given by constraint / edge $c_{ij}$. If you are using g2o, then this is not something you need to calculate yourself, you only need to provide an initial estimate for every pose, as well as the edge measurements ($R,t$) and their information matrices $\mathcal{I}$.

Depending on whether you are operating in SE3 or SE2, add an edge (EdgeSE2 / EdgeSE3) between the two vertices where the loop closure was determined, using the registration data (transformation and information matrix) of the two associated point clouds to specify the edge constraint.

I would strongly advise you to look into robust methods like Dynamic Covariance Scaling (Agarwal et al., ICRA 2013), Switchable Constraints (Sünderhauf et al., IROS 2012) and RRR (Lasif et al., IJRR 2013), as erroneous loop closure constraints will wreak havok on the shape of your optimization problem / pose graph.

If you are talking about the information matrix of the edges / constraints between poses, then they will come from your registration algorithm, odometry system, IMU, etc. Anything that measures the geometric offset between two poses should provide an estimate of the certainty or uncertainty of this measurement. In Pose Graph SLAM you will typically have constraints generated by Odometry or IMU, and / or from registration algorithms such as ICP.

Depending on your parametrization of the transformation, this uncertainty is represented as a covariance matrix $\Sigma^{n\times n}$ where $n$ is the dimensionality of your transformation representation (e.g. in 2D we would have $n=3$). The Fisher information matrix $\mathcal{I}$ required by e.g. g2o is simply the inverse of the covariance matrix $\Sigma$ , i.e.
$$
\mathcal{I}_{ij}={\Sigma_{ij}}^{-1}
$$

Hope this helps.

Title: Regarding Pose Graph Slam using Lidar


问:
Using IR LED and photodiode to estimate range and bearing of other Swarm Robots
I'm trying to make swarm robots that use 8 IR LEDs and 8 Photodiodes arranged alternately along the circumference of the circular body to determine the range and bearing of other nearby swarm robots (similar ro Rice University's r-one)

Each IR LED and Photodiode is wired as shown below:

The IR LED's and Photo-diodes on one robot are separated by some opaque object. The intention is that when a high analog value is read from certain Photo-diode(s) of the 8 present, another robot's relative range and bearing can be estimated.

The problem is that a high analog value is read even when the robot is near an obstacle because of the reflect infrared light from it's own LED's.

Is there any way for a robot to determine if a high analog value read is because of another robot or because of an obstacle?

Thanks in advance!

答:
There is a lot of ways. Maybe the most simple is to use some pin to drive those LEDs (via transistor, as you need a lot of current for 8 LEDs, I would use it even for 1). Then in some random (or pseudorandom) interval turn those LED off and then make your measurment, so you do not see your light, then tourn them on again, so others can see you.

I would suggest leaving relatively long intervals of ON and doing fast reading just time to time. Also you want somehow make those intervals random, so not to get to synchronized state, when all robots turn light off at the same time and so nobody see nobody.

The least is use cycles like [5,7,11,13] to get out of sync, better is use some kind of noise, eg if you have other sensors too (like ultrasound or gyro or compass), then add last few digits/bits of result to the interval, as they get more noise than information and it helps to make those interval more random.


问:
Roslaunch Gazebo Turtlebot Simulation Not Working
I have installed and am trying to run a turtlebot package using gazebo and roslaunch.

Installation seems to have gone fine and I am now following the first tutorial, which just explains how to get the simulation started. The tutorial can be found here:

http://wiki.ros.org/turtlebot_gazebo/Tutorials/indigo/Gazebo%20Bringup%20Guide

I entered the command: source /opt/ros/indigo/setup.bash
That seemed to go fine, there were no errors.
Then I entered the command: roslaunch turtlebot_gazebo turtlebot_world.launch, which resulted in the following error log:

```<code>    while processing /opt/ros/indigo/share/turtlebot_gazebo/launch/includes/kobuki.launch.xml:
    Invalid  tag: Cannot load command parameter [robot_description]: command [/opt/ros/indigo/share/xacro/xacro.py '/opt/ros/indigo/share/turtlebot_description/robots/kobuki_hexagons_asus_xtion_pro.urdf.xacro'] returned with code [1].

    Param xml is (param command="$(arg urdf_file)" name="robot_description"/)
    The traceback for the exception was written to the log file</code>```

I tried asking on the ROS Answers website first but got no answer, so I'm hoping the good people of Stack Exchange can help me figure out what is causing this problem.

Additional information: The version of ROS I have installed is Indigo, and I'm on Ubuntu 14.04

答:
I've figured out what the problem was.

Ubuntu was running a non-default version of python that I had installed for a University python project a couple of years go. I deleted it and now the default version of python is being used, which has solved the problem.


答:
One way for a robot to determine if the light detected by a photodiode is from another robot, or from itself reflected off an obstacle, is to blink the LEDs in a pattern.

A blink detected by a robot when all of its own LEDs are turned off must have come from some other robot.

There are ways of designing a set of blink patterns, encoding a unique ID number in each pattern, such that, even with a simple binary sensor that only distinguishes between "at least one robot has its LED turned on" vs "none of the robots in range have any LEDs turned on, not even me", it's possible to not only detect that some other robot is in range, but also the unique ID numbers of which specific other robots are in range.
You might find it interesting to give each specific LED its own blink pattern, so that you can detect which specific LEDs of your own and other robots are in range of some specific photodiode.

Title: Using IR LED and photodiode to estimate range and bearing of other Swarm Robots

Body:
I'm trying to make swarm robots that use 8 IR LEDs and 8 Photodiodes arranged alternately along the circumference of the circular body to determine the range and bearing of other nearby swarm robots (similar ro Rice University's r-one)

Each IR LED and Photodiode is wired as shown below:

The IR LED's and Photo-diodes on one robot are separated by some opaque object. The intention is that when a high analog value is read from certain Photo-diode(s) of the 8 present, another robot's relative range and bearing can be estimated.

The problem is that a high analog value is read even when the robot is near an obstacle because of the reflect infrared light from it's own LED's.

Is there any way for a robot to determine if a high analog value read is because of another robot or because of an obstacle?

Thanks in advance!


问:
Space elevator: What is still needed, apart from the cable and propulsion?
In order to build and operate a space elevator moving crafts and people into space, there are two big challenges that have not been solved yet:

Finding a cable with enough tensile strength,

Moving stuff along the cable at a reasonnable speed.

Apart from those two ones, what are the other technical challenges to solve, especially things that do not exist yet in robotics, and need to be invented?

答:
I don't think there would be parts not yet existent in robotics. The elevator itself is pretty much a cabin moving on a rail. It may not be built from off-the-shelf components, but building it shouldn't be difficult.

There are other reasons however, some of which are mentioned in the same Wikipedia article you have linked to. So additional to "not having cables strong enough" and "the slow speed":

Cable wear: The cable used for the elevator will be partially below the clouds (subject to warmth, dry air, rain, snow, wind, tornadoes etc), partially in and above clouds (Humid, Cold), partially in vacuum (extreme cold, no air pressure), sometimes in the sun (extremely hot outside atmosphere), sometimes in the shade (near 0 degrees outside atmosphere) etc. So any cable used for the elevator should be able to resist all these environments.

The situation is made worse by considering the friction between the "wheels" of the elevator and the track, chemical reactions with the environment, radiation, solar flares and even biological reasons (for example an eagle deciding he doesn't like the cable, or a sort of moss evolving to take advantage (aka eat) the material)

Cable repair: Sooner or later, the cable needs repair. There is no such thing as "indestructible". Now imagine, how would you repair such a long cable? Surely, you can't just cut the bad parts and patch it, because the "welded" parts would probably not be strong enough. Replacing the whole cable would be an option which is quite expensive, given the special tough material and the long length. So, at least this issue would delay building the space elevator until they are sure it's doable.

Safety of the elevator: One challenge is to be able to provide safety for the elevator. For example, how would you prevent asteroids from damaging it?

Safety of the passengers: Another challenge is to provide safety for the passengers. With a slow speed, passing through the Allen radiation belts is not healthy. This means that there should be adequate radiation shielding on the elevator, adding weight to it and making it harder to build.

Safety of others: Yet another issue is providing safety for others, such as airplanes or space satellites. Most probably they would all need to become aware of the existence of this new concept, involving reprogramming a large number of air/space-crafts.

In conclusion, probably the most important thing hindering the construction of the space elevator is the construction of the cable. Not only should it be strong enough to withhold the stress, but it should be practically able to withstand any kind of environment. Other reasons would be minor nuisance in comparison, but still they are and would further delay the deployment of the space elevator.


问:
Jacobian-based trajectory following
I would like to control my 7 DOF robot arm to move along a Cartesian trajectory in the world frame.  I can do this just fine for translation, but I am struggling on how to implement something similar for rotation.  So far, all my attempts seem to go unstable.

The trajectory is described as a translational and rotational velocity, plus a distance and/or timeout stopping criteria.  Basically, I want the end-effector to move a short distance relative to its current location.  Because of numerical errors, controller errors, compliance, etc, the arm won't be exactly where you wanted it from the previous iteration.  So I don't simply do $J^{-1}v_e$.  Instead, I store the pose of the end-effector at the start, then at every iteration I compute where the end-effector should be at the current time, take the difference between that and the current location, then feed that into the Jacobian.

I'll first describe my translation implementation.  Here is some pseudo OpenRave Python:

```<code># velocity_transform specified in m/s as relative motion
def move(velocity_transform):
  t_start = time.time()
  pose_start = effector.GetTransform()
  while True:
    t_now = time.time()
    t_elapsed = t_now - t_start
    pose_current = effector.GetTransform()
    translation_target = pose_start[:3,3] + velocity_transform[:3,3] * t_elapsed
    v_trans = translation_target - pose_current[:3,3]
    vels = J_plus.dot(v_trans) # some math simplified here
</code>```

The rotation is a little trickier.  To determine the desired rotation at the current time, i use Spherical Linear Interpolation (SLERP).  OpenRave provides a quatSlerp() function which I use.  (It requires conversion into quaternions, but it seems to work).  Then I calculate the relative rotation between the current pose and the target rotation.  Finally, I convert to Euler angles which is what I must pass into my AngularVelocityJacobian.  Here is the pseudo code for it.  These lines are inside the while loop:

```<code>rot_t1 = np.dot(pose_start[:3,:3], velocity_transform[:3,:3]) # desired rotation of end-effector 1 second from start
quat_start = quatFromRotationMatrix(pose_start) # start pose as quaternion
quat_t1 = quatFromRotationMatrix(rot_t1) # rot_t1 as quaternion

# use SLERP to compute proper rotation at this time
quat_target = quatSlerp(quat_start, quat_t1, t_elapsed) # world_to_target
rot_target = rotationMatrixFromQuat(quat_target) # world_to_target
v_rot = np.dot(np.linalg.inv(pose_current[:3,:3]), rot_target) # current_to_target
v_euler = eulerFromRotationMatrix(v_rot) # get rotation about world axes
</code>```

Then v_euler is fed into the Jacobian along with v_trans.  I am pretty sure my Jacobian code is fine.  Because i have given it (constant) rotational velocities ok.

Note, I am not asking you to debug my code.  I only posted code because I figured it would be more clear than converting this all to math.  I am more interested in why this might go unstable.  Specifically, is the math wrong?  And if this is completely off base, please let me know.  I'm sure people must go about this somehow.

So far, I have been giving it a slow linear velocity (0.01 m/s), and zero target rotational velocity.  The arm is in a good spot in the workspace and can easily achieve the desired motion.  The code runs at 200Hz, which should be sufficiently fast enough.

I can hard-code the angular velocity fed into the Jacobian instead of using the computed v_euler and there is no instability.  So there is something wrong in my math.  This works for both zero and non-zero target angular velocities.  Interestingly, when i feed it an angular velocity of 0.01 rad/sec, the end-effector rotates at a rate of 90 deg/sec.

Update: If I put the end-effector at a different place in the workspace so that its axes are aligned with the world axes, then everything seems works fine.  If the end-effector is 45 degrees off from the world axes, then some motions seem to work, while others don't move exactly as they should, although i don't think i've seen it go unstable.  At 90 degrees or more off from world, then it goes unstable.

答:
Ugo's answer refers to "Sciavicco-Siciliano" which is a good book I'll quote as well.

Chapter 3.6 introduces the so-called analytical Jacobian which is not the same as the so called geometrical Jacobian as it shows up in:
$
\omega=J_{geom} \cdot \dot{q},
$
but has to be obtained from $J_{geom}$ with the help of the transformation matrix $T_A$:

$
J_{analytical}=T_A \cdot J_{geom}
$

or even more informative:

$
\dot{\phi}=J_{analytical} \cdot \dot{q}
$

I allow myself one quotation from this chapter:

From a physical viewpoint, the meaning of $\omega_e$ is more intuitive
  than that of $\dot{\phi}_e$. The three components of ωe represent the
  components of angular velocity with respect to the base frame.
  Instead, the three elements of $\dot{\phi}_e$ represent nonorthogonal
  components of angular velocity defined with respect to the axes of a
  frame that varies as the end-effector orientation varies. On the other
  hand, while the integral of $\dot{\phi}_e$ over time gives $\phi_e$,
  the integral of $\omega_e$ does not admit a clear physical
  interpretation...

So the good news is that we can work with $\phi_e$ -- orientation of the end effector expressed in Euler angles, the bad news is that the transformation matrix $T_A$ introduces singularities which makes the authors of 2 abandon
the concept of analytical Jacobian.

But here comes OpenRave:

```<code>manip=robot.GetManipulators()[0]
# we calculate the rotational Jacobian in quaternion space
quatJ = manip.CalculateRotationJacobian()
quatManip0 = manip.GetTransformPose()[0:4]
# order of multiplication may be wrong in the next line
quatManipTarget = qmult(quatManip0, deltaQuat) #
wSol=np.linalg.lstsq(quatJ, quatManipTarget)
</code>```

<s>So by using the analytical Jacobian in quaternion space you avoid the singularities of the transformation matrix $T_A$. Your Jacobian can still have numeric instability, but this is something different. </s>

UPDATE:
I thought again about the singularities of $T_A$ and possible numeric instabilities of the analytical Jacobian in quaternion space. Indeed my previous claim about the singularities in $\mathbb{R}^3$ being not the same as numeric instabilities in quaternion space is too "strong".

Title: Jacobian-based trajectory following

Body:
Ugo's answer refers to "Sciavicco-Siciliano" which is a good book I'll quote as well.

Chapter 3.6 introduces the so-called analytical Jacobian which is not the same as the so called geometrical Jacobian as it shows up in:
$
\omega=J_{geom} \cdot \dot{q},
$
but has to be obtained from $J_{geom}$ with the help of the transformation matrix $T_A$:

$
J_{analytical}=T_A \cdot J_{geom}
$

or even more informative:

$
\dot{\phi}=J_{analytical} \cdot \dot{q}
$

I allow myself one quotation from this chapter:

From a physical viewpoint, the meaning of $\omega_e$ is more intuitive
  than that of $\dot{\phi}_e$. The three components of ωe represent the
  components of angular velocity with respect to the base frame.
  Instead, the three elements of $\dot{\phi}_e$ represent nonorthogonal
  components of angular velocity defined with respect to the axes of a
  frame that varies as the end-effector orientation varies. On the other
  hand, while the integral of $\dot{\phi}_e$ over time gives $\phi_e$,
  the integral of $\omega_e$ does not admit a clear physical
  interpretation...

So the good news is that we can work with $\phi_e$ -- orientation of the end effector expressed in Euler angles, the bad news is that the transformation matrix $T_A$ introduces singularities which makes the authors of 2 abandon
the concept of analytical Jacobian.

But here comes OpenRave:

manip=robot.GetManipulators()[0]
# we calculate the rotational Jacobian in quaternion space
quatJ = manip.CalculateRotationJacobian()
quatManip0 = manip.GetTransformPose()[0:4]
# order of multiplication may be wrong in the next line
quatManipTarget = qmult(quatManip0, deltaQuat) #
wSol=np.linalg.lstsq(quatJ, quatManipTarget)

<s>So by using the analytical Jacobian in quaternion space you avoid the singularities of the transformation matrix $T_A$. Your Jacobian can still have numeric instability, but this is something different. </s>

UPDATE:
I thought again about the singularities of $T_A$ and possible numeric instabilities of the analytical Jacobian in quaternion space. Indeed my previous claim about the singularities in $\mathbb{R}^3$ being not the same as numeric instabilities in quaternion space is too "strong".