移动机器人仿真：基于Webots软件的分析 课件全套1-6绪论 -避障规划和轨迹规划

第1讲绪论移动机器人(MobileRobot)具有移动能力的机器人是对人类移动能力的一种模拟和扩展有效扩大了机器人的工作范围和活动空间1954年，第一台机器人面向操作作业，安装在固定基座上1968年第一台轮式移动机器人斯坦福大学机器人研究所1969年第一台双足移动机器人日本早稻田大学加藤一郎移动作业机器人工业应用移动机器人的应用工业应用空间探测1985～移动机器人的应用工业应用空间探测1985～家庭应用2002～移动机器人的应用近年来应用领域不断扩大移动机器人分类按移动方式按工作环境按工作空间按应用领域……按移动方式分类轮式履带式躯干式足式1.轮式(wheeled)1.轮式(wheeled)主要特点：机构简单与地面为连续点接触效率极大地依赖于环境情况，特别是地面的平坦和硬度，在非结构环境中移动性能较差2.履带式(track)通过履带的面接触方式来适应地面的不平整2.履带式(track)履带机构主要由履带、支撑履带的链轮、滚轮以及承载这些零件部件的行驶框架构成驱动轮旋转驱动履带循环，诱导轮和驱动轮一起支撑履带；下部滚轮用来减少履带下部着地压强的不均匀性；上部滚轮的作用是防止履带下垂；驱动轮诱导轮上滚轮下滚轮行驶框架2.履带式(track)主要特点：与地面为连续面接触，可较好地适应不平整地面和松软地面稳定性好、接地比压大、

牵引力大会对地面造成较大磨损适合于军事、救援等领域3.足式(Legged)足式机器人模拟了人或足式动物与地面非连续点接触，对行走路面的要求很低3.足式(Legged)优势能够适应复杂多变的地形能够适应不同的地面状况能够跨越障碍物和沟壑具有较小的地面支撑压力能够主动调节身体高度能够主动隔振、确保稳定具有静态稳定运动容错性能够利用腿足操作物体离散落脚点多自由度、多肢体3.足式(Legged)足式移动机器人是机器人技术研究的重要难题易受扰动而失稳自由度高，不易建模控制腿部多关节对驱动和机构要求高成本高3.足式(Legged)4.躯干式(trunk)陆地空中水下依附于空间的移动方式，以仿生为主要研发趋势移动方式发展趋势:混合移动机构移动方式发展趋势：新型驱动移动机器人分类按移动方式：轮式、履带式、足式、躯干式按工作环境：室内/室外，结构/半结构/非结构按工作空间：陆地、水下、空中、空间按应用领域：工业、农业、军事、服务…………移动机器人的关键性能机动度：空间运动的灵活度，或称为移动自由度轮式移动机器人机动度可移动度：通过速度控制可以实现的参考点移动能力可操纵度：通过方向控制可以实现的参考点移动能力移动机器人的关键性能速度：最大最小的速度/加速度载荷能力：在满足其他性能要求的情况下，机器人能够承载的负荷重量运动精度：到点精度：机器人移动到点的实际位置和理想位置之间的差距重复精度：在相同的位置指令下，机器人连续重复运动若干次，其位置的分散情况移动机器人的关键性能运动稳定性静态稳定：质心点在支撑域内动态稳定：ZMP/CoP等在支撑域内移动机器人的关键性能移动自主性遥控、半自主、全自主WhereamI?(在哪里?)WhereamIgoing?(到哪里?)HowdoIgetthere?(怎么去?)机器人自主移动需要解决的关键问题?自定位目标规划导航规划环境地图地图表示未知环境地图构建机器人自主移动主要研究内容机器人地图目标定位地图构建导航规划执行自主移动机器人一般结构：感知-决策-执行导航规划信息处理感知运动控制执行测量数据局部观测/地图位置环境信息参考点运动轨迹驱动指令感知控制任务指令（目标）知识、数据库环境自定位/地图构建End！第2讲移动机器人运动学建模运动学建模运动学是指从几何的角度描述和研究物体位置、速度或者加速度随时间的变化规律机器人运动学建模是建立机器人参考点运动控制与各个驱动运动控制之间的数学模型运动学建模是实现机器人运动的核心基础是机器人系统设计的重要参考参考点运动控制驱动运动控制正运动学逆运动学运动学建模与机器人的机械结构密切相关轮式移动机器人运动学建模建立轮子的驱动控制/转速

与

机器人质心（或某一参考点）的运动控制/速度之间的关系模型轮式移动机构由车体、车轮、车体-车轮之间的支撑机构、以及车轮驱动机构组成轮子的类型、个数和排布方式是影响轮式移动机器人运动学的主要因素轮子的类型

标准轮脚轮Swedish轮球轮标准轮两个自由度：主转动轴，垂直旋转轴具有很高的方向性，为了移向不同方向，必须先沿着垂直轴调整轮子方向存在无侧滑运动学约束固定标准轮转向标准轮脚轮两个自由度旋转垂直轴不通过地面接触点具有很高的方向性、不存在无侧滑约束脚轮与标准轮的区别标准轮可以无偏地完成调向，其旋转垂直轴通过轮子与地面的接触点，存在无侧滑约束脚轮则是沿着偏离轴转动，不存在无侧滑约束，同时导致在调向时对移动底盘施加了一个力矩Swedish轮3个自由度：绕轮子主轴转动、绕滚子轴心转动、绕轮子和地面的接触点转动45度Swedish轮（Macanumwheel）90度Swedish轮连续切换轮存在不连续振动振动较小球轮是真正的全方向轮，被设计为可以主动驱动沿着任意轴旋转设计机制来源于计算机鼠标，利用鼠标球逆驱动方式实现驱动轮子的排布采用不同的轮子类型和不同的轮子数量可以构建不同的轮式移动机器人不同的配置使得机器人在稳定性、移动性和操控性方面具有不同的特性目前主要排布类型独轮车两轮车三轮车四轮车多轮全方位移动车独轮车(1)香港中文大学研发的独轮机器人整体是一个标准轮与地面是连续点接触具有动态稳定性静态不稳定独轮车(2)卡耐基梅隆大学研发的独轮机器人采用球轮具有全向移动特性问题：请分析说明其静态稳定性和动态稳定性两轮车(1)

采用后轮固定标准轮和前轮转向标准轮组成后轮进行速度控制，前轮进行方向控制两轮车(2)

两个固定标准轮左右并列同轴排布、独立驱动（也称为差分驱动）逆钟摆方式实现动态平衡

三轮车(1)

