答:
The easiest way to accomplish lithium charging is to use a brownout tolerant DC-DC converter. By brownout tolerant I mean the converter should automatically derate output voltage to maintain the max current limit with no need to reset or recover once the condition clears.

The output current limit of the converter should be less than or equal to the current limit for the constant current phase of the charge curve. The output voltage should be less than or equal to the constant voltage phase of the charge curve.

In this scheme, when the battery is low on charge, it will also be low on voltage. A larger voltage differential will draw more current, up to the current limit on the converter, which should be approximately equal to the constant current charge limit.

The battery charges and, as it does,  the terminal voltage rises. At some time the voltage differential decreases to the point that the brownout condition ends and the DC-DC converter resumes rated voltage output, which should now be at the constant charge voltage.

A couple of notes, though:

As @hauptmech points out in their terrific content, assume the copter runs 10min at 100W for flight. This means the battery stores about 16 Watt-hours, or 1000 Watt-minutes of energy. Assuming the solar cell outputs 1W, this means it will take about 1000 minutes, or about 16 hours, to recharge the battery. When you try to connect the solar cell output to the system to charge, there is a very high likelihood that you will brown out the solar cell because of the power draw. This means the DC-DC converter needs to have a wide input voltage range, conceivably down to near 0V input, AND that the solar cell needs to be short-circuit tolerant, as that's likely to be the normal operating condition. You could get a DC-DC converter with a current limit closer to the equivalent power output of the solar cell, which may help.

Charging lithium batteries is dangerous. They are banned on most flights, banned on most air mail services, for good reason. See also: hover boards.

You need to be sure to stay under the max ratings. Exceeding the current limit causes heat issues (not that I think you'd exceed a lithium battery's current limit with a solar cell), but exceeding the voltage limit causes dielectric breakdown in the battery, which also causes fires. That is very possible with a solar cell.

Again, as hauptmech mentioned, you need an EE. This isn't really something to guess at, and if you're here looking for advice you should honestly consider if you know what you're doing and seek guidance at least from a senior EE student.


问:
How to find kinematics of differential drive caster robot?
I'm working on a little project where I have to do some simulations on a small robot.

I my case I'm using a differential-drive robot as one of the wheels of a bigger robot platform (which has two differential-drive casters), and I really do not understand how to find its kinematics in order to describe it in a model for finding the speed V_tot of the platform.

This is my robot and I know the following parameters

d is the distance between a joint where my robot is costrained

blue point is the joint where the robot is linked to the robot platform

L is distance between the wheels

r the radius of the wheel

the robot can spin around the blue point and with and THETA angle

As I know all this dimensions, I would like to apply two velocities V_left and V_right in order to move the robot.

Let's assume that V_left = - V_right how do I find analitically the ICR (Istantaneous Center of Rotation) in this costrained robot?

I mean that I cannot understand how to introduce d in the formula.

答:
Kinematics of mobile robots

For the figure on the left:

I = Inertial frame;

R = Robot frame;

S = Steering frame;

W = Wheel frame;

$\beta$ = Steering angle;

For the figure on the right:

L = Distance between the wheels;

r = radious of the wheel;

Now we can derive some useful equations.

Kinematics:

$\hspace{2.5em}$ $\vec{v}_{IW} = \vec{v}_{IR} + \vec{\omega}_{IR} \times \vec{r}_{RS}$

If we express the equation above in the wheel frame:

$\hspace{2.5em}$ $\begin{bmatrix} 0 \\ r\dot{\varphi} \\ 0 \end{bmatrix} = R(\alpha+\beta)R(\theta)\begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{bmatrix} + \begin{bmatrix} 0 & -\dot{\theta} & 0 \\ \dot{\theta} & 0 & 0 & \\ 0 & 0 & 0 \end{bmatrix}\begin{bmatrix} lcos(\beta) \\ -lsin(\beta) \\ 0 \end{bmatrix}$

We obtain the rolling-constraint and the no-sliding constraint respectively:

$\hspace{2.5em}$ $[-sin(\alpha+\beta)\hspace{1.0em}cos(\alpha+beta)\hspace{1.0em}lcos(\beta)]\dot{\xi}_{R} = \dot{\varphi}r$

$\hspace{2.5em}$ $[cos(\alpha+\beta)\hspace{1.0em}sin(\alpha+beta)\hspace{1.0em}lsin(\beta)]\dot{\xi}_{R} = 0$

where $\dot{\xi}_{R} = [\dot{x_{R}}\hspace{1.0em}\dot{y_{R}}\hspace{1.0em}\dot{\theta}]^{T}$

Now we need to apply each of this constraints in the differential wheels

For the left wheel: $\alpha = -\frac{\pi}{2}$, $\beta = 0$, $l = -\frac{L}{2}$

For the right wheel: $\alpha = -\frac{\pi}{2}$, $\beta = 0$, $l = \frac{L}{2}$

Stacked equation of motion:

$\hspace{2.5em}$ $\begin{bmatrix} 1 & 0 & \frac{L}{2} \\ 1 & 0 & -\frac{L}{2} & \\ 0 & -1 & 0 \\ 0 & -1 & 0 \end{bmatrix}\dot{\xi}_{R} = \begin{bmatrix} r & 0\\ 0 & r \\ 0 & 0 \\ 0 & 0 \end{bmatrix}\begin{bmatrix} \dot{\varphi}_{r} \\ \dot{\varphi}_{l} \end{bmatrix} $

$\hspace{2.5em}$ $A\dot{\xi}_{R} = B\dot{\varphi} $

For the forward kinematic solution, just do:

$\hspace{2.5em}$ $\dot{\xi}_{R} = \big( A^{T}A \big)^{-1}A^{T}B\dot{\varphi} $

which yields:

$\hspace{2.5em}$ $\begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{bmatrix} = \begin{bmatrix} \frac{r}{2} & -\frac{r}{2} \\ 0 & 0 \\ \frac{r}{L} & -\frac{r}{L} \end{bmatrix} \begin{bmatrix} \dot{\varphi}_{r} \\ \dot{\varphi}_{l} \end{bmatrix}$

An excellent chapter that I suggest here.

ContentLicense: CC BY-SA 3.0


问:
Figure out PID values from drone specs
I have all the specs from a quadcopter, everthing, would it be possible to figure out the pid from those specs?</p>

答:
Quadrotor Control Problem

It depends! You have a good mathematical model? After you achieve one, you might need to choose a good system identification method. Comparing their outputs you should have enough data to start thinking on the controller design.

Beyond this, I can't say precisely what you want or need...

Title: Figure out PID values from drone specs


问:
How to find the body jacobain, for each link in a robot manipulator?
The links twist could be obtained, and thus The spatial manipulator Jacobian could be done, but when it comes to the body Jacobian, it is becomes difficult. Moreover, the adjoint transformation relates both Jacobain, but however that is 4*4 while the Jacobian is 6*n; how does it works? as in the picture, he is getting a body jacobian for each link, not one jacobian matrix for the whole robot, I don't know.
Any help is highly regarded.
Like this example or here for
full details</p>

Body:
Worked example

$\hspace{2.5em}$ $\vec{q}$ = $[q_{1}\hspace{1em}q_{2}]^{T}$ $\hspace{1.5em}$ [Generalized coordinate]

$\hspace{2.5em}$ $\vec{J}$ =  $\frac{\partial \vec{r}_{OA}(\vec{q})}{\partial\vec{q}}$ = $\begin{bmatrix} \frac{\partial \vec{r}_{1}}{\partial\vec{q}_{1}} & ... & \frac{\partial \vec{r}_{1}}{\partial\vec{q}_{n}} \\ ... &  & ... & \\ \frac{\partial \vec{r}_{m}}{\partial\vec{q}_{1}} & ... & \frac{\partial \vec{r}_{m}}{\partial\vec{q}_{n}} \end{bmatrix}$

The kinematic equation:

$\hspace{5.em}$ $_{o}\vec{r}_{OA}$ = $_{o}\vec{r}_{O1}$ + $_{o}\vec{r}_{12}$ + $_{o}\vec{r}_{2A}$

We have two rotations over joint 1 and 2. Substituting in the equation above:

$\hspace{5.em}$ $_{o}\vec{r}_{OA}$ = $_{0}\vec{r}_{O1}$ + $R{(q_{1})_{01}}$ $ _{1}\vec{r}_{12}$ + $R{(q_{1}+q_{2})_{12}}$ $ _{2}\vec{r}_{2A}$

Where $R(\bullet)$ is the rotation matrix.

$\hspace{5.em}$ $_{0}\vec{r}_{O1}$ = $\begin{bmatrix} l_{0} \\ 0 \\ 0 \end{bmatrix}$

$\hspace{5.em}$ $R{(q_{1})_{01}}$ $ _{1}\vec{r}_{12}$ = $\begin{bmatrix} cos(q_{1}) & -sin(q_{1}) & 0 \\ sin(q_{1}) & cos(q_{1}) &  0 \\ 0 & 0 & 1\end{bmatrix}$  $\begin{bmatrix} l_{1} \\ 0 \\ 0 \end{bmatrix}$

$\hspace{5.em}$ $R{(q_{1}+q_{2})_{12}}$ $ _{2}\vec{r}_{2A}$ = $\begin{bmatrix} cos(q_{1}+q_{2}) & -sin(q_{1}+q_{2}) & 0 \\ sin(q_{1}+q_{2}) & cos(q_{1}+q_{1}) &  0 \\ 0 & 0 & 1\end{bmatrix}$  $\begin{bmatrix} l_{2} \\ 0 \\ 0 \end{bmatrix}$

$\hspace{5.em}$ $ _{0}\vec{r}_{OA}$ = $\begin{bmatrix} l_{0} + l_{1}cos(q_{1}) + l_{2}cos(q_{1}+q_{2}) \\ 0 + l_{1}sin(q_{1}) + l_{2}sin(q_{1}+q_{2}) \\ 0 \end{bmatrix}$

The Jacobian:

$\hspace{2.5em}$ $\vec{J}$ =  $\frac{\partial \vec{r}_{OA}(\vec{q})}{\partial\vec{q}}$ = $\begin{bmatrix} -l_{1}sin(q_{1}) -l_{2}sin(q_{1}+q_{2}) & -l_{2}sin(q_{1}+q_{2}) \\ l_{1}cos(q_{1})+l_{2}cos(q_{1}+q_{2}) & l_{2}cos(q_{1}+q_{2}) \\ 0 & 0\end{bmatrix}$

$\hspace{5.em}$ $ _{0}\dot{\vec{r}}_{OA}$ = $\vec{J}\dot{\vec{q}}$

For the goal, you have to use $\vec{q} = [0\hspace{0.5em}0]^{T}$. Usually the Jacobian is not invertible, so you need to use the pseudo-inverse.


问:
Forward kinematic computing the transformation matrix
I am the moment trying to compute the transformation matrix for robot arm, that is made of 2 joints (serial robot arm), with which I am having some issues. L = 3, L1 = L2 = 2, and q = ($q_1$,$q_2$,$q_3$) = $(0 , \frac{-\Pi}{6},\frac{\Pi}{6})$

Based on this information I have to compute the forward kinematic, and calculate the position of each joint.

Problem here is though, how do I compute the angle around x,y,z.. for the transformation matrix.  Using sin,cos,tan is of course possible, but what do their angle corresponds? which axis do they correspond to?

I tried using @SteveO answer to compute the $P_0^{tool}$ using the method he provided in his answer, but I somehow mess up something, as the value doesn't resemble the answer given in the example..

