问:
Help !!Homogeneous transformation matrix
I came across many good books on robotics. In particular I am interested in Inverse kinematic of 6dof robot. All books have example which goes on like this "given homogeneous transformation matrix as below, find the angles ?".. Problem, is how do I find components of a homogeneous transformation. matrix in real world? i.e. how do i practically derive 9 components of the rotation matrix embeded in homogeneous transformation matrix?.....</p>

答:
The upper left 3x3 submatrix represents the rotation of the end effector coordinate frame relative to the base frame.   In this submatrix, the first column maps the final frame's x axis to the base frame's x axis; similarly for y and z from the next two columns.

The first three elements of the right column of the homogeneous transform matrix represent the position vector from the base frame origin to the origin of the last frame.

EDIT BASED ON COMMENT:

There are several ways to define the nine components of the rotation submatrix, $R$, given a particular task in space.  Note that $R$ is orthonormal, so you don't really need to define all 9 based on just the task.  A very common approach is to represent the task orientations (with respect to the global coordinate system) using Euler angles.  With this representation, each column of $R$ describes a rotation about one of the axes.  Be careful with Euler angles, though, because the order of rotation matters.  Commonly, but not exclusively, the first column of $R$ describes a rotation about the global $z$ axis; the second column describes a rotation about the now-rotated $y$ axis; and the third column describes a rotation about the $x$ axis, which has been rotated by the two previous angles.  There are other Euler angle representations, also.

There are other ways to use $R$ to describe the task orientation.  I find Waldron's text very readable for this.  Check out section 1.2.2 of his draft Handbook of Robotics sourced by Georgia Tech.  In this section he describes not only Z-Y-X Euler angles, but also Fixed Angles, quaternions, and Angle-Axis representations for orientation.

The important thing is to ensure you consider whatever representation you use for $R$ when you compute the inverse kinematics.  The homogeneous transformation describes how the position and rotation vary based on joint angles, but you need to ensure that your definition for $R$ is properly inverted in computing the final three joint angles for your robot.

Blockquote


问:
What is the difference between Task-Level and Joint-Level Control Systems?
While doing a literature review of mobile robots in general and mobile hexapods in particular I came across a control system defined as "Task level open loop" and "Joint level closed loop" system.

The present prototype robot has no external sensors by
  which its body state may be estimated. Thus, in our simulations and experiments, we have used joint space closed
  loop (“proprioceptive”) but task space open loop control
  strategies.

The relevant paper is A simple and highly mobile hexapod

What is the meaning of the terms "joint-level" and "task-level" in the context of the Rhex hexapod?

答:
These are just terms to describe the "layers" of control on the robot.  The "joint level" means the position of each actuator (leg), and the "task level" means the current goal of the robot (like go forward, go east, go to location X, etc).

This paragraph is about sensing.  There are (apparently) position sensors in all of the leg joints, so the robot is capable of closed loop control at that level -- no user intervention needed.  ("Proprioceptive" is a $5 word for "I know where my legs are because I can sense them".)

However, having no external sensors means that the robot can't tell where it is in relation to the world and as a result it can't determine what action it should take to accomplish its goals.  So, it must run as open loop.  Presumably this means that they are either giving it a pre-scripted set of actions, or directing it via remote control.

So, the task-level control handles the "go forward" concept, but the joint-level control handles the uneven terrain underfoot.

ContentLicense: CC BY-SA 3.0


问:
Do robots usually have databases?
I am a junior web developer working mainly with Bash, Javascript, and Drupal. I'm more fascinated writing scripts and programs that do certain concrete actions instead of querying and manipulating databases.

I do desire to step into robotics in the future (after completions) and had the following question in my mind:

Do robots usually have databases (similar to these of websites in quality and quantity) and if so, please give a practical example what are they using for? Maybe in the context of machine vision or machine motion.

Update (13/11/19):

All the answers here are good in my opinion. If I could I would accept all of them, I suggest to start by reading the answer of user16549 which makes an introduction to DBs in robots, then continue to the answer by FooTheBar, and then read other answers.

答: Most robots I have encountered don't have databases. I think this is mostly because there is usually only one process or "user" that needs the data they store in memory. With that said robots can often store a lot of data in log files for debugging purposes.</p>

ContentLicense: CC BY-SA 4.0

Title: Do robots usually have databases?


问:
What is the intuitive explanation of using Jacobian of observation model while calculating Kalman gain in EKF SLAM?
The idea of using Kalman gain in EKF SLAM is to figure out how much we trust our motion model and sensor/observation model. As explained in The Extended Kalman Filter: An Interactive Tutorial for Non-Experts - Part 5: Computing the Gain, the Kalman gain can be calculated as,

$$K_t = \frac{p}{(p +r)}$$

where $p$ denotes prediction error and $r$ denotes sensor noise.

Now, if we look into the equation in the image,

$$K_t = \bar{\Sigma_t}H_t^T(H_t\bar{\Sigma_t}H_t^T +Q_t)^{-1}$$

we can see that Kalman gain is calculated using Covariance matrix ($\Sigma$), Jacobian of observation model ($H$) and Sensor noise ($Q$). Comparing with earlier equation, $p$ can be considered equivalent of $\Sigma$, while $r$ can be equivalent of $Q$.

How does $H$ fit in, in this equation? What would be an intuitive explanation?

答:
Let's try breaking it down.

Projection of uncertainty

$H\Sigma H^T$ is projecting the state uncertainty into measurement space. How do we know that?

$\Sigma$ denotes the the covariance of our belief in the filter's state

$h(x)$ maps some belief about state to a measurement that we would expect if that were the true state

$H(x)$ is a linear approximation of $h(x)$.

Why? Covariance itself provides little intuition for what is happening under the "hood". What is more informative is a square root of the covariance. Imagine that $\Sigma = LL^T$ for some $L$. $L$ is what is really stretching the space in a linear fashion. If you were to deform the unit circle by $L$, you would get the uncertainty ellipse that we all know and love. By stretching and shrinking $L$, we can change our distribution.

That means that $HL$ would first deform the unit circle into an ellipse governed by $L$, and then $H$ would stretch and shear that ellipse into a different ellipse. Now we can recover the covariance of the deformed points: it would be $HL(HL)^T$, which is $HLL^TH^T = H\Sigma H^T$

Why does any of this matter?

The kalman gain is like a ratio, governing how much we should trust a new measurement. Often, your measurements will have different dimension and different units than your state. So you need to express uncertainty in the filter state as uncertainty in observation space.

The first equation is meaningless if $p$ and $r$ are different shapes or different units. We need to munge $p$ into the same shape and units as $r$ by projecting into $p$ observation space. The above rambling outlines how one does that.


答:
In the hardware or controller itself, I haven't seen the use of database. Anyway, having databases can be useful in planning for, e.g., motions or grasps for robots. For example, having a database of a manipulator's kinematic reachability can speed up motion planning. Another common example is a grasp database: for a given object, a set of possible grasps is computed and stored for further use in planning.

You might want to explore OpenRAVE, a robot simulation environment, where there are scripts provided specifically for computing various databases.

Title: Do robots usually have databases?

Body:
I am a junior web developer working mainly with Bash, Javascript, and Drupal. I'm more fascinated writing scripts and programs that do certain concrete actions instead of querying and manipulating databases.

I do desire to step into robotics in the future (after completions) and had the following question in my mind:

Do robots usually have databases (similar to these of websites in quality and quantity) and if so, please give a practical example what are they using for? Maybe in the context of machine vision or machine motion.

Update (13/11/19):

All the answers here are good in my opinion. If I could I would accept all of them, I suggest to start by reading the answer of user16549 which makes an introduction to DBs in robots, then continue to the answer by FooTheBar, and then read other answers.


答:
Databases are important for robots to store information...

...about itself:

Where was I when I was turned off

How is my hardware configuration (e.g. intrinsic calibration for cameras)

How warm was my motor when I used it last time for 10 minutes (important to detect defects)

How did I move my arm the last time to grasp (caching of planned movements)

How far did I drive with this set of wheels?

...about the environment:

a map

important positions in the map (a table, a door, a CNC machine)

where am I not allowed to drive to

...about objects:

how does this coke-bottle look like that I have to bring to my owner

...about jobs:

What was I doint when I had to stop for a recharge

How fast did I finish this task the last 500 Times

Persistent storage is very important for robots. Some information (like calibration) is only important for a single robot, other infos like maps or object descriptions should be shared between robots and have to be synced.

So databases are very important for modern robots.

Title: Do robots usually have databases?


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

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

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

What should the equations for thrust and torque be?

What have we tried?

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

答:
The solution is actually not quite linear. There are at least two cases:
1) The fastest solution does not require maximum turning at all times.
2) The fastest solution does require maximum turning at all times.

For an example of 1), consider the goal is straight ahead of the drone.
For an example of 2), consider the goal is very close, but straight behind the drone; the fastest solution is a circle-like path.

Finally, the problem is somewhat under-specified, unless you also add desired velocity/spin at the time of arrival.

So, the problem, as stated (even with adding mass), requires solving discontinuous differential equations in an underspecified problem system. There may exist a closed form solution to this, but when I've had to do this (in simulation systems, not real-world systems) I've ended up using simple planners and PID-type beahviors, rather than finding the "optimal" solution.

For example, one simple planner could be to first apply torque in a closed form to turn to face the desired target point at spin zero (this is a simple quadratic equation) and then turn on max thrust; you will get to the point (at a pretty high velocity.)
If you want to get to the point with velocity 0, then you have to reverse thrust at the middle of the trajectory.

Title: Drone targeting


问:
Irobot Create 2 Open Interface Communication
I am able to connect the create 2 robot to my laptop with a serial cable.  I am using putty_beta terminal to run. I am not able to key in commands in the terminal. but I am able to receive information from the robot. I want to achieve two way serial connection. What can I do?

答:
The Roomba expects binary data.  When you type in the PuTTY terminal, you are sending ASCII data.  For example when you type "128" into the terminal, you are actually sending 3 bytes of data: [49 50 56].  And because many commands of the OI Spec are outside the typeable ASCII range, using the terminal in this manner won't work.

You should use a programming language such as C, C#, Python, etc. to send the data packet.  There are many libraries for the Create2 freely available online.


问:
Create 2 packet group 100
I tried getting pack sensors for packet group 100. What I noticed was the order of the packets did not match what's in the document. Anyone notice the same problem?

For example here is the output of packet group 6 & 101 - which should be the same at 100 but that's not the case. It would seem like the order of the packets are not the same in group 100.

Here is packet group 6:

