问:
Multiple robots in ROS Gazebo SITL with separate MAVlink/MAVproxy codes

Background:

This question is about simulating one ErleCopter and one ErleRover simultaneously for a non-commercial research. I would like to have the quadcopter follow a rover, which in turn is to be tasked with following a line. I am trying to spawn the vehicles in Gazebo and control them using MAVProxy.

The Problem:

Any time I try this, I run into one of two problems:

Spawning the second vehicle terminates the first MAVProxy instance, or

The second vehicle spawned cannot be linked to the second instance of MAVProxy.

I'm not sure what to do about this because I am not sure if this problem is one problem or if it's composed of two sub-problems. The first problem is in spawning the robots and the second one is in controlling both independently (and obtain the state parameters of both vehicles and use that as feedback).

I believe a contributing factor to this problem is that I'm trying to do the simulation and both MAVProxy instances on one computer, a Lenovo-y50-70 is being used with Ubuntu 14.04. Two computers are not easy to obtain immediately and there are network stability issues where I am.

The Question:

The entire question probably reduces to "How to link second robot spawned by rosrun to second MAVproxy instance?".

Desired Outcome:

I would like help either getting the simulation to run as desired (two vehicles co-simulated in one virtual world with two MAVProxy instances, one linked to each vehicle), OR official documentation somewhere that this is not possible.

What I've Attempted:

Initial attempts can be seen in earlier edits of this question. For clarity, that information has been removed, but again, if interested, see an earlier version of this question. Fast-forwarding to the relevant:

The second robot has been spawned successfully as mentioned in this video by using the commands mentioned below.

```<code>cd path_to_urdf_model_files
rosrun gazebo_ros spawn_model -file rover.urdf -urdf -model rover_object
</code>```

Note: ROS Indigo is different; gazebo_ros must be used instead of gazebo_worlds

The weird behaviour of the robot rotating about itself is probably because of MAVproxy; I have experienced this before. Attempt to establish separate network connections to Copter and Rover has been successful so far. The original structure is as shown below:

rover_circuit.launch -> apm_sitl.launch -> node.launch (node name:
  "mavros")

The current architecture is as shown below:

copter_circuit.launch -> apm_sitl_copter.launch -> node_copter.launch
  ("mavros_copter")

rover_circuit1.launch -> apm_sitl_rover.launch -> node_rover.launch  ("mavros_rover")

rover_ciruit1.launch is as shown below:

```<code><launch>

  <include file="$(find mavros)/launch/apm_sitl_rover.launch"></include>
  <arg name="enable_logging" default="true"/>
  <arg name="enable_ground_truth" default="true"/>
  <arg name="log_file" default="rover"/>
  <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
  <arg name="model" default="$(find ardupilot_sitl_gazebo_plugin)/urdf/rover.urdf"/>

  <param name="robot_description" command="
    $(find xacro)/xacro.py '$(arg model)'
    enable_logging:=$(arg enable_logging)
    enable_ground_truth:=$(arg enable_ground_truth)
    log_file:=$(arg log_file)"
  />
  <param name="tf_prefix" type="string" value="$(arg tf_prefix)" />

  <node name="spawn_rover" pkg="gazebo_ros" type="spawn_model"
    args="-param robot_description -urdf -model 'rover' " respawn="false" output="screen"></node>

</launch>
</code>```

This is the minimal launch file, and it works. I had thought of rosrun apm_sitl_rover.launch followed by rosrun rover.urdf, but I have been unable to find a suitable package which launches apm_sitl_rover.launch directly. It is easy to roslaunch a launch file that has an  method appended.

Naming issues and other network errors have been resolved.

Outstanding Problem Remaining:

I'm still having issues launching and linking the second vehicle, but now it seems like I've narrowed it down such that the only problem is with UDP bind port, which is 14555 by default, and this is crashing Gazebo for the second instance because the second instance is using the same bind port.

It looks like libmavconn is getting called somehow, particularly interface.cpp which has url_parse_host(bind_pair, bind_host, bind_port, "0.0.0.0", 14555); and the udp.h included in interface.cpp has a function MAVConnUDP() which has bind_port=14555, and this has resulted in "udp1: Bind address: 14555" and "GCS: DeviceError:udp:bind: Address already in use".

Trying to assess the connection between sim_vehicle.sh, libmavconn, and Gazebo, I was able to figure out that sim_vehicle.sh calls mavproxy.py in one of the ending lines which in turn uses pymavlink. I have been unable to find further relationship currently.

Leading Questions / An Approach to the Solution

As I have a strong intuition that this is the final stage, I currently resolve to fix this by using interface_copter.cpp and interface_rover.cpp. I think, if I could get answers to the following questions, I can work out where the failure is in successfully launching and link the second (or subsequent) vehicles:

How does sim_vehicle.sh trigger the libmavconn package and ultimately Gazebo?

Is there a software architecture diagram which describes the complete structure from sim_vehicle.sh to joints and controllers?

答:
I feel like I'm having to do a lot of work to understand this question. You are trying to simulate two vehicles, and... what's the issue? You are trying to simulate them in Gazebo, right?

Have you seen this answer on the ROS website that shows launching two robots in one simulation? As I mention, it's very difficult to read your post, which may explain why it's received such little attention. For instance, what is the first code block you show? What is the second code block?

In both code blocks, you have the following lines:

```<code><arg name="model" default="$(find ardupilot_sitl_gazebo_plugin)/urdf/erlecopter_base.xacro"/>
<arg name="model" default="$(find ardupilot_sitl_gazebo_plugin)/urdf/rover.urdf"/-->
</code>```

BOTH code blocks have these lines. That is, you have "model" assigned to the Erlecopter, then you have "model" assigned to the Rover. Both instances of "model" occur in the same namespace, so I'm not sure how they would be distinguished.

Again I would refer you to the answer on the ROS forums

I'll reproduce part of the article here for posterity for future visitors:

The answer is based on the turtlebot robot, but of course it can be used with any robot(s).

For multiple robots the key point is to manage the namespaces and tf_prefixes. Although usually we will set them to the same value for each robot in this example I will you different for purpose.

In setting up the navigation stack(s) each robot must operate in it's own namespace, but they do have to share the map (and usually robot_description).

(emphasis added)

Simulation
  First of all we have to launch the simulation, in our case gazebo. To make things more readable I split launch files in few files.

Launch file for one robot. We name it one_robot.launch:

```<code><launch>
    <arg name="robot_name"/>
    <arg name="init_pose"/>

    <node name="spawn_minibot_model" pkg="gazebo" type="spawn_model"
     args="$(arg init_pose) -urdf -param /robot_description -model $(arg robot_name)"
     respawn="false" output="screen" />

    <node pkg="robot_state_publisher" type="state_publisher"
          name="robot_state_publisher" output="screen"/>

    <!-- The odometry estimator, throttling, fake laser etc. go here -->
    <!-- All the stuff as from usual robot launch file -->
</launch>
</code>```

Two things to mention for above launch file:

Create two arguments to pass for robot initialization: name and position

Notice that -param /robot_description is with the slash i.e. fully-qualified, as it will be shared.

Having the robot declaration we can now play with the namespaces. Your main launch file for multiple robots should look like (robots.launch):

```<code><launch>
  <!-- No namespace here as we will share this description.
       Access with slash at the beginning -->
  <param name="robot_description"
    command="$(find xacro)/xacro.py $(find turtlebot_description)/urdf/turtlebot.urdf.xacro" />

  <!-- BEGIN ROBOT 1-->
  <group ns="robot1">
    <param name="tf_prefix" value="robot1_tf" />
    <include file="$(find your_pkg)/launch/one_robot.launch" >
      <arg name="init_pose" value="-x 1 -y 1 -z 0" />
      <arg name="robot_name"  value="Robot1" />
    </include>
  </group>

  <!-- BEGIN ROBOT 2-->
  <group ns="robot2">
    <param name="tf_prefix" value="robot2_tf" />
    <include file="$(find your_pkg)/launch/one_robot.launch" >
      <arg name="init_pose" value="-x -1 -y 1 -z 0" />
      <arg name="robot_name"  value="Robot2" />
    </include>
  </group>
</launch>
</code>```

So I've reproduced here the steps up to launching both robots. The full answer goes into a lot more depth.

Just above the comments for the post is a link that says (more). It expands the rest of the answer.

Be sure to read the full answer as it goes into more detail about steps to take for setting up navigation and visualization.

If you try these steps and get a specific error, please tell us what exactly the error said. Comments like, "This didn't work," or, "This had errors," are not helpful.

I do hope this works for you! If I've missed the mark as to the scope/direction of your question then please try to clarify the exact errors you're getting.

Body:
I feel like I'm having to do a lot of work to understand this question. You are trying to simulate two vehicles, and... what's the issue? You are trying to simulate them in Gazebo, right?

Have you seen this answer on the ROS website that shows launching two robots in one simulation? As I mention, it's very difficult to read your post, which may explain why it's received such little attention. For instance, what is the first code block you show? What is the second code block?

In both code blocks, you have the following lines:

<arg name="model" default="$(find ardupilot_sitl_gazebo_plugin)/urdf/erlecopter_base.xacro"/>
<arg name="model" default="$(find ardupilot_sitl_gazebo_plugin)/urdf/rover.urdf"/-->

BOTH code blocks have these lines. That is, you have "model" assigned to the Erlecopter, then you have "model" assigned to the Rover. Both instances of "model" occur in the same namespace, so I'm not sure how they would be distinguished.

Again I would refer you to the answer on the ROS forums

I'll reproduce part of the article here for posterity for future visitors:

The answer is based on the turtlebot robot, but of course it can be used with any robot(s).

For multiple robots the key point is to manage the namespaces and tf_prefixes. Although usually we will set them to the same value for each robot in this example I will you different for purpose.

In setting up the navigation stack(s) each robot must operate in it's own namespace, but they do have to share the map (and usually robot_description).

(emphasis added)

Simulation
  First of all we have to launch the simulation, in our case gazebo. To make things more readable I split launch files in few files.

Launch file for one robot. We name it one_robot.launch:

