问:
How is PIV control performed?
I'm considering experimenting with PIV control instead of PID control. Contrary to PID, PIV control has very little explanation on the internet and literature. There is almost a single source of information explaining the method, which is a technical paper by Parker Motion.

What I understand from the control method diagram (which is in Laplace domain) is that the control output boils down to the sum of:

Kpp*(integral of position error)

-Kiv*(integral of measured velocity)

-Kpv*(measured velocity)

Am I correct? Thank you.

答:
It seems to me like there are three basic differences between the classic PID topology and the so-called PIV topology mentioned in the white paper:

Desired velocity is assumed to be proportional to position error, the $K_p$ term regulates this.

The integral error gain $K_i$ works to remove steady state errors in velocity, not position. That is essentially the same thing, however, due to item #1.

The velocity estimate is fed directly through the $K_v$ term (instead of considering the derivative of position error).

In the paper they claim that the main advantage of this topology is that it is easier to tune.

The output of the controller is formed as follows:

$e_\theta=\theta^*-\theta\\
e_\omega = (K_pe_\theta-\hat{\omega})\\
\text{output} = \int K_ie_\omega dt - K_v\hat{\omega}$

Of course, since you will probably be programming this, the integral is replaced by an accumulator variable as follows:

$e_\theta=\theta^*-\theta\\
e_\omega = (K_pe_\theta-\hat{\omega})\\
\text{integral} = \text{integral} + K_ie_\omega \Delta t \\
\text{output} = \text{integral} - K_v\hat{\omega}$

Title: How is PIV control performed?


问:
CompressedImage to an Image in a node

Update

Hey I have the following subscriber on Nvidia TX1 board running on an agricultural robot. we have the following issue with subscribing to Sensor_msgs::Compressed:

```<code>ImageConverter(ros::NodeHandle &n) :  n_(n), it_(n_)
{
  image_pub_ = it_.advertise("/output_img",1);

  cv::namedWindow(OPENCV_WINDOW);
  image_transport::TransportHints TH("compressed");
  image_sub_compressed.subscribe(n,"/Logitech_webcam/image_raw/compressed",5,&ImageConverter::imageCallback,ros::VoidPtr(),TH);
}
</code>```

And the callback function

```<code>void imageCallback(const sensor_msgs::CompressedImageConstPtr& msg)
</code>```

When I compile this I get an error:

```<code>from /home/johann/catkin_ws/src/uncompressimage/src/publisher_uncompressed_images.cpp:1:
/usr/include/boost/function/function_template.hpp: In instantiation of ‘static void boost::detail::function::function_void_mem_invoker1<MemberPtr, R, T0>::invoke(boost::detail::function::function_buffer&, T0) [with MemberPtr = void (ImageConverter::*)(const boost::shared_ptr<const sensor_msgs::CompressedImage_<std::allocator<void> > >&); R = void; T0 = const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&]’:
/usr/include/boost/function/function_template.hpp:934:38:   required from ‘void boost::function1<R, T1>::assign_to(Functor) [with Functor = void (ImageConverter::*)(const boost::shared_ptr<const sensor_msgs::CompressedImage_<std::allocator<void> > >&); R = void; T0 = const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&]’
/usr/include/boost/function/function_template.hpp:722:7:   required from ‘boost::function1<R, T1>::function1(Functor, typename boost::enable_if_c<boost::type_traits::ice_not<boost::is_integral<Functor>::value>::value, int>::type) [with Functor = void (ImageConverter::*)(const boost::shared_ptr<const sensor_msgs::CompressedImage_<std::allocator<void> > >&); R = void; T0 = const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&; typename boost::enable_if_c<boost::type_traits::ice_not<boost::is_integral<Functor>::value>::value, int>::type = int]’
/usr/include/boost/function/function_template.hpp:1069:16:   required from ‘boost::function<R(T0)>::function(Functor, typename boost::enable_if_c<boost::type_traits::ice_not<boost::is_integral<Functor>::value>::value, int>::type) [with Functor = void (ImageConverter::*)(const boost::shared_ptr<const sensor_msgs::CompressedImage_<std::allocator<void> > >&); R = void; T0 = const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&; typename boost::enable_if_c<boost::type_traits::ice_not<boost::is_integral<Functor>::value>::value, int>::type = int]’
/home/johann/catkin_ws/src/uncompressimage/src/publisher_uncompressed_images.cpp:27:126:   required from here
</code>```

The red error statement was:

```<code>/usr/include/boost/function/function_template.hpp:225:11: error: no match for call to ‘(boost::_mfi::mf1<void, ImageConverter, const boost::shared_ptr<const sensor_msgs::CompressedImage_<std::allocator<void> > >&>) (const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&)’
           BOOST_FUNCTION_RETURN(boost::mem_fn(*f)(BOOST_FUNCTION_ARGS));
</code>```

I am not using BOOST, and searching around hasn't helped me solve it

答: Boost is linked against your code by default when you're using ROS. I'll take a guess, you have namespace issue. Refrain from using "using namespace" and explicitly use cv:: in front of your calls. It's probably a name collision with Boost.</p>

ContentLicense: CC BY-SA 3.0

Title: CompressedImage to an Image in a node


问:
What is the actual physical actuated quantity when controlling the position of a servo?
I'm trying to learn about servo control. I have seen that the most generic position control method for servos is PID, where the control input is position error. However, I am not sure about what is the actuated quantity. I am guessing that it is one of:

Voltage applied to the motor

Current applied to the motor

I am then guessing that the actuated quantity gets turned into one of:

Torque that the motor exerts

Angular velocity that the motor runs at

I haven't been able to get my hands on and explicitly control a physical servo so I cannot confirm that the actuated quantity is any of these. I know very little of the electronics that controls the motor. It might well be that the controlled quantities are different for different series servos.

My bet is on torque control. However, assume that the servo is holding a weight at a distance (so it is acting against gravity), which means an approximately constant torque load. In this case, if the position error is zero and the servo is at rest, then each of P, I and D components are zero, which means the exerted torque is zero. This would cause the weight to sink, which is countered by the error in its position causing P,I components to increase. Wouldn't this situation cause the lifted weight to oscillate and balance at a constant position which is significantly different from the goal position? This isn't the case with the videos of servos I have seen lifting weights. Or is this the case and friction is smoothing everything out? Please help me understand.

答:
Your intuition is right but your question is actually a bit off, I would say. The variable controlled is indeed position. What you want to know is what variable is actuated, that is, what do we use to control position.

I don't know much about the internals of servomotors, but when performing PID control of DC motors it is quite common to use the voltage applied to the motor through pulse-width modulation.

Regarding the second part of your question, part of what you see is probably due to the gears or simply appropriate tuning of the controller, but I can't say much about it without seeing the videos you mention.

If you want to know more about the interplay of angular velocity, torque, etc., you should look up DC motor models, which is one of the building blocks of servos. That way you will find that it is common to assume that angular velocity is proportional to armature current and other things, and understand how the blue box the white paper you mentioned represents the motor.


问:
Using robotic simulator for prediction step in probabilistic localization approaches
Probabilistic localization approaches like Kalman or Monte Carlo benefit from an accurate prediction step. The more accurate the prediction step, the more accurate is the belief of the robots pose. In most approaches probabilistic motion models are applied, mainly because robot dynamics are more difficult to model. Still some approaches rely on dynamic models in order to increase the accuracy.

Therefore, I was wondering if it’s reasonable to utilize a robotic simulator like V-REP or Gazebo for the prediction step. The advantages I see in doing so are the following:

the robots kinematic is solved by default, simply through modeling it in the robotic simulator

the robots dynamics are taken into account

nonlinear behaviors like slippage or collision can be modelled up to a certain extend

the robots workspace is taken into account, by modeling its environment (if the robot drives against a wall previous models would predict it behind the wall, which won’t happen in a robotic simulator)

With the shown advantages I hope to achieve a more accurate prediction.

However there might be some problems using a robotic simulator. For a start it has to ensure real time behavior and there will be delay in the prediction due to the communication with the simulator.

I was looking for some papers which pick up on that idea but couldn’t find any. Are there any approaches similar to my idea? If not, are there any reasons why nobody is using a robotic simulator for the prediction? What are your opinions about my proposal?

答:
You are spot on by stating that a full-blown simulator would take into account physical interactions with the environment, e.g. "forbidding" the robot to get into a wall.

However, something like a kalman filter requires the prediction step to both predict the mean and the covariances of the probability distribution, something that general simulators do not do.

Using such a simulator would be doable within a particle filter, though, and might be beneficial if the prediction heavily depends on complex interactions with the environment.


答:
Probabilistic localization is normally done with a hypothesis tracker aka particle filter. To use this approach together with a physics-engine, different engines of Box2D has to used in parallel (one instance for one particle). To generate a new instance the following code-snippet will work:

```<code>for i in range(len(self.engine[0].joint)):
  self.engine[destination].joint[i].motorSpeed = self.engine[source].joint[i].motorSpeed
  self.engine[destination].joint[i].bodyA.angularVelocity = self.engine[source].joint[i].bodyA.angularVelocity
  self.engine[destination].joint[i].bodyB.angularVelocity = self.engine[source].joint[i].bodyB.angularVelocity
for i in range(len(self.engine[0].body)):
  self.engine[destination].body[i].position = self.engine[source].body[i].position
  self.engine[destination].body[i].angle = self.engine[source].body[i].angle
  self.engine[destination].body[i].linearVelocity = self.engine[source].body[i].linearVelocity
  self.engine[destination].body[i].angularVelocity = self.engine[source].body[i].angularVelocity
</code>```

Physics based localization is not discussed in literature, but Physics based control:

DANCE: Faloutsos, Petros and Van de Panne, Composable controllers for physics-based character animation (2001)

MuJoCo: Todorov, Emanuel and Erez, Tom, A physics engine for model-based control, 2012

Title: Using robotic simulator for prediction step in probabilistic localization approaches

Body:
Probabilistic localization approaches like Kalman or Monte Carlo benefit from an accurate prediction step. The more accurate the prediction step, the more accurate is the belief of the robots pose. In most approaches probabilistic motion models are applied, mainly because robot dynamics are more difficult to model. Still some approaches rely on dynamic models in order to increase the accuracy.

Therefore, I was wondering if it’s reasonable to utilize a robotic simulator like V-REP or Gazebo for the prediction step. The advantages I see in doing so are the following:

the robots kinematic is solved by default, simply through modeling it in the robotic simulator

the robots dynamics are taken into account

nonlinear behaviors like slippage or collision can be modelled up to a certain extend

the robots workspace is taken into account, by modeling its environment (if the robot drives against a wall previous models would predict it behind the wall, which won’t happen in a robotic simulator)

With the shown advantages I hope to achieve a more accurate prediction.

However there might be some problems using a robotic simulator. For a start it has to ensure real time behavior and there will be delay in the prediction due to the communication with the simulator.

I was looking for some papers which pick up on that idea but couldn’t find any. Are there any approaches similar to my idea? If not, are there any reasons why nobody is using a robotic simulator for the prediction? What are your opinions about my proposal?

Body:
Probabilistic localization is normally done with a hypothesis tracker aka particle filter. To use this approach together with a physics-engine, different engines of Box2D has to used in parallel (one instance for one particle). To generate a new instance the following code-snippet will work:

for i in range(len(self.engine[0].joint)):
  self.engine[destination].joint[i].motorSpeed = self.engine[source].joint[i].motorSpeed
  self.engine[destination].joint[i].bodyA.angularVelocity = self.engine[source].joint[i].bodyA.angularVelocity
  self.engine[destination].joint[i].bodyB.angularVelocity = self.engine[source].joint[i].bodyB.angularVelocity
for i in range(len(self.engine[0].body)):
  self.engine[destination].body[i].position = self.engine[source].body[i].position
  self.engine[destination].body[i].angle = self.engine[source].body[i].angle
  self.engine[destination].body[i].linearVelocity = self.engine[source].body[i].linearVelocity
  self.engine[destination].body[i].angularVelocity = self.engine[source].body[i].angularVelocity