(1)两个独立驱动标准轮+一个随动轮（脚轮/球轮）

结构简单，旋转半径可以0到无穷，以P点为旋转中心三轮支撑域大，具有静态稳定特性差分驱动三轮车(2)

(2)前轮采用一个转向标准轮，分别进行方向控制和转速控制，后轮采用两个随动轮

从动轮驱动操舵轮

三轮车(3)

(3)前轮采用一个转向标准轮，进行方向控制，后轮采用两个通过差动齿轮进行驱动的固定标准轮

操舵轮差动齿轮

四轮车差分驱动Ackman底盘全方位移动车全方位移动车四个独立驱动的90度Swedish轮构成浙江大学小型足球机器人2013,2014,2018,2019四次获得国际冠军全方位移动车四个独立驱动的麦克纳姆轮构成全方位移动车三个独立驱动的90度Swedish轮构成轮式移动机器人运动学建模主要方法基于作用的运动学建模（前向运动学建模）基于约束的运动学建模运动作用运动约束分析假设刚体，忽略内部和轮子的关节和自由度在水平面上运动，总维数为3机器人可用空间中的一个点表示空间中的点在水平面上的投影坐标系定义全局(世界)坐标系：机器人(局部)坐标系两者速度关系

机器人姿态表示为坐标系定义全局(世界)坐标系：机器人(局部)坐标系两者速度关系

机器人姿态表示为运动学建模问题：以差分驱动机器人为例由两个独立驱动转速的固定标准轮和一个无驱动的随动脚轮组成轮子半径为轮子到两轮中间中点P的距离为两轮旋转速度分别为运动学建模问题：以差分驱动机器人为例

轮子半径为轮子到两轮中间中点P的距离为两轮旋转速度分别为只需求局部坐标系下机器人速度与两轮速度关系即可前向运动学建模（1）机器人运动是每个轮子的旋转速度对P点作用的叠加对P点在方向平移速度的作用一个旋转，一个静止同时旋转对P点在方向平移速度的作用

前向运动学建模（2）对P点旋转分量的作用仅右轮向前旋转，P点以左轮为

中心逆时针旋转，旋转速度为仅左轮向前旋转，P点以右轮为

中心顺时针旋转，旋转速度为

前向运动学建模（3）基于约束的运动学建模标准轮是各类轮子的基本部件标准轮存在的约束滚动约束，即轮子在相应方向发生运动时必须转动，即沿着轮平面的所有运动必须通过适当的旋转转量实现无侧滑约束，即轮子不能在垂直于轮子平面的方向发生滑动，即轮子垂直于轮平面的运动分量必须为零

（1）固定标准轮对底盘的角度固定设机器人坐标系下，轮A的位置为

轮平面法向量相对于PA连线的角度为（1）固定标准轮滚动约束：沿着轮平面的所有运动必须通过适当的旋转转量实现

（1）固定标准轮无侧滑约束：轮子垂直于轮平面的运动分量必须为零

（1）固定标准轮滚动约束：沿着轮平面的所有运动必须通过适当的旋转转量实现无侧滑约束：轮子垂直于轮平面的运动分量必须为零（2）转向标准轮转向标准轮可以控制轮子绕着穿过轮子中心和地面接触点的垂直轴旋转（2）转向标准轮滚动约束无侧滑约束转向位置的变化对机器人当前的运动约束没有直接影响（3）脚轮可以绕着垂直轴转向，但其旋转垂直轴并不通过地面接触点轮平面始终与AB对齐（3）脚轮标准轮的滚动约束

（3）脚轮

将分解到轮平面和法线方向

即为将分解到法线和轮平面方向

法线上为轮平面上为

（3）脚轮标准轮滚动约束

转向位置的变换和旋转垂直轴的偏移对平行于轮平面的运动不起作用

（3）脚轮标准轮的无侧滑约束

脚轮标准轮的无侧滑约束

脚轮无侧滑约束

转向位置的变换和旋转垂直轴的偏移使得脚轮侧向移动不再为零，要求通过一个等量而相反的转向运动进行平衡，因此，通过控制

的值，将使得任意侧向运动变得可行（3）脚轮滚动约束无侧滑约束总是能够被满足（4）Swedish轮由固定标准轮和附在轮子周围的转子组成相对于标准轮增加了一个自由度转子轴和主轮平面之间的夹角

（4）Swedish轮82无侧滑约束

滚动约束滚子的转速是自由的，因此滚子的无侧滑约束总是能够被满足，Swedish轮不存在无侧滑约束通过变化值，可以构造任意满足约束的期望运动向量（4）球轮是一种全方向系统对某个剖面来讲，其运动学描述和固定标准轮的完全相同当作为随动轮时，由于是自由变量，

这两个约束总是能被满足，不会对机器

人的运动产生影响基于约束的运动学建模

基于约束的运动学建模（差分驱动）根据每个标准轮的滚动约束和无侧滑约束构建运动学模型，忽略脚轮的运动学约束，因为脚轮无动力，它的滚动约束和无侧滑约束都能被满足

基于约束的运动学建模（差分驱动）根据每个标准轮的滚动约束和无侧滑约束构建运动学模型，忽略脚轮的运动学约束，因为脚轮无动力，它的滚动约束和无侧滑约束都能被满足

基于约束的运动学建模（差分驱动）

基于作用的运动学建模基于约束的运动学建模

基于约束的运动学建模假设机器人共有N个标准轮组成个固定标准轮，轮子角度向量为，旋转速度向量为

个转向标准轮，轮子角度向量为，旋转速度向量为固定标准轮转向标准轮常数随时间变化所有轮子的滚动约束为基于约束的运动学建模假设机器人共有N个标准轮组成个固定标准轮，轮子角度向量为，旋转速度向量为

个转向标准轮，轮子角度向量为，旋转速度向量为所有轮子的无侧滑约束为固定标准轮转向标准轮常数随时间变化基于约束的运动学建模假设机器人共有N个标准轮组成个固定标准轮，轮子角度向量为，旋转速度向量为

个转向标准轮，轮子角度向量为，旋转速度向量为所有轮子的总约束表达式为实例：三轮全方位移动机器人运动学建模移动机器人的机动性（灵活性）包括两个方面可移动性:

可操纵性:机动度是机动性的量化描述，是可以实现的移动自由度机动度=可移动度+可操纵度93通过控制轮子的速度实现的移动能力通过控制轮子的方向实现的移动能力可移动度基于作用的可移动度分析基于约束的可移动度分析/计算根据作用的可移动度分析根据定义直观分析：通过控制轮子的速度可以实现的移动自由度差分驱动机器人改变轮速度，即可以控制方向变化率，也可以控制前后移动速度，可移动度为2根据作用的可移动度分析根据定义直观分析：通过控制轮子的速度可以实现的移动自由度自行车改变轮速度只能改变前后速度，通过改变转向标准轮的方向，才可以控制方向的变化，可移动度为1根据作用的可移动度分析根据定义直观分析：通过控制轮子的速度可以实现的移动自由度3个Swedish轮构成的移动底盘改变轮速度，可以直接控制移动机器人的三个自由度，

可移动度为3根据约束的可移动度分析与计算可移动度=工作空间维度-独立约束数目机器人的可移动度是机器人运动上约束数目的函数，而不是轮子数目的函数根据约束的可移动度分析利用零运动直线和转动瞬时中心ICR分析零运动直线：几何上经过轮子的轴心并垂直于轮平面的线，当受无侧滑约束时，轮子在该直线上不能存在运动

根据约束的可移动度分析利用零运动直线和转动瞬时中心ICR分析可移动度=工作空间维度-独立约束数目独立约束数目=2差轮驱动机器人（2个驱动轮+1个随动脚轮）独立约束数目=1可移动度=1可移动度=2根据约束的可移动度分析利用零运动直线和转动瞬时中心ICR分析可移动度=工作空间维度-独立约束数目3个Swedish轮构成的移动底盘独立约束数目=0可移动度=3根据约束的可移动度分析利用零运动直线和转动瞬时中心ICR分析可移动度=工作空间维度-独立约束数目独立约束数目=2，可移动度=1每个机器人有且只有一个ICR一个前轮的零运动直线受后轮和另一个前轮的零运动直线约束根据约束的可移动度计算可移动度=工作空间维度-独立约束数目

无侧滑约束方程表示根据约束的可移动度计算

根据约束的可移动度计算

可移动度

车辆只能沿着一个圆或者一条直线行走，这种结构被称为移动性退化极端情况，机器人在三个方向都是完全受约束的，完全无法在平面中运动可操纵度通过控制轮子的方向能够实现的移动自由度对机器人移动姿态的影响是间接的增加可操纵的标准轮可能在增加可操纵度的同时减少可移动度

无侧滑约束方程表示可操纵度

机动度

移动机器人的完整性（Holonomic）完整性：对于移动机器人，特指机器人底盘的运动学约束完整运动学约束：可明确表示为仅包含位置变量的函数非完整运动学约束：需要微分关系，并且无法通过积分得到一个只包含位置变量的约束完整机器人：没有任何非完整运动学约束非完整(non-holonomic)机器人：存在至少一个非完整运动学约束标准轮无侧滑约束滚动约束移动机器人的完整性也可以基于机器人的移动自由度和工作空间维度之间的关系来描述完整机器人：

一个机器人是完整的，当且仅当其可移动度等于工作空间维度。第3讲地图创建概述地图环境知识的一种表达方式，是移动机器人定位、导航的基础

地图表示方法：需要有效表示空间环境帮助机器人完成特定任务容易加入新的信息，并便于计算机处理主要地图表示方法特征地图占用栅格地图拓扑地图点云地图特征地图（FeaturebasedMap）：基于连续线段结构化地图由直线（lines）或线段（segments）构成特征地图（FeaturebasedMap）：基于路标的非结构化地图标志物/路标（landmarks）的坐标/特征表示特征地图特点摘自中国大学MOOC《自主移动机器人》-浙江大学栅格地图（OccupiedGridMap）将环境分解成一系列离散栅格，每个栅格一个值，表示该栅格被占用的情况用概率表示栅格占用可能性。0表示自由空间，1表示有障碍物占据栅格状态是时变的栅格地图特点摘自中国大学MOOC《自主移动机器人》-浙江大学拓扑地图（TopologicalMap）将环境表示为由节点（node）和连接线（edge）组成的拓扑结构图节点表示环境中位置点边表示节点间连接关系拓扑地图特点摘自中国大学MOOC《自主移动机器人》-浙江大学点云地图（PointsCloudMap）直接用空间中的点集合表示：2D/3D坐标3D激光雷达点云Camera点云2D激光雷达点云点云地图特点摘自中国大学MOOC《自主移动机器人》-浙江大学第3讲地图创建点/路标地图创建方法目录基于非视觉类传感器的点/路标的特征提取基于视觉类传感器的点/路标的特征提取目录基于非视觉类传感器的点/路标的特征提取基于视觉类传感器的点/路标的特征提取基于非视觉类传感器的点/路标的特征提取传感器GPS，北斗；室内无法使用测距仪：超声波、激光传感器数据格式GPS：经纬度坐标（x，y坐标）测距仪：距离+角度，2D坐标（x，y）基于非视觉类传感器的点/路标的特征提取点/路标的参数以全局坐标系为基准进行存储传感器原始数据以传感器坐标系为基准坐标变换：全局坐标系、机器人坐标系、传感器坐标系为简化问题复杂性，将机器人坐标系和传感器坐标系合并基于非视觉类传感器的点/路标的特征提取2D坐标（x，y）的变换基于非视觉类传感器的点/路标的特征提取距离+角度的变换目录基于非视觉类传感器的点/路标的特征提取基于视觉类传感器的点/路标的特征提取基于视觉类传感器的点/路标的特征提取传感器摄像头传感器数据格式：图片彩色图灰度图深度图基于视觉类传感器的点/路标的特征提取图像数据是以图像坐标系为基准进行存储坐标变换：机器人坐标系、摄像机坐标系、图像坐标系为简化问题复杂性，将机器人坐标系和传感器坐标系合并摄像机传感器已经完成了摄像机坐标系与图像坐标系之间的变换什么是好的特征？一个好的特征在旋转、缩放、光照、视角等条件改变的情况下，依然能被重复检测到主要问题旋转不变性，缩放不变性主要问题基于视觉类传感器的点/路标的特征提取两种点/路标特征：角点（corner）和斑块（blob）角点（corner）：两/多条边的交点对定位任务而言，角点比斑块具有更高的准确性角点看起来都很像，区分度不高基于视觉类传感器的点/路标的特征提取两种点/路标特征：角点（corner）和斑块（blob）斑块（blob）：依照”灰度、纹理突变”的特征所确定的一块连通区域斑块不适用于定位任务斑块比角点具有更高的区分度角点（corner）检测Harris角点检测如何检测角点？沿任一方向移动一个窗（window），至少在两个方向上检测出灰度值的显著变化即检测出角点“flat”

region:nointensity

change“corner”:significantchangein

atleast2