<launch>
    <arg name="robot_name"/>
    <arg name="init_pose"/>

    <node name="spawn_minibot_model" pkg="gazebo" type="spawn_model"
     args="$(arg init_pose) -urdf -param /robot_description -model $(arg robot_name)"
     respawn="false" output="screen" />

    <node pkg="robot_state_publisher" type="state_publisher"
          name="robot_state_publisher" output="screen"/>

    <!-- The odometry estimator, throttling, fake laser etc. go here -->
    <!-- All the stuff as from usual robot launch file -->
</launch>

Two things to mention for above launch file:

Create two arguments to pass for robot initialization: name and position

Notice that -param /robot_description is with the slash i.e. fully-qualified, as it will be shared.

Having the robot declaration we can now play with the namespaces. Your main launch file for multiple robots should look like (robots.launch):

<launch>
  <!-- No namespace here as we will share this description.
       Access with slash at the beginning -->
  <param name="robot_description"
    command="$(find xacro)/xacro.py $(find turtlebot_description)/urdf/turtlebot.urdf.xacro" />

  <!-- BEGIN ROBOT 1-->
  <group ns="robot1">
    <param name="tf_prefix" value="robot1_tf" />
    <include file="$(find your_pkg)/launch/one_robot.launch" >
      <arg name="init_pose" value="-x 1 -y 1 -z 0" />
      <arg name="robot_name"  value="Robot1" />
    </include>
  </group>

  <!-- BEGIN ROBOT 2-->
  <group ns="robot2">
    <param name="tf_prefix" value="robot2_tf" />
    <include file="$(find your_pkg)/launch/one_robot.launch" >
      <arg name="init_pose" value="-x -1 -y 1 -z 0" />
      <arg name="robot_name"  value="Robot2" />
    </include>
  </group>
</launch>

So I've reproduced here the steps up to launching both robots. The full answer goes into a lot more depth.

Just above the comments for the post is a link that says (more). It expands the rest of the answer.

Be sure to read the full answer as it goes into more detail about steps to take for setting up navigation and visualization.

If you try these steps and get a specific error, please tell us what exactly the error said. Comments like, "This didn't work," or, "This had errors," are not helpful.

I do hope this works for you! If I've missed the mark as to the scope/direction of your question then please try to clarify the exact errors you're getting.


问:
Adding magnetic field vector to a Kalman filter
I currently have an error state Kalman filter with the state vector $(p, v, q, \omega, a, g)$ where $q$ is the quaternion orientation. I would like to add the information coming from a magnetometer to this sensor fusion.

I have calibrated the magnetometer and we can assume that we are getting processed data at the point of input to the filter.

How do I extend my state vector to account for the new input, or since I do not directly care about estimating it, should I not include it?

I think that I can initialize my initial state vector correctly by performing TRIAD using the magnetic field vector, is this the right approach?

How does the magnetic field vector help in stabilizing my quaternion attitude?

I tried to search around but I didn't find many resources on how the math works when I include the magnetometer. Any links would be very helpful as well.

答:
I doubt you need to add anything to the state vector regarding the magnetometer calibration. The magnetometer measurement should just be treated at another signal that triggers a measurement/correction update.

Adding the magnetometer makes yaw ( how much you've turned) observable. Without the mag you will just be integrating gyro measurements.

"Three-axis attitude determination via Kalman filtering of magnetometer data"

ContentLicense: CC BY-SA 3.0


问:
How to assemble brushless motors and propellers?
I'm building a quadcopter and I've received my motors and propellers.

What's the right way to assemble those together?

I'm not confident with what I've done, as I'm not sure the propeller would stay in place on a clockwise rotating motor.

I mean, if the motor rotates clockwise, will the screw stay tightly in place, even with the prop's inertia pushing counter-clockwise?

Here's what I've done (of course i'll tighten the screw...) :

答: The ring should be below the propeller. It is designed to keep the three screws in place in case some of the screws come out during the flight.</p>


问:
Where does Gazebo set the GAZEBO_MODEL_PATH environment variable?
I'm starting out with Gazebo (1.5) at the moment and am following a tutorial off the internet. In order to get Gazebo to find the model, the author advocates manually exporting the GAZEBO_MODEL_PATH environment variable via

```<code>export GAZEBO_MODEL_PATH=[...]/models:$GAZEBO_MODEL_PATH
</code>```

But that will only work for the current terminal. So I wanted to change the environment variable permanently.

The Gazebo User Guide claims that GAZEBO_MODEL_PATH, along with all the other environment variables, is set by /usr/share/gazebo-1.5/setup.sh but my (virgin) Gazebo install doesn't list it:

```<code>export GAZEBO_MASTER_URI=http://localhost:11345
export GAZEBO_MODEL_DATABASE_URI=http://gazebosim.org/models
export GAZEBO_RESOURCE_PATH=/usr/share/gazebo-1.5:/usr/share/gazebo_models
export GAZEBO_PLUGIN_PATH=/usr/lib/gazebo-1.5/plugins
export LD_LIBRARY_PATH=/usr/lib/gazebo-1.5/plugins:${LD_LIBRARY_PATH}
export OGRE_RESOURCE_PATH=/usr/lib/i386-linux-gnu/OGRE-1.7.4

# This line is needed while we're relying on ROS's urdfdom library
export LD_LIBRARY_PATH=/opt/ros/fuerte/lib:${LD_LIBRARY_PATH}
</code>```

But when I start Gazebo, GAZEBO_MODEL_PATH is already set to $HOME/.gazebo/models, so it must be set somewhere. I guess I could probably simply add GAZEBO_MODEL_PATH to the setup.sh script, but since it is set somewhere, I'd still like to know where and whether it is better practice to set it in there.

答:
You asked two questions.

Where does Gazebo set the default model path

Where should I set the value

The second question is perfectly right answered by @Armin Meisterhirn. Best practice is to set the variable in the .bashrc file.

The first question, I asked myself as well when starting working with gazebo.

In case of the pure Gazebo (no roslaunch, no plugins) the answer is as simple as that:

In the source code. Gazebo always sets an additional path which is the default value.

Look here.

Line 84:

```<code>this->modelPaths.push_back(home + "/.gazebo/models");
</code>```

As a result, you always have a default path, whatever you do (except you change the source code).

Body:
You asked two questions.

Where does Gazebo set the default model path

Where should I set the value

The second question is perfectly right answered by @Armin Meisterhirn. Best practice is to set the variable in the .bashrc file.

The first question, I asked myself as well when starting working with gazebo.

In case of the pure Gazebo (no roslaunch, no plugins) the answer is as simple as that:

In the source code. Gazebo always sets an additional path which is the default value.

Look here.

Line 84:

this->modelPaths.push_back(home + "/.gazebo/models");

As a result, you always have a default path, whatever you do (except you change the source code).


问:
Do you have to stop a programmable logic controller to update its logic?
Must the operation of a PLC (programmable logic controller) be halted while its logic is being changed or can new logic be downloaded to it during runtime, without memory loss, giving a seamless transition from one operation cycle to other?</p>

答:
The function you are looking is called online change (e.g. Beckhoff Online Change), at least by some PLC Manufacturers. If you change a few blocks (in case of FBD) or a few contacts (in case of LAD) or a few lines of code (in case of IL or ST) you can do it online by most major PLC manufacturers. (Siemens, Beckhoff and the Codesys world has this functionality). Usually you can only change or add some blocks or owerwrite memory addresses. Adding new (large) program parts usually requires arestart. You can only use this functionality if you have the exact version of the PLC program which is running on the PLC.

The sequence is not exactly as you mentioned. You have to open the PLC Program in the IDE and go online (in monitoring mode) with the running PLC. Now you can change blocks and apply the changes. Diffrent PLCs from different manufacurers behave differently, but if you have to press "upload" (or dowload) to PLC button or similar in most cases it will loose data stored in volatile memory.

I would advise to always make a test first with that specific PLC and IDE (same version and firmware if possible) if memory is lost before attempting to change a live system (where critical data can be lost) this way!

Further example:
Siemens S7-200 Edit in Run Mode:


问:
What are these mechanical parts attached to robot arm structures, seen on larger robots?
Between the shoulder and elbow pitch joints, I see two types of connecting structures on larger robots. I've attached a picture.

My questions:

1) What are each of the respective mechanical parts called?

2) What is their purpose for the robot arm?

Thanks!

