DASE 7503 · 中文逐页精讲

06 定位与导航

DASE7503 Localisation and Navigation_2026.pdf · 共32页

没有找到对应内容,换一个词试试。

原课件第1页 / 32页定位与导航:我在哪里,怎样过去

06 定位与导航 原课件第1页
点击原页可放大查看

定位回答“我现在在哪里、朝向哪里”,导航回答“怎样从这里到目标”。这一讲从信标定位开始,再讲地图上的最短路,最后讲边走边建图的SLAM。

想象你第一次进入一栋楼:先认出自己站在哪个门口,再找通往教室的路线,最后沿路线走。机器人也要做这几步,只是用传感器测量和程序计算来完成。

原课件第2页 / 32页全讲路线与几个关键词

06 定位与导航 原课件第2页
点击原页可放大查看

前半讲localisation(定位),包括无线电、声波和光信标;中间讲navigation(导航)及Dijkstra、A*最短路径算法;最后讲SLAM,同时定位与建图。

先认识三种对象:pose是机器人位姿;map是环境表示;path是经过环境的路线。位姿、地图和路径不是同一个东西。最短路算法输入的是节点和边组成的图,而SLAM的任务之一是从传感器数据建立可用于导航的环境表示。

原课件第3页 / 32页定位、建图和导航怎样相互依赖

06 定位与导航 原课件第3页
点击原页可放大查看

机器人计划走一条路线,必须先知道自己从哪里出发。如果把当前位置估错,即使路线计算正确,也可能走错路口。地图告诉它哪里能走,定位把机器人放到这张地图里,导航再决定往哪里走。

本讲选择有地图的路径规划:先把环境表示成图,节点可代表门口、转角或路口,边代表两点间可通行的连接,边权代表距离或其他代价。SLAM则面向地图未知的情况,一边估计自身位置,一边完善地图,并用概率方法处理测量不确定性。

原课件第4页 / 32页位姿不只是一个坐标点

06 定位与导航 原课件第4页
点击原页可放大查看

二维位姿通常写成(x,y,θ):x、y描述位置,θ描述朝向。同一个地面点,朝门和背对门是两个不同的位姿,后续需要的动作也不同。

清洁机器人要知道自己清扫过哪些地带;送货机器人要知道自己相对起点或楼层地图在哪里。编码器可以从起点累加运动,但误差也会累积。定位问题的难点不是保存三个数字,而是根据有噪声的观测,持续更新这三个数字,使其与真实运动保持接近。

原课件第5页 / 32页借助外部基准定位

06 定位与导航 原课件第5页
点击原页可放大查看

室外可用GNSS,例如GPS;室内可布置信标,让机器人相对这些已知位置的基准测量。信标可以发红外线、超声波、激光或无线电信号。

定位的基本做法是:事先知道基准在哪里,再测机器人与基准的距离、方向或信号到达时间,最后解出机器人位置。“全局”表示相对于共同的环境坐标系,不等于每种传感器都能直接给出完整位姿。室内墙体和遮挡会影响信号传播,布置位置也会影响测量几何。

原课件第6页 / 32页从传播时间得到距离信息

06 定位与导航 原课件第6页
点击原页可放大查看

图中彩色圆圈代表不同信标发出的波向外传播。发射时刻已知、接收端时间同步时,传播距离d=cΔt;c是信号速度,Δt=接收时刻−发射时刻。

例如声速取340 m/s,传播时间10 ms=0.01 s,距离为340×0.01=3.4 m。这个计算使用声波速度;无线电测距需使用电磁波的传播速度。

若两个信标同时发射同类信号,到达时刻分别为t_1、t_2,则d_1−d_2=c(t_1−t_2)。例:左侧信号早2 ms到达,t_左−t_右=−0.002 s,距离差=340×(−0.002)=−0.68 m,说明机器人离左侧近0.68米。只知道这个距离差时,候选位置构成双曲线;知道各自距离时,才分别画测距圆。

课堂补充(11-16 Part01 00:00:00–00:10:50):课堂用信号到达的先后比较机器人离两侧信标的远近。读本页时,先分清测的是传播时间还是两信号的到达时间差。

原课件第7页 / 32页两个信标为什么可能仍不够