答:
Try the approach  used in here

The rotation matrix, using generalized coordinates, it's straightforward! Do the math and check it by trigonometry identities.

Good luck!


问:
Proportional controller error doesn't approach zero
I'm reading this pdf. The dynamic equation of one arm is provided which is

$$
l \ddot{\theta} + d \dot{\theta} + mgL sin(\theta) = \tau
$$

where

$\theta$ : joint variable.

$\tau$ : joint torque

$m$ : mass

$L$ : distance between centre mass and joint.

$d$ : viscous friction coefficient

$l$ : inertia seen at the rotation axis.

I would like to use P (proportional) controller for now.

$$
\tau = -K_{p} (\theta - \theta_{d})
$$

My Matlab code is

```<code>clear all
clc

t = 0:0.1:5;

x0 = [0; 0];

[t, x] = ode45('ODESolver', t, x0);

e = x(:,1) - (pi/2); % Error theta1

plot(t, e);
title('Error of \theta');
xlabel('time');
ylabel('\theta(t)');
grid on
</code>```

For solving the differential equation

```<code>function dx = ODESolver(t, x)

dx = zeros(2,1);
%Parameters:

m  = 2;
d  = 0.001;
L  = 1;
I = 0.0023;
g  = 9.81;

T = x(1) - (pi/2);

dx(1) = x(2);

q2dot = 1/I*T - 1/I*d*x(2) - 1/I*m*g*L*sin(x(1));

dx(2) = q2dot;
</code>```

The error is

My question is why the error is not approaching zero as time goes? The problem is a regulation track, so the error must approach zero.

答:
Controllers type

A more mathematical approach to the error.

Suppose you have a close loop system like above. The equation is:

$\hspace{2.5em}$ $Y(s) = \frac{G(s)C(s)}{1+G(s)C(s)} R(s)$

The error equation is:

$\hspace{2.5em}$ $E(s) = R(s) - Y(s)$

$\hspace{2.5em}$ $E(s) = \frac{1}{1+G(s)C(s)} R(s)$ $\hspace{2.5em}[1]$

The final value theorem states that:

$\hspace{2.5em}$ $e(\infty) = \lim_{s \rightarrow 0}sE(s)$ $\hspace{2.5em}[2]$

Using $[1]$ in $[2]$:

$\hspace{2.5em}$ $e(\infty) = \lim_{s \rightarrow 0}s\frac{1}{1+G(s)C(s)} R(s)$

Ok! Now, we can conclude some cool stuffs.

Suppose $R(s) = \frac{1}{s}$:

$\hspace{2.5em}$ $e(\infty) = \lim_{s \rightarrow 0}\frac{1}{1+G(s)C(s)}$ $\hspace{2.5em}$ From calculus, is the same as:

$\hspace{2.5em}$ $e(\infty) = \frac{1}{1+\lim_{s \rightarrow 0} (G(s)C(s))}$

$\hspace{2.5em}$ $e(\infty) = \frac{1}{1+G(0)C(0)}$

If $C(s) = K$, where $K$ is a proportional gain, $G(0)$ have a finite DC gain, your system is going to be the responsible for the finite final term. But, however, you can apply a PD control, $C(s) = \frac{K}{s}$.

$\hspace{2.5em}$ $C(0) = \lim_{s \rightarrow 0}\frac{K}{s} = \infty$

Then:

$\hspace{2.5em}$ $e(\infty) = \frac{1}{1+G(0)(\infty)} = 0$

And that's the resume why you can't get null error only with a proportional gain.


问:
Inverse Kinematics of DLR/HIT II Hand
I am trying to find the inverse kinematics formulation of DLR/HIT II hand. Till now I had success in finding an analytical method described in the thesis of Mavrogiannis, Christoforos I. named Grasp Synthesis Algorithms for Multifingered Robot Hands, given in appendix B.
My question is regarding the A.28 where the author calculates q3 but has mentioned previously in the text that q3 is equal to q2.

Note: q here denotes joint angles

答:
It looks like maybe the author ditched the zero-based numbering, but I agree that it is hard to tell. </p>

<p>Why not try to contact the author directly? His LinkedIn page shows he's currently a graduate research assistant at Cornell. I've contacted authors in the past and everyone I've gotten in touch with seemed pleased that I did so. </p>

Title: Inverse Kinematics of DLR/HIT II Hand

Body:
It looks like maybe the author ditched the zero-based numbering, but I agree that it is hard to tell.

Why not try to contact the author directly? His LinkedIn page shows he's currently a graduate research assistant at Cornell. I've contacted authors in the past and everyone I've gotten in touch with seemed pleased that I did so.


问:
q-state vector used to define the transformation matrix? how?
How can it be used to determine the transformation matrix?

an example could be at computing the inverse kinematics for small displacements:
J(q)$\Delta$q = $\Delta$u

$\Delta$U is a vector defining the difference between current and desired position. The desires position can always be computed, but if keep solving this in such manner that every time you solve $$J(q)\Delta q = \Delta u$$

you do this

q:= q + $\Delta$q

Compute $T_{base}^{tool}(q)$

Compute the difference between $[T^{tool}_{base}]_{desired~position} $ and $T_{base}^{tool}(q)$.

If change is less than 10^-5 finish and output Q, if not resolve.

How would you compute The transformation matrix based on q state vector.

答:
The "state vector"  $\vec q$ is the vector of joint angles.  Use Craig's, or Paul's, book for understanding how to compute the transformation matrix that relates the end effector position and orientation, $\vec u$, to $\vec q$.   You can also use 3x3 rotation matrices and a vector loop.  If you don't have those references, just search for Denavit-Hartenburg.  </p>

<p>After you have that transformation matrix, you can find $J$.  $J$ is the partial derivative of your transformation matrix with respect to the individual joint angles.   That should be enough to feed the algorithm you describe.  </p>

Body:
The "state vector"  $\vec q$ is the vector of joint angles.  Use Craig's, or Paul's, book for understanding how to compute the transformation matrix that relates the end effector position and orientation, $\vec u$, to $\vec q$.   You can also use 3x3 rotation matrices and a vector loop.  If you don't have those references, just search for Denavit-Hartenburg.

After you have that transformation matrix, you can find $J$.  $J$ is the partial derivative of your transformation matrix with respect to the individual joint angles.   That should be enough to feed the algorithm you describe.


问:
How can I send video from my Arduino camera module video to my Android screen?
I'm trying to connect a camera module to my Arduino Mega, connect my Mega to my Android phone (throught BlueTooth or other), and send the live view of the camera to the mobile phone.

I saw a video online that showed this for still images -- an image captured by the camera module on the Arduino was sent to Android and the output image was viewed after a couple of seconds (the time to send image by BT).

Is this doable with live video instead of image?  If yes, please guide me; if no, please suggest some workarounds.

答:
Yes , it is possible but please note the following points.

1)if you are using Bluetooth, it data transfer rate depends upon different versions like for v2 blue tooth its data transfer rate 3 Mb\s.

2)Now let suppose you are using a camera like ov7670 in QVGA mode(320 x 240 pixels for one frame) each pixel is 8 bits, for a good video you require 30 frames per second, which is 76800 bytes for one frame, for 30 frame it is  2.3 Mb\s.

3) Now 2.3 MB\S < 3 MB\s, you can live vedio streaming.

4) The rest depends how good you are in understanding programing and making Electronics connections with different modules.


问:
Typical Problem in Simple Line Follower Using 3 sensors
I am working on building a line follower robot using ATmega2560 and I want its movement to be more precise. I am facing a very typical problem.
It consists of three(3) IR sensors. The thickness of the line to be followed is 1.2cm and the gap between the sensors is more than that, around 1.8cm.

So if the black line comes between the center and any of the side sensors, all the three sensors are on white and it stops.
And I need the robot to stop over white, due to my application.
So please can anyone suggest me a good algorithm to tackle this situation.
I think PID control can be of good use, as i searched on Google. But I don't understand how to implement it with three sensors.
Please Help

答:
I don't think you'll be able to use PID control successfully because you don't have a continuous feedback signal. Your feedback is binary on each of three sensors.

From your description, you need to monitor or eliminate the gap between the sensors. Monitor meaning add more sensors in the gap, and eliminate meaning move the existing sensors close enough together that the gap is smaller than the line thickness.

You have a good description of your problem, but you have a poor description of the scenario. For instance:

Why did you set the gap to be wider than the line?

What happens when the robot is supposed to stop? Does the line you were following just end?

How quickly do you need to stop after detecting all white?

Why are you using three sensors?

If I were you, I would use two sensors to follow the line. Set them equal distances on either side of the "center" you would like to use for line following. The total distance between the sensors should be less than or equal to the line thickness.

Now, with two sensors, if both register black, you are on the line and should go straight. If the left sensor fails to register black then you are too far to the left and you need to turn right. When both register black then turn a little to get straight again then resume going straight. Same process for the right sensor: when it fails to register black, start turning, then once both register black then course-correct back to straight and resume straight driving again.

With one sensor only to detect if you're on the line you won't know what action to take after you depart from your intended course because you have no way of knowing which way you need to go back for course correction.


问:
Arduino Nano + Raspberry Pi = UAV Ground Station?
I'm a programmer by trade, and an amateur aerospace nut, with some degree-level training in both fields. I'm working on a UAV project, and while the good people over at DIY Drones have been very helpful, this question is a little less drone-related and a little more general robotics/electronics. Essentially, I'm looking at options for ground stations, and my current rough plan is something like this:

I have a PC joystick with a broken sensor in the base, which I plan to dismantle, separate the handle from the base, insert an Arduino Nano into the (mostly hollow) handle and hook it up to all the buttons and the hat thumbstick. Then, where the hole is that used to accept the stem to the base, I fit a bracket that runs horizontally to hold a smallish touchscreen (think Razer's Project Fiona tablet with only one stick), behind which is mounted a Raspberry Pi. The Nano talks to the RPi over USB as a HID input. The RPi will be running some custom software to display telemetry and other data sent down from the UAV.

My main question whether that Nano would have enough power to run the XBee that provides the telemetry link without causing lag in the control inputs. It's worth mentioning that the UAV will be doing fly-by-wire moderation, so slight stutters won't result in wobbly flying, but serious interruptions will still be problematic - and annoying. It's also worth mentioning that this will only be used as a simplified "guiding hand" control; there will ALWAYS be a regular remote control available (not least because of EU flight regulations) so this is just for when I don't want to use that. If that Nano won't do, what are my options? My first thought is to get a second Nano and get that to drive the XBee (the RPi has two USB ports after all) but there may well be a better way.

答:
The Arduino Nano should be able to interface with the XBee no problem.

If all you are using the Arduino for is forwarding a message that comes from the RPi, you might as well just use an XBee Explorer board (basically a FTDI Chip with a socket fit for an XBee). After configuration, all you need to do is to write to a serial port to get the XBee to send a message... All processing is done in-chip.


问:
How to combine odometry information with time-shifted information from IMU?
I'm working with a differential-drive robot that has odometry measurements from wheel shaft encoders and heading information from an IMU (I'm using BNO055 in IMU mode to get Euler angles, primarily the heading angle).

I'd like to use the IMU header angle to augment the odometry which is prone to slipping and other errors, but the IMU lags the odometry by up to 100 ms.

How can I combine these measurements to get the best estimate of the robot pose?

Thanks for any word on this.

答:
Combining two sensor outputs that should theoretically give you the same information is called "sensor fusion".