答:
Both previous answers 'could' be accurate re the "pump looking things" in that they say they are hydraulic springs/dampeners such as you would find on a hatch back rear door, the point of them as mentioned is to reduce the apparent load e.g. if your robots forearm weighs 20Lb and you want to lift a weight of 1Lb the motor would need to apply 21Lb of force but put a 20Lb spring on it and the spring supports the arm and the motor only needs to lift 1Lb.  It is also possible they could be hydraulic or pneumatic actuators (hydraulic using oil or water, pneumatic using gas, normally Co2), the liquid or gas is pushed into the cylinder which then forces the rod forward and the rod moves forward at a greater force than the pressure in the cylinder (bore or diameter of the cylinder is greater than the width of the rod so you achieve mechanical advantage - you find these on JCB diggers or tipper trucks.  You can see both spring and actuator cylinders in the photo of my robot, the spring is the one holding the body shell open and the flipper actuator can been seen at the bottom of the robot.

Again both previous answers are correct re the 'Bit at the back' but just to point out linkages may be powered or unpowered, unpowered linkages are simply used to manage axis at the end of an arm section, in the pic below axis A & B will always remain parallel as will C & D.  If you were to increase the length of D then A would tilt up away from the B axis as A moves forward.  Adding a motor into the linkage can allow finer movement or greater dexterity for the arm.  The linkage in the final pic is being used to drive a spike forward and down as the rear flipper goes up. Just to say there's absolutely nothing wrong with previous answers, I just wanted to let you know there are additional possibilities and show some practical applications


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

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

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

答:
You have different options (priority by difficulty level):

Using a smartphone as a camera.
There are actually apps for this. If you can attach one phone to your robot then you can stream the live video either on another phone or on the web. However the phone attached to the robot should have an internet connection(3G or even Edge). Search for "live stream" in App store or Google Play.

Using an IP Camera.
This option is quite simple and requires you to have an IP camera wich then can be controlled by the Arduino. The IP camera can transmit video wirelessly to your Router or A Raspberry pie. You can then connect another phone or computer to the router to see the live image.

Using a Raspberry Pi.
Id normally would not do video or image manipulations in the Arduino platform because of the low performance it has got. You could connect a Raspberry pi to the Arduino. And a good camera to Raspberry pi. Arduino can send commands to Raspberry pi on when to start transmitting and so on. You can then do image manipulations on the Pi as well. And Since Raspberry pi can work as a router you could connect you smartphone to the raspberry pi through wifi or even Bluetooth. You will get a nice and high FPS video even with text/color manipulations.

You could use the Android ADK as your main controller. And then the Arduino is set up as an accessory. In this way the Arduino can command Android what to do. A cheap android based mobile phone will cost you around 100USD and you will get lots of features for the price. Like GPS, Camera, Battery etc. Most modern smart phone have the ability of creating a wifi hotspot. You can then connect any other wifi platform to that hotspot and control the robot or in your case viewing a live stream video. This is by the most elegant method in my opinion.


问:
Problems connecting to the Granite Devices USB6AX motion controller
I am working on a setup from 2008 with a 6 axis USB motion controller and 4 vsd-e drives very similar to the setup shown here.

The motion controller board is exactly the same as in the first post of these two threads, it also says "USB-SPI 6 axis Rev 0" on the PCB:

Motion controller board example 1.

Motion controller board example 2.

I have installed GDtool and SimpleMotion library. When trying to connect to the device with GDtool, the first step produces the following output in the event log:

```<code>USB mode configured.

Enabling configure mode.
Shell command: OPEN NORMAL

Running OPEN...
Normal mode enabled
>
Got input from SPI shell
Saatiin viesti:  0

Parametreja: 2
 0

USB mode configured.
isBootloaderMode
</code>```

The second step fails with the message "Connection failed. Please check connections." and the following output in the event log:

```<code>Configuring connection
Shell command: OPEN

Running OPEN...
cDevice::sendCommandOnly( 2, 0 )
0x200008a -> 0x0
cDevice::sendCommandOnly( 2, 0 )
0x200008a -> 0x0
cDevice::sendCommandOnly( 2, 0 )
0x200008a -> 0x0
Connection failed.
>
Got input from SPI shell
Saatiin viesti:  0
 0

Parametreja: 3
 0

setConnectionStatus
connected
Connection failed
 0

setConnectionStatus
connected
Connection failed
</code>```

I played around with the example program "SimpleMotionTest" and "FT_Prog" by FTDI to manipulate the USB-controllers Product Description string. The best I could do with SimpleMotionTest was "Communication error. Possibly drive not in SPI mode."

It seems that the axis names are given by the USB Product Description string, because if axis name in SimpleMotionTest and the descriptor string do not match, it says "USB device with given axis not found." This makes sense with the Granite Devices tuning cables but not with a 6 axis controller which can only have one Product Descriptor string.

Is it possible to configure the USB6AX with GDtool? Is it possible to control it with the SimpleMotion library? If yes what am I doing wrong and if no what is the suitable configuration utility and how can I interface the controller with my LabView/C++/... application?

If anyone is out there who still uses the same controller thanks you for sharing you experiences!

答: The USB6AX never was finished, and it is not supported. It cannot be configured with the SimpleMotion field bus.</p>


问:
What kinematic equations should I use for Kalman filter state propagation in IMU based quadcopter navigation?
I am working on implementing a Kalman filter for position and velocity estimation of a quadcopter using IMU and vision. First I am trying to use the IMU to get position and velocity.
In a tutorial [1] the process model for velocity estimation using IMU sensor data is based on Newton's equation of motion

$$
v = u + at \\
\\
\begin{bmatrix}
\dot{x} \\
\dot{y} \\
\dot{z}
\end{bmatrix}_{k+1} = \begin{bmatrix}
\dot{x} \\
\dot{y} \\
\dot{z}
\end{bmatrix}_{k} + \begin{bmatrix}
\ddot{x} \\
\ddot{y} \\
\ddot{z}
\end{bmatrix}_k \Delta T
$$

while in the paper [2] the process model uses angular rates along with acceleration to propagate the linear velocity based on the below set of equations.

$$
\begin{bmatrix}
u \\
v \\
w \\
\end{bmatrix}_{k+1} = \begin{bmatrix}
u \\
v \\
w \\
\end{bmatrix}_{k} + \begin{bmatrix}
0& r& -q \\
-r& 0& p \\
-p& q& 0
\end{bmatrix} \begin{bmatrix}
u \\
v \\
w \\
\end{bmatrix}_{k} \Delta T+ \begin{bmatrix}
a_x \\
a_y \\
a_z \\
\end{bmatrix}_{k} \Delta T + \begin{bmatrix}
g_x \\
g_y \\
g_z \\
\end{bmatrix}_{k} \Delta T
$$

where u, v, w are the linear velocities | p, q, r are the gyro rates while a_x,a_y,a_z are the acceleration |  g_x,g_y,g_z are the gravity vector

Why do we have two different ways of calculating linear velocities? Which one of these methods should I use when modeling a quadcopter UAV motion?

[1] http://campar.in.tum.de/Chair/KalmanFilter

[2] Shiau, et al. Unscented Kalman Filtering for Attitude Determination Using Mems Sensors Tamkang Journal of Science and Engineering, Tamkang University, 2013, 16, 165-176

答:
The first form, from both the math and the webpage to which you link, is for linear motion only.

Tie a string to a rock. Swing the rock around in a circle above your head at a constant speed. If you cut the string, the rock flies outward because the centripetal force is no longer present to "pull" the rock onto the circular path. That is, the "forward" force is zero, because the rock is moving at a constant speed, but the "sideways" force is $mv^2/r$, the centrifugal force.

Put an accelerometer on a car. X-axis points forward and y-axis points out the right window. Drive the car around in a circle at a constant speed. The "forward" force is zero because the car is moving at constant speed. The "sideways" force is again $mv^2/r$, and is what causes the car to "hug" the curve.

If you were reading the output from an accelerometer, though, you would see that $a_x = 0$ and, from $F=ma$ and $F=mv^2/r$ for centripetal forces, you would see that $a_y = (mv^2/r)/m = v^2/r$.

That is, you will see an apparent acceleration on the y-axis that is used to "bend" the forward velocity of the vehicle around the curve.

The problem here is that the y-axis acceleration is constant while you are moving at constant speed around a circle of constant radius. That is, if you were to double-integrate the y-axis acceleration, then it would appear that the vehicle is accelerating to the right when in reality the vehicle is moving in a circle.

The y-axis centripetal force "bends" the x-axis about the z-axis, so the correct way to account for this is to apply the yaw rate to the y-axis acceleration.


问:
Adafruit PCA9685 servo driver connected to Beaglebone Green Wireless generating erratic movement when driving multiple servos
I have a hexapod that a friend and I built this summer, but there is a big problem whenever we try to move multiple servos. When using the Adafruit_Python_PCA9685 library, I am able to move the servos perfectly fine for a short period, but then they will breakdown and start erratically twitching.

To illustrate the problem, I just modified a few lines from Adafruit's simpletest.py program.

Here is the code:

```<code># Simple demo of of the PCA9685 PWM servo/LED controller library.
# This will move channel 0 from min to max position repeatedly.
# Author: Tony DiCola
# License: Public Domain
from __future__ import division
import time

# Import the PCA9685 module.
import Adafruit_PCA9685

# Uncomment to enable debug output.
import logging
logging.basicConfig(level=logging.DEBUG)

# Initialise the PCA9685 using the default address (0x40).
# pwm = Adafruit_PCA9685.PCA9685()

# Alternatively specify a different address and/or bus:
pwm = Adafruit_PCA9685.PCA9685(address=0x40, busnum=2)

# Configure min and max servo pulse lengths
servo_min = 300  # Min pulse length out of 4096
servo_max = 400  # Max pulse length out of 4096

# Helper function to make setting a servo pulse width simpler.
def set_servo_pulse(channel, pulse):
pulse_length = 1000000    # 1,000,000 us per second
pulse_length //= 60       # 60 Hz
print('{0}us per period'.format(pulse_length))
pulse_length //= 4096     # 12 bits of resolution
print('{0}us per bit'.format(pulse_length))
pulse *= 1000
pulse //= pulse_length
pwm.set_pwm(channel, 0, pulse)

# Set frequency to 60hz, good for servos.
pwm.set_pwm_freq(60)

print('Moving servo on channel 0, press Ctrl-C to quit...')
while True:
# Move servo on channel O between extremes.
for i in range(0, 3):
    for j in range(0,3):
    k = (4*i)+j
    pwm.set_pwm(k, 0, servo_min)
    time.sleep(1)
    pwm.set_pwm(k, 0, servo_max)
    time.sleep(1)
</code>```

And here is a video of the "erratic movement" (the first 8 seconds are normal movement)

I am running the code on a Beagle Bone Green Wireless with ubuntu on it and I am using turnigy TGY-S091D servos.

Here is a photo of the wiring

I don't have enough reputation to post more detailed pictures, but hopefully this is enough.

Please help.

答:
The movement you are seeing is what I would expect to see if your ground was rising becouse of current draw. Basically as you turn on more servos you will see there current draw will make the apparent voltage drop and the pulses for PWM grow more indistinct, digital servos will generally still work ok but analog servos like you have will stutter as you show.

In order to resolve this problem I would suggest you wire up the power in a more robust way directly connecting much larger gauge wire to your servo boards and your power supply, definitely not through a breadboard, and possibly installing thease capacitors in the provided spots on the boards


答:
Definitely possible on +90% of modern PLC's (example, Allen-Bradley compactlogix permits but Allen-Bradley micrologix 1500 does not) with some limitations.  Normally just modifying existing logic is feasible, sometimes with a test mode, however modifying memory allocation or hardware configuration is locked until the plc is out of run state.

Normally online changes actually write the new module elsewhere in the controller memory and make the program jump over the old one.  Over time the program can become fragmented, which usually has a negligible impact but limits the amount of online changes that can be done in a controller without a 'clean up' or 'rebuild all', finally requiring a plc stop and full download.

Title: Do you have to stop a programmable logic controller to update its logic?

Body: Must the operation of a PLC (programmable logic controller) be halted while its logic is being changed or can new logic be downloaded to it during runtime, without memory loss, giving a seamless transition from one operation cycle to other?</p>


问:
IMU in a gimbal system
I have short question about the alexmos gimbal controller. The Controller receives the gyroscope and accelerometer sensor data from the IMU, that is mounted on the camera. In optimal case, the camera should stay in perfect position, which means, that there would be no gyroscope data, since there is no movement. So the only data for positioning would be the accelerometer. Is there a second IMU onboard that receives the gyrometer data?

All the gimbal controllers from aliexpress seems not to have an IMU onboard, but it that case the controller can only use the accelerometer, right?

答: I am not familiar with the gimbal controller you are using, but you cannot stabilize a gimbal with only an accelerometer. You must be able to measure the states or change in states of the gimbal for stabilization. Can you please provide a link to the controller you have the question about.</p>

Title: IMU in a gimbal system


问:
How does fast slam creates grid maps?
I've implemented fast slam using landmark detection and the map stored is a feature map, made of landmarks positions.
I would like to create a grid map, and my questions are about how does the robot create a grid map in slam:

Another landmark class is used, or the occupancy grid is the landmark itself? In other words, the grid map generation is made separatly of the feature map?

About the alignment of the map with the previous maps measures, this is similar to the fast slam 2.0 due to the fast  that considers the robot pose and the measurement of t-1?

Thanks in advance

答: I believe using a grid map will only decrease the accuracy of you estimates. I am assuming your feature map is basically a vector containing the position of each feature as real values. If you convert you feature to a grid map (occupancy grid), then you will be shifting every feature into a grid cell. This will result in a loss of resolution because the grid cells are discrete. To generate an occupancy grid, you need to first choose a resolution. So, let each cell represent a 1 meter by 1 meter area, and then you can simply assign a 1 to any cell containing a landmark and assign a 0 to any cell not containing a landmark.</p>

Title: How does fast slam creates grid maps?


问:
Derivative of a Jacobian matrix
I have an RRR planar robot:

Its forward kinematics transform is:

$$
{}^{0}T_3 = \\ \left[\begin{array}{cccc} \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) & - \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) & 0 & \mathrm{l_2}\, \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2}\right) + \mathrm{l_1}\, \cos\!\left(\mathrm{\theta_1}\right) + \mathrm{l_3}\, \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right)\\ \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) & \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) & 0 & \mathrm{l_2}\, \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2}\right) + \mathrm{l_1}\, \sin\!\left(\mathrm{\theta_1}\right) + \mathrm{l_3}\, \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right)\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{array}\right]
$$