06 定位与导航 原课件第7页
点击原页可放大查看

知道某个信标到机器人的距离,就以信标为圆心、这段距离为半径画圆。两个圆相交时通常留下两个候选点。

例:信标A在(0,0),B在(6,0),两段距离都是5米。机器人位置(x,y)满足:

x²+y²=25;(x−6)²+y²=25。

展开第二式为x²−12x+36+y²=25。用它减第一式,得到−12x+36=0,所以x=3。代回第一式:9+y²=25,y²=16,因此y=4或−4。两个候选位置为(3,4)、(3,−4)。第三个位置合适的信标可帮助选出正确点。

“两个信号同时到达”只表示等距离;所有等距点构成连线的垂直平分线,中点只是其中一个。只测到达时间差时,应使用距离差约束,而不是把它当成两个已知半径的圆。

原课件第8页 / 32页知道位置后,怎样知道朝向

06 定位与导航 原课件第8页
点击原页可放大查看

只有单个接收点的距离定位,通常只能给位置,不能直接给车头朝向。可以用指南针测朝向,也可在移动后,用两次位置差求运动方向:θ_motion=atan2(y_2−y_1,x_2−x_1)。

例如从(1,1)走到(2,2),位移为(1,1),运动方向45°。若机器人正在倒车或侧移,运动方向就不一定等于车头方向。图下方还提示另一种布置:把测距传感器装在机器人上、环境放反射标记,由机器人主动测量固定基准。

课堂补充(11-16 Part01 00:00:00–00:10:50):本段强调,确定位置与确定机体朝向是两件事。位置点相同,车头仍可以朝不同方向。

原课件第9页 / 32页光信标:先测相对方向

06 定位与导航 原课件第9页
点击原页可放大查看

光信标像灯塔,用不同颜色区分身份。机器人旋转扫描,或用全景视觉系统,同时记录各信标相对车体的方向。图示绿灯45°、红灯165°,两者都是机器人自己的局部坐标角。

两条视线夹角为165°−45°=120°。但“看见两个相对方向”还不能单独确定二维位置和朝向,因为未知量有x、y、θ三个,而这里只提供两个角度约束。此页先展示观测,下一页加入更多信息来解决定位。

课堂补充(11-16 Part01 00:00:00–00:10:50):光信标部分讨论通过旋转观察或全向视觉,测量已识别信标的方位角。

原课件第10页 / 32页两个角度为何对应许多位置

06 定位与导航 原课件第10页
点击原页可放大查看

图上方几个机器人站在不同位置、朝向也不同,却能看见相同夹角的红绿灯。所以仅有两个局部方位角,还不足以唯一确定位置和朝向。

一种补充是由罗盘等提供全局朝向θ。局部信标角为α时,全局视线方向就是θ+α。设地图的 x 轴向右,逆时针为正。车头相对 x 轴逆时针转了30°,信标又在车头左侧45°,所以全局视线方向是 +30°+45°=75°。

有了全局视线方向,就从两个已知信标沿各自视线的反方向画射线。两条射线不平行且有交点时,该交点给出机器人位置。另一种做法是增加第三个位置合适的信标;下图用蓝灯提供额外约束。读图同时看车的位置、朝向和视线,便能看出增加的测量怎样排除其他候选。

原课件第11页 / 32页导航是一整条处理链

06 定位与导航 原课件第11页
点击原页可放大查看

导航是让机器人从当前位置到达目标的过程,包含定位、建图、路径规划、运动控制和避障。传感器持续提供环境信息,使路线与运动能随情况更新。

例如去走廊另一端:定位先给当前位姿;地图标出墙和通道;规划器选择经过哪些位置;控制器把路线转换成轮速;避障模块发现突然出现的人后减速或绕行。路径只是计划,控制负责执行,传感器反馈让整个过程不断修正。

原课件第12页 / 32页全局规划与局部处理各做什么

06 定位与导航 原课件第12页
点击原页可放大查看

感知把原始测量变成环境信息;定位估计位姿;建图保存空间结构;规划决定路线;运动控制执行路线;避障处理眼前碰撞风险。

