问:
motor inertia tensor?
In modeling dynamics of a robot ,in which servo motor is adjusted inside the link, there is a need to find inertia tensor of the motor itself,Right?

So if it is needed how can i get the inertia tensor of motor since i couldn't find its solid works model having internal components,i mean gears and other stuff(with related specified materials)?

答:
It is a little difficult to understand exactly what you are needing.   The motor specs should have a value for rotor inertia.   The gears would depend a lot on their design, but I have always been able to ignore their rotary inertias and just use the gear ratio in the overall drive train inertia calculations.   Those would be for the inertias distal to the motor.   For the inertias seen by the proximal motors, it is usually sufficient to model it as a point mass located at its cg.

Or, are you trying to inderstand how to create the inertia and mass matrices?

Title: motor inertia tensor?


问:
Testbed for testing navigation algorithms
I'm looking for a testbed (simulator or web-based interface that lets me to have control on a robot) for testing different routing and navigation algorithms. Is there such a system on the web?</p>

答: You could use player/stage or gazebo</p>

ContentLicense: CC BY-SA 3.0

Body: You could use player/stage or gazebo


问:
Difference between Degrees of Freedom (DOF) and Degrees of Motion (DOM)
What is a difference between degrees of freedom (DOF) and degrees of motion (DOM)? I know that DOF is the number of independent movements a manipulation arm can make and robot system can have max 6 independent DOF and unlimited number of DOM but I do not distinguish them from each other.</p>

答:
The way you are describing it, DOM is the number of independent dimensions in $\vec q$.   DOF is the number of independent dimensions in $\vec x$.  In practice, a robotics engineer will use DOF to represent the number of independent actuators of the robot, which you are calling DOM.   Better notation would be to call DOM the mobility of the system, as described in the Mobility formula section here.

Using your notation:

Think of a telescoping pole made out of 5 sections that can collapse inside each other.   The degree of freedom is the length of the pole.  That value can be anything between its fully-collapsed length, up to its fully-extended length.  DOF = 1.

If there is a sliding joint between each section of the pole, there will be four independent motions you can use to set the total length.  DOM = 4.

In kinematics, the length would typically be denoted by $x$, and the individual joint lengths would be assigned values of $q_1$ through $q_4$.

$\vec x = f(\vec q)$


问:
How can my robot find its position in any given map without GPS, including when the initial point is not given?
Consider this map

The Contest arena shown in figure 1 consists of two sub arenas, both the sides are identical to each other and their scientists and safe zone locations are similar.

Each sub arena has 3 different colored rooms and a fourth shared room. Each robot will be placed at identical start locations, respective to their arena. These locations will be random and anywhere on the map.

Each room (other than the shared room) will have two entry and exit gates. Both of these gates will be open at all times. The robot can enter and exit from any gate it chooses.

答:
I would say that you have 2 main possibilities:

You base your computation on the IMU (Inertial Measurement Unit) of your robot. Usually from those you can basically have an acceleration, then you need to integrate it 2 times to get the position and therefore the displacement in meters.

You base your computation on simple encoders. Encoders are sensors which  convert the angular position or motion of a shaft to an analog or digital code. From this, if you know the radius of the wheel (which you should assume constant, if the wheel does not change over time) you will know the displacement in meters associated to the displacement read by the encoder.

I hope this helped you.


问:
How to check for a sharp angle with a line follower?
I have the mBot robot and I want to program it to follow the line. So far it can pass any kind of line that is >90°.

I want it to be able to pass 90°-ish angles as well. Like this one:

90° angle">

The problem is that my mBot robot has only 2 line following sensors (they are 5 mm apart and the line is 2 cm wide) so I can't use just the sensors.

Most of the times it just goes to the line and when it's supposed to turn it just misses the line (goes on the white) and goes back to get back on track. Once it's back on the black line it once again tries to go forward but goes on the white instead of taking a turn. This happens endlessly.

Sometimes it passes the angle by going back and forth and accidentally turning, but that's not even a workaround, let alone a solution.

Here's a test course of the first round of the competition.

My robot can pass this without a problem, but it gets stuck on this (poorly edited, sorry) course:

It can't pass the 20 block if the robot enters it from a 15 or 20 block (so basically it gets stuck if it's coming from an angle and hits a 90 degree turn).

The sensor value could be read as either 0, 1, 2 or 3 depending on what the robot currently sees:

0 - on the line

1 - on the right of the line

2 - on the left of the line

3 - not on the line

Pseudo code of my current program:

```<code>loop forever:
    if (on the right of the line):
        turn_left()
    if (on the left of the line):
        turn_right()
    if (on the line):
        go_forward()
    if (not on the line):
        go_backwards()
</code>```

So how would I go about taking such sharp turns?

答:
Sorry I have no experience of line following robots, I use ultrasonic sensors, so this answer may be a bit naïve and there allsorts of reasons why you can't do this.

Your bot travels forwards and realises it has lost the line.  Assuming that the distance travelled is not "very far", less than 1/2 the length of the bot, you should be able to do a complete 360 and find two lines.  One of those lines will be at about 180 degrees from where you started turning (this should be measurable by the number of turns of the motor, if its a stepper), this is not the line you are looking for, you need to follow the other one.

Does that help?


答:
The problem seems to be that your robot functions by moving its sensors perpendicular to the line; when the line becomes perpendicular to itself, the robot loses the assumption that lateral movement will bring the line back into the center of the sensors.

The only way to fix is this is to change the sensor configuration (adding more if necessary) to break this ambiguity.

Title: How to check for a sharp angle with a line follower?

Body:
I have the mBot robot and I want to program it to follow the line. So far it can pass any kind of line that is >90°.

I want it to be able to pass 90°-ish angles as well. Like this one:

90° angle">

The problem is that my mBot robot has only 2 line following sensors (they are 5 mm apart and the line is 2 cm wide) so I can't use just the sensors.

Most of the times it just goes to the line and when it's supposed to turn it just misses the line (goes on the white) and goes back to get back on track. Once it's back on the black line it once again tries to go forward but goes on the white instead of taking a turn. This happens endlessly.

Sometimes it passes the angle by going back and forth and accidentally turning, but that's not even a workaround, let alone a solution.

Here's a test course of the first round of the competition.

My robot can pass this without a problem, but it gets stuck on this (poorly edited, sorry) course:

It can't pass the 20 block if the robot enters it from a 15 or 20 block (so basically it gets stuck if it's coming from an angle and hits a 90 degree turn).

The sensor value could be read as either 0, 1, 2 or 3 depending on what the robot currently sees:

0 - on the line

1 - on the right of the line

2 - on the left of the line

3 - not on the line

Pseudo code of my current program:

```<code>loop forever:
    if (on the right of the line):
        turn_left()
    if (on the left of the line):
        turn_right()
    if (on the line):
        go_forward()
    if (not on the line):
        go_backwards()
</code>```

So how would I go about taking such sharp turns?


答:
Determining your location when you have a map but not your starting location is a job for a particle filter.

(Wikipedia's entry on particle filters is not very helpful to beginners.)  See also, this question.

Here's an animation of a particle filter in use.  The black boxes are walls, the green turtle is the robot, the red marks are possible locations, and the grey circle is the center point of all the estimates.  As the robot moves, the possible locations become more tightly grouped until there is no more ambiguity.

The idea is that by moving around the space and measuring the distance to walls around you (or by bumping into them), you can get a better sense of where you "might be".

For example: in the map you showed, imagine that your robot moves forward by 4 spaces but does not hit a wall.  There are only a few spots on the map where this is possible, so now you know you must be in one of those places.  The actual movement doesn't matter, but as you build up your knowledge of where the walls are (and where you don't find walls), some moves will be more useful in determining your location than others.  And eventually, you'll have roughly 100% confidence in your location.

Title: How can my robot find its position in any given map without GPS, including when the initial point is not given?

Body:
Consider this map

The Contest arena shown in figure 1 consists of two sub arenas, both the sides are identical to each other and their scientists and safe zone locations are similar.

Each sub arena has 3 different colored rooms and a fourth shared room. Each robot will be placed at identical start locations, respective to their arena. These locations will be random and anywhere on the map.

Each room (other than the shared room) will have two entry and exit gates. Both of these gates will be open at all times. The robot can enter and exit from any gate it chooses.


答:
Generally, you shouldn't worry about modeling the dynamics.  You only need to do this in the case where you've got everything set up, but things aren't working as accurately as you'd like them to.  And in that case, the inertia that caused the overshoot should be visible in your data -- you can use the direct measurement instead of the theoretical calculation.

If you're asking about this because you're planning to push the servo to its physical limits, you should probably rethink your design.

Title: motor inertia tensor?

Body:
In modeling dynamics of a robot ,in which servo motor is adjusted inside the link, there is a need to find inertia tensor of the motor itself,Right?

So if it is needed how can i get the inertia tensor of motor since i couldn't find its solid works model having internal components,i mean gears and other stuff(with related specified materials)?


问:
What is the opposite of 'Antagonistic'?
A robotic joint is connected to two actuators, e.g. air muscles. One flexes the joint, while the other extends it. This arrangement is called 'antagonistic'.

But what if I had an electric motor instead of the air muscles? In that case it can only pull on one tendon at a time, and it's not antagonistic. What it the arrangement called in this case? Untagonistic?

答:
If you have an electric motor which can drive a linear system in two directions, essentially it is its own antagonist.

Edit:

muscles, as well as single-acting pneumatic cylinders, can only provide force in one direction, so they are arranged in agonist/antagonist pairs.

A motor, perhaps running a belt, is capable of pulling the belt in each direction, thereby providing motive force to what you might consider to be the agonist belt in one direction and the antagonist belt in the other.

Title: What is the opposite of 'Antagonistic'?


问:
How can a quadcopter be made to hover perfectly still?
I need to get my drone flying still enough that I can rest a glass of water on it.

I've tried a few KK boards and APM 2.6 (3.1 software). I've balanced props, set PID settings, auto-trim / autotune and the drone still tends to inconsistently drift a little one way or another.

What is a plausible way to completely isolate drift?

答:
I am not a UAV guy, so others may be able to provide more specific answers.   But from a controls perspective, I would break the problem into two separate areas:  aerodynamics and controller response.

For aero, you want to consider both the inherent stability of your system, and the flight dynamics.   Essentially you want the uncontrolled system to be as stable as possible, then add the controller to handle unwanted disturbances.  For example, where is the cg of your robot?  Would moving some masses lower lead to a more naturally stable system?  You want as much inherent stability in the mechanics as you can get, so your controller can use its energy to combat (reject) the disturbances.  How else can you improve its aerodynamics?  What about a larger propeller, or more prop blades?  Or stronger motors?  Or a lighter frame?  Getting a higher lift to weight ratio is a benefit to stability.  Also, consider the system's cross-section if wind is contributing to the unsteadiness.   A tapered profile may help prevent impulsive disturbances in windy conditions.   These areas are complicated and beyond my experience.   But hopefully they will help you look into the system's natural dynamics to see if improvements can be made there.

For the control responsiveness, there are also many items that may help.  Of course, faster control loop rates, and highly accurate sensors help.  What is the loop rate of your controller?  Can you program it more efficiently?   Or use a faster micro?  And, is the most current sensor data getting to the controller quickly?  I suspect a loop rate of around 1 kHz, and sensors with that type of bandwidth, would be required to achieve the type of stability you desire.  I would also look at the responsiveness and accuracy of your altitude and attitude sensors.   If you are using an IMU, could you add other sensors to get more accurate and/or faster data?  What about a downward-pointing (possibly laser) rangefinder to get very peppy altitude measurements?  Does your power supply and controller have enough slew rate to accommodate the control output changes needed for stability?  Finally, what model are you using in your controller?  A basic PID controller should be pretty good if the above items are okay, but if you still have nonsteady motion you might need to look into feedforward control, or maybe H$\infty$ control to get the stability you need.