With the joint parameters $q = \left[\begin{array}{ccc} \mathrm{\theta_1} & \mathrm{\theta_2} & \mathrm{\theta_3} \end{array}\right]^T$ and the end-effector position $X = \left[\begin{array}{ccc} \mathrm{x} & \mathrm{y} & \mathrm{\theta} \end{array}\right]^T$. $\theta$ is constrained to 0. (Also the image is misleading, $\theta$ is actually $\theta_1 + \theta_2 + \theta_3$.)

The jacobian matrix is

$$
J = \\ \left[\begin{array}{ccc}  - \mathrm{l_2}\, \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2}\right) - \mathrm{l_1}\, \sin\!\left(\mathrm{\theta_1}\right) - \mathrm{l_3}\, \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) &  - \mathrm{l_2}\, \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2}\right) - \mathrm{l_3}\, \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) & - \mathrm{l_3}\, \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right)\\ \mathrm{l_2}\, \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2}\right) + \mathrm{l_1}\, \cos\!\left(\mathrm{\theta_1}\right) + \mathrm{l_3}\, \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) & \mathrm{l_2}\, \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2}\right) + \mathrm{l_3}\, \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) & \mathrm{l_3}\, \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right)\\ 1 & 1 & 1 \end{array}\right]
$$

I'm trying to find $\ddot{q}$:

$$
[\ddot{q}] = J^{-1}(q) \cdot \left([\ddot{X}] - \dot{J}(q) \cdot J^{-1}(q) \cdot [\dot{X}] \right)
$$

My question is: how can I find $\dot{J}$? What is it?

答: Look at example 283 and its derivation here.  You just take the time derivative of each element of J, paying particular attention to the chain rule.

Title: Derivative of a Jacobian matrix


问:
What determines the amount of noise an actuator produces?
Many robots and other mechanical devices produce the signature whirring noise as they move, some produce less. What makes the difference? What restrictions a silence requirement places on a robot?</p>

答:
Whilst the two answers in place focus on mechanical noise (which is what I think you are asking about) there is also, of course, electrical noise which manifests itself as Electro-Magnetic Interference (EMI or also known as EMC)

Anything that contains a motor is likely to generate a level of EMI... normally a small level that you don't notice it, but with bigger motors and/or high switching speeds it will become noticeable.

Symptoms include picture breakthrough on your TV (etc) with the motor running.


答:
You can use Matlab to compute $\dot{J}$. For example, to compute $\dot{J}_{11}$, we can use the following code

```<code>clear all
clc
syms T1 T2 T3 l1 l2 l3 t

T1(t) = symfun(sym('T1(t)'), t);
T2(t) = symfun(sym('T2(t)'), t);
T3(t) = symfun(sym('T3(t)'), t);

J11 = -l2*sin(T1(t)+T2(t)) - l1*sin(T1(t)) -l3*sin(T1(t)+T2(t)+T3(t));

dJ11dt = diff(J11,t)
</code>```

which yields

```<code>- l3*cos(T1(t) + T2(t) + T3(t))*(diff(T1(t), t) + diff(T2(t), t) + diff(T3(t), t)) - l1*cos(T1(t))*diff(T1(t), t) - l2*cos(T1(t) + T2(t))*(diff(T1(t), t) + diff(T2(t), t))
</code>```

Or

$$
\dot{J}_{11} = -l_3\cos(\theta_1 + \theta_2 + \theta_3)(\dot{\theta}_1+\dot{\theta}_2+\dot{\theta}_3) - l_1\cos(\theta_1) \dot{\theta}_1 - l_2\cos(\theta_1+\theta_2)(\dot{\theta}_1+\dot{\theta}_2)
$$

Now it is easy to compute the rest.

Title: Derivative of a Jacobian matrix

Body:
I have an RRR planar robot:

Its forward kinematics transform is:

$$
{}^{0}T_3 = \\ \left[\begin{array}{cccc} \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) & - \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) & 0 & \mathrm{l_2}\, \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2}\right) + \mathrm{l_1}\, \cos\!\left(\mathrm{\theta_1}\right) + \mathrm{l_3}\, \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right)\\ \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) & \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) & 0 & \mathrm{l_2}\, \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2}\right) + \mathrm{l_1}\, \sin\!\left(\mathrm{\theta_1}\right) + \mathrm{l_3}\, \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right)\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{array}\right]
$$

With the joint parameters $q = \left[\begin{array}{ccc} \mathrm{\theta_1} & \mathrm{\theta_2} & \mathrm{\theta_3} \end{array}\right]^T$ and the end-effector position $X = \left[\begin{array}{ccc} \mathrm{x} & \mathrm{y} & \mathrm{\theta} \end{array}\right]^T$. $\theta$ is constrained to 0. (Also the image is misleading, $\theta$ is actually $\theta_1 + \theta_2 + \theta_3$.)

The jacobian matrix is

$$
J = \\ \left[\begin{array}{ccc}  - \mathrm{l_2}\, \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2}\right) - \mathrm{l_1}\, \sin\!\left(\mathrm{\theta_1}\right) - \mathrm{l_3}\, \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) &  - \mathrm{l_2}\, \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2}\right) - \mathrm{l_3}\, \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) & - \mathrm{l_3}\, \sin\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right)\\ \mathrm{l_2}\, \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2}\right) + \mathrm{l_1}\, \cos\!\left(\mathrm{\theta_1}\right) + \mathrm{l_3}\, \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) & \mathrm{l_2}\, \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2}\right) + \mathrm{l_3}\, \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right) & \mathrm{l_3}\, \cos\!\left(\mathrm{\theta_1} + \mathrm{\theta_2} + \mathrm{\theta_3}\right)\\ 1 & 1 & 1 \end{array}\right]
$$

I'm trying to find $\ddot{q}$:

$$
[\ddot{q}] = J^{-1}(q) \cdot \left([\ddot{X}] - \dot{J}(q) \cdot J^{-1}(q) \cdot [\dot{X}] \right)
$$

My question is: how can I find $\dot{J}$? What is it?

Body:
You can use Matlab to compute $\dot{J}$. For example, to compute $\dot{J}_{11}$, we can use the following code

clear all
clc
syms T1 T2 T3 l1 l2 l3 t

T1(t) = symfun(sym('T1(t)'), t);
T2(t) = symfun(sym('T2(t)'), t);
T3(t) = symfun(sym('T3(t)'), t);

J11 = -l2*sin(T1(t)+T2(t)) - l1*sin(T1(t)) -l3*sin(T1(t)+T2(t)+T3(t));

dJ11dt = diff(J11,t)

which yields

- l3*cos(T1(t) + T2(t) + T3(t))*(diff(T1(t), t) + diff(T2(t), t) + diff(T3(t), t)) - l1*cos(T1(t))*diff(T1(t), t) - l2*cos(T1(t) + T2(t))*(diff(T1(t), t) + diff(T2(t), t))

Or

$$
\dot{J}_{11} = -l_3\cos(\theta_1 + \theta_2 + \theta_3)(\dot{\theta}_1+\dot{\theta}_2+\dot{\theta}_3) - l_1\cos(\theta_1) \dot{\theta}_1 - l_2\cos(\theta_1+\theta_2)(\dot{\theta}_1+\dot{\theta}_2)
$$

Now it is easy to compute the rest.


问:
Why are industrial machines called robots?
The definition of a robot is as follow: "A robotic paradigm can be described by the relationship between the three primitives of robotics: Sense, Plan, and Act."

An example could be the famous "Kuka Robots". The Kuka robot is preprogrammed and does mainly one loop over and over again. Some of them could have measurement sensors but that is all. They do not think or plan nor do they make decisions.

An automatic door opener, used in a building is not a robot either but according to the robotic paradigm definition they are more a robot than a Kuka machine. They actually get some data from a sensor followed by planning and acting.

So why are Kuka machines called robots?

答:
No, that's the definition of "robotic paradigm", which is basically a class of paradigms for designing complex robots.