全局规划通常在较大范围地图中选择总体路线,例如“走A走廊再左转”;局部规划结合附近最新观测,决定接下来几秒如何移动,例如“前方有人,先停或从旁边绕”。两者可以同时工作,局部避障不等于抛弃地图,全局规划也不等于完全不看新传感器数据。

课堂补充(11-16 Part01 00:10:50–00:24:13):本段串联感知与定位、建图、规划、运动控制和避障。复习时按“传感器提供信息→地图和位置支持规划→电机执行路线”追一遍。

原课件第13页 / 32页Dijkstra:从起点向外确定最短距离

06 定位与导航 原课件第13页
点击原页可放大查看

把环境画成带非负边权的图。每个节点保存两个量:dist是从起点到它目前找到的最短距离,pre是这条路线中它前面的节点。Ready集合保存已经确定最短距离的节点。

每轮从未确定的节点中选择dist最小的一个,再尝试通过它改善邻居的距离。它不要求任意两节点直接相连;不可达节点会保留无穷大。用普通数组选择最小值约需O(V²+E),常用优先队列实现约为O((V+E)log V)。V为节点数,E为边数。

课堂补充(11-16 Part01 00:10:50–00:24:13):Dijkstra部分围绕距离图、Ready集合、邻居与距离表更新展开;可结合后面各页的表格计算回听。

原课件第14页 / 32页把Dijkstra伪代码逐句读懂

06 定位与导航 原课件第14页
点击原页可放大查看

初始化三类记录:dist[S]=0,其余dist=∞;Ready为空;pre全部留空。dist记目前最短距离,Ready记已确定节点,pre记路线前驱。

每轮按以下次序处理:

  1. 在未确定的节点中,选择dist最小的n。
  2. 把n加入Ready。
  3. 对n的每个邻居m,计算candidate=dist[n]+edge(n,m)。
  4. 若candidate<dist[m],更新dist[m]=candidate,同时记pre[m]=n;否则保留原记录。
  5. 回到第1步,直到需要的节点都处理完。

第3、4步叫松弛:问“改从n走到m,会不会更短?”例如dist[n]=5、边长3,候选=5+3=8;原来到m记12,所以改成8,前驱改为n。先比较距离,再同步改前驱,路线记录才能与距离一致。

原课件第15页 / 32页算例第0步:全部距离怎样初始化

06 定位与导航 原课件第15页
点击原页可放大查看

图有S、a、b、c、d五个节点。边权为S-a=10、S-c=5、S-d=9、a-c=3、a-b=1、c-b=9、c-d=2、d-b=6。

起点S到自己为0;a、b、c、d都先记∞,表示尚未找到路线,并不是它们真的无限远。Ready={},所有前驱为空。第一轮比较0、∞、∞、∞、∞,最小的是S,所以先处理S。右侧表每列是一节点,上行记距离,下行记前驱;后面按这两个字段更新。

原课件第16页 / 32页算例第1、2步:先S,再c

06 定位与导航 原课件第16页
点击原页可放大查看

处理S后:a=10、c=5、d=9,前驱都为S;b仍∞。Ready={S}。剩余节点中c的5最小,下一步处理c。

由c出发逐个比较:到a为5+3=8,小于原10,所以a改8、pre[a]=c;到b为5+9=14,小于∞,所以b改14、pre[b]=c;到d为5+2=7,小于原9,所以d改7、pre[d]=c。处理完Ready={S,c},未确定距离为d=7、a=8、b=14,下一轮应选d。

原课件第17页 / 32页算例第3、4、5步:得到最终表

06 定位与导航 原课件第17页
点击原页可放大查看

第3步选d,当前距离7。经d到b为7+6=13,比原14短,所以b改13,pre[b]=d。此时Ready={S,c,d}。

第4步选a,当前距离8。经a到b为8+1=9,比13短,所以b改9,pre[b]=a。第5步选b,已经没有更短更新,算法结束。

最终dist[S,a,b,c,d]=[0,8,9,5,7],前驱为a←c、b←a、c←S、d←c。每轮都选尚未确定的最小距离,再更新邻居,不能按字母顺序选点。

原课件第18页 / 32页从前驱表还原真正的路线

06 定位与导航 原课件第18页
点击原页可放大查看