Physics based localization is not discussed in literature, but Physics based control:

DANCE: Faloutsos, Petros and Van de Panne, Composable controllers for physics-based character animation (2001)

MuJoCo: Todorov, Emanuel and Erez, Tom, A physics engine for model-based control, 2012


问:
PID Gains: Drop in control loop rate, need to retune?
Good Day,

I am working on an autonomous quadcopter. May I ask if there is a significant difference if my control loop dropped from 500Hz to 460Hz due to added lines of code that would require retuning of the PID gains? And if retuning is required, is it correct to assume that only the I and D gains should be retweaked since they are the only constants which are time dependent? Thank you :)

答:
All three constants should be able to stay the same assuming that you calculate the derivative and integral using the actual time step size.

The only effect of a lower sampling rate should be a bigger "delay" in the plant, you are trying to control, due to zero order hold. Also the plant should not have any important dynamics above half the sampling frequency (Nyquist frequency).


问:
Path planning of 2 arm 4dof Robot
I am working on path planning for a 2 arm 4dof (2 dof for each arm) robot. I am currently using a centralised planning methodology (considering the multi robot system as a single one with higher dof, 4 in this case) and A* algorithm to find the shortest path. The problem with this algorithm is its high computation time.Is there any way to reduce the computation time while still obtaining the shortest route ?

Note:decentralised path planning is not good enough for my case.

答: According to https://stackoverflow.com/questions/221311/what-is-the-most-efficient-way-of-finding-a-path-through-a-small-world-graph A* is one of the best algorithm. Other algorithm like RRT working with the same principle (generate a graph of possible moves and select the best). To improve the performance a datadriven approach could help. In the paper "Block A*: Database-Driven Search with Applications in Any-angle Path-Planning" this kind of algorithm is explained. Another possibility (which i prefer) is E-RRT which is described in "Bruce, James: Real-time randomized path planning for robot navigation, 2002". The implementation is difficult and the topic is under current research.</p>

Title: Path planning of 2 arm 4dof Robot


问:
Battery damaged?
Could you please see the attached battery images and tell me if it is safe to continue using this battery or should I discard it?

答:
If the battery cost less than the thing you're powering, then you should probably discard the battery.

If you're stubborn (or very strapped for cash) and the battery didn't catch fire when the original damage happened, then it might perform okay, but I'd say there's a moderate chance of fire, especially at high currents.

Check battery voltage with a multimeter, then come back and check it again after a few hours. You're looking for a high self-discharge rate, which would indicate the battery is internally shorted out.

If the battery voltage drops noticeably after a few hours (or even a day), then you should trash the battery.

The terminal voltage on a lithium battery only drops about a quarter of a volt from 80% to 20% state of charge, so anything more than a hundredth of a volt or two and I'd call that significant. Every hundredth is about 2.4 percent of total battery capacity in that range, so 0.02V is roughly 5% of battery capacity.

Title: Battery damaged?


问:
Understanding and correct drift when using BreezySLAM (aka tinySLAM / CoreSLAM)
I was looking for a Python implementation of SLAM and stumbled upon BreezySLAM which implements tinySLAM aka CoreSLAM.

My robot is equipped with the hokuyo urg-04lx-ug01.

I have odometry hence passing it to the updater:

```<code>self.slam.update(ls_array, (dxy_mm, dtheta_deg, dt));
</code>```

As I start moving the robot starts discovering room A and then room B & C already the map seems to have rotated. I come back to room A and return the initial pose end=start using the same path. Now I noticed room A has significantly rotated in relation to the other room. Consequently the map isn't correct at all, neither is the path travelled by the robot.

Wasn't the SLAM supposed to store and keep the boundaries for the first room it discovered?

Why this rotation may be happening?

How could I try to troubleshoot this issue with the data I have collected (odometry, calculated position, liDAR scans)?

Can I tune SLAM to do a better job for my robot?

SLAM is pretty new to me, so please bear with me, any pointers on literature that may clarify and moderate my expectations of what SLAM can do.

Extra

... and Here the best video I found to understand particle filter

答:
You'll find that Gmapping works a lot better. I have used core slam quite a bit with the 04lx, tweaked the code, and tuned the algorithm. It works in a lot of cases, but...

If you really want to keep using it, adjust your parameters so that it searches more (more particles) and really make sure that the robot motion is inside the search domain.

While tuning (and in general) monitor your map and restart with better parameters if the map jumps or rotates.


问:
What are the specifications of the digital compass used in iPhone 6S
What are the specifications of the digital compass used in the iPhone 6S?
I am trying to measure yaw angle using the magnetometer.  I observed the magnetometer/digital compass in the iPhone is really very stable. The north direction is always the same, while the magnetometer I am using (or the magnetometer used in Nexus) needs to be calibrated again and again to function properly.

I found that the digital compass AK8963C is used in the iPhone 6, but it needs calibration.  So I am not sure what is inside iPhone 6S because it works without a calibration procedure.

答:
Typically, and unsurprisingly, proprietary information (to include part numbers, schematics, etc.) is closely guarded by companies. As such, I am not sure how much information you can expect to receive without significant effort on your part. For starters, you can be sure that Apple will not release its specifications or parts list.

Take a look at the iFixIt tear down of the iPhone 6s and you will see no mention of the compass chip. Doing some more research, you can find the ChipWorks parts list of the iPhone 6s, and again, the compass part number and manufacturer are listed as "unknown." The reason for this is the A9 system on a chip, which now incorporates a number of sensors -- including the magnetometer -- which were discretely included in previous models. Furthermore, you can read about the M9 co-processor that sits inside the A9 and is used to process sensor data.

Despite the lack of information, I would wager that the AK8963C is the digital compass used in the iPhone 6s. I base this speculation on the fact that the AK8963C was used in both the iPhones 5, 5s, and 6. In fact, the same IMUs are used between most, if not all, of these models, which suggests that Apple has invested in this set of sensors and is content with their performance. Lastly, I am not intimately familiar with the iPhone 6 and 6s, but again, I would wager that the difference between the two, at least in terms of capability, is marginal. Assuming that the AK8963C is the compass, the datasheet is available, and should answer several high-level and many low-level questions that you have.

As for calibration--yes, the AK8963C needs it. Again, I suspect that you will have a difficult time getting any manufacturer to detail the procedure, especially Apple. Among other relevant things, the datasheet highlights some tuning parameters, but at the end of the day, this is all happening in software--whether you require the user to calibrate, or handle it automatically, calibration is almost certainly happening. Still, this article and the many like it suggest that perhaps the iPhone 6s sensor outputs are not a accurate as your observations exist.


问:
quaternion implementation
I am trying to implement quaternions and i am using CC2650 sensortag board from TI. This board has MPU9250 from invensense which has Digital Motion Processor (DMP) in it. This DMP gives quaternion, but for my understanding i implemented my own quaternion. I used Gyroscope and acceleorometer values coming out of DMP (which are calibrated ) to calculate angle of rotation. I feed this angle, in 3 directions (x,y,z), to my quaternion. I am not able to match my quaternion values with DMP quaternion values. In fact it's way off, so wondering what I have done wrong.

Following are detailed steps that i did :

1)  Tapped Gyro sensor values from function “read_from_mpl”.

2)  Converted gyro values in to float by diving by 2^16. As gyro values are in Q16 format.

3)  Now used Gyro values of 3 axis and found out resultant using formula :
Gr = sqrt(Gx^2+Gy^2+Gz^2)
     Where Gx,Gy and Gz are Gyro values along x-axis,y-axis and z-axis respectively.

4)  Now Angle is derived using above found resultant Gr by :
*Angle = Gr*1/sample_rate*
       Where sample_rate is found using API call ,mpu_get_sample_rate(&sample_rate)

5)  This Angle is fed to angle_to_quater function which basically converts angle to axis and then quaternion multiplication.

```<code>/* Angle to axis and quaternion multiplication: */
temp.w = cos((Angle*1.0/RAD_TO_DEG)/2);
temp.x = sin((Angle*1.0/RAD_TO_DEG)/2);
temp.y = sin((Angle*1.0/RAD_TO_DEG)/2);
temp.z = sin((Angle*1.0/RAD_TO_DEG)/2);
temp.x = temp.x *gyro_axis[0];//gyro_axis[0]=Gx
temp.y = temp.x *gyro_axis[1]; //gyro_axis[0]=Gy
temp.z = temp.x *gyro_axis[2]; //gyro_axis[0]=Gz
/* quaternion multiplication and normalization */
res = quat_mul(*qt,temp);
quat_normalize(&res);
*qt = res;*
</code>```

6)  I also added  doing angle calculations from accelerometer as follows : Here also accelerometer is converted to float by dividing by 2^16, as acceleorometer values also in Q16 format.

```<code>*//acc_data[0]->Ax, acc_data[1]->Ay, acc_data[2]->Az
temp = (acc_data[0]*acc_data[0]) + (acc_data[1]*acc_data[1]);
acc_angle[0]=atan2(acc_data[2],temp)*RAD_TO_DEG;
temp = (acc_data[1]*acc_data[1]) + (acc_data[2]*acc_data[2]);
acc_angle[1]=atan2(acc_data[0],temp)*RAD_TO_DEG;
temp = (acc_data[1]*acc_data[1]) + (acc_data[0]*acc_data[0]);
acc_angle[2]=atan2(acc_data[1],temp)*RAD_TO_DEG;*
</code>```

*Find resultant angle of this also as :

```<code>inst_acc_angle = (sqrt(acc_angle[0]*acc_angle[0] + acc_angle[1]*acc_angle[1] + acc_angle[2]*acc_angle[2]));*
</code>```

7)  Then complimentary filter is :
*FinalAngle = 0.96*Angle + 0.04*inst_acc_angle;
This Final Angle is fed to step 5 to get quaternion.*

Quaternion multiplication is done as below and then normailized to get new quaternion (q).

quater_mul :

```<code>q3.w = -q1.x * q2.x - q1.y * q2.y - q1.z * q2.z + q1.w * q2.w;
q3.x =  q1.x * q2.w + q1.y * q2.z - q1.z * q2.y + q1.w * q2.x;
q3.y = -q1.x * q2.z + q1.y * q2.w + q1.z * q2.x + q1.w * q2.y;
q3.z =  q1.x * q2.y - q1.y * q2.x + q1.z * q2.w + q1.w * q2.z;
</code>```

quat_normalize:

```<code>double mag = pow(q->w,2) + pow(q->x,2) + pow(q->y,2) + pow(q->z,2);
mag = sqrt(mag);
q->w = q->w/mag;
q->x = q->x/mag;
q->y = q->y/mag;
q->z = q->z/mag;
</code>```

When i check my quaternion values with DMP, they are WAY off. Can you please provide some insights in to what could be wrong here.

Source code :

```<code>acc_data[0]=data[0]/65536.0;
acc_data[1]=data[1]/65536.0;
acc_data[2]=data[2]/65536.0;
double temp = (acc_data[0]*acc_data[0]) + (acc_data[1]*acc_data[1]);
acc_angle[0]=atan2(acc_data[2],temp)*RAD_TO_DEG;
temp = (acc_data[1]*acc_data[1]) + (acc_data[2]*acc_data[2]);
acc_angle[1]=atan2(acc_data[0],temp)*RAD_TO_DEG;
temp = (acc_data[1]*acc_data[1]) + (acc_data[0]*acc_data[0]);
acc_angle[2]=atan2(acc_data[1],temp)*RAD_TO_DEG;*

gyro_rate_data[0]=data[0]/65536.0;
gyro_rate_data[1]=data[1]/65536.0;
gyro_rate_data[2]=data[2]/65536.0;

float inst_angle = (sqrt(gyro_rate_data[0]*gyro_rate_data[0] +  gyro_rate_data[1]*gyro_rate_data[1] + gyro_rate_data[2]*gyro_rate_data[2]));
gyro_rate_data[0] = gyro_rate_data[0]/inst_angle;
gyro_rate_data[1] = gyro_rate_data[1]/inst_angle;
gyro_rate_data[2] = gyro_rate_data[2]/inst_angle;
inst_angle = inst_angle *1.0/sam_rate;
float inst_acc_angle = (sqrt(acc_angle[0]*acc_angle[0] + acc_angle[1]*acc_angle[1] + acc_angle[2]*acc_angle[2]));
inst_angle = WT*inst_angle + (1.0-WT)*inst_acc_angle;

angle_to_quat(inst_angle,gyro_rate_data,&q);

/* The function for angle to quaterinion and multiplication,normalization */
void angle_to_quat(float Angle,float *gyro_axis,struct quat *qt)
{
    struct quat temp;
    struct quat res;
    temp.w = cos((Angle*1.0/RAD_TO_DEG)/2);
    temp.x = sin((Angle*1.0/RAD_TO_DEG)/2);
    temp.y = sin((Angle*1.0/RAD_TO_DEG)/2);
    temp.z = sin((Angle*1.0/RAD_TO_DEG)/2);
    temp.x = temp.x *gyro_axis[0];
    temp.y = temp.x *gyro_axis[1];
    temp.z = temp.x *gyro_axis[2];
    res = quat_mul(*qt,temp);
    quat_normalize(&res);
    *qt = res;
}
</code>```