```<code>2017/05/31 193538 PACKET GROUP 6 len:52
        Bump & Wheel Drop:  0  - 1
                     Wall:  0  - 1
               Cliff Left:  0  - 1
         Cliff Front Left:  0  - 1
        Cliff Front Right:  0  - 1
              Cliff Right:  0  - 1
             Virtual Wall:  0  - 1
        Wheel Overcurrent:  0  - 1
              Dirt Detect:  0  - 1
                 Unused 1:  0  - 1
             Omni IR Code:  161  - 1
                  Buttons:  0  - 1
                 Distance:  0 - 2
                    Angle:  0 - 2
                 Charging:  0  - 1
                  Voltage:  15140 - 2
                  Current:  -211 - 2
              Temperature:  25  - 1
           Battery Charge:  2234 - 2
         Battery Capacity:  2696 - 2
              Wall Signal:  0 - 2
        Cliff Left Signal:  2938 - 2
  Cliff Front Left Signal:  2047 - 2
 Cliff Front Right Signal:  1399 - 2
       Cliff Right Signal:  2232 - 2
                 Unused 2:  0  - 1
                 Unused 3:  0 - 2
          Charging Source:  0  - 1
                  OI Mode:  1  - 1
              Song Number:  0  - 1
             Song Playing:  0  - 1
       Num Stream Packets:  0  - 1
            Req. Velocity:  0 - 2
              Req. Radius:  0 - 2
      Req. Right Velocity:  0 - 2
       Req. Left Velocity:  0 - 2
</code>```

Here is packet group 101:

```<code>2017/05/31 193859 PACKET GROUP 101 len:28
             Left Encoder:  10 - 2
            Right Encoder:  6 - 2
                   Bumper:  0  - 1
       Bumper Left Signal:  11 - 2
 Bumper Front Left Signal:  6 - 2
Bumper Center Left Signal:  8 - 2
Bumper Center Right Signal:  0 - 2
Bumper Front Right Signal:  11 - 2
      Bumper Right Signal:  0 - 2
             IR Code Left:  0  - 1
            IR Code Right:  0  - 1
       Left Motor Current:  0 - 2
      Right Motor Current:  0 - 2
       Main Brush Current:  0 - 2
       Side Brush Current:  0 - 2
                   Stasis:  0  - 1
</code>```

And here is packet group 100:

```<code>2017/05/31 193654 PACKET GROUP 100 len:80
        Bump & Wheel Drop:  0  - 1
                     Wall:  0  - 1
               Cliff Left:  0  - 1
         Cliff Front Left:  0  - 1
        Cliff Front Right:  0  - 1
              Cliff Right:  0  - 1
             Virtual Wall:  0  - 1
        Wheel Overcurrent:  0  - 1
              Dirt Detect:  0  - 1
                 Unused 1:  0  - 1
             Omni IR Code:  161  - 1
                  Buttons:  0  - 1
                 Distance:  0 - 2
                    Angle:  0 - 2
                 Charging:  0  - 1
                  Voltage:  15140 - 2
                  Current:  -219 - 2
              Temperature:  25  - 1
           Battery Charge:  2230 - 2
         Battery Capacity:  2696 - 2
              Wall Signal:  0 - 2
        Cliff Left Signal:  2950 - 2
  Cliff Front Left Signal:  2198 - 2
 Cliff Front Right Signal:  0 - 2
       Cliff Right Signal:  3072 - 2
                 Unused 2:  0  - 1
                 Unused 3:  0 - 2
          Charging Source:  0  - 1
                  OI Mode:  0  - 1
              Song Number:  0  - 1
             Song Playing:  0  - 1
       Num Stream Packets:  0  - 1
            Req. Velocity:  0 - 2
              Req. Radius:  0 - 2
      Req. Right Velocity:  0 - 2
       Req. Left Velocity:  0 - 2

             Left Encoder:  8 - 2
            Right Encoder:  4 - 2
                   Bumper:  0  - 1
       Bumper Left Signal:  11 - 2
 Bumper Front Left Signal:  6 - 2
Bumper Center Left Signal:  9 - 2
Bumper Center Right Signal:  0 - 2
Bumper Front Right Signal:  0 - 2
      Bumper Right Signal:  0 - 2
             IR Code Left:  0  - 1
            IR Code Right:  0  - 1
       Left Motor Current:  0 - 2
      Right Motor Current:  0 - 2
       Main Brush Current:  0 - 2
       Side Brush Current:  0 - 2
                   Stasis:  0  - 1
</code>```

答:
I don't see an order difference in the list you have posted here, so I'm not sure what your question is.

I also went back and checked the original source code (I am the author) and there is no discrepancy. The contents of packets 6 + 101 do equal packet 100.

Title: Create 2 packet group 100


问:
Differential GPS
I want to use DGPS on a robot. I understand how DGPS works but I am having trouble figuring out what specific hardware I need. Is there a good resource for how to actually setup DGPS? Thanks for your help</p>

答:
Basically, you need the following:

A target DGPS-capable unit

A source DGPS unit

A data link to carry the differential data from source to target

and of course necessary software and cabling.

You can find several white papers and application notes on manufacturers' internet pages. One of them is the following : trimble

A poor man's DGPS setup could be experimented, where you simply use two GPS Receivers and use the difference to correct the target GPS pos. This would not technically be as accurate as the complete DGPS solution. But, for hobby purposes this would probably be enough (not sure, never tried).

Title: Differential GPS


问:
Coordinate transform of accelerometer on rigid body
I have a question about something that seems like it would be pretty basic, but so fair I haven't been able to find a whole lot of discussion on the issue. It's possible I'm not not familiar enough with the terminology.

I have a rigid body with an accelerometer/gyro IC dev board nailed to it. I would like to know what the accelerometer would measure at another point on this board, in this case, the sensor of a camera that is also nailed to it.

My thinking is that I can use the accelerator, gyroscope and differentiated gyroscope data and the equation $a_t = a_m + \omega' \times r + \omega \times (\omega \times r)$, where

$a_t$ = transformed acceleration

$a_m$ = measure acceleration

$\omega$ = measured gyroscope reading

$\omega'$ = first derivative of the gyroscope reading

$r$ = the vector between the accelerometer/gyro and the point I want transformed to.

My plan is to get $\omega'$ with a Savitzky-Golay filter, though this makes implementation a lot less convenient, because I have to buffer my data, and try to figure out how the filter effects the noise variance of the sensor.

Does this plan make sense? Is there a better accepted way that I don't know about? I'm surprised that ROS or tf2 doesn't have a built in function for this. Is there something I am missing? Thanks!

答:
I believe this is correct, with the caveat that the acceleration will still be in the frame of the accelerometer. You'll need to multiply by the rotation matrix from m to t to know the acceleration in the t frame.

The reasons this isn't usually done include (1) the measurement errors of the accelerometer will be (mostly) statistically independent in the IMU frame but will have dependencies in the transformed frame, so any sort of signal processing should be done in the IMU frame, and (2) differentiating a gyro signal will add a ton of noise and the result will probably suck.


问:
Handling changing frames in ROS
I have a robot that is inherently symmetric in nature. Sometimes one side is the base while the other is the end-effector and vice versa. This 'mode' can change while the robot is moving around.

Judging from the URDF tutorials wiki, it looks like the commonly used URDF package in ROS is static and so the base of the robots is assumed to stay the base. Are there ways to get around this so that I can still use the TF package?

答:
Possible approaches:

1: Choose one end as the primary end, and set its position from your own calculations. Don't use URDF and write your own robot state publisher.

2: Maintain two models and switch between them. You publish which TF chain is valid, downstream nodes that care use that to choose which to use.

Title: Handling changing frames in ROS


问:
Read Multiple Channels of RX-TX with arduino
I have a 9 channel RF RX/TX and want to connect 3 motors to it.  I am able to connect channel 1 with motor 1 but unable to connect channel 2 with motor 2 simultaneously with arduino.

Here is the code I am currently using:

```<code>int motor1Left = 7;// defines pin 5 as connected to the motor
int motor1Right= 9;// defines pin 6 as connected to the motor
int motor2Left = 22;// defines pin 7 as connected to the motor
int motor2Right = 26;// defines pin 8 as connected to the motor
int enable = 5;
int enable2 = 10;
int channel1 = 2; // defines the channels that are connected
int channel2 = 3;// to pins 9 and 10 of arduino respectively

int Channel1 ; // Used later to
int Channel2 ; // store values

void  setup ()
{
  pinMode (motor1Left, OUTPUT);// initialises the motor pins
  pinMode (motor1Right, OUTPUT);
  pinMode (motor2Left, OUTPUT);
  pinMode (motor2Right, OUTPUT);// as outputs
  pinMode (channel1, INPUT);// initialises the channels
  pinMode (channel2, INPUT);// as inputs

  Serial.begin (9600); // Sets the baud rate to 9600 bps
}

void  loop ()
{
  Channel1 = (pulseIn (channel1, HIGH)); // Checks the value of channel1
  Serial.println (Channel1); //Prints the channels value on the serial monitor
  delay(1000);

  Channel2 = (pulseIn (channel2, HIGH)); // Checks the value of channel1
  Serial.println (Channel2); //Prints the channels value value on the serial monitor
  delay(1000);

  if (Channel1 > 1470 && Channel1 < 1500) /*These are the values that I got from my transmitter, which you may customize according to your transmitter values */
  {
    digitalWrite (motor1Left, LOW); // Sets both the
    digitalWrite (motor1Right, LOW);// motors to low
    analogWrite(enable, 100);
  }

  if (Channel1 < 1460) // Checks if Channel1 is lesser than 1300
  {
    digitalWrite (motor1Left, HIGH);// Turns the left
    digitalWrite (motor1Right, LOW); // motor forward
    analogWrite(enable, 100);
  }

  if (Channel1 > 1510) // Checks if Channel1 is greater than 1500
  {
    digitalWrite (motor1Left, LOW);// Turns the right

    digitalWrite (motor1Right, HIGH);// motor forward
    analogWrite(enable, 70);
  }

  if (Channel2 > 1480 && Channel1 < 1500 )
  {
    digitalWrite (motor2Left, LOW);// Sets both the
    digitalWrite (motor2Right, LOW);// motors to low
    analogWrite (enable2, 100);
  }

  if (Channel2 < 1300) // Checks if Channel2 is lesser than 1300
  {
    digitalWrite (motor2Left, LOW);// Turns the left
    digitalWrite (motor2Right, HIGH);// motor backward
    analogWrite (enable2, 100);
  }

  if (Channel2 > 1500) // Checks if Channel2 is greater than 1500
  {
    digitalWrite (motor2Left, HIGH);// Turns the right
    digitalWrite (motor2Right, LOW);// motor backward
    analogWrite (enable2, 100);
  }
}
</code>```

答:
You cannot use the synchronous "pulseIn()" function to read multiple parallel inputs.
In general, in embedded systems, you will end up writing a loop that runs as fast as possible, over and over, and does the following:

