网站排名工具,wordpress 主题和插件下载,深圳网站制作哪里济南兴田德润简介,甘肃省交通建设项目招投标中心网站目录 0 专栏介绍1 栅格地图1.1 应用场景1.2 基本概念 2 占据栅格地图2.1 更新模型2.2 截断策略 3 仿真实现3.1 算法流程3.2 Matlab实现 0 专栏介绍
#x1f525;附C/Python/Matlab全套代码#x1f525;课程设计、毕业设计、创新竞赛必备#xff01;详细介绍全局规划(图搜索… 目录 0 专栏介绍1 栅格地图1.1 应用场景1.2 基本概念 2 占据栅格地图2.1 更新模型2.2 截断策略 3 仿真实现3.1 算法流程3.2 Matlab实现 0 专栏介绍
附C/Python/Matlab全套代码课程设计、毕业设计、创新竞赛必备详细介绍全局规划(图搜索、采样法、智能算法等)局部规划(DWA、APF等)曲线优化(贝塞尔曲线、B样条曲线等)。
详情图解自动驾驶中的运动规划(Motion Planning)附几十种规划算法 1 栅格地图
1.1 应用场景 栅格地图(grid map)是在机器人和自动化领域中广泛使用的一种地图表示方法。它将环境划分为规则的网格单元并在每个单元中存储关于该区域的信息。每个单元可以表示空闲、障碍物、未知区域或其他地图属性。
栅格地图最主要的应用是服务于机器人导航中的路径规划和避障。机器人可以利用栅格地图中的障碍物信息来规划安全的路径并避开可能的碰撞或危险区域。同时栅格地图也是SLAM算法中常用的地图表示方式之一。通过与传感器数据融合机器人能够同时进行自身位置估计和地图构建。 总之栅格地图是一种简单且直观的地图表示方法它可以提供对环境的可视化和语义信息并为机器人的感知、规划和决策提供基础。然而栅格地图也存在分辨率、存储消耗和精度等方面的限制在实际应用中需要权衡和优化。
1.2 基本概念
栅格地图的基本概念总结如下 邻域模式 栅格地图中常用的邻域模式有8邻域法、24邻域法和48邻域法如下所示 栅格示数 栅格地图中每个栅格都被赋予栅格示数其指明了该栅格在全局环境中表达的语义。例如0表示无障碍的自由栅格1表示障碍物 栅格坐标 栅格地图可视为离散直角坐标系其中可用有序二元组 ( i , j ) (i,j) (i,j)定位栅格 栅格序号 栅格按照行列顺序依次进行的编号称为栅格序号由于栅格序号是一维线性的因此可以加速信息处理与运算 栅格粒度 栅格对应物理世界的比例系数称为栅格粒度栅格粒度越小环境分辨率越大对环境的刻画越具体、丰富。但相应地存储地图所占的内存、处理地图耗费的时间越多
2 占据栅格地图
在工程上通常使用占据法构建占据栅格地图(Occupancy Grid Map)。考虑到构建栅格地图使用的激光雷达存在噪声即在相同条件下对障碍物的相对距离探测可能有误差因此引入概率定义栅格状态对于地图中的栅格 s s s P ( s 1 ) P\left( s1 \right) P(s1)表示栅格被占据的概率 P ( s 0 ) 1 − P ( s 1 ) P\left( s0 \right) 1-P\left( s1 \right) P(s0)1−P(s1)表示栅格自由的概率。实际应用时使用一种更紧凑的表达 o d d ( s ) P ( s 1 ) P ( s 0 ) \mathrm{odd}\left( s \right) \frac{P\left( s1 \right)}{P\left( s0 \right)} odd(s)P(s0)P(s1)
2.1 更新模型
构建地图的问题可形式化为已知机器人激光传感器的观测序列 更新地图中栅格的后验概率 根据贝叶斯公式和马尔科夫链可得 P ( s ∣ z 1 : t ) P ( z t ∣ s , z 1 : t − 1 ) P ( s ∣ z 1 : t − 1 ) P ( z t ∣ z 1 : t − 1 ) P ( z t ∣ s ) ⏞ M a r k o v P ( s ∣ z 1 : t − 1 ) P ( z t ∣ z 1 : t − 1 ) P ( s ∣ z t ) P ( z t ) ⏞ B a y e s P ( s ∣ z 1 : t − 1 ) P ( s ) P ( z t ∣ z 1 : t − 1 ) \begin{aligned}P\left( s|z_{1:t} \right) \frac{P\left( z_t|s,z_{1:t-1} \right) P\left( s|z_{1:t-1} \right)}{P\left( z_t|z_{1:t-1} \right)}\\\,\, \frac{{ \overset{\mathrm{Markov}}{\overbrace{P\left( z_t|s \right) }}}P\left( s|z_{1:t-1} \right)}{P\left( z_t|z_{1:t-1} \right)}\\\frac{{ \overset{\mathrm{Bayes}}{\overbrace{P\left( s|z_t \right) P\left( z_t \right) }}}P\left( s|z_{1:t-1} \right)}{{ P\left( s \right) }P\left( z_t|z_{1:t-1} \right)}\end{aligned} P(s∣z1:t)P(zt∣z1:t−1)P(zt∣s,z1:t−1)P(s∣z1:t−1)P(zt∣z1:t−1)P(zt∣s) MarkovP(s∣z1:t−1)P(s)P(zt∣z1:t−1)P(s∣zt)P(zt) BayesP(s∣z1:t−1)
计算后验概率优势比 P ( s 1 ∣ z 1 : t ) P ( s 0 ∣ z 1 : t ) P ( s 1 ∣ z t ) P ( s 0 ∣ z t ) ⋅ P ( s 1 ∣ z 1 : t − 1 ) P ( s 0 ∣ z 1 : t − 1 ) ⋅ P ( s 0 ) P ( s 1 ) \frac{P\left( s1|z_{1:t} \right)}{P\left( s0|z_{1:t} \right)}\frac{P\left( s1|z_t \right)}{P\left( s0|z_t \right)}\cdot \frac{P\left( s1|z_{1:t-1} \right)}{P\left( s0|z_{1:t-1} \right)}\cdot \frac{P\left( s0 \right)}{P\left( s1 \right)} P(s0∣z1:t)P(s1∣z1:t)P(s0∣zt)P(s1∣zt)⋅P(s0∣z1:t−1)P(s1∣z1:t−1)⋅P(s1)P(s0)
一般令先验概率 P ( s 0 ) P ( s 1 ) 0.5 P\left( s0 \right) P\left( s1 \right) 0.5 P(s0)P(s1)0.5引入Logistic变换 L ( p ) log [ p / ( 1 − p ) ] L\left( p \right) \log \left[ {{p}/{\left( 1-p \right)}} \right] L(p)log[p/(1−p)]
则 L ( s ∣ z 1 : t ) L ( s ∣ z t ) L ( s ∣ z 1 : t − 1 ) L\left( s|z_{1:t} \right) L\left( s|z_t \right) L\left( s|z_{1:t-1} \right) L(s∣z1:t)L(s∣zt)L(s∣z1:t−1)
称为栅格状态的更新模型。更新模型中与新测量值 z t z_t zt有关的项是 L ( s ∣ z t ) L\left( s|z_t \right) L(s∣zt)由于激光雷达的测量值只有两种情况因此定义 { l o o c c u : L ( s ∣ z t 1 ) l o f r e e : L ( s ∣ z t 0 ) \begin{cases} \mathrm{looccu}: L\left( s|z_t1 \right)\\ \mathrm{lofree}: L\left( s|z_t0 \right)\\\end{cases} {looccu:L(s∣zt1)lofree:L(s∣zt0)
必须指出 l o o c c u \mathrm{looccu} looccu与 l o f r e e \mathrm{lofree} lofree表达了在获得感知数据的情况下栅格真实状态的概率这是与传感器性能有关的常数。传感器性能越好测量结果越接近真实值 l o o c c u \mathrm{looccu} looccu越大 l o f r e e \mathrm{lofree} lofree越小。一般地可以设定 l o o c c u 0.9 \mathrm{looccu}0.9 looccu0.9、 l o f r e e − 0.7 \mathrm{lofree}-0.7 lofree−0.7
2.2 截断策略
从更新模型可以看出 L ( s ∣ z 1 : t ) L\left( s|z_{1:t} \right) L(s∣z1:t)是对历史观测序列的整合。换言之若假设 ∣ l o o c c u ∣ ∣ l o f r e e ∣ \left| \mathrm{looccu} \right|\left| \mathrm{lofree} \right| ∣looccu∣∣lofree∣则当一个栅格被观察到 k k k次自由状态后必须再被观察到至少 k k k次占据状态才有可能被设置为占据栅格。这导致实际应用时动态环境中的地图可能无法被快速更新。
为了建图的适应性采用截断策略定义概率上下限来限制改变栅格状态所需的更新次数代价是概率在0-1区间内不再完备靠近边界的概率丢失 L ( s ∣ z 1 : t ) max { min { L ( s ∣ z t ) L ( s ∣ z 1 : t − 1 ) , L max } , L min } L\left( s|z_{1:t} \right) \max \left\{ \min \left\{ L\left( s|z_t \right) L\left( s|z_{1:t-1} \right) , L_{\max} \right\} , L_{\min} \right\} L(s∣z1:t)max{min{L(s∣zt)L(s∣z1:t−1),Lmax},Lmin}
3 仿真实现
3.1 算法流程
算法流程如下所示 其中关于Bresenham视线法原理请参考路径规划 | 图解Theta*算法(附ROS C/Python/Matlab仿真)
3.2 Matlab实现
核心代码如下所示
for i 1:N x scan_pose(1, i);y scan_pose(2, i);theta scan_pose(3, i);robot_pos [ceil(x * resolution) origin(1), ceil(y * resolution) origin(2)];% Find grids hit by the rays (in the gird map coordinate)rays scan_ranges(:, i);x_occ rays .* cos(scan_angles theta) x;y_occ -rays .* sin(scan_angles theta) y;occ_pos [ceil(x_occ * resolution) origin(1), ceil(y_occ * resolution) origin(2)];% Find occupied-measurement cells and free-measurement cellsocc_id sub2ind(size(grid_map), occ_pos(:, 2), occ_pos(:, 1));free [];for j 1:scans_num[ix_free, iy_free] bresenham(robot_pos, occ_pos(j, :)); free [free; iy_free, ix_free];endfree_id sub2ind(size(grid_map), free(:, 1), free(:, 2));% Update the log-oddsgrid_map(occ_id) grid_map(occ_id) lo_occ;grid_map(free_id) grid_map(free_id) - lo_free;% Saturate the log-odd valuesgrid_map(grid_map lo_max) lo_max;grid_map(grid_map lo_min) lo_min;
end完整工程代码请联系下方博主名片获取 更多精彩专栏
《ROS从入门到精通》《Pytorch深度学习实战》《机器学习强基计划》《运动规划实战精讲》… 源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系