The easiest way to go about this is with a tool known as a complimentary filter. The complimentary filter uses a "blend" value, between 0 and 1, to produce the output:

```<code>filterOutput = blend*sensor1 + (1-blend)*sensor2;
</code>```

Typically you have the sensor that is more reliable short term weighted much more than the sensor that is more reliable long term. Blend values around 0.95 are common. If you think both sensors are equally reliable, then set blend to 0.5. This simply averages the two.

In your case you know odometry is unreliable long term because of wheel slip and IMU short term because of delays, so I would weight odometry 0.95 and IMU 0.05 to start. See how that works and adjust as necessary.

ContentLicense: CC BY-SA 4.0

Body:
Combining two sensor outputs that should theoretically give you the same information is called "sensor fusion".

The easiest way to go about this is with a tool known as a complimentary filter. The complimentary filter uses a "blend" value, between 0 and 1, to produce the output:

filterOutput = blend*sensor1 + (1-blend)*sensor2;

Typically you have the sensor that is more reliable short term weighted much more than the sensor that is more reliable long term. Blend values around 0.95 are common. If you think both sensors are equally reliable, then set blend to 0.5. This simply averages the two.

In your case you know odometry is unreliable long term because of wheel slip and IMU short term because of delays, so I would weight odometry 0.95 and IMU 0.05 to start. See how that works and adjust as necessary.


答:
You're robot must be moving pretty fast for this delay to cause problems.

Use a circular buffer to store the odometry readings, so you'll have a record of the last 100ms of odometry readings.

Use the IMU and odom combined at time $T_{-100ms}$ to estimate your last known 'accurate' state. Integrate the odom forward from $T_{-100ms}$ to $T_{0}$ to get your current state. Repeat this at every time step.

If your control can handle the delay, don't bother with integrating the odom. Just use your accurate state 100ms ago.

Title: How to combine odometry information with time-shifted information from IMU?

Body:
I'm working with a differential-drive robot that has odometry measurements from wheel shaft encoders and heading information from an IMU (I'm using BNO055 in IMU mode to get Euler angles, primarily the heading angle).

I'd like to use the IMU header angle to augment the odometry which is prone to slipping and other errors, but the IMU lags the odometry by up to 100 ms.

How can I combine these measurements to get the best estimate of the robot pose?

Thanks for any word on this.


问:
Autonomous robots hardware structure planning
I ask this question to clear my concept about hardware structure of humanoid autonomous fire robot, Here is scenario a fire robot detect humans from fire, there are some vision cameras some temperature and smoke sensors which help to perform this task. Now a days in market there are many processors like ARMV7 and Snapdragon which process tasks in any device and control the system i don't think autonomous fire robot use some kind of processors.

Does autonomous robots like fire robot use processors or micro controllers. Does it require OS or Rams environment? Like any computer system which use these kind of things.

答:
Vision will typically use as much processing power as you can provide. Large humanoid (human size) control systems also take a lot of processing power. It is common for a large humanoid to have at least two large computers (one for vision, one for control) or more. Also, each motor controller and many sensor subsystems will have microcontrollers.

There is no preferred OS that I know of. Linux with Xenomai or preempt patch seems to be popular and what I use. I would not be surprised to find vxworks, qnx, xPC or realtime windows running the current generation of humanoids. The PC doing the motor control will need a real-time capable OS (which all of the above are).

The Darpa Robotics Challenge last year generated a lot of good documentation describing various humanoid systems. You can find lots of papers describing in detail the computer systems used for many of the contestants. An example is http://drc.mit.edu/docs/2014_jfr_fallon.pdf


答:
As the other answers suggest, 100ms of delay in control may not be significant to your application. It would be prudent to first solve your data-fusion problem and then see if the delay is an issue to your controller. I would first record some data of a closed-loop path and do some offline filtering to see if the results are good. For instance, make your robot drive a square and record that data.

Your first problem is then to synchronise the data streams; since your IMU is lagging by 100ms, then simply operate on the newest IMU data you have, with the odometry from the corresponding time.