This variation is coming when i am keeping my device stationary.

Y-Axis : Resultant of all 3 gyro axis.

X-axis : The number of samples. (have not converted them to time)

Sample_rate is 3Hz.

答:
So, as I mentioned in an earlier comment, it looks like you're using a mashup of methods. You're not applying any one method correctly; instead you're mis-using part of one method, then using the results of that to another mis-applied half-method, and so on.

First, maybe a refresher on rotation matrices. You can rotate about each of the three primary axes - the x-axis ($\phi$ or "roll"), the y-axis ($\theta$ or "pitch"), and the z-axis ($\psi$ or "yaw").

The order in which you do these operations matters - if you pitch then yaw, that gives you a different orientation than if you yaw then pitch. Try with your hand and see, and remember that each rotation happens about the hand's orientation axes, not global axes.

For each rotation, you multiply the rotation matrices together. Conceptually, this is representing a set of successive rotations. For example, if you have:

$$
\mbox{output} = R(\psi)R(\theta)R(\phi) \mbox{input} \\
$$
then you can see that this is equivalent to:
$$
\mbox{intermediate}_{\phi} = R(\phi)\mbox{input} \\
\mbox{intermediate}_{\theta} = R(\theta)\mbox{intermediate}_{\phi} \\
\mbox{output} = R(\psi)\mbox{intermediate}_{\theta} \\
$$

This explicitly shows successive rotations, but you can also "collapse" the rotation matrices into one "shortcut" rotation matrix -

$$
R(\mbox{net}) = R(\psi)R(\theta)R(\phi) \\
\mbox{output} = R(\mbox{net})\mbox{input} \\
$$

First portion of your code.

That mentioned, take a look at the beginning of your code. I think the MathJax is easier to look at than code, so I'll use that. First, you get the accelerometer data and then "convert to angles" as follows:

$$
\phi_x = \tan^{-1}{\left(\frac{a_z}{a_x^2 + a_y^2}\right)} \\
\theta_y = \tan^{-1}{\left(\frac{a_x}{a_y^2 + a_z^2}\right)} \\
\psi_z = \tan^{-1}{\left(\frac{a_y}{a_y^2 + a_x^2}\right)} \\
$$

First thing to note about this is that generally you have one axis divided by the other two, except for the last line - there you have y/(y^2+x^2), which breaks your pattern.

Second thing to note about this is that it seems like maybe you're trying to use the same method given in this application note from Freescale Semiconductor (Document Number: AN3461) that says (briefly, from Page 10):

Solving for the roll and pitch angles... and using the subscript xyz to denote that the roll and pitch angles are computed according to the rotation sequence $R_{xyz}$, gives:

$$
\tan{\phi}_{xyz} = \left(\frac{G_{py}}{G_{pz}}\right) \\
\tan{\theta}_{xyz} = \frac{-G_{px}}{\sqrt{G_{py}^2 + G_{pz}^2}} \\
$$

Solving for the roll and pitch angles... and using the subscript yxz to denote that the angles are computed according to the rotation sequence $R_{yxz}$, gives:

$$
\tan{\phi}_{xyz} = \frac{G_{py}}{\sqrt{G_{px}^2 + G_{pz}^2}} \\
\tan{\theta}_{xyz} = \left(\frac{-G_{px}}{G_{pz}}\right) \\
$$

The document also states (bottom of Page 9) that isn't not possible to get yaw angle without a compass. So, hopefully at this point you can begin to see what I'm calling a "misapplied mashup":

Your method uses $\tan^{-1}\left(a/(b^2 + c^2)\right)$ for each angle. This is missing a square root symbol in the denominator.

More importantly, if you use that form for roll and pitch angles, then you're getting roll from one rotation sequence and pitch from a different rotation sequence.

Also, the sign on that form should change.

Regardless of points 1-3 above, you cannot use this method for yaw angle.

Second portion of your code.

Now we move on from here to the gyro section. Remember that a gyroscope outputs angular velocities. It looks like you calculate the "rotational velocity magnitude" as:

$$
\mbox{inst_angle} = \sqrt{\omega_x^2 + \omega_y^2 + \omega_z^2} \\
$$

And then you calculate an "axis" as:

$$
\mbox{x-axis component} = \frac{\omega_x}{\mbox{inst_angle}} \\
\mbox{y-axis component} = \frac{\omega_y}{\mbox{inst_angle}} \\
\mbox{z-axis component} = \frac{\omega_z}{\mbox{inst_angle}} \\
$$

This seems to work out, except you calculate the incremental angle that you use to rotate about this axis:

$$
\mbox{inst_angle} = \mbox{inst_angle}/\mbox{sample rate} \\
$$
which, for the record, is equivalent to:
$$
\mbox{inst_angle} = \mbox{inst_angle} * \mbox{sample period} \\
$$

At this point, you get the "mis-applied" portion of the gyro code. The accelerometer-angle equations from the first section should give absolute angles (based on whichever rotation sequence you choose to use*. The gyro-angle equation above gives you an incremental angle. Further, the gyro-angle is the incremental rotation about the axis you calculate, where the accelerometer-angle is the absolute angle relative to the world frame.

However, even though these are different angles (incremental vs. absolute), and even though they're in different frames (about an axis vs. relative to world frame), you proceed to push the two together by smooshing the accelerometer-angles into a "magnitude" (which doesn't make sense) and then add them with a complementary filter:

$$
\mbox{inst_accel_angle} = \sqrt{\phi_x^2 + \theta_y^2 + \psi_z^2} \\
\mbox{inst_angle} = \alpha \left(\mbox{inst_angle}\right) + (1-\alpha)\left(\mbox{inst_accel_angle}\right) \\
$$

Final portion of your code:

At this point, you have an "angle" and an "axis", and it looks like you correctly convert the axis/angle set to a quaternion, however, this quaternion is more of an incremental quaternion because the axis is based off of rate (angular velocity) information. The angle is a mash-up of incremental and absolute angles, so it doesn't mean much of anything.

Consider this - if you actively move the sensor to a new orientation, your quaternion will change to some non-zero axis while you're moving it because the axis is based on your angular velocity. When you hold it stead at the new orientation, even though it's pointed in some direction your axis information is now zero because your rate is zero.

The solution

Remember that a quaternion represents an orientation, so let's leave out the accelerometers to begin with. Follow these steps to implement a first-pass quaternion representation:

At the beginning of your code, set the starting orientation. Remember, a gyroscope gives a velocity (speed), so you have to tell it where it's starting. I would suggest orienting along a cardinal axis with no rotation. So, for example, if you align along the x-axis with no rotation, then you'd wind up with $Q = [0,1,0,0]$ for w,x,y,z.

Get your gyro data and build a rate vector (with values in rad/s!) as $S = [0,\omega_x,\omega_y,\omega_z]$.

Find how quickly this means your quaternion is moving by finding $\frac{dQ}{dt} = \frac{1}{2}Q\otimes S$ where $\otimes$ is the quaternion multiplication as you've defined it in your question.

Increment your quaternion based on the quaternion rate you calculated in Step 3 above: $Q = Q + \frac{dQ}{dt}/\mbox{sample rate}$ or $Q = Q + \frac{dQ}{dT}*\mbox{sample period}$.

That's it! That's all that you do to get the quaternion.