The definition of "robot", in this context, is:

A robot is a mechanical or virtual artificial agent, usually an electro-mechanical machine that is guided by a computer program or electronic circuitry.

or

A machine capable of carrying out a complex series of actions automatically.

("complex" is ambiguous here)

This Wikipedia section confirms that there is an ambiguity:

While there is no single correct definition of robot, a typical robot will have several, or possibly all, of the following characteristics.

It is an electric machine which has some ability to interact with physical objects and to be given electronic programming to do a specific task or to do a whole range of tasks or actions. It may also have some ability to perceive and absorb data on physical objects, or on its local physical environment, or to process data, or to respond to various stimuli. This is in contrast to a simple mechanical device such as a gear or a hydraulic press or any other item which has no processing ability and which does tasks through purely mechanical processes and motion.

(emphasis mine)

In the end, almost any mechanical device of some level of complexity can be classified as a robot.

Also, note that Kuka robots will have a small amount of sensors for fine-tuning (even if the bot is just picking up an object off a pedestal, it needs to correct its trajectory to align perfectly with the object)


问:
How can I apply different control approach without having a state space model?
Recently i am working on a hydraulic control system of a vehicle. But in this system I don't have any mathematical modeling of the system so i am not able to get any state space model for the given system. I have to control the length of Piston in Hydraulic Cylinder by giving input of desired piston position. Modeling of this system is established in Matlab/Simulink using physical modeling tools SimScape Toolbox and simFluids. As one approach i had already try to apply a feedback with P-control as show in the figure Matlab System  and also the gain of P control is derived by trial and error.

Now if i want to apply state obeserver or optimal Control than how can i apply on this given system.

And is necessary to derive mathematical model of this system to desing controller?

Thank You.

答:
Many control strategegies (PID is not neceserily one of these) are computing some of their parameters from your system. If you do not want to model your system yourself (or play around with it until you identify its charecteristics) you can use Matlab to identify your system model using the system identification toolbox.

The System Identification Toolbox will give generate some inputs for your system and will record the responce for these. Based on this it will generate a simplfyied model which can be used to represent your system. It is simplifyed but in many cases it works well as a substitute.

When having this identifyed model you can freely implement any control strategy using the equations given after the system identification process.


问:
How is whole body control different from computed torque control?
From my understanding Whole body control is built upon Computed Torque control. What is the difference between the two? What is the way for whole body control if the dynamic model is not available?</p>

答: Whole body control (WBC) refers to how to control antropmorph structure (as a system), while the computed torque control (CTC) is a method to compute the torque in 1 single joint (i.e. to control 1 element in a system). Computed torque control can be used only is you have an (inverse) dynamic model. I cannot comment on the question of using WBC without a dynamic model, I am not aware of any possiblities to do so. </p>


问:
Hi i am working on this project regarding localization of a mobile robot using laser scanner and odometry ( encoder sensors) using labview
I have a Sick laser scanner LMS 100 attached with my robot which scans the environment. Data acquistion is already done for the laser scanner and now  Encoder sensor attached on the wheel gives the distance travelled by the robot. A map in indoor room is already given in our case which consists of 3 landmarks.I am working in Labview environment.My question is now how can i localize the robot based on distance and angle? </p>

答: If I understand correctly you have 2 point clouds, one complete map and one partial scan of the environment. The Iterative Closes point method (used in SLAM Methods) can give you the your postion on the map based on the partial scan. </p>


问:
"Time-varying" and "nonautonomous" dynamical systems and their Lyapunov analysis
It is possible to distinguish the properties "time-varying" and "nonautonomous" in dynamical systems regarding Lyapunov stability analysis?

Does it make a difference if the system depends explicitly on $t$ or indirectly on $t$ due to a time-varying parameter?

I want to explain the problem in detail:

Let a dynamical system denoted by $\dot x = f$, with state $x$.
We say that a dynamical system is nonautonomous if the dynamics $f$ depend on time $t$, i.e. $$\dot x = f(t,x).$$

For instance the systems $$ \dot x = - t x^2 $$ and $$ \dot x = -a(t)x,$$ are nonautonomous. Let $a(t)$ be a bounded time-varying parameter, i.e. $||a(t)|| 0$.

Particularly, the second example is more likely denoted as a time-varying linear system, but of course it is nonautonomous.

In Lyapunov stability analysis autonomous and nonautonomous systems must be strongly distinguished to make assertions about stability of the system, and the Lyapunov analysis for nonautonomuos systems is much more difficult.

And here for me some questions arise. When i want to analize stability of the second example must i really use the Lyapunov theory for nonautonomous systems?

It follows for the candidate $V = 1/2 x^2$

$$\dot V = -a(t)x^2,$$

which is negative definite. Is the origin really asymptotically stable, as i suppose, or must i take the nonautomous characteristic into account in this case?

I would suppose it makes a difference if a system depends explicitly on $t$ as in the first example or just indirect due to a time-varying parameter, since $t$ approaches infinity, but a parameter does not.

答:
There is no difference between the two types that you mention. In fact my problem is understanding why you are able to distinguish them in your mind (honestly I don't understand, sorry for this), since there is no mathematical difference between them.

True that nonautonomous systems can be made autonomous by adding some variables (some equations) but you need to know what you are doing because often adding these equations (it need not be $t'=1$ the type of stability changes).


问:
Changing the logic on a PLC while it is running - exact timings
Following my last question which confirmed that you can change the logic on a PLC while it is running, I'm now trying to understand the timings with which this happens.

Say that a PLC is sent a command to update its logic (I'm assuming that this can be done without using the PLC programming software, but could be wrong), and that the new, pending program code is stored in an area of memory which program execution then switches to when all of the new logic has been downloaded onto it. My questions are this:

1) Does a command need to be sent to switch to the new logic, or does this happen automatically once it has been downloaded?

2) Will the PLC switch to the new logic at the start of the next scan cycle (i.e. before that scan cycle's input scan), or at the start of the logic scan.

3) Would it always be that the new logic takes effect the scan cycle after it has finished being downloaded, or could there be a delay? I am trying to look for timing relationships between the networking data and the updated PLC logic, so need to be strict.

If anyone knows of any documentation for commands to update a PLC's logic while it is still running could they please point me to it?

Many thanks.

答:
Preface - I don't know of any way to program or download a PLC without using the associated programming software. Industrial PLCs are not cheap and generally not open source (that is, there is no API or other documentation on how to implement your own version of their programming software).

As mentioned in an answer your earlier question, the changes you are allowed to make without stopping the controller are generally limited to what is referred to as a "word for word" change, which means you can typically only replace one hard-coded constant for another.

So, to your questions:

Like I mention above, you're generally changing one address with one address, or the contents of one register with one new register. There can be a list of changes like this, but that's the scope of what you're allowed to do. Changing logic, or the flow of the PLC, usually requires restarting. The changeover happens at the end of downloading, but I don't know this happens or what the trigger looks like; as I mentioned there's no explicit definition (that I've seen) of what the packet structure looks like for downloading software.

I don't know what you mean when you call "logic scan" and "scan cycle" as two different terms. I don't think the change would happen mid-scan, but I could be wrong.

I don't know if there could be a delay. Simple PLC code I work on has a sweep time of 10-15ms, complex PLC code has a sweep time of 30-40ms. This is a negligible amount of time relative to the amount of time to download new code, even if it IS just a word-for-word change. That word-for-word change may take 2-3 seconds to download, but that's maybe 100 cycles of the PLC.

I don't know what you're trying to do, but there shouldn't be any time where you're relying on downloading and re-downloading PLC code down and expecting to get timing accuracy down to the individual sweep cycle. Get an I/O card and toggle a switch, or write a message and toggle a bit, etc., if you need timing that close.

More background on what you're trying to do would be helpful. From your questions, "I'm looking for timing relationships between the networking data and the updated PLC logic" - again, I don't know WHY you care about this, downloading is a horrible way to switch code (that's what a switch statement is for, or a state machine), but what I would do would look like this pseudo-code:

```<code>loopCount = loopCount+1;
myConst = 5;
TCP_WRITE(loopCount, myConst);
</code>```

You continuously read messages coming off the PLC. Each message gets 1) a number that increments every sweep, and 2) a number that is fixed.

You update the PLC code to change myConst from 5 to 6 or anything else, download the change, and see how many cycles elapse between when you push download and when myConst changes. You can have multiple constants, change them all, and see if they all update simultaneously or if loopCount increments between updates.

But again, seriously, don't expect to ever be able to reliably download logic to the individual sweep precision.

Body:
Preface - I don't know of any way to program or download a PLC without using the associated programming software. Industrial PLCs are not cheap and generally not open source (that is, there is no API or other documentation on how to implement your own version of their programming software).

As mentioned in an answer your earlier question, the changes you are allowed to make without stopping the controller are generally limited to what is referred to as a "word for word" change, which means you can typically only replace one hard-coded constant for another.

So, to your questions:

Like I mention above, you're generally changing one address with one address, or the contents of one register with one new register. There can be a list of changes like this, but that's the scope of what you're allowed to do. Changing logic, or the flow of the PLC, usually requires restarting. The changeover happens at the end of downloading, but I don't know this happens or what the trigger looks like; as I mentioned there's no explicit definition (that I've seen) of what the packet structure looks like for downloading software.

I don't know what you mean when you call "logic scan" and "scan cycle" as two different terms. I don't think the change would happen mid-scan, but I could be wrong.

I don't know if there could be a delay. Simple PLC code I work on has a sweep time of 10-15ms, complex PLC code has a sweep time of 30-40ms. This is a negligible amount of time relative to the amount of time to download new code, even if it IS just a word-for-word change. That word-for-word change may take 2-3 seconds to download, but that's maybe 100 cycles of the PLC.

I don't know what you're trying to do, but there shouldn't be any time where you're relying on downloading and re-downloading PLC code down and expecting to get timing accuracy down to the individual sweep cycle. Get an I/O card and toggle a switch, or write a message and toggle a bit, etc., if you need timing that close.

More background on what you're trying to do would be helpful. From your questions, "I'm looking for timing relationships between the networking data and the updated PLC logic" - again, I don't know WHY you care about this, downloading is a horrible way to switch code (that's what a switch statement is for, or a state machine), but what I would do would look like this pseudo-code:

loopCount = loopCount+1;
myConst = 5;
TCP_WRITE(loopCount, myConst);

You continuously read messages coming off the PLC. Each message gets 1) a number that increments every sweep, and 2) a number that is fixed.

You update the PLC code to change myConst from 5 to 6 or anything else, download the change, and see how many cycles elapse between when you push download and when myConst changes. You can have multiple constants, change them all, and see if they all update simultaneously or if loopCount increments between updates.

But again, seriously, don't expect to ever be able to reliably download logic to the individual sweep precision.


答: here's how i managed to solve my problem, i got my old wildfire installed IP Webcam, set-up a wifi hotspot from my nexus4, connected to it from the wildfire, took the wildfire's ip and put it in the nexus4's web viewer.</p>

Title: How can I send video from my Arduino camera module video to my Android screen?

Body:
I'm trying to connect a camera module to my Arduino Mega, connect my Mega to my Android phone (throught BlueTooth or other), and send the live view of the camera to the mobile phone.

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

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


问:
Inverse kinematics: how to specify TCP constraints in an iterative algorithm?
I am trying to write some simple code to perform IK for a 6 DoF redundant robot using the Jacobian pseudo-inverse method. I can solve IK for a desired pose using the iterative method, and I want to now focus on applying constraints to the solution. Specifically I'm interested in

Keep the end effector orientation constant as the robot moves from initial to final pose

Avoid obstacles in the workspace

I've read about how the redundancy/null space of the Jacobian can be exploited to cause internal motions that satisfy desired constraints, while still executing the trajectory, but I am having trouble implementing this as an algorithm. For instance, my simple iterative algorithm looks like

```<code>error = pose_desired - pose_current;
q_vel = pinv(J)*error(:,k);
q = q + q_vel;
</code>```

where $q$ is 'pushed' towards the right solution, updated until the error is minimized. But for additional constraints, the equation (Siciliano, Bruno, et al. Robotics: modelling, planning and control) specifies

$$
\dot{q} = J^\dagger*v_e - (I-J^\dagger J)\dot{q_0}
\\
\dot{q_0} = k_0*(\frac{\partial w(q)}{\partial q})^T
$$

where $w$ is supposed to be a term that minimizes/maximizes a chosen constraint. I don't understand the real world algorithmic implementation of this 'term' in the context of my desired constraints: so if I want to keep the orientation of my end effector constant, how can I define the parameters $w$, $q_0$ etc.? I can sense that the partial derivative signifies that it is representing the difference between how the current configuration and a future configuration affect my constraint, and can encourage 'good' choices, but not more than that.

答:
The pseudoinverse (Moore-Penrose inverse) provides the solution which is minimum in a least-squares sense.  It is rather similar to the Simplex method in that sense.  You need some type of optimization such as this to coordinate the extra axes of motion while posing the end effector at the desired position and orientation.

To also optimize a run-time constraint, you take advantage of the null space of J.  That is your term multiplying $\dot{q_0}.$  Frankly it doesn't really matter what $\omega$ is, in terms of the position and orientation of the end effector.  The idea is that any constraint $\omega$ only results in self-motion of the manipulator, which does not change the end effector's pose.

Look up John Ballieul's work for excellent descriptions of the use of the null space of $J$ for these purposes.


答:
You have the definition of "nonautonomous" incorrect. Please consult authoritative  references on control theory such as one by Sastry, Khalil or Slotine and Li

Here is a brief summary of the terms as copied from math stackexchange

A system is time invariant if the system parameters does not depend on time

These systems are represented by:

$$\dot x = f(x,u), \dot x = f(x)$$ or when the system is linear $$\dot x = Ax + Bu, \dot x = Ax$$

A system is time varying if the system parameters does depend on time

These systems are represented by:

$$\dot x = f(x,u,t)$$ or $$\dot x = A(t) x + B(t)u$$

For a RLC circuit, $A(t)$ could represent the matrix containing time varying capacitance, inductance or resistance. Similarly, for a mass-spring-damper system, $A(t)$ could represent time varying damping, friction, and mass. Of course, all real system are time varying albeit on the scale of hours, years, or even millennia.

A (time invariant) system is autonomous if the input $u$ is a function of the state:

These systems are represented by:

$$\dot x = f(x,u(x)) = f(x)$$ or $$\dot x = Ax + Bu(x) = (A-BK)x$$ Supposing that we are using feedback $u = -Kx$. Any state feedback systems are autonomous, because your input $u$ is a function of your state.

And you might have guessed it, a (time invariant) non-autonomous system is when your input is not a function of the state

These systems are represented by:

$$\dot x = f(x,u)$$ or $$\dot x = Ax + Bu$$

For example, $u$ could be the irradiation of the sun hitting a solar panel, where as $x$ encapsulates the states of the solar panel. The solar panel is not going to affect sunshine or the sun for that matters, or the cloud passing the sun.

For your question, you (most likely)* cannot use the Lyapunov function as proposed for your system, namely:

Using $$V(x) = \dfrac{1}{2}x^2$$

to prove stability of origin for

$$\dot x = -a(t)x$$

Because your system has time varying parameters. It is autonomous, and time varying.

What you need to do is to construct a time varying Lyapunov function, and in the process you will encounter when a  Lyapunov function () is said to be descrescent, etc. Those are not a part of the classical Lyapunov theory, which deals with time-invariant, autonomous system. Your best reference is the control theory text of Slotine and Li.

Note: I am not that familiar with time varying Lyapunov functions

Title: "Time-varying" and "nonautonomous" dynamical systems and their Lyapunov analysis

Body:
It is possible to distinguish the properties "time-varying" and "nonautonomous" in dynamical systems regarding Lyapunov stability analysis?

Does it make a difference if the system depends explicitly on $t$ or indirectly on $t$ due to a time-varying parameter?

I want to explain the problem in detail:

Let a dynamical system denoted by $\dot x = f$, with state $x$.
We say that a dynamical system is nonautonomous if the dynamics $f$ depend on time $t$, i.e. $$\dot x = f(t,x).$$

For instance the systems $$ \dot x = - t x^2 $$ and $$ \dot x = -a(t)x,$$ are nonautonomous. Let $a(t)$ be a bounded time-varying parameter, i.e. $||a(t)|| 0$.

Particularly, the second example is more likely denoted as a time-varying linear system, but of course it is nonautonomous.

In Lyapunov stability analysis autonomous and nonautonomous systems must be strongly distinguished to make assertions about stability of the system, and the Lyapunov analysis for nonautonomuos systems is much more difficult.

And here for me some questions arise. When i want to analize stability of the second example must i really use the Lyapunov theory for nonautonomous systems?

It follows for the candidate $V = 1/2 x^2$

$$\dot V = -a(t)x^2,$$

which is negative definite. Is the origin really asymptotically stable, as i suppose, or must i take the nonautomous characteristic into account in this case?

I would suppose it makes a difference if a system depends explicitly on $t$ as in the first example or just indirect due to a time-varying parameter, since $t$ approaches infinity, but a parameter does not.


答: You can localize the robot using dead reckoning with the encoders alone, but the result would be very inaccurate. What is the representation of the landmarks? Also, what do you mean by localize using distance and angle? Are you saying you have the distance and angle to each landmark? Please provide more information.</p>

Title: Hi i am working on this project regarding localization of a mobile robot using laser scanner and odometry ( encoder sensors) using labview

Body: I have a Sick laser scanner LMS 100 attached with my robot which scans the environment. Data acquistion is already done for the laser scanner and now  Encoder sensor attached on the wheel gives the distance travelled by the robot. A map in indoor room is already given in our case which consists of 3 landmarks.I am working in Labview environment.My question is now how can i localize the robot based on distance and angle? </p>


答:
If you want to keep orientation constant, the simplest approach is to, well.. do exactly that. Do not solve for the tartget point and let the joint move along a seemingly arbitratry cartesian path (but infact well defined in joint space).

Solve the inverse kinematics problem for a number of points (depending on your robots precision maybe every milimeter) along a line or any other path shape you need for obstacle avoidance. The line (or other shape) gives you the TCP positions and the orientation, as you require it, should be set constant. You will get joint coordinates for each point on a line. If you feed these squentially to the motors your robot will execute the desired trajectory with the desired orientation. No null space needed.

If you do not want to complicate things with additional shapes and IK solving, you can also constrain orientation in your method easily:

Instead of planning the whole path in one shot like you specified:

```<code>error = pose_desired - pose_current;
q_vel = pinv(J)*error(:,k);
q = q+ q_vel;
</code>```

Break it down to 3 seperate parts

1 Set the constat orientation you require during the path execution

```<code>error = postition_current_with_const_orientation - pose_current;
q_vel = pinv(J)*error(:,k);
q = q+ q_vel;
</code>```

If the error as an RMS value is below a threshold value you can proceed to the next step

2 Move to the target position (not pose) with the constant orientation

```<code>error = postition_target_with_const_orientation - pose_current;
q_vel = pinv(J)*error(:,k);
q = q+ q_vel;
</code>```

If the error as an RMS value is below a threshold value you can proceed to the next step

3 Orient the TCP for the target orientation to attain the target pose

```<code>error = pose_target - pose_current;
q_vel = pinv(J)*error(:,k);
q = q+ q_vel;
</code>```

If the error as an RMS value is below a threshold value you can proceed to the next task/motion

EDIT: Please note that I am not sure why you used error(:,k) and not just error but I assumed it has a reason and used it as you did.

Title: Inverse kinematics: how to specify TCP constraints in an iterative algorithm?

Body:
I am trying to write some simple code to perform IK for a 6 DoF redundant robot using the Jacobian pseudo-inverse method. I can solve IK for a desired pose using the iterative method, and I want to now focus on applying constraints to the solution. Specifically I'm interested in

Keep the end effector orientation constant as the robot moves from initial to final pose

Avoid obstacles in the workspace

I've read about how the redundancy/null space of the Jacobian can be exploited to cause internal motions that satisfy desired constraints, while still executing the trajectory, but I am having trouble implementing this as an algorithm. For instance, my simple iterative algorithm looks like

```<code>error = pose_desired - pose_current;
q_vel = pinv(J)*error(:,k);
q = q + q_vel;
</code>```