I know this isn't a real answer, but hopefully it gives you some ideas.


问:
Quadcopter PID Control: Is it possible to stabilize a quadcopter considering only angle measurements?
Good day,

I am a student currently working on an autonomous quadcopter project, specifically the stabilization part as of now. I am using a tuned propeller system and I also already considered the balancing of the quadcopter during component placements. I had been tuning the PID's of my quadcopter for the past 3 1/2 weeks now and the best I've achieved is a constant angle oscillation of the quadcopter by +-10 degrees with 0 degrees as the setpoint/desired angle. I also tried a conservative 7 degrees setpoint with the same results on the pitch axis.

As of now my PID code takes in the difference of the angle measurement from the complementary filter ( FilteredAngle=(0.98)*(FilteredAngle + GyroAngleVel*dt) + (0.02)*(AccelAngle) ) and the desired angle.

I have read somewhere that it is IMPOSSIBLE to stabilize the quadcopter utilizing only angle measurements, adding that the angular rate must be also taken into consideration. But I have read a lot of works using only a single pid loop with angle differences (Pitch Yaw and Roll) as the input.

In contrast to what was stated above, I have read a comment from this article (https://www.quora.com/What-is-rate-and-stabilize-PID-in-quadcopter-control) by Edouard Leurent that a Single PID control loop only angle errors and a Cascaded PID loop (Angle and Rate) that utilizes both angle errors and angular velocity errors are equivalent Mathematically.

If I were to continue using only the Single PID loop (Angle) method, I would only have to tune 3 parameters (Kp, Ki & Kd).

But if I were to change my code to utilize the Cascaded Loop (Angle and Angular Velocity),

Would I have to tune two sets of the 3 parameters (Kp, Ki & Kd for angle and Kp, Ki & Kd for the angular velocity)?

Would the cascaded PID control loop give better performance than the single PID control loop?

In the Cascaded Loop, is the set point for the angular velocity for stabilized flight also 0 in deg/sec? What if the quadcopter is not yet at its desired angle?

Thank you :)

答:
It is possible to stabilize a quadcopter using only angle measurements in a single loop pid. However it is easier to stabilize a quadcopter using a cascaded PID controller. Yes you are tuning more parameters. Firstly you tune first the inner loop rate PID controller using the gyroscope's (the fast sensor, but drifts) angular rate readings then tune the outer loop stabilize PID using an angle setpoint and angle measurements from the sensor fusion of the angle readings from both the accelerometer and the angles integrated from the angular velocity readings from the gyroscope. I found that it was the easiest way to achieve stable flight coupled with my now current control loop rate of 530Hz.

Other related helpful questions with answers:

PID output does not reach setpoint precisely enough

Need help for a quadcopter PID

Resources:

https://www.quora.com/What-is-rate-and-stabilize-PID-in-quadcopter-control


问:
Smart Home Model with Raspberry Pi
I'm still new to RPi and I am currently trying to do a smart home model.

I planned to use RPi only to control 5 servos (which will be controlling the open/close of the doors by setting the angle) and 5 LEDs.

Will I need to use an external circuit to supply the power for the servos or is it fine to just connect them to the RPi?

答:
Though, there is not much information about your servos let's assume that they consume at least 1A each. This value will probably be higher given that you want to move doors and that requires torque.

Now let's see the RPi's capabilities. An answer from Stack Exchange Raspberry Pi, tells you the necessary information and sources about the RPi's power rails and GPIOs.

The 3.3 V supply can source a maximum of 50 mA, as the GPIOs are driven from this rail too. A GPIO can source max 16 mA, the overall current sourced by GPIOs cannot exceed the 3.3 V rail's max 50 mA.

This is far not enough but also 3.3 V would be too low for supplying a servo, as servos usually need 4-6 V.

The 5 V supply rail is limited by the power supply that powers the RPi, minus the RPi's current consumption. You did not provide what model do you use, so here is a table with each (except RPi3) models power requirements.

Now, if you want to power your servos from the RPi's 5 V rail, your maximum current is at least 5A + RPi's consumption. If you check, most USB micro type B connectors are rated on 1.8 A, but even if the connector can handle 5-6 A, driving this much current through the board just to supply your servos would be risky.

The best solution would be an external supply which provides 6 V as your servos will have higher torque at 6 V. Also it must be able to supply six times a servo's current.

A possible block diagram:

The driving circuit can be a simple current limiting resistor an each pin of the RPi or you can use an appropriate small-signal FET as an interface.

As for connecting the LEDs, you can use a simple current limiting resistor and GPIOs, but be careful to not exceed the 50 mA of the 3.3 V rail.

Title: Smart Home Model with Raspberry Pi


问:
Principle of virtual force - General help in understanding / explanation
I'm an Electronics student taking a module in Robotics.

From the example,

I understand line 1 as the Jacobian is found from the time derivative of the kinematics equation and such relates joint angles to velocity.

I do not understand why the transpose has been taken on line 3 and how line 4 is produced.

答:
How the third equation is used in the derivation would be helpful in order to explain why $\delta x$ is being transposed.   But when you take $$\delta x = J \delta q$$ and transpose it, you must reverse the order of the terms of the right hand side.   This makes sense if you consider the dimensions of $J$.   If $x \in \Re^n$ and $q \in \Re^m$ then $J$ is $n$ x $m$.  To postmultiply $q^T$ (which is 1 x $m$) you must transpose $J$.

The fourth equation is just a statement that the seesaw is in equilibrium - the total moment is zero.


答:
Using existing resources:

Very similar answer to @SteveO - I'd focus on tuning the PID control loop for each motor individually, if possible, to eliminate any instability in the power delivery which would result in the aircraft leaning and therefore drifting.

A great video tutorial on PID control that helped me through a control exam

Using additional resources:

The most stable implementation of drift elimination I've ever seen used an optical flow sensor on the base of the aircraft - much like the old-school optical mouse sensors this looks for an offset of pixels in a certain direction through a small camera. The resultant vector of this movement then becomes your drift value, which is corrected by adjusting the power output in the opposite direction. This is what the Parrot Quadcopters in my lab use and their stability is fantastic.

APM's page on adding an optical flow sensor to your drone controller

Title: How can a quadcopter be made to hover perfectly still?

Body:
I need to get my drone flying still enough that I can rest a glass of water on it.

I've tried a few KK boards and APM 2.6 (3.1 software). I've balanced props, set PID settings, auto-trim / autotune and the drone still tends to inconsistently drift a little one way or another.

What is a plausible way to completely isolate drift?


问:
Quadcopter Flight Controller:Why does Using gyroscope data give better results?
I have succeeded in making my first quadcopter from scratch with a readymade frame. I designed the flight controller myself with help from YMFC-3D youtube series of videos. https://www.youtube.com/watch?v=2pHdO8m6T7c

But in the process, I discovered that using the euler angles or the 'ypr' values from the MPU6050 as the feeback to the PID loop makes it super difficult to tune the quadcopter and even then it doesn't fly great.

Whereas although not intuitive to me, using the gyroscope values with a complementary filter instantly made the quad respond much better and the tuning also was not too difficult.

Let me clearly define the response in both cases.

Using ypr values:-
+Always keeps overshooting or 'underreaching'
+Very small range of values that can let the quad fly stable
+Drastic Reactions to extreme values of P (Kp)values

Using gyro values:-
+Reaction is much more stable
+Tuning the PID was also simple
+ Even under high values of P(Kp) the quad might crash due to oscillations but not flip or react extremely

Below is a portion of the PID loop:

```<code>//gyrox_temp is the current gyroscope output

gyro_x_input=(gyro_x_input*.8)+(gyrox_temp*0.2);//complementary filter

pidrate_error_temp =gyro_x_input - setpoint;//error value for PID loop

pidrate_i_mem_roll += pidrate_i_gain_roll * pidrate_error_temp;
//integral portion

pidrate_output_roll = pidrate_p_gain_roll * pidrate_error_temp + pidrate_i_mem_roll + pidrate_d_gain_roll * (pidrate_error_temp - pidrate_last_roll_d_error);
//output of the pid loop
/pidrate_p_gain_roll-Kp
//pidrate_i_gain_roll-Ki
//pidrate_d_gain_roll-Kd
//this output is given as the pwm signal to the quad plus throttle
</code>```

答:
The gyro is a less-noisy sensor with gradual drift, the accelerometer is a noisy sensor with integration error (due to the noise). Using them both in conjunction, with a set ratio of either sensor's information can improve the performance of an IMU (Inertial Measurement Unit) dramatically, as well as tuning the sample rate of either sensor.

The problem with the MEMS accelerometers and gyros you get in cheaper implementations of flight controllers etc is that they're actually so susceptible to noise that their drift is usually measured in degrees a second, so the longer they're running the worse they get.

A 'step-up' from complementary filtering is sensor fusion, which involves blending in data from sensors external to your IMU, usually GPS, to further combat the issues you've been having.

To further understand the characteristics of these sensors and the constraints I'd recommend this primer on 'Allan Deviation' which shows you how to characterise accelerometers and gyroscopes in order to optimise drift and noise removal, as well as sample rates. There's also this incredible AHRS algorithm developed by Sebastian Madgwick which blows my mind a bit.


问:
E: Unable to locate package ros-jade-desktop-full
I want to install ROS on my Xubuntu 16.04, Xenial Xerus. I have followed the ROS's site instruction: http://wiki.ros.org/jade/Installation/Ubuntu, and did the following: First, setup my sources.list:

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

Second, set up keys:

sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 0xB01FA116

Then, make sure my package is up-to-date:

sudo apt-get update

Last, try to install ROS jade:

sudo apt-get install ros-jade-desktop-full

And get this error:

E: Unable to locate package ros-jade-desktop-full

Where did I go wrong, and how can I get ROS (any version is ok) running on my Xubuntu 16.04?

答:
On the page you indicated http://wiki.ros.org/jade/Installation/Ubuntu it is said that it only support 14.04 14.10 and 15.04, therefore i don't think it is available yet for the 16.04 version.
Either try to see via auto-completion if there is any other version available,
or compile from source.

You will have better answers on the ros site http://answers.ros.org/question/226098/ros-on-ubuntu-xenial-1604/


问:
Step size in numerical differentiation
I get position information and a corresponding timestamp from a motion tracking system (for a rigid body) at 120 Hz. The position is in sub-millimeter precision, but I'm not too sure about the time stamp, I can get it as floating point number in seconds from the motion tracking software. To get the velocity, I use the difference between two samples divided by the $\Delta t$ of the two samples:

$\dot{\mathbf{x}} = \dfrac{\mathbf{x}[k] - \mathbf{x}[k-1]}{t[k]-t[k-1]}$.

The result looks fine, but a bit noisy at times. A realized that I get much smoother results when I choose the differentiation step $h$ larger, e.g. $h=10$:

$\dot{\mathbf{x}} = \dfrac{\mathbf{x}[k] - \mathbf{x}[k-h]}{t[k]-t[k-h]}$.

On the other hand, peaks in the velocity signal begin to fade if I choose $h$ too large. Unfortunately, I didn't figure out why I get a smoother signal with a bigger step $h$. Does someone have a hint? Is there a general rule which differentiation step size is optimal with respect to smoothness vs. "accuracy"?

