我是靠谱客的博主 彪壮唇彩,这篇文章主要介绍里程计运动模型,现在分享给大家,希望可以做个参考。

里程计模型使用相对运动信息,该信息由机器人内部里程计测量。为了提取相对距离,运动信息被转变成三个步骤的序列:旋转、直线运动(平移)和另一个旋转。 

                                     

算法流程: 

                

变量delta _{trans}^{'}, delta _{rot1}^{'}delta _{rot2}^{'}是将高斯零均值随机噪声添加到实际测距读数的结果。实际上,将一个小的附加误差相加到每个姿势分量以避免对于零度量测量增量,所有粒子的运动精确地变为零,这可能导致粒子滤波器退化。

代码:

复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
case ODOM_MODEL_DIFF: { // Implement sample_motion_odometry (Prob Rob p 136) double delta_rot1, delta_trans, delta_rot2; double delta_rot1_hat, delta_trans_hat, delta_rot2_hat; double delta_rot1_noise, delta_rot2_noise; // Avoid computing a bearing from two poses that are extremely near each // other (happens on in-place rotation). if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + ndata->delta.v[0]*ndata->delta.v[0]) < 0.01) delta_rot1 = 0.0; else delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), old_pose.v[2]); delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] + ndata->delta.v[1]*ndata->delta.v[1]); delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1); // We want to treat backward and forward motion symmetrically for the // noise model to be applied below. The standard model seems to assume // forward motion. delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), fabs(angle_diff(delta_rot1,M_PI))); delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), fabs(angle_diff(delta_rot2,M_PI))); for (int i = 0; i < set->sample_count; i++) { pf_sample_t* sample = set->samples + i; // Sample pose differences delta_rot1_hat = angle_diff(delta_rot1, pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise + this->alpha2*delta_trans*delta_trans)); delta_trans_hat = delta_trans - pf_ran_gaussian(this->alpha3*delta_trans*delta_trans + this->alpha4*delta_rot1_noise*delta_rot1_noise + this->alpha4*delta_rot2_noise*delta_rot2_noise); delta_rot2_hat = angle_diff(delta_rot2, pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise + this->alpha2*delta_trans*delta_trans)); // Apply sampled update to particle pose sample->pose.v[0] += delta_trans_hat * cos(sample->pose.v[2] + delta_rot1_hat); sample->pose.v[1] += delta_trans_hat * sin(sample->pose.v[2] + delta_rot1_hat); sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat; } }

 

最后

以上就是彪壮唇彩最近收集整理的关于里程计运动模型的全部内容,更多相关里程计运动模型内容请搜索靠谱客的其他文章。

本图文内容来源于网友提供,作为学习参考使用,或来自网络收集整理,版权属于原作者所有。
点赞(75)

评论列表共有 0 条评论

立即
投稿
返回
顶部