Now, as I mentioned earlier, the accelerometers give an absolute oriention (in roll and pitch only!!), so you can use those values to correct for long-term drift in the results you get from using the gyroscope. For more information on how to do this, try checking out the Madgwick Filter. (Final comment: I was going to link you directly to Sebastian Madgwick's page, but it looks like it's down. Not sure what happened, but I'd get the paper I linked sooner rather than later if you're interested, if he's taking down his IP.)

In response to your questions

OP asks (paraphrased) - "A quaternion $q$ represents a pose, so there is no angular rate information, so how does quaternion multiplication $q\otimes S$ make sense units-wise? If I consider $q$ to have units of rate, then the numeric integration doesn't make sense."

My response - I wish I could explain the physical meaning of the quaternion math, but I can't; I don't know how to. All I can say is, the formula for quaternion rate of change is $dQ/dt = \frac{1}{2}q\otimes S$, and that you then perform numeric integration by $q = q + (dQ/dt)*dt$.

Quaternions are unitless, so that may help your conceptual understanding. Consider the quaternion rate equation to be "scaling" the gyro rates to fit the quaternion format. That is, the term $\frac{1}{2}q\otimes$ converts $S$ to $dQ/dt$.

If you're having stability issues, the very first thing I would check is that you are actually passing your angular rates as radians per second and not degrees per second. Based on your math above, it looks like your gyro outputs in units of deg/s.

The next thing to do would be to use the correct sample_rate. For this, rather than dividing by the sample_rate I would multiply by the sample period. It's important to use the sample period of your software and not of the sensor. For example, if your software is running at 3Hz, but your sensor is running at 30Hz, then your sensor will report a sample period of 33.3ms, when in actuality you're only collecting samples at 3.3ms. This will cause your numeric integration to not work correctly.

Finally, the last to note is that this method of updating the quaternion is used to determine how the world coordinates convert to sensor coordinates. If you are interested in viewing sensor coordinates in world coordinates (for example, if you would like to plot a representation of the sensor), then you need to take the quaternion conjugate. This is akin to taking the inverse of a rotation matrix to flip directionality between world/sensor frames.

```<code>qConj = QuaternionConjugate(q)
qConj.w = q.w;
qConj.x = -q.x;
qConj.y = -q.y;
qConj.z = -q.z;
end;
</code>```

Title: quaternion implementation

Body:
So, as I mentioned in an earlier comment, it looks like you're using a mashup of methods. You're not applying any one method correctly; instead you're mis-using part of one method, then using the results of that to another mis-applied half-method, and so on.

First, maybe a refresher on rotation matrices. You can rotate about each of the three primary axes - the x-axis ($\phi$ or "roll"), the y-axis ($\theta$ or "pitch"), and the z-axis ($\psi$ or "yaw").

The order in which you do these operations matters - if you pitch then yaw, that gives you a different orientation than if you yaw then pitch. Try with your hand and see, and remember that each rotation happens about the hand's orientation axes, not global axes.

For each rotation, you multiply the rotation matrices together. Conceptually, this is representing a set of successive rotations. For example, if you have:

$$
\mbox{output} = R(\psi)R(\theta)R(\phi) \mbox{input} \\
$$
then you can see that this is equivalent to:
$$
\mbox{intermediate}_{\phi} = R(\phi)\mbox{input} \\
\mbox{intermediate}_{\theta} = R(\theta)\mbox{intermediate}_{\phi} \\
\mbox{output} = R(\psi)\mbox{intermediate}_{\theta} \\
$$

This explicitly shows successive rotations, but you can also "collapse" the rotation matrices into one "shortcut" rotation matrix -

$$
R(\mbox{net}) = R(\psi)R(\theta)R(\phi) \\
\mbox{output} = R(\mbox{net})\mbox{input} \\
$$

First portion of your code.

That mentioned, take a look at the beginning of your code. I think the MathJax is easier to look at than code, so I'll use that. First, you get the accelerometer data and then "convert to angles" as follows:

$$
\phi_x = \tan^{-1}{\left(\frac{a_z}{a_x^2 + a_y^2}\right)} \\
\theta_y = \tan^{-1}{\left(\frac{a_x}{a_y^2 + a_z^2}\right)} \\
\psi_z = \tan^{-1}{\left(\frac{a_y}{a_y^2 + a_x^2}\right)} \\
$$

First thing to note about this is that generally you have one axis divided by the other two, except for the last line - there you have y/(y^2+x^2), which breaks your pattern.

Second thing to note about this is that it seems like maybe you're trying to use the same method given in this application note from Freescale Semiconductor (Document Number: AN3461) that says (briefly, from Page 10):

Solving for the roll and pitch angles... and using the subscript xyz to denote that the roll and pitch angles are computed according to the rotation sequence $R_{xyz}$, gives:

$$
\tan{\phi}_{xyz} = \left(\frac{G_{py}}{G_{pz}}\right) \\
\tan{\theta}_{xyz} = \frac{-G_{px}}{\sqrt{G_{py}^2 + G_{pz}^2}} \\
$$

Solving for the roll and pitch angles... and using the subscript yxz to denote that the angles are computed according to the rotation sequence $R_{yxz}$, gives:

$$
\tan{\phi}_{xyz} = \frac{G_{py}}{\sqrt{G_{px}^2 + G_{pz}^2}} \\
\tan{\theta}_{xyz} = \left(\frac{-G_{px}}{G_{pz}}\right) \\
$$

The document also states (bottom of Page 9) that isn't not possible to get yaw angle without a compass. So, hopefully at this point you can begin to see what I'm calling a "misapplied mashup":

Your method uses $\tan^{-1}\left(a/(b^2 + c^2)\right)$ for each angle. This is missing a square root symbol in the denominator.

More importantly, if you use that form for roll and pitch angles, then you're getting roll from one rotation sequence and pitch from a different rotation sequence.

Also, the sign on that form should change.

Regardless of points 1-3 above, you cannot use this method for yaw angle.

Second portion of your code.

Now we move on from here to the gyro section. Remember that a gyroscope outputs angular velocities. It looks like you calculate the "rotational velocity magnitude" as:

$$
\mbox{inst_angle} = \sqrt{\omega_x^2 + \omega_y^2 + \omega_z^2} \\
$$

And then you calculate an "axis" as:

$$
\mbox{x-axis component} = \frac{\omega_x}{\mbox{inst_angle}} \\
\mbox{y-axis component} = \frac{\omega_y}{\mbox{inst_angle}} \\
\mbox{z-axis component} = \frac{\omega_z}{\mbox{inst_angle}} \\
$$

This seems to work out, except you calculate the incremental angle that you use to rotate about this axis:

$$
\mbox{inst_angle} = \mbox{inst_angle}/\mbox{sample rate} \\
$$
which, for the record, is equivalent to:
$$
\mbox{inst_angle} = \mbox{inst_angle} * \mbox{sample period} \\
$$

At this point, you get the "mis-applied" portion of the gyro code. The accelerometer-angle equations from the first section should give absolute angles (based on whichever rotation sequence you choose to use*. The gyro-angle equation above gives you an incremental angle. Further, the gyro-angle is the incremental rotation about the axis you calculate, where the accelerometer-angle is the absolute angle relative to the world frame.

However, even though these are different angles (incremental vs. absolute), and even though they're in different frames (about an axis vs. relative to world frame), you proceed to push the two together by smooshing the accelerometer-angles into a "magnitude" (which doesn't make sense) and then add them with a complementary filter:

$$
\mbox{inst_accel_angle} = \sqrt{\phi_x^2 + \theta_y^2 + \psi_z^2} \\
\mbox{inst_angle} = \alpha \left(\mbox{inst_angle}\right) + (1-\alpha)\left(\mbox{inst_accel_angle}\right) \\
$$

Final portion of your code:

At this point, you have an "angle" and an "axis", and it looks like you correctly convert the axis/angle set to a quaternion, however, this quaternion is more of an incremental quaternion because the axis is based off of rate (angular velocity) information. The angle is a mash-up of incremental and absolute angles, so it doesn't mean much of anything.

Consider this - if you actively move the sensor to a new orientation, your quaternion will change to some non-zero axis while you're moving it because the axis is based on your angular velocity. When you hold it stead at the new orientation, even though it's pointed in some direction your axis information is now zero because your rate is zero.

The solution

Remember that a quaternion represents an orientation, so let's leave out the accelerometers to begin with. Follow these steps to implement a first-pass quaternion representation:

At the beginning of your code, set the starting orientation. Remember, a gyroscope gives a velocity (speed), so you have to tell it where it's starting. I would suggest orienting along a cardinal axis with no rotation. So, for example, if you align along the x-axis with no rotation, then you'd wind up with $Q = [0,1,0,0]$ for w,x,y,z.

Get your gyro data and build a rate vector (with values in rad/s!) as $S = [0,\omega_x,\omega_y,\omega_z]$.

Find how quickly this means your quaternion is moving by finding $\frac{dQ}{dt} = \frac{1}{2}Q\otimes S$ where $\otimes$ is the quaternion multiplication as you've defined it in your question.

Increment your quaternion based on the quaternion rate you calculated in Step 3 above: $Q = Q + \frac{dQ}{dt}/\mbox{sample rate}$ or $Q = Q + \frac{dQ}{dT}*\mbox{sample period}$.

That's it! That's all that you do to get the quaternion.

Now, as I mentioned earlier, the accelerometers give an absolute oriention (in roll and pitch only!!), so you can use those values to correct for long-term drift in the results you get from using the gyroscope. For more information on how to do this, try checking out the Madgwick Filter. (Final comment: I was going to link you directly to Sebastian Madgwick's page, but it looks like it's down. Not sure what happened, but I'd get the paper I linked sooner rather than later if you're interested, if he's taking down his IP.)

In response to your questions

OP asks (paraphrased) - "A quaternion $q$ represents a pose, so there is no angular rate information, so how does quaternion multiplication $q\otimes S$ make sense units-wise? If I consider $q$ to have units of rate, then the numeric integration doesn't make sense."

My response - I wish I could explain the physical meaning of the quaternion math, but I can't; I don't know how to. All I can say is, the formula for quaternion rate of change is $dQ/dt = \frac{1}{2}q\otimes S$, and that you then perform numeric integration by $q = q + (dQ/dt)*dt$.

Quaternions are unitless, so that may help your conceptual understanding. Consider the quaternion rate equation to be "scaling" the gyro rates to fit the quaternion format. That is, the term $\frac{1}{2}q\otimes$ converts $S$ to $dQ/dt$.

If you're having stability issues, the very first thing I would check is that you are actually passing your angular rates as radians per second and not degrees per second. Based on your math above, it looks like your gyro outputs in units of deg/s.

The next thing to do would be to use the correct sample_rate. For this, rather than dividing by the sample_rate I would multiply by the sample period. It's important to use the sample period of your software and not of the sensor. For example, if your software is running at 3Hz, but your sensor is running at 30Hz, then your sensor will report a sample period of 33.3ms, when in actuality you're only collecting samples at 3.3ms. This will cause your numeric integration to not work correctly.

Finally, the last to note is that this method of updating the quaternion is used to determine how the world coordinates convert to sensor coordinates. If you are interested in viewing sensor coordinates in world coordinates (for example, if you would like to plot a representation of the sensor), then you need to take the quaternion conjugate. This is akin to taking the inverse of a rotation matrix to flip directionality between world/sensor frames.

qConj = QuaternionConjugate(q)
qConj.w = q.w;
qConj.x = -q.x;
qConj.y = -q.y;
qConj.z = -q.z;
end;


问:
Lidar problems in a multi-robot setup
Consider multiple mobile bases driving around in some area. In order to get meaningful data from the lidar of each base, the sensors should be mounted as horizontal as possible. Due to safety regulations, the lidars should also be mounted at a height of 15 cm from the floor. When I checked the data sheet of SICK lidars, it shows that all models use the wavelength 904 nm. Does that mean that mobile bases equipped with lidars with a coplanar scan lines will end up mutually blinding each other?

If it is the case, how is this problem solved? (I don't consider tilting the lidars a solution as it defeats the purpose of having "2D" lidars where even if the tilting angle is known, what the lidar observes becomes dependent on the robot's pose and distance from eventual obstacles)

答: I know that IR has a similar issue. One of the ways it's over come is to have the sensor send the signal in different patterns (ie a duty cycle of 75 verse 50). If you know what your expecting back you know if there was interference. From there you can have it wait a random amount of time and try again.</p>

Title: Lidar problems in a multi-robot setup


问:
Need help regarding odometry using Encoder motor and raspberry pi
I am doing project on odometry using raspberry pi. I know that encoder motor will tell me how much distance my robot has covered, but I have no idea ho to implement completely. I just need guideline about which steps to follow. Till now I have interfaced motor with raspberry pi and counted the number of rotation. I have questions as follow?

How to plot map of odometry using which language and library?

If you know anything, just give me guideline about steps to follow.

答:
You could maybe use Matlab to plot the position of your vehicle? This is how I'm trying to do that:
I have a 'logging'-program running on the Raspberry Pi that counts each sampling time the pulses from the encoders. This I log in a file. This file I upload in Matlab to calculate X and Y coordinate and the angle under which the vehicle is standing, assuming the vehicle starts at coordinate (0,0) and angle 0°.
These are the formula's I use. With 'triangle'-r and 'triangle'-l I mean the distance traveled by left and right wheel (can be calculated using pulses in log file and diameter of the wheel). Distance '2L' is the distance between the 2 wheels.

I wrote a matlab GUI in which I can browse a data file (from the Pi), calculates the X and Y position every sampling period and plot this is an XY graph.

Ps: you didn't mention it, but I suppose you have a differential drive vehicle?!


问:
Robot positioning using IMU quaternion data?
I want to use a MPU9150 to give me the position (XY) and heading (angle) of a wheeled robot. This MPU9150 from invensense has a Digital Motion Processor in it which can give me a quaternion.
But how do I convert this quaternion data to an XY-coordinate and an angle so I can plot the position of my vehicle?</p>

答:
The quaternion only contains information about the rotation of the vehicle. It will not contain information about the location of you vehicle on a 2-d plane.

One method of converting quaternions to euler angles is to create the transformation matrix that is defined by the rotation and get the euler angles from that.

https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles


问:
Working of Autonomous Lawn mower(ALM) in an unbounded area without a perimeter wire
I have an Autonomous Lawn mower(ALM) which can mow a certain lawn area when that area is bounded by a perimeter wire. Even when that perimeter wire is removed, it has to mow the above mentioned area accurately without slipping into a neighboring area.

Constraints and problems:

The ALM is an open loop system.

Differential GPS was tried, but it did not yield proper results.

Any iterative pattern of area coverage can be used provided the error in each iteration is not added cumulatively which can result in unpredictable error in the end.

I do not expect full fledged solution. But I need a starting point to understand motion planning particularly for unbounded robotics to solve this problem.

I searched on internet to know about the knowledge sources about motion planning but could not get good results. Can anyone guide me to know about such sources preferably books and articles on internet which can help me to solve this problem?

EDIT:
Addition of information:

The above picture shows the irregular lawn area which does not have any enclosures and perimeter
wire
1.The red mark shows the center point of lawn .

2.The grey area is the initial scaled down area which resembles in shape to the larger area .I could not draw the grey area which exactly resembles the larger green area .

3.The grey lines are the contours which from the tracks to be followed by the lawn mower

Idea description:

1.Using planimeter app for onetime , the shape and dimension of the lawn area (green area) can be known
Link:https://play.google.com/store/apps/details?id=com.vistechprojects.planimeter&hl=en

2.Center of polygon can be found by using the method in the following link
http://en.wikipedia.org/wiki/Centroid#Centroid_of_polygon

3.Calculation of area of grey shape in the above figure .

4 . Grey shape is the least possible area which can be grazed by the ALM . Grey shape is similar to the green area shape and it is formed when Green area is scaled down

To determine the scale down factor which is a numerical value ‘ n’ (n<1)
Where Grey area = n * Green area

Once the Grey area is known , the number of contours or tracks to be grazed by ALM have to be determined manually .

The width of contour is equal to the distance between the blades on the either end i.e. the width which can be grazed by ALM in a single stroke .

Green area = Grey area + area of track 1 + area of track 2 + area of track3 + . . . . . . + area of track n

5.Once the lawn mower is switched on ,it should reach the center of the lawn (red mark showed in the above figure)

6.Then, ALM should graze the least possible area or grey area .

7.After that ALM should Switch to contour circumscribing the grey area . It should continue circumscribing in each track till all the tracks are completed( decision has to be made by validating against the calculated and preset value ' No.of tracks' in ALM)

In this way entire lawn can be mowed without the need of perimeter wire and also ALM would not mow the neighbor’s lawn

Challenges :

a. Enable ALM to reach the center point of the lawn

a. To make ALM mow the grey area accurately

b. To make the ALM switch from one track to track .

c. To bypass the obstacle in track and return to the same track .

When i mentioned this idea to my colleague ,he mentioned the about possible cumulative addition of error in each iteration resulting in an unpredictable error in the end .

I intend to minimize the error and fix the boundary as correct as possible.
In fact this deviation should be predictable before it can be corrected .

答:
As others have said, you over constrained the problem.  In order to be workable you need to loosen some constraints.  Chiefly, you need to close the loop.  You have admitted as much when you said "There should also be a validation at certain crucial points of task, to check whether it's acting according to the algorithm"

You must recognize that an open loop motion command does not always result in consistent motion.  There will usually be one side driving on long grass and the opposite side on short, so it will tend to accumulate systematic error which can be compensated.  With careful observation, you could tune out path error caused by ground contour; for one location only, not a very universal solution.  But how do you compensate for varying moisture levels(coefficient of friction), obstacles, changing power levels in prime mover, wheel spin.  These are just a few of the problems unaddressed by your plan.  You just need some type of sensor feedback.

The simplest feedback system I can think of is a number of lever switches around the front and both sides with 9in^2 paddles that float on top of the grass and detect grass height(mowed or unmowed).

The operator would begin by manually mowing a perimeter strip.  Finishing with one side on already mowed grass then engaging autonomous mode.  If at any point all of the switches detect mowed grass, stop because you are done, or on your way to a neighbors yard.  Also stop if all switches detect unmowed grass because then you are again blazing a trail to the neighbors yard.  Either way, operator intervention is now required.  As long as one side is mowed and the opposite unmowed, proceed forward.  Use the array off switches along the front to proceed with a line following algorithm.

Working from the outside inward is preferred as the operator would still be onsite to observe the first few passes which present the greatest risk and probability of failure.  Is there a compelling reason that you chose to work from the inside outward without any way to detect the perimeter?


问:
battery question for DW558 Explorer (small quadrocopter)
I recently bought a DW558 Quadrocopter (http://www.gearbest.com/rc-quadcopters/pp_110531.html).

After few minutes, the battery is dead. Which is understandable since the battery is so tiny. It is a 3.7V 250mAh battery, included with the Quadrocopter. I was thinking about buying spare batteries for it, and I have few questions about this:

1: Can I buy any kind of battery 3.7V 250mAh of the same size or is there any other property I have to pay attention?

2: Can I buy batteries of 3.7V and 350mAh (100 more than the included battery) and expect my Quadrocopter to be more "energic"? Is it bad to buy batteries with more mAh ?

2b: If I buy few 3.7V 350mAh batteries, will I be able to charge them with the same charger I got with my 3.7V 250 mAh batteries or do I have to buy a specific charger for these too?

(these are the batteries I want to buy, any comment is greatly appreciated: 350mAh batteries x5 http://www.gearbest.com/rc-quadcopter-parts/pp_196991.html and/or 4x 250mAh batteries + charger http://www.gearbest.com/rc-quadcopter-parts/pp_331372.html)

Thank you very much for your input. I think I just discovered my new hobby and I can't wait to have my spare batteries!

答:
Looking at the links you gave, the 350mAh is slightly larger dimensions than the 250mAh and seems to have the same connector.

As long as it fits and has the same connector then you can put in anything you want. So long as it is still a Li-Po.

The "energic"ness of the quad will depend on the C rating of the battery, which is the current rating. A higher C rating battery will react faster to changes in load and hence provide a more snappy response. Although I doubt it will make much difference with such a small quad.

You should be able to charge them with the same charger, it will just take slightly longer. Since battery chargers work on the voltage and current draw of the battery while charging they will work irrespective of the capacity of the battery.

So just make sure it fits the battery compartment and it should be fine.


问:
How do I interface with a drone?
I recently bought a drone(quadcopter). I like how the drone works but now I would like to create an application that will allow me to control the drone remotely from my PC or Phone.

How does a computer or phone interface to an aerial vehicle?

Some of my initial thoughts were

Get a RF detector and detect what signals are being sent to the drone and replicate it using a new transmitter.

Replace the control circuit in the drone with an Arduino and send corresponding signals to it to fly the drone

Both these methods seem to be kind of far fetched and hard, but I'm not sure how this could otherwise be done.

答:
With a conventional RC vehicle, I would probably suggest getting your own receiver-transmitter set (something like XBEE transceivers or similar) and completely gutting the original system, but with a small quadcopter like you have, I think trying to replace the receiver would be very difficult to do while maintaining the aerodynamics of the vehicle.

You could try to monitor the control signals and re-create them, but a third option you might consider would be to spoof the input devices on the transmitter. That is, as I see it, you could:

Replace the transmitter-receiver pair (not recommended due to receiver form factor).

Spoof the transmitter output (requires purchasing another transmitter-receiver pair of the same frequency with which to line monitor and spoof).

Spoof the transmitter input.

The last option is what I would recommend. Get a multimeter and check the joystick outputs. I would imagine they're just potentiometers; I would expect a battery V+, a battery ground, and an analog voltage output. Then all you would need to do is to duplicate that functionality. Best case scenario would be that analog voltage falls within the same band as an Arduino analog output, in which case you could just wire ground to Arduino ground and the analog output direct to the joystick analog output line (after you remove the joystick).

Failing that, you could buy digital potentiometers and duplicate the functionality. Depending on how clever you wanted to be (and how much space is available) you could install a USB port and a switch to jump between joystick and Arduino control and have a modified computer-capable transmitter that can also be controller by people.

Title: How do I interface with a drone?


问:
Malfuction in motors using L298N
I have purchased the undermentioned robot chassis with DC motors supported with plastic gears from a local store. There is a 3A battery holder and when I connect robot to that and put it on the ground both the motors working fine and smooth. But when I connect the L298N motor controller which was made by myself and tested with proteus, only one motor is working. When I switch the wires of both motors, still the motor which worked before is working and the other motor never runs unless I manually give a little rotation to the wheel.

I use a PIC18F4431 to control the motor controller and tried USB power and a 5V regulator created by myself and the result is the same in both occasions. What could be the issue here? If I tried oiling the malfunctioning motor will it work? I heard one of my friends having the same issue, one motor is not working. But both motors work with just two pen torch batteries and can't think of a valid reason that the motor is faulty. May be my motor controller? But when I swap the wires from faulty to the working one, the working one still works as I've mentioned above.

答:
Sounds like a load balancing problem in combination with a current issue. Do you have another L298N to try? Make sure the IC isn't defective, or have you checked the current draw at each motor.
Do these motors have a capacitor installed across the leads on each motor? I doubt you hit the torque limit of the motors, as it appears that they came in a kit and were probably selected for the kit by the manufacturer because of their specs. It seems like You're not getting enough current to get past starting torque, a capacitor on each motor will help with the initial current draw.

How are you powering the USB? Is it from a laptop? Most USB ports on laptops put out pretty low current to save on battery. Try a powered USB hub.

Title: Malfuction in motors using L298N


问:
Need help regarding development of Extended Kalman Filter for sensor-data fusion of odometry and IMU data
I'm trying to develop an Extended Kalman Filter (EKF) for the positioning of a wheeled vehicle. I have a 'Baron' robot frame with 4 static wheels, all driven by a motor. On the 2 rear wheels I have an encoder. I want to fuse this odometry data with data from an 'MPU9150' 9 DOF IMU.

This is my mathlab code for the what I call 'medium-size' EKF. This uses data from encoders, accelerometer in x and y axis and gyroscope z-axis.

Medium-size EKF

Inputs:   x: "a priori" state estimate vector (8x1)

           t: sampling time [s]

           P: "a priori" estimated state covariance vector (8x8)

           z: current measurement vector (5x1) (encoder left; encoder right; x-acceleration, y-acceleration, z-axis gyroscope)

  Output:   x: "a posteriori" state estimate vector (8x1)

           P: "a posteriori" state covariance vector (8x8)

State vector x: a 8x1 vector $\begin{bmatrix} x \rightarrow X-Position In Global Frame \\ \dot x \rightarrow Speed In X-direction Global Frame \\ \ddot x \rightarrow Acceleration In X-direction Global Frame \\ y \rightarrow Y-Position In Global Frame \\ \dot y \rightarrow Speed In Y-direction Global Frame \\ \ddot y \rightarrow Acceleration In Y-direction Global Frame \\ \theta \rightarrow Vehicle Angle In Global Frame \\ \dot \theta \rightarrow Angular Speed Of The Vehicle \end {bmatrix}$

Measurement vector z:

  a 5x1 vector $\begin{bmatrix} \eta_{left} \rightarrow Wheelspeed Pulses On Left Wheel \\ \eta_{right} \rightarrow Wheelspeed Pulses On Right Wheel \\ \dot \theta_z \rightarrow GyroscopeMeasurementInZ-axisVehicleFrame \\ a_x \rightarrow AccelerometerMeasurementX-axisVehicleFrame \\ a_y \rightarrow AccelerometerMeasurementY-axisVehicleFrame \end {bmatrix}$

```<code>function [x,P] = moodieEKFmedium(x,t,P,z,sigma_ax,sigma_ay,sigma_atau,sigma_odo,sigma_acc,sigma_gyro)

% Check if input matrixes are of correct size
[rows columns] = size(x);
if (rows ~= 8 && columns ~= 1)
    error('Input vector size incorrect')
end
[rows columns] = size(z);
if (rows ~= 5 && columns ~= 1)
    error('Input data vector size incorrect')
end

% Constants
n0 = 16;
r = 30;
b = 50;

Q = zeros(8,6);
Q(3,3) = sigma_ax;
Q(6,6) = sigma_ay;
Q(8,8) = sigma_atau;
%[Q(1,8),Q(3,6),Q(6,3)] = deal(small);

dfdx = eye(8);
[dfdx(1,2),dfdx(2,3),dfdx(4,5),dfdx(5,6),dfdx(7,8)] = deal(t);
[dfdx(1,3),dfdx(4,6)] = deal((t^2)/2);

dfda = zeros(6,6);
[dfda(3,3),dfda(6,6),dfda(8,8)] = deal(1);

dhdn = eye(5,5);

R = zeros(5,5);
[R(1,1),R(2,2)] = deal(sigma_odo);
R(3,3) = sigma_gyro;
[R(4,4),R(5,5)] = deal(sigma_acc);
%[R(2,1),R(1,2)] = deal(small);

% Predict next state
% xk = f(xk-1)
xtemp = zeros(8,1);
xtemp(1) = x(1) + t*x(2)+((t^2)/2)*x(3);
xtemp(2) = x(2) + t*x(3);
u1 = normrnd(0,sigma_ax);
xtemp(3) = x(3) + u1;

xtemp(4) = x(4) + t*x(5)+((t^2)/2)*x(6);
xtemp(5) = x(5) + t*x(6);
u2 = normrnd(0,sigma_ay);
xtemp(6) = x(6) + u2;

xtemp(7) = x(7) + t*x(8);
u3 = normrnd(0,sigma_atau);
xtemp(8) = x(8) + u3;

x = xtemp

% Predict next state covariance
% Pk = dfdx * Pk-1 * transpose(dfdx) + dfda * Q * transpose(dfda)
P = dfdx * P * transpose(dfdx) + dfda * Q * transpose(dfda);

% Calculate Kalman gain
% Kk = P * transpose(dhdx) [dhdx * P + dhdn * R * transpose(dhdn)]^-1
dhdx = zeros(5,8);
if(x(2) == 0 && x(5) == 0)
    [dhdx(1,2),dhdx(2,2)] = deal(0);
    [dhdx(1,4),dhdx(2,4)] = deal(0);
else
    [dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(5)^2)));
    [dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(5)/sqrt(x(2)^2+x(5)^2)));
end
%[dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(5)^2)));
%[dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(5)/sqrt(x(2)^2+x(5)^2)));
dhdx(1,6) = (t*n0*b)/(2*pi*r);
dhdx(2,6) = -(t*n0*b)/(2*pi*r);

dhdx(4,3) = sin(x(7));
dhdx(4,6) = -cos(x(7));
dhdx(4,7) = (x(3)*cos(x(7)))+(x(6)*sin(x(7)));

dhdx(5,3) = cos(x(7));
dhdx(5,6) = sin(x(7));
dhdx(5,7) = (-x(3)*sin(x(7)))+(x(6)*cos(x(7)));

Kk = P * transpose(dhdx) * (dhdx * P * transpose(dhdx) + dhdn * R * transpose(dhdn))^(-1)

% Update state
H = zeros(5,1);
n1 = normrnd(0,sigma_odo);
H(1) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))+(((t*n0*b)/(2*pi*r))*x(6)) + n1;
n2 = normrnd(0,sigma_odo);
H(2) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))-(((t*n0*b)/(2*pi*r))*x(6)) + n2;
n3 = normrnd(0,sigma_gyro);
H(3)= x(8) + n3;
n4 = normrnd(0,sigma_acc);
H(4)=(x(3)*sin(x(7))-(x(6)*cos(x(7))))+n4;
n5 = normrnd(0,sigma_acc);
H(5)=(x(3)*cos(x(7))+(x(6)*sin(x(7))))+n5;

x = x + Kk*(z-H)

% Update state covariance
P = (eye(8)-Kk*dhdx)*P;
</code>```

end

This is the filter in schematic :

These are the state transition equations I use :
$$\ x_{t+1} = x_{t} + T \cdot \dot x_{t} + \frac{T^{2}}{2} \cdot \ddot x_{t}$$
$$\ \dot x_{t+1} = \dot x_{t} + T \cdot \ddot x_{t} $$
$$\ \ddot x_{t+1} = \ddot x_{t} + u_{1} $$
$$\ y_{t+1} = y_{t} + T \cdot \dot y_{t} + \frac{T^{2}}{2} \cdot \ddot y_{t}$$
$$\ \dot y_{t+1} = \dot y_{t} + T \cdot \ddot y_{t} $$
$$\ \ddot y_{t+1} = \ddot y_{t} + u_{2} $$
$$\ \dot \theta_{t+1} = \dot \theta_{t} + T \cdot \ddot \theta_{t} $$
$$\ \ddot \theta_{t+1} = \ddot \theta_{t} + u_{3} $$

These are the observation equations I use :

$$\ \eta_{left} = \frac{T \cdot n_{0}}{2 \cdot \pi \cdot r} \cdot \sqrt{\dot x^{2} + \dot y^{2}} + \frac{T \cdot n_{0} \cdot b}{2 \cdot \pi \cdot r} \cdot \dot \theta + n_{1}$$
$$\ \eta_{right} = \frac{T \cdot n_{0}}{2 \cdot \pi \cdot r} \cdot \sqrt{\dot x^{2} + \dot y^{2}} - \frac{T \cdot n_{0} \cdot b}{2 \cdot \pi \cdot r} \cdot \dot \theta + n_{2}$$
$$\ \dot \theta_{z} = \dot \theta + n_{3}$$
$$\ a_{x} = \ddot x \sin \theta - \ddot y \cos \theta + n_{4}$$
$$\ a_{y} = \ddot x \cos \theta + \ddot y \sin \theta + n_{5}$$

Small-size EKF

I wanted to test my filter, therefore I started with a smaller one, in which I only give the odometry measurements as input. This because I know that if I always receive the same amount of pulses on the left and right encoder, than my vehicle should be driving a straight line.

Inputs:   x: "a priori" state estimate vector (6x1)

           t: sampling time [s]

           P: "a priori" estimated state covariance vector (6x6)

           z: current measurement vector (2x1) (encoder left; encoder right)

  Output:   x: "a posteriori" state estimate vector (6x1)

           P: "a posteriori" state covariance vector (6x6)

State vector x: a 6x1 vector $\begin{bmatrix} x \rightarrow X-Position In Global Frame \\ \dot x \rightarrow Speed In X-direction Global Frame  \\ y \rightarrow Y-Position In Global Frame \\ \dot y \rightarrow Speed In Y-direction Global Frame \\  \theta \rightarrow Vehicle Angle In Global Frame \\ \dot \theta \rightarrow Angular Speed Of The Vehicle \end {bmatrix}$

Measurement vector z:
  a 2x1 vector $\begin{bmatrix} \eta_{left} \rightarrow Wheelspeed Pulses On Left Wheel \\ \eta_{right} \rightarrow Wheelspeed Pulses On Right Wheel \end {bmatrix}$

```<code>% Check if input matrixes are of correct size
[rows columns] = size(x);
if (rows ~= 6 && columns ~= 1)
    error('Input vector size incorrect')
end
[rows columns] = size(z);
if (rows ~= 2 && columns ~= 1)
    error('Input data vector size incorrect')
end

% Constants
n0 = 16;
r = 30;
b = 50;

Q = zeros(6,6);
Q(2,2) = sigma_ax;
Q(4,4) = sigma_ay;
Q(6,6) = sigma_atau;
%[Q(1,8),Q(3,6),Q(6,3)] = deal(small);

dfdx = eye(6);
[dfdx(1,2),dfdx(3,4),dfdx(5,6)] = deal(t);

dfda = zeros(6,6);
[dfda(2,2),dfda(4,4),dfda(6,6)] = deal(1);

dhdn = eye(2,2);

R = zeros(2,2);
[R(1,1),R(2,2)] = deal(sigma_odo);
%[R(2,1),R(1,2)] = deal(small);

% Predict next state
% xk = f(xk-1)
xtemp = zeros(6,1);
xtemp(1) = x(1) + t*x(2);
u1 = normrnd(0,sigma_ax);
xtemp(2) = x(2) + u1;
xtemp(3) = x(3) + t*x(4);
u2 = normrnd(0,sigma_ay);
xtemp(4) = x(4) + u2;
xtemp(5) = x(5) + t*x(6);
u3 = normrnd(0,sigma_atau);
xtemp(6) = x(6) + u3;

x = xtemp

% Predict next state covariance
% Pk = dfdx * Pk-1 * transpose(dfdx) + dfda * Q * transpose(dfda)
P = dfdx * P * transpose(dfdx) + dfda * Q * transpose(dfda);

% Calculate Kalman gain
% Kk = P * transpose(dhdx) [dhdx * P * transpose(dhdx) + dhdn * R * transpose(dhdn)]^-1
dhdx = zeros(2,6);
if((x(2) < 10^(-6)) && (x(4)< 10^(-6)))
    [dhdx(1,2),dhdx(2,2)] = deal((t*n0)/(2*pi*r));
    [dhdx(1,4),dhdx(2,4)] = deal((t*n0)/(2*pi*r));
else
    [dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(4)^2)));
    [dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(4)/sqrt(x(2)^2+x(4)^2)));
end
%[dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(4)^2)));
%[dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(4)/sqrt(x(2)^2+x(4)^2)));
dhdx(1,6) = (t*n0*b)/(2*pi*r);
dhdx(2,6) = -(t*n0*b)/(2*pi*r);

Kk = P * transpose(dhdx) * ((dhdx * P * transpose(dhdx) + dhdn * R * transpose(dhdn))^(-1))

% Update state
H = zeros(2,1);
n1 = normrnd(0,sigma_odo);
H(1) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))+(((t*n0*b)/(2*pi*r))*x(6)) + n1;
n2 = normrnd(0,sigma_odo);
H(2) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))-(((t*n0*b)/(2*pi*r))*x(6)) + n2;

x = x + Kk*(z-H)

% Update state covariance
P = (eye(6)-Kk*dhdx)*P;
</code>```

end

Odometry observation equations

If you would wonder how I come to the observation equations for the odometry data:

$\ V_{vl} = V{c} + \dot \theta \cdot b \rightarrow V_{vl} = \sqrt{ \dot x^{2} + \dot y^{2}} + \dot \theta \cdot b$

Problem

If I try the small-size EKF, using a Matlab user interface, it does seem to drive a straight line, but not under a heading of 0° like I would expect. Eventhough I start with a state vector of $\ x= \begin{bmatrix}0\\0\\0\\0\\0\\0\end{bmatrix}$ meaning starting at position [0,0] in the global coordinate frame, with speed and acceleration of zero and under an angle of 0°.

In the top right corner you can see the measurement data which I give as input, which is 5 wheelspeed counts on every wheel, every sampling period. (Simulating straight driving vehicle)
In the top left corner you see a plot of the X and Y coordinate (from state vector) at the end of one predict+update cycle of the filter, labeled with the timecycle.
Bottom left corner is a plot of the angle in the state vector. You see that after 12 cycles the angle is still almost 0° like I would expect.

Could anyone please provide some insights in to what could be wrong here?

Solutions I've been thinking on

I could use the 'odometry motion model' like explained in this question. The difference is that the odometry data is inserted in the predict step of the filter. But if I would do this, I see 2 problems: 1) I don't see how to make a small-size version of this for testing purposes, because I don't know which measurements to add in the update-step and 2) for the medium-size version I don't know how to make the observation equations as the state vector doesn't imply velocity and acceleration.

I could use the 'odometry motion model' and in the update step use the Euler-angle, which can be linked to $\ \theta $. This Euler-angle I can obtain from the Digital Motion Processor (DMP), implemented in the IMU. Then it is no problem that angular velocity is not in the state matrix. But than I still have a problem with the acceleration observation equations.

答:
You should first validate your filter is working before second-guessing your modelling choices. But I agree both those filters look OK (although I did not double check all the maths) and both of your suggested changes should also work.

There's a process I used to find errors in KF's, which is in no way comprehensive:

Plot the measurement residuals at every update.

They should start large and decrease asymptotically to the noise floor.

Your residuals should fall within the gates for the usual Gaussian tolerance intervals (68, 95 and 99.7)% of residuals fall within ($1\sigma$, $2\sigma$, $3\sigma$) respectively.

If this is not the case then your filter is not working properly which is usually due to errors in the actual KF implementation, or a measurement model.

If it is converging but the quality is poorer than expected, it could simply be a covariance tuning problem

Validate your measurement models and update step by disabling your predict stage

For each predict step, set $x_{k} = x_{k-1}$ but continue to grow $P$

If the residuals aren't decreasing, your measurement models are probably wrong (since your models make the angle and position observable)

Enable your Predict stage again and now validate your process model in the same way

For a comprehensive set of lecture notes, see ACFR's KF Tutorial, specifically after slide 121.


答:
Assessing damage from a picture isn't going to tell you anything concrete.  If you're already considering discarding this battery, you might as well test its electrical properties (and physical properties) against an intact replacement.

At a minimum, you should check whether the damaged battery has more internal resistance (i.e. can't supply as much current), runs hotter, has more self-discharge, and/or has less capacity than an intact battery.

The safest thing to do is discard it.

Title: Battery damaged?

Body: Could you please see the attached battery images and tell me if it is safe to continue using this battery or should I discard it?


问:
What does Simultaneous Localization And Mapping (SLAM) software do?
I took a course to have a better understanding of drones and their design. At the end of the course there was a test question that I got wrong and I would like to understand why.

I was supposed to select the choices that best describe SLAM.

and the possible answers were:

Estimates the location of features in the environment?

Controls the robot's flight through the environment?

Causes the robot to avoid obstacles in the environment?

Navigate in a cluttered environment?

Estimates the position and orientation of the robot with respect to
    the environment?

At first I knew that at least 3 and 4 were right because I watched a drone doing these things. I also thought that the last answer was linked to these two so I said yes to it too. Finally, I thought that the only thing that was still controlled by the user would be the flight...

Yet I failed again... Therefore what does Simultaneous Localization And Mapping (SLAM) software do?

答:
Sir
The answer to this question is 1&5,as SLAM is used to map the location based on the landmarks.So to do this it has to estimate the location of the features and also every robot must estimate its own position so the it takes the help of SLAM to estimate its position and orientation.

for more information I'm sharing the site from which I read about slam

https://www.gislounge.com/robotic-mapping-simultaneous-localization-and-mapping-slam/


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

答: Many parts of the actuation system create noise. One important noise generator is the gear train between the motor and the effector. All of those toothed wheels rattle slightly against each other, each contributing slightly to the orchestra of noise. This is one of the reasons behind the development of Maxon's Koaxdrive gear, which is targeted at low noise applications:


问:
view angle of distance sensor
I need a distance sensor (IR or Optical or any other) with 90 degree view angle to sense a rectangle surface.
in this case sensor must putting at the same level of surface area. please help me to solve this.

答:
I can think of two approaches to your problem.

Use a stereo camera pair. You can use a stereo camera pair utilizing a camera with a 90 degree field of view while setting the appropriate baseline to the range of or minimum distance from the from the two cameras which the rectangle needs to be detected. It fairly easy to implement. You can find many sources online. It can also be implemented on common SBC's such as the Raspberry Pi.
See here:Example Implementations using the Raspberry Pi I am currently able to get 6 fps for my purposes for the previous 2 fps.

Use a LIDAR sensor with the appropriate field of view. A colleague of mine working with our lab's automated vehicle utilized the LIDAR Lite sensor (lowest cost LIDAR sensor I think while also designing a sweep mechanism using a cheap motor) to get a 1-dimensional 360 degree sweep of his surroundings and characterize the objects through clustering if they are a pole, a wall or a car. I think you can do a similar approach but in 3D. See this for reference: https://www.kickstarter.com/projects/scanse/sweep-scanning-lidar/description. It also uses the Lidar Lite but the second version.

There are many more approaches to the problem like using an ultrasonic or IR sensor matrix, but I think the 2 above are the most effective and easiest approach however not necessarily the lower cost option.

I hope it helps :)

Title: view angle of distance sensor


答:
Adding to the above, my favorite way to debug a misbehaving filter is to isolate each step.

Make sure your prediction step works before correcting it. Your bot should drive straight right with 0,0,0 as a starting state and constant vel. Otherwise, your measurements are correcting your model, not sensor noise

Remove the measurement step and feed residuals by hand. A zero vector residual should produce the same state you started with  (also great for verifying your state transitions, which may be wrong as above). If a zero residual produces a non zero change, your K matrix is usually wrong, and usually because your state transition Jacobian is wonky.

If prediction and residuals make sense, then check your measurements separately as well. Feed states and Odom measurements into it to make sure the resulting residuals match what you expect. E.g. 1,1 goes straight, 2,1 right curve, 1,2 left,  1,-1 adjusts only heading, etc.

If all that works, and your result still doesn't make sense when you put it together or display it, then the plumbing or display is the problem. I've wasted hours redefining all the equations because of a display bug.

In your case I'm not clear about the odometery equations. You seem to be controlling vel in x,y,theta, but use odometery to update that velocity. Is it not the case that you are actually issuing wheel velocity commands?

Title: Need help regarding development of Extended Kalman Filter for sensor-data fusion of odometry and IMU data

Body:
I'm trying to develop an Extended Kalman Filter (EKF) for the positioning of a wheeled vehicle. I have a 'Baron' robot frame with 4 static wheels, all driven by a motor. On the 2 rear wheels I have an encoder. I want to fuse this odometry data with data from an 'MPU9150' 9 DOF IMU.

This is my mathlab code for the what I call 'medium-size' EKF. This uses data from encoders, accelerometer in x and y axis and gyroscope z-axis.

Medium-size EKF

Inputs:   x: "a priori" state estimate vector (8x1)

           t: sampling time [s]

           P: "a priori" estimated state covariance vector (8x8)

           z: current measurement vector (5x1) (encoder left; encoder right; x-acceleration, y-acceleration, z-axis gyroscope)

  Output:   x: "a posteriori" state estimate vector (8x1)

           P: "a posteriori" state covariance vector (8x8)

State vector x: a 8x1 vector $\begin{bmatrix} x \rightarrow X-Position In Global Frame \\ \dot x \rightarrow Speed In X-direction Global Frame \\ \ddot x \rightarrow Acceleration In X-direction Global Frame \\ y \rightarrow Y-Position In Global Frame \\ \dot y \rightarrow Speed In Y-direction Global Frame \\ \ddot y \rightarrow Acceleration In Y-direction Global Frame \\ \theta \rightarrow Vehicle Angle In Global Frame \\ \dot \theta \rightarrow Angular Speed Of The Vehicle \end {bmatrix}$

Measurement vector z:

  a 5x1 vector $\begin{bmatrix} \eta_{left} \rightarrow Wheelspeed Pulses On Left Wheel \\ \eta_{right} \rightarrow Wheelspeed Pulses On Right Wheel \\ \dot \theta_z \rightarrow GyroscopeMeasurementInZ-axisVehicleFrame \\ a_x \rightarrow AccelerometerMeasurementX-axisVehicleFrame \\ a_y \rightarrow AccelerometerMeasurementY-axisVehicleFrame \end {bmatrix}$

```<code>function [x,P] = moodieEKFmedium(x,t,P,z,sigma_ax,sigma_ay,sigma_atau,sigma_odo,sigma_acc,sigma_gyro)

% Check if input matrixes are of correct size
[rows columns] = size(x);
if (rows ~= 8 && columns ~= 1)
    error('Input vector size incorrect')
end
[rows columns] = size(z);
if (rows ~= 5 && columns ~= 1)
    error('Input data vector size incorrect')
end

% Constants
n0 = 16;
r = 30;
b = 50;

Q = zeros(8,6);
Q(3,3) = sigma_ax;
Q(6,6) = sigma_ay;
Q(8,8) = sigma_atau;
%[Q(1,8),Q(3,6),Q(6,3)] = deal(small);

dfdx = eye(8);
[dfdx(1,2),dfdx(2,3),dfdx(4,5),dfdx(5,6),dfdx(7,8)] = deal(t);
[dfdx(1,3),dfdx(4,6)] = deal((t^2)/2);

dfda = zeros(6,6);
[dfda(3,3),dfda(6,6),dfda(8,8)] = deal(1);

dhdn = eye(5,5);

R = zeros(5,5);
[R(1,1),R(2,2)] = deal(sigma_odo);
R(3,3) = sigma_gyro;
[R(4,4),R(5,5)] = deal(sigma_acc);
%[R(2,1),R(1,2)] = deal(small);

% Predict next state
% xk = f(xk-1)
xtemp = zeros(8,1);
xtemp(1) = x(1) + t*x(2)+((t^2)/2)*x(3);
xtemp(2) = x(2) + t*x(3);
u1 = normrnd(0,sigma_ax);
xtemp(3) = x(3) + u1;

xtemp(4) = x(4) + t*x(5)+((t^2)/2)*x(6);
xtemp(5) = x(5) + t*x(6);
u2 = normrnd(0,sigma_ay);
xtemp(6) = x(6) + u2;

xtemp(7) = x(7) + t*x(8);
u3 = normrnd(0,sigma_atau);
xtemp(8) = x(8) + u3;

x = xtemp

% Predict next state covariance
% Pk = dfdx * Pk-1 * transpose(dfdx) + dfda * Q * transpose(dfda)
P = dfdx * P * transpose(dfdx) + dfda * Q * transpose(dfda);

% Calculate Kalman gain
% Kk = P * transpose(dhdx) [dhdx * P + dhdn * R * transpose(dhdn)]^-1
dhdx = zeros(5,8);
if(x(2) == 0 && x(5) == 0)
    [dhdx(1,2),dhdx(2,2)] = deal(0);
    [dhdx(1,4),dhdx(2,4)] = deal(0);
else
    [dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(5)^2)));
    [dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(5)/sqrt(x(2)^2+x(5)^2)));
end
%[dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(5)^2)));
%[dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(5)/sqrt(x(2)^2+x(5)^2)));
dhdx(1,6) = (t*n0*b)/(2*pi*r);
dhdx(2,6) = -(t*n0*b)/(2*pi*r);

dhdx(4,3) = sin(x(7));
dhdx(4,6) = -cos(x(7));
dhdx(4,7) = (x(3)*cos(x(7)))+(x(6)*sin(x(7)));

dhdx(5,3) = cos(x(7));
dhdx(5,6) = sin(x(7));
dhdx(5,7) = (-x(3)*sin(x(7)))+(x(6)*cos(x(7)));

Kk = P * transpose(dhdx) * (dhdx * P * transpose(dhdx) + dhdn * R * transpose(dhdn))^(-1)

% Update state
H = zeros(5,1);
n1 = normrnd(0,sigma_odo);
H(1) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))+(((t*n0*b)/(2*pi*r))*x(6)) + n1;
n2 = normrnd(0,sigma_odo);
H(2) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))-(((t*n0*b)/(2*pi*r))*x(6)) + n2;
n3 = normrnd(0,sigma_gyro);
H(3)= x(8) + n3;
n4 = normrnd(0,sigma_acc);
H(4)=(x(3)*sin(x(7))-(x(6)*cos(x(7))))+n4;
n5 = normrnd(0,sigma_acc);
H(5)=(x(3)*cos(x(7))+(x(6)*sin(x(7))))+n5;

x = x + Kk*(z-H)

% Update state covariance
P = (eye(8)-Kk*dhdx)*P;
</code>```

end

This is the filter in schematic :

These are the state transition equations I use :
$$\ x_{t+1} = x_{t} + T \cdot \dot x_{t} + \frac{T^{2}}{2} \cdot \ddot x_{t}$$
$$\ \dot x_{t+1} = \dot x_{t} + T \cdot \ddot x_{t} $$
$$\ \ddot x_{t+1} = \ddot x_{t} + u_{1} $$
$$\ y_{t+1} = y_{t} + T \cdot \dot y_{t} + \frac{T^{2}}{2} \cdot \ddot y_{t}$$
$$\ \dot y_{t+1} = \dot y_{t} + T \cdot \ddot y_{t} $$
$$\ \ddot y_{t+1} = \ddot y_{t} + u_{2} $$
$$\ \dot \theta_{t+1} = \dot \theta_{t} + T \cdot \ddot \theta_{t} $$
$$\ \ddot \theta_{t+1} = \ddot \theta_{t} + u_{3} $$

These are the observation equations I use :

$$\ \eta_{left} = \frac{T \cdot n_{0}}{2 \cdot \pi \cdot r} \cdot \sqrt{\dot x^{2} + \dot y^{2}} + \frac{T \cdot n_{0} \cdot b}{2 \cdot \pi \cdot r} \cdot \dot \theta + n_{1}$$
$$\ \eta_{right} = \frac{T \cdot n_{0}}{2 \cdot \pi \cdot r} \cdot \sqrt{\dot x^{2} + \dot y^{2}} - \frac{T \cdot n_{0} \cdot b}{2 \cdot \pi \cdot r} \cdot \dot \theta + n_{2}$$
$$\ \dot \theta_{z} = \dot \theta + n_{3}$$
$$\ a_{x} = \ddot x \sin \theta - \ddot y \cos \theta + n_{4}$$
$$\ a_{y} = \ddot x \cos \theta + \ddot y \sin \theta + n_{5}$$

Small-size EKF

I wanted to test my filter, therefore I started with a smaller one, in which I only give the odometry measurements as input. This because I know that if I always receive the same amount of pulses on the left and right encoder, than my vehicle should be driving a straight line.

Inputs:   x: "a priori" state estimate vector (6x1)

           t: sampling time [s]

           P: "a priori" estimated state covariance vector (6x6)

           z: current measurement vector (2x1) (encoder left; encoder right)

  Output:   x: "a posteriori" state estimate vector (6x1)

           P: "a posteriori" state covariance vector (6x6)

State vector x: a 6x1 vector $\begin{bmatrix} x \rightarrow X-Position In Global Frame \\ \dot x \rightarrow Speed In X-direction Global Frame  \\ y \rightarrow Y-Position In Global Frame \\ \dot y \rightarrow Speed In Y-direction Global Frame \\  \theta \rightarrow Vehicle Angle In Global Frame \\ \dot \theta \rightarrow Angular Speed Of The Vehicle \end {bmatrix}$

Measurement vector z:
  a 2x1 vector $\begin{bmatrix} \eta_{left} \rightarrow Wheelspeed Pulses On Left Wheel \\ \eta_{right} \rightarrow Wheelspeed Pulses On Right Wheel \end {bmatrix}$

```<code>% Check if input matrixes are of correct size
[rows columns] = size(x);
if (rows ~= 6 && columns ~= 1)
    error('Input vector size incorrect')
end
[rows columns] = size(z);
if (rows ~= 2 && columns ~= 1)
    error('Input data vector size incorrect')
end

% Constants
n0 = 16;
r = 30;
b = 50;

Q = zeros(6,6);
Q(2,2) = sigma_ax;
Q(4,4) = sigma_ay;
Q(6,6) = sigma_atau;
%[Q(1,8),Q(3,6),Q(6,3)] = deal(small);

dfdx = eye(6);
[dfdx(1,2),dfdx(3,4),dfdx(5,6)] = deal(t);

dfda = zeros(6,6);
[dfda(2,2),dfda(4,4),dfda(6,6)] = deal(1);

dhdn = eye(2,2);

R = zeros(2,2);
[R(1,1),R(2,2)] = deal(sigma_odo);
%[R(2,1),R(1,2)] = deal(small);

% Predict next state
% xk = f(xk-1)
xtemp = zeros(6,1);
xtemp(1) = x(1) + t*x(2);
u1 = normrnd(0,sigma_ax);
xtemp(2) = x(2) + u1;
xtemp(3) = x(3) + t*x(4);
u2 = normrnd(0,sigma_ay);
xtemp(4) = x(4) + u2;
xtemp(5) = x(5) + t*x(6);
u3 = normrnd(0,sigma_atau);
xtemp(6) = x(6) + u3;

x = xtemp

% Predict next state covariance
% Pk = dfdx * Pk-1 * transpose(dfdx) + dfda * Q * transpose(dfda)
P = dfdx * P * transpose(dfdx) + dfda * Q * transpose(dfda);

% Calculate Kalman gain
% Kk = P * transpose(dhdx) [dhdx * P * transpose(dhdx) + dhdn * R * transpose(dhdn)]^-1
dhdx = zeros(2,6);
if((x(2) < 10^(-6)) && (x(4)< 10^(-6)))
    [dhdx(1,2),dhdx(2,2)] = deal((t*n0)/(2*pi*r));
    [dhdx(1,4),dhdx(2,4)] = deal((t*n0)/(2*pi*r));
else
    [dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(4)^2)));
    [dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(4)/sqrt(x(2)^2+x(4)^2)));
end
%[dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(4)^2)));
%[dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(4)/sqrt(x(2)^2+x(4)^2)));
dhdx(1,6) = (t*n0*b)/(2*pi*r);
dhdx(2,6) = -(t*n0*b)/(2*pi*r);

Kk = P * transpose(dhdx) * ((dhdx * P * transpose(dhdx) + dhdn * R * transpose(dhdn))^(-1))

% Update state
H = zeros(2,1);
n1 = normrnd(0,sigma_odo);
H(1) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))+(((t*n0*b)/(2*pi*r))*x(6)) + n1;
n2 = normrnd(0,sigma_odo);
H(2) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))-(((t*n0*b)/(2*pi*r))*x(6)) + n2;

x = x + Kk*(z-H)

% Update state covariance
P = (eye(6)-Kk*dhdx)*P;
</code>```

end

Odometry observation equations

If you would wonder how I come to the observation equations for the odometry data:

$\ V_{vl} = V{c} + \dot \theta \cdot b \rightarrow V_{vl} = \sqrt{ \dot x^{2} + \dot y^{2}} + \dot \theta \cdot b$

Problem

If I try the small-size EKF, using a Matlab user interface, it does seem to drive a straight line, but not under a heading of 0° like I would expect. Eventhough I start with a state vector of $\ x= \begin{bmatrix}0\\0\\0\\0\\0\\0\end{bmatrix}$ meaning starting at position [0,0] in the global coordinate frame, with speed and acceleration of zero and under an angle of 0°.

In the top right corner you can see the measurement data which I give as input, which is 5 wheelspeed counts on every wheel, every sampling period. (Simulating straight driving vehicle)
In the top left corner you see a plot of the X and Y coordinate (from state vector) at the end of one predict+update cycle of the filter, labeled with the timecycle.
Bottom left corner is a plot of the angle in the state vector. You see that after 12 cycles the angle is still almost 0° like I would expect.

Could anyone please provide some insights in to what could be wrong here?

Solutions I've been thinking on

I could use the 'odometry motion model' like explained in this question. The difference is that the odometry data is inserted in the predict step of the filter. But if I would do this, I see 2 problems: 1) I don't see how to make a small-size version of this for testing purposes, because I don't know which measurements to add in the update-step and 2) for the medium-size version I don't know how to make the observation equations as the state vector doesn't imply velocity and acceleration.