This is a sample plot of one velocity component (blue: step size 1, red: step size 10):

答:
This answer valid only if $\Delta{t} = \mathbf{t}[k] - \mathbf{t}[k-1]$ is a constant. Then you can rewrite your equation as:
$$\dot{\mathbf{x}} = \dfrac{\mathbf{x}[k] - \mathbf{x}[k-1]}{\Delta{t}}$$

Consider:

$$
\dot{\mathbf{x}}_l = \dfrac{1}{h}\sum_{i=1}^{h}\dot{\mathbf{x}_i} =
\dfrac{(\mathbf{x}[k] - \mathbf{x}[k-1])+(\mathbf{x}[k-1] - \mathbf{x}[k-2])+\dotsb+(\mathbf{x}[k-h+1] - \mathbf{x}[k-h])}{h\Delta{t}}
= \bigg(\dfrac{\mathbf{x}[k] - \mathbf{x}[k-h]}{h\Delta{t}}\bigg)
$$

$$
h\Delta{t} = \mathbf{t}[k] - \mathbf{t}[k-h]
$$

Here $\dot{\mathbf{x}}_i$ is the $i^{th}$ sample of the reading and passing it through a moving average filter (which is a low pass filter) you can obtain $\dot{\mathbf{x}_l}$. So $\dot{\mathbf{x}_l}$ is smooth as it is a low pass signal. When you increase the value of $h$ you can minimize the bandwidth of $\dot{\mathbf{x}_l}$. So when you increase the value of $h$ the result is getting more smoother. So peaks begin to fade(Peaks means high frequency components).

As I know there isn't a generalize way to determine $h$ to get the result smoother and accurate. You have to choose appropriate $h$ by a trial and error or if you know the transfer function of the sensor, you can use that to determine an appropriate value for $h$.

Title: Step size in numerical differentiation


问:
Are all Flight Controllers and Remote Controls using the same protocol?
I'm about to start a project, where I'm sniffing data between remote controls and flight controllers on RC copters and doing stuff with that information.  Do all (or most) flight controllers use the same protocol to communicate with the remote controls, or does it vary based on which one you buy?  I would be testing on drones (DJI phantom and the like).

So, my real question is:

If I want to write something to read the data, will I need to buy a different flight controller for each protocol used, or do they all use the same protocol, and I can just buy one flight controller, and the info I can get out will be the same for all types of flight controllers?

Also, are the protocols only spoken by the ground remote control and the flight controller?  Does the receiver care what protocol is being used, or is it just a middle man?

答:
There are standard protocols, and then there are proprietary protocols.  The choice of what protocol to use depends on many things.

Without listing the precise set of flight controllers you are planning to buy, it's difficult to provide anything more than a very general answer to this question.


问:
How to apply A bang-bang signal of amplitude 1 N and 1 s width as an input force to reproduce certain results in Matlab?
I working on dynamic modeling and simulation of a mechanical system (overhead crane), after I obtained the equation of motion, in the form: $$ M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)=Q $$

All the matrices are know inertia, $ M(q)$, Coriolis-Centrifugal matrix $ C(q,\dot{q})$, and gravity $ G(q)$ as functions of the generalized coordinates $q$, and their derivatives $\dot{q}$.

I want to solve for $q$, using Matlab ODE (in m-file), I got the response for some initial conditions and zero input, but, I want to find the response, for the aforementioned control signal (A bang-bang signal of amplitude 1 N and 1 s width), I'm trying to regenerate some results from the literature, and what the authors of that work said, regrading the input signal is the following: "A bang-bang signal of amplitude 1 N and 1 s width is used as an input force, applied at the cart of the gantry crane. A bang-bang force has a positive (acceleration) and negative (deceleration) period allowing the cart to, initially, accelerate and then decelerate and eventually stop at a target location." I didn't grasp what do they mean by bang-bang signal, I know in Matlab we could have step input, impulse, ...etc. But bang-bang signal, I'm not familiar with. According to this site and this bang bang is a controller rather.

Could anyone suggest to me how to figure out this issue and implement this input signal? preferably in m-file.

The code I'm using is given bellows, two parts:

```<code>function xdot = AlFagera(t,x,spec)
% xdot = zeros(8,1);
xdot = zeros(12,1); % to include the input torque

% % Crane Specifications
mp = spec(1);
mc = spec(2);
mr = spec(3);
L = spec(4);
J = spec(5);

g = 9.80;               % accelatrion of gravity (m/s^)

% % matix equations
M11 = mr+mc+mp; M12 = 0; M13 = mp*L*cos(x(3))*sin(x(4)); M14 = mp*L*sin(x(3))*cos(x(4));
M21 = 0; M22 = mp+mc; M23 = mp*L*cos(x(3))*cos(x(4)); M24 = -mp*L*sin(x(3))*sin(x(4));
M31  = M13; M32 = M23; M33 = mp*L^2+J; M34 = 0;
M41 = M14; M42 = M24; M43 = 0; M44 = mp*L^2*(sin(x(3)))^2+J;
M = [M11 M12 M13 M14; M21 M22 M23 M24; M31 M32 M33 M34; M41 M42 M43 M44];

C11 = 0; C12 = 0; C13 = -mp*L*sin(x(3))*sin(x(4))*x(7)+mp*L*cos(x(3))*cos(x(4))*x(8);
C14 = mp*L*cos(x(3))*cos(x(4))*x(7)-mp*L*sin(x(3))*sin(x(4))*x(8);
C21 = 0; C22 = 0; C23 = -mp*L*sin(x(3))*cos(x(4))*x(7)-mp*L*cos(x(3))*sin(x(4))*x(8);
C24 = -mp*L*cos(x(3))*sin(x(4))*x(7)-mp*L*sin(x(3))*cos(x(4))*x(8);
C31 = 0; C32 = 0; C33 = 0; C34 = -mp*L^2*sin(x(3))*cos(x(3))*x(8);
C41 = 0; C42 = 0; C43 = -C34; C44 = mp*L^2*sin(x(3))*cos(x(4))*x(7);

C = [C11 C12 C13 C14; C21 C22 C23 C24; C31 C32 C33 C34; C41 C42 C43 C44];
Cf = C*[x(5); x(6); x(7); x(8)];

G = [0; 0; mp*g*L*sin(x(3)); 0];

fx = 0;

if t >=1 && t<=2
fy = 1.*square(t*pi*2);
else fy = 0;
end

F =[fx; fy; 0; 0];     % input torque vector,

xdot(1:4,1)= x(5:8);
xdot(5:8,1)= M\(F-G-Cf);
xdot(9:12,1) = F;
</code>```

And:

```<code>clear all; close all; clc;

t0 = 0;tf = 20;

x0 = [0.12 0.5 0 0, 0 0 0 0,0 0 0 0];  % initional conditions

% % spectifications
Mp = [0.1 0.5 1];      % variable mass for the payload
figure
plotStyle = {'b-','k','r'};
for i = 1:3
mp = Mp(i);
mc = 1.06; mr = 6.4;           % each mass in kg
L = 0.7; J = 0.005;            % m, kg-m^2 respe.
spec = [mp mc mr L J];
% % Call the the function
[t,x] = ode45(@(t,x)AlFagera(t,x,spec),[t0 :0.001: tf],x0);

legendInfo{i} = ['mp=',num2str(Mp(i)),'kg'];

fx = diff(x(:,9))./diff(t);
fy = diff(x(:,10))./diff(t);
tt=0:(t(end)/(length(fx)-1)):t(end); % this time vector

% to plot the cart positions in x and y direcitons
subplot(1,2,1)
plot(t,x(:,1),plotStyle{i})
axis([0 20 0 0.18]);
grid
xlabel('time (s)');
ylabel('cart position in x direction (m)');
hold on
legend(legendInfo,'Location','northeast')

subplot(1,2,2)
plot(t,x(:,2),plotStyle{i})
axis([0 20 0 1.1]);
grid
xlabel('time (s)');
ylabel('cart position in y direction (m)');
hold on
legend(legendInfo,'Location','northeast')

end

% to plot the input torque, (bagn-bang signal), just one sample
figure
plot(tt,fy)
grid
set(gca,'XTick',[0:20])
xlabel('time (s)');
ylabel('input signal, f_y (N)');
</code>```

Furthermore, the results I'm getting and what I supposed to get are shown:

Major difficulties, initial conditions are not clearly stated in the paper, the input force direction, is only in y (which it should be), or it has different direction. I appreciate any help.

the paper I'm trying to recreate is:
  R. M. T. Raja Ismail, M. A. Ahmad, M. S. Ramli, and F. R. M. Rashidi, “Nonlinear Dynamic Modelling and Analysis of a 3-D Overhead Gantry Crane System with System Parameters Variation.,” International Journal of Simulation–Systems, Science & Technology, vol. 11, no. 2, 2010.
  http://ijssst.info/Vol-11/No-2/paper2.pdf

答:
In bang-bang control, control inputs can only be activated similar to a switch: when they are engaged they are fully on positive, or fully on negative.   Some bang-bang control studies (not many that I can recall) account for a slew rate of the controller's output, so the outputs are not modelled as square waves, but rather as trapezoids.   I think this is the "bang-bang signal" you need to generate.  The phrasing is odd.

It sounds as if you need to add a control force to your model that is just a trapezoid (acceleration period, constant full-on period, and deceleration period).  In m-code it would be something like the following.  Here I am assuming your step size is 1 msec, with a 10% slew time.  You may want different slew parameters.  I've written it so you can change step size and slew percents in one place.

```<code>max_F = 1.0;
steps_per_sec = 1000;
slew_up = 0.1;
slew_down = 0.1;
accel_steps = floor(steps_per_sec * slew_up);
decel_steps = floor(steps_per_sec * slew_down);
full_on_steps = steps_per_sec - accel_steps - decel_steps;

loops = 1;
// acceleration period
for i = 1:accel_steps
    F(i) = i * max_F / accel_steps;
    loops = loops + 1;
end
// full-on period
for i = loops : accel_steps + full_on_steps
    F(loops) = max_F;
    loops = loops + 1;
end
// deceleration period
for i = loops : accel_steps + full_on_steps + decel_steps
    F(loops) = max_F - (i - accel_steps - full_on_steps) * max_F / decel_steps;
    loops = loops + 1;
end
// after input is back to zero
for i = loops : 10 * steps_per_sec
    F(i) = 0.0;
end

// now apply F to your simulation
</code>```