directions“edge”:nochangealongthe

edgedirectionHarris角点检测如何用数学方法去刻画角点特征？当窗口发生[u,v]移动时，那么滑动前与滑动后对应的窗口中的像素点灰度变化描述如下(x,y)是窗口内所对应的像素坐标位置，窗口有多大，就有多少个位置Harris角点检测w(x,y)是窗口函数，最简单情形就是窗口内的所有像素所对应的w权重系数均为1。有时候，会将w(x,y)函数设定为以窗口中心为原点的二元正态分布。Harris角点检测为了减小计算量，利用泰勒级数进行简化公式Harris角点检测为了减小计算量，利用泰勒级数进行简化公式Harris角点检测矩阵M的关键性难道我们是直接求上述的E(u,v)值来判断角点吗？Harris角点检测并没有这样做，而是通过对窗口内的每个像素的x方向上的梯度与y方向上的梯度进行统计分析。以Ix和Iy为坐标轴，因此每个像素的梯度坐标可以表示成(Ix,Iy)Harris角点检测针对平坦区域，边缘区域以及角点区域三种情形进行分析：Harris角点检测对这三种情况窗口中的对应像素的梯度分布进行绘制：Harris角点检测如果使用椭圆进行数据集表示，则绘制图示如下：Harris角点检测利用M特征值对图像点进行分类：特征值都比较大时，

即窗口中含有角点特征值一个较大，一个较小

窗口中含有边缘特征值都比较小，

窗口处在平坦区域Harris角点检测如何度量角点响应？即评价角点的强度其中k是常量，一般取值为0.04-0.06，这个参数仅仅是这个函数的一个系数，它的存在只是调节函数的形状而已。Harris角点检测为什么用R来度量角点响应？绘制的R函数图像如下：最后设定R的阈值，进行角点判断Harris角点检测旋转不变性Image

1Image

2Harris角点特点Allpointswillbeclassifiedas

edgesCorner!Image

1Image

2Harris角点不具备缩放不变性缩放不变性？Harris角点特点应用最广泛的角点检测算法具有：旋转不变性亮度变化不变性不具有：缩放不变性Harris角点特点小结缩放/尺度不变性（ScaleInvariant）检测缩放/尺度不变性（ScaleInvariant）检测设计一个函数对应与各个尺寸的圆缩放/尺度不变性（ScaleInvariant）检测该函数对于各个尺度下的圆都能保持具有一个局部极值好的函数只有一个极值。这样的函数称为检测算子（detector）Iregion

sizebadIregion

sizebadIregion

sizeGood

!缩放/尺度不变性（ScaleInvariant）检测斑块（blob）检测常用的斑块检测算子有：LoG:LaplacianofGaussianoperatorDoG:DifferenceofGaussianSIFT(itusesDoGfeatures)SURF(it’sanfastimplementationofSIFT)CenSurEMSER斑块特征斑块检测算子：LoG算子将图像与核函数进行卷积，核函数称为检测算子LoG算子：高斯函数的拉普拉斯变换斑块对应的是卷积后图像的最大值或最小值斑块检测算子：LoG算子LoG斑块看起来是什么样子？用两个高斯函数的差(DifferenceoftwoGaussianfunctions)来近似LoGDoG算子与LoG算子的特点几乎一致，但DoG算子针对缩放/尺度变换具有更高的计算效率|斑块检测算子：DoG算子LoG和DoG算子非常适合缩放/尺度变换的不变性检测斑块检测算子：LoG/DoG算子对缩放/尺度变换的不变性SIFT斑块特征SIFT(ScaleInvariantFeatureTransform)是一种检测斑块的方法

由D.Lowe(Univ.ofBristishColumbia,Canada)教授于2004提出应用非常广泛（场景识别，消费DC……）特点：SIFT采用的是DoG核

具有缩放/尺度变换不变性；具有方向、视角变换不变性(upto60度)描述子基于梯度的方向(gradientorientations)，比灰度值更具鲁棒性SIFT特征SIFT主要步骤在不同尺度下提取关键点（Extractkeypoints+scale）赋予关键点方向（Assignkeypointorientation）生成关键点描述子（Generatekeypointdescriptor）SIFT特征生成尺度空间金字塔：不同尺度重采样，利用高斯核模糊化原始图像生成DoG金字塔：取相继高斯模糊图像差提取关键点：DoG金字塔中的局部极值SIFT特征Step1：keypointlocation+scaleSubsampleDoG:BlurredbyGaussiankernelSIFT特征Step2：assignkeypointorientation对关键点邻域中每个像素计算灰度梯度值（幅值和方向）构建灰度梯度的方向直方图直方图中的尖峰对应该关键点的主方向对于每一个关键点，都拥有位置、尺度以及方向三个信息。为每个关键点建立一个描述符，用一组向量将这个关键点描述出来，使其不随各种变化而改变，比如光照变化、视角变化等等。这个描述子不但包括关键点，也包含关键点周围对其有贡献的像素点，并且描述符应该有较高的独特性，以便于提高特征点正确匹配的概率。SIFT特征Step3：SIFT描述子SIFT描述子:描述的是所有梯度方向相对关键点主方向的方向（Describeallgradientorientationsrelativetothekeypointorientation）在关键点周围采用16×16的邻域，将该16×16区域进一步划分为4×4子块在关键点尺度空间内4×4的窗口中计算梯度直方图信息，然后用高斯窗口对其进行加权运算。SIFT特征Step3：SIFT描述子黄色圈代表高斯加权的范围（越靠近关键点的像素梯度方向信息贡献越大）SIFT描述子:描述的是所有梯度方向相对关键点主方向的方向（Describeallgradientorientationsrelativetothekeypointorientation）在每4×4的小块上计算8个方向的梯度方向直方图，绘制每个梯度方向的累加值，即可形成一个种子点，共4×4×8=128维向量表征。SIFT特征Step3：SIFT描述子SIFT特征：检测结果示例SIFT特征对视角改变的稳定性具有旋转变换不变性；缩放/尺度变换不变性；光照条件变换不变性；方向、视角变换不变性(upto60度)具有高区分度的特征计算量巨大改进算法：SURFSIFT特征小结第3讲地图创建直线/线段地图创建方法目录基于非视觉类传感器的直线/线段特征提取基于视觉类传感器的直线/线段特征提取目录基于非视觉类传感器的直线/线段特征提取基于视觉类传感器的直线/线段特征提取基于非视觉类传感器的直线/线段特征提取传感器测距仪：超声波、激光基于非视觉类传感器的直线/线段特征提取

