问:
I want to control a sewing machine motor; need help with choices
I've got an industrial sewing machine (think "can sew with thread that looks like string, and has no trouble pounding through 20 layers of Sunbrella fabric"); it's got a 1 HP motor to power it. (I've got a smaller machine as well, w/ a 1/2 or 3/4 HP motor, which I might work on first.) The motor is a "clutch motor" which is always on, and a foot-pedal engages a clutch, so unless you "slip the clutch", you're basically always sewing fast or stopped. I'd like better control. In particular, I'd like to

Be able to stop with the needle "up"

Be able to stop with the needle "buried" (i.e., most of the way down)

Be able to press a button to move forward exactly one stitch

Be able to adjust -- probably with a knob -- the top speed of the motor

Have the motor off when I'm not actually sewing

The 1 HP motor is probably overkill for what I'm doing. I don't suppose I've ever used more than about 1/4 HP even on the toughest jobs.

I'd appreciate any comments on my thinking so far:

From what I've read, it looks as if a DC motor is the way to go (max torque at zero speed, which is nice for that initial "punch through the material" thing, and the ability to "brake" by shorting the + and - terminals). Brushless would be nice...but expensive. And I have a nice DC treadmill motor, and if I drive it at about 12-24V, it'll give me more or less the right speed; adjusting pulleys will do the rest. Such DC motors are powered (in electric lawnmowers, for instance) by running AC through a diode bridge rectifier to produce pulsating DC, and I've got such a bridge rectifier handy. I also have an old autotransformer that I can use to get 24VAC pretty easily. Thus I can get 24V pulsating DC to drive the thing ... but that may or may not be a good idea.

I've also got an Arduino and the skills to program it, and several years of electronics tinkering, and some RC experience...but no experience handling larger DC motors like this one. I've been told the magic words "H-bridge", and found this motor driver which certainly seems as if it'll allow me to turn on/off the motor, and regulate the voltage going to the motor. I don't know whether, when presented with pulsating DC at the input, it'll still work. Any thoughts on this?

I also can't tell -- there doesn't seem to be a handy datasheet or instruction page -- whether this thing can do braking.

For position sensing, there are lots of places I can get information -- either from the needle baror the handwheel of the sewing machine, so I'm not too concerned about that part. To give a sense of the problem, a typical stitching speed is something like 1000 stiches per minute, so if I'm just sensing whether the needle is in the top 1/4 of its travels or the bottom quarter, we're talking about something on the order of 10-50Hz, which doesn't sound like a challenging control scenario.

I guess my questions are these:

Will pulsating DC work with a controller like the one I've cited?

Would I be better off with an RC motor-controller? I can't seem to find one designed for the 24V range that can handle 50 amps, unless it's for a brushless motor, which I don't have. And I think that I want one that has braking ability as well. And I worry that with an RC controller, the software in the controller may prevent me from making the motor stop at a particular position.

Any comments/suggestions appreciated. (BTW, I'm happy with mathematics and with reading data sheets, and I've read (a few years ago) about half of "The Art of Electronics", just so you have an idea of the level of answer that's appropriate.)

To answer @jwpat's questions:

I got my voltage value by saying that the motor is rated for (I think) 130V, and is 2.5HP (yikes), but turns at something like 6700 RPM. (Here's one that looks just like mine).  Dividing by 5 or 6, I got "about 24 V" to give me about 1400 RPM. (I'm at the office; the motor's at home, so I can't tell you the exact part number, alas.) I honestly don't think that the no-load vs load condition is a big deal, because I can wangle a factor of 2 with pulleys.

The sewing machine is a Juki 562

Current motor/clutch are similar to this

Sorry for the lack of detail here,

答:
Well, when you say 'pulsating' it's actually a square wave, it goes up then down.

We don't usually like those types of wave, that's why we regulated them to make a straight line or at least a line that represents a sine wave(those that move with smooth curves).

The type of current that is running through your circuitry doesn't generally affect what controller you should use.

Since you said you had experience with r/c then I think you're pretty fond of using r/c devices. Therefore, I don't suggest you use r/c since someone might operate a controller different from yours and accidentally operate your machine.

You might wanna try infared(Its range is a bit more secluded than radio so it has a lesser chance of being operated unintentionally), bluetooth(Pairing), or via the internet(I don't recommend this).

Plus, r/c communication is generally used when ranges are quite too far and directionless, I don't suppose you'd be operating your machine while taking a bath would you?

About the driver, drivers are usually used to increase the signal current (stepper motors, servo--), not the 'power' current. Since you're using dc, there won't be any signal current-- just those wires that supply power(Though there really are drivers for dc motors, you'd be better off constructing your own circuit if they just turn your dc from on to off and regulate them). I suggest you use relays that could handle your load, maybe 3 relays to handle the back and forth(Only in one speed).

If you ever want a motor where you could accurately control the location and even the speed, then look for a stepper motor(This one will use a motor driver---- unless it's a really small powered motor).

Personally, I've never tried under powering with differences like that. It might take a while for your motor to turn (worst case, it might not even turn) since it's waiting for the electricity to travel along its coil.

All the components should be relatively cheap, the relay would be the most expensive one. Best of luck.


问:
How to calculate vehicle detection distance?
I would like to know how to calculate the distance to each car when I run my application for an autonomous vehicle in real-time. In addition, I want to know how to implement the calculation in C++.

You can see in the images we can know the distance for each vehicle but I don't know what code I should use to make all these calculations for every vehicle.

Please check the photo to understand more about what I'm trying to achieve.

答:
From the images, I am assuming you do not have a multi camera setup.

So, the short answer, based on the information I think you have, you cannot. In an ideal scenario, if you know the calibration parameters of the camera, you could use the distance to a known object to compute the scale factor. But in this case, you do not have a fixed object in each image.

However, having said this, you should understand that longitudinal distance computation using single camera systems are generally error prone in the real world.(as you can see from the distance measurements in the images). This can be attributed to the non uniformity of objects as well as varying illumination conditions.

A more constrained case would be to track only objects moving in the same direction as the host. In that case, you could try to detect the labe markers(they generally have a uniform width, length and distance between them) and use the principles of similar triangles to compute the distance to the target vehicle.

ContentLicense: CC BY-SA 4.0

ContentLicense: CC BY-SA 3.0


问:
How can I modify a low cost hobby servo to run 'freely'?
I've got some hobby servos (Power HD 1501MGs) and I'd like to be able to control them (via an Arduino) so they will either go to the angle I set, or put them in a 'free running' mode, where the load will take them wherever it goes.

Is this even possible, or am I just going to end up stripping the gears?

My first thought is to simply kill the power to the servo, but the force required to move them in that state is more than I'd like.

If it is possible, am I looking at a hardware change, or could I do it in software?

答:
Interesting question.

The first option I'd investigate would be cutting the power as you suggested (perhaps control a MOSFET in software to switch the power).

Another more complicated approach might be to monitor the current consumption of the servo to detect when it is under a heavy load. When you want to go into "free running" mode, have your software attempt to continuously move the servo into a position that results in the least current consumption. This will not entirely result in a "free running" servo, since there will be some jittery movements from the servo as the software attempts to find the "right" position, but it might work depending on your application.

You would not need to modify your servos to monitor current consumption. A board such as this one from SparkFun will sit in between the servo and its power source (you need one of these per servo) and will output an analog voltage (that you can read on a microcontroller ADC pin) that corresponds to the current:

Image from SparkFun


问:
What good robotics software platforms / operating systems are available?
My company will soon be starting a brand new robotics project, and we are still trying to decide whether we should design and code a robotics software platform from scratch, or if there are any good existing ones.

It would be very useful if there was a software platform which was commonly used among both academics and industry so that our robotic system was generally compatible with others, and so that people were already familiar with it.

We would like the software platform to be able to:

Integrate new robotic hardware components easily.

Already contain a wide array of useful data processing and visualisation tools

Make efficient use of computing hardware

答: Player/Stage is still one of the most popular open source robotics projects out there. It's been around for a long time and some of its developers have moved on to start ROS, but that doesn't detract from Player's usefulness. Indeed, all three main components, Player (the framework), Stage (the 2D simulator), and Gazebo (the 3D simulator), have been made to be somewhat compatible with ROS.</p>


问:
In HRI, how is the "uncanny valley" experienced by people on the autism spectrum?
I'm familiar with the idea of the uncanny valley theory in human-robot interaction, where robots with almost human appearance are perceived as creepy. I also know that there have been research studies done to support this theory using MRI scans.

The effect is an important consideration when designing robotic systems that can successfully interact with people. In order to avoid the uncanny valley, designers often create robots that are very far from humanlike. For example, many therapeutic robots (Paro, Keepon) are designed to look like animals or be "cute" and non-threatening.

Other therapeutic robots, like Kaspar, look very humanlike. Kaspar is an excellent example of the uncanny valley, since when I look at Kaspar it creeps me out. However, people on the autism spectrum may not experience Kaspar the same way that I do. And according to Shahbaz's comment, children with autism have responded well to Kaspar.

In the application of therapeutic robots for people on the autism spectrum, some of the basic principles of human-robot interaction (like the uncanny valley) may not be valid. I can find some anecdotal evidence (with Google) that people on the autism spectrum don't experience the uncanny valley, but so far I haven't seen any real studies in that area.

Does anyone know of active research in human-robot interaction for people on the autism spectrum?  In particular, how does the uncanny valley apply (or doesn't it apply) when people on the autism spectrum interact with a humanlike robot?

答: These robots appear creepy because they look like deformed humans and people with autism who find it difficult to read facial expressions and emotions may not perceive the disformities in the robot. They could also react the other way and be very frightened and creeped out by the robots. As someone with ADHD, I can tell you that they definitely creep me out. It's the ones like you have referenced that creep me the most, the futuristic robots from movies don't scare me so much.</p>


问:
Ceiling depth with a monocular camera
Having a camera mounted on my robot and looking upwards, I want to estimate the distance of the ceiling as the robot moves and also the position of landmarks observed on the ceiling (lamps for example).
I know this is a structure from motion problem but I was very confused on how to implement it. This case is a much simpler case than bundle adjustment as the intrinsic calibration of the camera is existing, the camera pose changes just in x and y directions, and the observed thing is a planar ceiling. Odometry might also be available but I would like to start solving it without. Do you know any libraries that offer a good and simple API to do such a thing? preferably based on levenberg-marquardt or similar optimization algorithms taking in more than just two observations. (Python bindings would be nice to have)</p>

答:
Not sure if I understood the problem correctly, but I understood that you wish to estimate the height of the observed objects hanging from the ceiling.

You have a mono camera but you can take two images from different positions and use them as a stereo pair. You can use OpenCV to do this more easily: http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html

Title: Ceiling depth with a monocular camera


问:
Add hardware reset button for Create2
Is there any way to add a reset button to the Create2 that would be the equivalent  of temporarily disconnecting the battery? </p>

答: You could put a momentary switch on the battery line. That would actually temporarily disconnect the battery.

Title: Add hardware reset button for Create2


答:
There is no good way to do this by hacking in a switch because the battery goes right into the main PCB, but you do have two other good options.

Send opcode 7: "This command resets the robot, as if you had removed and reinserted the battery"

Press and hold the SPOT and DOCK buttons for 10 seconds.

Title: Add hardware reset button for Create2

Body: Is there any way to add a reset button to the Create2 that would be the equivalent  of temporarily disconnecting the battery? </p>


答:
Most packages utilize stereo images to calculate distances. StereoVision is a python package that can be used to generate 3d point clouds. Also, this will require the use of odometry information.

In order to utilize information from more than two sequential images, an implementation of extended kalman filter can be utilized. Successive point clouds can be used to update the estimate of the ceiling.

Title: Ceiling depth with a monocular camera

Body:
Having a camera mounted on my robot and looking upwards, I want to estimate the distance of the ceiling as the robot moves and also the position of landmarks observed on the ceiling (lamps for example).
I know this is a structure from motion problem but I was very confused on how to implement it. This case is a much simpler case than bundle adjustment as the intrinsic calibration of the camera is existing, the camera pose changes just in x and y directions, and the observed thing is a planar ceiling. Odometry might also be available but I would like to start solving it without. Do you know any libraries that offer a good and simple API to do such a thing? preferably based on levenberg-marquardt or similar optimization algorithms taking in more than just two observations. (Python bindings would be nice to have)</p>


问:
Weave Weld Lincoln Electric Mig Robot
This is a simple question that I can't seem to find the answer for but when setting up the weave function how exactly does frequency (Hz) determine how fast it moves back and forth? </p>

<p>In other words if I raise frequency will it move quicker or slower and what factors must I consider? </p>

答:
Frequency refers to how quickly something is cycling, and the Hertz (Hz) unit means specifically "cycles per second."

A cycle just refers to any repetitive motion, relative to some arbitrary reference point. The cycle is complete when the object in reference is at the same point moving the same way when it started.

A trip from my house to work and back is one cycle, but the cycle is not 8 hours because I don't go immediately back to work as soon as I get home. Instead, the cycle is actually 24 hours, because I'm not at my house moving toward work again until the next morning.

You could also look at a tire turning relative to a mark on the ground, or you could look at a weld rod relative to a butt joint. The weld rod completes one weave cycle when it starts, goes all the way to one side, goes all the way to the other side, then returns to where it started.

Now, the frequency in Hertz (Hz) indicates how many times per second the weld rod is going to do this. If you want a fast back-and-forth with less material deposited, you would want a higher frequency. If you want a slower back and forth with more material deposited, you want a lower frequency.

I'm not sure how your welder is configured, but if it's possible to set it to zero that would also disable the weave function because you are indicating that you want the weld rod to go back and forth "zero times per second."

I hope this gives you a better conceptual understanding of frequency!

Title: Weave Weld Lincoln Electric Mig Robot


问:
Quadcopter accelerating or not
I am on the project quadcopter. So i have to use PID for stabalizing it. I think i am going wrong because i am adding the pid output to motors thrust. While the motors thrust means to be its acceleraTion. The reason of my previous statment is that when the quad is static in air(not goin up nor below), that time the thrust is enough to cancel gravity, means thrust is negative gravity, that is acceleration. So if i add pid output to thrust that is acceleration of motors, it will be wrong. I have to add pid to speed of motors, which is not visible. My quad is not stabalizing the reason i see is this, that i am adding pid to acc, while it should be added to speed(virtually). What should i do. Should i derivate the pid output and add to thrust? https://mbasic.facebook.com/photo.php?fbid=1545278952394916&id=100007384772233&set=a.1447457675510378.1073741830.100007384772233&refid=17&ft=top_level_post_id.1545278952394916%3Athid.100007384772233%3A306061129499414%3A69%3A0%3A1443682799%3A-1394728329505289925&tn=E

https://mbasic.facebook.com/photo.php?fbid=1545281645727980&id=100007384772233&set=a.1447457675510378.1073741830.100007384772233&refid=17&tn=E

This is the drawing of my circuit. I am giving the current from one esc to whole of the circuit. Other esc's has only pwm wire connected to circuit.

答: The propellers convert a rotary speed into thrust. You should still be controlling the motor speed. The un-modified output of a PID controller should be the input to your plant/system. </p>

Title: Quadcopter accelerating or not


问:
Why can't you buy continuous servos with absolute positioning?
I've been looking at parts for a beginners robotics kit (I teach at a museum) and have been wondering about servos.

You can buy continuous servos with relative position encoders. But I can't find continuous rotation servos with absolute position encoders. Do these exist? If not, why not?

I understand that some forums don't like shopping questions, but I suspect that this part doesn't exist and I'd like to understand why.

Also, I understand that most servos use a potentiometer as a position encoder and that these don't turn more than 1 rotation, but there are other types of encoders that seem like they would do the job.

Thanks for the help!

答:
I assume you mean hobby servos, like the one pictured here:

These were primarily designed for RC planes and cars to actuate control surfaces and steering.  Continuous rotation is not needed.  So a simple and cheap potentiometer is used for absolute position measurement.  Cheap pots like this have a large dead zone.  Their internal construction is something like this:

And to prevent the motor from reaching the dead zone there is typically some hard stop or peg on one of the gears.  Due to the prevalence and cheap cost of these hobby servos, people have found ways to hack them and make them continuous rotation.  This typically involves trimming the hard stop and replacing the potentiometer with a pair of regular resistors.  Now to get the servo to spin, you kind of need to trick the control algorithm.  The pair of resistors make the servo think it is always in the middle of its range (for example).  Then you command it to go to some other angle, and it spins at some speed to get there, but since it always thinks it is in the middle, it will spin forever.  So what is normally a position command is now a speed command.  Voila!

Now the word servo actually applies to any motor coupled with a sensor for position feedback with the required control to go to a desired position.  See better definition here.  So by hacking the hobby servo to make it continuous rotation, it is no longer actually a servo.  That is unless you now add your own sensing and code to servo to your desired position.

But this is a pretty round-about way of getting what you want.  So why not just pick a motor, gear train, and sensing scheme to specifically suit your needs?  But this is rather advanced for beginners.  So you should find some all-in-one real servos.  Dynamixels are a popular choice.  They are continuous rotation, have real absolution position measurement, are individually addressable, and can be daisychained.

And the LEGO NXT and EV3 motors are also real servos.


问:
Firmware upgrade for iRobot Create 2
Is there a firmware upgrade for available for the Create 2? I had some issues in March when using these for assigning a University of Tennessee  programming project. We are getting ready to use them again (we have 10 now) and I'd like to get them all updated to the latest firmware.</p>

答: Yes, iRobot has found and fixed many firmware issues related to Create.   You should e-mail create@irobot.com for assistance.</p>

Title: Firmware upgrade for iRobot Create 2


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

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

Also the errors are not exponentially dereasing as they should.

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

```<code>function VisualServo()

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

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

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

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

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

    t0 = 0;
    tf = 50;

    s0 = cam;

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

end

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

    global A3D B3D C3D D3D Ad Bd Cd Dd

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

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

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

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

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

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

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

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

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

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

function R = calc_Rotation_matrix(theta_x, theta_y, theta_z)

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

    R = Rx*Ry*Rz;

end

function L = L_matrix(x,y,z)

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

Cases that work:

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

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

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

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

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

答:
So, to be perfectly honest, I don't understand the application, but I'm very good with Matlab and controls.

That said, your code is really hard on the eyes. I'm not sure why you would jump straight to tracking 4 points before you got it working for 1, but that could be part of your application (again:ignorant).

It's hard to read mostly because of your naming of so many very similar variables and your inconsistent use of matrix operations. For instance, you define A through D as 3D(1:2)/3D(3) in your main code, but then you define x1,y1,x2...y4 in your euler function despite the fact that they are calculated the same way.

I would recommend you consider a structure array. Consider the following:

```<code>point(1).image = point(1).world(1:2)/point(1).world(3);
</code>```

This does several things for you:

Your variable names become readable.

You can use as many or few points as you want - poll how many points you have with numel(points) and you can add more points with points(end+1)=....

You can operate on all of the points with one operation inside a for loop - points(i).var = ....

Most importantly - you can pass everything to do with your points in and out of your function as a function parameter rather than declaring them as global.

FINALLY - about your problem, the only thing I can see is at the last step - $v_c = -\lambda L_x e$. I don't see where this is calculated, but as this is a control problem I must assume that $\lambda$ refers to an eigenvalue, which are typically the poles of the system.

The poles dictate system response - a negative pole indicates a stable system, where a positive pole indicates an unstable system. Because your equation shows a negative $\lambda$, I would think that the entire term should be positive for a stable controller. Your term is negative, so that might be the cause of your problem, but again:ignorant.

SO, sorry for the lecture, but I think you might be finding that messy code is hard to debug. If you could find the derivation of the equation then I'd be happy to take a look at it, but I'm reading/posting from my phone and the TeX equations on that page are microscopic relative to the regular text so it's very hard for me to read. Try flipping the sign on your camera speed and if that doesn't fix try cleaning up your code and see if you've got some transposition error somewhere.

Body:
So, to be perfectly honest, I don't understand the application, but I'm very good with Matlab and controls.

That said, your code is really hard on the eyes. I'm not sure why you would jump straight to tracking 4 points before you got it working for 1, but that could be part of your application (again:ignorant).

It's hard to read mostly because of your naming of so many very similar variables and your inconsistent use of matrix operations. For instance, you define A through D as 3D(1:2)/3D(3) in your main code, but then you define x1,y1,x2...y4 in your euler function despite the fact that they are calculated the same way.

I would recommend you consider a structure array. Consider the following:

point(1).image = point(1).world(1:2)/point(1).world(3);

This does several things for you:

Your variable names become readable.

You can use as many or few points as you want - poll how many points you have with numel(points) and you can add more points with points(end+1)=....

You can operate on all of the points with one operation inside a for loop - points(i).var = ....

Most importantly - you can pass everything to do with your points in and out of your function as a function parameter rather than declaring them as global.

FINALLY - about your problem, the only thing I can see is at the last step - $v_c = -\lambda L_x e$. I don't see where this is calculated, but as this is a control problem I must assume that $\lambda$ refers to an eigenvalue, which are typically the poles of the system.

The poles dictate system response - a negative pole indicates a stable system, where a positive pole indicates an unstable system. Because your equation shows a negative $\lambda$, I would think that the entire term should be positive for a stable controller. Your term is negative, so that might be the cause of your problem, but again:ignorant.

SO, sorry for the lecture, but I think you might be finding that messy code is hard to debug. If you could find the derivation of the equation then I'd be happy to take a look at it, but I'm reading/posting from my phone and the TeX equations on that page are microscopic relative to the regular text so it's very hard for me to read. Try flipping the sign on your camera speed and if that doesn't fix try cleaning up your code and see if you've got some transposition error somewhere.


答: Thanks, I had thought of the momentary switch, but as Ben says the battery configuration makes it hard to get to. The whole purpose of the reset is to avoid the awkward 10 second 2 button sequence. I think I've done that literally 100's of times. Sending the opcode isn't a good solution because we are running these wirelessly and often the reset is because we've lost communications. I have an idea on how to get a switch inline with the battery - I will follow up if it works.</p>

Title: Add hardware reset button for Create2


问:
Arduino mobile robot
Is there a way I can control my arduino robot from anywhere in the world. The robot goes out of range of my home wifi so my wifi shield can't help. Is there a way to make sure the robot is always on the Internet no matter where it goes? </p>

答:
There are mobile data shields available. Although this won't work absolutely anywhere the robot goes, this solution should provide a way to control the robot at most urban places.

A low tech solution to this problem is to use a dtmf based control. Although this is similar to the previous solution, this solution will enable you to control the robot even in places without a reliable data connection.

Title: Arduino mobile robot


问:
Using a Bitmap maze image to navigate the maze
I'm working on an robot that would be able to navigate through a maze, avoid obstacles and  identify some of the objects in it. I have a monochromatic bitmap of the maze, that is supposed to be used in the robot navigation.

I am just a first year electrical engineering student, and so need help on how I can use the bmp image. I will be making my robot using the Arduino mega microcontroller.

So how should I get started on it.

If you need me to elaborate on anything kindly say so.

Link: http://ceme.nust.edu.pk/nerc/files/theme_ind_2015.pdf

答:
We begin with a image as BMP of 56 x 96 Pixels as on sample demo on NERC page

The file BMP with name Blue Black BMP gives us the below Matrix
generated by Arduino Code

Now Row Number 0 is Entry Wall

```<code>1 (1,1)     1 (1,2)     1 (1,3)     1 (1,4)     1 (1,5)     Now Row Number 1
0 (2,1)     0 (2,2)     1 (2,3)     0 (2,4)     1 (2,5)     Now Row Number 2
1 (3,1)     1 (3,2)     0 (3,3)     1 (3,4)     1 (3,5)     Now Row Number 3
1 (4,1)     1 (4,2)     1 (4,3)     2 (4,4)     1 (4,5)     Now Row Number 4
1 (5,1)     0 (5,2)     1 (5,3)     1 (5,4)     1 (5,5)     Now Row Number 5
0 (6,1)     1 (6,2)     1 (6,3)     0 (6,4)     1 (6,5)     Now Row Number 6
2 (7,1)     0 (7,2)     1 (7,3)     1 (7,4)     1 (7,5)     Now Row Number 7
1 (8,1)     0 (8,2)     0 (8,3)     1 (8,4)     1 (8,5)     Now Row Number 8
1 (9,1)     1 (9,2)     1 (9,3)     1 (9,4)     0 (9,5)     Now Row Number 9
0 (10,1)    1 (10,2)    1 (10,3)    1 (10,4)    0 (10,5)    Now Row Number 10
0 (11,1)    0 (11,2)    0 (11,3)    0 (11,4)    0 (11,5)    Now Top Wall   11
</code>```

Each above is 8 Bits Cell - Given are 5 block 5x8 = 40 bits + 8 bits Left Side + 8 Bits on Right Side 40 + 16 are 56
and 10 Rows each 8 bits info 10 x 8 = 80 and one line on top and one below each 8 Bits so 80 + 8 + 8 = 96 bits info

Please feel free to ask any info regarding more on reading images / Image processing on Micros or Fpga

Well wishes

Body:
We begin with a image as BMP of 56 x 96 Pixels as on sample demo on NERC page

The file BMP with name Blue Black BMP gives us the below Matrix
generated by Arduino Code

Now Row Number 0 is Entry Wall

1 (1,1)     1 (1,2)     1 (1,3)     1 (1,4)     1 (1,5)     Now Row Number 1
0 (2,1)     0 (2,2)     1 (2,3)     0 (2,4)     1 (2,5)     Now Row Number 2
1 (3,1)     1 (3,2)     0 (3,3)     1 (3,4)     1 (3,5)     Now Row Number 3
1 (4,1)     1 (4,2)     1 (4,3)     2 (4,4)     1 (4,5)     Now Row Number 4
1 (5,1)     0 (5,2)     1 (5,3)     1 (5,4)     1 (5,5)     Now Row Number 5
0 (6,1)     1 (6,2)     1 (6,3)     0 (6,4)     1 (6,5)     Now Row Number 6
2 (7,1)     0 (7,2)     1 (7,3)     1 (7,4)     1 (7,5)     Now Row Number 7
1 (8,1)     0 (8,2)     0 (8,3)     1 (8,4)     1 (8,5)     Now Row Number 8
1 (9,1)     1 (9,2)     1 (9,3)     1 (9,4)     0 (9,5)     Now Row Number 9
0 (10,1)    1 (10,2)    1 (10,3)    1 (10,4)    0 (10,5)    Now Row Number 10
0 (11,1)    0 (11,2)    0 (11,3)    0 (11,4)    0 (11,5)    Now Top Wall   11

Each above is 8 Bits Cell - Given are 5 block 5x8 = 40 bits + 8 bits Left Side + 8 Bits on Right Side 40 + 16 are 56
and 10 Rows each 8 bits info 10 x 8 = 80 and one line on top and one below each 8 Bits so 80 + 8 + 8 = 96 bits info

Please feel free to ask any info regarding more on reading images / Image processing on Micros or Fpga

Well wishes


问:
How can Microhard 920 series modems be made compatible with Microhard 910 series?
Microhard Systems currently sells several types of 900MHz modems, which are mostly used in robotics and SCADA applications.  One of their product lines, the 910 series (MHX910, n910, Spectra 910), is obsolete and no longer sold.  However, some older equipment is built with OEM versions of the 910 series soldered in place.

Microhard currently sells a 920 series (MHX920, n920, Spectra 920) that shares many of the specs with the 910 series, but cannot establish a radio link with a 910 series modem due to differences in encryption and hopping pattern.  Therefore, if you want to make new equipment communicate with equipment using the 910 series, your options are:

De-solder the old 910 modem and replace it with the footprint-compatible 920 modem, or

Reconfigure a 920 series modem to communicate with the 910 series modem.

Option 1 is undesirable, since I don't have access to the firmware on the older equipment in question.  Does anyone know how to accomplish option 2?

答:
After some conversations with Microhard's technical support, the following procedure worked for me:

Email Microhard technical support.  Explain what you're trying to do, and ask for a version of 920 series firmware that will communicate with your 910 series modem.  This firmware is available for the n920 via email, but is not on their website.  There is a firmware file for the Spectra 920 on their downloads page; I have not tried it.

Connect a serial cable to the data port of the modem. (Not the diagnostics port)

You must have hardware with an "S2" switch accessible, or with access to the !bootprog pin on the OEM 920 module.  Hold the switch down or hold the !bootprog line low, and power cycle the modem.  Do not release until procedure is completed.

Use the Microhard Firmware loader (available on the Microhard downloads page) to load the provided firmware.

Release the S2 switch (or raise the !bootprog line) and power cycle the modem.  Your 920 series modem is now ready to communicate with 910 series modems.


问:
indoor positioning system: which is better?
Which method is better, in term of accuracy, for detection of indoor localization of a drone. Camera based system or wireless techniques like WLAN or Bluetooth?</p>

答: As far as I know, wireless signals are quite less acurate than camera based ones. Probably the best would be IR sensors equipment with cameras, but that's also the most expensive</p>


答: Definitely camera based options, however, I would go with IR sensors or some even some sound-based sensors if you want a really specific measurement. </p>

Title: indoor positioning system: which is better?

Body: Which method is better, in term of accuracy, for detection of indoor localization of a drone. Camera based system or wireless techniques like WLAN or Bluetooth?</p>


问:
PID quaternion contoller
I want to control the attitude(roll, pitch, yaw) of a vehicle capable of pitching and rolling. To do this I have created a quaternion PID controller. First I take the current attitude of the vehicle converting it to a quaternion Qc and do the same for the desired attitude with the quaternion Qd. I then calculate the input of my PID controller as Qr = Qc' x Qd. The imaginary parts of the quaternions are then fed as force requests on the roll, pitch, yaw axes of the vehicle. I test on a simulator and the control works but becomes unstable in some cases (request for R: 60 P: 60 Y:60). I also want this to work around singularities (i.e. pitch 90)

Does anyone know why I get this behavior and if so explain (thoroughly) what I'm doing wrong?

答:
You are using one PID controller to try to drive a quaternion? A quaternion by definition represents three degrees of freedom, roll pitch yaw, and a PID controller is Single Input, Single Output (SISO) controller.

You're trying to mask a Multiple Input, Multiple Output (MIMO) system by hiding your three variables in a quaternion.

I would suggest maybe you consider using three PID controllers. If you're concerned about singularities, try going for an axis-angle configuration, where two controllers drive the azimuth and elevation of the axis and a third drives the rotation.

Title: PID quaternion contoller


问:
Quadcopter multiple ESC angles glitch
I'm developing my fligth controller board on Tiva Launchpad for quadcoper and while calibrating PID I discovered an unexpected behaviour: sometimes quadcopter seems to experience random angle errors. While trying to investigate it, I've figured out that my code if fairly trying to compensate tham, as soon as they appear - but do not cause them. Even more - i've discovered that such behaviour appears only when two (or more) motors are adjusted, while one motor system shows pretty good stabilisation.

Here is code for PMW output for different motors:

```<code>torque[0] = (int16_t)(+ angles_correction.pitch - angles_correction.roll) + torque_set;
torque[1] = (int16_t)(+ angles_correction.pitch + angles_correction.roll) + torque_set;
torque[2] = (int16_t)(- angles_correction.pitch + angles_correction.roll) + torque_set;
torque[3] = (int16_t)(- angles_correction.pitch - angles_correction.roll) + torque_set;
</code>```

and here is recorded angles for system with one motor and two motors:

To be sure that it's not the algorithm problem, while recording this angles only Integral part of PID was non-zero, so angles were not even stabilised.

My question is - could esc noise each other (in my quad they are quite close to each other - just few sentimeters away) to cause such behaviour?
Thanks a lot!

答:
To me, the straightforward explanation seems that you are applying a desired torque, but on multiple motors -- when you use 1 motor you get 1× torque, and when you use two motors you are getting 2× torque.

I'd imagine that your code should look more like this:

```<code>motors_active = 2
c_p = angles_correction.pitch / motors_active
c_r = angles_correction.roll / motors_active
torque[0] = (int16_t)(+ c_p - c_r) + torque_set;
torque[1] = (int16_t)(+ c_p + c_r) + torque_set;
torque[2] = (int16_t)(- c_p + c_r) + torque_set;
torque[3] = (int16_t)(- c_p - c_r) + torque_set;
</code>```

I'm unsure why all 4 motors are contributing to both pitch and roll (I would have expected pitch and roll axes to be aligned on the quadrotor axes, but technically things will still work if they are 45° rotated).  If that's the case, I assume you've corrected for the rotation in the calculation of angles_correction.

Title: Quadcopter multiple ESC angles glitch

Body:
To me, the straightforward explanation seems that you are applying a desired torque, but on multiple motors -- when you use 1 motor you get 1× torque, and when you use two motors you are getting 2× torque.

I'd imagine that your code should look more like this:

motors_active = 2
c_p = angles_correction.pitch / motors_active
c_r = angles_correction.roll / motors_active
torque[0] = (int16_t)(+ c_p - c_r) + torque_set;
torque[1] = (int16_t)(+ c_p + c_r) + torque_set;
torque[2] = (int16_t)(- c_p + c_r) + torque_set;
torque[3] = (int16_t)(- c_p - c_r) + torque_set;

I'm unsure why all 4 motors are contributing to both pitch and roll (I would have expected pitch and roll axes to be aligned on the quadrotor axes, but technically things will still work if they are 45° rotated).  If that's the case, I assume you've corrected for the rotation in the calculation of angles_correction.


问:
Simple Neural Network with hardcoded positions for walk optimisation
I'm building a quadrupedal robot that will learn how to walk. From the responses I got from asking if  its possible to run a NN on a micro controller I realised I needed to think of a clever system that wouldn't take 1000 years to be effective and would still be able to demonstrate onboard learning. I've designed a system but I'm not sure how effective it will be.

Firstly I hardcode 5-20 positions for the legs.

I set up a (simple) neural network where each node is a different set of positions for the legs, which I will write.

The robot moves from one node to another and the weight of the joint is determined by how far forward the robot moves.

Eventually there will be strong connections between the best nodes/positions and the robot will have found a pattern of moves that are most successful in walking.

How effective would this be in learning to walk?

Note: instead of positions I could write short gaits and the process would work out which sets work best when combined.

答:
This reminds me of QWOP (seriously).  You need to plan a sequence of button presses to move forward.  For every situation the runner finds himself in, we need to know what button to press, how long to hold it, etc. When you play that, you probably look a lot like this video I stumbled across (pun alert): NN training bi-pedal walkers

What you need to recognize is what configuration is my robot in and you need to plan what leg should move to where?  So you need a topology like this

Leg encoder values → NN → Leg desired encoder values

The difference is you need actual input in the form of sensor values, and output in the form of actions.

Note, if you had other sensors (IMU would be helpful) it would be included in the inputs, and if you had force-controlled joints, those would be your outputs.


问:
What I need to learn to build robots
What subjects are involved in robotics. If I want to build robots then what necessary things I need to learn consecutively as a beginner.</p>

答:
To give you an answer that is similarly broad to your question:
Mechanical engineering, electrical engineering, control theory or computer science are a good start.

To give a more in-depth answer, you would have to tell us a little bit more. For starters, are you interested in robotics as a hobby or study, and what is your background? Is there any specific field of robotics that interests you?

Title: What I need to learn to build robots


问:
How to sumarize Kalman filter covariances for display?
I'm implementing an extended Kalman filter and I'm facing a problem with showing the covariances to the user.

The covariance matrix estimate contains all the information we have about the current value estimate, but that is too much to display.
I would like to have a single number that says "our estimate is really good" when close to 0 and "our estimate is not worth much" when large.

My intuitive simple solution would be to average all the values in the covariance estimate matrix (or maybe just the diagonal), except that in my case the values have different units and different ranges.

Is it possible to do something like this?

答:
You want to display $\frac{n(n+1)}{2}$ values of a symmetric $n \times n$ matrix, where $n=26$, which means you have 351 values.

It doesn't make too much sense to display all those numeric values. After all you want to display them to a human being, which is unlikely able to oversee all these values if displayed as numbers.

Then as you say, the values have different units and ranges. that means there's no automatic way of finding the "good" range.
I suggest running the system for a while, disturbing it, gathering some rough estimates of where the values are and to what peaks they can rise.
With these values you can provide a rough estimate of "good" and "bad", somewhat fuzzy if you will.

Back to the original problem: displaying all those values. A numerical display is out of the question. And that precision isn't even desired, so why not map the values to colors and display each value as a pixel? Make good green and the red bad. Now the job looks a lot simpler: display some 351 pixels1. It's easy to see when some pixels go red if something goes wrong and what other pixels go "hot" at the same moment. It's also easy to see if some values are generally uncertain, constantly staying in the orange color.

Depending on your needs you can expand that debug facility further, allowing the user to only display a subset of the matrix. Or allow closer inspection, displaying the true values behind the colors and/or plot them to see how they change to the "bad" state.

1of course you can display these values as large as you want. I'd start with a a 10x10px box for each number, resulting in a 260x260px box, which should be big enough to see clearly, yet small enough to add other debugging/observation tools around it.


答:
Yes you can do so by converting the covariance matrix to a gray level image. For example, in the below picture the covariance matrix is represented as a gray image in which the gray intensity represents the correlation between the elements, the darker the stronger correlation.

More information see FastSLAM
A Scalable Method for the Simultaneous Localization and Mapping Problem in Robotics

This shows the correlation. To show how the uncertainty of a quantity decreasing, then plot the $x\pm 3\sigma_{x}$ where $x$ the true value not the estimate. You can't describe the entire system with one single value. Averaging has no meaning in this context. In my opinion, pick an element of your state vector and its uncertainty and plot them according to $x\pm 3\sigma_{x}$. EKF is a sub-optimal filter which means converging to the true value might not be possible as the case in KF. This is due to the linearization step, however, EKF is a good choice and generally yields satisfactory results.

Title: How to sumarize Kalman filter covariances for display?

Body:
I'm implementing an extended Kalman filter and I'm facing a problem with showing the covariances to the user.

The covariance matrix estimate contains all the information we have about the current value estimate, but that is too much to display.
I would like to have a single number that says "our estimate is really good" when close to 0 and "our estimate is not worth much" when large.

My intuitive simple solution would be to average all the values in the covariance estimate matrix (or maybe just the diagonal), except that in my case the values have different units and different ranges.

Is it possible to do something like this?


问:
continuous vs discrete simulation in robotics
As far as I know, a robot sends orders as discrete signals. However, isn't computer simulation based on continuous simulation? Do you know if it may happen any important difference when comparing reality to simulation in some cases? I heard that cable-driven robots were quite sensitive.</p>

答:
Computer simulations are not done in the continuous time domain. Simulator designers approximate continuous simulation by reducing the difference between consecutive discrete time points to such a low level that it "looks like" continuous simulation.

If you are modelling something in a 'pseudo continuous' simulator, the best way to get similar results in a hardware implementation would be to increase the frequency of your control signals.


答: This problem is called Chaumette Conundrum and it is a big negative of IBVS. You should try other VS algorithms.</p>

Title: Image Based Visual Servoing algorithm in MATLAB

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

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

Also the errors are not exponentially dereasing as they should.

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

```<code>function VisualServo()

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

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

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

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

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

    t0 = 0;
    tf = 50;

    s0 = cam;

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

end

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

    global A3D B3D C3D D3D Ad Bd Cd Dd

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

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

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

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

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

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

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

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

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

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

function R = calc_Rotation_matrix(theta_x, theta_y, theta_z)

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

    R = Rx*Ry*Rz;

end

function L = L_matrix(x,y,z)

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

Cases that work:

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

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

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

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

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


答:
It's tough to say anything about a really large covariance matrix. This is because (presumably) the matrix includes the uncertainty about a whole bunch of features, not just the robot pose. Typically what matters in real life is the uncertainty in the position of the robot (and possibly orientation). So with only three spatial deimensions, you can easily display a 3D covariance ellipse which will tell you the expected position and uncertainty of the robot.

In literature there are several methods for displaying some statistics about an arbitrarily large state space. The most common is some measure of the eigenvalues of the matrix. (For a 2x2 or 3x3 matrix, this is often a covariance ellipse as mentioned above) For an nxn matrix, you can display the blockwise covariances for each feature, which will tell you which features are very, very roughly estimated, or you can output some function of the covariance itself.

For example, the determinant is a common measure of uncertainty used in GPS as the Dillution of Precision. Another common measure is the maximum eigenvalue, though that only really makes sense on a feature-by-feature basis.


答:
Really this is a question to be answered by experiments. Will it work? It seems like it could. Two things that will be important to look at are:

Training time - You are still using a neural network and they take time to train. Whether this formulation would reduce the number of rounds required for training is really to be seen. It will of course change with the number of connections in the net as your agent will need to test each multiple times.

Training method - Based on your description it seems you are planning to use a Recurrent Neural Network (RNN) which, if memory serves, makes training more computationally intense.

There is a bit of literature on this topic already. For example a quick google reveals a paper titled A neural network model for quadruped gait generation and transitions. It may be worth looking to see what has already been tried. But then sometimes it is just fun to run the experiments.

Title: Simple Neural Network with hardcoded positions for walk optimisation

Body:
I'm building a quadrupedal robot that will learn how to walk. From the responses I got from asking if  its possible to run a NN on a micro controller I realised I needed to think of a clever system that wouldn't take 1000 years to be effective and would still be able to demonstrate onboard learning. I've designed a system but I'm not sure how effective it will be.

Firstly I hardcode 5-20 positions for the legs.

I set up a (simple) neural network where each node is a different set of positions for the legs, which I will write.

The robot moves from one node to another and the weight of the joint is determined by how far forward the robot moves.

Eventually there will be strong connections between the best nodes/positions and the robot will have found a pattern of moves that are most successful in walking.

How effective would this be in learning to walk?

Note: instead of positions I could write short gaits and the process would work out which sets work best when combined.


问:
Raspberry pi quadcopter drifts like crazy
I have recently built a raspberry pi based quadcopter that communicates with my tablet over wifi. The problem is that it drifts a lot. At first I thought that the problem was vibration, so I mounted the MPU-6050 more securely to the frame. That seemed to help a bit, but it still drifts. I have tried tuning the PID, tuning the complementary filter, and installing a real time OS. Nothing seems to help very much. Below is my code written completely in java. Any suggestions are appreciated.

QuadServer.java:

```<code>package com.zachary.quadserver;

import java.net.*;
import java.io.*;
import java.util.*;

import com.pi4j.io.i2c.I2CBus;
import com.pi4j.io.i2c.I2CDevice;
import com.pi4j.io.i2c.I2CFactory;

import se.hirt.pi.adafruit.pwm.PWMDevice;
import se.hirt.pi.adafruit.pwm.PWMDevice.PWMChannel;

public class QuadServer {
    private final static int FREQUENCY = 490;

    private static final int MIN = 740;
    private static final int MAX = 2029;

    private static Sensor sensor = new Sensor();

    private static double PX = 0;
    private static double PY = 0;
    private static double PZ = 0;

    private static double IX = 0;
    private static double IY = 0;
    private static double IZ = 0;

    private static double DX = 0;
    private static double DY = 0;
    private static double DZ = 0;

    private static double kP = 1.95; //2.0
    private static double kI = 10.8; //8.5
    private static double kD = 0.15; //0.14

    private static long time = System.currentTimeMillis();

    private static double last_errorX = 0;
    private static double last_errorY = 0;
    private static double last_errorZ = 0;

    private static double outputX;
    private static double outputY;
    private static double outputZ;

    private static int val[] = new int[4];

    private static int throttle;

    static double setpointX = 0;
    static double setpointY = 0;
    static double setpointZ = 0;

    static double errorX;
    static double errorY;
    static double errorZ;

    static long receivedTime = System.currentTimeMillis();

    private static String data;

    static int trimX = -70;
    static int trimY = 70;

    public static void main(String[] args) throws IOException, NullPointerException {
        DatagramSocket serverSocket = new DatagramSocket(40002);

        PWMDevice device = new PWMDevice();

        device.setPWMFreqency(FREQUENCY);

        PWMChannel esc0 = device.getChannel(0);
        PWMChannel esc1 = device.getChannel(1);
        PWMChannel esc2 = device.getChannel(2);
        PWMChannel esc3 = device.getChannel(3);

        /*Runtime.getRuntime().addShutdownHook(new Thread(new Runnable() {
                public void run() {
                    System.out.println("terminating");
                    try {
                        esc0.setPWM(0, calculatePulseWidth(MIN/1000.0, FREQUENCY));
                    esc1.setPWM(0, calculatePulseWidth(MIN/1000.0, FREQUENCY));
                    esc2.setPWM(0, calculatePulseWidth(MIN/1000.0, FREQUENCY));
                    esc3.setPWM(0, calculatePulseWidth(MIN/1000.0, FREQUENCY));
                } catch (IOException e) {
                    e.printStackTrace();
                }
                }
            }));
            System.out.println("running");*/

        Thread read = new Thread(){
                public void run(){
                    while(true) {
                    try {
                            byte receiveData[] = new byte[1024];
                            DatagramPacket receivePacket = new DatagramPacket(receiveData, receiveData.length);
                            serverSocket.receive(receivePacket);
                            String message = new String(receivePacket.getData());

                            data = ""+IX;
                        addData(IY);

                        addData(sensor.readAccelAngle(0));
                        addData(sensor.readAccelAngle(1));

                        byte[] sendData = new byte[1024];
                            sendData = data.getBytes();
                                InetAddress IPAddress = InetAddress.getByName("192.168.1.9");
                                DatagramPacket sendPacket = new DatagramPacket(sendData, sendData.length, IPAddress, 1025);
                                serverSocket.send(sendPacket);

                            setpointX = Double.parseDouble(message.split("\\s+")[0])*0.7;
                        setpointY = Double.parseDouble(message.split("\\s+")[1])*0.7;

                            throttle = (int)(Integer.parseInt((message.split("\\s+")[3]))*12.67)+MIN;

                            kP = Math.round((Integer.parseInt(message.split("\\s+")[4])*0.05)*1000.0)/1000.0;
                            kI = Math.round((Integer.parseInt(message.split("\\s+")[5])*0.2)*1000.0)/1000.0;
                            kD = Math.round((Integer.parseInt(message.split("\\s+")[6])*0.01)*1000.0)/1000.0;

                            trimX = (Integer.parseInt(message.split("\\s+")[7])-50)*2;
                            trimY = (Integer.parseInt(message.split("\\s+")[8])-50)*2;

                            double accelSmoothing = 0.02;//(Integer.parseInt(message.split("\\s+")[8])*0.05)+1;
                            double gyroSmoothing = 0.04;//(Integer.parseInt(message.split("\\s+")[7])*0.01);

                            sensor.setSmoothing(gyroSmoothing, accelSmoothing);

                            //System.out.println("trimX: "+trimX+" trimY: "+trimY);

                            System.out.println("kP: "+kP+", kI: "+kI+", kD: "+kD+", trimX: "+trimX+", trimY: "+trimY);

                        receivedTime = System.currentTimeMillis();

                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                    }
                }
        };
        read.start();

        while(true)
        {
            Arrays.fill(val, throttle);

            errorX = sensor.readGyro(0)-setpointX;
            errorY = -sensor.readGyro(1)-setpointY;
            errorZ = sensor.readGyro(2)-setpointZ;

            double dt = (double)(System.currentTimeMillis()-time)/1000;

            double accelAngleX = sensor.readAccelAngle(0);
            double accelAngleY = sensor.readAccelAngle(1);

            if(dt > 0.005)
            {

                PX = errorX;
                PY = errorY;
                PZ = errorZ;

                IX += (errorX)*dt;
                IY += (errorY)*dt;
                //IZ += errorZ*dt;

                IX = 0.98*IX+0.02*accelAngleX;
                IY = 0.98*IY+0.02*accelAngleY;

                DX = (errorX - last_errorX)/dt;
                DY = (errorY - last_errorY)/dt;
                //DZ = (errorZ - last_errorZ)/dt;

                last_errorX = errorX;
                last_errorY = errorY;
                last_errorZ = errorZ;

                outputX = kP*PX+kI*IX+kD*DX;
                outputY = kP*PY+kI*IY+kD*DY;
                outputZ = kP*PZ+kI*IZ+kD*DZ;

                time = System.currentTimeMillis();
            }

            //System.out.println(IX+", "+IY+", "+throttle);

            add(-outputX-outputY-outputZ-trimX+trimY, 0);       //clockwise
            add(-outputX+outputY+outputZ-trimX-trimY, 1);   //counterClockwise
            add(outputX+outputY-outputZ+trimX-trimY, 2);    //clockwise
            add(outputX-outputY+outputZ+trimX+trimY, 3);        //counterclockwise

            //System.out.println(val[0]+", "+val[1]+", "+val[2]+", "+val[3]);

            try {
                if(System.currentTimeMillis()-receivedTime < 1000)
                {
                    esc0.setPWM(0, calculatePulseWidth(val[0]/1000.0, FREQUENCY));
                    esc1.setPWM(0, calculatePulseWidth(val[1]/1000.0, FREQUENCY));
                    esc2.setPWM(0, calculatePulseWidth(val[2]/1000.0, FREQUENCY));
                    esc3.setPWM(0, calculatePulseWidth(val[3]/1000.0, FREQUENCY));
                } else
                {
                    esc0.setPWM(0, calculatePulseWidth(800/1000.0, FREQUENCY));
                    esc1.setPWM(0, calculatePulseWidth(800/1000.0, FREQUENCY));
                    esc2.setPWM(0, calculatePulseWidth(800/1000.0, FREQUENCY));
                    esc3.setPWM(0, calculatePulseWidth(800/1000.0, FREQUENCY));
                }
            } catch (IOException e) {
                e.printStackTrace();
            }

        }
    }

    private static void add(double value, int i)
    {
        if(val[i]+value > MIN && val[i]+value < MAX)
        {
            val[i] += value;
        }else if(val[i]+value < MIN)
        {
            //System.out.println("low");
            val[i] = MIN;
        }else if(val[i]+value > MAX)
        {
            //System.out.println("low");
            val[i] = MAX;
        }
    }

    static void addData(double value)
    {
        data += " "+value;
    }

    private static int calculatePulseWidth(double millis, int frequency) {
        return (int) (Math.round(4096 * millis * frequency/1000));
    }
}
</code>```

Sensor.java:

```<code>package com.zachary.quadserver;

import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinDigitalOutput;
import com.pi4j.io.gpio.PinState;
import com.pi4j.io.gpio.RaspiPin;
import com.pi4j.io.i2c.*;

import java.net.*;
import java.io.*;

public class Sensor {
    static I2CDevice sensor;
    static I2CBus bus;
    static byte[] accelData, gyroData;
    static long accelCalib[] = {0, 0, 0};
    static long gyroCalib[] = {0, 0, 0};

    static double gyroX;
    static double gyroY;
    static double gyroZ;

    static double smoothedGyroX;
    static double smoothedGyroY;
    static double smoothedGyroZ;

    static double accelX;
    static double accelY;
    static double accelZ;

    static double accelAngleX;
    static double accelAngleY;

    static double smoothedAccelAngleX;
    static double smoothedAccelAngleY;

    static double angleX;
    static double angleY;
    static double angleZ;

    static boolean init = true;

    static double accelSmoothing = 1;
    static double gyroSmoothing = 1;

    public Sensor() {
        try {
            bus = I2CFactory.getInstance(I2CBus.BUS_1);

            sensor = bus.getDevice(0x68);

            sensor.write(0x6B, (byte) 0x0);
            sensor.write(0x6C, (byte) 0x0);
            System.out.println("Calibrating...");

            calibrate();

            Thread sensors = new Thread(){
                    public void run(){
                        try {
                            readSensors();
                        } catch (IOException e) {
                        e.printStackTrace();
                    }
                    }
            };
            sensors.start();
        } catch (IOException e) {
            System.out.println(e.getMessage());
        }
    }

    private static void readSensors() throws IOException {
        long time = System.currentTimeMillis();
        long sendTime = System.currentTimeMillis();

        while (true) {
            accelData = new byte[6];
            gyroData = new byte[6];

            int r = sensor.read(0x3B, accelData, 0, 6);

            accelX = (((accelData[0] << 8)+accelData[1]-accelCalib[0])/16384.0)*9.8;
            accelY = (((accelData[2] << 8)+accelData[3]-accelCalib[1])/16384.0)*9.8;
            accelZ = ((((accelData[4] << 8)+accelData[5]-accelCalib[2])/16384.0)*9.8)+9.8;
            accelZ = 9.8-Math.abs(accelZ-9.8);

            double hypotX = Math.sqrt(Math.pow(accelX, 2)+Math.pow(accelZ, 2));
            double hypotY = Math.sqrt(Math.pow(accelY, 2)+Math.pow(accelZ, 2));

            accelAngleX = Math.toDegrees(Math.asin(accelY/hypotY));
            accelAngleY = Math.toDegrees(Math.asin(accelX/hypotX));

            //System.out.println(accelAngleX[0]+" "+accelAngleX[1]+" "+accelAngleX[2]+" "+accelAngleX[3]);

            //System.out.println("accelX: " + accelX+" accelY: " + accelY+" accelZ: " + accelZ);

            r = sensor.read(0x43, gyroData, 0, 6);

            gyroX = (((gyroData[0] << 8)+gyroData[1]-gyroCalib[0])/131.0);
            gyroY = (((gyroData[2] << 8)+gyroData[3]-gyroCalib[1])/131.0);
            gyroZ = (((gyroData[4] << 8)+gyroData[5]-gyroCalib[2])/131.0);

            if(init)
            {
                smoothedAccelAngleX = accelAngleX;
                smoothedAccelAngleY = accelAngleY;

                smoothedGyroX = gyroX;
                smoothedGyroY = gyroY;
                smoothedGyroZ = gyroZ;

                init = false;
            } else {
                smoothedAccelAngleX = smoothedAccelAngleX+(accelSmoothing*(accelAngleX-smoothedAccelAngleX));
                smoothedAccelAngleY = smoothedAccelAngleY+(accelSmoothing*(accelAngleY-smoothedAccelAngleY));

                smoothedGyroX = smoothedGyroX+(gyroSmoothing*(gyroX-smoothedGyroX));
                smoothedGyroY = smoothedGyroY+(gyroSmoothing*(gyroY-smoothedGyroY));
                smoothedGyroZ = smoothedGyroZ+(gyroSmoothing*(gyroZ-smoothedGyroZ));

                /*smoothedAccelAngleX = accelAngleX;
                smoothedAccelAngleY = accelAngleY;

                smoothedGyroX = gyroX;
                smoothedGyroY = gyroY;
                smoothedGyroY = gyroY;*/

                /*smoothedAccelAngleX += (accelAngleX-smoothedAccelAngleX)/accelSmoothing;
                smoothedAccelAngleY += (accelAngleY-smoothedAccelAngleY)/accelSmoothing;

                smoothedGyroX += (gyroX-smoothedGyroX)/gyroSmoothing;
                smoothedGyroY += (gyroY-smoothedGyroY)/gyroSmoothing;
                smoothedGyroZ += (gyroZ-smoothedGyroZ)/gyroSmoothing;*/

            }

            angleX += smoothedGyroX*(System.currentTimeMillis()-time)/1000;
            angleY += smoothedGyroY*(System.currentTimeMillis()-time)/1000;
            angleZ += smoothedGyroZ;

            angleX = 0.95*angleX + 0.05*smoothedAccelAngleX;
            angleY = 0.95*angleY + 0.05*smoothedAccelAngleY;

            time = System.currentTimeMillis();

            //System.out.println((int)angleX+"  "+(int)angleY);
            //System.out.println((int)accelAngleX+", "+(int)accelAngleY);
        }
    }

    public static void calibrate() throws IOException {
        int i;
        for(i = 0; i < 100; i++)
        {
            accelData = new byte[6];
            gyroData = new byte[6];
            int r = sensor.read(0x3B, accelData, 0, 6);
            accelCalib[0] += (accelData[0] << 8)+accelData[1];
            accelCalib[1] += (accelData[2] << 8)+accelData[3];
            accelCalib[2] += (accelData[4] << 8)+accelData[5];

            r = sensor.read(0x43, gyroData, 0, 6);
            gyroCalib[0] += (gyroData[0] << 8)+gyroData[1];
            gyroCalib[1] += (gyroData[2] << 8)+gyroData[3];
            gyroCalib[2] += (gyroData[4] << 8)+gyroData[5];
            try {
                Thread.sleep(1);
            } catch (Exception e){
                e.printStackTrace();
            }
        }
        gyroCalib[0] /= i;
        gyroCalib[1] /= i;
        gyroCalib[2] /= i;

        accelCalib[0] /= i;
        accelCalib[1] /= i;
        accelCalib[2] /= i;

        System.out.println(gyroCalib[0]+", "+gyroCalib[1]+", "+gyroCalib[2]);
        System.out.println(accelCalib[0]+", "+accelCalib[1]+", "+accelCalib[2]);
    }

    public double readAngle(int axis)
    {
        switch (axis)
        {
            case 0:
                return angleX;
            case 1:
                return angleY;
            case 2:
                return angleZ;
        }

        return 0;
    }

    public double readGyro(int axis)
    {
        switch (axis)
        {
            case 0:
                return smoothedGyroX;
            case 1:
                return smoothedGyroY;
            case 2:
                return smoothedGyroZ;
        }

        return 0;
    }

    public double readAccel(int axis)
    {
        switch (axis)
        {
            case 0:
                return accelX;
            case 1:
                return accelY;
            case 2:
                return accelZ;
        }

        return 0;
    }

    public double readAccelAngle(int axis)
    {
        switch (axis)
        {
            case 0:
                return smoothedAccelAngleX;
            case 1:
                return smoothedAccelAngleY;

        }

        return 0;
    }

    public void setSmoothing(double gyro, double accel)
    {
        gyroSmoothing = gyro;
        accelSmoothing = accel;
    }
}
</code>```

答:
From your sensor code,

I'm not sure I understand what your gyroSmoothing is doing. For one, it's set =1, so it's not doing anything at all currently, but I don't know what it would do at other values either.

Further, in looking at the section featuring gyroSmoothing, you have:

```<code>smoothedGyroN = gyroN;
smoothedGyroN = smoothedGyroN + (gyroN - smoothedGyroN)/gyroSmoothing;
</code>```

But, you just set smoothedGyroN = gyroN;, so you have:

```<code>smoothedGyroN = gyroN;
smoothedGyroN = smoothedGyroN + <zero>
</code>```

So, I don't know what the point of writing all of that is. It looks almost like you're trying to write a lag filter, but that's not quite the right form for a lag filter either.

Also, later you integrate gyro speeds to get angles, except you fail to multiply the z-axis gyro by the sample time:

```<code>angleX += smoothedGyroX*(System.currentTimeMillis()-time)/1000;
angleY += smoothedGyroY*(System.currentTimeMillis()-time)/1000;
angleZ += smoothedGyroZ;
</code>```

Just after that you actually do implement a lag filter on the X- and Y-axis angles.

From your control code,

You should calculate errors as desired - actual, but instead you have:

```<code>errorX = sensor.readGyro(0)-setpointX;
errorY = -sensor.readGyro(1)-setpointY;
errorZ = sensor.readGyro(2)-setpointZ;
</code>```

So the sign is flipped on your errorX and errorZ - I'm assuming maybe this is why you have -sensor.readGyro(1) for your errorY? Just flipping signs until it works? Not harassing - I've done it, too.

Just after you calculate integral error, you have a complimentary filter between integral error and. . . angular position?? I have no idea why you would do this:

```<code>IX = 0.98*IX+0.02*accelAngleX;
IY = 0.98*IY+0.02*accelAngleY;
</code>```

The integral error term should be just the integral error term - don't add anything, don't smooth it, nothing. If you find it's jumping all over the place (which, if your system has mass, it shouldn't jump around), but if you find it does jump, then you should filter your sensor feedback. Leave the error terms alone!

Also, FYI, you have disabled I/D gains on the Z-axis.

I don't like that you've got an if statement around your PID controller - I don't know why you would want this, it seems like this would intermittently provide a control signal. I would suggest either adding a delay at the end of the PID calculations if you want it to run slower (not sure why) or just let it run all the time. You're calculating sample time correctly so I really have no idea what this is for.

When you set the motor speed outputs, it looks like you might be adding trim terms incorrectly. I'm not familiar with adding trim terms like this - if I were going to add trim I would do it by adding the trim term to the angle setpoint, not to the motor speed command. That said, you have:

```<code>add(-outputX-outputY-outputZ-trimX+trimY, 0);       //clockwise
add(-outputX+outputY+outputZ-trimX-trimY, 1);   //counterClockwise
add( outputX+outputY-outputZ+trimX-trimY, 2);    //clockwise
add( outputX-outputY+outputZ+trimX+trimY, 3);        //counterclockwise
</code>```

In all cases, the sign of trimX matches outputX, but trimY is always the opposite of outputY. Also, this is probably just my ignorance of Java, but I don't see a variable assignment for your add statements. You reference var[N] just below, but I don't see where the add terms get assigned to var.

Body:
From your sensor code,

I'm not sure I understand what your gyroSmoothing is doing. For one, it's set =1, so it's not doing anything at all currently, but I don't know what it would do at other values either.

Further, in looking at the section featuring gyroSmoothing, you have:

smoothedGyroN = gyroN;
smoothedGyroN = smoothedGyroN + (gyroN - smoothedGyroN)/gyroSmoothing;

But, you just set smoothedGyroN = gyroN;, so you have:

smoothedGyroN = gyroN;
smoothedGyroN = smoothedGyroN + <zero>

So, I don't know what the point of writing all of that is. It looks almost like you're trying to write a lag filter, but that's not quite the right form for a lag filter either.

Also, later you integrate gyro speeds to get angles, except you fail to multiply the z-axis gyro by the sample time:

angleX += smoothedGyroX*(System.currentTimeMillis()-time)/1000;
angleY += smoothedGyroY*(System.currentTimeMillis()-time)/1000;
angleZ += smoothedGyroZ;

Just after that you actually do implement a lag filter on the X- and Y-axis angles.

From your control code,

You should calculate errors as desired - actual, but instead you have:

errorX = sensor.readGyro(0)-setpointX;
errorY = -sensor.readGyro(1)-setpointY;
errorZ = sensor.readGyro(2)-setpointZ;

So the sign is flipped on your errorX and errorZ - I'm assuming maybe this is why you have -sensor.readGyro(1) for your errorY? Just flipping signs until it works? Not harassing - I've done it, too.

Just after you calculate integral error, you have a complimentary filter between integral error and. . . angular position?? I have no idea why you would do this:

IX = 0.98*IX+0.02*accelAngleX;
IY = 0.98*IY+0.02*accelAngleY;

The integral error term should be just the integral error term - don't add anything, don't smooth it, nothing. If you find it's jumping all over the place (which, if your system has mass, it shouldn't jump around), but if you find it does jump, then you should filter your sensor feedback. Leave the error terms alone!

Also, FYI, you have disabled I/D gains on the Z-axis.

I don't like that you've got an if statement around your PID controller - I don't know why you would want this, it seems like this would intermittently provide a control signal. I would suggest either adding a delay at the end of the PID calculations if you want it to run slower (not sure why) or just let it run all the time. You're calculating sample time correctly so I really have no idea what this is for.

When you set the motor speed outputs, it looks like you might be adding trim terms incorrectly. I'm not familiar with adding trim terms like this - if I were going to add trim I would do it by adding the trim term to the angle setpoint, not to the motor speed command. That said, you have:

add(-outputX-outputY-outputZ-trimX+trimY, 0);       //clockwise
add(-outputX+outputY+outputZ-trimX-trimY, 1);   //counterClockwise
add( outputX+outputY-outputZ+trimX-trimY, 2);    //clockwise
add( outputX-outputY+outputZ+trimX+trimY, 3);        //counterclockwise

In all cases, the sign of trimX matches outputX, but trimY is always the opposite of outputY. Also, this is probably just my ignorance of Java, but I don't see a variable assignment for your add statements. You reference var[N] just below, but I don't see where the add terms get assigned to var.


问:
composition of rotation matrices
I am the moment learning about rotation matrices.  It seems confusing how it could be that  $R_A^C=R_A^BR_B^C$ is the rotation from coordinate frame <s>A to C</s> C to A, and A,B,C are different coordinate frames.

$R_A^C$ must for a 2x2 matrix be defined as
$$
R_A^C=
\left(
\begin{matrix}
xa⋅xb  &  xa⋅xb \\
ya⋅yb &  ya⋅yb
\end{matrix}
\right)
$$

$x_a, y_a and x_b,y_b$ are coordinates for points given in different coordinate frame.
I don't see how, using this standard, the multiplication stated above will give the same matrix as for $R_A^C$. Some form for clarification would be helpful here.

答:
I'm not sure what you mean by:

$R_A^C$ must for a 2x2 matrix be defined as $[xa \cdot xb , xa \cdot xb ; ya \cdot yb , ya \cdot yb]$

because I don't know where you are getting the names or formulas from. If you have:

$$
R_A^B = \left[ \begin{array}{ccc}
a & b \\
c & d \end{array} \right] \\
R_B^C = \left[ \begin{array}{ccc}
e & f \\
g & h \end{array} \right] \\
$$

Then in multiplying the two you get:

$$
R_A^C = R_A^B R_B^C = \left[ \begin{array}{ccc}
(ae + bg) & (af + bh) \\
(ce + dg) & (cf + dh) \end{array} \right] \\
$$

A 2x2 rotation matrix has the form:

$$
R_A^B = R(\alpha) = \left[ \begin{array}{ccc}
\cos{\alpha} & -\sin{\alpha} \\
\sin{\alpha} & \cos{\alpha} \end{array} \right] \\
R_B^C = R(\beta) = \left[ \begin{array}{ccc}
\cos{\beta} & -\sin{\beta} \\
\sin{\beta} & \cos{\beta} \end{array} \right] \\
$$

If you go through two successive rotations:

$$
R_A^C = R(\alpha + \beta) = R(\alpha)R(\beta) = R_A^B R_B^C \\
R_A^C = \left[ \begin{array}{ccc}
(\cos{\alpha} \cos{\beta} + (-\sin{\alpha} \sin{\beta})) & ((-\cos{\alpha} \sin{\beta}) + (-\sin{\alpha} \cos{\beta})) \\
(\sin{\alpha} \cos{\beta} + \cos{\alpha} \sin{\beta}) & ((-\sin{\alpha} \sin{\beta}) + \cos{\alpha} \cos{\beta}) \end{array} \right] \\
$$

Using the angle sum identities:

$$
\sin{(\alpha + \beta)} = \sin{\alpha}\cos{\beta} + \cos{\alpha}\sin{\beta} \\
\cos{(\alpha + \beta)} = \cos{\alpha}\cos{\beta} - \sin{\alpha}\sin{\beta} \\
$$

You can simplify the result of the matrix multiplication:
$$
R_A^C = \left[ \begin{array}{ccc}
(\cos{\alpha} \cos{\beta} + (-\sin{\alpha} \sin{\beta})) & ((-\cos{\alpha} \sin{\beta}) + (-\sin{\alpha} \cos{\beta})) \\
(\sin{\alpha} \cos{\beta} + \cos{\alpha} \sin{\beta}) & ((-\sin{\alpha} \sin{\beta}) + \cos{\alpha} \cos{\beta}) \end{array} \right] \\
$$
$$
R_A^C = \left[ \begin{array}{ccc}
(\cos{\alpha} \cos{\beta} - \sin{\alpha} \sin{\beta}) & -(\cos{\alpha} \sin{\beta} + \sin{\alpha} \cos{\beta}) \\
(\sin{\alpha} \cos{\beta} + \cos{\alpha} \sin{\beta}) & (-\sin{\alpha} \sin{\beta} + \cos{\alpha} \cos{\beta}) \end{array} \right] \\
$$
$$
R_A^C = \left[ \begin{array}{ccc}
\cos{(\alpha + \beta)} & -\sin{(\alpha + \beta)} \\
\sin{(\alpha + \beta)} & \cos{(\alpha + \beta)} \end{array} \right] \\
$$

And that's how you get $R_A^C = R_A^B R_B^C$. It works out because the matrix multiplication results in angle additions.

Title: composition of rotation matrices


问:
What kp, ki, and kd should I keep?
```<code>  // MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include<Wire.h>
#include <Servo.h>
Servo firstESC, secondESC,thirdESC,fourthESC; //Create as much as Servoobject you want. You
const int MPU=0x68;  // I2C address of the MPU-6050
int speed1=2000,speed2=0,speed3=0,speed4;
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
float integ=0,der=0,pidx=0,kp = .5,ki=0.00005 ,kd=.01,prerror,dt=100;
void setup(){
  firstESC.attach(3);    // attached to pin 9 I just do this with 1 Servo
  secondESC.attach(5);    // attached to pin 9 I just do this with 1 Servo
  thirdESC.attach(6);    // attached to pin 9 I just do this with 1 Servo
  fourthESC.attach(9);    // attached to pin 9 I just do this with 1 Servo
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Wire.beginTransmission(MPU);
  Wire.write(0x1c);  // PWR_MGMT_1 register
  Wire.write(0<<3);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);
  firstESC.writeMicroseconds(0);
  secondESC.writeMicroseconds(0);
  thirdESC.writeMicroseconds(0);
  fourthESC.writeMicroseconds(0);
  firstESC.writeMicroseconds(2000);
  secondESC.writeMicroseconds(2000);
  thirdESC.writeMicroseconds(2000);
  fourthESC.writeMicroseconds(2000);
  delay(2000);
  firstESC.writeMicroseconds(700);
  secondESC.writeMicroseconds(700);
  thirdESC.writeMicroseconds(700);
  fourthESC.writeMicroseconds(700);
  delay(2000);
}
void loop(){
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,4,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  firstESC.writeMicroseconds(0);
  secondESC.writeMicroseconds(700-(pidx/10));
  thirdESC.writeMicroseconds(700+(pidx/10));
  fourthESC.writeMicroseconds(0);
  PID();
  //if(Serial.available())
    //speed1 = Serial.parseInt();
  //Serial.print("AcX = "); Serial.print(AcX);
  //Serial.print(" | AcY = "); Serial.print(AcY);
  //Serial.println();
  //delay(333);
}

void PIdD()
{
  float error;
  error = (atan2(AcY,AcZ)*180/3.14);
  now = millis();
  dt = now-ptime;
  if(error>0)error=180-error;
  else error = -(180+error);
  error=0-error;
  integ = integ+(error*dt) ;
  der = (error - prerror)/dt ;
  prerror=error;
  pidx = (kp*error);
  pidx+=(ki*integ);
  pidx+=(kd*der);
  if(pidx>1000)pidx=1000;
  if(pidx<-1000)pidx=-1000;
  ptime = now;
  }
</code>```

The above is my program for my quadcopter, but now I have to tune the PID values, that is kp, ki, and kd. My accelome is at 2g. Please point to me what is wrong with the program? Is the error signal not appropriate? Please also give me or help me choose correct PID tuning. My limitation is I always have to connect my Arduino to pc and change kp ki or kd values, that is I have no remote control available currently.

答:
Some things that are wrong in your program are:

You do not calculate your angular displacement properly, leading to a wrong error for the PID controller

You are only using one PID controller, for stabilization you need at least two

You just power two motors, why not all four?

You do not use a sampletime (the delay is commented). Your PID is likely to fail, regardless of the parameters.

EDIT:

you are trying to implement a digital PID controller. These type of controllers work best, when there is a specific time between the calculation of the controller. This time is usually called sampletime. The better you can keep this time constant the better the controller is able to perform. Usually this sampletime has to be short enought, that the controller is able to "see" distrubances. For your example the controller must be fast enought to see if the quadcopter has been tilted by the wind or similar things.

So basically for quadcopters you choose a control frequency of 50 Hz. For two reasons: First 20ms is fast enought to see disturbances and react on them. And the other point (a more technical one) ESC cannot be controlled faster then 50Hz, because they need a 20ms control signal (You create them with ESCxxx.WriteMicrosenconds())

The mistake in your implementation is that you do not have this timing factor(it is commented) so the controller is calculating new setpoints as fast as possible. As far as I know Arduinos it should be around 1-10kHz. This frequency is too high for your hardware and it is hard to tell if the timeing stays constant (at least I cannot). The I2C do not have to be constant in time. So your sampling frequency can change from iteration to iteration. This is usually the dead of a PID implementation. We (the team I'm working in in my company) invest a lot of work to keep samplingtime in controller implementation constant. You totally ignored this.

The parameters do not have to be multiplied or divided by any value, because they are not tuned at all. At the end: If you get your parameters tuned and you want to change the samplingtime, then you have to adjust those values. But you do not need to do this, in the untuned case.

EDIT2:

Without using a sampletime your system can be "non-controllable" at all. This depends on lot of different factors like used hardware and quality of the electric connections and the ICs.

Use the sampletime of 20ms and tune your PID Parameter, those values depend on your hardware setup. There are no: Take these values and it's fine.
You have to tune them and you there are tons of different methods. I recommend you to use an experimental iterative method (it's the best one, if you got no mathematical model or deeper controls knowledge). So guess some parameters and see how the system behaves. If is doing more or less nothing, then increase P. If it seem not controlled and bouncing up and down, then decrease the I. You will find lot of different (and well written) instructions online, which tell you how to tune PID controllers.

Title: What kp, ki, and kd should I keep?


问:
IRobot Create 2: Powering Up after Sleep
I've notice the IRobot Create 2 does not respond to the app's commands when it has been sleeping.  If I press the Clean button and re-run the app then the robot is responsive to the commands.

My initialization sequence (Android/Java) using usb-serial-for-android:

```<code>port.open(connection);
port.setParameters(115200, 8, UsbSerialPort.STOPBITS_1,UsbSerialPort.PARITY_NONE);
command(Opcode.START);
command(Opcode.SAFE);
</code>```

The physical architecture is IRobot Create 2 connected by IRobot Serial Cable to Google Project Tango Tablet.

How can my app wake up the Roomba from it's sleep?

答: Steve is right: orange and green wires are swapped in the official Create cable. If you swap the two wires the robot will wake up as soon as BRC is brought to zero. </p>


问:
How to use the Homogeneous transformation matrix?
I am trying to understand how to use, what it requires compute the homogenous transformation matrix.

I know 2 points from 2 different frames, and 2 origins from their corresponding frames.

I how transformation matrix looks like, but whats confusing me is how i should compute the (3x1) position vector which the matrix needs.  As i understand is, this vector a origin of the old frame compared to the new frame.  But how to calculate it, the obvious answer (I think) would be to subtract both ($O_{new} - O_{old}$ ), but it does not feel right.

I know its a simple question but my head cannot get around this issue, and how can i prove it the right way, with the information i know?

答:
A homogeneous transformation matrix $H$ is often used as a matrix to perform transformations from one frame to another frame, expressed in the former frame. The translation vector thus includes [x,y(,z)] coordinates of the latter frame expressed in the former. Perhaps that this already answers your question, but below is a more elaborate explanation.

The transformation matrix contains information about both rotation and translation and belongs to the special Eucledian group $SE(n)$ in $n$-D. It consists of a rotation matrix $R$ and translation vector $r$. If we permit no shear, the rotation matrix contains only information about the rotation and belongs to the orthonormal group $SO(n)$. We have:

$$H=\begin{bmatrix} R & r \\ \bar{0} & 1 \end{bmatrix}$$

Let's define  $H^a_b$ the transformation matrix that expresses coordinate frame $\Phi_b$ in $\Phi_a$, expressed in $\Phi_a$. $\Phi_a$ can be your origin, but it can also be an other frame.

You can use the transformation matrix to express a point $p=[p_x\ p_y]^\top$ (vectors) in another frame:
$$P_a = H^a_b\,P_b$$
$$P_b = H^b_c\,P_c$$
with
$$P = \begin{bmatrix} p \\ 1 \end{bmatrix}$$
The best part is that you can stack them as follows:
$$P_a = H^a_b H^b_c\,P_c = H^a_c\,P_c $$
Here a small 2 D example. Consider a frame $\Phi_b$ translated $[3\ 2]^\top$ and rotated $90^\circ$ degrees with respect to $\Phi_a$.
$$H^a_b = \begin{bmatrix}\cos(90^\circ) & -\sin(90^\circ) & 3 \\ \sin(90^\circ) & \cos(90^\circ) & 2 \\ 0 & 0 & 1   \end{bmatrix}=\begin{bmatrix}0 & -1 & 3 \\ 1 & 0 & 2 \\ 0 & 0 & 1   \end{bmatrix}$$
A point $p_b=[3\ 4]^\top$ expressed in frame $\Phi_b$ is
$$\begin{bmatrix}p_{a,x} \\ p_{a,y} \\ 1 \end{bmatrix} = \begin{bmatrix}0 & -1 & 3 \\ 1 & 0 & 2 \\ 0 & 0 & 1   \end{bmatrix}\begin{bmatrix}3 \\ 4 \\ 1  \end{bmatrix}=\begin{bmatrix}-1 \\5 \\1 \end{bmatrix} \to p_a = \begin{bmatrix}-1\\5\end{bmatrix}$$
Try to make a drawing to improve your understanding.


问:
What factors should i consider when selecting a motor for a free wheeled cart-pole balancing robot?
I'm developing a small scale cart-pole balancing robot consisting of two wheels driven by a single motor at the base (essentially like a unicycle, but with two wheels to constrain balance to a one dimensional problem).

I'm not sure what qualities to look for in that motor.  I think the motor should be able to accelerate quickly in directions opposite of motion as dictated by the control system.  However, i'm not sure if this rapid acceleration should correlate with higher torque motors or faster speed motors.  I think higher torque motors would be too slow to react to control commands.  In contrast, fast speed motors may not be able to overcome the momentum of the cart.

Are there any design equations or other calculations i can make based on my robot's dimensions and weight to determine the right specs needed for my robot's motor?  How can i determine the right motor specs for this application without resorting to brute-force trial & error experiments?

答:
High acceleration from a static (balanced) configuration is only related to high torque. However, the maximum torque you can deliver decreases as function of speed. So, in the end it depends on both. In theory you could use a tiny motor with a huge gear ratio able to deliver a lot of torque, but it speeds up really quickly, meaning that it will fail coping with larger disturbances.  It is recommended to run some simulations of desired behaviour. Then calculate the required power by multiplying velocity and torque curves of the actuated joint.

Find a motor (brushed) that is able to deliver this power. Depending on a desire for efficiency or size, select a motor based on its nominal power or all the way up to near-maximum mechanical power respectively, or somewhere in between. I would recommend the former for now, as it's an easier approach. Nominal power is often given, or easily calculated by multiplying the nominal velocity and nominal torque. Maximum mechanical power is higher, and approximated by multiplying the no-load speed and stall torque divided by four.

Lastly, you will need a transmission to obtain desired torque-velocity characteristics. For instance, if your motor is able to deliver 10 mNm at 1000 rpm, whereas you need 2 Nm at 5 rpm, then use a transmission ratio of $R=2000/10=200$.

Update your simulation model by including the motor's reflected inertia $R^2\,I_{rotor}$ and mass to check whether your motor is still capable of delivering sufficient power. If not, then you have to re-iterate motor selection. Also, check your electrical power source and motor controller. Are they capable of delivering the required voltage and current?