@AlFagera, Thanks for posting a reference to the actual paper.  Following the references and re-searching led me to this paper:  M. S. Alam, M. A. Hossain, and M. O. Tokhi, Design of Command Shaper Using Gain-Delay Units and Particle Swarm Optimisation for Vibration Control of a Flexible System, 14th International Congress on Sound & Vibration, Cairns, Australia 2007 (https://www.acoustics.asn.au/conference_proceedings/ICSV14/papers/p197.pdf).  In that paper they describe a bang-bang signal as I've done above, except their acceleration and deceleration periods are quite short.

Body:
In bang-bang control, control inputs can only be activated similar to a switch: when they are engaged they are fully on positive, or fully on negative.   Some bang-bang control studies (not many that I can recall) account for a slew rate of the controller's output, so the outputs are not modelled as square waves, but rather as trapezoids.   I think this is the "bang-bang signal" you need to generate.  The phrasing is odd.

It sounds as if you need to add a control force to your model that is just a trapezoid (acceleration period, constant full-on period, and deceleration period).  In m-code it would be something like the following.  Here I am assuming your step size is 1 msec, with a 10% slew time.  You may want different slew parameters.  I've written it so you can change step size and slew percents in one place.

max_F = 1.0;
steps_per_sec = 1000;
slew_up = 0.1;
slew_down = 0.1;
accel_steps = floor(steps_per_sec * slew_up);
decel_steps = floor(steps_per_sec * slew_down);
full_on_steps = steps_per_sec - accel_steps - decel_steps;

loops = 1;
// acceleration period
for i = 1:accel_steps
    F(i) = i * max_F / accel_steps;
    loops = loops + 1;
end
// full-on period
for i = loops : accel_steps + full_on_steps
    F(loops) = max_F;
    loops = loops + 1;
end
// deceleration period
for i = loops : accel_steps + full_on_steps + decel_steps
    F(loops) = max_F - (i - accel_steps - full_on_steps) * max_F / decel_steps;
    loops = loops + 1;
end
// after input is back to zero
for i = loops : 10 * steps_per_sec
    F(i) = 0.0;
end

// now apply F to your simulation

@AlFagera, Thanks for posting a reference to the actual paper.  Following the references and re-searching led me to this paper:  M. S. Alam, M. A. Hossain, and M. O. Tokhi, Design of Command Shaper Using Gain-Delay Units and Particle Swarm Optimisation for Vibration Control of a Flexible System, 14th International Congress on Sound & Vibration, Cairns, Australia 2007 (https://www.acoustics.asn.au/conference_proceedings/ICSV14/papers/p197.pdf).  In that paper they describe a bang-bang signal as I've done above, except their acceleration and deceleration periods are quite short.


问:
Dead Reckoning: Obtaining Position Estimation from Accelerometer Acceleration Integration
Good day,

I have been reading papers about position integration from accelerometer readings.

I have consulted this paper from freescale on how that is achievable and this article regarding leaky integrators to help in preventing accumulation of errors from integration.

I was testing this algorithm by moving the imu by approximately 0.1 meter. The algorithm does get it right at the instant it arrives at approx 0.1 meter however when left still at that position, the integrated position goes to zero.

It turns out the velocity readings become negative at a certain period after reaching 0.1 meters.

Does anyone have any suggestions in dealing with this error?

Plots (Red is the position, Blue is the velocity.)

The imu(accelerometer) was moved alternating positions 0 meters and 0.1 meters with a stop of approximately 3-5 seconds in between before moving to the next position

Actual Data

Desired Data output (Green - Desired position integration)

Code:

```<code>// Get acceleration per axis
float AccX = accelmagAngleArray.AccX;
float AccY = accelmagAngleArray.AccY;
float AccZ = accelmagAngleArray.AccZ;

AccX -= dc_offsetX;
AccY -= dc_offsetY;
AccZ -= dc_offsetZ;

//Calculate Current Velocity (m/s)
float leakRateAcc = 0.99000;
velCurrX = velCurrX*leakRateAcc + ( prevAccX + (AccX-prevAccX)/2 ) * deltaTime2;
velCurrY = velCurrY*leakRateAcc + ( prevAccY + (AccY-prevAccY)/2 ) * deltaTime2;
velCurrZ = velCurrZ*0.99000 + ( prevAccZ + (AccZ-prevAccZ)/2 ) * deltaTime2;
prevAccX = AccX;
prevAccY = AccY;
prevAccZ = AccZ;

//Discrimination window for Acceleration
if ((0.12 > AccX) && (AccX > -0.12)){
  AccX = 0;
}

if ((0.12 > AccY) && (AccY > -0.12)){
  AccY = 0;
}

//Count number of times acceleration is equal to zero to drive velocity to zero when acceleration is "zero"
//X-axis---------------
if (AccX == 0){ //Increment no of times AccX is = to 0
    counterAccX++;
}
else{ //Reset counter
    counterAccX = 0;
}

if (counterAccX>25){ //Drive Velocity to Zero
velCurrX = 0;
    prevVelX = 0;
    counterAccX = 0;
}

//Y-axis--------------
if (AccY == 0){ //Increment no of times AccY is = to 0
    counterAccY++;
}
else{ //Reset counter
    counterAccY = 0;
}

if (counterAccY>25){ //Drive Velocity to Zero
    velCurrY = 0;
    prevVelY = 0;
    counterAccY = 0;
}

//Print Acceleration and Velocity
cout << " AccX = " << AccX ;// << endl;
cout << " AccY = " << AccY ;// << endl;
cout << " AccZ = " << AccZ << endl;

cout << " velCurrX = " << velCurrX ;// << endl;
cout << " velCurrY = " << velCurrY ;// << endl;
cout << " velCurrZ = " << velCurrZ << endl;

//Calculate Current Position in Meters
float leakRateVel = 0.99000;
posCurrX = posCurrX*leakRateVel + ( prevVelX + (velCurrX-prevVelX)/2 ) * deltaTime2;
posCurrY = posCurrY*leakRateVel + ( prevVelY + (velCurrY-prevVelY)/2 ) * deltaTime2;
posCurrZ = posCurrZ*0.99000 + ( prevVelZ + (velCurrZ-prevVelZ)/2 ) * deltaTime2;
prevVelX = velCurrX;
prevVelY = velCurrY;
prevVelZ = velCurrZ;

//Print X and Y position in meters
cout << " posCurrX = " << posCurrX ;// << endl;
cout << " posCurrY = " << posCurrY ;// << endl;
cout << " posCurrZ = " << posCurrZ << endl;
</code>```

答:
I am now able to get position from the accelerometer.
Plot (Green - Acc, Blue -Vel, Red - Pos)

I was able to get the propper values through disabling the leaky integrator (setting the factor from 0.99 to 1).


问:
Why do space probes need heating?
I know that temperature influences the characteristics of semiconductors and other materials, but we know how and can take that into account. Furthermore, lower temperatures makes electronics more efficient, sometimes even superconducting.

I remember reading somewhere that engineers building Curiosity even considered low temperature electronics for the motors driving the wheels but still decided against it in the end.

Why is it, apparently, so hard to build components with operating temperatures matching those on Mars, Europa, or in space?

Edit: None of the answers address my question thus far. I know that all parts, both electronic and mechanical, and greases and so on have relatively narrow working temperatures. My question is, why don't we build special cold metals and cold greases and cold chips that have their narrow operating temperature band at -100 C or whatever?

Valid answers could be: it's too expensive, insufficient science has been done to determine materials appropriate for such cold, such cold materials cannot be manufactured in the sweltering heat of planet Earth.

答:
Because the parts work reliably only in a limited range of temperatures. If the temperature is out of the range, the chips may not work correctly or even not work at all.

Probes usually also have some kind of backup battery and batteries lose capacity really fast if they get colder than 0°C. It simply is easier and more efficient to keep the battery and electronics warm than to compensate for different characteristics.

Title: Why do space probes need heating?


问:
Mobile robot algorithm implementation error
I am working in reproducing a robotics paper, first simulating it in MATLAB in order to implement it to a real robot afterwards. The robot's model is:

$$\dot{x}=V(t)cos\theta $$
$$\dot{y}=V(t)sin\theta$$
$$\dot{\theta}=u$$

The idea is to apply an algorithm to avoid obstacles and reach a determines target. This algorithm uses a cone vision to measure the obstacle's properties. The information required to apply this system is:

1) The minimum distance  $ d(t) $ between the robot and the obstacle (this obstacle is modelled as a circle of know radius $ R $).

2) The obstacle's speed  $ v_{obs}(t) $

3)The angles $ \alpha_{1}(t)$ and $ \alpha_{2}(t)$  that form the robot's cone vision, and

4) the heading $ H(t) $  from the robot to the target

First a safe distance  $ d_{safe}$  between the robot and the obstacle is defined. The robot has to reach the target without being closer than        $ d_{safe}$ to the obstacle.

An extended angle $ \alpha_{0} \ge arccos\left(\frac{R}{R+d_{safe}} \right) $  is defined, where $ 0 \le \alpha_{0} \le \pi $

Then the following auxiliary angles are calculated:

$ \beta_{1}(t)=\alpha_{1}(t)-\alpha_{0}(t)$

$ \beta_{2}=\alpha_{2}(t)+\alpha_{0}(t)$

Then the following vectors are defined:

$ l_{1}=(V_{max}-V)[cos(\beta_{1}(t)),sin(\beta_{1}(t))]$

$ l_{2}=(V_{max}-V)[cos(\beta_{2}(t)),sin(\beta_{1}(2))]$

here $ V_{max}$  is the maximum robot's speed and  $ V $ a constant that fulfills  $ \|v_{obs}(t)\| \le V \le V_{max} $

This vectors represent the boundaries of the cone vision of the vehicle

Given the vectors $ l_{1} $  and $ l_{2}$ , the angle $ \alpha(l_1,l_2)$  is the angle between $ l_{1}$  and $ l_{2} $  measured in counterclockwise direction, with $  \alpha \in (-\pi,\pi) $ . Then the function $f$ is

The evasion maneuver starts at time $t_0$. For that the robot find the index h:

$h = min|\alpha(v_{obs}(t_0)+l_j(t_0),v_R(t_0))|$

where $j={1,2}$ and $v_R(t)$ is the robot's velocity vector

Then, from the two vectors  $v_{obs}(t_0)+l_j(t_0)$ we choose that one that forms the smallest angle with the robot's velocity vector. Once h is determinded, the control law is applied:

$u(t)=-U_{max}f(v_{obs}(t)+l_h(t),v_R(t))$

$V(t)=\|v_{obs}(t)+l_h(t)\| \quad \quad (1)$

This is a sliding mode type control law, that steers the robot's velocity  $v_R(t)$ towards a switching surface equal to the vector $v_{obs}(t)+l_h(t)$. Ideally the robot avoids the obstacle by surrounding it a

While the robot is not avoiding an obstacle it follows a control law:

$u(t)=0$

$V(t)=V_{max} \quad \quad  (2) $

Hence the rules to switch between the two laws are:

R10 Switching from (2) to (1) occurs whenthe distance to the obstacle is equal to a constant C, which means when $d(t_0)=C$ and this distance is becoming smaller in time  i.e. $\dot{d(t)}<0$

R11 Switching from (1) to (2) occurs when $d(t_*)<1.1a_*$ and the vehicle is pointing towards the obstacle, i.e. $\theta(t_*)=H(T_*)$

where $a_*=\frac{R}{cos\alpha_0}-R $

Ideally the result should be similar to this

But I'm getting this instead

While I understand the theory there's obviously a flaw in my implementation that I haven't been able to solve. In my opinion the robot manages to avoid the obstacle but at certain point (in the red circle), the robot turns to the wrong side, making impossible the condition $H(t) = \theta(t) $ to be achieved.

I feel that I am not measuring properly the angle alpha between the $v_{obs}(t)+l_h(t)$ and $v_{R}(t)$ , because while debugging I can see that at certain point it stops switching between negative and positive values and become only positive, leading the robot's to the wrong side. It also seems to be related with my problem here: Angle to a circle tangent line

