以下是个人的理解，能力不足，请指证。
TinySlam代码分析
1. 读取LDS文件
1） 函数:int read_sensor_data(cart_sensor_data_t *data)
2） test_lab.dat文件格式及字段含义
225820438 94981 -102848 -2.48414 1.54308321.912 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0 0 0 0 995 995 995 998 1005 1065 0 0 0 0 0 0 0 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 00 0 0 0 2778 2773 2773 2778 2779 2800 2815 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0 0 0 0 0 7 0 7 0 3546 3551 3551 3551 3575 3576 3592 3611 36263649 7 0 0 0 0 3728 3728 3728 0 0 0 0 0 0 0 0 0 0 0 0 3999 3999 3999 0 0 0 0 00 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 7 0 0 0 7 0 2621 2394 2394 2394 24432466 2543 2558 2582 2582 2600 2641 2652 2652 2652 2646 2561 2561 2544 2544 25132434 2434 2431 2422 2422 2420 2403 2397 2389 2379 2378 2365 2363 2358 2356 23452337 2334 2326 2317 2312 2306 2300 2292 2292 2282 2276 2270 2255 2254 2253 22482243 2243 2238 2232 2230 2230 2230 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 6 6 6 6 6 6 0 0 0 0 0 0 0 0 7 0 0 0 00 0 6 6 0 6 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0 0 0 0 3106 3106 3106 3148 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 7 00 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 1504 1487 1484 1481 1471 1471 1471 1472 0 0 0 0 0 0 0 0 0 0 0 00 0 0 0 2024 2024 2024 2047 2060 2096 2127 2150 2150 2163 2189 2210 2228 22532280 2299 2299 2299 2294 2294 2285 2285 2285 2285 2431 2443 2443 2431 2429 24292429 0 0 0 0 0 0 0 0 0 0 0 0 0 7 0 7 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 00 0 0 0 7 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 5369 5345 5341 5341 5341 5336 53365336 5330 5330 5346 5351 5358 5402 5402 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 0 0
字段 | 含义 | 说明 |
timestamp | 时间戳 |
|
q1 | 左轮读数 | 里程计数据 |
q2 | 右轮读数 | 里程计数据 |
10个无用数据 | 未明白没什么不使用它 |
|
d[682] | 记录激光是否扫到障碍物的距离 | 每一度3个采样距离数据 |
a)每一度3个采样距离数据
angle_deg = TEST_ANGLE_MIN + ((double)(i * SPAN + j)) * (TEST_ANGLE_MAX - TEST_ANGLE_MIN) / (TEST_SCAN_SIZE * SPAN - 1);
b)如果距离大于TEST_HOLE_WIDTH / 2=300，则表示有障碍物，并进行极坐标转成直角坐标系。
if (d[i] > TEST_HOLE_WIDTH / 2) {
scan->x[scan->nb_points] = d[i] * cos(angle_rad);
scan->y[scan->nb_points] = d[i] * sin(angle_rad);
scan->value[scan->nb_points]= TS_OBSTACLE;
scan->x[scan->nb_points] += TEST_OFFSET_LASER;
scan->nb_points++;
}
2. 初始化地图：ts_map_init(&map);
每个栅格的值为：initval = (TS_OBSTACLE + TS_NO_OBSTACLE) / 2= 65500/2= 32750;
3. 初始化障碍物地图
4. 初始化机器位姿
position.x = 0.3 * TS_MAP_SIZE / TS_MAP_SCALE=6143.99;
position.y = 0.3 * TS_MAP_SIZE / TS_MAP_SCALE=6143.99;;
position.theta = 0;
5. 循环读取每一行(每一条记录)进行处理
1） 里程计的计算
m = params.r * M_PI / params.inc;
vodo = m * (nq1 - q1 + (nq2 -q2) * params.ratio);
thetarad = position.theta * M_PI / 180;
position2 = position;
position2.x += vodo * 1000 *cos(thetarad);
position2.y += vodo * 1000 *sin(thetarad);
psidotodo = (m * ((nq2 - q2) *params.ratio - nq1 + q1) / params.R) * 180 / M_PI;
position2.theta += psidotodo;
注意：里程计计算的位姿不使用！后来通过蒙特卡罗计算位姿覆盖了。
2） 蒙特卡罗推算机器位姿
a) ts_distance_scan_to_map
激光雷达坐标转到全局坐标。
x = (int)floor((pos->x + c * scan->x[i] - s * scan->y[i]) * TS_MAP_SCALE + 0.5);
y = (int)floor((pos->y + s * scan->x[i] + c * scan->y[i]) * TS_MAP_SCALE + 0.5);
以上算法与图是个相反的过程。
if (x >= 0 && x < TS_MAP_SIZE && y >= 0 && y < TS_MAP_SIZE) {
sum += map->map[y * TS_MAP_SIZE + x];
nb_points++;
}
sum = sum * 1024 / nb_points
求出当前机器的位姿下观察到的所有障碍物所落到的地图上的栅格的值的平均值，当作当前的距离。
b) 对当前位姿加高斯噪音
currentpos.x += 50 * (((double)rand()) / RAND_MAX - 0.5);
currentpos.y += 50 * (((double)rand()) / RAND_MAX - 0.5);
currentpos.theta += 50 * (((double)rand()) / RAND_MAX - 0.5);
c) 在对当前位姿加高斯噪音后，现计算当前距离
currentdist =ts_distance_scan_to_map(scan, map, ¤tpos);
d) 如果计算出的距离小于未加高斯噪音的值，则认为找到了最优解，否则进行下一步
e) 如果迭代到100次后，则选择当前与上一次中的最小值。否则从b)开始迭代。
6. 计算通过蒙特卡罗推算出的新位姿与原始位姿坐标关系来获得机器行走的速度及角速度
v = sqrt((position2.x - position.x) *(position2.x - position.x) + (position2.y - position.y) * (position2.y -position.y));
psidot = position2.theta -position.theta;
if (cnt_scans !=0) {
v *= 1000.0 / (timestamp - told);
psidot *= 1000000.0 / (timestamp - told);
} else {
v = 0;
psidot = 0;
}
7. 更新地图：ts_map_update(&sensor_data[cnt_scans].scan,&map, &position, 50, TEST_HOLE_WIDTH);
a)激光雷达坐标
x1 = (int)floor(pos->x * TS_MAP_SCALE + 0.5);
y1 = (int)floor(pos->y * TS_MAP_SCALE + 0.5);
b) 计算xp,yp,x2,y2
x2p = c * scan->x[i] - s * scan->y[i];
y2p = s * scan->x[i] + c * scan->y[i];
xp = (int)floor((pos->x + x2p) * TS_MAP_SCALE + 0.5);
yp = (int)floor((pos->y + y2p) * TS_MAP_SCALE + 0.5);
dist = sqrt(x2p * x2p + y2p * y2p);
add = hole_width / 2 / dist;
x2p *= TS_MAP_SCALE * (1 + add);
y2p *= TS_MAP_SCALE * (1 + add);
x2 = (int)floor(pos->x * TS_MAP_SCALE + x2p + 0.5);
y2 = (int)floor(pos->y * TS_MAP_SCALE + y2p + 0.5);
if (scan->value[i] == TS_NO_OBSTACLE) {
q = quality / 4;
value = TS_NO_OBSTACLE;
} else {
q = quality;
value = TS_OBSTACLE;
}
c)画出上面显示的图: ts_map_laser_ray(map, x1, y1, x2, y2, xp, yp, value, q);
8. 设置当前位姿为障碍物点
x = (int)floor(position.x * TS_MAP_SCALE + 0.5);
y = ((int)floor(position.y * TS_MAP_SCALE + 0.5))
if (x >= 0 && y >= 0 && x< TS_MAP_SIZE && y < TS_MAP_SIZE)
trajectory.map[y * TS_MAP_SIZE + x] = 0;
9. 更新障碍物点地图：draw_scan(&sensor_data[cnt_scans].scan,&trajectory, &position);
x2p = c * scan->x[i] - s * scan->y[i];
y2p = s * scan->x[i] + c * scan->y[i];
x2p *= TS_MAP_SCALE;
y2p *= TS_MAP_SCALE;
x2 = (int)floor(pos->x * TS_MAP_SCALE + x2p + 0.5);
y2 = (int)floor(pos->y * TS_MAP_SCALE + y2p + 0.5);
if (x2 >= 0 && y2 >= 0 && x2 < TS_MAP_SIZE && y2 < TS_MAP_SIZE) map->map[y2 * TS_MAP_SIZE + x2] = 0;
10. 记录当前地图:record_map(&map,&trajectory, filename, TS_MAP_SIZE, TS_MAP_SIZE);
for (xp = 0; xp < width; x++, xp++) {
if (overlay->map[ (TS_MAP_SIZE - 1 - y) * TS_MAP_SIZE + x] == 0)
fprintf(output, "0 ");
else
fprintf(output, "%d ", (int)(map->map[ (TS_MAP_SIZE - 1 - y) * TS_MAP_SIZE + x]) >> 8);
}
P2
2048 2048 255
127 127 127 127 127 127 127 127 127 127 127 127 127 127 127127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127 127127 127 127 127 127 127 127 127 127 127 127
3748
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