距离表告诉我们从S到b最短为9,但要知道经过哪些节点,还要沿pre倒着找。先从终点b开始:pre[b]=a,再查pre[a]=c,再查pre[c]=S。

得到逆序b→a→c→S,把顺序反转,就得到S→c→a→b。检验路程:S-c=5,c-a=3,a-b=1,总和5+3+1=9,与dist[b]一致。前驱存的是紧接着的上一个节点,不是整条路线;反复追前驱才能恢复完整路径。

原课件第19页 / 32页A*:同时看已走距离和剩余估计

06 定位与导航 原课件第19页
点击原页可放大查看

Dijkstra按从起点已走的代价g排序;A*按f=g+h排序。h是从当前节点到目标的剩余代价估计,f是这条候选路线的预计总代价。例如路线长度作为代价时,可用直线距离作为h。

候选A的g=5、h=3,所以f=8;候选B的g=6、h=1,所以f=7。A*先处理B,因为B的预计总代价更小。这里的h是估计,h较小不等于真实剩余路程一定更短。

“扩展”是把选中的路线再向邻居延长一步,然后把新候选放回待选列表,重新比较。用不高估真实剩余距离的 h 作下界。一般实现遇到更小的 g 时要更新记录,必要时把已扩展节点重新放回候选表。若每条边都满足 h(当前) ≤ 边权 + h(邻居),称为启发函数一致,可以只扩展每个节点一次。课件这组边权和 h 满足这一条件。目标路线成为列表中最优并被取出时,才按相应条件停止。

课堂补充(11-16 Part01 00:24:14–00:35:36):A*部分讨论直线距离估计、候选路线比较和最佳优先扩展。把本页的已走代价g与剩余估计h分开读,再看候选为什么换序。

原课件第20页 / 32页A*算例:先比较S的三条支路

06 定位与导航 原课件第20页
点击原页可放大查看

终点为b,图中节点旁的h值为a=1、b=0、c=3、d=5,边上的数才是实际路程。先从S生成三条候选:

候选路线 已走g 剩余估计h 合计f
S→a 10 1 11
S→c 5 3 8
S→d 9 5 14

取最小f=8的S→c,把它从待选表取出,再向c的邻居延长一步。本算例不重复经过路线中已有的节点,所以不走回S。新候选为:S→c→a,f=(5+3)+1=9;S→c→b,f=(5+9)+0=14;S→c→d,f=(5+2)+5=12。

原来的S→a(11)、S→d(14)仍留在表中。把旧候选和新候选一起按f排序,下次取S→c→a(9)。g累加沿途边长,h只取当前末端节点的一次估计。

小演示 · 逐步运行 Dijkstra 和 A*

原课件第21页 / 32页A*算例:继续扩展最有希望的路线

06 定位与导航 原课件第21页
点击原页可放大查看

本轮取出S→c→a。它已走g=5+3=8,末端a的h=1,故f=9。a的邻居有S、c、b;S和c已经在这条路线中,跳过后只向b延长。

新路线S→c→a→b的g=5+3+1=9。终点b已经到了,剩余距离h(b)=0,所以f=9+0=9。把新路线放回待选表,再与旧候选比较:

候选路线 f
S→c→a→b 9
S→a 11
S→c→d 12
S→c→b 14
S→d 14

最小的一项既到达目标,又排在全部候选之前,下一页检查为什么可停止。图中保留不同路线便于演示;按节点保存最佳g的实现可淘汰明显更差的重复到达记录。

原课件第22页 / 32页为什么到b的9可以确定为最短

06 定位与导航 原课件第22页
点击原页可放大查看

当前待选表最小的是S→c→a→b,已经到达目标,真实总代价为5+3+1=9。其余候选的预计总代价依次为11、12、14、14。

这里h不高估真实剩余距离,因此f=g+h只是最终代价的下界。比如S→a的下界为11,它继续走完后总长至少11;其他候选同样至少12或14。它们都超过9,所以无法得到更短路线,可以停止。

本页原文把次小候选写成12;按上一页保留全部路线的表,S→a仍在,次小应为11。两者都大于9,最短路线结论相同。停止判断要比较整个待选表,不能在刚生成任意一条到终点的路线时就立即结束。

