课程进度 60% · 第8/12章第8/12章 · 标签 1/4
— 1 —
导航基础
机器人导航是移动机器人的核心技术，包括定位、建图、路径规划和控制。经典的导航框架是"感知-规划-控制"循环。
导航框架
- 感知：传感器数据采集和处理
- 定位：确定机器人位置
- 建图：构建环境地图
- 规划：生成运动路径
- 控制：执行运动指令
— 2 —
粒子滤波定位
python
1
import numpy as np
2
3
class ParticleFilter:
4
def __init__(self, n_particles=1000):
5
self.particles = np.random.rand(n_particles, 3) * 10
6
self.weights = np.ones(n_particles) / n_particles
7
8
def predict(self, vel, omega, dt):
9
theta = self.particles[:, 2]
10
self.particles[:, 0] += vel * np.cos(theta) * dt
11
self.particles[:, 1] += vel * np.sin(theta) * dt
12
self.particles[:, 2] += omega * dt
13
self.particles += np.random.randn(*self.particles.shape) * 0.1
14
15
def update(self, landmarks, measurements, std):
16
for i, p in enumerate(self.particles):
17
dist = np.linalg.norm(p[:2] - landmarks, axis=1)
18
self.weights[i] = np.prod(np.exp(-0.5*((dist-measurements)/std)**2))
19
self.weights /= np.sum(self.weights)