where $q$ is 'pushed' towards the right solution, updated until the error is minimized. But for additional constraints, the equation (Siciliano, Bruno, et al. Robotics: modelling, planning and control) specifies

$$
\dot{q} = J^\dagger*v_e - (I-J^\dagger J)\dot{q_0}
\\
\dot{q_0} = k_0*(\frac{\partial w(q)}{\partial q})^T
$$

where $w$ is supposed to be a term that minimizes/maximizes a chosen constraint. I don't understand the real world algorithmic implementation of this 'term' in the context of my desired constraints: so if I want to keep the orientation of my end effector constant, how can I define the parameters $w$, $q_0$ etc.? I can sense that the partial derivative signifies that it is representing the difference between how the current configuration and a future configuration affect my constraint, and can encourage 'good' choices, but not more than that.

Body:
If you want to keep orientation constant, the simplest approach is to, well.. do exactly that. Do not solve for the tartget point and let the joint move along a seemingly arbitratry cartesian path (but infact well defined in joint space).

Solve the inverse kinematics problem for a number of points (depending on your robots precision maybe every milimeter) along a line or any other path shape you need for obstacle avoidance. The line (or other shape) gives you the TCP positions and the orientation, as you require it, should be set constant. You will get joint coordinates for each point on a line. If you feed these squentially to the motors your robot will execute the desired trajectory with the desired orientation. No null space needed.

If you do not want to complicate things with additional shapes and IK solving, you can also constrain orientation in your method easily:

Instead of planning the whole path in one shot like you specified:

error = pose_desired - pose_current;
q_vel = pinv(J)*error(:,k);
q = q+ q_vel;

Break it down to 3 seperate parts

1 Set the constat orientation you require during the path execution

error = postition_current_with_const_orientation - pose_current;
q_vel = pinv(J)*error(:,k);
q = q+ q_vel;

If the error as an RMS value is below a threshold value you can proceed to the next step

2 Move to the target position (not pose) with the constant orientation

error = postition_target_with_const_orientation - pose_current;
q_vel = pinv(J)*error(:,k);
q = q+ q_vel;

If the error as an RMS value is below a threshold value you can proceed to the next step

3 Orient the TCP for the target orientation to attain the target pose

error = pose_target - pose_current;
q_vel = pinv(J)*error(:,k);
q = q+ q_vel;

If the error as an RMS value is below a threshold value you can proceed to the next task/motion

EDIT: Please note that I am not sure why you used error(:,k) and not just error but I assumed it has a reason and used it as you did.


答:
I'm not sure I agree with Chuck, or maybe I'm just not following what he's saying, so I'll have a go.

First of all, these questions might be easier to handle with a particular controller in mind, but I'll answer them assuming we're dealing with something mainstream such as Siemens or Allen-Bradley.

You can most definitely write an entire new module with hundreds of lines of codes and sometimes even create new variables and DB's (depending on processor) or extensively modify an existing module while a PLC is online and running, with negligible impact to the cycle time.

The timing of this will be the PLC will continue to loop through its typical scan/task assignment in parallel with the download.  After download is finished there's going to be some sort of error checking / handshake to make the code is downloaded and complete.  Some PLC's let you download the change, then test it, such as Allen-Bradley.  And conversely you can remove it from test but leave it in the controller.  Others, however, will immediately put it into effect when you download the change.  Regardless of what initiates it, it will happen seemlessly, possibly between cycles, or perhaps if a module is updated prior to it being called in that scan/task cycle, it may call the new one.  That depends on the controller.

A possible scenario:
Cycle 1 (running old logic, downloading in parallel)

Cycle 2 (running old logic, downloaded finishes during scan)

Cycle 3 (running new logic)

But regardless, if your concern is if there's a delay in the scan cycle or if the processing is somehow interfered with during download, I would say no in most cases.

Title: Changing the logic on a PLC while it is running - exact timings

Body:
Following my last question which confirmed that you can change the logic on a PLC while it is running, I'm now trying to understand the timings with which this happens.

Say that a PLC is sent a command to update its logic (I'm assuming that this can be done without using the PLC programming software, but could be wrong), and that the new, pending program code is stored in an area of memory which program execution then switches to when all of the new logic has been downloaded onto it. My questions are this:

1) Does a command need to be sent to switch to the new logic, or does this happen automatically once it has been downloaded?

2) Will the PLC switch to the new logic at the start of the next scan cycle (i.e. before that scan cycle's input scan), or at the start of the logic scan.

3) Would it always be that the new logic takes effect the scan cycle after it has finished being downloaded, or could there be a delay? I am trying to look for timing relationships between the networking data and the updated PLC logic, so need to be strict.

If anyone knows of any documentation for commands to update a PLC's logic while it is still running could they please point me to it?

Many thanks.


问:
At which stage should filtering be applied to the sensors data?
Shall I filter (kalman/lowpass) after getting the raw values from a sensor or after converting the raw values to a usable data? Does it matter? If so, why?

Example:
Filter after getting raw values from IMU
or
filter after converting raw values to a usable data eg. flight dynamics parameters.

答:
Filter the raw data.

Filtering weeds out (hopefully) most noise and errors. Raw data usually is not so useful.

Gyros drift, compasses has a lot of noise. Kalman can remove both.


问:
What is the difference between a CNC router versus a CNC mill?
People at the RepRap 3d-printing project often mention CNC routers or CNC mills.

Both kinds of machines almost always have a motorized spindle with stepper motors to move the spindle in the X, Y, and Z directions.

What is the difference between a CNC router versus a CNC mill?

(Is there a better place for this sort of question -- perhaps the Woodworking Stack Exchange?)

答:
Not a big fan of using head motions or type of spindle to define the difference, as there are lots of exceptions to each.  Many 5-axis mills move the head in various ways beyond just Z.  Datron makes CNC mills (excellent ones, BTW) that look like these CNC "Routers" but they call them "Mills".

Some refer to "Gantry Style" mills as routers.  The "Gantry" is that travelling bridge with the head on it.  But, Some gantries still move the table, often in just one axis, usually Y.

There are CNC "Routers" that have very expensive high speed spindles with toolchange capability.  They're not just lashing a woodworking handheld router onto the CNC.

In the end, "Router" seems most often to refer to a gantry-style machine that is most often used to cut wood and soft materials like plastic or foam board.  "Mill" seems to most often refer to machines that are usually not (but can be) gantry-style that are most often used to cut metal and harder materials.

Personally, I prefer to use the terms interchangeably and realize that you can use one to do the other's job in many cases.

ContentLicense: CC BY-SA 4.0


问:
What is minimum torque required for CNC stepper motors and spindle for aluminium milling?
I am planning to buy CNC mechanical skeleton without motors, spindle and controller. I will be using the CNC mainly for aluminium milling. Are there any specifications for minimum torque requirement for stepper motors and spindle to perform aluminium milling ?</p>

答:
Here's the important thing to realize:

While many want to size motors based on cutting forces, that approach will almost always leave you with a motor that's too weak for good performance.

Cutting force is not the limiting factor the motor needs to overcome.  Rather, the limiting factor will be acceleration.  Your motors must produce enough torque to accelerate the axis quickly enough to reach your machine's target speeds in a reasonably short distance.

I've written some detailed articles about how to figure all this out in Ultimate Benchtop CNC Mini Mill Part 4: Motion Performance.


问:
How to convert G code to motor command?
In cnc machine, programs change G-code to commands to stepper motors using parallel port.

I want to know what is the G-code and how can it be converted to stepper motor commands?

The programs doing this are not open-source, So can I find open source project doing the same?

答:
LinuxCNC is a good example.  A simpler example would be GRBL, which runs on Arduinos:

https://github.com/grbl/grbl/wiki

Title: How to convert G code to motor command?


答: I suggest you look at the Marlin firmware for reprap 3d printers. Essentially it parses out lines of CNC code and identifies each of them as a command.  The trickiest of these are the mutli-axis coordinated movements where multiple steppers are involved such as g3/g4 circle interpolate.  The magic is in the timers and trig.</p>

Title: How to convert G code to motor command?

Body:
In cnc machine, programs change G-code to commands to stepper motors using parallel port.

I want to know what is the G-code and how can it be converted to stepper motor commands?

The programs doing this are not open-source, So can I find open source project doing the same?


问:
How to implement a particle filter when sensors can't identify landmarks?
I'm attempting to build a robot that leverages a particle filter to identify where it is relative to a map that is known. The robot only has IR sensors, so while it is able to determine its distance from landmarks, it does not know what landmark it is "looking" at.

I'm following this very helpful book to build my particle filter. In incorporating the sensor measurements, it is assumed that you know both the distance to a landmark and which specific landmark you are looking at. What would need to be done if you know the map and distance measurements, but not the specific landmark that you're observing? Would this require SLAM? Or could you simply increase the probability for particles that are about that distance from a landmark?

答:
SLAM is only needed when you are also building a map. You already have a map so the problem is localization. To be exact the problem you want to research is Monte Carlo localization or particle filter localization. A fantastic book on it is Probabilistic Robotics if you can get your hands on it.

Some slides describing the resources in the book can be found here.

Instead of a landmark map you need an occupancy grid. With your sensor setup you are able to get bearing and range. For the weights calculation of your particles you are going to calculate it via this formula p(z|x,m) where z is you measurement, x is your robot position and m is your stored map.

To actual implement this there are two different measurement models you can use to calculate this probability. One is called the beam range finder model and the other is likelihood fields. So just adjust your particle filter to use that and everything else should be the same.


答:
The KF estimates the robot pose based on all sensor inputs and the sensor correlation. If you do an EKF on the compass data, you'd really need the robot pose to determine how likely a given compass reading is. Without that, you are
just low-pass filtering (not using a probabilistic filter like the KF).

If you filter before you put everything in the same frame, then I don't know what information you'd have to do filtering on.
Since I don't know exactly what you mean by "usable" I assume you have converted all sensor data into the coordinate frame of the robot. In that case, filtering is very easy since you can put all the sensor readings directly into one EKF. In fact, this is the "normal" way to do filtering, that I'm familiar with.

Example:
Let's say your IMU is attached to a robot. The IMU will be used in estimating the pose of the robot. It doesn't matter what units you use as long as the IMU is telling you something about how the robot is moving. Then you can use the corellation of the IMU to other things that measure movement, such as the compass or odometers. That is what the KF is for. The KF is not a sensor filter like a bandpass filter or something.