摘自中国大学MOOC《自主移动机器人》-浙江大学基于非视觉类传感器的直线/线段特征提取直线/线段的参数以全局坐标系为基准进行存储传感器原始数据以传感器坐标系为基准坐标变换：全局坐标系、机器人坐标系、传感器坐标系为简化问题复杂性，将机器人坐标系和传感器坐标系合并基于非视觉类传感器的直线/线段特征提取RANSACRANSAC(RAndomSAmple

Consensus)针对噪声数据，RANSAC是一种很有效的模型拟合方法可以用于直线拟合（2D/3D)，也适用于从噪声数据中提取模型参数的问题，如摄像机标定迭代法：找出一组无噪声的数据集用于提取参数需要很多步迭代缺点：每次迭代的结果可能不同参考文章M.

A.Fischler

and

R.

C.Bolles.

Random

sample

consensus:

A

paradigm

for

model

fitting

with

applicatlons

to

image

analysis

andautomated

cartography.

Graphics

and

Image

Processing,

24(6):381–395,

1981.RANSACSelectsampleof2pointsatrandom|RANSACSelect

sample

of

2

points

atrandomCalculatemodelparametersthatfitthedatainthe

sampleRANSACSelect

sample

of

2

points

atrandomCalculatemodel

parametersthatfitthedatainthe

sampleCalculateerrorfunction

foreachdata

pointRANSACSelect

sample

of

2

points

atrandomCalculatemodel

parametersthatfitthedatainthe

sampleCalculateerrorfunctionfor

eachdata

pointSelectdatathat

supportscurrent

hypothesisRANSACSelect

sample

of

2

points

atrandomCalculatemodel

parametersthatfitthedatainthe

sampleCalculateerrorfunctionfor

eachdata

pointSelectdatathat

supportscurrent

hypothesisRepeatsamplingRANSACSelect

sample

of

2

points

atrandomCalculatemodel

parametersthatfitthedatainthe

sampleCalculateerrorfunctionfor

eachdata

pointSelectdatathat

supportscurrent

hypothesisRepeatsamplingRANSACSetwiththemaximumnumberofinliersobtainedafterk

iterations需要多少步迭代？理想：在含有N个点的数据集中，所有2个点的组合N↑，迭代次数非常巨大真的需要这么多次迭代吗？如果粗略估计有效数据在整个数据集中的占比，那么不需要可通过概率的方式实现减少迭代次数RANSACRANSAC

摘自中国大学MOOC《自主移动机器人》-浙江大学RANSAC摘自中国大学MOOC《自主移动机器人》-浙江大学Splitandmerge源于计算机视觉领域的一种常用直线提取算法以迭代的方式分割、拟合直线Algorithm1:Split-and-Merge

(standard)源于计算机视觉领域的一种常用直线提取算法以迭代的方式分割、拟合直线Algorithm1:Split-and-Merge

(standard)Algorithm2:Split-and-Merge(iterative

end-point-fit)Algorithm2:Split-and-Merge(iterative

end-point-fit)SplitAlgorithm2:Split-and-Merge(iterative

end-point-fit)SplitSplitSplitAlgorithm2:Split-and-Merge(iterative

end-point-fit)No

moreSplitsSplitSplitSplitAlgorithm2:Split-and-Merge(iterative

end-point-fit)MergeNo

moreSplitsSplitSplitSplit目录基于非视觉类传感器的直线/线段特征提取基于视觉类传感器的直线/线段特征提取基于视觉类传感器的直线/线段特征提取传感器摄像头传感器数据格式：图片彩色图灰度图深度图基于视觉类传感器的直线/线段特征提取图像数据是以图像坐标系为基准进行存储坐标变换：机器人坐标系、摄像机坐标系、图像坐标系为简化问题复杂性，将机器人坐标系和传感器坐标系合并摄像机传感器已经完成了摄像机坐标系与图像坐标系之间的变换基于视觉类传感器的直线/线段特征提取线段提取与边沿检测的区别边沿：在某一个方向上检测出灰度值的显著变化检测出的边沿有助于提取直线/线段霍夫变换（Hough-Transform）以投票的形式从边沿图像中找出直线/线段投票空间称作霍夫空间（Houghspace）霍夫变换（Hough-Transform）图像中的直线/线段在霍夫空间映射为一个点霍夫变换（Hough-Transform）令(x0,y0)为图片中的一个点，通过该点的所有直线可以表示为在霍夫空间中表示为霍夫变换（Hough-Transform）在图片中增加一个新点(x1,y1)，相应的表达式为在霍夫空间中表示为霍夫变换（Hough-Transform）在霍夫空间中怎样表示同时经过(x0,y0)和(x1,y1)的直线？是和的交点（b*，m*）霍夫变换（Hough-Transform）若霍夫空间以(b,m)表示会有什么问题？m和b论阈是无边界的[-∞，+∞]如何解决？霍夫变换（Hough-Transform）图像中的点映射到(ρ,θ)空间霍夫变换（Hough-Transform）图像中的点映射到(ρ,θ)空间霍夫变换（Hough-Transform）霍夫变换（Hough-Transform）存在的问题噪声的影响平面栅格地图实现方法本课件中所有图、表均摘自宾夕法尼亚大学开放课程“Robotics:EstimationandLearning”。感谢该开放课程的所有讲师和作者。我们希望在贝叶斯框架下根据传感器数据更新每个栅格的占据概率，但是，直接跟踪概率是困难的，介绍新的表示方法。如果有一个某件事发生的概率，记做p(X)赢率(odd)可以被记为一个比例，这个比例是这件事发生的概率与它不发生的概率之比:利用栅格被占据的赢率表示为图上所示的后验概率形式：log

odd+(后验)=log

odd

meas(似然)+log

odd−(先验)当在使用这个更新法则时需要记住俩件事1.只更新被观察到的栅格2.更新后的值将变成先验信息当你在后续点获取了新的测量时，整个过程是递归发生的。那么如何更新赢率（odd）呢？在一个被初始化为全0对数赢率的占据栅格地图中，意味着这种初始化等价于网格被占据和开放的概率是一样的，怎样利用测距仪数据建立栅格地图呢？坐标变换给定的栅格地图是具有一定分辨率的方格地图，如何从连续的地图（即没有栅格）的位置转换到栅格地图上的位置，对于他们中的点，使用Bresenham画线算法。在实际情况下，距离传感器将往不同方向发射多条射线，其中，α代表每条射线基于机器人x轴的方向。Bresenham算法Bresenham算法核心思想：

假设：k=dy/dx。因为直线的起始点在象素中心，所以误差项d的初值d0＝0。