I could use the 'odometry motion model' and in the update step use the Euler-angle, which can be linked to $\ \theta $. This Euler-angle I can obtain from the Digital Motion Processor (DMP), implemented in the IMU. Then it is no problem that angular velocity is not in the state matrix. But than I still have a problem with the acceleration observation equations.


问:
Addressing the sample impoverishment in particle filter
I have implemented a particle filter algorithm for the state estimation of a mobile robot.

There are several external range sensors(transmitters) in the environment which gives information on the distance (radius) of the robot based on the time taken for the receiver on the robot to send back its acknowledgement. So, using three or more such transmitters it will be possible to triangulate the position of the robot.

The particle filter is initialized with 15000 particles and the sensor noise is relatively low (0.02m).

Update Phase: At each iteration a range information from an external sensor is received. This assigns higher weights to the particles along the radius of the external sensor. Not all the particles are equally weighted since the process noise is low. Hence in most of the cases, the particle relatively closer to the robot gets lower weight than an incorrect one that happens to be along the radius. The weight is a pdf.

Resampling Phase: At this stage, the lower weighted particle(the correct one) that has negligible weight gets lost because the higher weighted particle gets picked up.

All this happens at the first iteration and so when the range information from another sensor arrives, the robot is already kidnapped.

Googling around, said that this problem is called as sample impoverishment and the most common approach is to resample only when the particle variance is low. (Effective Sample Size < number of particles / 2)