课堂补充(11-16 Part01 00:24:14–00:35:36):回听A*候选比较及到达目标后的停止判断,再用本页的下界比较检查为什么可以停止。

原课件第23页 / 32页SLAM:没有地图时边走边建立

06 定位与导航 原课件第23页
点击原页可放大查看

SLAM全称Simultaneous Localisation and Mapping,即同时定位与建图。机器人不知道完整环境布局,却需要根据观测估计自己的位姿,并把墙、角点等信息放进地图。

这两件事相互依赖:知道自己在哪里,才能把新障碍画到正确位置;已有地图又帮助判断自己在哪里。SLAM用连续运动和观测共同约束两者。在室内、地下或卫星信号无法使用的环境中,这种能力尤其有用。

课堂补充(11-16 Part01 00:24:14–00:35:36):老师用室内工厂、仓库中GPS信号可能受阻的情景,引出机器人为什么需要SLAM。

原课件第24页 / 32页SLAM里的三种工作

06 定位与导航 原课件第24页
点击原页可放大查看

定位估计机器人位姿,建图估计环境结构,传感器融合把不同来源的测量合起来。比如编码器估计轮程、IMU感知角运动、激光雷达看到墙的位置,三者各提供不同约束。

融合并不是简单把数字平均,而是先对齐时间和坐标,再结合各测量的误差特点。例如轮子打滑时编码器可能高估位移,激光与固定墙面的匹配可帮助修正;快速转动期间,IMU又可补充较高频率的运动信息。

课堂补充(11-16 Part01 00:35:36–00:47:20):本段讨论把多种传感器数据结合起来。复习时抓住“不同观测提供不同信息”,再看本页各传感器怎样互补。

原课件第25页 / 32页SLAM的一次更新怎样发生

06 定位与导航 原课件第25页
点击原页可放大查看

先根据上一时刻位姿和运动输入预测当前位置;再读取传感器,识别墙、角点或其他稳定特征;把新特征与地图中已有特征对应;最后调整机器人位姿和地图,使观测尽量一致。

例如机器人估计前进1米,但看到墙面位置与预测差0.1米,就需要修正运动估计或地图。卡尔曼滤波、粒子滤波、图优化是处理这类估计的不同方法。它们共同面对的问题是:运动有误差、观测有噪声,不能把每次读数都当绝对正确的位置。

原课件第26页 / 32页几类SLAM分别以什么为线索

06 定位与导航 原课件第26页
点击原页可放大查看

视觉SLAM利用相机图像中的特征;双目或RGB-D还提供深度信息。激光SLAM利用扫描几何,例如墙和角点。足式或轮式SLAM描述所结合的运动平台,常与视觉或激光方法配合。

语义SLAM进一步给地图元素加类别,如门、桌、椅。几何层回答“这里有障碍、在什么位置”,语义层回答“它是什么”。这些分类不是互斥选项:一台轮式机器人可以同时用激光、相机,并给识别到的物体加语义标签。

课堂补充(11-16 Part01 00:35:36–00:47:20):回听视觉特征与LiDAR SLAM的对比,分别对应本页的图像线索与扫描几何线索。

原课件第27页 / 32页把SLAM放进ROS 2系统

06 定位与导航 原课件第27页
点击原页可放大查看

ROS 2负责把传感器节点、SLAM节点、坐标变换和可视化工具连接起来。SLAM算法本身负责估计,ROS 2提供消息通信、配置与启动组织方式。

可以沿数据流理解:激光节点发布扫描,里程计节点发布运动估计,SLAM节点结合它们输出地图和定位关系,RViz显示结果,导航系统再利用地图规划。不要把“启动ROS”理解成“已经完成SLAM”;还要让正确的观测、时间与坐标关系真正进入算法。

课堂补充(11-16 Part01 00:35:36–00:47:20):回听ROS 2中SLAM及实时、离线处理功能;然后按本页箭头分清传感器、估计算法与显示工具。

原课件第28页 / 32页slam_toolbox与Nav2分工

06 定位与导航 原课件第28页
点击原页可放大查看

