崎岖起伏地形下机器人地形感知与移动控制方法及系统

1.本发明涉及机器人

技术领域

：
：，尤其涉及崎岖起伏地形下机器人地形感知与移动控制方法及系统。

背景技术

：
：：2.在大多数室内场景中，移动机器人都配有结构化环境的地图，该地图被分为可穿越和不可穿越的单元，包含障碍或墙壁的单元格被标记为不可穿越，而没有障碍的单元格被标记为可穿越。机器人不需要考虑地形属性，只需准确地识别障碍物。然而，在户外场景中，移动机器人必须穿越不平坦、粗糙和崎岖的恶劣自然地形，地形的几何和物理性质增加了机器人导航问题的复杂性。为了到达预期的目的地，机器人必须具有评估地形的能力，并确定与穿越每个地形段相关的代价。一个主要的挑战是开发一个感知框架，以快速和准确的区分可通过和不可通过的区域。3.视觉传感器作为机器人传感系统的重要组成部分，能够提供大量关于场景的信息。目前主流的感知处理主要是通过激光雷达进行建立地图，激光雷达获取的数据稀疏无序，无法获取颜和纹理信息，成本太高无法单独应用。在视觉传感器类型中，双目视觉具有非接触和被动的优点，是机器人获取障碍物信息和重建全局场景的关键技术。4.机器人的自主移动主要包括建立地图之后的路径规划和轨迹跟踪控制，现有的地面机器人自主移动地图主要围绕2维和3维，2维系统下对环境信息表达不够充分，只区分可通过区域和不可通过区域，不适用于复杂环境下多种地形表达；3维系统虽然将周围环境细致表达，但数据量太过庞大，数据处理过程过于复杂，成本太高，不适用于大规模应用。5.目前户外移动机器人各方面的性能已经不局限于可通行和不可通行区域，可以翻越或者跨越一些不规则复杂障碍物，与单一的结构化环境不同，复杂崎岖地形下通过不同区域所需要的代价也不一样，相对平面下的自主移动方法，包括路径规划算法和轨迹跟踪方法不能适用于起伏地形下的机器人自主移动，以往的路径规划等算法计算出的路径已不是最优路径，以往的导航和轨迹跟踪控制方法已经不满足越障移动机器人的发展。技术实现要素：6.针对现有技术存在的不足，本发明的目的是提供崎岖起伏地形下机器人地形感知与移动控制方法及系统，基于2.5d地图，实现机器人的路径规划和轨迹跟踪控制的自主移动；适用于大规模复杂环境，能够满足智能化和实用性的要求。7.为了实现上述目的，本发明是通过如下的技术方案来实现：8.第一方面，本发明的实施例提供了崎岖起伏地形下机器人地形感知与移动控制方法，包括：9.获取图像特征点，并进行求解优化，基于优化结果建立点云地图；10.提取点云地图的可穿越性信息，得到2.5d代价地图；11.基于改进的a*算法，在2.5d代价地图中生成带有障碍物可通过性信息的代价最小的路径，并进行全局路径规划和局部路径规划；12.通过带有高程约束信息的mpc控制算法，跟随局部路径进行轨迹跟踪，形成机器人移动控制方案。13.作为进一步的实现方式，对图像进行特征点提取并进行匹配，利用纯运动优化平移矩阵和旋转矩阵，通过最小化重投影误差定位每帧图片；14.通过对观测误差进行求解优化管理局部地图并优化。15.作为进一步的实现方式，回环检测计算图像之间的相似性，提取每个图像的特征点进行相似性评分，并通过位姿图优化消除累计漂移误差；16.在位姿图优化消除误差后，基于全局ba算法计算整个系统最优结构和运动结果，输出机器人在相机坐标系下的位姿和全局下的稀疏点云信息。17.作为进一步的实现方式，将点云进行坐标变换到地图坐标系下，进行滤波和降采样；根据相机坐标系与地图坐标系的相对关系，得到对应的点云全局信息。18.作为进一步的实现方式，将凹陷到水平面以下的地形作为负障碍物，通过地形凹陷深度和坡度进行判断地形是否可通过，以控制机器人在复杂地形下移动；19.将三维动态点云地图进行体素滤波变为三维八叉树地图，以体素大小执行均匀采样以离散化该点云集合，生成采样点云集合；从采样点集中提取局部地形特征，作为代价以设定权重得出结果投影到相应的xy栅格地图中。20.作为进一步的实现方式，在带有地形代价信息2.5d地图下的a*路径规划算法为：21.在2.5d代价地图中结合移动机器人自身性能，生成一条带有障碍物可通过性信息的代价最小的路径，进行全局路径规划和局部路径规划，生成一条轨迹使机器人绕过或翻越障碍物；将地形代价信息加入到路径规划中，控制机器人按照代价判断是直接通过崎岖路面还是绕过崎岖路面选择相对路径长、但地面信息相对平坦的路径进行移动。22.作为进一步的实现方式，所述带有高程约束信息的mpc控制算法包括：23.获取当前机器人的状态和路径规划生成的参考路径，将地图坐标系转换为机器人坐标系，以机器人初始位置作为原点，以移动机器人的方向为x轴方向；建立矩阵方程，使用qr分解解得位置参数，使用多项式拟合规划所跟踪的路径；24.根据当前状态和路径预测轨迹，将轨迹划分成n个控制点，间隔时间为dt；将处理之后t+1时刻的输出量yt+1与上一阶段t时刻的产生的控制量ut进行组合，作为新的状态量进行输入，形成预测方程得到预测输出yt+2；25.从第一个控制点完成之后，从第一步再递归进行，直到执行到yt+n时结束，n个输出结果构成了预测步长，n个控制量构成了控制步长。26.第二方面，本发明的实施例还提供了崎岖起伏地形下机器人地形感知与移动控制系统，包括：27.点云地图建立模块，用于获取图像特征点，并进行求解优化，基于优化结果建立点云地图；28.2.5d代价地图生成模块，用于提取点云地图的可穿越性信息，生成2.5d代价地图；29.路径规划模块，用于基于改进的a*算法，在2.5d代价地图中生成带有障碍物可通过性信息的代价最小的路径，并进行全局路径规划和局部路径规划；30.机器人移动控制方案生成模块，用于通过带有高程约束信息的mpc控制算法，跟随局部路径进行轨迹跟踪，形成机器人移动控制方案。31.第三方面，本发明的实施例还提供了崎岖起伏地形下机器人地形感知与移动控制设备，包括存储器以及处理器；32.所述存储器用于存储相关的程序代码；33.所述处理器用于调用所述程序代码，执行所述的机器人地形感知与移动控制方法。34.第四方面，本发明的实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质用于存储计算机程序，所述计算机程序用于执行所述的机器人地形感知与移动控制方法。35.本发明的有益效果如下：36.(1)本发明基于2.5d代价地图，实现机器人的路径规划和轨迹跟踪控制的自主移动；适用于大规模复杂环境，能够满足智能化和实用性的要求。37.(2)本发明的建图方式还增加了负障碍物，即凹陷到水平面以下的地形作为负障碍物，通过地形凹陷深度和坡度进行判断地形是否可通过，进而来控制机器人在复杂地形下移动。38.(3)本发明在传统mpc的基础上，增加了地图信息变化量与速度相融合的代价，当机器人移动通过高度变化的地形时地图信息变化量会变大，在约束条件中地图信息变化量与速度融合信息固定不变，所以速度会降低来缓冲掉地图信息的变化量，使得机器人在通过高度变化路面时，降低通过的速度进而使得机器人移动更加平稳。附图说明39.构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。40.图1是本发明根据一个或多个实施方式的导航堆栈架构图；41.图2是本发明根据一个或多个实施方式的坐标系变化示意图；42.图3是本发明根据一个或多个实施方式的改进a星算法路径规划示意图；43.图4是本发明根据一个或多个实施方式的差速机器人坐标系示意图；44.图5是本发明根据一个或多个实施方式的导航过程流程图。具体实施方式45.实施例一：46.本实施例提供了崎岖起伏地形下机器人地形感知与移动控制方法，包括：47.获取图像特征点，并进行求解优化，基于优化结果建立点云地图；48.提取点云地图的可穿越性信息，得到2.5d代价地图；49.基于改进的a*算法，在2.5d代价地图中生成带有障碍物可通过性信息的代价最小的路径，并进行全局路径规划和局部路径规划；50.通过带有高程约束信息的mpc控制算法，跟随局部路径进行轨迹跟踪，形成机器人移动控制方案。51.本实施例在ros(机器人操作系统)中首先利用双目rgb相机采集图像信息，解算特征点变换和相机位姿信息，建立起稀疏点云地图，并将点云信息实时变换到地图坐标系下，对全局坐标系下的点云数据特征进行代价计算并投影到二维平面，得到全局2.5d代价地图；利用ros提供的navigation堆栈进行了构建，将改进的a星算法作为全局规划器，在带有代价信息的2.5d地图中进行全局路径的生成。52.为了用非线性mpc实现崎岖起伏地形路径跟踪，将局部规划器部分作为mpc路径追踪器实现，在机器人周围生成局部路径进行跟踪控制，如图1所示；通过这样实施，可以直接使用现有navigation堆栈提供的定位(本地化)、地图服务和全局规划等。53.具体的，如图5所示，包括以下步骤：54.步骤a：将realsensed435i相机传入的图像进行orb特征点的提取并进行匹配，利用纯运动ba(bundleadjustment)优化相机的平移矩阵tc和旋转矩阵rc，通过最小化重投影误差进行定位每帧图片的相机，将相机位姿和图片中提取的特征点位置都看成优化变量一起优化。55.将三维坐标点计算得到像素坐标后，为了减少累计误差通过执行局部ba优化，通过对观测误差进行求解优化对位姿和路标同时做了调整，优化了一些主要的关键帧以及关键帧中可见的所有点，管理局部地图并优化。56.回环检测计算图像之间的相似性，提取每个图像的特征点进行相似性评分，取一个先验相似度s(vt,vt-δt)表示某时刻的图像中的关键点与上一时刻图像中关键点的相似性，其他都按这一分值进行归一化。[0057][0058]此步骤进行了检测回环并通过位姿图优化消除累计漂移误差。[0059]在回环检测位姿图优化消除误差后会启动第4个线程执行全局ba算法，计算整个系统最优结构和运动结果，最终输出机器人在相机坐标系下的位姿和稀疏点云信息。[0060]如图2所示，进行实时tf(transformframe)坐标变换，将点云进行坐标变换到地图坐标系下，进行滤波和降采样，根据相机坐标系与地图坐标系的相对关系，相机点云pc向地图坐标系pm的转换可以表示为：[0061]pm＝r×pc+t；[0062]其中r为旋转矩阵，t为平移向量(动态坐标变换)通过公式将相机坐标系中的点云信息转换为地图坐标系，至此就得到了相应的点云全局信息。[0063]步骤b：对建立的点云地图进行可穿越性信息的提取，将该信息和高程信息嵌入到状态空间中，形成2.5d代价地图。[0064]传统的octomap生成三维八叉树地图和二维栅格地图，本实施例将二者结合，既保留了三维信息，又兼顾了二维地图的易操作性。[0065]本实施例对于传统的建图方式还增加了负障碍物，即凹陷到水平面以下的地形作为负障碍物，通过地形凹陷深度和坡度进行判断地形是否可通过，进而来控制机器人在崎岖起伏地形下移动。[0066]环境由n个点组成的点云表示，将此点云集合称为原始点集qo，将三维动态点云地图进行体素滤波变成三维八叉树地图(octomap)，以体素大小rs执行均匀采样以离散化该点云集合，生成采样点云集合qs。我们从采样点集qs中提取局部地形特征，如粗糙度，倾斜度以及与相邻点集之间的几何关系，把这些作为代价以一定的权重得出结果投影到相应的xy栅格地图中。这些特征用于推导地图的可穿越性度量。对于采样点集qs中的第i个体素单元，选择几个评价因素来确定此体素单元的成本值：[0067]体素内点云的z轴平均值μz，用于表示障碍物的高度，并判断障碍物类型(正/负障碍物)。[0068]体素内所有点云的z轴方差dz，用于确定地形的粗糙度。[0069]对体素内点云进行表面重建计算表面z轴切线法向量nz，计算表面坡度。[0070]对第i个体素单元为中心，以五个单位长度为半径，计算此区域内所有体素单元z轴平均值μ的方差σz，确定地形起伏复杂度。[0071]然后对每个评价因素进行线性加权，得到成本函数：[0072]jc＝wμμz+wddz+wnnz+wσσz；[0073]其中，wμ,wd,wn,wσ是可调节的参数，根据所需的行为来衡量每个评价因素。在默认设置中，每个权重的值都是0.25。在实际应用中，这几个因素中影响最大的是μz，如果μz大于移动机器人的离地间隙，直接判定为不可通过区域；通过μz还能判定此障碍物的属性，是凸障碍物还是凹障碍物，对于不同属性的障碍物，另外三个评价因素对机器人移动的影响是有区别的，从而进行后续机器人复杂移动处理。[0074]将每个体素单元的成本用不同颜深度的灰度值(0-100)来投影到栅格地图(portablegraymap)中形成2.5d代价地图。对于每个体素单元的成本映射到地图中的具体实施公式如下：[0075][0076]其中jm表示最大成本阈值，填入地图的datai∈[0,100]。[0077]地图更新，来自距离传感器的新测量值被处理为空间中的点，并被映射到高程图。这将在高程图中的单元格(x,y)上产生一个新的高度测量值p(假设每个测量点更新一个单元格)。在地图坐标系中，高度测量值近似为高斯概率分布，为均值p和方差在传感器坐标系中给出位置rsp可以转换为对应的高度测量值p：[0078]p＝p(φ-1(rsp)-rsm)；[0079]投影矩阵p＝[001]将三维测量映射到标量高度测量p(在地图坐标系下)。[0080]步骤c：通过相机和imu融合获得当前移动机器人的位姿，然后通过改进的2.5d中的a*算法，在2.5d代价地图中结合移动机器人自身性能，生成一条带有障碍物可通过性信息的代价最小的路径，进行全局路径规划和局部路径规划，生成一条轨迹使机器人绕过或翻越障碍物。[0081]进一步的，改进的a*全局路径规划算法包括以下步骤：[0082]改进的a*算法的计算公式为：[0083]f(n)＝g(n)+h(n)+c(n)；[0084]其中，f(n)作为评估函数，评估每个节点到目标点的代价；g(n)是从初始状态移动到状态n的移动代价，在平面中移动n个单位距离需要的代价；h(n)是从状态n到目标状态的最佳路径的估计代价，也就是a星算法的启发函数。[0085]对于启发函数，如果网格形式的地图只允许朝上下左右四个方向移动，则可以使用曼哈顿距离如果网格形式的地图中允许朝八个方向移动，则可以使用对角距离，如果网格形式的地图中允许朝任何方向移动，可以使用欧氏距离，本方法采用欧式距离作为启发函数距离，表示两节点之间的先行距离。[0086]c(n)是在2.5d高程地图下，从初始状态移动到状态n的地形代价，地图中的的每个栅格中有代表通过此地形代价的信息，将地形代价信息加入到路径规划中，控制机器人按照代价判断是直接通过崎岖路面还是绕过崎岖路面选择相对路径长、但地面信息相对平坦的路径进行移动，c(n)取决于每个节点中所代表的地形可通过性信息的大小。[0087]根据传入的2.5d高程地图，给定全局目标和位姿，通过视觉里程计和imu获取当前位姿信息。如图3所示，初始化两个空表open_set和close_set，算法首先将起始点周围的所有可通过的节点加入到open_set中，将起点的代价设置为0，判断open_set是否为空，如果是，那么路径规划失败，结束操作，如果不为空，从open_set中选取当前所处节点周围所有可通过节点中代价最小点n，判断n是否是终点，如果是终点，则反向搜索前序节点，生成最优路径，路径规划成功，如果不是终点，将节点n从open_set中删除，加入close_set，然后遍历节点n周围的所有可通行的节点，如果节点n周围的可通行节点m不在close_set中，设置m的parent为n，用上述公式计算m点的代价，将m点加入open_set中执行上述判断，进行循环操作直到到终点。在每次搜索中，算法总是选择当前open_set中总代价f(n)最小的点进行展开，并重复此过程，直到生成代价最小的节点序列，即最优的路径规划。[0088]步骤d：由于本实施例中包括mpc在内的局部规划器进行移动穿越障碍物，而没有考虑识别和回避障碍物的问题，因此对于全局规划器使用上述改进的带有高程代价的a*算法反映障碍物的地图，以频繁的时间间隔(1秒)更新生成到目标地点的路径。局部规划器根据全局规划器生成的路径中机器人移动的位置，在机器人前方5米的路径生成局部规划轨迹，计算出机器人的加速度和转向速度。实际机器人控制所需的输入——前进速度v是将加速度a积分求得，最终各驱动轮的速度利用转速公式由电机控制器调节。[0089]首先要建立移动机器人的运动学模型，如图4所示，在全局坐标系下，差速轮式机器人的运动方程表示为：[0090][0091]偏航角的转速ω＝(vr-vl)/w，w是轮距，根据公式求解机器人驱动电机所需的旋转速度是ωr和ωl，可以根据公式计算：[0092][0093]与之前a星路径规划保持一致，横向轨迹误差表示二维坐标上机器人位置与路径之间的欧氏距离为了应用于路径跟踪问题，将横向轨迹误差和方向误差添加到状态变量中，方向误差衡量移动机器人前进方向与路径切线方向的差异。[0094]获取当前机器人的状态x和路径规划生成的参考路径，首先要进行坐标系变换，把地图坐标系转换为机器人坐标系，以机器人初始位置作为原点，以移动机器人的方向为x轴方向，然后建立矩阵方程，使用qr分解解得位置参数，使用三次多项式曲线拟合规划机器人的运动轨迹。[0095]根据上一时刻的机器人状态，预测下一时刻的状态实现模型的建立，离散后地模型为：[0096]xt+1＝f(xt,ut)；[0097]使用状态变量x＝(xyθvdηc)t建模为离散系统的机器人运动xt+1＝f(xt,ut)的非线性方程如下：[0098]xt+1＝xt+vtcos(θ)·dt；[0099]yt+1＝yt+vtsin(θ)·dt；[0100]θt+1＝θt+ωt·dt；[0101]vt+1＝vt+at·dt；[0102]dt+1＝g(xt)-yt+vtsin(η)·dt；[0103][0104]ct+1＝δdata[mx×xt+1+yt+1]×vt+1。[0105]dt是用于离散化的时间间隔，g(xt)是一个以xt为输入，输出y坐标的多项式曲线拟合函数，表示曲线拟合函数g(xt)的斜率，d代表横向轨迹误差，η方向误差，d和η表示机器人位置到预测路径的距离误差和机器人方向与预测路径切线的方向误差,δdata[]是当前状态和前一状态地图信息差值的绝对值，c是地图信息和机器人速度的融合量。[0106]mpc算法的步骤是：1.获取当前机器人的状态x和路径规划算法发出的参考路径，首先要进行坐标系变换，把地图坐标系转换为机器人坐标系，以机器人初始位置作为原点，以移动机器人的方向为x轴方向2.根据当前状态和路径，预测轨迹，将轨迹划分成n个控制点，间隔时间为dt，总时间t＝n*dt。3.控制驱动器输入，第一个控制点的数据4.从第一个控制点完成之后，从第一步再递归进行。[0107]mpc的优化问题表现为式的代价函数和式的约束条件：[0108][0109]其中，[0110]xt＝x(t)ꢀꢀꢀ(a)[0111][0112][0113][0114](a)式将当前状态变量x(t)作为优化过程中运动预测区间的初始值，表达式(b)表示机器人在移动区间遵循运动方程，表达式(c)表示状态约束条件，表达式(d)表示用于移动控制机器人的速度和各速度输入的限制范围。[0115]约束条件的第一部分为了使车辆轨迹和参考路径的误差最小，且使车辆的速度尽量接近参考速度，另速度和参考速度的差值、横向轨迹误差、方向误差最小，第二部分通过另角加速度和加速度尽可能地小，使车辆行驶更加平稳，尽量减小每一次驱动器的输入大小，第三部分为了使得机器人移动更加平滑，尽量减少相邻两次驱动器输入的角加速度和线加速度差距，第四部分为了控制移动机器人在通过地形崎岖路面时稳定通过，使得地图信息和机器人速度信息的融合量变化尽可能地小，当地图信息变化时，速度降低。[0116]表1mpc优化权重[0117][0118]mpc的优化权重设置为表1。优先考虑路径跟随性能，将优先顺序放在横向轨迹误差、方向误差上；为了快速行驶，权重体现在正向速度误差上。为了防止突然变化，考虑了偏航加速度和阻尼(jerk)的加权。最重要的，考虑到地图对移动机器人的影响体现在地图速度融合误差上。[0119]传统的mpc算法在二维地图平面上控制移动机器人进行轨迹跟踪，没有考虑地面起伏、需要跨越障碍物的情况，导致实际应用中，不能控制机器人平稳准确的跟随路径移动；本实施例在传统mpc的基础上，增加了地图信息变化量与速度相融合的代价，当机器人移动通过高度变化的地形时地图信息变化量会变大，在约束条件中地图信息变化量与速度融合信息固定不变，所以速度会降低来缓冲掉地图信息的变化量，使得机器人在通过高度变化路面时，降低通过的速度进而使得机器人移动更加平稳。[0120]实施例二：[0121]本实施例提供了崎岖起伏地形下机器人地形感知与移动控制系统，包括：[0122]点云地图建立模块，用于获取图像特征点，并进行求解优化，基于优化结果建立点云地图；[0123]2.5d代价地图生成模块，用于提取点云地图的可穿越性信息，生成2.5d代价地图；[0124]路径规划模块，用于基于改进的a*算法，在2.5d代价地图中生成带有障碍物可通过性信息的代价最小的路径，并进行全局路径规划和局部路径规划；[0125]机器人移动控制方案生成模块，用于通过带有高程约束信息的mpc控制算法，跟随局部路径进行轨迹跟踪，形成机器人移动控制方案。[0126]实施例三：[0127]本实施例提供了崎岖起伏地形下机器人地形感知与移动控制设备，包括存储器以及处理器；[0128]所述存储器用于存储相关的程序代码；[0129]所述处理器用于调用所述程序代码，执行实施例一所述的机器人地形感知与移动控制方法。[0130]实施例四：[0131]本实施例提供了一种计算机可读存储介质，所述计算机可读存储介质用于存储计算机程序，所述计算机程序用于执行实施例一所述的机器人地形感知与移动控制方法。[0132]以上所述仅为本技术的优选实施例而已，并不用于限制本技术，对于本领域的技术人员来说，本技术可以有各种更改和变化。凡在本技术的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本技术的保护范围之内。当前第1页12当前第1页12