答:
So I'll point out what might be some problems, at-a-glance, but wow this is one of the longest questions I've seen here. I'm pretty swamped with real-life stuff at the moment, so I'll just point these few issues out, you try them, update the question with the results (please don't respond with new information in comments on this answer), and then if it's still broke I'll take another pass.

For starters, some of your code uses variables that I didn't see mentioned anywhere, such as (confusingly) tk and kt. Furthermore, there are some magic numbers that I can't understand, such as the assignments for tk and kt - they're both set to 2 despite the comment beside tk=2;, which says, % Locks in the chosen sign. If you manually set tk=2;, then it's not locking in anything chosen programmatically; it's just a hard-coded value that becomes difficult to change.

That said, I would guess that your real problem is probably in your use of atan. In fact, this function has bitten me so many times that I will never use it again. Use atan2 everywhere you ever would consider using atan. The problem with atan is that if you have (-y)/x or y/(-x), both of those expressions evaluate to -(y/x), and you lose quadrant information, giving you incorrect angles.

atan2 fixes this by accepting two arguments, y and x, as opposed to y/x, and it treats the signs correctly. In your ApplyRule10 function, under the line, % AUXILIARY ANGLES, I AM NOT SURE IF THIS IS THE BEST WAY TO CALCULATE THEM:, you have:

```<code>aux_angle_1 = atan(vr_t(2)/vr_t(1)) - atan(aux_1(2)/aux_1(1)) ;
aux_angle_2 = atan(vr_t(2)/vr_t(1)) - atan(aux_2(2)/aux_2(1)) ;
</code>```

My suggestion would be to change these atan functions to atan2 and see if that fixes anything. Again, update your question with the results and we'll go from there!

Body:
So I'll point out what might be some problems, at-a-glance, but wow this is one of the longest questions I've seen here. I'm pretty swamped with real-life stuff at the moment, so I'll just point these few issues out, you try them, update the question with the results (please don't respond with new information in comments on this answer), and then if it's still broke I'll take another pass.

For starters, some of your code uses variables that I didn't see mentioned anywhere, such as (confusingly) tk and kt. Furthermore, there are some magic numbers that I can't understand, such as the assignments for tk and kt - they're both set to 2 despite the comment beside tk=2;, which says, % Locks in the chosen sign. If you manually set tk=2;, then it's not locking in anything chosen programmatically; it's just a hard-coded value that becomes difficult to change.

That said, I would guess that your real problem is probably in your use of atan. In fact, this function has bitten me so many times that I will never use it again. Use atan2 everywhere you ever would consider using atan. The problem with atan is that if you have (-y)/x or y/(-x), both of those expressions evaluate to -(y/x), and you lose quadrant information, giving you incorrect angles.

atan2 fixes this by accepting two arguments, y and x, as opposed to y/x, and it treats the signs correctly. In your ApplyRule10 function, under the line, % AUXILIARY ANGLES, I AM NOT SURE IF THIS IS THE BEST WAY TO CALCULATE THEM:, you have:

aux_angle_1 = atan(vr_t(2)/vr_t(1)) - atan(aux_1(2)/aux_1(1)) ;
aux_angle_2 = atan(vr_t(2)/vr_t(1)) - atan(aux_2(2)/aux_2(1)) ;

My suggestion would be to change these atan functions to atan2 and see if that fixes anything. Again, update your question with the results and we'll go from there!


答:
It can't pass the 20 block if the robot enters it from a 15 or 20 block (so basically it gets stuck if it's coming from an angle and hits a 90 degree turn).

Somehow coming from an angled line makes it more likely that your robot moves orthogonal to the 90°-ish turn, so that it gets stuck in the back-and-forth loop. Maybe if it moved very slowly backwards it could detect the moment when only one sensor goes on, which would lead to a turn. But it's a competition after all, so let's not move backwards, especially not very slowly.

I'm not sure if the image of the different sensor values is accurate in terms of scale. It looks like the line is not much thicker than the two sensors are apart. Now imagine the edge case that both distances were the same: the sensors are as far apart as the line is thick. In this case, the robot can only really move in the exact direction of the line. This is very bad for a 90°-ish turn for the reason mentioned above. The ratio between sensor distance and line thickness seems to play a role in how the robot behaves. You cannot change anything about the thickness of the line or the distance of the sensors, but I think it's important to think about what impact those values have or at least could have.

What you can change though is what you consider to be the line. You can follow a line by following its edge, too. The code could look like this:

```<code>loop forever:
    if (on the right of the line):
        turn_left()
    if (on the left of the line):
        go_forward()
    if (on the line):
        turn_left()
    if (not on the line):
        turn_right()
</code>```

As the edge is only a line, the behaviour depends more or less only on the distance of the sensors, not the ratio between the distance of the sensors and the thickness of the line. I'd give it a go and see how the behaviour changes.

A ratio that you can change more continuously is that of the velocities. How many degrees is turn_left() and how many millimetres is go_forward()?
Playing around with those values will lead to different, possibly more preferable behaviour of the robot.

theory interlude

In a nutshell, you want to control a system with two inputs: angular and radial velocity. Setting either one will influence the other. In your case, the radial velocity control loop freaked out and became unstable: back-and-forth motion forever. The important thing is that the system should have reacted with a large increase in angular velocity (to do the hard 90°-ish turn), but instead the radial velocity went mad.

This is a sign that the interaction between the two is not well controlled and requires further investigation.

Changing the ratio between both velocities essentially modifies the gains of the controllers. Lowering both velocities means more sensor readings per travelled distance and thus a higher sampling rate, which in general leads to more stable systems.

tl,dr;

Things to try

Change both angular and radial velocities, but keep their ratio the same and see if that changes anything

Change the ratio between the two and see if that changes the behaviour of the robot. Especially on different types of tiles. What ratio works best on straight tiles, what works better on curvy tiles. Is there some setting that works for both?

Try both 1. and 2. for the on-the-line approach and the on-the-edge-of-the-line approach and see if you get to different results.

Body:
It can't pass the 20 block if the robot enters it from a 15 or 20 block (so basically it gets stuck if it's coming from an angle and hits a 90 degree turn).

Somehow coming from an angled line makes it more likely that your robot moves orthogonal to the 90°-ish turn, so that it gets stuck in the back-and-forth loop. Maybe if it moved very slowly backwards it could detect the moment when only one sensor goes on, which would lead to a turn. But it's a competition after all, so let's not move backwards, especially not very slowly.

I'm not sure if the image of the different sensor values is accurate in terms of scale. It looks like the line is not much thicker than the two sensors are apart. Now imagine the edge case that both distances were the same: the sensors are as far apart as the line is thick. In this case, the robot can only really move in the exact direction of the line. This is very bad for a 90°-ish turn for the reason mentioned above. The ratio between sensor distance and line thickness seems to play a role in how the robot behaves. You cannot change anything about the thickness of the line or the distance of the sensors, but I think it's important to think about what impact those values have or at least could have.

What you can change though is what you consider to be the line. You can follow a line by following its edge, too. The code could look like this:

loop forever:
    if (on the right of the line):
        turn_left()
    if (on the left of the line):
        go_forward()
    if (on the line):
        turn_left()
    if (not on the line):
        turn_right()

As the edge is only a line, the behaviour depends more or less only on the distance of the sensors, not the ratio between the distance of the sensors and the thickness of the line. I'd give it a go and see how the behaviour changes.

A ratio that you can change more continuously is that of the velocities. How many degrees is turn_left() and how many millimetres is go_forward()?
Playing around with those values will lead to different, possibly more preferable behaviour of the robot.

theory interlude

In a nutshell, you want to control a system with two inputs: angular and radial velocity. Setting either one will influence the other. In your case, the radial velocity control loop freaked out and became unstable: back-and-forth motion forever. The important thing is that the system should have reacted with a large increase in angular velocity (to do the hard 90°-ish turn), but instead the radial velocity went mad.

This is a sign that the interaction between the two is not well controlled and requires further investigation.

Changing the ratio between both velocities essentially modifies the gains of the controllers. Lowering both velocities means more sensor readings per travelled distance and thus a higher sampling rate, which in general leads to more stable systems.

tl,dr;

Things to try

Change both angular and radial velocities, but keep their ratio the same and see if that changes anything

Change the ratio between the two and see if that changes the behaviour of the robot. Especially on different types of tiles. What ratio works best on straight tiles, what works better on curvy tiles. Is there some setting that works for both?

Try both 1. and 2. for the on-the-line approach and the on-the-edge-of-the-line approach and see if you get to different results.


答:
You still haven't described:

How you are doing interpolation, and

What the Q values are.

Are the Q values angles? Are they distances? What do they mean? Can you post a picture of your arm and label the axes of motion and describe how the drawing relates to the Q values?

I post this as an answer because I hope I have a lead on your problem. You state, "The idea is that i want my arm be on this sphere, point inward to the center," but, while your pathPlanning::sphere function sets a radius of 5, neither your start nor end positions are a distance of 5 from the origin. They are also greater than 10 from each other (the end radius from origin is 117!!), meaning that this is not a sphere at the origin but it is also not a sphere anywhere else in space.

My suggestion would be to fix the start and end coordinates at a value such that $\sqrt{x^2 + y^2 + z^2} = r_{\mbox{sphere}}$ and see if that makes anything better.

Current it appears that you're starting off of the sphere, and you're trying to get to somewhere else (really far) off the sphere, but you're trying to do path planning to keep the end effector on the sphere. This means that you're trying to generate an impossible path, which could explain the results you're getting.

:EDIT:

I was going to add this as a comment, but there's enough content that I figured I'd alter the answer.

If you start at a point on a sphere, and you end at a point on a sphere, and you linearly increment the start path to the end path, then you will traverse a trajectory of a line through the sphere. You need to be "smarter" about interpolating by choosing points that all fall on the surface of the sphere.

The way I would attempt to do this is to find the angle between the two vectors that describe the start and end points (via a dot or cross product), then interpolate the angle. Find the plane that contains the two vectors, apply the interpolated angle and constrain the interpolated points to that plane.

It's a little involved, but not fantastically complicated. That said, you need to make sure that your core functionality is working correctly to begin with. To that end, I would probably hand-pick some points first, to ensure that your problem isn't with interpolating, and try your inverse kinematics with those.

Then, once you've got the inverse kinematics working correctly, then write the interpolate function. If you try to solve all of your problems at the same time it becomes impossible to tell what is working and what isn't.

Title: Is this a singularity or incorrect implementation of inverse kinematics?

Body:
I at moment trying to compute the Q configuration that moves my robot from it current state described by this transformation matrix.
with rotation

```<code>0.00549713  0.842013  -0.539429
0.999983 -0.00362229 0.00453632
0.00186567 -0.539445 -0.842019

</code>```

and position as this:

```<code>-0.0882761
-0.255069
 0.183645
</code>```

To this rotatation

```<code>    0  0.942755 -0.333487
    1         0         0
    0 -0.333487 -0.942755
</code>```

and this position

```<code>8.66654
19.809
115.771
</code>```

Due to the drastic change in the Z direction, I thought i could split the path between start and end into small chunks by creating data inbetween by interpolating, and compute the inverse kinematics for each of these small position.  Problem is that the output i am getting is pretty large.. Which making me suspect that some of the output might be wrong. The simulation i am using constrains the rotation to 360 degrees.. I think something goes wrong..

The only reason I could think would do this, would be if the jacobian i am was using had singularities... Which why i assumed that i was running into singualarity issue..