本页介绍slam_toolbox用于二维激光SLAM,可持续建立地图,并通过回环等机制修正累计误差。Nav2负责导航任务,如规划、控制、行为协调,可以使用SLAM提供的地图和定位结果。

想象先用slam_toolbox边遥控边建图;地图可用后,再让Nav2接收目标并带车过去。RViz负责展示地图、激光、位姿和路线。三个角色分别是“估计环境”“决定与执行导航”“显示数据”,配合使用,但不能互相替代。

原课件第29页 / 32页Cartographer与RTAB-Map这两个例子

06 定位与导航 原课件第29页
点击原页可放大查看

课件继续列出Cartographer和RTAB-Map作为SLAM工具例子。Cartographer侧重将连续测量与运动信息组织为一致的地图;RTAB-Map常用于带图像、深度和其他传感器的建图与定位。

理解这些名称时先看输入是什么、输出二维还是三维、是否需要相机深度及多少计算资源。选择软件包后,还需将传感器主题与坐标系接入其配置。课件中的项目链接用于找到代码和说明,包名本身不会自动完成驱动、标定或导航配置。

原课件第30页 / 32页ROS 2建图流程与命令含义

06 定位与导航 原课件第30页
点击原页可放大查看

先确认雷达在 /scan 提供激光数据,里程计与 TF 能把雷达观测放到机器人和地图坐标中。SLAM Toolbox 的本页流程使用二维激光扫描。

在课件使用的 Ubuntu / ROS 2 Jazzy 环境中,如果是仿真,启动建图时指定仿真时间:

ros2 launch slam_toolbox online_async_launch.py use_sim_time:=true

使用实机传感器且没有仿真时钟时,把 true 改成 false。launch 启动一个配置好的节点组合,slam_toolbox 是包名,online_async_launch.py 是启动文件。原页还列 Cartographer 作为另一套工具,它的启动文件按实际安装的机器人配置选择。

另开终端用 RViz 查看地图:

rviz2 -d "$(ros2 pkg prefix --share slam_toolbox)/config/slam_toolbox_default.rviz"

$(...) 先查找软件包的共享目录。-d 后面是 RViz 显示配置。用遥控让机器人探索,观察 /map 的空闲、占据和未知栅格怎样变化。

地图完成后保存:

ros2 run nav2_map_server map_saver_cli -f my_map

run 运行包中的一个程序,map_saver_cli 把地图保存为图像与 YAML。-f my_map 指定文件名前缀。下次导航加载 YAML,才能按同样的原点和尺度读地图。

SLAM Toolbox Jazzy 启动文件与配置

原课件第31页 / 32页SLAM四个重要主题怎样连接

06 定位与导航 原课件第31页
点击原页可放大查看

/map通常发布占据栅格,表示哪里空闲、哪里有障碍;/scan发布激光扫描的角度与距离测量;/odom发布里程计运动估计;/tf传递坐标系之间的变换。

常见坐标链是map→odom→base_link→传感器。激光测到“前方2米”先在雷达坐标系里表达,借助TF转换到地图后,才能把障碍放对位置。/odom消息与TF有关联但并非同一消息;两者应保持一致。时间戳也要对应,否则移动中的扫描会被放到错误的位置。

课堂补充(11-16 Part01 00:35:36–00:47:20):本段提到map与scan。用本页对照它们:map是逐步建立的环境表示,scan是传感器当前取得的一次扫描。

原课件第32页 / 32页SLAM的四个主要难点

06 定位与导航 原课件第32页
点击原页可放大查看

计算量:观测不断进入,需要及时处理。噪声与漂移:每次小误差累积后,地图可能弯曲或错位。动态物体:人和车会移动,不适合长期当作固定地标。回环:重返旧地点时,识别“这里来过”并修正历史误差。

例如绕走廊一圈,估计起点和终点相差半米;识别到同一扇门后,回环约束可帮助把这一圈轨迹调整得更一致。但若把两扇相似的门认成同一个位置,错误回环又会扭曲地图,所以识别与几何匹配都很重要。

课堂补充(11-16 Part01 00:35:36–00:47:20):本段提到动态环境带来的问题。结合本页思考:同一物体如果一直在移动,为什么不适合直接作为固定地标?