X下标每增加1，d的值相应递增直线的斜率值k，即d＝d＋k。一旦d≥1，就把它减去1，这样保证d在0、1之间。

当d≥0.5时，最接近于当前象素的右上方象素（x+1,y+1）

而当d<0.5时，更接近于右方象素(x+1,y）为方便计算，令e＝d-0.5，

e的初值为-0.5，增量为k。

当e≥0时，取当前象素（xi，yi）的右上方象素（x+1,y+1）

而当e<0时，更接近于右方象素(x+1,y）第4讲

定位概率框架下基于地图的信息融合定位目录概述算法目录概述算法概述假设有一个移动机器人在一个已知地图的环境中运动概述当其开始运动时，其初始位置是精确确定的，采用里程计方法推算其位置概述当其开始运动时，其初始位置是精确确定的，采用里程计方法推算其位置概述当其开始运动时，其初始位置是精确确定的，采用里程计方法推算其位置概述机器人根据其观测到的环境信息更新其位置以减小误差（降低不确定性）概述概率框架下基于地图的定位包含的主要部分概述定位精度的概率置信度表示ParticleFilter目录概述算法算法框架算法框架算法框架算法框架算法框架示例展示-1DCase将环境分为10个栅格假设机器人初始位置的概率模型满足0-3一致分布算法框架示例展示-1DCase运动更新（ActionUpdate）假设机器人向前运动，其运动满足如下概率模型机器人运动后，新位置的置信度为多少？算法框架示例展示-1DCase运动更新（ActionUpdate）机器人运动后，新位置的置信度为多少？将两个概率分布进行卷积算法框架示例展示-1DCase运动更新（ActionUpdate）将两个概率分布进行卷积：细化算法框架示例展示-1DCase感知更新（PerceptionUpdate）假设机器人携带一个测距仪，可以测量以原点为起始点的距离，该测距仪的统计模型为该模型表明测距仪可以测量以原点为起点的5-6格的距离算法框架示例展示-1DCase感知更新（PerceptionUpdate）机器人经过感知环境后，其位置的置信度为多少？算法框架示例展示-2DCase机器人在一个16个栅格的环境中运动，其初始位置的概率分布为右图所示算法框架示例展示-2DCase机器人的运动指令（编码器，里程计模型）为

该指令的置信度即概率分布为算法框架示例展示-2DCase机器人运动到（2，3）栅格的概率是多少？(3,3)→(2,3)(2,3)→(2,3)(3,2)→(2,3)(3,4)→(2,3)算法框架示例展示-2DCase对于(3,3)→(2,3)算法框架示例展示-2DCase同理可以算出(2,3)→(2,3),(3,2)→(2,3),(3,4)→(2,3).05.20.18.20算法框架示例展示-2DCase机器人运动到（2，3）栅格的概率是4种类情况之和，即同理，我们可以计算出所有栅格在机器人运动之后的概率置信度。算法框架示例展示-2DCase假设机器人携带的传感器为测距仪，可测量机器人正前方1-2个单位的距离，其概率分布为中图所示试确定机器人在利用传感器感知环境信息后位于(2,3)的置信度求解方法与运动更新类似，即用各栅格的概率乘以感知概率，最后再针对具体栅格求和所有栅格进行完感知更新后，需要进行归一化处理，以保证栅格概率总和为1.80算法框架总结概率框架下基于地图的定位包含两个重要的过程（步骤）运动更新，即后续卡尔曼滤波定位算法中的Predictionprocess感知更新，即后续卡尔曼滤波定位算法中的Updatingprocess将两种信息进行融合提高机器人位置的置信度摘自宾夕法尼亚大学开放课程

“Robotics:EstimationandLearning”第4讲定位卡尔曼滤波定位算法目录概述实现目录概述实现概述卡尔曼其人（1930.5.19-2006.7.2）鲁道夫·卡尔曼（RudolfEmilKalman），匈牙利裔美国数学家1930年出生于匈牙利首都布达佩斯。1953年于麻省理工学院获得电机工程学士，翌年硕士学位。1957年于哥伦比亚大学获得博士学位。概述卡尔曼主要成就提出了系统的能控性和能观性提出卡尔曼滤波ANewApproachtoLinearFilteringandPredictionProblems（1960）2009年获美国国家科学奖章Kalmanfilter引子-状态估计摘自Mathworks官方教程Kalmanfilter引子-状态估计摘自Mathworks官方教程Kalmanfilter引子-状态估计摘自Mathworks官方教程Kalmanfilter-最优状态估计摘自Mathworks官方教程Kalmanfilter-最优状态估计摘自Mathworks官方教程Kalmanfilter-最优状态估计摘自Mathworks官方教程两个pdf相乘Kalmanfilter-最优状态估计摘自Mathworks官方教程Kalmanfilter-最优状态估计摘自Mathworks官方教程Kalmanfilter-最优状态估计摘自Mathworks官方教程Kalmanfilter-最优状态估计摘自Mathworks官方教程Kalmanfilter-最优状态估计摘自Mathworks官方教程Kalmanfilter-最优状态估计摘自Mathworks官方教程高斯（正态）分布的线性特征Kalmanfilter-最优状态估计摘自Mathworks官方教程Kalmanfilter-最优状态估计摘自Mathworks官方教程Kalmanfilter-最优状态估计摘自Mathworks官方教程Kalman增益Kalmanfilter-最优状态估计Kalman增益怎样得到的呢？[1]BromileyPA.ProductsandConvolutionsofGaussianProbabilityDensityFunctionsDensityFunctions.2003.[1]Kalmanfilter-最优状态估计Kalman增益怎样得到的呢？[1]BromileyPA.ProductsandConvolutionsofGaussianProbabilityDensityFunctionsDensityFunctions.2003.[1]华丽的分割线Therefore，Kalmanfilter-最优状态估计摘自Mathworks官方教程Kalmanfilter-最优状态估计摘自Mathworks官方教程迭代Kalmanfilter-最优状态估计摘自Mathworks官方教程非线性情况-ExtendedKalmanFilter(EKF)摘自Mathworks官方教程非线性情况-ExtendedKalmanFilter(EKF)摘自Mathworks官方教程目录概述实现卡尔曼定位实现-类比摘自Mathworks官方教程Autonomousrobotw/o

GPSonlywithmapfeatures通过传感器感知环境特征间接获取自身位置信息卡尔曼定位实现-类比摘自Mathworks官方教程非线性模型；传感器可能感知多个地图特征卡尔曼定位实现-非线性方法EKFPrediction线性化或者求出解析偏导很复杂卡尔曼定位实现-非线性方法EKFPrediction卡尔曼定位实现-非线性方法EKFUpdate线性化卡尔曼定位实现-非线性方法EKFMatching怎样确定yk和m1,m2,m3哪个最匹配呢？距离穷举法定义：Innovation（新息）则马氏距离卡尔曼定位实现-总结第5讲SLAM目录概述EKF-SLAM算法目录概述EKF-SLAM算法概述什么是SLAM？SimultaneousLocalizationAndMapping（同时制图与定位）比纯定位任务复杂何时需要SLAM？机器人对所处环境没有任何先验知识，即没有任何预设地图机器人所处环境无法使用GPS或其他类型的定位装置机器人必须知道其当前所处的位置概述-SLAM基本原理概述-SLAM基本原理概述-SLAM基本原理概述-SLAM基本原理概述-SLAM基本原理概述-SLAM基本原理概述-SLAM基本原理概述-SLAM基本原理概述-SLAM基本原理目录概述EKF-SLAM算法EKF-SLAM算法与EKF定位算法的关系：相同点通俗的讲，EKF-SLAM可以看作是放大版的EKF定位都含有prediction，matching和update过程运动模型和测量模型完全一样EKF-SLAM算法与EKF定位算法的关系：不同点状态变量不同EKF-SLAM算法与EKF定位算法的关系：不同点Update过程中的状态变量既包括机器人位姿也包括地图特征参数EKF-SLAM算法与EKF定位算法的关系：不同点Matching过程中会增加新的地图元素EKF-SLAM的缺点与EKF定位相比状态变量规模大随着新加入的地图元素增加，协方差矩阵P呈指数级增长第6讲移动机器人导航规划导航规划机器人地图目标规划定位地图构建导航规划执行在给定环境的全局或局部知识以及一个或者一系列目标位置的条件下，使机器人能够根据知识和传感器感知信息高效可靠地到达目标位置感知导航方式固定路径导引：有人工标识导引无轨导航：有人工标识导引的无固定路径（无轨）导航无标识导引的自然无轨导航方式1：有人工标识导引的固定路径导航磁条导航磁感应线导航磁钉导航二维码导航优点：技术成熟、稳定可靠、价格优惠缺点：需要施工和维护、路线无法调整AGV：Automatic

GuidedVehicle自动导引车方式2：有人工标识导引的无轨导航激光反射板导航优点：技术成熟、路径可调缺点：需要施工和维护、价格昂贵方式3：无人工标识导引的无轨导航自然导航优点：无需施工、路径可调、精确定位、室内外通用缺点：算法复杂，环境变化影响定位可靠性和稳定性导航规划问题分解路径规划：避障规划：轨迹生成：根据所给定的地图和目标位置，规划一条使机器人到达目标位置的路径（只考虑工作空间的几何约束，不考虑机器人的运动学模型和约束）根据所得到的实时传感器测量信息，调整路径/轨迹以避免发生碰撞根据机器人的运动学模型和约束，寻找适当的控制命令，将可行路径转化为可行轨迹。路径规划、避障规划、轨迹规划三者关系路径规划根据所给定的地图和目标位置，规划一条使机器人到达目标位置的路径避障规划根据所得实时传感器测量信息，调整轨迹以避免发生碰撞战略方法战术方法互补轨迹规划根据机器人的运动学模型和约束，寻找适当的控制命令，将可行路径转化为可行轨迹机器人执行互补路径规划根据所给定的地图和目标位置，规划一条使机器人到达目标位置的路径通常简化为只考虑工作空间的几何约束，不考虑机器人的运动学模型和约束工作空间与位形空间(C-Space)工作空间：移动机器人上的参考点能达到的空间集合，机器人采用位置和姿态描述，并需考虑体积位形空间：机器人成为一个可移动点，不考虑姿态、体积和非完整运动学约束障碍物按机器人半径进行膨胀忽略非完整约束对姿态的限制位形空间障碍物空间：不可行的位形集合在该空间中，机器人会与障碍物发生碰撞自由空间：可行的位形集合在该空间中，机器人将无碰地安全移动路径规划就是在自由位形空间中为机器人寻找一条路径，使其从初始位置运行到目标位置路径规划方法需要具备完备性完备性：当解存在时，能够在有限的时间内找到解路径规划算法挑战：在连续空间内搜索，难以保证时间确保完备性的方法基本思路：对空间作离散化分辨率完备：解析性离散化，确保获得可行解行车图法：基于障碍物几何形状分解姿态空间单元分解法：区分空闲单元和被占单元势场法：

根据障碍物和目标对空间各点施加虚拟力概率完备:基于概率进行随机采样离散化，使获得解的概率趋近于1PRM（ProbabilisticRoadMaps）RRT（Rapid-ExploringRadomTrees）连通图中搜索最优路径的方法精确最优搜索法：深度优先法、宽度优先法近似最优搜索法启发式搜索法：A*，D*准启发式搜索算法：退火、进化和蚁群优化等分辨率完备的路径规划方法1.行车图法基本思想：基于障碍物几何形状分解位形空间，将自由空间的连通性用一维曲线的网格表示，在加入起始点和目标点后，在该一维无向连通图中寻找一条无碰路径构建行车图的典型方法：可视图（Visibilitygraph）Voronoidiagram1.1可视图法可视图由所有连接可见顶点对的边组成可见指顶点之间无障碍物初始位置和目标位置也作为顶点1.1可视图法优点：非常简单，特别是当环境地图用多边形描述物体时可得到在路径长度上最优的解缺点：所得路径过于靠近障碍物，不够安全。常用的解决方法：以远大于机器人半径的尺寸膨胀障碍物，但容易造成可行路径的消失在路径规划后修改所得路径，使其与障碍物保持一定的距离1.2

Voronoidiagram基本思想：取障碍物之间的中间点，以最大化机器人和障碍物之间的距离1.2

Voronoidiagram构建方法：对于自由空间中的每一点，计算它到最近障碍物的距离；在垂直于二维空间平面的轴上用高度表示该点到障碍物的距离，类似于画直方图；当某个点到两个或多个障

碍物距离相等时，其距离

点处出现尖峰，Voronoi

diagram就由连接这些

尖峰点的边组成。1.2

Voronoidiagram优点：安全性高缺点：计算复杂、路径长度较可视图法长、不适用于短距离定位传感器2.单元分解法基本思想首先，将位形空间中的自由空间分为若干的小区域，每一个区域作为一个单元，以单元为顶点、以单元之间的相邻关系为边构成一张连通图；其次，在连通图中寻找包含初始姿态和目标姿态的单元，搜索连接初始单元和目标单元的路径；最后，根据所得路径的单元序列生成单元内部的路径主要方法精确单元分解近似单元分解2.1精确单元分解单元边界严格基于环境几何形状分解，所得单元完全空闲2.1精确单元分解优点：机器人不需要考虑它在每个空闲单元中的具体位置，只需要考虑如何从一个单元移动到相邻的空闲单元单元数与环境大小无关缺点：计算效率极大地依赖于环境中物体的复杂度2.2

近似单元分解栅格表示法，将环境分解成若干个大小相同的栅格并不是每个单元都是完全被占或者完全空闲的，因此分解后的单元集合是对实际地图的一种近似2.2近似单元分解优点非常简单，与环境的疏密和物体形状的复杂度无关缺点：对存储空间有要求可变大小的近似单元分解四叉树表示法：递归地把环境分为4个大小相等的子区域。直到每个区域中所包含的基本元素全为0或全为1。3.人工势场法基本思想：目标点对机器人产生吸引力，障碍物对机器人产生排斥力所有力的合成构成机器人的控制律3.

人工势场法步骤1：构建人工势场(ArtificialPotentialField)目标点：吸引势场人工势场法步骤1：构建人工势场(ArtificialPotentialField)目标点：吸引势场障碍物：推斥势场被评估点和障碍物点之间的距离预定义距离阈值3.人工势场法步骤2：根据人工势场计算力对势场求偏导数3.人工势场法步骤3：计算合力，并进而由力计算得到控制律力的方向就是机器人运动方向，大小可以对应加速度控制3.人工势场法机器人是受人工势场影响的一个点，沿着势场方向就可以避开障碍物达到目标点3.

人工势场法不仅是一种路径规划方法，所构建的势场也构成了机器人的控制律，能够较好地适应目标的变化和环境中的动态障碍物，可以作为实时避障算法3.

人工势场法缺点：存在局部最小，容易产生振荡和死锁第6讲规划与导航最优路径搜索算法目录树的构建深度优先搜索算法（DFS）Dijkstra’s算法A*算法目录树的构建深度优先搜索算法（DFS）Dijkstra’s算法A*算法树的构建边可以被赋予权重，表示该路径的消耗以起点做为根节点，根据连通关系构建树目录树的构建深度优先搜索算法（DFS）Dijkstra’s算法A*算法深度优先搜索算法（DFS）BADB

CGIJDFS例子DFS例子BADB

CGIJACGADB

CGIJACBGCDDFS例子ADB

CGIJJACBGDIDFS例子ADB

CGIJJACBGDIDFS例子B

CGIADJJACBGDIDFS例子深度优先搜索算法（DFS）若有权重，则选权重和最小的路径目录树的构建深度优先搜索算法（DFS）Dijkstra’s算法A*算法Dijkstra’s算法EdsgerDijkstra1959年提出采用优先级定义的广度优先搜索思想有权重的图以起始点为中心，按路径长度递增方式层层向外扩展，直到扩展到终止点ADBCGIJ533252 225132{}S:{}B(0)T:{ }{B(0)}B(0)Dijkstra’s算法例子摘自中国大学MOOC《自主移动机器人》-浙江大学T:{}S:{B(0)

}A(5)C(2)G(3){C

(2),G

(3),A(5)

}ADBCGIJ533252 225132523B(0)Dijkstra’s算法例子T:C

(2),{G

(3),A(5)

}ADBCGIJ533252 225132{G

(3),A(5)

}A(5)C(2)G(3)523B(0)A(4)2{G

(3),A

(4)}S:{B(0)

}{B

(0),C(2)

}Dijkstra’s算法例子T:{G

(3),A(4)

}ADBCGIJ533252 225132C(2)G(3)23B(0)A(4)2S:{B(0)

}{B

(0),C(2)

}D(4)2G(4)2{G

(3),A

(4),D(4)

}Dijkstra’s算法例子T:{G

(3),A

(4),D(4)

}ADBCGIJ533252 225132C(2)G(3)23B(0)A(4)2S:{B(0)

}{B

(0),C(2)

}D(4)2I(7)5{G

(3),A

(4),D

(4),I (7)

}Dijkstra’s算法例子T:{A

(4),D

(4),I (7)

}ADBCGIJ533252 225132C(2)G(3)B(0)A(4)S:{B

(0),C

(2),G(3)

}D(4)I(7)I(8)Dijkstra’s算法例子T:{D

(4),I (7)

}ADBCGIJ533252 225132C(2)G(3)B(0)A(4)S:{B

(0),C

(2),G

(3),A(4)

}D(4)I(7)D(7)Dijkstra’s算法例子T:{

I (7)

}ADBCGIJ533252 225132C(2)G(3)B(0)A(4)S:{B

(0),C

(2),G

(3),A

(4),D(4)

}D(4)I(7)I(5){I(5)

}Dijkstra’s算法例子T:{I(5)

}ADBCGIJ533252 225132C(2)G(3)B(0)A(4)S:{B

(0),C

(2),G

(3),A

(4),D(4)

}D(4)I(5){I(5)

,J(6)

}J(6)Dijkstra’s算法例子T:{I(5)

}ADBCGIJ533252 225132C(2)G(3)B(0)A(4)S:{B

(0),C

(2),G

(3),A

(4),D

(4),I(5)

}D(4)I(5){J(6)

}J(6)Dijkstra’s算法例子T:{I(5)

}ADBCGIJ533252 225132C(2)G(3)B(0)A(4)S:{B

(0),C

(2),G

(3),A

(4),D

(4),I

(5),J(6)

}D(4)I(5){

}J(6)Dijkstra’s算法例子目录树的构建深度优先搜索算法（DFS）Dijkstra’s算法A*算法A*算法摘自中国大学MOOC《自主移动机器人》-浙江大学ADBGIJ533252 2

2 C5132h=

1h=

2h=

3h=

5A*算法例子ADBCGIJ533252 225132{}Closed:{}B(0)Open

(Q):{ }{B(0)}B(0)A*算法例子Open

(Q):{}Closed:{B(0)

}A(5)C(2)G(3){C

(2+3),G

(3+5),A(5+5)

}523B(0)ADBGIJ533252 22

C5132+5+3+5A*算法例子Open

(Q):{G

(3+5),A

(5+5)}Closed:{B(0)

,C(2)

}A(5)C(2)G(3){G

(3+5),A(4+5)

}523B(0)AJDBGI533252 22

C5132+5+5A(4)2+5A*算法例子Closed:{B(0)

,C(2)

}C(2