```<code>setQ{22.395444, 319.402231, 90.548314, -228.989836, -295.921218, -336.808799}
setQ{8.209388, 362.472468, 108.618073, -232.346755, -299.935844, -334.929518}
setQ{8.479842, 399.892521, 127.432982, -234.017882, -303.852583, -335.063821}
setQ{8.224516, 362.232497, 108.666778, -232.319554, -299.899932, -334.928688}
setQ{7.718908, 286.832458, 71.150606, -228.913831, -291.982659, -334.658147}
setQ{7.468625, 249.092444, 52.400638, -227.206436, -288.018036, -334.522738}
setQ{7.220023, 211.325766, 33.656081, -225.496018, -284.049424, -334.387237}
setQ{-6.134091, -2538.260148, -1283.375216, -96.331289, 7.920957, -324.531125}
setQ{-6.261661, -2577.946595, -1301.730132, -94.403263, 12.176863, -324.388990}
setQ{-6.634286, -2697.165915, -1356.762411, -88.601053, 24.968521, -323.962029}
setQ{-6.991781, -2816.625206, -1411.745985, -82.771641, 37.796090, -323.534239}
setQ{-7.334148, -2936.324468, -1466.680853, -76.915029, 50.659572, -323.105620}
setQ{-7.661386, -3056.263702, -1521.567017, -71.031215, 63.558965, -322.676171}
setQ{-8.642914, -3457.794271, -1704.169136, -51.222052, 106.816303, -321.238686}
setQ{-8.988457, -3619.153075, -1777.058457, -43.213761, 124.230964, -320.661112}
setQ{-9.382564, -3821.451508, -1868.048346, -33.135395, 146.089069, -319.937071}
setQ{-9.528439, -3902.557525, -1904.406419, -29.082892, 154.860242, -319.646810}
setQ{-9.667591, -3983.770196, -1940.742846, -25.018300, 163.647376, -319.356179}
setQ{-9.734645, -4024.416527, -1958.902942, -22.981471, 168.046928, -319.210726}
setQ{-9.986053, -4187.268484, -2031.489209, -14.803929, 185.685040, -318.627992}
setQ{-10.210564, -4350.547057, -2103.988889, -6.578030, 203.386994, -318.043783}
setQ{-10.312734, -4432.346324, -2140.206259, -2.446947, 212.261912, -317.751125}
setQ{-10.453381, -4555.245201, -2194.491727, 3.772345, 225.604215, -317.311448}
setQ{-10.496902, -4596.264820, -2212.576060, 5.851488, 230.059630, -317.164705}
setQ{-10.538741, -4637.311102, -2230.654980, 7.933652, 234.519035, -317.017869}
setQ{-10.617377, -4719.483658, -2266.796587, 12.107048, 243.449816, -316.723922}
setQ{-10.812941, -4966.641247, -2375.091527, 24.699772, 270.337923, -315.839868}
setQ{-10.839651, -5007.927501, -2393.121742, 26.809138, 274.833240, -315.692203}
setQ{-10.888029, -5090.579998, -2429.165939, 31.036936, 283.835844, -315.396596}
</code>```

setQ is just a function for my simulation, the numbers are the actual Q values starting from 0 - 5. (I am using a 6 jointed robot (UR5))

Update

I am using a sphere to compute my desired transformation matrix.. The idea is that i want my arm be on this sphere, point inward to the center.

