问:
Quadcopter PID Controller for distance
I am trying to use a PID controller to stop a quadcopter at a specific location while travelling horizontally, however currently it overshoots/undershoots depending on the max velocity. I have tried manually tuning the P,I and D gains with limited success. Essentially the velocity needs to go from maxSpeed to 0 at the very end of the flight path.

It uses the DJI SDK so the inputs to the quadcopter are very simplistic, the pitch input to the quadcopter is in m/s and I recalculate the distance(m) to the target on each iteration.

I run a loop that executes every 0.1 of a second. Some pseudo code:

```<code>kP = 0.25
kI = 0.50
kD = 90
timeStep = 0.1
maxSpeed = 10

currentError = initialDistanceToLocation - currentDistanceToLocation
derivativeError = (currentError - previousError) / timeStep
previousError = currentError
output = kP * currentError + kI * integralError + kD * derivativeError
integralError = integralError + currentError * timeStep

if >= maxSpeed {
    output = maxSpeed
} else if output <= 0 {
    output = 0
}
return output
</code>```

Is there a way to reliably tune this PID controller to this system that will work for different max velocities, or is it too simplistic and there are other factors I need to consider?

答:
I don't know to which extent your pseudo-code can be taken as representative of your real code, but I can see the following concerns with it.

Derivative term

In practical applications, the derivative term computed as a one-step finite difference as below is completely useless:

```<code>derivativeError = (currentError - previousError) / timeStep
</code>```

There is a large number of reasons for that, mainly related to the fact that physical signals just like the measured distance from the target are heavily affected by the noise, which in turn has high-frequency components that get amplified by this operation degrading inevitably the PID performance and causing instability.

Thereby, you should instead resort to more sophisticated techniques such as velocity observers (e.g. the Kalman state estimator) or robust filtering (e.g. Savitzky–Golay filtering), or even better sensors that are apt to measure the velocity natively (they are rare and quite expensive devices though).

Don't panic/worry, just drop the derivative term lightheartedly. Remember that 90% of the worldwide PID controllers are simply PI

Integral term

This is the most problematic part. You cannot threshold the controller output within [0, maxSpeed] without telling the PID what is going on. It won't simply work out. By doing this, in fact, you will elicit unwanted oscillations that are right what you have been observing.

To let the controller be aware of actuation bounds, you shall rely on a standard anti-windup diagram.

This is a well-known procedure in literature. You can find it out in the bible of PID controllers (par. 3.5), plus you could read up this interesting MATLAB resource.

Essentially, you need to integrate also the difference between the thresholded output and the sheer output (see the illustration below).

```<code>integralError += (currentError + thresOutput - sheerOutput) * timeStep
</code>```

Final remarks

Once you'll have fiddled with all the points above, you'll certainly have more chances to reduce the over/undershoots.
That being said, a pure velocity controller turns to be a kind of simplistic approach. Therefore, to be even more effective and accurate, you should be eventually playing with the dynamical model of the quadcopter.

Hope it helps out.

Title: Quadcopter PID Controller for distance

Body:
I don't know to which extent your pseudo-code can be taken as representative of your real code, but I can see the following concerns with it.

Derivative term

In practical applications, the derivative term computed as a one-step finite difference as below is completely useless:

derivativeError = (currentError - previousError) / timeStep

There is a large number of reasons for that, mainly related to the fact that physical signals just like the measured distance from the target are heavily affected by the noise, which in turn has high-frequency components that get amplified by this operation degrading inevitably the PID performance and causing instability.

Thereby, you should instead resort to more sophisticated techniques such as velocity observers (e.g. the Kalman state estimator) or robust filtering (e.g. Savitzky–Golay filtering), or even better sensors that are apt to measure the velocity natively (they are rare and quite expensive devices though).

Don't panic/worry, just drop the derivative term lightheartedly. Remember that 90% of the worldwide PID controllers are simply PI

Integral term

This is the most problematic part. You cannot threshold the controller output within [0, maxSpeed] without telling the PID what is going on. It won't simply work out. By doing this, in fact, you will elicit unwanted oscillations that are right what you have been observing.

To let the controller be aware of actuation bounds, you shall rely on a standard anti-windup diagram.

This is a well-known procedure in literature. You can find it out in the bible of PID controllers (par. 3.5), plus you could read up this interesting MATLAB resource.

Essentially, you need to integrate also the difference between the thresholded output and the sheer output (see the illustration below).

integralError += (currentError + thresOutput - sheerOutput) * timeStep

Final remarks

Once you'll have fiddled with all the points above, you'll certainly have more chances to reduce the over/undershoots.
That being said, a pure velocity controller turns to be a kind of simplistic approach. Therefore, to be even more effective and accurate, you should be eventually playing with the dynamical model of the quadcopter.

Hope it helps out.


问:
High derivative gain in PD control
Why does high derivative gain in PD control results in motor noise or chatter?
The graph that I see on MATLAB simulation is smooth enough, so following that it should not make noise..</p>

答:
Chuck's answer is spot on. Anyway, if you want to derive the reason mathematically, you can start off from the most common form of a PD controller where we employ a setpoint-weighting for the derivative part:

$$
u(t) = K \cdot \left( e(t) - T_d \cdot \dot{y}(t) \right).
$$

The Laplace transform of a feasible $D$ term is thus:

$$
D(s) = -\frac{sKT_d}{1+sT_d/N}\cdot Y(s),
$$

where $N$ is typically in the order of 8÷20 since the pole $-N/T_d$ needs to be located at high frequency to make $D(s)$ behave like a derivative at the frequencies of interest.

The high-frequency gain of a PD controller is finally $K^{PD}_{hf} = K(1+N)$, which is $(1+N)$ times higher than the high-frequency gain of a simple P controller $K^P_{hf} = K$.

This means that a PD controller enhances $(1+N)$ times the high-frequency noise – which is always present in the closed-loop – with respect to the pure proportional counterpart.

Put differently, a sinusoidal measurement $y=a\sin\omega t$ gives the following contribution to the derivative term of the control signal:
$$
u_D=-aKT_d\omega \cos\omega t.
$$

The amplitude of the control signal can thus be arbitrarily large if the noise has a sufficiently high frequency ($\omega$). The high-frequency gain of the derivative term is therefore limited to avoid this difficulty.

ContentLicense: CC BY-SA 4.0

Title: High derivative gain in PD control


问:
Is Kalman filter really desired here?
I am trying to use Kalman Filter in my project to eliminate outliers that go beyond certain limit.

I am use 1D lidar to get the distance between the robot and an object. I get pretty accurate values in mm. The output from 3 such lidars looks as follows:

Now, I know that the outlier can be removed using Kalman Filter + Maholobonis distance. It is efficient in doing so. But I too can do that by passing all the values through a limiter (that rejects all values above a threshold). (e.g., `if(val > threshold): continue).

Another inherent feature of this Kalman filter is that it smoothes the values, as shown in this demo.

(

Do I really need a Kalman Filter ?

答:
TL;DR

Nope! You don't really need Kalman filtering in this context!

To perform outliers rejection, there are other techniques that you can apply successfully and more easily without the bother of dealing with the Kalman filter's requirements.

Kalman filtering can be demonstrated to be the optimal stochastic filter when we have:

a linear model of our process.

Gaussian noise affecting the measurements.

In reality, neither condition 1 nor condition 2 are ever verified, as the model cannot encapsulate the whole knowledge of our physical system and the noise is never perfectly Gaussian. Anyway, engineers do not tend to get scared about that and keep applying Kalman filtering and its variants everywhere regardless of the extent to which those conditions are true.

If we think about it, the same applies to PID controllers, which are linear systems and as such can be used only paired with linear processes. There exists no single linear process in nature. What the heck? Well, who cares, we keep on using both PID and Kalman estimator, which do work really great!

In this digression, I have purposely exaggerated the context, as engineers are pretty much careful to gauge the limits of applicability of these useful linear tools (the extent I mentioned earlier). To this end, they study the model of the process at hand together with its relevant properties, which is often the very first step of the analysis.

Do you have a model for your process and a rough estimate of the noise statistics? If the answer is yes, you could continue to have a deeper look and make attempts to characterize this model (identification) and work out a good first guess for the noise and process variance to then run some tests to verify your hypotheses. However, this would entail that you sit down and put in a non-negligible effort.

Want to try out something simpler, where resorting to heuristics is – so to speak – more reasonable and sound than betting with Kalman parameters? Well, make use of Median Filtering!

Median filtering is a very simple nonlinear technique that does its best with outliers removal without introducing significant lag in the response.

You can appreciate the 1D median filter at work here:
https://www.mathworks.com/help/signal/ref/medfilt1.html

Title: Is Kalman filter really desired here?


问:
How can we calculate likelihood of sensor values wrt to actual real world?
I am trying to understand Bayes Filter. In its update step we have P(z_t|x_t) as observation model.

Slide 37: Bullet 5 of this lecture states "Likelihood of measurement is given by “probabilistically comparing” the actual with the expected measurement."

For example from the following data:

Observation: [0, 1, 0, 0, 0, 0, 1, 0, 0, 0] -> This what the robot sense

Actual World: [0, 0, 1, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1, 0]

And actual world we can say [0, 1] resembles to [white, black] tile and sensor recognize them with [White: 0.7, Black: 0.9] probability. I am not able to understand here how can we calculate this likelihood for each step
?

答:
The likelihood is with respect to your predicted state not the world state (You will never know the world state)

For example, if you predict you will be at [0,0] but the measurement tells you [.5,.5] you can calculate the likelihood of that measurement given your predicted state. In that scenario, you can use a normal distribution or some more complex function to calculate the likelihood.

In your case of a white black tile vector you can possibly treat each entry as a binomial distribution then calculate the joint probability.


问:
catkin_make unable to build and throws "Makefile:138: recipe for target 'all' failed make: *** [all] Error 2 Invoking "make -j4 -l4" failed"
I have been trying to build my ROS workspace using catkin_make, where i have cloned a repo/PACKAGE. I tried many of the things mentioned in previous answers to questions similar to my issue but without any progress. I tried building another package and it worked well. I am new to ROS and Linux, and this issue has been impeding me for a 3 days now.

Many thanks in advance!

```abouseif@abouseif-ThinkCentre-M92p:~/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws$ catkin_make
Base path: /home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws
Source space: /home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/src
Build space: /home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/build
Devel space: /home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/devel
Install space: /home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/install
####
#### Running command: "cmake /home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/src -DCATKIN_DEVEL_PREFIX=/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/devel -DCMAKE_INSTALL_PREFIX=/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/install -G Unix Makefiles" in "/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/build"
####
-- Using CATKIN_DEVEL_PREFIX: /home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/devel
-- Using CMAKE_PREFIX_PATH: /opt/ros/kinetic
-- This workspace overlays: /opt/ros/kinetic
-- Using PYTHON_EXECUTABLE: /usr/bin/python
-- Using Debian Python package layout
-- Using empy: /usr/bin/empy
-- Using CATKIN_ENABLE_TESTING: ON
-- Call enable_testing()
-- Using CATKIN_TEST_RESULTS_DIR: /home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/build/test_results
-- Found gtest sources under '/usr/src/gtest': gtests will be built
-- Using Python nosetests: /usr/bin/nosetests-2.7
-- catkin 0.7.8
-- BUILD_SHARED_LIBS is on
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- ~~  traversing 3 packages in topological order:
-- ~~  - gazebo_grasp_plugin
-- ~~  - kuka_arm
-- ~~  - kr210_claw_moveit
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- +++ processing catkin package: 'gazebo_grasp_plugin'
-- ==> add_subdirectory(RoboND-Kinematics-Project/gazebo_grasp_plugin)
-- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
-- Boost version: 1.58.0
-- Found the following Boost libraries:
--   thread
--   signals
--   system
--   filesystem
--   program_options
--   regex
--   iostreams
--   date_time
--   chrono
--   atomic
-- Found Protobuf: /usr/lib/x86_64-linux-gnu/libprotobuf.so
-- Boost version: 1.58.0
-- Looking for OGRE...
-- OGRE_PREFIX_WATCH changed.
-- Found PkgConfig: /usr/bin/pkg-config (found version "0.29.1")
-- Checking for module 'OGRE'
--   Found OGRE, version 1.9.0
-- Found Ogre Ghadamon (1.9.0)
-- Found OGRE: optimized;/usr/lib/x86_64-linux-gnu/libOgreMain.so;debug;/usr/lib/x86_64-linux-gnu/libOgreMain.so
-- Looking for OGRE_Paging...
-- Found OGRE_Paging: optimized;/usr/lib/x86_64-linux-gnu/libOgrePaging.so;debug;/usr/lib/x86_64-linux-gnu/libOgrePaging.so
-- Looking for OGRE_Terrain...
-- Found OGRE_Terrain: optimized;/usr/lib/x86_64-linux-gnu/libOgreTerrain.so;debug;/usr/lib/x86_64-linux-gnu/libOgreTerrain.so
-- Looking for OGRE_Property...
-- Found OGRE_Property: optimized;/usr/lib/x86_64-linux-gnu/libOgreProperty.so;debug;/usr/lib/x86_64-linux-gnu/libOgreProperty.so
-- Looking for OGRE_RTShaderSystem...
-- Found OGRE_RTShaderSystem: optimized;/usr/lib/x86_64-linux-gnu/libOgreRTShaderSystem.so;debug;/usr/lib/x86_64-linux-gnu/libOgreRTShaderSystem.so
-- Looking for OGRE_Volume...
-- Found OGRE_Volume: optimized;/usr/lib/x86_64-linux-gnu/libOgreVolume.so;debug;/usr/lib/x86_64-linux-gnu/libOgreVolume.so
-- Looking for OGRE_Overlay...
-- Found OGRE_Overlay: optimized;/usr/lib/x86_64-linux-gnu/libOgreOverlay.so;debug;/usr/lib/x86_64-linux-gnu/libOgreOverlay.so
CMake Warning at /opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake:166 (message):
  catkin_package() DEPENDS on 'gazebo' but neither 'gazebo_INCLUDE_DIRS' nor
  'gazebo_LIBRARIES' is defined.
Call Stack (most recent call first):
  /opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake:102 (_catkin_package)
  RoboND-Kinematics-Project/gazebo_grasp_plugin/CMakeLists.txt:32 (catkin_package)

-- +++ processing catkin package: 'kuka_arm'
-- ==> add_subdirectory(RoboND-Kinematics-Project/kuka_arm)
-- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
-- kuka_arm: 0 messages, 1 services
-- +++ processing catkin package: 'kr210_claw_moveit'
-- ==> add_subdirectory(RoboND-Kinematics-Project/kr210_claw_moveit)
-- Configuring done
WARNING: Target "trajectory_sampler" requests linking to directory "/opt/ros/kinetic/lib/roslib".  Targets may link only to libraries.  CMake is dropping the item.
-- Generating done
-- Build files have been written to: /home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/build
####
#### Running command: "make -j4 -l4" in "/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/build"
####
Scanning dependencies of target trajectory_msgs_generate_messages_lisp
Scanning dependencies of target actionlib_msgs_generate_messages_lisp
Scanning dependencies of target trajectory_msgs_generate_messages_nodejs
Scanning dependencies of target trajectory_msgs_generate_messages_py
[  0%] Built target trajectory_msgs_generate_messages_nodejs
[  0%] Built target actionlib_msgs_generate_messages_lisp
[  0%] Built target trajectory_msgs_generate_messages_lisp
[  0%] Built target trajectory_msgs_generate_messages_py
Scanning dependencies of target trajectory_msgs_generate_messages_eus
Scanning dependencies of target gazebo_msgs_generate_messages_py
Scanning dependencies of target dynamic_reconfigure_gencfg
Scanning dependencies of target dynamic_reconfigure_generate_messages_py
[  0%] Built target trajectory_msgs_generate_messages_eus
[  0%] Built target dynamic_reconfigure_generate_messages_py
[  0%] Built target gazebo_msgs_generate_messages_py
[  0%] Built target dynamic_reconfigure_gencfg
Scanning dependencies of target dynamic_reconfigure_generate_messages_cpp
Scanning dependencies of target dynamic_reconfigure_generate_messages_eus
Scanning dependencies of target dynamic_reconfigure_generate_messages_lisp
Scanning dependencies of target tf2_msgs_generate_messages_nodejs
[  0%] Built target dynamic_reconfigure_generate_messages_cpp
[  0%] Built target dynamic_reconfigure_generate_messages_eus
[  0%] Built target dynamic_reconfigure_generate_messages_lisp
[  0%] Built target tf2_msgs_generate_messages_nodejs
Scanning dependencies of target tf2_msgs_generate_messages_lisp
Scanning dependencies of target tf2_msgs_generate_messages_eus
Scanning dependencies of target tf2_msgs_generate_messages_cpp
Scanning dependencies of target dynamic_reconfigure_generate_messages_nodejs
[  0%] Built target tf2_msgs_generate_messages_lisp
[  0%] Built target tf2_msgs_generate_messages_eus
[  0%] Built target tf2_msgs_generate_messages_cpp
[  0%] Built target dynamic_reconfigure_generate_messages_nodejs
Scanning dependencies of target std_srvs_generate_messages_cpp
Scanning dependencies of target _catkin_empty_exported_target
Scanning dependencies of target roscpp_generate_messages_nodejs
Scanning dependencies of target tf_generate_messages_eus
[  0%] Built target std_srvs_generate_messages_cpp
[  0%] Built target _catkin_empty_exported_target
[  0%] Built target roscpp_generate_messages_nodejs
[  0%] Built target tf_generate_messages_eus
Scanning dependencies of target geometry_msgs_generate_messages_nodejs
Scanning dependencies of target std_msgs_generate_messages_lisp
Scanning dependencies of target roscpp_generate_messages_lisp
[  0%] Built target geometry_msgs_generate_messages_nodejs
Scanning dependencies of target geometry_msgs_generate_messages_lisp
[  0%] Built target std_msgs_generate_messages_lisp
[  0%] Built target roscpp_generate_messages_lisp
Scanning dependencies of target std_srvs_generate_messages_lisp
Scanning dependencies of target geometry_msgs_generate_messages_eus
[  0%] Built target geometry_msgs_generate_messages_lisp
Scanning dependencies of target roscpp_generate_messages_cpp
[  0%] Built target std_srvs_generate_messages_lisp
[  0%] Built target geometry_msgs_generate_messages_eus
Scanning dependencies of target actionlib_msgs_generate_messages_py
Scanning dependencies of target std_msgs_generate_messages_py
[  0%] Built target roscpp_generate_messages_cpp
Scanning dependencies of target gazebo_msgs_generate_messages_nodejs
[  0%] Built target actionlib_msgs_generate_messages_py
[  0%] Built target std_msgs_generate_messages_py
Scanning dependencies of target gazebo_msgs_generate_messages_cpp
[  0%] Built target gazebo_msgs_generate_messages_nodejs
Scanning dependencies of target std_msgs_generate_messages_nodejs
Scanning dependencies of target geometry_msgs_generate_messages_py
[  0%] Built target gazebo_msgs_generate_messages_cpp
Scanning dependencies of target trajectory_msgs_generate_messages_cpp
[  0%] Built target std_msgs_generate_messages_nodejs
[  0%] Built target geometry_msgs_generate_messages_py
Scanning dependencies of target geometry_msgs_generate_messages_cpp
[  0%] Built target trajectory_msgs_generate_messages_cpp
Scanning dependencies of target sensor_msgs_generate_messages_nodejs
Scanning dependencies of target rosgraph_msgs_generate_messages_cpp
[  0%] Built target geometry_msgs_generate_messages_cpp
Scanning dependencies of target rosgraph_msgs_generate_messages_nodejs
[  0%] Built target sensor_msgs_generate_messages_nodejs
[  0%] Built target rosgraph_msgs_generate_messages_cpp
[  0%] Built target rosgraph_msgs_generate_messages_nodejs
Scanning dependencies of target roscpp_generate_messages_eus
Scanning dependencies of target std_msgs_generate_messages_eus
Scanning dependencies of target std_srvs_generate_messages_py
Scanning dependencies of target gazebo_msgs_generate_messages_eus
[  0%] Built target roscpp_generate_messages_eus
[  0%] Built target std_msgs_generate_messages_eus
[  0%] Built target std_srvs_generate_messages_py
[  0%] Built target gazebo_msgs_generate_messages_eus
Scanning dependencies of target rosgraph_msgs_generate_messages_eus
Scanning dependencies of target std_srvs_generate_messages_eus
Scanning dependencies of target tf_generate_messages_cpp
Scanning dependencies of target actionlib_generate_messages_eus
[  0%] Built target rosgraph_msgs_generate_messages_eus
[  0%] Built target tf_generate_messages_cpp
[  0%] Built target actionlib_generate_messages_eus
[  0%] Built target std_srvs_generate_messages_eus
Scanning dependencies of target rosgraph_msgs_generate_messages_lisp
Scanning dependencies of target std_srvs_generate_messages_nodejs
Scanning dependencies of target gazebo_ros_gencfg
Scanning dependencies of target rosgraph_msgs_generate_messages_py
[  0%] Built target rosgraph_msgs_generate_messages_lisp
[  0%] Built target gazebo_ros_gencfg
[  0%] Built target rosgraph_msgs_generate_messages_py
[  0%] Built target std_srvs_generate_messages_nodejs
Scanning dependencies of target tf_generate_messages_lisp
Scanning dependencies of target roscpp_generate_messages_py
Scanning dependencies of target tf_generate_messages_py
Scanning dependencies of target tf_generate_messages_nodejs
[  0%] Built target tf_generate_messages_lisp
[  0%] Built target roscpp_generate_messages_py
[  0%] Built target tf_generate_messages_py
[  0%] Built target tf_generate_messages_nodejs
Scanning dependencies of target sensor_msgs_generate_messages_cpp
Scanning dependencies of target gazebo_msgs_generate_messages_lisp
Scanning dependencies of target sensor_msgs_generate_messages_lisp
Scanning dependencies of target sensor_msgs_generate_messages_py
[  0%] Built target sensor_msgs_generate_messages_cpp
[  0%] Built target gazebo_msgs_generate_messages_lisp
[  0%] Built target sensor_msgs_generate_messages_lisp
[  0%] Built target sensor_msgs_generate_messages_py
Scanning dependencies of target tf2_msgs_generate_messages_py
Scanning dependencies of target actionlib_generate_messages_cpp
Scanning dependencies of target std_msgs_generate_messages_cpp
Scanning dependencies of target actionlib_msgs_generate_messages_nodejs
[  0%] Built target actionlib_generate_messages_cpp
[  0%] Built target tf2_msgs_generate_messages_py
[  0%] Built target actionlib_msgs_generate_messages_nodejs
[  0%] Built target std_msgs_generate_messages_cpp
Scanning dependencies of target actionlib_msgs_generate_messages_cpp
Scanning dependencies of target sensor_msgs_generate_messages_eus
Scanning dependencies of target actionlib_generate_messages_py
Scanning dependencies of target actionlib_generate_messages_nodejs
[  0%] Built target sensor_msgs_generate_messages_eus
[  0%] Built target actionlib_msgs_generate_messages_cpp
[  0%] Built target actionlib_generate_messages_py
[  0%] Built target actionlib_generate_messages_nodejs
Scanning dependencies of target actionlib_generate_messages_lisp
Scanning dependencies of target actionlib_msgs_generate_messages_eus
Scanning dependencies of target graph_msgs_generate_messages_lisp
Scanning dependencies of target octomap_msgs_generate_messages_cpp
[  0%] Built target actionlib_generate_messages_lisp
[  0%] Built target actionlib_msgs_generate_messages_eus
[  0%] Built target octomap_msgs_generate_messages_cpp
[  0%] Built target graph_msgs_generate_messages_lisp
Scanning dependencies of target object_recognition_msgs_generate_messages_nodejs
Scanning dependencies of target object_recognition_msgs_generate_messages_lisp
Scanning dependencies of target octomap_msgs_generate_messages_py
Scanning dependencies of target moveit_msgs_generate_messages_lisp
[  0%] Built target object_recognition_msgs_generate_messages_nodejs
[  0%] Built target object_recognition_msgs_generate_messages_lisp
[  0%] Built target octomap_msgs_generate_messages_py
[  0%] Built target moveit_msgs_generate_messages_lisp
Scanning dependencies of target moveit_msgs_generate_messages_eus
Scanning dependencies of target object_recognition_msgs_generate_messages_eus
Scanning dependencies of target visualization_msgs_generate_messages_lisp
Scanning dependencies of target moveit_msgs_generate_messages_cpp
[  0%] Built target object_recognition_msgs_generate_messages_eus
[  0%] Built target visualization_msgs_generate_messages_lisp
[  0%] Built target moveit_msgs_generate_messages_eus
Scanning dependencies of target object_recognition_msgs_generate_messages_py
[  0%] Built target moveit_msgs_generate_messages_cpp
Scanning dependencies of target graph_msgs_generate_messages_cpp
Scanning dependencies of target _kuka_arm_generate_messages_check_deps_CalculateIK
[  0%] Built target object_recognition_msgs_generate_messages_py
Scanning dependencies of target shape_msgs_generate_messages_cpp
[  0%] Built target graph_msgs_generate_messages_cpp
Scanning dependencies of target graph_msgs_generate_messages_py
[  0%] Built target shape_msgs_generate_messages_cpp
Scanning dependencies of target moveit_msgs_generate_messages_nodejs
Scanning dependencies of target shape_msgs_generate_messages_py
[  0%] Built target moveit_msgs_generate_messages_nodejs
[  0%] Built target graph_msgs_generate_messages_py
[  0%] Built target shape_msgs_generate_messages_py
Scanning dependencies of target visualization_msgs_generate_messages_cpp
Scanning dependencies of target visualization_msgs_generate_messages_py
[  0%] Built target _kuka_arm_generate_messages_check_deps_CalculateIK
[  0%] Built target visualization_msgs_generate_messages_cpp
Scanning dependencies of target visualization_msgs_generate_messages_eus
[  0%] Built target visualization_msgs_generate_messages_py
Scanning dependencies of target object_recognition_msgs_generate_messages_cpp
Scanning dependencies of target visualization_msgs_generate_messages_nodejs
[  0%] Built target visualization_msgs_generate_messages_eus
Scanning dependencies of target octomap_msgs_generate_messages_eus
[  0%] Built target object_recognition_msgs_generate_messages_cpp
[  0%] Built target visualization_msgs_generate_messages_nodejs
[  0%] Built target octomap_msgs_generate_messages_eus
Scanning dependencies of target moveit_msgs_generate_messages_py
Scanning dependencies of target octomap_msgs_generate_messages_lisp
Scanning dependencies of target graph_msgs_generate_messages_eus
Scanning dependencies of target shape_msgs_generate_messages_lisp
[  0%] Built target moveit_msgs_generate_messages_py
[  0%] Built target octomap_msgs_generate_messages_lisp
[  0%] Built target graph_msgs_generate_messages_eus
Scanning dependencies of target kuka_arm_generate_messages_py
[  0%] Built target shape_msgs_generate_messages_lisp
Scanning dependencies of target shape_msgs_generate_messages_eus
[  8%] Generating Python code from SRV kuka_arm/CalculateIK
Scanning dependencies of target octomap_msgs_generate_messages_nodejs
Scanning dependencies of target kuka_arm_generate_messages_eus
[  8%] Built target shape_msgs_generate_messages_eus
[ 16%] Built target octomap_msgs_generate_messages_nodejs
[ 16%] Generating EusLisp code from kuka_arm/CalculateIK.srv
Scanning dependencies of target moveit_ros_planning_gencfg
[ 25%] Generating EusLisp manifest code for kuka_arm
[ 25%] Built target moveit_ros_planning_gencfg
Scanning dependencies of target graph_msgs_generate_messages_nodejs
[ 25%] Built target graph_msgs_generate_messages_nodejs
Scanning dependencies of target moveit_ros_manipulation_gencfg
[ 25%] Built target moveit_ros_manipulation_gencfg
Scanning dependencies of target kuka_arm_generate_messages_cpp
[ 33%] Generating C++ code from kuka_arm/CalculateIK.srv
Scanning dependencies of target kuka_arm_generate_messages_nodejs
[ 41%] Generating Javascript code from kuka_arm/CalculateIK.srv
[ 50%] Generating Python srv __init__.py for kuka_arm
[ 50%] Built target kuka_arm_generate_messages_nodejs
Scanning dependencies of target shape_msgs_generate_messages_nodejs
[ 50%] Built target shape_msgs_generate_messages_nodejs
Scanning dependencies of target gazebo_grasp_fix
[ 50%] Built target kuka_arm_generate_messages_py
Scanning dependencies of target kuka_arm_generate_messages_lisp
[ 58%] Generating Lisp code from kuka_arm/CalculateIK.srv
[ 58%] Built target kuka_arm_generate_messages_cpp
[ 58%] Built target kuka_arm_generate_messages_lisp
[ 75%] Building CXX object RoboND-Kinematics-Project/gazebo_grasp_plugin/CMakeFiles/gazebo_grasp_fix.dir/src/GazeboGraspFix.cpp.o
[ 75%] Building CXX object RoboND-Kinematics-Project/gazebo_grasp_plugin/CMakeFiles/gazebo_grasp_fix.dir/src/GazeboGraspGripper.cpp.o
[ 75%] Built target kuka_arm_generate_messages_eus
Scanning dependencies of target kuka_arm_generate_messages
Scanning dependencies of target trajectory_sampler
[ 75%] Built target kuka_arm_generate_messages
[ 83%] Building CXX object RoboND-Kinematics-Project/kuka_arm/CMakeFiles/trajectory_sampler.dir/src/trajectory_sampler.cpp.o
/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/src/trajectory_sampler.cpp: In constructor ‘TrajectorySampler::TrajectorySampler(ros::NodeHandle)’:
/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/src/trajectory_sampler.cpp:180:43: error: cannot convert ‘moveit::planning_interface::MoveItErrorCode’ to ‘bool’ in initialization
     bool success = move_group.plan(my_plan);
                                           ^
/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/src/trajectory_sampler.cpp:224:15: error: cannot convert ‘moveit::planning_interface::MoveItErrorCode’ to ‘bool’ in assignment
       success = move_group.execute(my_plan);
               ^
/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/src/trajectory_sampler.cpp:291:39: error: cannot convert ‘moveit::planning_interface::MoveItErrorCode’ to ‘bool’ in initialization
         bool worked = move_group.move();
                                       ^
/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/src/trajectory_sampler.cpp:313:13: error: cannot convert ‘moveit::planning_interface::MoveItErrorCode’ to ‘bool’ in assignment
     success = move_group.move();
             ^
/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/src/trajectory_sampler.cpp:339:13: error: cannot convert ‘moveit::planning_interface::MoveItErrorCode’ to ‘bool’ in assignment
     success = move_group.move();
             ^
/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/src/trajectory_sampler.cpp:351:13: error: cannot convert ‘moveit::planning_interface::MoveItErrorCode’ to ‘bool’ in assignment
     success = move_group.plan(my_plan);
             ^
/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/src/trajectory_sampler.cpp:375:15: error: cannot convert ‘moveit::planning_interface::MoveItErrorCode’ to ‘bool’ in assignment
       success = move_group.execute(my_plan);
               ^
/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/src/trajectory_sampler.cpp:452:39: error: cannot convert ‘moveit::planning_interface::MoveItErrorCode’ to ‘bool’ in initialization
         bool worked = move_group.move();
                                       ^
/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/src/trajectory_sampler.cpp:497:13: error: cannot convert ‘moveit::planning_interface::MoveItErrorCode’ to ‘bool’ in assignment
     success = move_group.move();
             ^
/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/src/trajectory_sampler.cpp: In member function ‘bool TrajectorySampler::OperateGripper(const bool&)’:
/home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/src/trajectory_sampler.cpp:543:33: error: cannot convert ‘moveit::planning_interface::MoveItErrorCode’ to ‘bool’ in initialization
   bool success = eef_group.move();
                                 ^
RoboND-Kinematics-Project/kuka_arm/CMakeFiles/trajectory_sampler.dir/build.make:62: recipe for target 'RoboND-Kinematics-Project/kuka_arm/CMakeFiles/trajectory_sampler.dir/src/trajectory_sampler.cpp.o' failed
make[2]: *** [RoboND-Kinematics-Project/kuka_arm/CMakeFiles/trajectory_sampler.dir/src/trajectory_sampler.cpp.o] Error 1
CMakeFiles/Makefile2:3178: recipe for target 'RoboND-Kinematics-Project/kuka_arm/CMakeFiles/trajectory_sampler.dir/all' failed
make[1]: *** [RoboND-Kinematics-Project/kuka_arm/CMakeFiles/trajectory_sampler.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
[ 91%] Linking CXX shared library /home/abouseif/main/udacity/roboticsnd/robotics_term1/ROS/catkin_ws/devel/lib/libgazebo_grasp_fix.so
[ 91%] Built target gazebo_grasp_fix
Makefile:138: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j4 -l4" failed
```

答:
Running catkin_make -j8 in my catkin workspace in the Python base environment, the error message was Invoking "make -j8" failed instead of Invoking "make -j4 -l4“ failed".

Here is the traceback:

```<code>Traceback (most recent call last):
  File "/opt/ros/melodic/share/gencpp/cmake/../../../lib/gencpp/gen_cpp.py", line 43, in <module>
    import genmsg.template_tools
  File "/opt/ros/melodic/lib/python2.7/dist-packages/genmsg/template_tools.py", line 39, in <module>
    import em
ImportError: No module named em
util/ros_canopen/can_msgs/CMakeFiles/can_msgs_generate_messages_cpp.dir/build.make:63: recipe for target '/home/stu16291/rosyard/devel/include/can_msgs/Frame.h' failed
make[2]: *** [/home/stu16291/rosyard/devel/include/can_msgs/Frame.h] Error 1
CMakeFiles/Makefile2:1641: recipe for target 'util/ros_canopen/can_msgs/CMakeFiles/can_msgs_generate_messages_cpp.dir/all' failed
make[1]: *** [util/ros_canopen/can_msgs/CMakeFiles/can_msgs_generate_messages_cpp.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
[  4%] Built target can_msgs_generate_messages_lisp
[  4%] Linking CXX executable /home/stu16291/rosyard/devel/lib/robot_localization/test_ros_robot_localization_listener_publisher
[  4%] Built target test_ros_robot_localization_listener_publisher
[  4%] Linking CXX shared library /home/stu16291/rosyard/devel/lib/libddynamic_reconfigure.so
[  4%] Built target ddynamic_reconfigure
[  6%] Linking CXX shared library /home/stu16291/rosyard/devel/lib/libros_filter_utilities.so
[  6%] Built target ros_filter_utilities
[  6%] Linking CXX executable /home/stu16291/rosyard/devel/lib/socketcan_interface/socketcan_dump
[  6%] Built target socketcan_dump
[  6%] Linking CXX shared library /home/stu16291/rosyard/devel/lib/libcomplementary_filter.so
[  6%] Built target complementary_filter
[  7%] Linking CXX shared library /home/stu16291/rosyard/devel/lib/libvelodyne_sensor_plugin.so
[  7%] Built target velodyne_sensor_plugin
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j8" failed
</code>```

Solution was to remove the catkin workspace and install it again.

Remove:
In your catkin workspace, usually your project folder, delete (or rename with suffix "_old" if you want to compare the not working catkin workspace later with the working catkin workspace):

build

devel

optionally src/CMakeLists.txt (I did that, not sure if it is really needed)

Source:
Removing catkin workspace

Afterwards, the fresh catkin_make -j8 in my catkin workspace in the Python base environment installed without errors.

ContentLicense: CC BY-SA 3.0

Body:
Running catkin_make -j8 in my catkin workspace in the Python base environment, the error message was Invoking "make -j8" failed instead of Invoking "make -j4 -l4“ failed".

Here is the traceback:

Traceback (most recent call last):
  File "/opt/ros/melodic/share/gencpp/cmake/../../../lib/gencpp/gen_cpp.py", line 43, in <module>
    import genmsg.template_tools
  File "/opt/ros/melodic/lib/python2.7/dist-packages/genmsg/template_tools.py", line 39, in <module>
    import em
ImportError: No module named em
util/ros_canopen/can_msgs/CMakeFiles/can_msgs_generate_messages_cpp.dir/build.make:63: recipe for target '/home/stu16291/rosyard/devel/include/can_msgs/Frame.h' failed
make[2]: *** [/home/stu16291/rosyard/devel/include/can_msgs/Frame.h] Error 1
CMakeFiles/Makefile2:1641: recipe for target 'util/ros_canopen/can_msgs/CMakeFiles/can_msgs_generate_messages_cpp.dir/all' failed
make[1]: *** [util/ros_canopen/can_msgs/CMakeFiles/can_msgs_generate_messages_cpp.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
[  4%] Built target can_msgs_generate_messages_lisp
[  4%] Linking CXX executable /home/stu16291/rosyard/devel/lib/robot_localization/test_ros_robot_localization_listener_publisher
[  4%] Built target test_ros_robot_localization_listener_publisher
[  4%] Linking CXX shared library /home/stu16291/rosyard/devel/lib/libddynamic_reconfigure.so
[  4%] Built target ddynamic_reconfigure
[  6%] Linking CXX shared library /home/stu16291/rosyard/devel/lib/libros_filter_utilities.so
[  6%] Built target ros_filter_utilities
[  6%] Linking CXX executable /home/stu16291/rosyard/devel/lib/socketcan_interface/socketcan_dump
[  6%] Built target socketcan_dump
[  6%] Linking CXX shared library /home/stu16291/rosyard/devel/lib/libcomplementary_filter.so
[  6%] Built target complementary_filter
[  7%] Linking CXX shared library /home/stu16291/rosyard/devel/lib/libvelodyne_sensor_plugin.so
[  7%] Built target velodyne_sensor_plugin
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j8" failed

Solution was to remove the catkin workspace and install it again.

Remove:
In your catkin workspace, usually your project folder, delete (or rename with suffix "_old" if you want to compare the not working catkin workspace later with the working catkin workspace):

build

devel

optionally src/CMakeLists.txt (I did that, not sure if it is really needed)

Source:
Removing catkin workspace

Afterwards, the fresh catkin_make -j8 in my catkin workspace in the Python base environment installed without errors.


问:
Failed to load plugin libgazebo...: libgazebo...: cannot open shared object file: No such file or directory
When running a command similar to the roslaunch gazebo_ros empty_world.launch of the Tutorial: Using roslaunch to start Gazebo, world files and URDF models, I get the following error:

[Err] [Plugin.hh:180] Failed to load plugin libgazebo[...]:
libgazebo[...]: cannot open shared object file: No such file or
directory

How to solve this?

答:
Solution was to remove the catkin workspace and install it again.

Remove:
In your catkin workspace, usually your project folder, delete (or rename with suffix "_old" if you want to compare the not working catkin workspace later with the working catkin workspace):

build

devel

optionally src/CMakeLists.txt (I did that, not sure if it is really needed)

Source:
Removing catkin workspace

Afterwards, the fresh catkin_make -j8 (or whatever catkin command you use) in my catkin workspace in the Python base environment installed without errors and the roslaunch command ran through.


问:
Why Does an Exponential Make ANYTHING a Probability Distribution
I am posting this here because my background in estimation theory and optimization has been developed entirely through my experience in robotics.

TLDR: What makes it so that any time you put something in an exponential you can suddenly call it a probability distribution?

The Deets: I thought of this question while reading A General and Adaptive Robust Loss Function by Jonathan Barron. The paper describes a single parameter loss function that can subsume the most commonly used individual loss functions:

$$f(x, \alpha, c)$$

Where $x$ is parameter, $\alpha$ is the "single parameter", and $c$ is a distance scale that describes the "quadratic bowl" around the origin which all the loss functions share in common. When $\alpha = 2$ it is squared error loss, $\alpha =1$ it is essentially absolute error loss, and when $\alpha = 0$ it is cauchy loss.

My confusion starts when the paper goes on to say "With our loss function we construct a general probability distribution".

$$ p(x|\mu,\alpha, c) = \frac{1}{cZ(\alpha)}\exp(-f(x-\mu, \alpha, c)) $$

My question is, why does dividing by a normalization constant, and taking the exponential of the negative loss function suddenly make it a pdf? I understand the results, I have used these same ideas in my estimation theory courses, but I just don't get how we can make the jump from loss to pdf OR why it is so intuitive to do so.

Cheers, and happy holidays :)

答:
I believe it's because you're essentially constructing an exponential distribution which has the form

Because your loss function will always be >= 0, you form a valid PDF (valid in that it integrates to 1, but your loss function might not make that practically true)


问:
Particle Filter for IMU tilt angle and bias estimation from Kalman Filter models
I understand the functioning of Particle Filters from the book Probabilistic Robotics and the robotics course provided by Cyrill Stachniss.

I want to implement, from scratch, a particle filter to estimate the tilt angle $\theta$, angular velocity $\omega := \dot\theta$ and bias $b$ in one direction. I want to implement the most basic PF version as shown below. I have the mathematical model (that I can't post here for legal reasons) to do so when I was learning Kalman Filters in a University course. To explain in short, I have a process model,
\begin{equation}
x_k = A x_{t-1},
\end{equation}
and a corresponding motion uncertainty matrix $Q$. Similarly, I have a measurement model,
\begin{equation}
z_t = C x_t,
\end{equation}
and the measurement covariance matrix $R$.

How do I go from this model to implementing the same in Particle Filters? PF requires to sample from the probabilistic state transition model:
\begin{equation}
x \sim p(x_t ~ | ~ x_{t-1}, u_t).
\end{equation}
Then, how do I assign a weight to each particle? That is, how do I evaluate this step of the algorithm
\begin{equation}
w = p(z_t ~ | ~ x_{t}),
\end{equation}
which requires evaluating the posterior for each particle $x_t$. I want to implement the most basic PF algorithm shown below, which I was able to do with Kalman Filters.

答:
The simplest way to construct $$x \sim p(x_t | x_{t-1}, u)$$ if you have $$x_t = Ax_{t-1}$$ is to just use $$x_t = Ax_{t-1} + \mathcal{N}(0,R)$$ where R is some process noise. You may way to sample from a more complex distribution.

Similarly, to construct $$p(z_t | x_t)$$ you can just use $$\mathcal{N}(Cx_t,Q)$$ (again you may want to use some more complex distribution)

$R$ is your process noise, $Q$ is your sensor noise.

As an addendum, Probabilistic Robotics has a whole chapter on constructing these probabilities for particle filters.


答:
To complement what Octopuscabbage correctly reported, there exists a strong theoretical foundation for using normal probability distributions in many different contexts, which builds on the Central Limit Theorem (CLT) that explains how the "exponential" distribution can work well with problems involving other types of distributions.

As a result, when you don't know the probability distribution of a random variable, picking up the normal distribution turns out to be the most preferable choice.

Title: Why Does an Exponential Make ANYTHING a Probability Distribution

Body:
I am posting this here because my background in estimation theory and optimization has been developed entirely through my experience in robotics.

TLDR: What makes it so that any time you put something in an exponential you can suddenly call it a probability distribution?

The Deets: I thought of this question while reading A General and Adaptive Robust Loss Function by Jonathan Barron. The paper describes a single parameter loss function that can subsume the most commonly used individual loss functions:

$$f(x, \alpha, c)$$

Where $x$ is parameter, $\alpha$ is the "single parameter", and $c$ is a distance scale that describes the "quadratic bowl" around the origin which all the loss functions share in common. When $\alpha = 2$ it is squared error loss, $\alpha =1$ it is essentially absolute error loss, and when $\alpha = 0$ it is cauchy loss.

My confusion starts when the paper goes on to say "With our loss function we construct a general probability distribution".

$$ p(x|\mu,\alpha, c) = \frac{1}{cZ(\alpha)}\exp(-f(x-\mu, \alpha, c)) $$

My question is, why does dividing by a normalization constant, and taking the exponential of the negative loss function suddenly make it a pdf? I understand the results, I have used these same ideas in my estimation theory courses, but I just don't get how we can make the jump from loss to pdf OR why it is so intuitive to do so.

Cheers, and happy holidays :)


问:
What is the difference between The state-space equation and The configuration-space equation
The craig(p.180) states that equations can be made to represent the dynamic equations. The first one being the state space equation.

$\tau=M(\Theta)\cdot\ddot{\Theta} + V(\Theta,\dot{\Theta}) + G(\Theta)$

The other one being the configuration state space equation.

$\tau=M(\Theta)\cdot\ddot{\Theta} + B(\Theta)[\dot{\Theta} \dot{\Theta}]+ C(\Theta)[\dot{\Theta}^2] + G(\Theta)$

Why would one need to split the velocity term into a Coriolis and centrifugal part? I need one of these versions to make a controller but can't see why one should be desirable than the other. Why is this important for computer control of a robot?

答:
They are exactly the same equation, being the former only a compact version of the latter.

From the Euler-Lagrange approach to the dynamics of a manipulator, you can build the $B$ and $C$ terms distinctively from the knowledge of your robot (e.g. distribution of masses of the links).

Thus, for constructing the dynamics that you aim to control, you have to deal with the second equation. This knowledge is way important (oftentimes a prerequisite) to design a good controller.

However, when it comes to designing the controller, you can conveniently group up terms in $\Theta$ and $\dot{\Theta}$ (first equation) as their influence can be aggregated.

Differently, if you don't have available the inertial parameters of your manipulator, you could think of estimating those dynamical terms, perhaps by means of adaptive control. In this specific case, you will most likely estimate $V$ rather than $B$ and $C$ separately, right because their effects accumulate and it is not straightforward to observe them in isolation.

Hope it helps out.


问:
APM Accelerometer Calibration
I am trying to manually calibrate the on-board accelerometer of an APM 2.6 controller.

I am using the following code (I found this somewhere, don't remember where) with Arduino 1.0.5 (in Windows environment) to fetch the accelerometer and gyro data:

```<code>  #include <SPI.h>
  #include <math.h>

  #define ToD(x) (x/131)
  #define ToG(x) (x*9.80665/16384)

  #define xAxis 0
  #define yAxis 1
  #define zAxis 2

  #define Aoffset 0.8

  int time=0;
  int time_old=0;

  const int ChipSelPin1 = 53;

  float angle=0;
  float angleX=0;
  float angleY=0;
  float angleZ=0;

  void setup() {
     Serial.begin(9600);

     pinMode(40, OUTPUT);
     digitalWrite(40, HIGH);

     SPI.begin();
     SPI.setClockDivider(SPI_CLOCK_DIV16);

     SPI.setBitOrder(MSBFIRST);
     SPI.setDataMode(SPI_MODE0);

      pinMode(ChipSelPin1, OUTPUT);

     ConfigureMPU6000();  // configure chip
    }

void loop()

{
 Serial.print("Acc X ");

  Serial.print(AcceX(ChipSelPin1));
  Serial.print("   ");
  Serial.print("Acc Y ");
  Serial.print(AcceY(ChipSelPin1));
  Serial.print("   ");
  Serial.print("Acc Z ");
  Serial.print(AcceZ(ChipSelPin1));
  Serial.print(" Gyro X ");
  Serial.print(GyroX(ChipSelPin1));
  Serial.print(" Gyro Y ");
  Serial.print(GyroY(ChipSelPin1));
  Serial.print(" Gyro Z ");
  Serial.print(GyroZ(ChipSelPin1));
  Serial.println();
}

void SPIwrite(byte reg, byte data, int ChipSelPin) {
  uint8_t dump;
  digitalWrite(ChipSelPin,LOW);
  dump=SPI.transfer(reg);
  dump=SPI.transfer(data);
  digitalWrite(ChipSelPin,HIGH);
}

uint8_t SPIread(byte reg,int ChipSelPin) {
  uint8_t dump;
  uint8_t return_value;
  uint8_t addr=reg|0x80;
  digitalWrite(ChipSelPin,LOW);
  dump=SPI.transfer(addr);
  return_value=SPI.transfer(0x00);
  digitalWrite(ChipSelPin,HIGH);
  return(return_value);
}

int AcceX(int ChipSelPin) {
  uint8_t AcceX_H=SPIread(0x3B,ChipSelPin);
  uint8_t AcceX_L=SPIread(0x3C,ChipSelPin);
  int16_t AcceX=AcceX_H<<8|AcceX_L;
  return(AcceX);
}

int AcceY(int ChipSelPin) {
  uint8_t AcceY_H=SPIread(0x3D,ChipSelPin);
  uint8_t AcceY_L=SPIread(0x3E,ChipSelPin);
  int16_t AcceY=AcceY_H<<8|AcceY_L;
  return(AcceY);
}

int AcceZ(int ChipSelPin) {
  uint8_t AcceZ_H=SPIread(0x3F,ChipSelPin);
  uint8_t AcceZ_L=SPIread(0x40,ChipSelPin);
  int16_t AcceZ=AcceZ_H<<8|AcceZ_L;
  return(AcceZ);
}

int GyroX(int ChipSelPin) {
  uint8_t GyroX_H=SPIread(0x43,ChipSelPin);
  uint8_t GyroX_L=SPIread(0x44,ChipSelPin);
  int16_t GyroX=GyroX_H<<8|GyroX_L;
  return(GyroX);
}

int GyroY(int ChipSelPin) {
  uint8_t GyroY_H=SPIread(0x45,ChipSelPin);
  uint8_t GyroY_L=SPIread(0x46,ChipSelPin);
  int16_t GyroY=GyroY_H<<8|GyroY_L;
  return(GyroY);
}

int GyroZ(int ChipSelPin) {
  uint8_t GyroZ_H=SPIread(0x47,ChipSelPin);
  uint8_t GyroZ_L=SPIread(0x48,ChipSelPin);
  int16_t GyroZ=GyroZ_H<<8|GyroZ_L;
  return(GyroZ);
}

//--- Function to obtain angles based on accelerometer readings ---//
float AcceDeg(int ChipSelPin,int AxisSelect) {
  float Ax=ToG(AcceX(ChipSelPin));
  float Ay=ToG(AcceY(ChipSelPin));
  float Az=ToG(AcceZ(ChipSelPin));
  float ADegX=((atan(Ax/(sqrt((Ay*Ay)+(Az*Az)))))/PI)*180;
  float ADegY=((atan(Ay/(sqrt((Ax*Ax)+(Az*Az)))))/PI)*180;
  float ADegZ=((atan((sqrt((Ax*Ax)+(Ay*Ay)))/Az))/PI)*180;
  switch (AxisSelect)
  {
    case 0:
    return ADegX;
    break;
    case 1:
    return ADegY;
    break;
    case 2:
    return ADegZ;
    break;
  }
}

//--- Function to obtain angles based on gyroscope readings ---//
float GyroDeg(int ChipSelPin, int AxisSelect) {
  time_old=time;
  time=millis();
  float dt=time-time_old;
  if (dt>=1000)
  {
    dt=0;
  }
  float Gx=ToD(GyroX(ChipSelPin));
  if (Gx>0 && Gx<1.4)
  {
    Gx=0;
  }
  float Gy=ToD(GyroY(ChipSelPin));
  float Gz=ToD(GyroZ(ChipSelPin));
  angleX+=Gx*(dt/1000);
  angleY+=Gy*(dt/1000);
  angleZ+=Gz*(dt/1000);
  switch (AxisSelect)
  {
    case 0:
    return angleX;
    break;
    case 1:
    return angleY;
    break;
    case 2:
    return angleZ;
    break;
  }
}

void ConfigureMPU6000()
{
  // DEVICE_RESET @ PWR_MGMT_1, reset device
  SPIwrite(0x6B,0x80,ChipSelPin1);
  delay(150);

  // TEMP_DIS @ PWR_MGMT_1, wake device and select GyroZ clock
  SPIwrite(0x6B,0x03,ChipSelPin1);
  delay(150);

  // I2C_IF_DIS @ USER_CTRL, disable I2C interface
  SPIwrite(0x6A,0x10,ChipSelPin1);
  delay(150);

  // SMPRT_DIV @ SMPRT_DIV, sample rate at 1000Hz
  SPIwrite(0x19,0x00,ChipSelPin1);
  delay(150);

  // DLPF_CFG @ CONFIG, digital low pass filter at 42Hz
  SPIwrite(0x1A,0x03,ChipSelPin1);
  delay(150);

  // FS_SEL @ GYRO_CONFIG, gyro scale at 250dps
  SPIwrite(0x1B,0x00,ChipSelPin1);
  delay(150);

  // AFS_SEL @ ACCEL_CONFIG, accel scale at 2g (1g=8192)
  SPIwrite(0x1C,0x00,ChipSelPin1);
  delay(150);
}
</code>```

My objective use to calibrate the accelerometers (and gyro), so that I can use them without having to depend on Mission Planner.

I'm reading values like:

```<code>Acc X 288   Acc Y -640   Acc Z 16884 Gyro X -322 Gyro Y 26 Gyro Z 74
Acc X 292   Acc Y -622   Acc Z 16854 Gyro X -320 Gyro Y 24 Gyro Z 79
Acc X 280   Acc Y -626   Acc Z 16830 Gyro X -328 Gyro Y 23 Gyro Z 71
Acc X 258   Acc Y -652   Acc Z 16882 Gyro X -314 Gyro Y 22 Gyro Z 78
Acc X 236   Acc Y -608   Acc Z 16866 Gyro X -321 Gyro Y 17 Gyro Z 77
Acc X 238   Acc Y -642   Acc Z 16900 Gyro X -312 Gyro Y 26 Gyro Z 74
Acc X 226   Acc Y -608   Acc Z 16850 Gyro X -321 Gyro Y 26 Gyro Z 68
Acc X 242   Acc Y -608   Acc Z 16874 Gyro X -325 Gyro Y 27 Gyro Z 69
Acc X 236   Acc Y -576   Acc Z 16836 Gyro X -319 Gyro Y 19 Gyro Z 78
Acc X 232   Acc Y -546   Acc Z 16856 Gyro X -321 Gyro Y 24 Gyro Z 68
Acc X 220   Acc Y -624   Acc Z 16840 Gyro X -316 Gyro Y 30 Gyro Z 77
Acc X 252   Acc Y -594   Acc Z 16874 Gyro X -320 Gyro Y 19 Gyro Z 59
Acc X 276   Acc Y -622   Acc Z 16934 Gyro X -317 Gyro Y 34 Gyro Z 69
Acc X 180   Acc Y -564   Acc Z 16836 Gyro X -320 Gyro Y 28 Gyro Z 68
Acc X 250   Acc Y -596   Acc Z 16854 Gyro X -329 Gyro Y 33 Gyro Z 70
Acc X 220   Acc Y -666   Acc Z 16888 Gyro X -316 Gyro Y 19 Gyro Z 71
Acc X 278   Acc Y -596   Acc Z 16872 Gyro X -307 Gyro Y 26 Gyro Z 78
Acc X 270   Acc Y -642   Acc Z 16898 Gyro X -327 Gyro Y 28 Gyro Z 72
Acc X 260   Acc Y -606   Acc Z 16804 Gyro X -308 Gyro Y 31 Gyro Z 64
Acc X 242   Acc Y -650   Acc Z 16906 Gyro X -313 Gyro Y 31 Gyro Z 78
Acc X 278   Acc Y -628   Acc Z 16898 Gyro X -309 Gyro Y 22 Gyro Z 67
Acc X 250   Acc Y -608   Acc Z 16854 Gyro X -310 Gyro Y 23 Gyro Z 75
Acc X 216   Acc Y -634   Acc Z 16814 Gyro X -307 Gyro Y 27 Gyro Z 83
Acc X 228   Acc Y -604   Acc Z 16904 Gyro X -326 Gyro Y 17 Gyro Z 75
Acc X 270   Acc Y -634   Acc Z 16898 Gyro X -320 Gyro Y 31 Gyro Z 77
</code>```

From what I understand, SPIread(...,...) returns an analog voltage value from the data pins of the sensor, which happens to be proportional to the acceleration values. Right?

My question is: How do I go about calibrating the accelerometer?

What I've tried till date: I've tried the "place horizontal... place nose down... left side, right side" technique used by mission planner.

Basically, when placed on horizontal position, the sensor is experiencing +1g on it's Z axis and 0g in X and Y axis. Left/right side provides ±1g on Y axis and nose down/up provides ±1g on X axis.

Now for every orientation, I've passed the raw sensor data through a LPF and then computed the mean, median and SD of this sensor data over 100 iterations. I store this mean, median and SD value in the EEPROM for each axis (one for +1g and one for 0g).

Now, when I use the sensor, I load the stats from the EEPROM, match the mean/median and standard deviation with the current reading of 4/5 iterations.

Here I'm working under the assumption that the values between 0g and +1g (and anything above 1g) can be interpolated/extrapolated from the data using a linear plot.

Is this the correct approach for calibration?

Can you suggest a better way?

I noticed that the maxima/minima for each axis is different. Is this
the expected outcome or is there something wrong in the code?

What do I do with the gyro? How to calibrate for angular
acceleration?

答:
You can use least square approach and solve this problem using Gauss-Newton method. Following blog post sums it up very well.

http://chionophilous.wordpress.com/2011/08/26/accelerometer-calibration-iii-improving-accuracy-with-least-squares-and-the-gauss-newton-method/

Title: APM Accelerometer Calibration


问:
PID on DC motor turning on both directions
I have a skid steer robot where I have four individual DC motors with an H bridge so I can control speed and direction. There are also four quadrature encoders with 6000 ticks per revolution.
The motors are 12v and I am controlling them with a 10bit PWM (0 to 1023). First test I did was to raise all wheels and test the minimum value where the wheels start moving, which is around 500.

I am able to measure the current speed of the wheels correctly by using a counter where the quadrature encoder set it to count up or down. Therefore, by reading the current count every 50ms (20Hz), I can measure the current speed:

```<code>       encoders_curr.FR = COUNTER_mReadReg(ENC0_BASEADDR, COUNTER_S00_AXI_SLV_REG1_OFFSET);
       encoders_curr.FL = COUNTER_mReadReg(ENC1_BASEADDR, COUNTER_S00_AXI_SLV_REG1_OFFSET);
       encoders_curr.RR = COUNTER_mReadReg(ENC2_BASEADDR, COUNTER_S00_AXI_SLV_REG1_OFFSET);
       encoders_curr.RL = COUNTER_mReadReg(ENC3_BASEADDR, COUNTER_S00_AXI_SLV_REG1_OFFSET);

       encoders_diff.FR = encoders_curr.FR - encoders_prev.FR;
       encoders_diff.FL = encoders_curr.FL - encoders_prev.FL;
       encoders_diff.RL = encoders_curr.RL - encoders_prev.RL;
       encoders_diff.RR = encoders_curr.RR - encoders_prev.RR;

       encoders_prev = encoders_curr;

       speed_measured.FR = (2*M_PI*WHEEL_RADIUS*encoders_diff.FR) / (0.05*TICKS);
       speed_measured.FL = (2*M_PI*WHEEL_RADIUS*encoders_diff.FL) / (0.05*TICKS);
       speed_measured.RR = (2*M_PI*WHEEL_RADIUS*encoders_diff.RR) / (0.05*TICKS);
       speed_measured.RL = (2*M_PI*WHEEL_RADIUS*encoders_diff.RL) / (0.05*TICKS);
</code>```

Which I convert to X, Y and Z with:

```<code>       // Inverse Kinematics: http://robotsforroboticists.com/drive-kinematics/
       kinematics_measured.linear.x = (speed_measured.FR+speed_measured.FL+speed_measured.RR+speed_measured.RL)*(WHEEL_RADIUS/4);
       kinematics_measured.linear.y = (-speed_measured.FR+speed_measured.FL+speed_measured.RR-speed_measured.RL)*(WHEEL_RADIUS/4);
       kinematics_measured.angular.z = (-speed_measured.FR+speed_measured.FL-speed_measured.RR+speed_measured.RL)*(WHEEL_RADIUS/(4*(WHEEL_SEPARATION_WIDTH+WHEEL_SEPARATION_LENGTH)));
</code>```

Now for the control part, to set the individual velocities of each of the four wheels, I am using these equations, as my input is linear x, linear Y and angular Z speeds (rad/seg):

```<code>       // Forward kinematics: http://robotsforroboticists.com/drive-kinematics/
       speed_desired.FL = (1/WHEEL_RADIUS)*(kinematics_desired.linear.x-kinematics_desired.linear.y-kinematics_desired.angular.z*(WHEEL_SEPARATION_LENGTH+WHEEL_SEPARATION_WIDTH));
       speed_desired.FR = (1/WHEEL_RADIUS)*(kinematics_desired.linear.x+kinematics_desired.linear.y+kinematics_desired.angular.z*(WHEEL_SEPARATION_LENGTH+WHEEL_SEPARATION_WIDTH));
       speed_desired.RL = (1/WHEEL_RADIUS)*(kinematics_desired.linear.x+kinematics_desired.linear.y-kinematics_desired.angular.z*(WHEEL_SEPARATION_LENGTH+WHEEL_SEPARATION_WIDTH));
       speed_desired.RR = (1/WHEEL_RADIUS)*(kinematics_desired.linear.x-kinematics_desired.linear.y+kinematics_desired.angular.z*(WHEEL_SEPARATION_LENGTH+WHEEL_SEPARATION_WIDTH));
</code>```

The PID function that I have implemented is the following:

```<code>float integral=0.0, prev_error=0.0;
#define dt  0.05    // in seconds
#define Kp  1
#define Ki  0
#define Kd  0
float pid(float desired, float measured)
{
    if(measured<0)
        measured = (-1)*measured;

    // Calculate error
    float error = desired-measured;

    // Proportional term
    float Pout = Kp*error;

    // Integral term
    integral += error*dt;
    float Iout = Ki*integral;

    // Derivate term
    float derivate = (error-prev_error)/dt;
    float Dout = Kd*derivate;

    return (Pout+Iout+Dout);
}
</code>```

So, after measuring the current speed, I can call it to obtain the speed I need to set the duty of each motor.

My questions:

The sign of the velocity indicates the direction of rotation (clockwise=forward, counterclockwise=backwards). Do I need to pass to the pid function the desired velocity as a signed or unsigned value?

What should I do with velocity values that would mean a lower pwm so the motor will not spin?

To convert from linear velocity that the pid will dictate to duty cycle, I am calculating the equation that passes over two points. For this, I measured the speeds at the minimum and maximum duties where the wheels rotation (500 and 1023) giving (0.81 and 1.03). I use these numbers to get the equation: p = 511 + (uint16_t)(((1023-511)/(1.02-0.8))*(v-0.8));. Should I consider that the speed can be negative to compute a "negative" duty which will later represent only the direction to set for the motors? If so, it means that I have to consider that the two points for the equation are (-1023, -1.03) and (1023, 1.03)?

As I have four motors, they all show different velocities. So, to simplify the process, even thought there will be 4 PIDs, I got for the lowest duty cycle the maximum velocity value of all motors and for the highest duty cycle the lowest velocity of all motors. Would that be correct?

Do I need to include the speed limits for the PID?

Considering the signs, won't be a problem if the measured speed is higher than the desired, the pid would compute a negative value and the fact that the rotation switches directions might cause an issue, or not?

Thank you for the help.

答:
Q1

Do I need to pass to the pid function the desired velocity as a signed or unsigned value?

Definitely, you shall deal with signed velocities. The PID will take care of all the signs down the operations' chain, providing positive and or negative rotational velocities.

Q2

What should I do with velocity values that would mean a lower pwm so the motor will not spin?

The integral part of the PID will be responsible for that. Anyway, see the note on the anti-windup.

Q3

Should I consider that the speed can be negative to compute a "negative" duty ...?

Sure! As per Q1.
Anyway, it's not clear to me why you're doing the conversion from velocity to duty. The PID will find its way to it. Let the PID do its job.

Q4

Would that be correct?

I don't think so. You're adding up knowledge on top of the task that is not required, making things overcomplicated. Just let the PID do its job based on the measurements and it will work. If the measurements are reliable, the 4 PID will make the 4 wheels spin at the same speed (after transients), regardless of their constructive differences.

The task of attaining the same speed on the 4 different wheels is thus doable at the steady-state. However, that doesn't mean that the robot will be reaching your target location or tightly following your desired path as controlling velocity does not guarantee that you can compensate for different position errors that will accumulate during transitions. To get around this, you may consider applying a hierarchical approach with an outer controller that will close the loop on the position (e.g. odometry, SLAM).

Q5

Do I need to include the speed limits for the PID?

Although it's not strictly required, it may be beneficial. Be careful as the speed limits (a.k.a. actuation bounds) need to be handled properly by means of the anti-windup.

Q6

... won't be a problem if the measured speed is higher than the desired ...?

Nope, the PID is apt to the task of controlling around 0 error, thus the signs are correctly handled.

Integral anti-windup

The PID needs to be aware that the process under control does show actuation limits. To this end, one can apply the standard mechanism of anti-windup to prevent the integral term from overcharging when the system working point is beyond those bounds. The net result is that the anti-windup purposely avoids unwanted oscillations.

See this answer for more information on the anti-windup.


问:
PID controller that targets a set rotation and a set velocity at that rotation?
I have a 3d physics simulation where I use a PID controller to reach a set orientation (orientation error drives a torque output).  Which works well enough some cases, but in others it's not working as nicely as I would like - especially in the case of rapid rotation.  I am still tuning however.

I'm not proficient in control theory, but I was thinking maybe perhaps that the controller is only ensuring the desired orientation is reached as a constraint, but not what the angular velocity should be when it gets there - ideally zero in most situations.

So I was wondering if it made sense to somehow combine a second PID controller that uses angular velocity rather than orientation mismatch as the error.  I'm not sure if this makes sense, and if so if it should just output another torque that is summed with the torque of the other PID?  Or if it's more complicated than that?  Or if it would be duplicating the work of the first PID anyway?

I think I would want the angular velocity closer to zero, the more closer the orientation error is to zero - so I am not sure if any kind of scaling to do that makes sense?  I can imagine that would then become a balancing act between the two PID's coefficients to determine how fast it can correct vs stability when stopping (But then that kind of sounds like what the first PID would be doing anyway!?).

Also in general I am not sure of the correct way to combine multiple PID controllers that are trying to balance concerns like that.

答:
Find here a similar question.

A well-tuned position PID is most of the time capable of accomplishing the task of reaching a target with zero velocity. The central point here is that you shall provide the PID with a smooth reference trajectory (planning or input-shaping) connecting your starting point with the desired orientation.

Usually, this reference trajectory is generated in the form of a minimum-jerk path that reduces oscillations, especially at the onset and the endpoint. To code this trajectory you ought to resort to a 5th order polynomial:

$$
r(t) = r_i + (r_d-r_i) \cdot \left( 10\left(\frac{t}{t_f}\right)^3 -15\left(\frac{t}{t_f}\right)^4 +6\left(\frac{t}{t_f}\right)^5\right),
$$
where $t_f$ is the time to execute the whole trajectory, hence $t \in [0,t_f]$ and $r_i$ and $r_d$ are the starting and the ending points, respectively.

This smooth input will guarantee that the PID won't struggle to deliver its effort to reach the target at a very low speed without overshoot.

Of course, you are still required to properly tune the PID to let it behave successfully when fed with stepwise inputs.

That said, adding cascaded PID controllers is also quite common. The diagram foresees that controllers are nested from the inner to the outer following this order:

inner: torque (current) PID

intermediate: velocity PID

outer: position PID

Have a look at this quite enlightening resource to see how this is done.

You could also have implementations with only 2 cascaded PID out of the 3 above (e.g. velocity and position) but always in the prescribed order.

The key point of cascaded PID controllers is that this design can improve the overall performance and allow for tuning according to this procedure:

First tune the torque (current) inner PID considering the motor+load as the plant;

Then, tune the velocity intermediate PID considering the motor+load+current_pid as the plant;

Finally, tune the position outer PID considering the motor+load+current_pid+velocity_pid as the plant.

Thereby, at the end of the story, there will be only one objective that is the position setpoint. Still, a cascaded controller cannot impose velocity setpoints, for example; rather, it will serve "merely" to improve the closed-loop system performance.

Find below an illustrative example concerned with the altitude control of a drone where a 2-level cascaded PID controller is employed. The outer altitude (position) controller is combined along with the inner velocity controller.

This example is taken from "Understanding PID Control, Part 7: Important PID Concepts" by Brian Douglas.


问:
Can I measure torque from this motor by only measuring the current?
I am new to robotics, please forgive/correct any errors.

My Project Plan

I want to use a PID controlled motor in a human-interface device where I need to measure the user's exerted force on an end actuator.  The force measurement does not need to be exact (it will just be used for "feel"), but ideally it should be reasonably repeatable.  The actuator will be held at position using a PID controller (position will eventually be dynamic).

I will likely be using a geared-down DC motor.  With the motor's voltage controlled via PID, my understanding is that I should be able to (roughly) measure the torque via a current sensor in series.

Question

I found a DC motor that meets all of my needs*, the JGB37-12V-76RPM.  However, after some research, I have found that this motor is likely a worm-gear drive.

Can I use current sensing to measure the torque on a wormgear drive?  As I understand it, wormgear drives cnnIf it does not backdrive, I imagine that current will not need to increase as load torque increases (and position stays the same), and therefore will not be a reliable signal.

If I cannot measure the torque using current sensing, what would be the best path forward?  I could use a load-cell, but I imagine that will make my design bulky.  Is there a plain torque sensor I could use?

Edit: My Solution

It turns out my requirements were off by a factor of 10x: I need 28kg-cm of torque, not 2.8kg-cm.

In addition to my speed requirement, I decided to use a 25 kg-cm servo (the next stronger standard, 35 kg-cm, is a bit slow).  To get my end actuator force readings, I will be using 3x 5 kg half-bridge micro load cells (each is 2 strain gauges on a small square load cell), mounted orthogonality, that should determine my actuator's contact force far better than torque measurements, without any of the possible issues (e.g. detecting force when there is no torque, like if all arm links are in a line and the user pushes axially).

* Motor needs:

>75RPM (no load)

>28kg-cm {old: 2.8kg-cm} (~2 lbs force at actuator)

Bi-directional encoder with >1400 clicks per rotation after reduction (translates to ~1mm encoder accuracy at actuator)

Not stupidly expensive.

答:
Can I use current sensing to measure the torque on a wormgear drive?

Nope as the worm-gear is not back-driveable by construction.

what would be the best path forward?

Using an FT sensor mounted at the end-effector would be the best solution but they are expensive.

Replacing the worm-gear with a BLDC motor being careful about the gear ratio. In fact, a high gear ratio will prevent you from sensing the applied force by measuring the motor current.


问:
how to get an 8 hour autonomy for an underwater robot?
CONTEXT: I'm working on a project consisting of making an underwater robot "fish" for an aquarium. It is mandatory to get at least 8 hours of autonomy. I'm responsible of the electrical part of the project and during all of my bachelor degree I've never got any lecture on batteries and I have no idea on how to get what is required. But I managed to do some research and try to figure it out:

I took the power consumption of all the potential components necessary for the project (motors & pumps, Usound captors, ucontroller and a set of RF receiver and transmitters) from the specs and datasheets available.

I got a battery of a 1200 mAh capacity. The battery is originally used for quadrocopters, i figured it would be good for the project since it's relatively the same thing (a software and motors).

Thanks to some references (Battery university, Eletropaedia), I estimated the battery autonomy. I used an ideal estimation [=Battery Capacity/sum of power consumptions] and I supposed all the components worked 100% of the time.

The results I got is far bellow needed :(

Observations :

It seems that the pump is the only component to have an excessive consumption

Difficulties :

Since I have no experience in the field, I don't know if the battery I'm using is good.

The pump seems to be necessary and irreplaceable. I talked to the mechanical team and replacing the pump with something else will make the project unrealistic.

The 8 hours autonomy is a necessity to the project's success.

The underwater environment.

Dimensions (less than 8 inches long)

QUESTIONS : First, is the 8 hours autonomy doable? What should I do to get that? Change battery? Change pump? Use a totally different approach?

LINKS :
PUMP
BATTERY

Thank you for your time

答:
Lithium Polymer batteries are, in general, the best you're going to get as far as either energy per weight or energy per volume.  Going to a different brand may gain your 10% more capacity per volume, but not much more (if you get to that point, and you're in the US, check out ThunderPower batteries -- I fly model airplanes in competition, and the folks I fly against who fly electric either use ThunderPower, or they're not seriously expecting to win).

There's not really a viable alternative -- diesel fuel has about 20 times the energy capacity as LiPo batteries, but you need a diesel engine (which doesn't exist in the form factor you need) or a diesel fuel cell (which even more so doesn't exist in the form factor you need).  There's some experimental battery technologies that get a lot of breathless press from University public relations departments, but nothing has been commercialized.

Do all the bits of the fish robot have to work continuously?  Can it swim more slowly?  Does the pump have to run all the time?  If you can conserve power by having it run intermittently, that'll help a lot.

If you can't get a save by operating things intermittently, you need to go back to the mechanical engineers and show them what LiPo cells are capable of, and mention that conservation of energy is a thing.  You'll be doing your part by making sure that no power is unnecessarily wasted -- but they have to make sure the goal can be accomplished with the available power.


问:
Forward and Revers Kinematics For Modified Hanging Plotter
I'm building a modified version of the standard hanging plotter (v-plotter). The basic idea is that you have two cables hanging from stepper motors which form a triangle supporting the pen at the tip.

My design the strings anchored at points $C$ and $D$ which causes the behavior to be somewhat different that the normal plotter, especially when operating close to the motors.

I was able to work out the forward kinematics fairly easily, but the inverse kinematics are turning out to be a real headache. You can see my attempt at a standard geometry solution on math.stackexchange here.

Is there anything specific to calculating kinematics for robotics which could help me?

I'm not interested in modifying the hardware to make the math easier. I'm also not interested in discussing the center of gravity of the pen, cable weight...etc. I want to calculate the kinematics for this as an ideal system.

Any and all advice is appreciated.

Thank you.

答:
I decided to build a V-plotter.
I am using Arduino based software.
I'd like to adapt your V-plotter math solution.
Can you solve an example problem with your calculations?
Can you help me.
I will be waiting for your answer.
thank you


问:
Can badly tuned Feedforward still decrease steady-state error?
Let's say we're trying to control the speed of a motor to 20 revs/sec, and we know that our controller output for that speed is approximately 5. We currently have a Proportional controller to drive the motor to our 20 revs/sec setpoint, with some steady-state error.

If we included Feedforward but tuned it poorly, such that it outputs 2 instead of 5, will the following error still be less than it would have been with only the Proportional controller?

I would think that for Feedforward to reduce the steady-state error, the output would need to be able to drive the motor closer to the setpoint than the Proportional controller could do by itself (without unacceptable oscillations). Is this the case?

答:
Your intuition is right as a feedforward term always helps the PID do its job, which in this case means that the steady-state error is lower.

We could have practical proof of this.

Imagine that in your example the effort value to get $20$ rev/s amounts to $6$, for which the steady-state error $e$ is thus $0$ rev/s.

Therefore the static gain of the process under control is $G_0=20/6$.

In the condition where we have a pure proportional controller with gain $K$, we can derive the following:
$$
K \cdot \left( 20 - 5 \cdot G_0\right) = 5.
$$
It comes out that the gain is $K = 3/2$, which is responsible for a steady-state error $e=3.33$ rev/s.

Now, let's plug the feedforward term delivering an effort $u_{ff}=2$:
$$
K \cdot \left(20 - (u_{ff} + u_{fb}) \cdot G_0) \right) = u_{fb}.
$$

It turns out that $u_{fb}=10/3$ and the error is $e=2.22$ rev/s, which is lower.


问:
Finding the right PD values for a quadcopter simulator
I am working on this short assignment where I have to find the right PD values for a simulated quadcopter.  I have to reach 0.9 meters under 1 second with an overshoot of less than 5%.  This is my code:

```<code>function [ u ] = pd_controller(~, s, s_des, params)
%PD_CONTROLLER  PD controller for the height
%
%   s: 2x1 vector containing the current state [z; v_z]
%   s_des: 2x1 vector containing desired state [z; v_z]
%   params: robot parameters

% u = 0;

error = s_des - s;
z_des = 0.9;

Kp = 0.5;
Kv = 1.0;

u = params.mass*(z_des + Kp*error(1) + Kv*error(2) + params.gravity);

end
</code>```

So far this is my output:

I've tried many different value combinations and so far this is the closest that I could get.

答:
This problem can be conveniently solved by resorting to a state-feedback controller as the one depicted below.

In this case, we have that $P=\frac{1}{m \cdot s^2}$, $x=\left[ z, \dot{z} \right]^T$, $K=\left[k_p, k_d\right]$, $k_f=\frac{k_p}{m}$. Then, we need to apply the feed-forward term $v=mg$ to counteract the gravity.

We end up with the transfer function of the closed-loop system:

$$
\frac{y(s)}{r(s)}=\frac{\frac{k_p}{m}}{s^2+\frac{k_d}{m}s+\frac{k_p}{m}}.
$$

This is a well-known and well-studied second-order system given in the form

$$
\frac{\omega_n^2}{s^2+2\xi\omega_ns+\omega_n^2},
$$

where the parameters $\omega_n$ and $\xi$ are expressed as functions of the gains $k_p$, $k_d$ and the mass $m$ as $\omega_n=\sqrt\frac{k_p}{m}$ and $\xi=\frac{k_d}{2\sqrt{m \cdot k_p}}$.

To meet the given requirements, have a look at the information you can peruse for example at https://www.javatpoint.com/control-system-time-response-of-second-order-system, where the overshoot and the settling-time are provided by the following expressions:
$$
\begin{cases}
e^\frac{\pi\xi}{\sqrt{1-\xi^2}}=0.05 \\
\frac{4}{\xi\omega_n}=1
\end{cases}.
$$

The gains $k_p$ and $k_d$ can be thus readily determined from the formulas above through substitution.

For a unitary mass $m=1$ kg, it comes out that $k_d=8$ Ns/m and $k_p=k_f=33.596$ N/m.

Here's the step response:

The step response is generally given with respect to a unitary input step; however, the system is linear and thus this behavior is preserved also for an input step of amplitude $0.9$ m.

To conclude, your controller's output shall be:

u = params.mass*params.gravity - kp*z - kd*v_z + kf*z_des;
```


问:
Tuning a cascaded attitude and attitude rate PID controller
Assume that I have an object in 3D, and I want to have a controller that stabilizes its orientation (also referred to as "attitude").

To be sure we are clear:

attitude is measured in "degrees" or "radians"

attitude rate is measured in "degrees/s" or "radians/s"

The controller is made of two cascaded control loops:

attitude controller: the error controlled by this controller is the difference between the reference attitude and the real attitude

attitude rate controller: the error controlled by this controller is the difference between the  reference attitude rate and the real attitude rate

The attitude controller is made only of a P term, and therefore it is not hard to tune. However, the attitude rate controller has all three terms P, I, and D.

I am using the well-known Ziegler-Nichols method to tune the attitude rate controller. However, I am not sure that theoretically, what I am doing is correct.

My question is: is it possible to tune the P, I, and D terms of the attitude rate controller while observing the step response by giving a command in attitude? To apply this procedure, should I provide an attitude rate command instead? (The main problem is I cannot give an attitude rate command because my object would start to rotate and this is not practical). FYI and for completeness: my object is a drone, but I don't think it matters too much regarding my question.

答:
You can operate the quadcopter with a poorly tuned attitude rate PID, log relevant quantities, build a model from the data, and finally use the model to tune the PID iteratively.

Suppose that such an inner PID delivers voltage commands to the propellers. You could instruct the quadcopter to safely perform a bunch of maneuvers and at the same time log/acquire the data you will then use to build the model. Candidate data will consist of pairs of voltage and measured speed samples.

The key point here is to control the quad such that the acquired dataset will be informative enough to support the subsequent identification stage. To this end, alternate low and quick maneuvers with different amplitudes. Anyway, since the inner and outer loops will be operational, the quad will fly safely.

Using those data you can perform closed-loop identification via the Predicted Error Method (PEM). Typical PEM based approaches resort to the well-known ARX, ARMAX, BJ, OE structures.

Once done with the model, you could tune your PID based on such knowledge by employing standard methods like pole placement, loop shaping and the like. If you use MATLAB, there exist automatic techniques for that.

Iterate through this process multiple times until you get acceptable performance.

Better off giving up with Ziegler-Nichols methods to adhere to model-based design instead.

You may find this webinar very much interesting.


答:
In underwater autonomous vehicles there are basically two solutions to increasing long term autonomy.

The first, better power generation, (which has already been mentioned) won't really work in your case. See here for a discussion page on larger fuel cells

The second, which is more common, is reducing the amount of actuation. One of the most popular underwater vehicle designs is the glider design. The slocum glider which only pumps water in and out and moves a tailfin (IT also has a very low power computer running)

My suggestion: the project in its current state is unfeasible. You need to redesign many components. Just running the motor for 8 hours is infeasible, not to mention the other components you want to run (computers and ultrasounds).

Title: how to get an 8 hour autonomy for an underwater robot?

Body:
CONTEXT: I'm working on a project consisting of making an underwater robot "fish" for an aquarium. It is mandatory to get at least 8 hours of autonomy. I'm responsible of the electrical part of the project and during all of my bachelor degree I've never got any lecture on batteries and I have no idea on how to get what is required. But I managed to do some research and try to figure it out:

I took the power consumption of all the potential components necessary for the project (motors & pumps, Usound captors, ucontroller and a set of RF receiver and transmitters) from the specs and datasheets available.

I got a battery of a 1200 mAh capacity. The battery is originally used for quadrocopters, i figured it would be good for the project since it's relatively the same thing (a software and motors).

Thanks to some references (Battery university, Eletropaedia), I estimated the battery autonomy. I used an ideal estimation [=Battery Capacity/sum of power consumptions] and I supposed all the components worked 100% of the time.

The results I got is far bellow needed :(

Observations :

It seems that the pump is the only component to have an excessive consumption

Difficulties :

Since I have no experience in the field, I don't know if the battery I'm using is good.

The pump seems to be necessary and irreplaceable. I talked to the mechanical team and replacing the pump with something else will make the project unrealistic.

The 8 hours autonomy is a necessity to the project's success.

The underwater environment.

Dimensions (less than 8 inches long)

QUESTIONS : First, is the 8 hours autonomy doable? What should I do to get that? Change battery? Change pump? Use a totally different approach?

LINKS :
PUMP
BATTERY

Thank you for your time


问:
Underwater navigation without the use of acoustic sensors
I have been reading about different types of underwater navigation.
According to this Youtube video, these are the following navigation systems:

Inertial navigation systems (use of Inertial Measurement Unit)

Acoustic (e.g., long-baseline)

Geophysical (use of external environment information)

This article states sonar affects whales and the use of the acoustic navigational systems should be minimized. It seems like the combination of inertial and geophysical underwater navigation is not enough. So what kind of underwater navigation system one should use in order to be environmentally friendly but also accurate?

答:
Usually, most underwater robots are running on just inertial navigation and surfacing occasionally. This tends to work better than one would expect and is what most large scale deployments end up using. Additionally, most of these use a Doppler Velocity Log, Pressure Sensor, and Compass in addition to their IMU.

Geophysical localization is fairly precise, SLAM solutions have been developed for mobile and underwater robotics for a long time. I believe underwater SLAM solutions are in a fairly mature place and if your robot is operating in a feature-rich environment and has the ability to use LIDAR or cameras you could get a fairly good localization solution.

As an aside, usually, the sonar on robots is much less powerful and less overused than shipboard sonar. It's probably not a huge issue unless you plan to explicitly work near whales (but I am not a marine biologist).


问:
Problem with calculating relative orientation
I am using an IMU which provides absolute orientation of the sensors frame $S$ relative to an earth-fixed frame $N$ in quaternion form, $^S_Nq$. In my experiments, I first obtain an initial orientation (when the sensor is relatively static) $^I_Nq$, and I would like to obtain the relative orientation of the sensor in the form of ZYX Euler angles (more precisely $ZY^{'}X^{''}$).

Here is the procedure that I have tried:

First, I invert the initial orientation,

$^N_Iq$ = $^I_Nq^{-1}$

then, I use the result the obtain the relative quaternion as follows,

$^S_Iq$ = $^S_Nq$ $\otimes$ $^N_Iq$

Finally, to visualize the results, I convert the relative orientation to Euler angles. I also have reference trajectories calculated in a motion capture software which uses the same data. However, my result looks completely different (and wrong) as seen below,

Calculated vs. reference  relative orientation

Curiously, if I manually set the $Z$ and $Y$ rotations of $^I_Nq$ to zero (and then convert the result back to quaternion form), the angle trajectories match exactly (except for the offset of $Z$ and $Y$).

Result with setting the first two rotations of initial orientation set to zero

What am I doing wrong?

By the way, this is the MATLAB code that I'm using. Note that initQ is $^I_Nq$ and relQ is $^S_Iq$.

```<code>% Average quaternion using meanrot to obtain initial orientation.
[q(1), q(2), q(3), q(4)] = parts(meanrot(quaternion(initData));
initQ = q;

% The second method, if I manually set the first two rotations to zero.
% initEul = quat2eul(q,'ZYX');
% initEul(2) = 0;
% initEul(1) = 0;
% initQ = eul2quat(initEul,'ZYX');

relQ = quatmultiply(mocapData,quatinv(initQ));
eulerAngles = quat2eul(quaternion(relQ),'ZYX')*180/pi;
</code>```

答:
Referencing book "Robotics Modelling, Planning and control" by Bruno Siciliano from Springer publication. Chapter 3.7 Orientation error i.e relative orientation.

Relative Eulers angle can only use if you have a spherical joint i.e 3-revolute joints each abut different perpendicular axis. Check your configuration before using Eulers angle. For more refer referenced book to understand more.

Check out the similar solved question- Inverse kinematic orientation problem


问:
Denavit-hartenberg parameters sign 6-DOF
Trying to understand how does the Denavit-Hartenberg work.

While I understood how is the table constructed, I however don't understand how are the signs for the orientation parameters (theta, joint angle and alpha, twist link) determined.

Is there any rule that I'm missing, that determines whether the value is going to be negative or positive?

Why is the joint angle of the last link 6 has the value of 180 even though there's no transformation between the end effector and the rotational joint?

答:
First thing first- you can consider any direction as positive or negative, but need to keep it constant while solving the problem and even while programming robot.

In almost all cases of 6-DOF manipulator, the co-ordinate system of joint 1 is aligned with the universal coordinate system.

Theta- its joint parameter, it signifies angle between 2 x-axis about the z-axis.

Alpha- its link parameter, it signifies twist of 2 z-axis about x-axis. Consider one link and try to visualize the coordinate system at each end of the link.

The answer to question-2 is- you can either write 0 or 180 degree, but remember it while programming for setting home position of the robot and while calibration of joints.

Hope this helps.


问:
Imaginary part in result of inverse laplace transform
I am working with a system(closed-loop feedback control) that has a transfer function like this

F = 20*(s+5)/(s^4+13.5s^3+41s^2+80*s+100)

this one is in the s-domain but for the response analysis, I needed it to be on the time-domain.

I used python sympy to get the inverse Laplace transform of this

```<code>import sympy as sp
s, t = sp.symbols('s, t', real=True)
F = 20*(s+5)/(s**4+13.5*s**3+41*s**2+80*s+100)
ilt = sp.inverse_laplace_transform(F, s, t)
</code>```

the thing that surprised me was that answer contained some imaginary part as well:-

so my quest is about the existence, correctness, and if it is possible to have an imaginary part then what could be the significance of such a system.

答:
We can solve this problem by using MATLAB and specifically by applying the partial fraction expansion by means of the function residue.

The MATLAB code below

[r, p] = residue([20 100], [1 13.5 41 80 100]);

syms s;
F1 = r(1) / (s - p(1));
F2 = r(2) / (s - p(2));
F3 = r(3) / (s - p(3));
F4 = r(4) / (s - p(4));

F34 = (2*real(r(3))*s - 2*real(r(3))*real(p(3)) - 2*imag(r(3))*imag(p(3))) / ...
      (s^2 - 2*real(p(3))*s + abs(p(3)));

F = F1 + F2 + F34;
f = ilaplace(F);

disp('F(s) = ');
pretty(vpa(F1 + F2 + F3 + F4, 5));
disp(' = ');
pretty(vpa(F, 5));

disp('f(t) = ');
pretty(vpa(f, 5));
```
<p>outputs the following result:</p>
```F(s) =
  1.0121       0.13579     - 0.57396 - 0.74149i    - 0.57396 + 0.74149i
---------- + ---------- + --------------------- + ---------------------
s + 2.2394   s + 10.138   s + 0.56117 - 2.0223i   s + 0.56117 + 2.0223i

 =
  1.0121     (1.1479 s - 2.3549) 1.0     0.13579
---------- - ----------------------- + ----------
s + 2.2394     2                       s + 10.138
              s  + 1.1223 s + 2.0987

f(t) =
exp(-2.2394 t) 1.0121 + exp(-10.138 t) 0.13579 - exp(-0.56117 t)

   (cos(1.3356 t) - sin(1.3356 t) 1.9561) 1.1479
```

The key is that the fractions F3 and F4 containing imaginary numbers can be conveniently grouped up into F34, which is instead a fraction with a second-order denominator containing only real numbers that in turn yields the well-known oscillatory behavior with an exponential profile.

In general, polynomials with real coefficients may exhibit only up to complex and conjugate roots, which can be always handled following the procedure above that basically lets imaginary numbers fade away into fractions with second-order denominators.

Body:
We can solve this problem by using MATLAB and specifically by applying the partial fraction expansion by means of the function residue.

The MATLAB code below

[r, p] = residue([20 100], [1 13.5 41 80 100]);

syms s;
F1 = r(1) / (s - p(1));
F2 = r(2) / (s - p(2));
F3 = r(3) / (s - p(3));
F4 = r(4) / (s - p(4));

F34 = (2*real(r(3))*s - 2*real(r(3))*real(p(3)) - 2*imag(r(3))*imag(p(3))) / ...
      (s^2 - 2*real(p(3))*s + abs(p(3)));

F = F1 + F2 + F34;
f = ilaplace(F);

disp('F(s) = ');
pretty(vpa(F1 + F2 + F3 + F4, 5));
disp(' = ');
pretty(vpa(F, 5));

disp('f(t) = ');
pretty(vpa(f, 5));

outputs the following result:

F(s) =
  1.0121       0.13579     - 0.57396 - 0.74149i    - 0.57396 + 0.74149i
---------- + ---------- + --------------------- + ---------------------
s + 2.2394   s + 10.138   s + 0.56117 - 2.0223i   s + 0.56117 + 2.0223i

 =
  1.0121     (1.1479 s - 2.3549) 1.0     0.13579
---------- - ----------------------- + ----------
s + 2.2394     2                       s + 10.138
              s  + 1.1223 s + 2.0987

f(t) =
exp(-2.2394 t) 1.0121 + exp(-10.138 t) 0.13579 - exp(-0.56117 t)

   (cos(1.3356 t) - sin(1.3356 t) 1.9561) 1.1479

The key is that the fractions F3 and F4 containing imaginary numbers can be conveniently grouped up into F34, which is instead a fraction with a second-order denominator containing only real numbers that in turn yields the well-known oscillatory behavior with an exponential profile.

In general, polynomials with real coefficients may exhibit only up to complex and conjugate roots, which can be always handled following the procedure above that basically lets imaginary numbers fade away into fractions with second-order denominators.


问:
Motor acceleration and position with PID control
I'm working on a robot with differential drive (2 drive wheels equipped with rotary encoders and 4 free spinning omni wheels for stability) and each wheel is controlled via a separate arduino nano.

I have implemented two PID loops, one for position control and one for speed control (first one gets a position in number of pulses and second gets an RPM as their goals).

The position PID works fine by produces jerky motion and acts more like a "bang bang" control where it starts at full speed. I'm not sure how I can combine these to plan a move that reaches a certain number of pulses with a given maximum acceleration and / or velocity.

I know 3D printers for example, have settings for jerk,acceleration and velocity so I tried to go through marlin to see how that's implemented but I had no luck! Can anyone please provide examples or links to tutorials / papers regarding this?

I think an easy solution would be pre calculating the motion profile and then tracking it with the position PID, but I'm not sure if there are better solutions for this.

答:
I believe the resource https://robotics.stackexchange.com/a/21571/6941 already pretty much answers your question.

In essence, both the following approaches are valuable and can be combined together:

A pre-calculated motion profile helps the system reduce the jerk.

Cascaded PID controllers allow for improving the system response.


答: Thank you for your answers and comments. We finally decided to change the pump with a less energy consuming system. We're testing the prototype in the next to weeks we hope we get the estimated autonpmy</p>


问:
Programming Robots with JavaScript
As somebody who is spending the majority of his time programming in JavaScript, what's the best route to get into small-robotics without needing to deviate too much from my current language focus?

Are there any project kits or tools that make use of the JavaScript language that might make the field more approachable for developers like myself? I would even be interested in virtual environments where all code is executed in a simulation.

答: The approach I took to this in my project, https://github.com/Ezward/Esp32CameraRover2, was to develop a small core for the robot in C++; enough to do closed loop speed control on motors, provide a command interpreter so the client can control the robot and to serve an html/css/js client application over http.  The client is served by the robot itself and connects to the robot over websockets.  Partitioning in this way allows very time critical tasks, like speed control, to run on the robot, and higher level tasks (like go-to-goal, path following or object recognition) to run in JavaScript.  This also allows the robot to be built using a very inexpensive microcontroller; this project uses an Esp32Cam.  It can stream real-time video from the robot to the client which opens up a lot of options for processing images in JavaScript.  The project is build on a very inexpensive platform (about $40 total for parts) and requires little-to-no soldering.  I'm using the ESP32 Arduino library in the PlatformIO IDE, which is a plugin for Visual Studio Code.  This is a great IDE for this kind of project; you get the ease of the Arduino framework for the low level robot code and you get a very nice JavaScript experience (unlike the Arduino IDE).  Debugging the client code is done using browser developer tools.</p>

Title: Programming Robots with JavaScript