But, when the particles are assigned negligible weights and there are relatively very few particles with higher weights, the diversity of the particles are lost at resampling phase. So, when the variance is higher resampling is done which removes the lower weighted particle and hence the diversity of the particles is lost. Isnt this completely the opposite of the above idea of ESS?

Is my understanding of sample impoverishment correct? Is there a way this issue can be fixed?

Any pointers or help would be highly appreciated.

答:
I'm not familiar with particle filtering, but if applying the weights between each sensor read is causing issues, why not accumulate weights to be applied between sensor calls and apply them in bulk after all sensors have been read?

For example, it sounds like you're doing:

[Read 1]-< apply weights >-[Read 2]-< apply weights >-[Read 3]-< apply weights >

What I'm suggesting:

[R1]-< accumulate weights >-[R2]-< accumulate >-[R3]-< accumulate >-< apply all weights >

I would imagine, in the scenario above, the particles that got the biggest accumulated weight would be the ones that are "seen" by all of the sensors. Those would be the ones you would want to keep, right?


问:
Building a balancing robot with differential drive
I've already built a two wheeled balancing robot using some continuous rotation servos and an accelerometer/gyroscope.  I upgraded the servos to some geared DC motors with 8-bit encoders with the goal having the robot drive around while balancing.

I'm kind of stuck on how to program it to drive around while still balancing.  I think one way would be to just have the control input to the motors act sort of like pushing it.  So the robot would be momentarily unbalanced in the direction I want it to travel.  That seems kind of clumsy to me though.  There must be a better way of doing?  I think I need to combine the dynamic model for the balancer with the differential drive but this is a bit beyond the control theory that I know.