There is a very relevant answer here.

Title: At which stage should filtering be applied to the sensors data?

Body:
Shall I filter (kalman/lowpass) after getting the raw values from a sensor or after converting the raw values to a usable data? Does it matter? If so, why?

Example:
Filter after getting raw values from IMU
or
filter after converting raw values to a usable data eg. flight dynamics parameters.


问:
Inverse kinematics
I'm developing a 2 DOF SCARA with inverse kinematics. It works fine at any desired point, but how can I draw a line? Is there an efficient algorithm to do this?</p>

答:
You can define your line as a sequence of very close points, and then just move from one to the next.   If you don't want the SCARA arm to "move, stop, move, stop" you can set the target point to the next one once the arm is 80 or 90 percent to the current target point.

EDIT (added example)

Example:

Let's say you want the robot to move its tool from (1, 1) to (2, 3).  Break that line into, say, 10 segments (actually 9 segments with 10 points including the endpoints).  The number of segments will depend on the accuracy you require, since the robot will not move in a straight line between its target points.  If you need more accuracy, divide the line into more segments.

First, position the robot at (1, 1).  The next point along the line, using the 10 points, would be (1.1, 1.2).   Tell the robot to move there, but when its x value reaches, say, 1.08, command it to move to the next point (1.2, 1.4).   I hope you can see how this method can be built into a couple of loops in your code.

Alternately, you can use velocity control (using the derivative of your kinematics) to tell it to move in a particular direction, and then update that direction continuously as it moves along the line.

Title: Inverse kinematics


问:
Robot Arm Simulation
I don't know if this is the right place to ask this question but I'll give it a try. A friend of mine is using unity 3D to simulate a robot arm,  however he's having some troubles when he needs to rotate the robot arm. The arm can already grab stuff with its hands, however seems like sometimes randomly crashes when is rotating the wrist.

Here's an image of the robot arm:

That's the arm he's working on for the simulation, and you can see that every part of the arm is connected through the unity's inheritance system so it can rotate every piece of the robot arm along with the nested parts that follows the inheritance path. However something seems like is failing when it rotates the wrist of the arm.

Three main questions:

How should be achieved the arm's rotation properly?

How should be achieved picking up things with the hand of the arm properly?

How could you make it move by itself on a given point that can reach?

I'm not asking for code or anything like that, just how these 3 things should be done properly in order to have fully functional robot arm simulation like the main concepts behind it.

答:
How should be achieved the arm's rotation properly?

In unity each game object has a transformation property. The rotation component of the transformation property has to be changes in order to rotate a linkage properly. If this is not working, check if you rotate in the correct units and correct reference frame. If yes, it has to work, try rotating in the unity editor using the panel for setting position and orientation. If it works there it has to work in code too.

How should be achieved picking up things with the hand of the arm properly?

Not an easy question. If you want to use the physics engine, you can model the scenario using friction. If the jaws of the gripper hold the object which is manipulated using a high enough force then it will be lifted. This is computationally intensive. Another approach would be just to make the object which is manipulated the child of the gripper if a certain condition is met (i.e. jaws are closed). It is not a precise solution but it requires very little computation time

How could you make it move by itself on a given point that can reach?

You will need to solve inverse kinematics model of the robot for this. You have a line equation, you can discretize it and for each point you can solve the inverse kinematics problem to get the joint angles of the robot. You apply those angles to the robot sequentially and it will move along a line. Alternatively you can experiment with unity IK. I am not sure if it can be used in your case.

Title: Robot Arm Simulation


问:
DH parameters for PRP manipulator
Can anyone share their DH parameters for a simple PRP manipulator. I have some confusion in setting up the axes and obtaining a solution.

答:
In some cases, as are you two cases in the upper part of the image, you might need additional "dummy" coordinate system to end up in the correct orientation. These are just additional lines in the DH table, with most of the coordinates set to 0.

In the upper left solution you might need an additional dummy coordinate system to transform from the 2nd coordinate system to the third one. Just add a dummy coordinate system with a pi/2 z rotation and the x rotation should be pi instead of pi/2 in the transformation which follows (now the 2nd line in the table).

the upper right one needs a dummy transformation with a pi/2 z rotation between system 2 and 3.

The lower left side solution is ok and so is the lower right side one.

Title: DH parameters for PRP manipulator


答: Kinematic frames are different from the physical structure and the working drawings we create can lead us astray. Frames don't necessarily sit on the physical structure. In the frames sketch below frame 1 and 2 overlap. A close mechanical analog with a shift on the x axis between 1 and 2 is shown. However the only requirement is that the z axis match the direction of motion. Also I find it helpful to add my q parameters to the DH table entries so that the offset (the DH constant) is clear.

Title: DH parameters for PRP manipulator

Body: Can anyone share their DH parameters for a simple PRP manipulator. I have some confusion in setting up the axes and obtaining a solution.


问:
Visual Odometry options?
What are the pros/cons of the different visual odometry options?

Stereo Camera

Optical Flow

SLAM

other?

Criteria:

how well it performs vs other odometry options/sensors (lidar, radar)

sensor fidelity

computation

accuracy

precision

drift

native resilience and repeadability in sensor noise or vehicle speed

ease of integrating with IMU/GPS

etc

In general, of course, because there are a lot of different ways the trade-offs go when we get into specifics about applications and hardware.  I'm asking out of curiosity, not for designing anything in particular.

答:
Have a look at KITTI Visual Odometry Benchmark/Evaluation. Its a recent and nice evaluation of different stereo visual odometry algorithms on some large outdoor urban scenes. They compare it against ground truth (GPS).

I would say Visual Odometry are quite accurate and elegant perception systems. And its much better option than the IMU/GPS alone (even the high end ones).

Title: Visual Odometry options?


问:
Adafruit PCA9685 servo driver connected to Beaglebone Green Wireless generating erratic movement when driving multiple servos
I have a hexapod that a friend and I built this summer, but there is a big problem whenever we try to move multiple servos. When using the Adafruit_Python_PCA9685 library, I am able to move the servos perfectly fine for a short period, but then they will breakdown and start erratically twitching.

To illustrate the problem, I just modified a few lines from Adafruit's simpletest.py program.

Here is the code:

```<code># Simple demo of of the PCA9685 PWM servo/LED controller library.
# This will move channel 0 from min to max position repeatedly.
# Author: Tony DiCola
# License: Public Domain
from __future__ import division
import time

# Import the PCA9685 module.
import Adafruit_PCA9685

# Uncomment to enable debug output.
import logging
logging.basicConfig(level=logging.DEBUG)

# Initialise the PCA9685 using the default address (0x40).
# pwm = Adafruit_PCA9685.PCA9685()

# Alternatively specify a different address and/or bus:
pwm = Adafruit_PCA9685.PCA9685(address=0x40, busnum=2)

# Configure min and max servo pulse lengths
servo_min = 300  # Min pulse length out of 4096
servo_max = 400  # Max pulse length out of 4096

# Helper function to make setting a servo pulse width simpler.
def set_servo_pulse(channel, pulse):
pulse_length = 1000000    # 1,000,000 us per second
pulse_length //= 60       # 60 Hz
print('{0}us per period'.format(pulse_length))
pulse_length //= 4096     # 12 bits of resolution
print('{0}us per bit'.format(pulse_length))
pulse *= 1000
pulse //= pulse_length
pwm.set_pwm(channel, 0, pulse)

# Set frequency to 60hz, good for servos.
pwm.set_pwm_freq(60)

print('Moving servo on channel 0, press Ctrl-C to quit...')
while True:
# Move servo on channel O between extremes.
for i in range(0, 3):
    for j in range(0,3):
    k = (4*i)+j
    pwm.set_pwm(k, 0, servo_min)
    time.sleep(1)
    pwm.set_pwm(k, 0, servo_max)
    time.sleep(1)
</code>```

And here is a video of the "erratic movement" (the first 8 seconds are normal movement)

I am running the code on a Beagle Bone Green Wireless with ubuntu on it and I am using turnigy TGY-S091D servos.

Here is a photo of the wiring

I don't have enough reputation to post more detailed pictures, but hopefully this is enough.

Please help.

答:
You should avoid breadboards when working with circuits subject to significant currents or vibration.

That said - the power side of the servos (centre wire) can and should be supplied by a private  and isolated supply. The arduino has its own on-board regulator but that is limited to under 500mA. If you are driving the servos directly from the arduino then when your servos are working they are drawing too much current, resulting in a voltage drop which is interfering with the signal.

The second issue may simply be circuit noise. All brushed motors are digitally noisy - that noise appears as spurious signals on the supply rail which can interfere with high frequency MCUs for example. If circuit noise is suspected - an oscilloscope on the arduino supply rail will show the problem. Often a simple capacitor (0.1uF) to ground is sufficient to attenuate the noise.

The solution is to use a BEC (Battery Elimination Circuit) which isolates the power used by the BEC connected devices, in this case your servos, from other devices connected to the same battery.  With the BEC in place and supplying power to the servos, the ground connection must be common with the control signal ground and the power ground. The control signal is therefore independent of the servo supply voltage and is always referenced to the arduino ground.


问:
Mobile robot identification
The problem: line follower robots - non linear systems - use linear PID regulation algorithm in order to bring error to zero. However, using linear regulator is not the best way to drive non linear system.

There is something like global linearization of non linear systems - an algorithm that can bring regulation error to zero. In order to use it, one has to know kinematics of robot: Coriolis, inertion, gravity and friction matrixes. Those were once measured in EDDA manipulator and are now used in science, and that is how I learned about global linearization.

The question: I'd like to identify <s>kinematical</s> dynamical parameters of my line follower robot. I already have kinematical model, since it is simple (2,0) platform. Has anyone got information about good sources on physical parameters identification of mobile robot like this?

答:
Linear regulators are frequently used to control nonlinear systems.   So are nonlinear controllers.   Much depends on the system identification from a controls, not kinematics, perspective.

I believe you are confusing the kinematics of motion with the system dynamics.  Kinematics is the science of motion without regard to the forces and torques that create or resist that motion.

To identify the dynamic properties of your line follower, a lot more information is needed about your system.   Right now this question is overly general in my opinion.

Title: Mobile robot identification