The data-fusion could be done using a Kalman filter for the 2D case (X,Y,heading). The heading is updated by the IMU and the velocity (and heading, depending on the model) is updated by the odometry.
See the system model in this paper, which uses odometry. A good solution would involve a nonlinear KF such as EKF, but I suspect for low speeds and high sensor rates you can get away with a linear KF. I think the states would be $\mathbf{[x, y, \phi, x', y', \phi']}^\top$, where your process model would be the same as the first paper I linked, and the measurement model is simply $\mathbf{y}= \mathbf{u} + v$, where v is Gaussian white noise with known standard deviation. Maybe see this kalman filter framework.

After you have some working results with your offline data, you can worry about implementing it online. The implementation will greatly depend on your choice of data-fusion algorithm.

See also: particle filter methods and a full ROS implementation.


问:
Modified DH Parameters?
Is the notation of the geometry of robots from Khalil and Kleinfinger be considered as one of the probably "many" Modified DH Parameters?</p>

答: Khalil himself says that it is a form of the modified D-H parameters.  See, e.g., Section 2.1 of the 2000 Springer-Verlag book Advances in Robot Kinematics.

Title: Modified DH Parameters?


问:
Is it possible to achieve arbitrary precision in camera calibration?
Is it possible to achieve arbitrary precision to the calibration of the extrinsic parameters of a camera or is there a minimum error wich can not be compensated (probably dictated by the camera's resolution)?</p>

答:
It is not possible to achieve arbitrary precision in camera calibration, precisely because of camera resolution and camera imperfections (e.g. lens distortion, which is only approximated in the calibration of intrinsic parameters).

That, however, isn't usually the worst part of the deal. You can go a long way if you are able to fabricate your calibration target with a decent precision. According to Trucco (p.124):

The accuracy of calibration depends on the accuracy of the measurements of the calibration pattern; that is, its construction tolerances. To be on the safe side, the calibration pattern should be built with tolerances one or two order of magnitudes smaller than the desired accuracy of calibration. For example, if the desired accuracy of calibration is  0.1mm, the calibration pattern should be built with tolerances smaller than 0.01mm.

A good quote for common sense, also by Trucco (p.136):

A practical guideline is: the effort going into improving calibration accuracy should be commensurate to the requirements of the application.

[1] E. Trucco, A. Verri. Introductory Techniques for 3-D Computer Vision. Prentice Hall, 1998


问:
Localization with only IMU
What will be the best approach to get the most localization accuracy out of only an accelerometer and gyroscope?</p>

答: I believe internal sensors such as those are impossible for localization.  Navigation from a known location is possible with common methods of inertial navigation.  These methods are limited by the accuracy of the sensors and external observations.</p>

Title: Localization with only IMU


问:
iRobot Create 2 IR bump light sensor specifications
Can someone tell me where can I find some specifications about the iRobot Create 2 IR bump light sensors?

We have an SDF model of the Create 2 that uses the Hoyuko laser range finder sensor and we have to simulate the behavior of the IR sensors starting from the laser scan data.

Hence we would like to have some additional information on the IR sensors which we can't find anywhere on the web - such as their exact position in the robot chassis, their maximum range, the shape of the obstacle detection field and so on.

答: Unfortunately, I believe this is proprietary information that you won't likely get out of iRobot.  So your best bet is to characterize the sensors yourself.  You can even take apart the bumper assembly if you like.  (And if you are careful you should be able to get it back together.)  </p>


答:
No, you cannot determine the forward kinematics transformation matrix from the Jacobian.  You can however go the other direction (kind of).  The analytical Jacobian is simply the partial derivatives of the forward kinematics equation.  Which makes sense because the forward kinematics equation gives you position, and the Jacobian (the derivative) gives you velocity.  But this is in general different than the geometric Jacobian.  Which is typically what people use.

The geometric and analytical Jacobians differ in how they handle rotational velocity.  The geometric Jacobian uses angular velocity $\omega$, which is a more physically intuitive representation.  Whereas the analytical Jacobian uses the time derivative of Euler angles $\dot\phi$, which suffers from a representational singularity.  Furthermore, you can integrate $\dot\phi$ over time to get $\phi$, however this cannot be done with $\omega$.

So if you have the forward kinematics equation, you can differentiate to get the analytical Jacobian, which you can then integrate to get back the forward kinematics equation, (but why?).  But if you start from the geometric Jacobian, you cannot integrate to get the proper forward kinematics equation.

See Robotics: Modelling, Planning and Control, by Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani, Giuseppe Oriolo.  Chapter 3.6 for full discussion.

Title: q-state vector used to define the transformation matrix? how?

Body:
How can it be used to determine the transformation matrix?

an example could be at computing the inverse kinematics for small displacements:
J(q)$\Delta$q = $\Delta$u

$\Delta$U is a vector defining the difference between current and desired position. The desires position can always be computed, but if keep solving this in such manner that every time you solve $$J(q)\Delta q = \Delta u$$

you do this

q:= q + $\Delta$q

Compute $T_{base}^{tool}(q)$

Compute the difference between $[T^{tool}_{base}]_{desired~position} $ and $T_{base}^{tool}(q)$.

If change is less than 10^-5 finish and output Q, if not resolve.

How would you compute The transformation matrix based on q state vector.


答:
This is similar to this question: imu position without GPS or camera

The short answer is that what you ask is not possible given realistic constraints.

Title: Localization with only IMU

Body: What will be the best approach to get the most localization accuracy out of only an accelerometer and gyroscope?</p>


答:
If I'm understanding your architecture (from joystick to UAV) correctly, it is this:

${[\text{joystick}]-\text{wires}- \atop [\text{RPi}]-\text{USB HID}-}-[\text{Nano}]-\text{XBee}-[\text{UAV}]$

You're considering laying it out like this:

$[\text{joystick}]-\text{wires}-[\text{Nano}]-\text{USB HID}-[\text{Rpi}]-\text{USB HID}-[\text{Nano}]-\text{XBee}-[\text{UAV}]$

The Nano should be more than powerful enough to run the XBee and the controls.  Even if it wasn't fast enough, I doubt that you could improve it by adding the latency introduced by two USB HID links and the processing onboard the RPi.

Title: Arduino Nano + Raspberry Pi = UAV Ground Station?

Body:
I'm a programmer by trade, and an amateur aerospace nut, with some degree-level training in both fields. I'm working on a UAV project, and while the good people over at DIY Drones have been very helpful, this question is a little less drone-related and a little more general robotics/electronics. Essentially, I'm looking at options for ground stations, and my current rough plan is something like this:

I have a PC joystick with a broken sensor in the base, which I plan to dismantle, separate the handle from the base, insert an Arduino Nano into the (mostly hollow) handle and hook it up to all the buttons and the hat thumbstick. Then, where the hole is that used to accept the stem to the base, I fit a bracket that runs horizontally to hold a smallish touchscreen (think Razer's Project Fiona tablet with only one stick), behind which is mounted a Raspberry Pi. The Nano talks to the RPi over USB as a HID input. The RPi will be running some custom software to display telemetry and other data sent down from the UAV.

My main question whether that Nano would have enough power to run the XBee that provides the telemetry link without causing lag in the control inputs. It's worth mentioning that the UAV will be doing fly-by-wire moderation, so slight stutters won't result in wobbly flying, but serious interruptions will still be problematic - and annoying. It's also worth mentioning that this will only be used as a simplified "guiding hand" control; there will ALWAYS be a regular remote control available (not least because of EU flight regulations) so this is just for when I don't want to use that. If that Nano won't do, what are my options? My first thought is to get a second Nano and get that to drive the XBee (the RPi has two USB ports after all) but there may well be a better way.


问:
Establishing Data Transfer between two Raspberry Pi's using GPIO
Good day

I am currently implementing an autonomous quadcopter with stereo vision using raspberry Pi. One (Let's call this Pi_1) is responsible for stereo vision the other is responsible for motor control and trajectory planning (Pi_2). I was looking for a way to transfer a 480 element float vector via GPIO from Pi_1 to Pi_2. Pi_1 stereovision program runs at 2Hz while Pi_2 motor control runs at 210Hz.

Is there any protocol fast enough to deliver this amount of information to the second raspberry pi via GPIO?

I am currently looking at SPI but I saw that the Raspberry Pi cannot be turned to a Slave making it not an option. I also looked at UART however it is too slow for my needs. All the I2c ports on the Pi are currently being used by the stereo vision cameras and the IMU's. If the gpio option is not feasible, I am also open for other suggestions such as using other hardware (middle man) or wireless options.

答:
I2C is a bus, not a particular direct connection. What's the problem with connecting the two over I2C? Actually, the Wikipedia page for I2C shows the image below, with two microcontrollers connected via the I2C bus.

You might be able to transition your other devices from I2C to SPI and leave the I2C bus open for uC-uC comms.


问:
Wheel encoder triggers interrupt too many times
I am building a simple robot with two driving wheel.
I want to control the wheel rotation using a wheel encoder like this one.

Here is a code I have on Arduino to try to understand the problem I'm facing:

int count = 0;
void setup() {
  Serial.begin(9600);
  attachInterrupt(digitalPinToInterrupt(2), upL, RISING);
  pinMode(2, INPUT);
}

void loop() {
  Serial.println(String(digitalRead(2)) + "__" + String(count));
}

void upL(){
  count++;
}
```

<p>What I notice is:<br>
The interrupt is triggered <strong>multiple times</strong> when the sensor beam is cut <strong>once</strong>.<br>
But when I <em>digitalRead</em> the pin, then there is only one change.  </p>

<p>I also noticed that the interrupt is also triggered when going from HIGH to LOW.</p>

<p>Here is an example of the ouput I have:</p>

```0__0
0__0
0__0
0__0
...
...
0__0
0__0
0__0   <<< change from LOW to HIGH...
1__9   <<< the interrupt must have incremented only once... 9 instead !
1__9
1__9
1__9
...
...
1__9
1__9   <<< change from HIGH to LOW. the interrupt shouldn't be triggered
0__24  <<< still... we have 15 increments
0__24
0__24
0__24
0__24
...
...
0__24
0__24   <<< same here...
1__51   <<< 26 increments
1__51
...
...
1__51  <<< same here...
0__67  <<< 16 increments
0__67
0__67
```

The only way I can explain that is that during a change of state, the signal received from the sensor is not really square, but somehow noisy.

Like in this image :

Therefore we would have, indeed, many RISING on one change....

(However reading the output of the sensor on an analog pin shows a direct variation from 880(HIGH) to 22(LOW))

Does anyone have another explanation? Or a solution to this problem ?

EDIT

Thanks to @TobiasK I know that this is called a bouncing effect. By doing further research I came across this solution:

playground.arduino.cc/Main/RotaryEncoders (Ctrl+F for rafbuff).

I'm trying it and i'll let you know.

答:
Your signal actual going back and forth like that; it's registering with the microcontroller because you're at an intermediate voltage. As @TobiasK mentions, this is called "bouncing".

You're trying to use this for controlling tires, so I would suggest you do a little math to determine whether or not a "subsequent" signal could be considered valid or not.

Distance traveled is d = r*theta;, where r is the wheel radius and theta is the angular distance (in radians) the wheel rotated. Your encoder has some number of pulses per revolution (ppr), so, at top speed, you can expect some number of pulses per minute, or pulses per second. You can either determine this with the maximum wheel speed in RPM or in maximum vehicle speed in [distance]per minute.

Then, take the inverse of the maximum pulses per second to get the minimum seconds per pulse. Any pulse that you detect in a time period shorter than the minimum seconds per pulse is then invalid, so you can ignore it.

Logically, some pseudo-code might look like:

```<code># Initialization:
validPulse = 0;

# In the loop:
if (pulse detected)
  if ( (currentTime) - validPulse ) < ( pulseThreshold )
    <do nothing>
  else
    validPulse = (currentTime);
    <do your thing here>
  end
end
</code>```

In thinking about this for a moment, I would actually caution you to double your pulses per second (halve the seconds per pulse) value because you're prone to bounce on the rising AND falling pulse, as your data above shows.

Ultimately you're going to have to do something to filter the noise (bounce) out of your signal. You can either do this in a fixed manner, with electronic components (RC filter) or you can do it in software. I would always recommend software whenever possible because it's so easy to update or modify if you find your parameters change down the line, such as upgrading to faster motors, etc.

You can even perform all of the calculations in your initialization section of your code, where the calculations are based on parameters you have. Then, if you swap motors, just update the motor top speed in the code and the bounce-checking duration updates automatically.

Body:
Your signal actual going back and forth like that; it's registering with the microcontroller because you're at an intermediate voltage. As @TobiasK mentions, this is called "bouncing".

You're trying to use this for controlling tires, so I would suggest you do a little math to determine whether or not a "subsequent" signal could be considered valid or not.

Distance traveled is d = r*theta;, where r is the wheel radius and theta is the angular distance (in radians) the wheel rotated. Your encoder has some number of pulses per revolution (ppr), so, at top speed, you can expect some number of pulses per minute, or pulses per second. You can either determine this with the maximum wheel speed in RPM or in maximum vehicle speed in [distance]per minute.

Then, take the inverse of the maximum pulses per second to get the minimum seconds per pulse. Any pulse that you detect in a time period shorter than the minimum seconds per pulse is then invalid, so you can ignore it.

Logically, some pseudo-code might look like:

# Initialization:
validPulse = 0;

# In the loop:
if (pulse detected)
  if ( (currentTime) - validPulse ) < ( pulseThreshold )
    <do nothing>
  else
    validPulse = (currentTime);
    <do your thing here>
  end
end

In thinking about this for a moment, I would actually caution you to double your pulses per second (halve the seconds per pulse) value because you're prone to bounce on the rising AND falling pulse, as your data above shows.

Ultimately you're going to have to do something to filter the noise (bounce) out of your signal. You can either do this in a fixed manner, with electronic components (RC filter) or you can do it in software. I would always recommend software whenever possible because it's so easy to update or modify if you find your parameters change down the line, such as upgrading to faster motors, etc.

You can even perform all of the calculations in your initialization section of your code, where the calculations are based on parameters you have. Then, if you swap motors, just update the motor top speed in the code and the bounce-checking duration updates automatically.


问:
Software to control an Arduino setup with a timing belt and stepper motors
I would like to know what software is available to control a timing belt with stepper motors for arduino board.Much like how its done in 3d printing.
But in this case i wont be making a 3d printer.Just one simple setup.</p>

答:
Typically stepper motor control isn't necessarily done in software but in hardware. See this stepper motor driver for an example. </p>

<p>The driver allows you to set bits to control how much you would like to step (full, half, quarter, etc.), and takes care of actually providing driving current to the motor. Some drivers may allow you to set a speed in steps/time, but typically after that point you send a pulse train to the motor driver; this can be done with an interrupt or other hardware timer. </p>

Body:
Typically stepper motor control isn't necessarily done in software but in hardware. See this stepper motor driver for an example.

The driver allows you to set bits to control how much you would like to step (full, half, quarter, etc.), and takes care of actually providing driving current to the motor. Some drivers may allow you to set a speed in steps/time, but typically after that point you send a pulse train to the motor driver; this can be done with an interrupt or other hardware timer.


答:
Here you go.  This will control 2 steppers on an Arduino.

```<code>#include <Stepper.h>

#define STEPS 200       // change this to the number of steps on your motor

// create an instance of the stepper class, specifying
// the number of steps of the motor and the pins it's attached to
Stepper stepperX(STEPS, 2, 3, 4, 5);    // Arduino pins attached to the stepper driver
Stepper stepperY(STEPS, 8, 9, 10, 11);

int previous = 0;       // the previous reading from the analog input

void setup()
{
    // set the speed of the motor to # RPMs
    stepperX.setSpeed(60);
    stepperY.setSpeed(60);

    // Initialize the Serial port:
    Serial.begin(9600);
}

void loop()
{
    int sensorPosition = analogRead(0);

    // Step forward # steps:
    Serial.println("Moving Forward...");
    stepperX.step(500);
    stepperY.step(500);
    delay(1000);

    // Step backward # steps:
    Serial.println("Moving Backward...");
    stepperX.step(-100);
    stepperY.step(-100);
    delay(1000);
}
</code>```

As mentioned in @Chuck's post, the Arduino doesn't directly control stepper motors.  What happens is that you plug in a stepper driver to the Arduino, and that driver is wired to & controls the motor.  You'll see that my pins are 2, 3, 4, and 5 for one driver & motor, and 8, 9, 10, and 11 for the other driver & motor.

```<code>Stepper stepperX(STEPS, 2, 3, 4, 5);    // Arduino pins attached to the stepper driver
Stepper stepperY(STEPS, 8, 9, 10, 11);
</code>```

This is because I'm using 4-phase unipolar motors.  Your choice of stepper motors will determine your own setup.

Here's a great writeup on the topic of motor selection:

AdaFruit stepper motor knowledge

Good luck!

Ryan

Title: Software to control an Arduino setup with a timing belt and stepper motors

Body:
I would like to know what software is available to control a timing belt with stepper motors for arduino board.Much like how its done in 3d printing.
But in this case i wont be making a 3d printer.Just one simple setup.</p>

Body:
Here you go.  This will control 2 steppers on an Arduino.

#include <Stepper.h>

#define STEPS 200       // change this to the number of steps on your motor

// create an instance of the stepper class, specifying
// the number of steps of the motor and the pins it's attached to
Stepper stepperX(STEPS, 2, 3, 4, 5);    // Arduino pins attached to the stepper driver
Stepper stepperY(STEPS, 8, 9, 10, 11);

int previous = 0;       // the previous reading from the analog input

void setup()
{
    // set the speed of the motor to # RPMs
    stepperX.setSpeed(60);
    stepperY.setSpeed(60);

    // Initialize the Serial port:
    Serial.begin(9600);
}

void loop()
{
    int sensorPosition = analogRead(0);

    // Step forward # steps:
    Serial.println("Moving Forward...");
    stepperX.step(500);
    stepperY.step(500);
    delay(1000);

    // Step backward # steps:
    Serial.println("Moving Backward...");
    stepperX.step(-100);
    stepperY.step(-100);
    delay(1000);
}

As mentioned in @Chuck's post, the Arduino doesn't directly control stepper motors.  What happens is that you plug in a stepper driver to the Arduino, and that driver is wired to & controls the motor.  You'll see that my pins are 2, 3, 4, and 5 for one driver & motor, and 8, 9, 10, and 11 for the other driver & motor.

Stepper stepperX(STEPS, 2, 3, 4, 5);    // Arduino pins attached to the stepper driver
Stepper stepperY(STEPS, 8, 9, 10, 11);

This is because I'm using 4-phase unipolar motors.  Your choice of stepper motors will determine your own setup.

Here's a great writeup on the topic of motor selection:

AdaFruit stepper motor knowledge

Good luck!

Ryan


答:
After studying the code I cited on the EDIT of my question.

I came up with this solution which is working so far

```<code># define encoderRight 2
# define encoderLeft 3

volatile int countR = 0;
volatile int countL = 0;
volatile boolean firstChangeR = true; // expecting the 1st change of the bounce
volatile boolean firstChangeL = true; // same
boolean right_set = true;
boolean left_set = true;

void setup() {
  Serial.begin(9600);
  // Right encoder
  attachInterrupt(digitalPinToInterrupt(encoderRight), doEncoderRight, CHANGE);
  pinMode(encoderRight, INPUT);
  digitalWrite(encoderRight, HIGH);  // turn on pullup resistor

  // Left encoder
  attachInterrupt(digitalPinToInterrupt(encoderLeft), doEncoderLeft, CHANGE);
  pinMode(encoderRight, INPUT);
  digitalWrite(encoderLeft, HIGH);
}

void loop() {
  firstChangeR = true; // we reset to true to expect next change triggered
  firstChangeL = true; // by the interrupt
  // printing
  String txt = String(countL);
  txt += "__";
  txt += String(countR);
  Serial.println(txt);
}

void doEncoderRight(){
  if (firstChangeR) delay(1); // if this is the first detection then we wait
                              // for the bounce to be over

  // if the current state is different from the last saved state then:
  // a real change happened and it's not part of the bouncing but
  // actually the real beginning of the change: the first bounce !
  if (digitalRead(2) != right_set) {
    right_set = !right_set; // so we change the real state
    countR ++;  // we also increment the right encoder

    // since this was the firstChange the next are part of bouncing, so:
    firstChangeR = false;
  }

}

void doEncoderLeft(){
  if (firstChangeL) delay(1);

  if (digitalRead(3) != left_set) {
    left_set = !left_set;
    countL ++;

    firstChangeL = false;
  }

}
</code>```

Tell me what do you think about it? Do you think it's reliable and is there any improvement you can propose?

Title: Wheel encoder triggers interrupt too many times

Body:
I am building a simple robot with two driving wheel.
I want to control the wheel rotation using a wheel encoder like this one.

Here is a code I have on Arduino to try to understand the problem I'm facing:

int count = 0;
void setup() {
  Serial.begin(9600);
  attachInterrupt(digitalPinToInterrupt(2), upL, RISING);
  pinMode(2, INPUT);
}

void loop() {
  Serial.println(String(digitalRead(2)) + "__" + String(count));
}

void upL(){
  count++;
}
```

<p>What I notice is:<br>
The interrupt is triggered <strong>multiple times</strong> when the sensor beam is cut <strong>once</strong>.<br>
But when I <em>digitalRead</em> the pin, then there is only one change.  </p>

<p>I also noticed that the interrupt is also triggered when going from HIGH to LOW.</p>

<p>Here is an example of the ouput I have:</p>

```0__0
0__0
0__0
0__0
...
...
0__0
0__0
0__0   <<< change from LOW to HIGH...
1__9   <<< the interrupt must have incremented only once... 9 instead !
1__9
1__9
1__9
...
...
1__9
1__9   <<< change from HIGH to LOW. the interrupt shouldn't be triggered
0__24  <<< still... we have 15 increments
0__24
0__24
0__24
0__24
...
...
0__24
0__24   <<< same here...
1__51   <<< 26 increments
1__51
...
...
1__51  <<< same here...
0__67  <<< 16 increments
0__67
0__67
```

The only way I can explain that is that during a change of state, the signal received from the sensor is not really square, but somehow noisy.

Like in this image :

Therefore we would have, indeed, many RISING on one change....

(However reading the output of the sensor on an analog pin shows a direct variation from 880(HIGH) to 22(LOW))

Does anyone have another explanation? Or a solution to this problem ?

EDIT

Thanks to @TobiasK I know that this is called a bouncing effect. By doing further research I came across this solution:

playground.arduino.cc/Main/RotaryEncoders (Ctrl+F for rafbuff).

I'm trying it and i'll let you know.

Body:
After studying the code I cited on the EDIT of my question.

I came up with this solution which is working so far

# define encoderRight 2
# define encoderLeft 3

volatile int countR = 0;
volatile int countL = 0;
volatile boolean firstChangeR = true; // expecting the 1st change of the bounce
volatile boolean firstChangeL = true; // same
boolean right_set = true;
boolean left_set = true;

void setup() {
  Serial.begin(9600);
  // Right encoder
  attachInterrupt(digitalPinToInterrupt(encoderRight), doEncoderRight, CHANGE);
  pinMode(encoderRight, INPUT);
  digitalWrite(encoderRight, HIGH);  // turn on pullup resistor

  // Left encoder
  attachInterrupt(digitalPinToInterrupt(encoderLeft), doEncoderLeft, CHANGE);
  pinMode(encoderRight, INPUT);
  digitalWrite(encoderLeft, HIGH);
}

void loop() {
  firstChangeR = true; // we reset to true to expect next change triggered
  firstChangeL = true; // by the interrupt
  // printing
  String txt = String(countL);
  txt += "__";
  txt += String(countR);
  Serial.println(txt);
}

void doEncoderRight(){
  if (firstChangeR) delay(1); // if this is the first detection then we wait
                              // for the bounce to be over

  // if the current state is different from the last saved state then:
  // a real change happened and it's not part of the bouncing but
  // actually the real beginning of the change: the first bounce !
  if (digitalRead(2) != right_set) {
    right_set = !right_set; // so we change the real state
    countR ++;  // we also increment the right encoder

    // since this was the firstChange the next are part of bouncing, so:
    firstChangeR = false;
  }

}

void doEncoderLeft(){
  if (firstChangeL) delay(1);

  if (digitalRead(3) != left_set) {
    left_set = !left_set;
    countL ++;

    firstChangeL = false;
  }

}

Tell me what do you think about it? Do you think it's reliable and is there any improvement you can propose?


问:
What to do when position control with trajectories is interrupted?
What are strategies used when trajectories, which are applied to a robotic joint, are interrupted? Say a robotic arm hits an obstacle, the controller just keeps applying the trajectory. Then at the end, the error gets so large, the torque can get quite strong and damage the robot or snap.</p>

答:
If you have no possibility to detect the obstacle apriori (e.g. with cameras, vicinity sensors...) If you already hit an obstacle and your position error increases, you can only detect the problem by the increasing position error or indirect by the increasing motor current.

Most of the motion controllers, I have seen, have tons of configuration parameters, including an error limit. This limit parameter tell the controller to disable the motor in case of this threshold is exceeded. This is done to protect the robot-hardware itself and also the environment (e.g. a wall, a crate or even an human being).

In the procedure of setting up the robot this parameter is usually tuned, to avoid damage in case of an error but also to avoid false-positive alarms in case of a to small threshold.

As it should be clear my answer is based on a system with a closed loop setup


答: In 1981 Raibert and Craig wrote a paper Hybrid Position/Force Control of Manipulators  which was published in the June 1981 issue of Journal of Dynamic Systems, Measurement, and Control.   It was republished in Brady's book Robot Motion: Planning and Control. You can find many similar concepts today, some being called "position and torque control," and others using velocity and/or force variables for the combination.   If you are able to sense joint torques (or motor currents), this would be a great approach.   If you cannot sense the joint torques, you can use an end effector force/torque sensor.  This would work for the scenario in which the robot cannot finish its positioning task when the object being gripped (or the gripper itself) hits an obstacle.   It would not, however, detect when an individual arm link encounters an obstacle.

Title: What to do when position control with trajectories is interrupted?

Body: What are strategies used when trajectories, which are applied to a robotic joint, are interrupted? Say a robotic arm hits an obstacle, the controller just keeps applying the trajectory. Then at the end, the error gets so large, the torque can get quite strong and damage the robot or snap.</p>


问:
Do these motors really have enough torque to lift 130 pounds
I was looking at these dc motors and I converted the torque to force at a meter distance and I got that two of them should be able to lift over 130 pounds, is this right? http://www.active-robots.com/high-torque-dc-servo-motor-10rpm-with-step-dir-drive There has to be some catch i am not seeing</p>

答:
Where did you get that number?

The first page of the datasheet posted on the page you linked states, "Max 15VDC and 7A" (near the top).

So the maximum input power for the motor is $P=IV$, or $P=(7)(15)=105\mbox{W}$.

Mechanical rotating power can be calculated by $P=\tau \omega $, where $\tau$ is torque in Nm and $\omega $ is rotational speed in rad/s.

The motor's top speed is 10 rpm. Multiply by 2$\pi$ to get to rad/m, then divide by 60 to convert to seconds and you get $\omega=10*(6.28/60) = 1.047 \mbox{rad/s} $.

Now, divide input power by rotational speed to get the theoretical maximum torque: $\tau = 105/1.047 = 100\mbox{Nm} $.

Note that this is Newton meters; a Newton is about a quarter of a pound, so the maximum force is about 25 pounds.

However, an electric motor is generally only about 80% efficient, so I wouldn't expect more than 20 pounds at 1m, max.


问:
using the brush assembly and Aerovac bin space?
A lot of the Create 2's interior space is taken up by the brush assembly and the Aerovac bin.  I'd like to take these out and put in my own stuff, but I'm concerned that the Roomba might get confused by the fact that I've unplugged these items.  Is there anything special I need to do, aside from adding an appropriate amount of weight in that area?</p>

答:
No need to fear.  The Roomba will run just fine without the brush deck, vacuum bin, and side brush motor.

Be sure to check out http://www.irobot.com/create where there are 3D files for an extended bottom half of the vacuum bin to hold your stuff.  There are even mounting posts for Arduino, RaspberryPi, and RaspberryPi2.


问:
Quadrotor - Control system, where to begin?
I am starting to assemble a quadrotor from scratch.

Currently, I have this:

Structure;

an IMU (accelerometer, gyro, compass);

4 ESCs and DC motors;

4 propellers;

Raspberry Pi to control the system, and;

LiPo battery.

I have calibrated the ESCs and the four motors are already working and ready.
But now I am stuck.

I guess the next step is to dive deeply in the control system, but I am not sure where to begin. I read some articles about the control using PIDs, but I don't know how many should I use, or whether I need to model the quadrotor first to compute kinematic and dynamic of the quadrotor inside the RPi.
Sorry if the question is too basic!

More details

The structure is from a kit. Well, all I have now is the ESCs calibrated, although I do not have documentation of them to adjust the cut off voltage for the LiPo battery. I have been made tests with some Python code I found to have PWM outputs for the motors and to control I2C bus to communicate with IMU.

One of my problems now is that I need RPIO library for PWM and the quick2wire-python-api to work with the I2C libraries from the MIT to control my IMU but as far as I know RPIO works with Python2 and quick2wire works with Python3 so I don't know how to manage this.

So actually, I have no code yet to control the four motors in parallel, only have testing code to test them separately and also with the IMU.

About the IMU, I am still learning how to work with it and how to use the MIT library. The unit includes those sensors:

ADXL345

HMC5883L-FDS

ITG3205

You can see a picture of the quadrotor below,

So as I said before, I would like to know how to handle the control system and how it is implemented inside the Raspberry Pi, and then start to work with the Python code to assemble the motors, the IMU and the control.

答:
Helping with inspiration

Hello,

I suggest you to evaluate separately the altitude and attitude dynamics. Then you can start thinking in the control you need to achieve your own objectives.

For example:

The altitude of a Quadrotor is how high it can get. For this, we need to have a mathematical model:

For this approach, you should read about euler angles & rotation matrices.

From Newton's equation:

$\ddot{z} = \frac{1}{M}{\cos(\phi)\cos(\theta)\sum_{n=1}^{4} \omega_n^{2}} - g$

Where the expression can be derived from the relationship of the two frames represented in the figure above! Please, observe that the angles are essential to control the altitude, but we can do (and should do!) some approximations to simplify our model. Suppose $\phi$ and $\theta$ very small

$\ddot{z} = \frac{1}{M}{\sum_{n=1}^{4} \omega_n^{2}} - g$

(...)

For more, I recommend this book here

Hope I have inspired you to start your cool project. Good luck!


问:
High voltage motor control with arduino
I'm trying to control a higher voltage motor than an arduino can source with a pin, with an arduino. I am trying to hook it up to a transistor. The battery pack is not supposed to be 4.8V, it's 6V, 4 D batteries.

Here is the setup:

Here is the arduino code I'm trying to run to it:

```<code>int motorpin = 2;

void setup()
{
    pinMode(motorpin, OUTPUT);
}

void loop()
{
    digitalWrite(motorpin, HIGH);
    delay(500);
    digitalWrite(motorpin, LOW);
    delay(500);
}
</code>```

Code gives me no errors, but no motor movement happens. What would make this work? Thanks.

答:
Your hardware configuration sounds wrong... your trying to provide all the drive current from the arduino

Given your description, your using a bipolar (NPN or PNP) transistor, I'd wire up as:

Base: To arduino

Collector: Motor -ve  (motor +ve to Vcc)

Emmitter: Ground

Alternatively:

Base: To arduino

Collector: Vcc

Emmitter: Motor +ve  (motor -ve to Ground)

This way, the processor is switching the current flow between Vcc and the motor, not supplying all the power.

Title: High voltage motor control with arduino


问:
iRobot Create 2 stuck in Clean mode?
I'm using the Delphi example to command my Create 2, I just adapted the demo code to Unicode (DelphiXE). I use the original iRobot USB to serial cable.

My Create 2 seemed to be responding fine to all the commands send via serial yesterday and correctly received all sensor data back this morning, until I recharged the battery. Now when I send "7" "Soft reset" the robot attempts every time to start a clean cycle. It also attempts to start the clean cycle when I press the clean button. It tells me to move the Roomba to a new location, which is normal in cleaning mode because my wheels are not touching my desk. Communication via serial seem to be fine because I still get the Soft Reset response texts in the log memo of my app when I use the 2 buttons method to soft reset my Create 2, so there is still communication both ways.

I must say I had the same yesterday after charging but after a while unexpectedly, don't know why, the robot responded again fine to my commands.

It really seems to me the Create 2 is stuck in the Cleaning mode, or am I missing something?

BTW, I also tried to fix the problem by removing the battery.

答:
I think I found the solution myself. When I send a "128 255" opcode to my Create it seems the robot is switched out of Clean mode, and back in to programming mode.

There is no mention anywhere in the specs of opcode "255", nor is there any mention of switching off Clean mode in the specs either.

Title: iRobot Create 2 stuck in Clean mode?


问:
How do I get the Create 2 to communicate with a laptop via the serial to USB cable?
My computer will not recognize the Serial to USB cable.  I have tried it on a Mac and an HP.

Is there a driver that I need to install?  If it is supposed to install automatically, it is not.

答:
If it's a FTDI device, download the latest driver from http://www.ftdichip.com/Drivers/VCP.htm

I assume its the original USB to Serial cable from iRobot you're using.


问:
How to estimate the position of multiple static ground targets captured from a down facing camera?
An aerial vehicle captures images of the ground using its down facing camera. From the images, multiple targets are converted from their pixel position to the camera reference frame using the pinhole camera model. Since the targets are static and there is information of the vehicle attitude and orientation, each sample is then converted to the world referencial frame. Note that all targets are on a flat, level plane.

The vehicle keeps "scanning" for the targets and converting them to the world referencial frame. Due to the quality of the camera and detection algorithm, as well as errors on the altitude information, the position of the "scanned" targets is not constant (not accurate). A good representation might be a gaussian distribution around the target true position, however it will also be influenced by the movement of the aerial vehicle.

What's the best approach to estimate the position of the targets from multiple readings?
This basically resumes to a problem of noise removal (as well as outlier removal) and estimation, so I would like to know what algorithms and strategies could solve the problem. In the end I expect to implement and test a collection of different approachs to understand their performance on this specific problem.

Furthermore, this system is implemented using ROS, so if you know of packages that already do what I'm searching for I would be glad to hear. You can also cite papers on the topic that you think might be of my interest.

答:
I see that you updated your question some, but maybe it's your scenario that's not clear. If your targets are all of about the same size, and all on a flat, level plane, then I can see that you could get their height by the the height of the aerial vehicle. I thought it was randomly distributed targets on an unknown surface, in which case the height of the aerial vehicle doesn't really buy you anything; you would need some other information to determine the 3D coordinates of the target. As it stands, knowing that the targets are all on a level plane is the extra information.

So, if you describe your noise as Gaussian, and the measurements are taken from a linear system (moving platform), then that's pretty much the definition of when to use a Kalman filter; "the [Kalman] filter yields the exact conditional probability estimate in the special case that all errors are Gaussian-distributed."

You'll need to know the covariance of the errors, but that shouldn't be too much of an issue as it sounds like you have already taken some measurements, enough to know that they are noisy and Gaussian in distribution.


问:
How to select cameras for a stereo vision system?
I am in the process of building a stereo vision system to be used on a UGV. The system is for a robot that will be used in a competition wherein the robot is teleoperated to find relatively small colored rocks in a large outdoor field. I understand how to calibrate such a system and process the data for a stereo vision system. I do not however know how to select cameras for such a system. What are the best practices for picking cameras for a stereo vision system?</p>

答:
You should start by calculating how many frames per second you need, and how much camera resolution you can process at that framerate.  If nothing else, that will prevent you from overspending or from buying a camera that won't suit your needs.

Beyond that, there are a variety of features that make the choice more difficult/interesting.  Different cameras (especially network cameras like Axis) allow you to alter the image quality, or to specify a max bitrate for the image stream.  Some cameras also give you choices on the shutter speed, allowing you to favor a constant exposure time or a constant average illumination in the image.  Some cameras are more sensitive than others (the last time I worked with this was in 2009, and we noticed that the PS3 Eye did really well in low light conditions).

Probably the best thing you can do would be to run your image processing algorithms on a few static images that you take with a DSLR, then attempt to reduce the frame size and quality to see where things begin to break down.


问:
Quadcopter program execution time optimization using Raspberry Pi by increasing i2c baudrate

Is it possible to speed up execution time of a c++ program in raspberry pi solely by increasing the i2c baudrate and increasing the sampling frequency of the sensors?

I have the issue of sudden jerkiness of my quadcopter and found the culprit which is the frequency at which my loop excecutes which is only about 14Hz. The minimum requirement for a quadcopter is 100-200hz. It is similar to the issue he faces here Raspberry Pi quadcopter thrashes at high speeds

He said that he was able to increase his sampling rate from 66hz to 200hz by increasing the i2c baudrate. I am confused on how that is done.

In the wiring pi library, it says that we can set the baudrate using this command:

```<code>gpio load i2c 1000

will set the baud rate to 1000Kbps – ie. 1,000,000 bps. (K here is times 1000)
</code>```

What I am curious about is how to set this baudrate to achieve my desired sampling rate?

I plan on optimizing it further to achieve at least a 100Hz sampling rate

As of now, the execution time of each loop in my quadcopter program is at 0.07ms or 14Hz.

It takes 0.01ms to 0.02ms to obtain data from the complementary filter.

I have already adjusted the registers of my sensors to output readings at 190Hz (Gyroscope L3GD20H) and 200Hz (Accelerometer LSM303) and 220Hz (Magnetometer LSM303).

答: It turns out, the default i2c baudrate becomes a bottle neck in reading the measurement data from the IMU's so increasing it from 100kbps to 400kbps was able to boost my execution frequency from 150Hz (After optimizing) to ~ 210Hz (0.00419s)</p>


问:
In the SLAM for Dummies, why are there extra variables in the Jacobian Matricies?
I am reading SLAM for Dummies, which you can find on Google, or at this link: SLAM for Dummies - A Tutorial Approach to Simultaneous Localization and Mapping.

They do some differentiation of matrices on page 33 and I am getting different answers for the resulting Jacobian matrices.

The paper derived

$$
\left[ {\begin{array}{c}
\sqrt{(\lambda_x - x)^2 + (\lambda_y - y)^2} + v_r \\ \tan^{-1}\left(\frac{\lambda_y - y}{\lambda_y - x}\right) - \theta + v_\theta \end{array}} \right]
$$

and got
$$
\left[ {\begin{array}{ccc}
\frac{x - \lambda_y}{r},& \frac{y - \lambda_y}{r},& 0\\ \frac{\lambda_y - y}{r^2},& \frac{\lambda_y - x}{r^2},& -1 \end{array}} \right]
$$

I don't get where the $r$ came from. I got completely different answers. Does anybody know what the $r$ stands for? If not, is there a different way to represent the Jacobian of this matrix?

答: Without checking, I assume $r$ is introduced as the range $\sqrt{(\lambda_x - x)^2 + (\lambda_y - y)^2}$. This is often useful when deriving such expressions, since the range often occurs in the derivative in first or higher order.</p>


答:
From the paper:

$\begin{bmatrix} range\\bearing \end{bmatrix} = \begin{bmatrix} \sqrt{(\lambda_x-x)^2 + (\lambda_y - y)^2 } + v_r \\ tan^{-1}(\frac{\lambda_y-y}{\lambda_x-x}) - \theta + v_{\theta} \end{bmatrix}$

The Jacobian is:

$H = \begin{bmatrix}\frac{\partial range}{\partial x} & \frac{\partial range}{\partial y} & \frac{\partial range}{\partial \theta}\\\frac{\partial bearing}{\partial x} & \frac{\partial bearing}{\partial y} & \frac{\partial bearing}{\partial \theta}\end{bmatrix} = \begin{bmatrix}\frac{-2(\lambda_x-x)}{2\sqrt{(\lambda_x-x)^2 + (\lambda_y - y)^2 }} & \frac{-2(\lambda_y-y)}{2\sqrt{(\lambda_x-x)^2 + (\lambda_y - y)^2 }}&0\\
\frac{1}{1+(\frac{\lambda_y-y}{\lambda_x-x})^2}\cdot\frac{\lambda_y-y}{(\lambda_x-x)^2}&\frac{1}{1+(\frac{\lambda_y-y}{\lambda_x-x})^2}\cdot\frac{-1}{(\lambda_x-x)}&-1\end{bmatrix}$

The expression $\sqrt{(\lambda_x-x)^2 + (\lambda_y - y)^2 }$ is just the ideal range (distance from the robot to the landmark), and so is called $r$. Then the previous expression simplifies to:

$H = \begin{bmatrix}\frac{x-\lambda_x}{\sqrt{(\lambda_x-x)^2 + (\lambda_y - y)^2 }} & \frac{y-\lambda_y}{\sqrt{(\lambda_x-x)^2 + (\lambda_y - y)^2 }}&0\\
\frac{\lambda_y-y}{(\lambda_x-x)^2+(\lambda_y-y)^2}&\frac{x-\lambda_x}{(\lambda_x-x)^2+(\lambda_y-y)^2}&-1\end{bmatrix} = \begin{bmatrix}\frac{x-\lambda_x}{r} & \frac{y-\lambda_y}{r}&0\\
\frac{\lambda_y-y}{r^2}&\frac{x-\lambda_x}{r^2}&-1\end{bmatrix}$

As you can see there is a sign error in the paper (second row, second column). You can check it with any differentiation tool (you can find a bunch of them online).

Title: In the SLAM for Dummies, why are there extra variables in the Jacobian Matricies?

Body:
I am reading SLAM for Dummies, which you can find on Google, or at this link: SLAM for Dummies - A Tutorial Approach to Simultaneous Localization and Mapping.

They do some differentiation of matrices on page 33 and I am getting different answers for the resulting Jacobian matrices.

The paper derived

$$
\left[ {\begin{array}{c}
\sqrt{(\lambda_x - x)^2 + (\lambda_y - y)^2} + v_r \\ \tan^{-1}\left(\frac{\lambda_y - y}{\lambda_y - x}\right) - \theta + v_\theta \end{array}} \right]
$$

and got
$$
\left[ {\begin{array}{ccc}
\frac{x - \lambda_y}{r},& \frac{y - \lambda_y}{r},& 0\\ \frac{\lambda_y - y}{r^2},& \frac{\lambda_y - x}{r^2},& -1 \end{array}} \right]
$$

I don't get where the $r$ came from. I got completely different answers. Does anybody know what the $r$ stands for? If not, is there a different way to represent the Jacobian of this matrix?


问:
Building a quadcopter, what motors, props and what are the calculations?
Before I start, I am a 13 year old, I would like to apologise because I am a beginner to all this and I wouldn't really understand any professional talk, I am only doing this for a hobby.

I am building a quadcopter,

Flight controller: KK 1.2.5

ESC: Q Brain 25amp

Frame: KK 260 / FPV 260

Frame Addon: KK/FPV 250 Long Frame Upgrade Kit

Tx & Rx: HobbyKing 6ch tx rx (Mode 2)

Battery: Turnigy Nano-Tech 2200 mAh 3S

I am not sure about what motor and propellers I should use.
All I know is: for the frame the motor mounts are: 16mm to 19mm with M3 screws
I am not sure what 1806 and 2208 means.

Here are my questions:

What calculations should I do to find out how much thrust the quad needs to produce / any other useful calculations

Using the calculations what would be the best and CHEAPEST motors I could have

And finally, what propeller would be best suited for the motor.

p.s: I am looking for a durable and really cheap motors also, I live in London, so shipping might be a problem if there is an immense bill.

Thanks a lot for your time,
Sid

答:
You've got most of the needed parts right however, since you are a begginner I would recommend you just buy ARF (Almost Ready to fly kits from HobbyKing or Amazon). An example is the DJI F450 kit with e300/e305 tuned propulsion system. The motors and propellers are already tuned so you do not need to balance them and the motors that come with either of these kits are designed to work seamlessly with the ESC's they come with. These also work with your KK flight controller. The recommended takeoff weight for this kit is 1.2 kgs which is enough for most applications. I've also seen people doing their thesis projects using this kit including me.

If you want to pursue building you own (Which I think you shouldn't do), you should first take into consideration the weight you are expecting.

Get the weight of your frame and all the peripherals you would mount such as the flight controller, the camera and the frame that you are going to use. Take into consideration the maximum weight capacity of your frame while doing this. You should also leave allowances for the addition of motors, propellers and esc's and the battery.

Once you've got the weight, its time to select motors. You should select motors based on their recommended take off weight (you can check this on their specs). If your the weight you've calculated plus the weight of the motors you've selected fits into the recommended take off weight then your good. Take note I said recommended take off weight, not maximum weight/thrust.

I'm not that familiar when it comes to selecting propellers though. You can check this link instead. He gives a better explaination: http://blog.oscarliang.net/how-to-choose-motor-and-propeller-for-quadcopter/

When selecting batteries you can use this calculator to check your estimated flight time. http://multicopter.forestblue.nl/lipo_need_calculator.html


问:
How to implement transmission in tracked chassis with one motor?
I see that in small robots tracked chassis is implemented with 2 motors, each powering one side of the vehicle, like this:

(image stolen from here)

But in real scale tanks I assume there is only one motor so there must be some way of applying power to both sides independently.

答:
Yes there is and the principle is same as wheeled vehicles. There is a part in vehicle chassis called differential which transmits the raw power from the engine to the axle. Tanks and other tracked vehicles use a similar system to utilize a single engine output for both sides.

Although going forward is pretty simple, there is a major problem that still persists. Since tanks don't have additional mechanism for turning, they use 2 different methods which acts as a solution.

The first one is a basic solution, there are 2 different brake clutches for individual tracks. When one of them is used, the corresponding track slows down and the vehicle turns in that direction by dragging the tracks.

The second one is called neutral steering and not that common. It was used in tanks such as German Panther. This system has a hybrid gearbox to set different speeds in different directions for each track. Vehicles with this mechanism can do a stationary 360 degree turn. Neutral steering looks pretty but trying to implement it into a robotic infrastructure requires a high knowledge in mechanical engineering as this kind of steering system is a very complex one.

More information about tracked vehicle steering can be found here: Tracked Vehicle Steering


问:
Quadcopter PID Controller: Derivative on Measurement / Removing the Derivative Kick
Good day,

I am currently implementing a single loop PID controller using angle setpoints as inputs. I was trying out a different approach for the D part of the PID controller.

What bought this about is that when I was able to reach a 200Hz (0.00419ms) loop rate, when adding a D gain, the quadcopter seems to dampen the movements in a non continous manner. This was not the case when my algorithm was running at around 10Hz. At an angle set point of 0 degrees, I would try to push it to one side by 5 degrees then the quad would try to stay rock solid by resisting the movements but lets go after while enabling me to get it of by 2 degrees (the dampening effect weakens over time) then tries to dampen the motion again.

This is my implementation of the traditional PID:

Derivative on Error:

```<code>//Calculate Orientation Error (current - target)
float pitchError = pitchAngleCF - pitchTarget;
pitchErrorSum += (pitchError*deltaTime2);
float pitchErrorDiff = pitchError - pitchPrevError;
pitchPrevError = pitchError;

float rollError = rollAngleCF - rollTarget;
rollErrorSum += (rollError*deltaTime2);
float rollErrorDiff = rollError - rollPrevError;
rollPrevError = rollError;

float yawError = yawAngleCF - yawTarget;
yawErrorSum += (yawError*deltaTime2);
float yawErrorDiff = yawError - yawPrevError;
yawPrevError = yawError;

//PID controller list
float pitchPID = pitchKp*pitchError + pitchKi*pitchErrorSum + pitchKd*pitchErrorDiff/deltaTime2;
float rollPID = rollKp*rollError + rollKi*rollErrorSum + rollKd*rollErrorDiff/deltaTime2;
float yawPID = yawKp*yawError + yawKi*yawErrorSum + yawKd*yawErrorDiff/deltaTime2;

//Motor Control - Mixing
//Motor Front Left (1)
float motorPwm1 =  -pitchPID + rollPID - yawPID + baseThrottle + baseCompensation;
</code>```

What I tried to do now is to implement a derivative on measurement method from this article to remove derivative output spikes. However the Derivative part seems to increase the corrective force than dampen it.

Derivative on Measurement:

```<code>//Calculate Orientation Error (current - target)
float pitchError = pitchAngleCF - pitchTarget;
pitchErrorSum += (pitchError*deltaTime2);
float pitchErrorDiff = pitchAngleCF - pitchPrevAngleCF; // <----
pitchPrevAngleCF = pitchAngleCF;

float rollError = rollAngleCF - rollTarget;
rollErrorSum += (rollError*deltaTime2);
float rollErrorDiff = rollAngleCF - rollPrevAngleCF; // <----
rollPrevAngleCF = rollAngleCF;

float yawError = yawAngleCF - yawTarget;
yawErrorSum += (yawError*deltaTime2);
float yawErrorDiff = yawAngleCF - yawPrevAngleCF; // <----
yawPrevAngleCF = yawAngleCF;

//PID controller list // <---- The D terms are now negative
float pitchPID = pitchKp*pitchError + pitchKi*pitchErrorSum - pitchKd*pitchErrorDiff/deltaTime2;
float rollPID = rollKp*rollError + rollKi*rollErrorSum - rollKd*rollErrorDiff/deltaTime2;
float yawPID = yawKp*yawError + yawKi*yawErrorSum - yawKd*yawErrorDiff/deltaTime2;

//Motor Control - Mixing
//Motor Front Left (1)
float motorPwm1 =  -pitchPID + rollPID - yawPID + baseThrottle + baseCompensation;
</code>```

My question now is:

Is there something wrong with my implementation of the second method?

Source: http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/

The way I've obtained the change in time or DT is by taking the timestamp from the start of the loop then taking the next time stamp at the end of the loop. Their difference is obtained to obtain the DT. getTickCount() is an OpenCV function.

```<code>/* Initialize I2c */
/* Open Files for data logging */
while(1){
    deltaTimeInit=(float)getTickCount();

    /* Get IMU data */
    /* Filter using Complementary Filter */
    /* Compute Errors for PID */
    /* Update PWM's */

    //Terminate Program after 40 seconds
    if((((float)getTickCount()-startTime)/(((float)getTickFrequency())))>20){
            float stopTime=((float)getTickCount()-startTime)/((float)getTickFrequency());
    gpioPWM(24,0);  //1
    gpioPWM(17,0);  //2
    gpioPWM(22,0);  //3
    gpioPWM(18,0);  //4
    gpioTerminate();
        int i=0;
    for (i=0 ; i < arrPitchCF.size(); i++){
        file8 << arrPitchCF.at(i) << endl;
    }

    for (i=0 ; i < arrYawCF.size(); i++){
        file9 << arrYawCF.at(i) << endl;
    }

    for (i=0 ; i < arrRollCF.size(); i++){
        file10 << arrRollCF.at(i) << endl;
    }

    for (i=0 ; i < arrPitchAccel.size(); i++){
        file2 << arrPitchAccel.at(i) << endl;
    }

    for (i=0 ; i < arrYawAccel.size(); i++){
        file3 << arrYawAccel.at(i) << endl;
    }

    for (i=0 ; i < arrRollAccel.size(); i++){
        file4 << arrRollAccel.at(i) << endl;
    }

    for (i=0 ; i < arrPitchGyro.size(); i++){
        file5 << arrPitchGyro.at(i) << endl;
    }

    for (i=0 ; i < arrYawGyro.size(); i++){
        file6 << arrYawGyro.at(i) << endl;
    }

    for (i=0 ; i < arrRollGyro.size(); i++){
        file7 << arrRollGyro.at(i) << endl;
    }

    for (i=0 ; i < arrPWM1.size(); i++){
        file11 << arrPWM1.at(i) << endl;
    }

    for (i=0 ; i < arrPWM2.size(); i++){
        file12 << arrPWM2.at(i) << endl;
    }

    for (i=0 ; i < arrPWM3.size(); i++){
        file13 << arrPWM3.at(i) << endl;
    }

    for (i=0 ; i < arrPWM4.size(); i++){
        file14 << arrPWM4.at(i) << endl;
    }

    for (i=0 ; i < arrPerr.size(); i++){
        file15 << arrPerr.at(i) << endl;
    }

    for (i=0 ; i < arrDerr.size(); i++){
        file16 << arrDerr.at(i) << endl;
    }

        file2.close();
        file3.close();
        file4.close();
        file5.close();
        file6.close();
        file7.close();
        file8.close();
        file9.close();
        file10.close();
        file11.close();
        file12.close();
        file13.close();
        file14.close();
        file15.close();
        file16.close();
        cout << " Time Elapsed = " << stopTime << endl;
        break;
    }

    while((((float)getTickCount()-deltaTimeInit)/(((float)getTickFrequency())))<=0.00419){ //0.00209715|0.00419
       cout << " DT end = " << deltaTime2 << endl;
       deltaTime2=((float)getTickCount()-deltaTimeInit)/(((float)getTickFrequency()));
    }

    cout << " DT end = " << deltaTime2 << endl;
}
</code>```

Here's my data:

答:
All PID controllers act on an error signal, so you definitely should stick with your first implementation. The derivative term in the PID controller wants to "see" how the error is changing - if the error is getting bigger, then the derivative term gets larger and "kicks in" a higher contribution. If the error is getting smaller, then the derivative term backs off.

Your problem description is vague; something about "pushing" the quadcopter and damping and feeling something.

Engineers don't have feelings, they have data.

For the moment, with no data, all I can tell you is to reuse your first implementation and double check how you are acquiring your deltaTime2 information. Typically, in industrial controls, the derivative term is left off of the controller and only a PI controller is implemented. This is because any bit of temporal sampling problems (not taking the sample on time or mis-judging the sampling rate) can cause huge problems with the derivative term.

Imagine your sampling rate is supposed to be 0.005 seconds (200Hz). If instead of that sampling rate, you mis-judged the sample rate to be 0.006 seconds for one sample, then the following happens:

No change to proportional error because it doesn't interact with the sampling rate.

Integral error multiplies proportional error by this sampling rate and then adds it to all of the previous integral error samples, so overall it's a pretty small impact.

Derivative error divides proportional error by this sampling rate with no other means of context.

So, conceivably, you could have something like:

```<code>prevIntError = 1.8;
prevDerError = 1.1
PropError = 1;
IntError = prevIntError + 1*0.006;
derError = (1 - 1.1)/0.006;
</code>```

So where the integral error is now 1.806 instead of 1.805 (an error of 0.05%), your derivative error is -16.6 instead of what it should be, (1 - 1.1)/0.005, which is -20. That means that error in the proportional error signal is 0, the error in the integral error signal is 0.05%, and the error in the derivative error signal is 17%!

As your sampling rate increases, your sampling period goes down. Errors in timing can begin to become significant after a point. For example, at 10Hz, you are sampling every 0.1s. If your timing accuracy is +/- 0.001s, then this means that your time sampling error is +/- 1%. If you push to 200Hz (0.005s), and your timing accuracy is still +/- 0.001s, then suddenly your time sampling error is +/- 20%!

So, in summary:

Post data, not feelings.

Revert to your first implementation.

Check how you acquire deltaTime2 and report back.

Please update your question to include the answers to 1 and 3 (don't post it as a comment to this answer please).

:EDIT:

I would move your timestamp evaluation to one location in your code, and I would put that location immediately before you use it. At the moment, you don't update the sampling time until after you've done your derivative/integral calculations. This is even more problematic given the fact that you are performing communications in the same sampling window.

Say your first sweep, you read some data, do your calculations, and that sample time is 5ms. Then, on the second sweep, something happens to the communications (no data, data error, IMU reset, etc.). The time it takes to get back to the derivative/integral calculations won't be 5ms, it'll be 1ms or 10ms or something similar. So you use the wrong sample time for that sample, but then your sample time gets updated after the fact to reflect that sweep took the 10ms or whatever, then that gets used on the following sample, which again may not be correct.

I would revise your code to the following:

```<code>/* Initialize I2c */
/* Open Files for data logging */
deltaTimeInit=(float)getTickCount(); //<--- Placeholder value for first pass
while(1){
    /* Get IMU data */
    deltaTime2=((float)getTickCount()-deltaTimeInit)/(((float)getTickFrequency())); //<--- Get the time that elapsed to NOW
    deltaTimeInit=(float)getTickCount(); //<--- Start the timer for the next loop
    /* Filter using Complementary Filter */
    /* Compute Errors for PID */ //<--- Use the actual time elapsed for the calculations
    /* Update PWM's */

  /* (more stuff) */
}
</code>```

Basically, you care about the time that has elapsed from when you get data to the next time that you get data, and you want to act on current information. Your sample time should be calculated before you use it, on the current sweep.

Body:
All PID controllers act on an error signal, so you definitely should stick with your first implementation. The derivative term in the PID controller wants to "see" how the error is changing - if the error is getting bigger, then the derivative term gets larger and "kicks in" a higher contribution. If the error is getting smaller, then the derivative term backs off.

Your problem description is vague; something about "pushing" the quadcopter and damping and feeling something.

Engineers don't have feelings, they have data.

For the moment, with no data, all I can tell you is to reuse your first implementation and double check how you are acquiring your deltaTime2 information. Typically, in industrial controls, the derivative term is left off of the controller and only a PI controller is implemented. This is because any bit of temporal sampling problems (not taking the sample on time or mis-judging the sampling rate) can cause huge problems with the derivative term.

Imagine your sampling rate is supposed to be 0.005 seconds (200Hz). If instead of that sampling rate, you mis-judged the sample rate to be 0.006 seconds for one sample, then the following happens:

No change to proportional error because it doesn't interact with the sampling rate.

Integral error multiplies proportional error by this sampling rate and then adds it to all of the previous integral error samples, so overall it's a pretty small impact.

Derivative error divides proportional error by this sampling rate with no other means of context.

So, conceivably, you could have something like:

prevIntError = 1.8;
prevDerError = 1.1
PropError = 1;
IntError = prevIntError + 1*0.006;
derError = (1 - 1.1)/0.006;

So where the integral error is now 1.806 instead of 1.805 (an error of 0.05%), your derivative error is -16.6 instead of what it should be, (1 - 1.1)/0.005, which is -20. That means that error in the proportional error signal is 0, the error in the integral error signal is 0.05%, and the error in the derivative error signal is 17%!

As your sampling rate increases, your sampling period goes down. Errors in timing can begin to become significant after a point. For example, at 10Hz, you are sampling every 0.1s. If your timing accuracy is +/- 0.001s, then this means that your time sampling error is +/- 1%. If you push to 200Hz (0.005s), and your timing accuracy is still +/- 0.001s, then suddenly your time sampling error is +/- 20%!

So, in summary:

Post data, not feelings.

Revert to your first implementation.

Check how you acquire deltaTime2 and report back.

Please update your question to include the answers to 1 and 3 (don't post it as a comment to this answer please).

:EDIT:

I would move your timestamp evaluation to one location in your code, and I would put that location immediately before you use it. At the moment, you don't update the sampling time until after you've done your derivative/integral calculations. This is even more problematic given the fact that you are performing communications in the same sampling window.

Say your first sweep, you read some data, do your calculations, and that sample time is 5ms. Then, on the second sweep, something happens to the communications (no data, data error, IMU reset, etc.). The time it takes to get back to the derivative/integral calculations won't be 5ms, it'll be 1ms or 10ms or something similar. So you use the wrong sample time for that sample, but then your sample time gets updated after the fact to reflect that sweep took the 10ms or whatever, then that gets used on the following sample, which again may not be correct.

I would revise your code to the following:

/* Initialize I2c */
/* Open Files for data logging */
deltaTimeInit=(float)getTickCount(); //<--- Placeholder value for first pass
while(1){
    /* Get IMU data */
    deltaTime2=((float)getTickCount()-deltaTimeInit)/(((float)getTickFrequency())); //<--- Get the time that elapsed to NOW
    deltaTimeInit=(float)getTickCount(); //<--- Start the timer for the next loop
    /* Filter using Complementary Filter */
    /* Compute Errors for PID */ //<--- Use the actual time elapsed for the calculations
    /* Update PWM's */

  /* (more stuff) */
}

Basically, you care about the time that has elapsed from when you get data to the next time that you get data, and you want to act on current information. Your sample time should be calculated before you use it, on the current sweep.


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

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

We would like the software platform to be able to:

Integrate new robotic hardware components easily.

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

Make efficient use of computing hardware

答: ROS is quickly becoming the new standard for both industrial and research robotics. Most research groups I know of are adopting ROS for their work and then pushing their results back into the open for everyone to use and improve. Willow Garage is developing a suite of hardware and software platforms which are all centered around ROS at their core. ROS is a good place to start your search.</p>


问:
Shape Memory Alloy wire for robot gripper arm actuation: How to vary grip pressure?
For a robotic gripper arm we are designing for factory floor use on very small components, we propose to use electrically activated Shape Memory Alloy (SMA) wire harnesses for actuation.

The device being designed is akin to Pick & Place machines used for circuit assembly, but moves over an aircraft-hanger sized work surface on wheels. It manipulates irregular shaped and porous objects between 0.5 cu.cm and 8 cu.cm each - hence the traditional vacuum P&P mechanism does not appeal. Also, individual objects in the assembly line have varying hardness and weights.

Our design constraints are:

Ensuring minimal to zero vibration and sound

Using minimal volume within the mechanism (batteries are at the wheelbase, providing stability, so their weight is not a concern)

Fine variation of gripper pressure

We believe SMA meets the first two constraints well, but need some guidance on achieving constraint 3, i.e. different levels of pressure of the gripper controlled electronically.

My questions:

Can PWM of a current above the activation threshold (320 mA for 0.005 inch Flexinol HT) provide variable, repeatable actuation force?

Would we need pressure sensors on each fingertip and a closed loop control for grip, or can the gripper be calibrated periodically and maintain repeatable force?

Is there any well-documented precedent or study we should be referring to?

答:
Why you shouldn't use SMA

Firstly, I have to wonder why you have chosen to use shape memory alloys in a robotic application. If you look at any of the application lists for SMAs, you'll almost never see a robotic application on the list.

Most SMA applications are non-actuated, and include things like glasses frames and golf clubs.

Some applications do use the SMA as an actuator, but usually only once or twice. These are applications like medical stents, which need to be inserted into an artery small, but open up once inside.

The reason there are no robotic applications where the SMA needs to act as an actuator and exert a force cause something to move, is that it's subject to fatigue. According to the Wikipedia:

SMA is subject to fatigue; a failure mode by which cyclic loading results in the initiation and propagation of a crack that eventually results in catastrophic loss of function by fracture.
  in addition to this failure mode, which is not exclusively observed in smart materials. SMA are also subject to Functional Fatigue, whereby the SMA does not fail structurally, but, due to a combination of applied stress, and/or temperature, loses (to some degree) its ability to undergo a reversible phase transformation.

But if you insist

Because SMA undergoes creep and fatigue, you will have to have some kind of force transducer and control system to make sure you're applying a known force.

What I would suggest
Rather than SMAs, there are many small actuators which can satisfy your constraints without the huge drawbacks of SMA.

Voice Coils: These simply consist of a permanent magnet and a coil. Adjusting the flow of current directly affects the force applied to the magnet. Ungeared, these are totally silent, and more power efficient than SMAs. The force applied is quite repeatable, as long as the ambient temperature doesn't vary enormously. You can buy these as ready made components from Moticont. Or open up a hard drive, look there's a ready made robotic finger!

Piezo actuators: There's a range of different motors based on piezo ceramics. These are usually very tiny, but expensive motors. Try the Squiggle motors from Newscale Tech.

There's a company called Flexsys who make actuators using both technologies.