Update
From Anorton's answer I have a good looking state matrix now.

Now about pole placement:  The A matrix will will have to be 4x4 based on the new state vector.  And B will then have to be a 4x2 matrix since I can only control the left/right wheel torque (u = 2x1 vector).

I may need to read more about this but is there a systematic way to determine the A matrix by pole placement?  It seems to me for this example and even more complicated examples, determining A by guess and check would be very difficult.

Update #2
After a bit of reading I think I understand it now.  I still need the dynamics of the robot to determine the A matrix.  Once I have that I can do the pole placement using matlab or octave.

答:
Disclaimer: I have never done this myself, but only have seen a description of it being done through Georgia Tech's "Control of Mobile Robotics" on Coursera.  My knowledge of controls is spotty, too.  Thus... take this with a grain of salt. :)

To keep the robot upright (and still), you're trying to stabilize (send to $0$) the state $x$, where:
$$x=\left[\begin{array}{c}\text{leftVelocity} \\ \text{rightVelocity} \\ \text{angle from vertical}\end{array}\right]$$

However, when this system is stable, the left and right wheel velocities will be $0$.  So, we want an offset for the target speed:

$$x_{new}=\left[\begin{array}{c}\text{leftVelocity}-\delta_L \\ \text{rightVelocity}-\delta_R \\ \text{angle from vertical}\end{array}\right]$$
Where $\delta$ is the target speed for either side.