1) read inputs
2) calculate updates to state
3) write outputs

Ideally, this loop runs many thousands of times per second.

I'm assuming from context that by "RX/TX" you mean RC-hobby-radio-control PWM pulses.
The problem you're having is reading the width of more than one pulse at the same time.
There are two ways to approach this:

1) Each time through the loop, read both input pins. If a pin is high and was previously low, write the current time (in microseconds) to a variable tracking that pin. If the pin is low and was previously high, write the difference between current time, and the value in the tracking variable, to a variable tracking "last length for pin X."

2) Install pin change interrupt handlers for the input pins. Each time one of the pins change, run the algorithm above, from within the interrupt handler. Because of possible race conditions, you need to check both pins for each pin change interrupt.

You will note with both these approaches that precision is pretty crummy in the Arduino environment. There are a few reasons for this, including interrupts used to run the milliseconds timer, and significant overhead in the digitalRead() function as compared to reading ports directly. To get high-quality input, you may need to read ports directly (check the Atmega data sheets for how) and you may also want to poll all the way until the pulse goes low in the interrupt handler (beware that a second pin may go high while the first pin is already high.)

This may sound complex if you're new to programming microcontrollers, but it's really the kind of thing that's done every day by those who actual work with this. This is one of several reasons why you need a fair bit of education and experience to become a seasoned developer ;-)


问:
.launch file associated while using a sensor
What is the general structure of .launch file for using a sensor?

For example:

1.Following code is example for using a JoyStick to control TurtleSim

```<code><launch>
  <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
  <node pkg="chapter4_tutorials" type="example1" name="example1" />
  <param name="axis_linear" value="1" type="int" />
  <param name="axis_angular" value="0" type="int" />
  <node respawn="true" pkg="joy"type="joy" name="teleopJoy">
    <param name="dev" type="string" value="/dev/input/js0" />
    <param name="deadzone" value="0.12" />
  </node>
</launch>
</code>```

2.Using a Laser Range finder

```<code><launch>
  <node pkg="hokuyo_node" type="hokuyo_node" name="hokuyo_node"/>
  <node pkg="rviz" type="rviz" name="rviz"
        args="-d $(find chapter4_tutorials)/example2.vcg"/>
  <node pkg="chapter4_tutorials" type="example2" name="example2" />
</launch>
</code>```

What exactly is the syntax?

Why do we have two <node pkg>?

答:
The first node, hokuyo_node, is the driver for the sensor.
The second node, rviz, is for visualizing the data coming from the hokuyo_node.
The third node is the tutorial program.

The launch file  format is well documented. However it assumes that you understand the basic organization of ROS, what nodes are, and how they interact.


问:
Can a state matrix have a row of zeros?
While formulating a state matrix of a system, say a system of a typical cruise controller,
\begin{equation}
\begin{bmatrix}
\dot{v}
\end{bmatrix} = \begin{bmatrix} -\frac{b}{m} \end{bmatrix} \begin{bmatrix} v \end{bmatrix} + \begin{bmatrix} -\frac{1}{m} \end{bmatrix} \begin{bmatrix} u \end{bmatrix}
\end{equation}
\begin{equation}
y = \begin{bmatrix} 1 \end{bmatrix} \begin{bmatrix} v \end{bmatrix}
\end{equation}
If we consider $b = 0$ (negligible), then does this state space matrix make sense any more and is it viable? A state matrix without state variable influence in equation?</p>