std::vector<Transform3D<>> pathPlanning::sphere(double dx, double dy, double dz)
{
    double r = 5.0; // Radius of the sphere -  set to 5.0 cm (TODO: has to be checked if that also is accurate)
    cout << "Create a sphere" << endl;

    double current_x = this->device->baseTframe(this->toolFrame,this->state).P()[0];
    double current_y = this->device->baseTframe(this->toolFrame,this->state).P()[1];
    double current_z = this->device->baseTframe(this->toolFrame,this->state).P()[2];

    // Formula for sphere (x-x0)²+(y-y0)²+(z-z0)²=r²
    // x: x = x_0 + rcos(theta)sin(phi)
    // y: y = y_0 + rsin(theta)sin(phi)
    // z: z = z_0 + rcos(phi)
    // Angle range: 0 <= theta <= 2M_PI ; 0 <= phi <= M_PI

    double obj_x = current_x + dx;
    double obj_y = current_y + dy;
    double obj_z = current_z + dz;

    std::vector<Transform3D<>> out;

    int count = 32;

    for(double azimuthal = 0; azimuthal <= M_PI ; azimuthal+=0.01 )
    {

        for(double polar = 0.35; polar <= M_PI-0.35 ; polar+=0.01 )
        {

            double sphere_x = obj_x + r*cos(azimuthal)*sin(polar);
            double sphere_y = obj_y + r*sin(azimuthal)*sin(polar);
            double sphere_z = obj_z + + r*cos(polar);

            //string text = to_string(sphere_x) + " , " + to_string(sphere_y)+ " , " + to_string(sphere_z);
            //positions << text << endl;

            Transform3D<> transformation_matrix = transform(obj_x,obj_y,obj_z,sphere_x,sphere_y,sphere_z);

            if(0.1<(transformation_matrix.P()[0] - current_x) || 0.1<(transformation_matrix.P()[1] - current_y) || 0.1<(transformation_matrix.P()[2] - current_z))
            {
                cout << "Interpolate: " << endl;

                std::vector<Transform3D<>> transformation_i = invKin_LargeDisplacement(transformation_matrix);
                out.insert(out.end(),transformation_i.begin(),transformation_i.end());
                cout << out.size() << endl;
                cout << "only returning one interpolation onto the sphere!" << endl;

                return transformation_i;
            }
            else
            {
                cout << "OK" << endl;
                out.push_back(transformation_matrix);

            }

            if(count == 32) //TODO: Why...... is this occuring?
            {
                //cout << "Theta: " << theta << " Phi: " << phi << endl;
                //cout << sphere_x << " , " << sphere_y <<" , "<< sphere_z << endl;
                count = 0;
            }
            else
            {
                count++;
            }
        }
    }

    return out;
}
```
<p>This function provides me with the point on the sphere, which is use to create my rotation matrix using <code>transform</code>.</p>
```Transform3D<> pathPlanning::transform(double obj_x, double obj_y, double obj_z, double sphere_x, double sphere_y ,double sphere_z)
{
    // Z-axis should be oriented towards the object.
    // Rot consist of 3 direction vector [x,y,z] which describes how the axis should be oriented in the world space.
    // Looking at the simulation the z-axis is the camera out. X, and Y describes the orientation of the camera.
    // The vector are only for direction purposes, so they have to be normalized....
    // TODO: case [0  0 -1]... Why is it happening at what can be done to undo it?
    cout << "inside Transform" << endl;
    cout << obj_x << "," << sphere_x << " ; "  << obj_y << " , " << sphere_y  <<" ; "<< obj_z << " , " << sphere_z  << endl;
    Vector3D<> dir_z((obj_x - sphere_x), (obj_y - sphere_y), (obj_z - sphere_z));
    //Vector3D<> dir_z((sphere_x-obj_x), (sphere_y - obj_y), (sphere_z-obj_z));
    dir_z = normalize(dir_z);
    Vector3D<> downPlane(0.0,0.0,-1.0);
    Vector3D<> dir_x = cross(downPlane,dir_z);
    dir_x = normalize(dir_x);
    Vector3D<> dir_y = cross(dir_z,dir_x);
    dir_y = normalize(dir_y);
    Rotation3D<> rot_out (dir_x,dir_y,dir_z);  // [x y z]

    Vector3D<> pos_out(sphere_x,sphere_y,sphere_z);

    Transform3D<> out(pos_out,rot_out);
    cout << "desired: " << out << endl;

    return out;
}
```
<p>The transform basically computes the rotation matrix. The math is based on the on this post by @Ben, which is an answer to a similar problem i am having..</p>
<p><strong>Update</strong></p>
<p>Error with the rotation matrix was due to the polar coordinate being 0 => sin(0) = 0.</p>
<p>I made this plot displaying the determinant of the jacobian, while i compute the inverse kinematics for the large displacement. For each inverse kinematics iteration, I set the robot to the new q_i and use that as current and continue computing until i reach the end configuration.</p>
<p><img src="https://i.stack.imgur.com/DBLWm.jpg" alt="Determinant of the different jacobians" /></p>
<p>It seems that alot of them goes toward a singularity or in general a pretty low number..</p>
<p><strong>Update</strong></p>
<p>Again i think the singularities might be the culprit here..</p>
```determinant: 0.0424284
Q{13.0099, -46.6613, -18.9411, 2.38865, 5.39454, -4.53456}
determinant: -0.0150253
Q{47.1089, -0.790356, 6.89939, -2.725, -1.66168, 11.2271}
determinant: -0.0368926
Q{15.7475, 8.89658, 7.78122, -2.74134, -5.32446, 1.11023}
determinant: -0.0596228
Q{180.884, 66.3786, 17.5729, 9.21228, -14.9721, -12.9577}
determinant: -0.000910399
Q{5426.74, 5568.04, -524.078, 283.581, -316.499, -67.3459}
determinant: -0.0897656
Q{16.6649, -37.4239, -34.0747, -16.5337, -3.95636, -7.31064}
determinant: -0.00719097
Q{-1377.14, 167.281, -125.883, -10.4689, 179.78, 56.3877}
determinant: 0.0432689
Q{22.2983, -10.1491, -15.0894, -4.41318, -2.07675, -3.48763}
determinant: -0.0430843
Q{82.6984, -39.02, -24.5518, 13.6317, 4.17851, -14.0956}
determinant: -0.0137243
Q{425.189, -9.65443, 20.9752, 7.63067, 25.4944, -52.4964}
```
<p>Everytime i compute a new Q I set the robot in that state, and perform inverse kinematics from that state.. Q is the joint angles for the 6 joints.</p>
<p><strong>Update</strong></p>
<p>Interpolation is done by lineary dividing the path from start to end into a specified amount of of data points.</p>
<p><img src="https://i.stack.imgur.com/gnHbZ.jpg" alt="enter image description here" /></p>
<p>This plot shows  each tranformation matrices generated from the interpolation and with their the position part plotted. The red dots is the path (every 1000th position). The blue ball is the object in want to track, and green dots represents the sphere.. As I am only doing this for the first point on the sphere, it only hits one point on the sphere, which is the top point, which the plot also shows.</p>
<p>Rotation doesn't show that much change, which also makes sense based difference between the current and desired rotations.</p>
<p><strong>Update</strong></p>
<p>My InvKin Implementation for LargeDisplacements:</p>
```std::vector<Q> pathPlanning::invKin_largeDisplacement(std::vector<Transform3D<>> t_tool_base_desired_i)
{

     Device::Ptr device_backup = this->device;  //Read in device parameter
     WorkCell::Ptr workcell_backup = this->workcell; //Read in workcell parameter
     State state_backup = this->state;
     std::vector<Q> output;

     for(unsigned int i = 0; i<t_tool_base_desired_i.size();  ++i)
     {
         Transform3D<> T_tool_base_current_i = device_backup->baseTframe(this->toolFrame,state_backup); //Read in Current transformation matrix

         Eigen::MatrixXd jq(device_backup->baseJframe(this->toolFrame,state_backup).e().cols(), this->device.get()->baseJframe(this->toolFrame,state_backup).e().rows());

         jq =  this->device.get()->baseJframe(this->toolFrame,state_backup).e(); // Get the jacobian for current_configuration

         //Least square solver - dq = [j(q)]T (j(q)[j(q)]T)⁻1 du  <=> dq = A*du
         Eigen::MatrixXd A (6,6);

         //A = jq.transpose()*(jq*jq.transpose()).inverse();
         A = (jq*jq.transpose()).inverse()*jq.transpose();

         Vector3D<> dif_p = t_tool_base_desired_i[i].P()-T_tool_base_current_i.P();  //Difference in position

         Eigen::Matrix3d dif = t_tool_base_desired_i[i].R().e()- T_tool_base_current_i.R().e(); //Differene in rotation
         Rotation3D<> dif_r(dif); //Making a rotation matrix the the difference of rotation
         RPY<> dif_rot(dif_r);    //RPY of the rotation matrix.

         Eigen::VectorXd du(6); //Creating du
         du(0) = dif_p[0];
         du(1) = dif_p[1];
         du(2) = dif_p[2];

         du(3) = dif_rot[0];
         du(4) = dif_rot[1];
         du(5) = dif_rot[2];

         Eigen::VectorXd q(6);
         q = A*du; // computing dq

         Q q_current;
         q_current = this->device->getQ(this->state);
         Q dq(q);
         Q q_new = q_current+ dq; // computing the new Q angles
         output.push_back(q_new); store it in the output vector
         device_backup->setQ(q_new,state_backup); //Set the robot to the calculated state.
     }
     return output;
}
```
<p>I am pretty sure that my interpolation works, as the plot shows.  My inverse kinematics on the other hand not so sure..</p>
<p><strong>Update</strong></p>
<p>@Chuck mentions in his answer that it would be a good idea to check the core functionality, which might shed some light on what could be going wrong.</p>
<p>I tried it with an inv.kin function i know would work, which didn't return any result, which make me doubt whether my transformation function i create is accurate?</p>
<p>The robot simulation is the one shown above..  The  <code>Transform</code> function shown above, is the function which i use to compute my desired, and provide my inverse kinematics..  Is something incorrectly setup?
<img src="https://i.stack.imgur.com/UOYcL.png" alt="enter image description here" /></p>
<p><strong>Update</strong></p>
<p>@Chuck came up with an different approach to my problem, which only has 3 DOF, being the position.  I choose change track, and peform a simple inverse kinematics given a distance dx,dy,dz.. Which for some reason isn't working quite good for me? even for small differences...</p>
<p>Here is my code:</p>
```    std::vector<Q>pathPlanning::invKin(double dx, double dy , double dz)
{

    kinematics::State state =  this->state;
    Transform3D<> t_tool_base_current =  this->device.get()->baseTframe(this->toolFrame,state);

    cout <<"Current: "<< t_tool_base_current.P().e()<< endl;

    Vector3D<> P_desired(0.000001+t_tool_base_current.P().e()[0],t_tool_base_current.P().e()[1],t_tool_base_current.P().e()[2]);
    cout <<"Desired: " <<P_desired << endl;

    Transform3D<> t_tool_base_desired(P_desired,t_tool_base_current.R());
    Eigen::MatrixXd jq(this->device.get()->baseJframe(this->toolFrame,state).e().cols(), this->device.get()->baseJframe(this->toolFrame,state).e().rows());
    jq =  this->device.get()->baseJframe(this->toolFrame,state).e();

    //Least square solver - dq = [j(q)]T (j(q)[j(q)]T)⁻1 du  <=> dq = A*du
    Eigen::MatrixXd A (6,6);

    //A = jq.transpose()*(jq*jq.transpose()).inverse();
    A = (jq*jq.transpose()).inverse()*jq.transpose();

    Vector3D<> dif_p = t_tool_base_desired.P()-t_tool_base_current.P();
    cout <<"difference: " <<dif_p << endl;

    Eigen::VectorXd du(6);
    du(0) = dif_p[0];
    du(1) = dif_p[1];
    du(2) = dif_p[2];

    du(3) = 0;
    du(4) = 0;
    du(5) = 0;

    Eigen::VectorXd q(6);
    q = A*du;

    Q q_current;
    q_current = this->device->getQ(this->state);
    Q dq(q);
    Q q_new = q_current+ dq;
    std::vector<rw::math::Q> output;
    if(!collision(q_new))
    {
        output.push_back(q_new);
    }
    else
    {
        cout << endl;
        cout << q_new << endl;
    }

    return output;
}
```
<p>which outputs this</p>
```Current: -0.000799058
-0.282
0.99963
Desired: Vector3D(-0.000789058, -0.282, 0.99963)
difference: Vector3D(1e-05, 0, 0)
setQ{1.559142, 110474925659325248.000000, -1834.776226, 55426871347211368.000000, 0.068436, 88275880260745.328125}
```

setQ is the state which moves the robot to the desires state..
Either is something wrong with my implementation, or it is a singularity..

Especially because i am not moving it that much (0.00001)!!!

Updates

I think I have solved the mystery.. It must be the sphere function which creates points that outside the reach of the robot.!!


答:
I have a similar idea like @Bending Unit 22, which is not to follow the line but its edge instead. So you detect the line only with one sensor (let's say only with the left one (blue dot)).

If the you both sensor indicates line then your robot should turn right until the right sensor (green dot) indicates white surface. This is the event circled with green in the image below.

If both sensor are lost sight of the line, then the robot should turn left until the left sensor is back on the track. This is the event circled with blue in the image below.

Basically, there could be two states of line-following:

Default state when the robot has one sensor on the line while the other is off the line.

Turning state if one of the events occur. The robot could stop and turn until the the left sensor gets back on line or till the right leaves the line. Since we can assume that the direction is known and the robot is not moving, we could turn on maximum speed (or at least you can try and see how it works). You can test it while the robot is in motion of course.

Previously, when your robot lost the line there was no information about the direction, so it could only go backwards hoping to find the line.

It is only a theory but I think it's worth a try or just to move forward your project.

You have mentioned that your cannot use more sensors but are you allowed to modify on the current hardware?

I have checked that the sensors have digital output which means [line] - [no line], not much information. But if you could connect the IR-sensors output voltage to some analog pins, this way you could obtain higher resolution.

To explain further this part:

A basic line detecting sensor is a IR photo-diode and photo-transistor pair. Like the one on your robot only it has a little bit more on the module, a newer version from Makeblock's page:

(Module on left, actual sensor on the right) Your robot has a previous version, which probably uses an other sensor. This one in the image is a TCRT500 from Vishay.

If we have a look at the schematic level:

The output is at the red circle. Now, if there is sufficient reflected IR light the transistor will conduct and the output will be pulled down to GND. If there is not enough reflected IR light, the transistor won't conduct and the output is pulled up through the resistor to supply voltage. But it is not working like a switch, so the out can be anything between 0 - V_Supply. For a given spectral distribution (IR in our case), the photo-current is linearly proportional to the illuminance . So as I said the output voltage can vary between 0 V and VCC (5 V in common).

Your sensor module works the same way, only it has a comparator on it which makes digital output of the analog signal.

When the IR LED illuminates the edge of the line, maybe half of the light is reflected or more, so the transistor can conduct slightly. This means that the output won't be pulled down near 0 V, it will be, let's say around 2 V. But not 0 V (which is the complete white surface), and this is the important because this means that it partly detects something (the line).

Now back to the comparator, it compares the output voltage of the photo-transistor to a reference voltage. It checks if the signal is above or below the reference and sets its output to LOW or HIGH accordingly.

If this the reference is 2.5 V then your module will say that it detects line when the output voltage is above 2.5V, so that information with the 2 V signal will be considered as a white surface where as we saw the edge, thus we lose resolution.

Now, if you could read this whole analog range with an ADC your resolution would be higher and maybe a more delicate controlling algorithm could be achieved.


问:
What is a good approach to a Quadruped Gait?
I have a small quadruped with three degree of freedom legs which I have been working on: 3DOF Mini Quadruped.

My original code for it was a simple servo controller on the arduino, and Scala code which would send servo commands over the wire.  I did all the Inverse Kinematics and Gait logic in Scala, and got it to walk: 3dof quadruped first gait.

My Gait logic in Scala was somewhat naive; it depended on the legs being in the right position at the beginning (one side extended fore and aft, the other side in toward each other).  The logic was simply translate all four feet backward by 1mm along y, and whenever a coxa angle became excessively rearward, stop and perform a little routine where that foot is lifted 10mm in z, then translated forward 60mm along y, and set back down.  Naive, but effective.

Now, I have rewritten my IK code in arduino C, and I'm trying to decide how to move forward with the Gait dynamics.  I've had a hard time finding good, easy to understand resources about gaits.  I do have some knowledge about the difference between dynamically stable gaits (like creep gaits) where the body is a stable tripod at all times and dynamically unstable gaits (walking, trotting), where two legs are off the ground at a time and the body is essentially falling forward into the advancing leg.

I had some thoughts about state machines and trying to calculate whether the body center falls within a triangle made by the remaining feet to decide which foot was safe to lift, but I'm not sure if these are ideas worth exploring.

I know this is kind of an overly general question, but I'm interested to see how other people have attacked this problem, and about all I've been able to find are research papers.

答:
The gait you've got is actually not too bad, although the feet don't have enough traction, so it's hard to see how good it really is.

I'm not sure about your terminology of dynamically stable gaits. As I've always understood it, the two tripod style gait is known as a statically stable gait, while a good walking biped like Petman uses a dynamically stable gait. Never heard of a dynamically unstable gait. That sounds more like drunkenness.

Two legs on the ground is probably the lower limit for this design of robot. Generally, it's easier to make this work with tall, humanoid robots, which take a long time to fall over. Wide flat robots need to lift and place their feet pretty quickly if they aren't leaving many on the floor.

But how to develop a gair for your robot. Firstly, you should decide what you want to achieve from the gait. Are you looking for maximum speed on a flat surface, or maximum stability on uneven surfaces?

If your goal is stability, then I certainly think it's worth having the robot be aware of its centre of gravity in relation to its feet. To help it know this, it might even be worth adding some force sensors to its feet so it can calculate this easily.

If your goal is maximum speed only, then I'd simply concentrate on funding an optimum gait pattern. This is harder to do. There are two good ways to approach it:

Modelling. Create a comprehensive computer model of the robot, including mass, stiffness, torque, etc. Use this model to fully understand the dynamic behaviour.

Trial and error. You can do this the hard way, programming in random gaits, adjusting them on a hunch and measuring their performance. Try making it walk both forwards, and diagonally. Or you can use a genetic algorithm to help you automatically search for better gaits. The difficulty of using a GA is that you need to have some way to automatically measure the performance of the gait.

A third way, is to make the robot into a scientist who runs concious experiments on itself to validate hypotheses about itself, and work out how to walk, like Cornell University's starfish robot.


问:
How do I compute the inverse kinematics given a desired transformation matrix?
I am at the moment trying to implement an inverse kinematics function which function is to take a desired transformation matrix, and the current transformation matrix, and compute the Q states that is needed to move my robot arm from current state to end state.

I have already written the code, but since my simulation isn't showing the right path, or what I would expect it to be, this makes me unsure as to whether my implementation is correct.  Could someone comment on my implementation and maybe spot an error?

```<code>std::vector<Q> pathPlanning::invKin_largeDisplacement(std::vector<Transform3D<>> t_tool_base_desired_i)
{

    for(unsigned int i = 0; i<t_tool_base_desired_i.size();  ++i)
    {
        Transform3D<> T_tool_base_current_i = device_backup->baseTframe(this->toolFrame,state_backup);
        Eigen::MatrixXd jq(device_backup->baseJframe(this->toolFrame,state_backup).e().cols(), this->device.get()->baseJframe(this->toolFrame,state_backup).e().rows());
        jq =  this->device.get()->baseJframe(this->toolFrame,state_backup).e();

        //Least square solver - dq = [j(q)]T (j(q)[j(q)]T)⁻1 du  <=> dq = A*du
        Eigen::MatrixXd A (6,6);
        //A = jq.transpose()*(jq*jq.transpose()).inverse();
        A = (jq*jq.transpose()).inverse()*jq.transpose();

        Vector3D<> dif_p = t_tool_base_desired_i[i].P()-T_tool_base_current_i.P(); // Difference in position between current_i and desired_i

        Eigen::Matrix3d dif = t_tool_base_desired_i[i].R().e()- T_tool_base_current_i.R().e(); // Difference in rotation between current_i and desired_i

        Rotation3D<> dif_r(dif); //Construct rotation matrix
        RPY<> dif_rot(dif_r); // compute RPY from rotation matrix

        //Jq*dq = du
        Eigen::VectorXd du(6);
        du(0) = dif_p[0];
        du(1) = dif_p[1];
        du(2) = dif_p[2];

        du(3) = dif_rot[0];
        du(4) = dif_rot[1];
        du(5) = dif_rot[2];

        Eigen::VectorXd q(6);
        q = A*du; // Compute change dq

        Q q_current;
        q_current = this->device->getQ(this->state); // Get Current Q
        Q dq(q);
        Q q_new = q_current+ dq; // compute new Q by adding dq
        output.push_back(q_new); // Pushback to output vector
        device_backup->setQ(q_new,state_backup); //set current state to newly calculated Q.

    }
    return output;
}
</code>```

Example of output:

```<code>Q{-1.994910, -94.421754, -123.448429, 15.218864, 6.602184, -13.742988}
Q{2627.867315, -2048.863588, -51.340574, 287.654959, 270.187026, 258.581800}
Q{12941.812459, -536.870516, -294.362593, -2145.963577, -31133.660814, -4742.343433}
Q{32.044799, -14.220020, -14.312226, -12.444921, 12.269179, -24.393637}
Q{125.537278, 28.626924, -55.646716, -20.945348, 17.536762, -2.656717}
Q{9.514525, -107.455064, -17.009190, -15.245588, -0.960273, -2.010570}
Q{8.255582, -3.010934, -4.882207, -1.369533, 0.848644, 1.175172}
Q{208.655993, -28.443465, -64.413952, -3.129896, 13.063806, -6.042187}
Q{-73.706483, -20.381540, -5.306434, -1.204419, -4.035149, 21.806934}
Q{10.003481, 10.867394, 13.256192, -6.491445, -1.711469, 2.896646}
Q{24.890626, -72.265307, -94.886507, 12.327304, -4.425786, 4.188531}
Q{7.111258, 31.500732, -0.111033, -20.434697, 5.302118, 1.781690}
Q{477.993581, 659.221820, 19.819916, -88.627757, 65.850191, -77.267367}
Q{-30.672145, -53.496243, -18.170871, 83.648574, 48.311796, -28.015005}
Q{-36.677982, -15.908633, 17.751008, 0.995766, -0.500259, 9.409435}
Q{114246.358249, -10664.813432, -75.904830, 462.907904, 7992.514723, -18484.319327}
Q{83.827086, -75.899321, -38.576446, 37.266068, 47.843725, 39.096061}
Q{-119.682661, -774.773093, -251.969174, 23.212110, -42.662580, 53.247454}
Q{98.608881, -28.013383, 132.896921, 17.121488, 36.916894, -14.627180}
Q{-11519.051453, 5761.564318, -364.916044, -1188.567128, -2582.813750, -462.784007}
Q{54802.605226, 40971.776641, 10204.739981, -654.963987, -244.277958, -8618.970216}
Q{-21.334047, -14.314134, 17.714174, 2.463993, 0.963385, 5.304530}
</code>```

答:
Shouldn't A be

```<code>jq.transpose() * (jq*jq.transpose()).inverse()
</code>```

instead of

```<code>(jq*jq.transpose()).inverse()*jq.transpose()
</code>```

?

Body:
Shouldn't A be

jq.transpose() * (jq*jq.transpose()).inverse()

instead of

(jq*jq.transpose()).inverse()*jq.transpose()

?


问:
How do the PID parameters (Kp, Ki, and Kd) affect the heading of a differential driving robot when they are increased individually?

Question: A PID controller has three parameters Kp, Ki and Kd which could affect the output performance. A differential driving robot is controlled by a PID controller. The heading information is sensed by a compass sensor. The moving forward speed is kept constant. The PID controller is able to control the heading information to follow a given direction. Explain the outcome on the differential driving robot performance when the three parameters are increased individually.

This is a question that has come up in a past paper but most likely won't show up this year but it still worries me. It's the only question that has me thinking for quite some time.
I'd love an answer in simple terms. Most stuff i've read on the internet don't make much sense to me as it goes heavy into the detail and off topic for my case.

My take on this:

I know that the proportional term, Kp, is entirely based on the error and that, let's say, double the error would mean doubling Kp (applying proportional force). This therefore implies that increasing Kp is a result of the robot heading in the wrong direction so Kp is increased to ensure the robot goes on the right direction or at least tries to reduce the error as time passes so an increase in Kp would affect the robot in such a way to adjust the heading of the robot so it stays on the right path.

The derivative term, Kd, is based on the rate of change of the error so an increase in Kd implies that the rate of change of error has increased over time so double the error would result in double the force. An increase by double the change in the robot's heading would take place if the robot's heading is doubled in error from the previous feedback result. Kd causes the robot to react faster as the error increases.

An increase in the integral term, Ki, means that the error is increased over time. The integral accounts for the sum of error over time. Even a small increase in the error would increase the integral so the robot would have to head in the right direction for an equal amount of time for the integral to balance to zero.

I would appreciate a much better answer and it would be great to be confident for a similar upcoming question in the finals.

答:
Proportional term: this controls how quickly to turn the steering when the heading is not at the set value.

A low P will lead to sluggish steering, reacting only slowly to set
heading changes. It may never reach the commanded value.

A higher P will give a snappier response, ideally with the steering
turning rapidly and smoothly to follow commanded heading changes.

Too high a P is an important case. If off course by a small amount to the left, a large
right turn command will be issued.  The turn rate will still be high when it's back on course, so it will overshoot. Too much P will give
steering that weaves alternately left and right, overshooting the
required heading; though it may eventually settle if the commanded course remains straight.

An even higher P will lead to exaggerated oscillations and possibly
shaking/vibration depending on the dynamics of the system, how heavy
it is, how flexible the tyres and carpet are...

Derivative term: this can be used to limit the speed of the steering response.

Assuming you already have a P term coarsely set in a sensible range,
this will serve to reduce the speed of the steering response when the error is changing rapidly.

If your P term is set high and overshoot or oscillation is occurring, adding Derivative action will limit the amount of hard turning done for large steering commands.

In other words increasing the D term will reduce the maximum "sway"
performed by the steering. Ideally this will serve to reduce
overshoot so if the P controller is tuned too high, the right amount
of Derivative action should stop oscillation.

Too much Derivative action will probably become sluggish in steering
again.

Extreme example: I tried a very high Derivative value in a motor position controller. When I grabbed the gear and tried to turn it by hand, it resisted my movement. It was like trying to stir very thick treacle. Rotating it by hand very slowly was easy, but rotating fast caused a very high resistance, like a damper.

Integral term: this adds more steering action if the error persists for too long.

If the existing P+D control terms lead to an overall response that
settles short of the required heading, an I term will slowly add a steering command to edge closer (we hope).

Too much Integral action can again cause overshoot/weaving, as the added Integral action turns in the correct way to get onto the commanded heading, but continues to steer because the large I term effectively has a long memory of steering in that direction... it will take a while for the integrated error to reduce and straighten up again.

The common advice often given for most systems is to increase the Proportional setting, until you get good performance. If you have to increase it so far that overshoot occurs (weaving, in the steering example) then start to add a little Derivative to stop the overshoot. Then add more P+D to get better performance. If you notice a state where it steers towards the centre but slows before getting there, start to add some I... but not so much that weaving begins again.

And bear in mind with the vehicle case, getting it all nicely tuned then putting some weight on the back might affect the system dynamics and require more retuning...


问:
Perspective n Point - RPnP algorithm
I need to caculate the pose of a camera using an image of an artificial landmkark. For this porpouse I am trying to use the Perspective n Point approach so I can calculate it using the intrinsic camera matrix, the world coordinates of the landmark (I am using 4 points) and its projection in the image.

There are some algorithms to solve this (PnP, EPnP, RPnP, etc) and I am trying to use the RPnP. I have found an implementation of this here:
http://xuchi.weebly.com/rpnp.html

I used this code but I am having some problems because I can't obtain the correct pose.

I am using the P.Corke's Robotics Toolbox for MATLAB to create a CentraCamera with a known pose and calculating the projection of the landmark in this camera, but the rotation and translation that the RPnP returns me is not the same as I defined before.

Anyone has used this RPnP algorithm to solve that kind of problems?

答: Since you only have 4 points, you should use the P3P algorithm. 3 points give you up to 4 solutions, and you need a fourth point to decide which one is correct. So 4 is the minimum number of points you need solve for the pose. Unfortunately, the P3P algorithm does not generalize to more points, which is why all those other algorithms you mentioned were invented.</p>

Title: Perspective n Point - RPnP algorithm


问:
Are propellers dangerous?
Aren't all propellers super dangerous? How are these startups like Hex and Pocket Drone selling drones as 'kid-friendly' to consumers? What happens if a kid puts his finger in a propeller's movement space while its flying? </p>

答:
Yeeeees!!!
They r very dangerous, I was building a quadcopter, when I finished I was calibrating the esc, during the process, you have to leave the transmitter throttle on max and press few buttons. My quad malfunctioned then went full throttle then attacked my wrist. It was millimetres away from my nerves and tendon. I am only 14 years old. For this accident, I went into surgery and I had 13 stitches!

To conclude, Propellors are extremely dangerous. THEY CAN KILL!!! Never misuse a drone.

Title: Are propellers dangerous?


问:
Steadier wheels - Pin them or lock springs
When running on a hard surface, the Create will shake sometimes during turns or acceleration.

Has anyone ever removed the springs or pinned the wheels in place so they can't move up and down?

答: We did take the springs out. It appears to make it steadier ride on a hard surface when accelerating or turning. </p>


问:
How to compensate the brushless DC motor for voltage drop?
I am working on a quadcopter project based on Arduino board, my system is powered by a 4S LiPo battery (14.8V) but the motors behave differently as the battery voltage drops, when discharging.

Is there any way that I can make the motors behave the same until a minimum value of, say, 5 volts?

My current system works fine at the range from 14.8 to 10 volts, but below that I can't even hover.

答:
STOP USING YOUR BATTERY IMMEDIATELY!

First of all you shouldn't let the voltage of a lipo battery drop below 3V per cell which in your case comes to 12V. Lipo Batteries are extremely dangerous and the way in which you are over-discharging it is one of the ways you definitely should NOT handle a lipo battery.

Please read the instructions given here completely before you start using your battery again -

http://www.dronethusiast.com/ultimate-drone-battery-care/

I don't want to scare you, but you should understand the various high-risk situations involved in using a lipo battery as part of a quadcopter.

Second, check for individual cell voltage of your lipo battery to make sure none of them are over-discharged.

Now, if their voltage is below 3V, be very careful as you might find links online to articles that claim they can help you 'restore' the lipo battery.
An average lipo can take upto 500 recharge cycles and if you've been consistently over-discharging them their capacity will decrease; ie, recharge faster but drain faster.

If this is your first lipo battery, I suggest you get another one and handle it with care from start, or if your current battery is not damaged and all the cells attain nominal voltage, you can continue using it,

But first get a voltage indicator that visually displays the voltage reading on an 7-segment led display.

Take care... No seriously...Take care of the battery.