When this system is stabilized, the robot will be upright, and will have each wheel rotating at a desired speed.

This is the basic approach/outline.  I'll edit this with some more details tomorrow (and actual math, etc.), but I wanted to at least post the general idea now.  (It's late in my time zone, and I have an early class to get to.)

EDITED:
Oh goodness.  So, I just looked back at the slides relating to this in the Coursera course (Section 4, slide 29).  You may want to go and enroll in that class just to download that slide set... :)

The hard part is computing the $A$ and $B$ matrices (it's a big linearization mess).  Anyway, you want to make your state matrix as follows (not as above--my memory wasn't exactly right):

$$x=\left[\begin{array}{c}v \\ \omega \\ \phi \\ \dot \phi\end{array}\right]$$

Where $v$ is the velocity of the segway, $\omega$ is the rotational velocity (how fast the robot is pivoting) and $\phi$ is the angle from vertical.

We want to have a desired velocity, so let's define a new state vector:
$$\begin{align}\tilde x &= x - \left[\begin{array}{c}v_d \\ \omega_d \\ 0 \\ 0 \end{array}\right]\\
&= x - \delta
\end{align}$$

Where $\delta$ is the desired velocity and rotational quantity (as seen above).

Differentiating:
$$\dot{\tilde x} = \dot x - \dot \delta \underset{\text{$\delta$ is constant}}{=} \dot x$$

Thus, we have
$$\dot{\tilde x} = Ax + Bu$$

Where $A$ is the standard coeff matrix $B$ is the input matrix, and $u$ is the control vector.  We can then say:

$$\dot{\tilde x} = A(x - \delta) + Bu + A\delta$$
But, based on the solution for $A$ (from Coursera), $A\delta = 0$.  Thus:
$$\dot{\tilde x} = A\tilde x+ Bu$$

Now you can use pole placement to determine the correct eigenvalues, etc.