答:
Yes, a state matrix with zero rows and/or columns makes sense and is viable.  It typically signify pure integrators in the system.  In the example you give,
$$
\dot{v} = -\frac{b}{m} v +\frac{1}{m} u
$$
where $v$ is the speed, $u$ is the externally applied force, and $bv$ is some viscous damping force. Now if the viscous damping coefficient is zero (no retarding force acting on the object of mass $m$, the differential equation becomes
$$
\dot{v} = \frac{1}{m} u
$$
The $\dot{v}$ is integrated kinematically to obtain $v$, and your state-space model becomes
\begin{eqnarray*}
[\dot{v}] &=& [0][v] + [1/m][u] \\
y &=& [1][v]
\end{eqnarray*}

Another way to look at it is to obtain the transfer function from $u$ to $y$ (the transfer function of the plant which you want to control), which becomes
$$
\frac{Y}{U} = \frac{1}{m}\cdot\frac{1}{s+b/m}
$$
which is a constant term ($1/m$) times a first-order lag.  For $b=0$, the transfer function becomes
$$
\frac{Y}{U} = \frac{1}{m}\cdot\frac{1}{s}
$$
which is simply a constant term ($1/m$) times an integrator ($1/s$).

Edit: Controllability of the System

If the externally applied force is zero (i.e., $u=0$) the output of the integrator ($v$) remains constant; if $u>0$, the resulting acceleration ($\dot{v}=u/m$) is integrated "up" and the speed increases; if $u<0$, $\dot{v}$ is integrated "down" and the speed decreases.  Speed, of course, is the state variable in your system.  Thus, it is possible to transfer the system state from any initial state, $v(t_0)$, to any other state, $v(t_f)$, in a finite time by means of the input, irrespective of the value of $b$. So, by purely physical reasoning we can say thet the system is controllable.

Since this is a linear time-invariant system, the controllability matrix
$$
\mathbf{Q} = \left[
\begin{array}{cccc}
\mathbf{B} & \mathbf{A}\mathbf{B} & \cdots & \mathbf{A}^{n-1}\mathbf{B}
\end{array}
\right]
$$
where $\mathbf{A}=[-b/m]$ is the state matrix, $\mathbf{B}=[1/m]$ is the input matrix, and $n=1$ is the number of state variables. For your system,
$$
\mathbf{Q} = \mathbf{B} = \left[
\begin{array}{c}
1/m
\end{array}
\right]
$$
i.e., a non-zero scalar which can be inverted (it is non-singular).  Thus, the rank of $\mathbf{Q}$ is one, and the system is controllable.

Title: Can a state matrix have a row of zeros?


问:
Kalman filter prediction questions
I have a dataset where measurements were taken at 1 Hz, and I am trying to use a Kalman filter to add predicted samples in between the measurements, so that my output is at 10 Hz. I have it working ok when the velocity is linear, but when the direction changes, the filter takes a while to catch up. I am new to Kalman models, so am very likely making some mistakes in my settings. What can I do to improve this? See attached image for an example, the red is measured data, with stepping in between measurements. The blue is the Kalman corrected.

std::vector<double> measurements is a dummy data array I am testing with.

The main Kalman code is based on Github: hmartiro/kalman-cppkalman.cpp

My code is:

```<code>int main(int argc, char* argv[]) {

  int n = 3; // Number of states
  int m = 1; // Number of measurements

  double dt = 1.0/30; // Time step

  Eigen::MatrixXd matA(n, n); // System dynamics matrix
  Eigen::MatrixXd matC(m, n); // Output matrix
  Eigen::MatrixXd matQ(n, n); // Process noise covariance
  Eigen::MatrixXd matR(m, m); // Measurement noise covariance
  Eigen::MatrixXd matP(n, n); // Estimate error covariance

  // Discrete motion, measuring position only
  matA << 1, dt, 0, 0, 1, dt, 0, 0, 1;
  matC << 1, 0, 0;

  // Reasonable covariance matrices
  matQ << 0.001, 0.001, .0, 0.001, 0.001, .0, .0, .0, .0;
  matR << 0.03;
  matP << .1, .1, .1, .1, 10000, 10, .1, 10, 100;

  // Construct the filter
  KalmanFilter kf(dt,matA, matC, matQ, matR, matP);

  // List of noisy position measurements (yPos)
  std::vector<double> measurements = {
     10,11,13,13.5,14,15.2,15.6,16,18,22,20,21,19,18,17,16,17.5,19,21,22,23,25,26,25,24,21,20,18,16
  };

  // Best guess of initial states
  Eigen::VectorXd x0(n);
  x0 << measurements[0], 0, 0;
  kf.init(dt,x0);

  // Feed measurements into filter, output estimated states
  double t = 0;
  Eigen::VectorXd y(m);

  for(int i = 0; i < measurements.size(); i++) { //ACTUAL MEASURED SAMPLE

      yPos << measurements[i];

      kf.update(yPos);

      for (int ji = 0; ji < 10; ji++)  // TEN PREDICTED SAMPLES
      {
          t += dt;

         kf.update(yPos);

          yPos << kf.state().transpose(); //USE PREDICTION AS NEW SAMPLE

      }
  }

  return 0;
}
</code>```

答:
This is what I believe is happening from a 1 min glance over your code.

You are passing true measurement data to your KF at 3 hz. Between the actual measurements you are passing the new state estimates to your KF as if they were measurements. This is incorrect. What you should be doing is  calling the prediction update. What you are doing now will artificially drive down the covariance of your KF because it thinks it is getting new measurements. This would explain why the filter responds slowly to actual measurements.

These are the lines from the git page that you want to run each time you want a new state estimate(without a new measurement!!!)

x_hat_new = A * x_hat;
 P = A*P*A.transpose() + Q;

Title: Kalman filter prediction questions


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

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

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

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

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

答: i have ended up using 2 RC ESCs they can easily handle the 35 amps and 12v.  the flysky remote also allows mixing so that the 2 motors/escs can handle skid steering on one stick of the remote.  So much cheaper</p>


问:
Rotate DC servo motor with ARDUINO
I have this servo motor (http://robokits.co.in/motors/high-torque-encoder-dc-servo-motor-60rpm-with-uart-i2c-ppm-drive?gclid=CLHf9_fCqNQCFVAeaAodhM0Ddg&). Generally, I have seen servo motor with three wires only but this servo motor have 6 wires. I want to rotate this using PPM signal and accordingly I made connection as described in motor manual. Arduino Code to rotate motor is:

```<code>// Include the Servo library
#include <Servo.h>
// Declare the Servo pin
int servoPin = 10;
// Create a servo object
Servo Servo1;
void setup() {
   // We need to attach the servo to the used pin number
   Servo1.attach(servoPin);
}
void loop(){
   // Make servo go to 0 degrees
   Servo1.write(0);
   delay(1000);
   // Make servo go to 90 degrees
   Servo1.write(90);
   delay(1000);
   // Make servo go to 180 degrees
   Servo1.write(180);
   delay(1000);
}
</code>```

But what the motor does is it continuously turning.
How to control motor position using PPM.
Thanks.

答:
Read this : http://www.robokits.co.in/documentation/RMCS220x_DCServo_Driver.pdf
This is not exactly a servo. But it is a brushed DC motor coupled with an encoder and an inbuilt microcontroller to control the position from encoder feedback. The datasheet clearly says you can use PPM signal for speed control (RPM control) and not position control. Hence it does not work as a servo with PPM signal.

You need to use UART or I2C using Arduino  to communicate with the motor, which is simple.
You can check this tutorial on serial with Arduino :
https://www.arduino.cc/en/reference/serial

Out of the six wires, only 4 will be used in case of controlling via UART:

V+ = 12V

UART RXD = Arduino TXD

UART TXD = Arduino RXD

GND = Arduino GND and power source ground

Now either you can program Arduino to send UART commands or use Arduino's Serial Terminal directly in the way the mentioned in Page 4. Make sure the baud rate is 9600 For a servo action you will mostly be using the commands :

GXXX

RXXX
where XXX can be replaced with suitable encoder value. By using encoder specification from Page 3 :
\begin{equation}
XXX = \frac{Angle (in\hspace{5pt}degrees)}{0.2}
\end{equation}

Title: Rotate DC servo motor with ARDUINO


答:
Your dT is 1/30 for some reason, but you said you had 1 Hz (1/1) and wanted 10 Hz (1/10).

But to your question, I would propagate the "filler" samples by basing off the last Kalman estimate, then resetting the filler states to the Kalman states whenever the Kalman filter updates. Your path will be a little jerky, but right now you're treating future estimates as measurements, so your predicted states and measured states are identical, which means it's going to screw with your filter's covariance estimates.

I would only update the filter when you have a real measurement.

So, for example, right now you have something like:

fastTs = 1/10; % High speed sample rate
while (true)
    if (currentMeasurementExists)
        KalmanStates = KalmanFilter(KalmanStates, Measurement, fastTs);
    else
        KalmanStates = KalmanFilter(KalmanStates, FillerMeasurement, fastTs);
    end
    FillerMeasurement = C*KalmanStates; % C is the output matrix that converts states to measurements
end
```

<p>What I would suggest instead is:</p>

<pre class="lang-matlab prettyprint-override"><code>fastTs = 1/10;
slowTs = 1/1;
fastA = A(fastTs); % Sorry, no easy way to show this because dT is tucked away
slowA = A(slowTs); % in your 'A' matrix, but you need to use the correct sample times in the correct locations.
while (true)
    if (currentMeasurementExists)
        KalmanStates = KalmanFilter(KalmanStates, Measurement, slowTs);
        FillerStates = KalmanStates;
    else
        FillerStates = fastA*FillerStates; % Remember to use fastTs for filler, slowTs for real measurements.
    end
    FillerMeasurements = C*FillerStates;
end
</code>```

So to reiterate again, you are treating filler samples as real measurements and updating the Kalman filter accordingly, which is updating your Kalman states (and updating Kalman covariances to expect better samples).

What I think you should do instead is to split the Kalman filter off to do only filtering on only real measurements. You know how your system should progress - this is the A matrix. So, evolve the system from the last known "good" state (last Kalman update) using the A matrix. When you get a new "good" state (new Kalman update), then reset the filler states to the Kalman states and continue propagating.

I would bet the biggest performance hit you're taking on this is that you're screwing up the covariance estimates, because all of your "measurements" are exactly what the filter would predict.

Title: Kalman filter prediction questions

Body:
I have a dataset where measurements were taken at 1 Hz, and I am trying to use a Kalman filter to add predicted samples in between the measurements, so that my output is at 10 Hz. I have it working ok when the velocity is linear, but when the direction changes, the filter takes a while to catch up. I am new to Kalman models, so am very likely making some mistakes in my settings. What can I do to improve this? See attached image for an example, the red is measured data, with stepping in between measurements. The blue is the Kalman corrected.

std::vector<double> measurements is a dummy data array I am testing with.

The main Kalman code is based on Github: hmartiro/kalman-cppkalman.cpp

My code is:

```<code>int main(int argc, char* argv[]) {

  int n = 3; // Number of states
  int m = 1; // Number of measurements

  double dt = 1.0/30; // Time step

  Eigen::MatrixXd matA(n, n); // System dynamics matrix
  Eigen::MatrixXd matC(m, n); // Output matrix
  Eigen::MatrixXd matQ(n, n); // Process noise covariance
  Eigen::MatrixXd matR(m, m); // Measurement noise covariance
  Eigen::MatrixXd matP(n, n); // Estimate error covariance

  // Discrete motion, measuring position only
  matA << 1, dt, 0, 0, 1, dt, 0, 0, 1;
  matC << 1, 0, 0;

  // Reasonable covariance matrices
  matQ << 0.001, 0.001, .0, 0.001, 0.001, .0, .0, .0, .0;
  matR << 0.03;
  matP << .1, .1, .1, .1, 10000, 10, .1, 10, 100;

  // Construct the filter
  KalmanFilter kf(dt,matA, matC, matQ, matR, matP);

  // List of noisy position measurements (yPos)
  std::vector<double> measurements = {
     10,11,13,13.5,14,15.2,15.6,16,18,22,20,21,19,18,17,16,17.5,19,21,22,23,25,26,25,24,21,20,18,16
  };

  // Best guess of initial states
  Eigen::VectorXd x0(n);
  x0 << measurements[0], 0, 0;
  kf.init(dt,x0);

  // Feed measurements into filter, output estimated states
  double t = 0;
  Eigen::VectorXd y(m);

  for(int i = 0; i < measurements.size(); i++) { //ACTUAL MEASURED SAMPLE

      yPos << measurements[i];

      kf.update(yPos);

      for (int ji = 0; ji < 10; ji++)  // TEN PREDICTED SAMPLES
      {
          t += dt;

         kf.update(yPos);

          yPos << kf.state().transpose(); //USE PREDICTION AS NEW SAMPLE

      }
  }

  return 0;
}
</code>```

Body:
Your dT is 1/30 for some reason, but you said you had 1 Hz (1/1) and wanted 10 Hz (1/10).

But to your question, I would propagate the "filler" samples by basing off the last Kalman estimate, then resetting the filler states to the Kalman states whenever the Kalman filter updates. Your path will be a little jerky, but right now you're treating future estimates as measurements, so your predicted states and measured states are identical, which means it's going to screw with your filter's covariance estimates.

I would only update the filter when you have a real measurement.

So, for example, right now you have something like:

fastTs = 1/10; % High speed sample rate
while (true)
    if (currentMeasurementExists)
        KalmanStates = KalmanFilter(KalmanStates, Measurement, fastTs);
    else
        KalmanStates = KalmanFilter(KalmanStates, FillerMeasurement, fastTs);
    end
    FillerMeasurement = C*KalmanStates; % C is the output matrix that converts states to measurements
end

What I would suggest instead is:

fastTs = 1/10;
slowTs = 1/1;
fastA = A(fastTs); % Sorry, no easy way to show this because dT is tucked away
slowA = A(slowTs); % in your 'A' matrix, but you need to use the correct sample times in the correct locations.
while (true)
    if (currentMeasurementExists)
        KalmanStates = KalmanFilter(KalmanStates, Measurement, slowTs);
        FillerStates = KalmanStates;
    else
        FillerStates = fastA*FillerStates; % Remember to use fastTs for filler, slowTs for real measurements.
    end
    FillerMeasurements = C*FillerStates;
end

So to reiterate again, you are treating filler samples as real measurements and updating the Kalman filter accordingly, which is updating your Kalman states (and updating Kalman covariances to expect better samples).

What I think you should do instead is to split the Kalman filter off to do only filtering on only real measurements. You know how your system should progress - this is the A matrix. So, evolve the system from the last known "good" state (last Kalman update) using the A matrix. When you get a new "good" state (new Kalman update), then reset the filler states to the Kalman states and continue propagating.

I would bet the biggest performance hit you're taking on this is that you're screwing up the covariance estimates, because all of your "measurements" are exactly what the filter would predict.


问:
How to call remote ROS node on mobile robot through laptop using wifi?
I've recently tried to get the remote ROS node on a mobile robot through wifi. This is as below picture.

I've run roscore command on both laptop and mobile robot and when I run the roscore command, I get the following warning message on my laptop server:

WARNING: ROS_MASTER_URI [http://192.168.7.2:11311] host is not set to this machine

```<code>auto starting new master
process[master]: started with pid[2719]
ROS_MASTER_URI=http://192.168.7.2:11311/

setting /run_id to 4ef6c0f8-bfdf-11d3-a450-4e699f75a6e7
process[rosout-1]:started with pid[2732]
started core service[/rosout]
</code>```

But, when I run rosparam list command on my laptop, I've got the following results:

```<code>root@duminda-laptop:~#rosparam list
/rosdistro
/roslaunch/uris/host_192_168_7_2_35078
/rosversion
/run_id
root@duminda-laptop:~#rosparam get /rosdistro
hydro
root@duminda-laptop:~#rosparam get /roslaunch/uris/host_192_168_7_2_35078
http://192.168.7.2:35078
root@duminda-laptop:~#rosparam get /run_id
4ef6c0f8-bfdf-11d3-a450-4e699f75a6e7
</code>```

These results show that both laptop and mobile robot connected ok.

Is my setup to receiving the mobile robots data to my laptop is ok?

Why there is a warning message like above when starting the roscore?

答:
This is how I make changes to my robot according to the book "Mastering ROS for Robotics Programming"

configuring mobile robot as ROS master

```<code>#setting LAPTOP_IP as mobile robot's ip
export LAPTOP_IP=192.168.7.2

#setting ROS_IP variable as laptop's ip
export ROS_IP=$LAPTOP_IP

#setting ROS_MASTER_URI as mobile robot's_ip
export ROS_MASTER_URI=http://192.168.7.2:11311
</code>```

configuring laptop as ROS computing node

```<code>#setting LAPTOP_IP as laptop's ip
export LAPTOP_IP=192.168.7.1

#setting ROS_IP variable as laptop's ip
export ROS_IP=$LAPTOP_IP

#setting ROS_MASTER_URI as mobile robot's_ip
export ROS_MASTER_URI=http://192.168.7.2:11311
</code>```

Also I've first tried this with different ros versions,ROS groovy on mobile robot and ROS indigo on my laptop . So as a result of that there was an error occurred described as in my question and in this attempt I've tried this configuration in both laptop and in the mobile robot using same ROS indigo versions. Hope this will help to others.

Update 2017.08.28

Finally I've experimented that it is ok with using two versions of ROS for this . Using ROS groovy for mobile robot and ROS indigo for our pc/laptop . The ROS master and ROS computing node ip configurations must be properly set as mentioned above in this answer.

Body:
This is how I make changes to my robot according to the book "Mastering ROS for Robotics Programming"

configuring mobile robot as ROS master

#setting LAPTOP_IP as mobile robot's ip
export LAPTOP_IP=192.168.7.2

#setting ROS_IP variable as laptop's ip
export ROS_IP=$LAPTOP_IP

#setting ROS_MASTER_URI as mobile robot's_ip
export ROS_MASTER_URI=http://192.168.7.2:11311

configuring laptop as ROS computing node

#setting LAPTOP_IP as laptop's ip
export LAPTOP_IP=192.168.7.1

#setting ROS_IP variable as laptop's ip
export ROS_IP=$LAPTOP_IP

#setting ROS_MASTER_URI as mobile robot's_ip
export ROS_MASTER_URI=http://192.168.7.2:11311

Also I've first tried this with different ros versions,ROS groovy on mobile robot and ROS indigo on my laptop . So as a result of that there was an error occurred described as in my question and in this attempt I've tried this configuration in both laptop and in the mobile robot using same ROS indigo versions. Hope this will help to others.

Update 2017.08.28

Finally I've experimented that it is ok with using two versions of ROS for this . Using ROS groovy for mobile robot and ROS indigo for our pc/laptop . The ROS master and ROS computing node ip configurations must be properly set as mentioned above in this answer.


答:
From Configuration of computer A and computer B on nubot-nudt/gazebo_visual:

Configuration of computer A and computer B

Computer A runs one ROS node called N1 (i.e. Gazebo in the figure above) along with roscore. Computer B runs another ROS node called N2 (i.e. Coach 1 and Robot team 1 can be treated as one combined node in the figure above).

The communication between computer A and computer B is via ROS master. The following is the configuration steps:

In computer A, add computer B's IP address in /etc/hosts; and in computer B, add computer A's IP address in /etc/hosts
e.g. In computer A, $ sudo gedit /etc/hosts and add "Maggie 192.168.8.100"
In computer B, $ sudo gedit /etc/hosts and add "Bart   192.168.8.101"

In computer A, run node N1; In computer B, before you run node N2, you should export ROS_MASTER_URI.
e.g. In computer B,  $ export ROS_MASTER_URI=http://Bart:11311

Title: How to call remote ROS node on mobile robot through laptop using wifi?

Body:
I've recently tried to get the remote ROS node on a mobile robot through wifi. This is as below picture.

I've run roscore command on both laptop and mobile robot and when I run the roscore command, I get the following warning message on my laptop server:

WARNING: ROS_MASTER_URI [http://192.168.7.2:11311] host is not set to this machine

```<code>auto starting new master
process[master]: started with pid[2719]
ROS_MASTER_URI=http://192.168.7.2:11311/

setting /run_id to 4ef6c0f8-bfdf-11d3-a450-4e699f75a6e7
process[rosout-1]:started with pid[2732]
started core service[/rosout]
</code>```

But, when I run rosparam list command on my laptop, I've got the following results:

```<code>root@duminda-laptop:~#rosparam list
/rosdistro
/roslaunch/uris/host_192_168_7_2_35078
/rosversion
/run_id
root@duminda-laptop:~#rosparam get /rosdistro
hydro
root@duminda-laptop:~#rosparam get /roslaunch/uris/host_192_168_7_2_35078
http://192.168.7.2:35078
root@duminda-laptop:~#rosparam get /run_id
4ef6c0f8-bfdf-11d3-a450-4e699f75a6e7
</code>```

These results show that both laptop and mobile robot connected ok.

Is my setup to receiving the mobile robots data to my laptop is ok?

Why there is a warning message like above when starting the roscore?


问:
How can I get Windows Kinect working on Angstrom on Beaglebone?
I have tried following a number of guides on the internet but most of them fall down as libfreenect does not exist in opkg, which is the apt-get of Angstrom. Has anyone got it working and if so what is the method?</p>

答:
Would it be possible to build it from source?  This thread on Google Groups talks about how to do that on Ubuntu 10.10 (although it doesn't work on future versions):

Install Libfreenect0.0:

sudo apt-get install libc6 libusb-1.0-0 udev libglu1-mesa libgl1-mesa-
  glx libsm6 libice6 libx11-6 libxext6 libglut3 libxmu6 libxi6 libstdc+
  +6 libgcc1

wget http://ftp.us.debian.org/debian/pool/main/libf/libfreenect/libfreenect0.0_0.0.1+20101211+2-2_armel.deb

sudo dpkg -i libfreenect0.0_0.0.1+20101211+2-2_armel.deb

install libfreenect-dev:

sudo apt-get install libglut3-dev libxi-dev libxmu-dev libc6-dev
  libglu1-mesa-dev libgl1-mesa-dev libsm-dev libice-dev libx11-dev
  libxext-dev libstdc++6-4.5-dev libusb-1.0-0-dev pkg-config

wget http://ftp.us.debian.org/debian/pool/main/libf/libfreenect/libfreenect-dev_0.0.1+20101211+2-2_armel.deb

sudo dpkg -i libfreenect-dev_0.0.1+20101211+2-2_armel.deb

install libfreenect-demos:

sudo apt-get install freeglut3 libgcc1 libgl1-mesa-glx libgl1 libglu1-
  mesa libglu1 libice6 libsm6 libstdc++6 libx11-6 libxext6 libxi6
  libxmu6 libglut3

wget http://ftp.us.debian.org/debian/pool/main/libf/libfreenect/libfreenect-demos_0.0.1+20101211+2-2_armel.deb

sudo dpkg -i libfreenect-demos_0.0.1+20101211+2-2_armel.deb

install freenect

wget http://ftp.us.debian.org/debian/pool/main/libf/libfreenect/freenect_0.0.1+20101211+2-2_armel.deb

sudo dpkg -i freenect_0.0.1+20101211+2-2_armel.deb

sudo cp /lib/udev/rules.d/51-kinect.rules /etc/udev/rules.d/51-
  kinect.rules


问:
Stepper motor control
I am trying to understand the stepper motor to Mach3 type software control interfaces, mostly from a logical perspective, deducing most of everything because I have no concrete resource to refer.

So basically I have purchased a "LinkSprite" 3 axis engraver. It says it comes with Arduino GRBL board which interprets G-code. Sends it to stepper driver shields.

Bottom

I can see the drivers need to send a approximate sign wave of some sort to one or both windings to actuate them.

Top

Mach3 I read essentially transmits two signals per axis to the motion controller/breakout board (which in turn is connected to the drivers), one being the number of steps the other the direction and probably via parallel ports GPIO or something.

So what underlying transmission protocol carries mach3 signals, like i2c or something via the parallel poet? How is steps and direction, and axis encoded?

What does the motion controller do exactly? Minimally does it just breakout out the signal to the drivers? What is the drivers inputs?

My Arduino GRBL board I read interprets G-code, but isn't that what Mach3 does?

How can I connect from stepper motor waveform on windings to some interface like Mach3 the encodings and concrete information of this logical path of the workings of the control of the stepper motors?

答:
Well, I guess the most difficult part is implementing the functions that calculate the Linear/Circular Interpolations and translating those to stepper motion. I've been working on a similar project, however, most of the motion required relies only on absolute positioning.

The 6-axis motion controller that I have designed for this project is built around the dSPIN L6470 from STMicroelectronics. ST Provides a decent library to program the device and the integrated motion-engine makes the job a lot easier.

However, it turns out that this approach is not as reliable for a CNC or even a 3D printer application, where a simple Go_To(abs_value); command is not sufficient to do the mission. Simply because the trajectory of the motion is unpredictable. Another drawback is the speed, aka the Feed Rate. There's no way to achieve a target speed with each absolute positioning command having its own distance and trajectory. I believe here comes into play what the "planner" does in GRBL and other implementations out there that are based on the same rule of thumb.

I tried to understand how GRBL does all of that, but because the functions concerned are distributed across different modules and they are all dependent on each other, I found it waste of time to dig more into this, as my own g-code interpreter follows a different path/approach and I'm quite satisfied with the results I have achieved so far. However, I would love to get a clear picture of other implementations and compare the performance with mine and see what's the best in different scenarios.

Title: Stepper motor control


问:
Two-wheeled self-balancing robot - Choosing the control system
Would an Arduino Uno have the precision-timing required (using only firmware) to control a two-wheeled inverted pendulum robot, or would it need a RTOS?

Note: Thanks for both answers, they both helped a lot.  I just chose the last answer as the accepted answer.

答:
There are several aspects that you have to consider.  If the size of the balancing robot is small, you can easily control two 5V small dc motors directly with arduino/RPi-3. But, if the motors are big, then you would need 2 big motors (12V) and you would need a Raspberry Pi 3 + motor controllers or Arduino + motor controllers.

Your choice of control algorithm (LQR, PID, etc) will affect the type of microcontroller board that you shuold use. But, Raspberry Pi-3 should be able to do the job as Arduino is lacking in the computational power when running complex algorithm.


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

Thank you.

答:
An accelerometer triad measures the non-field specific force vector ($\mathbf{f}$) at it's location. If it is located at the centre of mass of the body, it will measure
$$
\mathbf{f} = \mathbf{\dot{v}} - \mathbf{g}
$$
where $\mathbf{\dot{v}}$ is the translational acceleration of the body and $\mathbf{g}$ is the acceleration due to gravity.

To be able to measure angular acceleration around the centre of mass, the accelerometer triad needs to be displaced a distance $\mathbf{\rho}$ from the centre of mass.  In that case it will measure
$$
\mathbf{f'} = \mathbf{f} + \mathbf{\dot{\omega}\times\rho} +\mathbf{\omega\times}(\mathbf{\omega\times\rho})
$$
where $\mathbf{\omega}$ is the angular velocity of the body.  So, to be able to use the accelerometer to (indirectly) measure angular acceleration, you will need to measure the acceleration of the centre of mass as well.  This can be done by having two accelerometer triads mounted to the body.  Then, you have to solve the equation given above to obtain $\mathbf{\dot{\omega}}$ and integrate it to obtain $\mathbf{\omega}$.

Gyroscopes do drift, yes.  But accelerometers also have biases, they are very noisy, and will also measure vibrations in your system.  These biases and noises will, of course, be integrated.


答:
Accelerometers (not gyroscopes) are by far the most common way of sensing inclination, in other words the rotation angle relative to "down", and adjusting the mobile phone screen and camera to "portrait mode" or "landscape mode".

Unlike gyroscopes, this method of measuring pitch and roll generally does not have long-term drift (although it often does have a lot of short-term errors).

Title: Angular Velocity from dual tri axial accelerometers

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

Thank you.


问:
Starting and stopping a heavy rotating disc
I am currently working at a project involving a heavy disc rotating around its center. The disc weights around 2 kg and has a radius of 0,25 m. At every angle π/6, there exists smaller discs of masses from range 0,1 kg to 1 kg 0,15 m from the center. The radius for these are 0,05 m.
A picture to illustrate:

I have roughly calculated the moment of inertia when all smaller discs weight 1 kg. Using the formula for a circular plate and Steiner's theorem, the result is:

$$ Inertia = \frac{0,25^2}{2} + 12(\frac{0,05^2}{4} + 0,15^2) = 0,34 kgm^2$$

Now I want the disc to be able to spin and stop at these specific angles. Say for instance I want the disc to rotate from 0 to π. This means I need a precise way to control my disc. My plan is to use a servo and some gears to drive this. I need the disc to turn 180° in at least 3 seconds (preferably less). With this angular velocity and inertia, I have realized it might not be the easiest thing to stop this spinning wheel, let alone accelerate it. Here is another image:

The motor does not need to be positioned like that, it would also be possible to drive the disc by positioning the motor on the edge of the disc for example.

What kind of motor should I be looking for, how would I handle stopping the disc? I am looking for general tips on how to accomplish this.

答:
Some of the issues here are the same as in Rotate (and stop) a large disk in very tiny increments, and some are different.  See the second answer there for drawings of one possible arrangement of the motor driving the plate.

I think it would make sense to use parts that now are widely used for 3D printer axis motion, such as Nema 17 stepper motors, encoders, timing-belt pullies / sprockets, and timing belts or timing belt material.  If a timing belt wraps around the 25-cm disk and then is driven by a 1-cm timing-belt pulley flanked by two tension idlers, the 25:1 mechanical reduction drops the required motor torque into the range that Nema 17 stepper motors can handle while running at a speed high enough to meet your requirements.

Such motors can run at 6000 to 7000 half-steps per second when providing 0.03 Nm torque.  (See, for example, the "Pull out Torque Curve" diagram in C140-A+datasheet.jpg at adafruit.com.)  This is 900 to 1050 RPM, somewhat above the necessary 250 RPM stepper speed to rotate the large disk half a turn in 3 seconds, which is 10 RPM.  (Ie, 10 RPM = (0.5/3 RPS)*60 S/M.)  The required torque at the stepper appears to be just under 0.03 Nm, as follows:  For the large disk, τ = Ia, where τ is torque, in Nm;  I is moment of inertia, listed in the question as 0.34 kg m2; and a is angular acceleration.  Here, the required angular acceleration is positive for 1.5 seconds and negative for 1.5 seconds, assuming the disk is at rest, moves, and is again at rest.  Moving π radians in 3 seconds from rest to rest is π/3 radians per second net, via sustained accelerations of ±2π/3 radians per second2.  Thus τ = 0.34 kg m2 * 2π/3 /s2 = 0.712 Nm.  Divided by 25, this is 0.028 Nm.

Note, running the stepper motor at a given angular-acceleration rate implies accelerating and decelerating it, vs immediately starting it out at 250 RPM.  That is, you would start out sending a fairly slow train of pulses to the stepper; during 1.5 seconds increase to maximum rate of 500 RPM; then for 1.5 seconds decrease the pulse rate back to zero.  Without stepper acceleration and deceleration, you risk losing pulses, which would lead to inaccuracy, due to exceeding stepper motor torque limits.  [Note: edit 1 revised this paragraph]

Note, it would make sense to have an encoder attached to the large wheel, to track its position rather than depending on open-loop operation of the stepping system.


答:
You only have to do simple calculations.

We have done it with different hardware: first with a Lego mindstorm to prototype and then with a powerfull myRIO.

The hardware used:

Prototype: Balancing on four wheels

Final device: Scalevo - The Stairclimbing Wheelchair - ETH Zurich

Title: Two-wheeled self-balancing robot - Choosing the control system

Body:
Would an Arduino Uno have the precision-timing required (using only firmware) to control a two-wheeled inverted pendulum robot, or would it need a RTOS?

Note: Thanks for both answers, they both helped a lot.  I just chose the last answer as the accepted answer.


问:
Telescoping linear actuator
I need to find a linear actuator that can extend to multiple times it's length. I am going to be fixing the actuator horizontally and it will carry a light vertical load.

So far I have thought of using something similar to the mechanism in a scissor lift. However, this is intended for a CNC application and needs high precision, and I'm not sure a scissor lift design would be rigid enough vertically when place horizontal. The system will eventually be feedback controlled so some give can be tolerated.

Is there something other than a scissor lift design that would be better suited to this application?

答:
You could use a linear motor like this one we use on our wheelchair(to push the seat up):

https://youtu.be/3lb_8nmy90c

Thouse motors are calles lamba motors and are made from phoenix mecano.

The have smaller and bigger one, the new version of the wheelchair has 1 stronger one.

Title: Telescoping linear actuator


问:
What's the difference between feedback and feedforward control?
I'm reading from Astrom & Murray (2008)'s Feedback Systems: An introduction for scientists and engineers about the difference between feedback and feedforward.  The book states:

Feedback is reactive: there must be an error before corrective actions are taken.  However, in some circumstances, it is possible to measure a disturbance before the disturbance has influenced the system.  The effect of the disturbance is thus reduced by measuring it and generating a control signal that counteracts it.  This way of controlling a system is called feedforward.

The passage makes it seem that feedback is reactive, while feedforward is not.  I argue that because feedforward control still uses sensor values to produce a control signal, it is still reactive to the conditions that the system finds itself in.  So, how can feedforward control possibly be any different from feedback if both are forms of reactive control?  What really separates the two from each other?

A illustrative example of the difference between the two would be very helpful.

答:
Sr. no  Point of Difference Feedback control system Feed Forward Control system:

Definition

Systems in which corrective action is taken after disturbances affect the output

Systems in which corrective action is taken before disturbances affect the output

Necessary requirement

Not required

Measurable Disturbance or noise

Corrective action

Corrective action taken after the disturbance occurs on the output.

Corrective action taken before the actual disturbance occurs on the output.

Block Diagram

enter image description here

enter image description here

Control Variable adjustment

Variables are adjusted depending on errors.

Variables are adjusted based on prior knowledge and predictions.

Example

Use of roll sensor as feedback element in ship stabilization system.

Use of flowmeter as feed forward block in temperature control systems.


问:
Why the IMU measurements only accumulate drift in 4DOF
I'm studying the Technical Report of VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator available at: https://github.com/HKUST-Aerial-Robotics/VINS-Mono/blob/master/support_files/paper/tro_technical_report.pdf

In section V, where Loop Closure is discussed is writen:
"The IMU measurements render roll and pitch angle fully
observable, so the accumulated drift only occurs in four
degrees-of-freedom (x, y, z and yaw angle). To avoid importing
spurious information, we directly optimize pose graph on these
four degrees-of-freedom."

I'm having trouble to understand this, in my conception, imu noise should propagate in all degrees of freedom. This might have something to do with the gravity vector?

Thank You All

答:
Correct, it's all about the gravity vector. I would argue that it is NOT necessarily possible to get the gravity vector out of the measurement, but that's because it's possible to put the IMU in a constant acceleration state that would skew the gravity vector (i.e. a rotation).

Check out Sebastian Madgwick's paper on IMU filtering.

Basically, if it's level, z-axis reads gravity and x- and y-axes read zero. As you tilt the IMU, the gravity vector projects onto the associated axis. Gravity is a constant (hand waving) so you know the input vector (0;0;g), the output vector, and you solve for the rotation matrix that gives you the roll and pitch values.

The trouble is that gravity isn't the same everywhere, may slowly shift over time (tidal motion), and it's not possible to discern gravitational acceleration from external acceleration. The solution seems to be to normalize the accelerometer outputs and change the input vector from (0;0;g) to (0;0;1).

This means that you wind up skewing the gravitational vector's orientation, but the effects seem to be minimal with regards to orientation. I haven't tried on a rotating platform though.


问:
How can I detect if a DC motor on a robot is starting to fail?
What characteristics can I look for which could be reliable early warning signs that a DC motor on my robot, say one used for the drive, could be failing? I'm looking for an answer that deals in terms of sensors rather than manual inspection, so a circuit could be constructed to warn of a potential failure before it happens.

I have a few ideas such as an increase in current draw or decrease in rotation speed / voltage, but I want to guard against false warnings caused by reasonable wear and tear, or just the robot struggling on tough terrain.

Obviously such a system will never be foolproof, but are there any points I can look out for?

答:
Temperature is a very simple number to measure and is a good aggregation of all the other factors since a weakening motor will be driven harder.

Generally a motor that is about to fail will be significantly hotter than the rest of the motors.


答:
This is the method I have found to build the libfreenect for beaglebone:

http://elinux.org/ECE497_Project_BeagleBoard_Kinect_with_Usable_Framerates

Summarised below:

Kinect Drivers--libfreenect

Full information about libfreenect and its installation can be found at its site, here.
  Here I've reproduced just the sections that are useful for getting the drivers set up and working on the BeagleBoard

Clone the git repository with all of the libfreenect source code in it

git clone git://github.com/OpenKinect/libfreenect.git

If you don't have git already installed on your BeagleBoard, install it with the following command

opkg install git

Get all of the packages you need for building the drivers
  To get the basic drivers compiled, you'll need the cmake and libusb-1.0-0 packages. Use the following command to install these packages to your BeagleBoard

opkg install cmake libusb-1.0-0

NOTE: For some reason on our system the cmake package failed to install the first time, it's some issue with ncurses and dependent libraries. We just reran the command to install cmake and it worked the second time.

Run the cmake utility to generate the necessary makefiles
  Go into your libfreenect directory and create a subdirectory named build

cd libfreenect

mkdir build

Run cmake

cmake ..

Make the tiltdemo

make tiltdemo

NOTE: If you try to make everything, by just running make like it says in the libfreenect instructions, your make will fail with a bunch of linker errors. This is because there is no GLUT library for the BeagleBoard, so the video demos fail when they try to link against GLUT. We'll be solving this problem later by replacing GLUT calls with PVR calls, but for now, go ahead and make the tiltdemo to be sure you have connectivity with the Kinect

Run the tiltdemo
  Go to the bin folder and

./tiltdemo

NOTE: You may have to

chmod +x tiltdemo

to get it to run. Now you should see the Kinect tilting up and down and cycling through the various LED options (Red, Green, Blink, Off). Press ctrl+c to exit the demo

Title: How can I get Windows Kinect working on Angstrom on Beaglebone?

Body: I have tried following a number of guides on the internet but most of them fall down as libfreenect does not exist in opkg, which is the apt-get of Angstrom. Has anyone got it working and if so what is the method?</p>


问:
Software for mission planning in multi-robot systems
I am interested in mission planning for multi-robot systems. Given multiple robots and multiple tasks and an environment, I need to specify missions and software should plan for the robot team to accomplish the mission.

To be more precise, tasks are just a bunch of waypoints with or without time-stamps or deadlines. More elaborately, a task in abstract sense is something like patrol location A using two robots which essentially can be coded up as two sets of way-points or trajectories for the robots. hence the assertion above that tasks can just be viewed as a bunch of waypoints.

So there are multiple tasks and what tasks have to be executed in which order has to be planned by user or software so as to fulfil a mission. I am looking for Github repositories where people have tackled such problems to take inspiration. I am open to any software framework.

As a prime example of the kind of work or software I am looking for, FLYAQ - An open source platform for mission planning of autonomous quadrotors is an example. Please share any code or PDF links, if possible.

答:
First, it's important to note that depending on your problem specifications, we may not know of any good algorithms to solve it - this is especially true when you start to add in more problem constraints.

There are three levels of abstraction here - your question primarily deals with the highest level, but FLYAQ covers all three so I include them here as well.

High level: Task assignment and ordering

Given a list of tasks, constraints, and costs, we first find the best set of ordered sets of waypoints for each robot in your team respects the constraints. This is generally called the "vehicle routing problem", and has many variants. One open source software is  OptaPlanner. The HeuristicLab also has an impressively mature software stack for these problems. You could also consider solving these problems using a linear program solver such as Gurobi or open source alternatives. If you have a more specific problem in mind, create another question and I'll post an answer (this is my area of research).

Mid-level: Trajectory planning

Once a robots has been assigned an ordered set of waypoints to visit, it still needs to decide how to connect the dots. Depending on your hardware this might already be abstracted away (e.g. with MotionPlanner and Ardu* planners). If you need to solve this problem, there are a few very good libraries for this, such as Open Motion Planning Library and Drake. The output of this layer is typically a polynomial or spline representation of a trajectory that is guaranteed to be collision free.

Low level: Rate control

At the lowest level the robot needs to translate a desired trajectory into rate commands for its actuators. This is entirely dependent on your hardware platform, and is often abstracted away. If you already have hardware, then see if a google search yields an example of how to go from a waypoint list or trajectory to rate commands.

Putting everything together

A typical software set-up will separate each level of abstraction into different applications, since they run at different rates (typically high level is ~0.1-1Hz, mid level is 1-10Hz, low level is 10-100Hz). One very popular middleware is the Robot Operating System, which has an active community and many tutorials.

This answer is vague by necessity - your question covers a range of topics which has thousands of published work on. If you have a more specific problem or abstraction layer in mind, there are definitely more specific answers available.


问:
I need help figuring out what motor is used in this design
I am learning how to make rigs for high speed motion control. Basically, whatever action needs to be executed must be done by a rig that is triggered at an exact moment.

In this particular case, I need something that pulls out extremely fast, so that the element that is on top falls without any alteration.

In these two pictures I've highlighted the rig that is being used:

Zoomed in on the relevant sections:

This action happens in 1/2 a second, give or take.

What is the motor capable of pulling out that fast, without altering the object on top?

In this case the burger bun falls intact, it lands perfectly.

答:
This looks to me like a linear pneumatic actuator.

Pneumatic cylinders can be very fast acting, depending on the air pressure on hand, but they can be hard to control at any position other than their end stops, so they are usually used to switch between a limited number of fixed positions. Although the actuators themselves can be small, they require significant infrastructure to run (air compressors, filters, pipes, valves, pressure gauges & limiters etc.).

Pneumatic cylinders are typically single or double acting:

Simple pneumatic actuators have valves which direct either atmospheric pressure, or pressurised air into the cylinder, so you might have one pneumatic solenoid valve for a single acting cylinder, while for a double acting cylinder you might have two pneumatic solenoid valves or a single three-way value depending on whether you want to pre-exhaust for faster actuation.

To answer the other part of your question. This system basically performs the Tabelcloth Trick with a burger bun. All you really need is for the mass of the bun, and thus it's inertia, to be high enough that it stays in place against the friction of the mechanism which hold it, for the time it takes for that mechanism to get out of the way.

The higher the jolt (or jerk) of the move, the more likely it is that the kinetic friction will dominate over the (usually higer) stiction (static friction), and a pneumatic actuator with no flow limiter has the potential to have quite a high jolt coefficient.


问:
What is the Name of the part I'm describing
I'm looking for a part that will do a particular function.  It has to be able to move along one axis and tell me the force that is being exerted on it.

Kind of how like a piston moves inside an engine (one axis of movement) except that something will be pushing at the top of the piston and I need to know how hard its pushing.  Another difference is that the piston won't be constantly moving back in forth, but needs to be able to receive commands like.  move x centimeters forward and then remain stationary at its new position.

I know to make this it would involved a sensor and something that can exert force but what is the name of the described machine?

Edit #1 - Response to Matthew Gordon

The piston would have to move between 0-6 centimeters.  The form factor would be small, ideally smaller than the palm of your hand.  (Smaller=better) The forces it would have to deal with are comparable to the forces exerted on a bicycle by its chain.  I'm a math/cs person not engineering so I don't know technical terms for these kinds of things off the top of my head.  It would have to be real time sensor reading, but the volume of data could be processed by a phone.  Would have to working in conjunction with wireless communication, probably Bluetooth, but I'd have to look into the latency requirements to be sure.

答:
Linear servo actuator, combined with a load-cell, inline, to measure the load that is transferred between the actuator and the load that it is connected to.

The main advantage of a load cell is that it gives analog output (voltage) that can be sensed. They don't require bridges to convert weak signals to senseable signals as do the strain gauges.

This combination is used for advanced robots, that have to be rapid for better control and also especially for back-driving the robot's manipulators.


问:
How can reading a quadrature encoder fail?

I am trying to the read the quadrature encoder on a ServoCity 624 RPM Premium Planetary Gear Motor w/Encoder with a SparkFun ESP32 Thing.

I am supposed to see 228 counts per revolution, but I see 230-232 instead.

Here is my Arduino code:

```<code>// Global variables modified by ISR
int state;
unsigned int count;

int error;

// ISR called on both edge of quadrature signals
void handleInterrupt()
{
  // Shift old state into higher bits
  state = ( state << 2 ) & 15;

  // Get current state
  if( digitalRead(34) ) state |= 2;
  if( digitalRead(35) ) state |= 1;

  // Check state change for forward or backward quadrature
  // Flag any state change errors
  switch( state )
  {
    case 1:
    case 7:
    case 14:
    case 8:
      count--;
      break;
    case 11:
    case 13:
    case 4:
    case 2:
      count++;
      break;
    default:
      error++;
  }
}

void setup()
{
  pinMode(33, OUTPUT); // PWM
  pinMode(32, OUTPUT); // DIR

  pinMode(34, INPUT_PULLUP); // QB
  pinMode(35, INPUT_PULLUP); // QA

  attachInterrupt( digitalPinToInterrupt(34), handleInterrupt, CHANGE );
  attachInterrupt( digitalPinToInterrupt(35), handleInterrupt, CHANGE );

  Serial.begin(74880);
}

void loop()
{
  int new_state;
  int old_state;

  // Start motor
  digitalWrite(32, HIGH); // clockwise
  digitalWrite(33, HIGH);

  // 57*4-17 found by trial and error to make a complete revolution
  for( int i = 0; i < (57*4-17); i++ )
  {
    // Busy wait for change
    do
    {
      // Get current state
      new_state = 0;
      if( digitalRead(34) ) new_state |= 2;
      if( digitalRead(35) ) new_state |= 1;
    }
    while( old_state == new_state );
    old_state = new_state;
  }

  // Stop motor
  digitalWrite(33, LOW);
  delay( 1000 );

  Serial.print( " state=" );
  Serial.print( state );
  Serial.print( " count=" );
  Serial.print( count % 228 );
  Serial.print( " error=" );
  Serial.println( error );
}
</code>```

On my scope, quadrature signals are very clean

I instrumented the code to look at interrupts and they appear to be in the right places and not too close together.

I don't see mechanical slipping, and it would be obvious because
over 100 revs, the count slips an entire revolution.

But the bottom line is: How can this be failing?  If the CPU is missing transitions, I would get illegal transitions and errors.  Noise would also cause errors.  But I am not getting any errors at all (except a single error at startup).

答:
ServoCity 624 RPM Premium Planetary Gear Motor w/Encoder has the encoder on the motor and a 19:1 gear reduction on the output shaft.

Mechanical tolerance in the gearbox accounts for the difference between 228 counts per revolution that I expected and 230.7 counts per revolution that I measured.


问:
How to measure and dispense a finite amount of powder or liquid
I've been watching too much How It's Made, and I've been wondering how they build devices that spray/inject/dispense a finite amount of liquid (to within some amount of error).  I wanted to try this for a hobby project. I'm working on that dispenses dry goods in the amount I specify.

Do I use some kind of special nozzle/valve which can open and close at high speeds? How can I dispense a known quantity from a reservoir of a fluid substance onto each individual unit passing along an assembly line, or an amount specified by the user into another container?

答:
Flow-rate meters based on measuring Coriolis-force reactions are widely used in process control systems.
The measurement principles are mentioned or illustrated at numerous web sites, including youtube video e2NGvrkmP8Y, Back to Basics: Flow Measurement (Corilolis) (via oscillating-tube phase changes) and Schenck Americas' Solids Flow Meters page, Coriolis Principle section (via vaned-rotor reaction forces) .  Coriolis-based meters measure the delivered mass of solids, liquids, and gases.  Delivered volume can be calculated if density is known.  Purchase of a Coriolis-based meter seems far more feasible than building one from scratch.

The Schenck page also illustrates impact-plate-based measuring systems and deflection-chute-based measuring systems.  These integrate impulses or deflection force to measure flow.  Again, purchase seems more feasible than building one yourself.

At a hobby level, I think two approaches should be considered first:  (A) calibrate a constant-flow-rate system, and control delivery time; (B) implement a fill-and-dump system, and control number of cycles.

(A) Using a method from previous questions (1,2) set up a system that moves your substance at a reasonably constant rate; measure that rate; thereafter, control how long or how fast the system runs to deliver a desired amount of substance.

(B) Again using methods from previous questions, set up a system to fill a cup, box, chute, or transfer bin some number of times, and dump it into a receiver.  Issues like detecting when the cup is full, preventing or recycling overflow of the cup, and dumping the cup arise but can be resolved.  The issue of limited resolution (where the delivered amount is an integer multiple of cup size) can be treated by using a smaller cup, or using different cups for different jobs, or using a box with an adjustable side or bottom, or having the box sit on a load cell during filling.


问:
MPU9255 gyro data to roll pitch?
So I have -32768 to +32768 coming out from MPU9255 (gx,gy,gz) which is converted to 0-250 dps(degrees per second) using 131 which is Gyro's sensitivity.

My question would be how do you use this data to convert it into Roll and Pitch?

I am trying to make a stabiliser. I have tried using
$$
\theta = \sum^n_n\omega*\delta(t)
$$

where n = infinity
I don't if my equation is wrong or not here is my code:

```<code>dt = now_c - pr_dt;
pr_dt = dt;
Pitch_gyro += Gxyz[1]*(dt/1000000.0);
Roll_gyro += Gxyz[0]*(dt/1000000.0);
</code>```

This is my function:

```<code>Gxyz[0] = (double)(gx/131);//131;
Gxyz[1] = (double)(gy/131);//131;// 250/32768.0;
Gxyz[2] = (double)(gz/131);//131;// 250/32768.0;
</code>```

Any guide as to how to solve this I have looked into euler angles. I still don't understand how you get the angles given angular velocity which is from gyro.

答:
For those of you looking for How to read gyro data from MPU9255. I manage to solve it some what. Like the answer above said you need the gyro bias to be initialised. I have looped it 2000 times to get reading from gyro then averaged it to find a good value for the bias. I myself don't understand it properly why it work. But to put it blandly, to get the angular rate you simply divide it by 131 given that you are using 250 dps check the link to MPU9255 register maps sheet. Then you further divide it by your sample rate which should be 8000 by default again that is if you are using 250 dps it is also the default setting when you initialise MPU9255. I was just tinker around with the code to give me the right values and it seems to do that. I do have a problem with the drift. So if anyone can point me in the right direction. I don't know if I am implementing complementary filter the right way or not, I came across a normal filterish don't know what it's called that seems to work pretty well. However it assumes that the value from accel is absolute or it gives pretty precise.

```<code>#include "Arduino.h"
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU9250.h"

MPU9250 accelgyro;
volatile float Gxyz[3] = {0,0,0};
volatile float Axyz[3] = {0,0,0};
//raw acc, gyro, compass values
int16_t ax,ay,az,gx,gy,gz,mx,my,mz = 0;
//gyro calibration
volatile float G_cal_x,G_cal_y,G_cal_z = 0;
volatile float angle_x,d_angle_x,angle_y,d_angle_y = 0;
//accel and complimentary variables
volatile float Roll, Pitch, Roll_output, Pitch_output = 0;
//PID compute variables
volatile float Kp = 0;
volatile float Output,error,Set_point,Input_angle = 0;

void get_gyro();
void get_accel();
void PID_compute();

void setup()
{
  Wire.begin();
  //initialise the clock speed, gyroscope, accel,
  accelgyro.initialize();
  //serial communication begin
  for(int i = 0; i < 2000; i++)
     {
       get_gyro();
       G_cal_x += Gxyz[0];
       G_cal_y += Gxyz[1];
       G_cal_z += Gxyz[2];
       delay(3);
     }
  G_cal_x /= 2000;
  G_cal_y /= 2000;
  G_cal_z /= 2000;
  Serial.begin(115200);
}

void loop()
{
 //gets gyro and accelerometer values
  get_accel();
  get_gyro();

  //gyro to roll and pitch calculation d_angle_x and d_angle_y are roll and pitch
  Gxyz[0] = Gxyz[0] - G_cal_x;
  Gxyz[1] = Gxyz[1] - G_cal_y;
 //here is the sample rate and 131 value which is 0.000000954 it seem
 //when you call accel function as well you need to multiply it by 2
  angle_x += Gxyz[0]*(0.000000954*2);
  angle_y += Gxyz[1]*(0.000000954*2);

  d_angle_x = angle_x*180;
  d_angle_y = angle_y*180;
  //accel calculation
  Roll = (float)atan2(Axyz[1],Axyz[2])*(180/3.1415);
  Pitch = (float)(-atan2(-Axyz[0], sqrt((Axyz[1] * Axyz[1]) + (Axyz[2] * Axyz[2]))) * (180 / 3.1415));
  //complimentary filter
  Roll_output = 0.98*(0.1*Roll_output+0.9*d_angle_x) + 0.02*Roll;
  Pitch_output = 0.98*(0.1*Roll_output+0.9*d_angle_y) + 0.02*Pitch;

  //Normal filter
  // Roll_output = d_angle_x + 0.75(Roll-d_angle_x);
  // Pitch_output = d_angle_y + 0.75(Pitch-d_angle_y);

  // printing to see the values
  Serial.print(d_angle_x);
  Serial.print(" ");
  Serial.print(d_angle_y);
  Serial.print(" ");
  Serial.print(Roll_output);
  Serial.print(" ");
  Serial.print(Pitch_output);
  Serial.println();
}

//initialising or getting the gyro data
void get_gyro()
{
  accelgyro.getMotion9(&ax,&ay,&az,&gx,&gy,&gz,&mx,&my,&mz);
  Gxyz[0] = gx;
  Gxyz[1] = gy;
  Gxyz[2] = gz;
}

void get_accel()
{
  accelgyro.getMotion9(&ax,&ay,&az,&gx,&gy,&gz,&mx,&my,&mz);
  Axyz[0] = (float)ax/16384;
  Axyz[1] = (float)ay/16384;
  Axyz[2] = (float)az/16384;
}
// Proportional Intergal and differentiate
void PID_compute()
{
 error = Set_point - Input_angle;
 Output = Kp * error;
}
</code>```

https://stanford.edu/class/ee267/misc/MPU-9255-Register-Map.pdf

Title: MPU9255 gyro data to roll pitch?

Body:
For those of you looking for How to read gyro data from MPU9255. I manage to solve it some what. Like the answer above said you need the gyro bias to be initialised. I have looped it 2000 times to get reading from gyro then averaged it to find a good value for the bias. I myself don't understand it properly why it work. But to put it blandly, to get the angular rate you simply divide it by 131 given that you are using 250 dps check the link to MPU9255 register maps sheet. Then you further divide it by your sample rate which should be 8000 by default again that is if you are using 250 dps it is also the default setting when you initialise MPU9255. I was just tinker around with the code to give me the right values and it seems to do that. I do have a problem with the drift. So if anyone can point me in the right direction. I don't know if I am implementing complementary filter the right way or not, I came across a normal filterish don't know what it's called that seems to work pretty well. However it assumes that the value from accel is absolute or it gives pretty precise.

#include "Arduino.h"
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU9250.h"

MPU9250 accelgyro;
volatile float Gxyz[3] = {0,0,0};
volatile float Axyz[3] = {0,0,0};
//raw acc, gyro, compass values
int16_t ax,ay,az,gx,gy,gz,mx,my,mz = 0;
//gyro calibration
volatile float G_cal_x,G_cal_y,G_cal_z = 0;
volatile float angle_x,d_angle_x,angle_y,d_angle_y = 0;
//accel and complimentary variables
volatile float Roll, Pitch, Roll_output, Pitch_output = 0;
//PID compute variables
volatile float Kp = 0;
volatile float Output,error,Set_point,Input_angle = 0;

void get_gyro();
void get_accel();
void PID_compute();

void setup()
{
  Wire.begin();
  //initialise the clock speed, gyroscope, accel,
  accelgyro.initialize();
  //serial communication begin
  for(int i = 0; i < 2000; i++)
     {
       get_gyro();
       G_cal_x += Gxyz[0];
       G_cal_y += Gxyz[1];
       G_cal_z += Gxyz[2];
       delay(3);
     }
  G_cal_x /= 2000;
  G_cal_y /= 2000;
  G_cal_z /= 2000;
  Serial.begin(115200);
}

void loop()
{
 //gets gyro and accelerometer values
  get_accel();
  get_gyro();

  //gyro to roll and pitch calculation d_angle_x and d_angle_y are roll and pitch
  Gxyz[0] = Gxyz[0] - G_cal_x;
  Gxyz[1] = Gxyz[1] - G_cal_y;
 //here is the sample rate and 131 value which is 0.000000954 it seem
 //when you call accel function as well you need to multiply it by 2
  angle_x += Gxyz[0]*(0.000000954*2);
  angle_y += Gxyz[1]*(0.000000954*2);

  d_angle_x = angle_x*180;
  d_angle_y = angle_y*180;
  //accel calculation
  Roll = (float)atan2(Axyz[1],Axyz[2])*(180/3.1415);
  Pitch = (float)(-atan2(-Axyz[0], sqrt((Axyz[1] * Axyz[1]) + (Axyz[2] * Axyz[2]))) * (180 / 3.1415));
  //complimentary filter
  Roll_output = 0.98*(0.1*Roll_output+0.9*d_angle_x) + 0.02*Roll;
  Pitch_output = 0.98*(0.1*Roll_output+0.9*d_angle_y) + 0.02*Pitch;

  //Normal filter
  // Roll_output = d_angle_x + 0.75(Roll-d_angle_x);
  // Pitch_output = d_angle_y + 0.75(Pitch-d_angle_y);

  // printing to see the values
  Serial.print(d_angle_x);
  Serial.print(" ");
  Serial.print(d_angle_y);
  Serial.print(" ");
  Serial.print(Roll_output);
  Serial.print(" ");
  Serial.print(Pitch_output);
  Serial.println();
}

//initialising or getting the gyro data
void get_gyro()
{
  accelgyro.getMotion9(&ax,&ay,&az,&gx,&gy,&gz,&mx,&my,&mz);
  Gxyz[0] = gx;
  Gxyz[1] = gy;
  Gxyz[2] = gz;
}

void get_accel()
{
  accelgyro.getMotion9(&ax,&ay,&az,&gx,&gy,&gz,&mx,&my,&mz);
  Axyz[0] = (float)ax/16384;
  Axyz[1] = (float)ay/16384;
  Axyz[2] = (float)az/16384;
}
// Proportional Intergal and differentiate
void PID_compute()
{
 error = Set_point - Input_angle;
 Output = Kp * error;
}

https://stanford.edu/class/ee267/misc/MPU-9255-Register-Map.pdf


问:
Find the robot pose from three beacon measurement
At every timestep my robot gets sensor measurements from a scanner that finds three beacons with known poses $B_1 = (B_{1x}, B_{1y})^T, B_2 = (B_{2x}^T, B_{2y}) , B_3 = (B_{3x}, B_{3y})^T$ these measurements include the distance and angle to the beacon, the measuremnt for $B_1$ would be $m_{1t} = (d_{it}, \omega_{1t})^T$. and equivalently for the other beacons.

From these measurements i want to calculate the robots pose containing its position and orientaion $x_t = (p_{xt}, p_{yt}, \Theta_{xt})^T$.

Calculating the position can be done by trilateration, but I can't seem to find a way to get the orientation of the robot from these measurements. Is there possible a model to calculate both in a single calculation?   If not a solution for finding the orientation of the robot would be great.

答:
If you have already computed the position of the robot, a single(!) angle towards a beacon is sufficient to get its orientation.

The computation could look roughly like

// b1 is position of beacon1, r is computed position of robot
alpha = atan2(b1.x-r.x, b1.y-r.y)

So if the robot is looking towards east, the angle-measurement of B1 should alpha. So roughly alpha-b1.w is your robot's orientation.