技术特征：

1.崎岖起伏地形下机器人地形感知与移动控制方法，其特征在于，包括：获取图像特征点，并进行求解优化，基于优化结果建立点云地图；提取点云地图的可穿越性信息，得到2.5d代价地图；基于改进的a*算法，在2.5d代价地图中生成带有障碍物可通过性信息的代价最小的路径，并进行全局路径规划和局部路径规划；通过带有高程约束信息的mpc控制算法，跟随局部路径进行轨迹跟踪，形成机器人移动控制方案。2.根据权利要求1所述的崎岖起伏地形下机器人地形感知与移动控制方法，其特征在于，对图像进行特征点提取并进行匹配，利用纯运动优化平移矩阵和旋转矩阵，通过最小化重投影误差定位每帧图片；通过对观测误差进行求解优化管理局部地图并优化。3.根据权利要求2所述的崎岖起伏地形下机器人地形感知与移动控制方法，其特征在于，回环检测计算图像之间的相似性，提取每个图像的特征点进行相似性评分，并通过位姿图优化消除累计漂移误差；在位姿图优化消除误差后，基于全局ba算法计算整个系统最优结构和运动结果，输出机器人在相机坐标系下的位姿和全局下的稀疏点云信息。4.根据权利要求3所述的崎岖起伏地形下机器人地形感知与移动控制方法，其特征在于，将点云进行坐标变换到地图坐标系下，进行滤波和降采样；根据相机坐标系与地图坐标系的相对关系，得到对应的点云全局信息。5.根据权利要求1所述的崎岖起伏地形下机器人地形感知与移动控制方法，其特征在于，将凹陷到水平面以下的地形作为负障碍物，通过地形凹陷深度和坡度进行判断地形是否可通过，以控制机器人在复杂地形下移动；将三维动态点云地图进行体素滤波变为三维八叉树地图，以体素大小执行均匀采样以离散化该点云集合，生成采样点云集合；从采样点集中提取局部地形特征，作为代价以设定权重得出结果投影到相应的xy栅格地图中。6.根据权利要求1所述的崎岖起伏地形下机器人地形感知与移动控制方法，其特征在于，在带有地形代价信息2.5d地图下的a*路径规划算法为：在2.5d代价地图中结合移动机器人自身性能，生成一条带有障碍物可通过性信息的代价最小的路径，进行全局路径规划和局部路径规划，生成一条轨迹使机器人绕过或翻越障碍物；将地形代价信息加入到路径规划中，控制机器人按照代价判断是直接通过崎岖路面还是绕过崎岖路面选择相对路径长、但地面信息相对平坦的路径进行移动。7.根据权利要求1所述的崎岖起伏地形下机器人地形感知与移动控制方法，其特征在于，所述带有高程约束信息的mpc控制算法包括：获取当前机器人的状态和路径规划生成的参考路径，将地图坐标系转换为机器人坐标系，以机器人初始位置作为原点，以移动机器人的方向为x轴方向；建立矩阵方程，使用qr分解解得位置参数，使用多项式拟合规划所跟踪的路径；根据当前状态和路径预测轨迹，将轨迹划分成n个控制点，间隔时间为dt；将处理之后t+1时刻的输出量y
t+1
与上一阶段t时刻的产生的控制量u
t
进行组合，作为新的状态量进行输入，形成预测方程得到预测输出y
t+2
；
从第一个控制点完成之后，从第一步再递归进行，直到执行到y
t+n
时结束，n个输出结果构成了预测步长，n个控制量构成了控制步长。8.崎岖起伏地形下机器人地形感知与移动控制系统，其特征在于，包括：点云地图建立模块，用于获取图像特征点，并进行求解优化，基于优化结果建立点云地图；2.5d代价地图生成模块，用于提取点云地图的可穿越性信息，生成2.5d代价地图；路径规划模块，用于基于改进的a*算法，在2.5d代价地图中生成带有障碍物可通过性信息的代价最小的路径，并进行全局路径规划和局部路径规划；机器人移动控制方案生成模块，用于通过带有高程约束信息的mpc控制算法，跟随局部路径进行轨迹跟踪，形成机器人移动控制方案。9.崎岖起伏地形下机器人地形感知与移动控制设备，其特征在于，包括存储器以及处理器；所述存储器用于存储相关的程序代码；所述处理器用于调用所述程序代码，执行权利要求1-7任一项所述的机器人地形感知与移动控制方法。10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质用于存储计算机程序，所述计算机程序用于执行权利要求1-8任一项所述的机器人地形感知与移动控制方法。

技术总结

本发明公开了崎岖起伏地形下机器人地形感知与移动控制方法及系统，涉及机器人技术领域，包括获取图像特征点，并进行求解优化，基于优化结果建立点云地图；提取点云地图的可穿越性信息，得到2.5D代价地图；基于改进的A*算法，在2.5D代价地图中生成带有障碍物可通过性信息的代价最小的路径，并进行全局路径规划和局部路径规划；通过带有高程约束信息的MPC控制算法，跟随局部路径进行轨迹跟踪，形成机器人移动控制方案。本发明基于2.5D地图，实现机器人的路径规划和轨迹跟踪控制的自主移动；适用于大规模复杂环境，能够满足智能化和实用性的要求。要求。要求。