CN112015183B - 一种能耗约束下在具有凹凸地形中移动机器人避障方法 - Google Patents

Abstract

一种能耗约束下在具有凹凸地形中移动机器人避障方法包括电机功率运行能耗，高度差造成的能耗，摩擦阻力造成的能耗，速度变化造成的能耗，转弯造成的能耗，加载移动机器人工作地的地图，定义移动机器人工作地的地图中障碍，绘出地图，遍历出A*星算法总代价值最小点设为下一个点的子模块，计算起点到当前点的新实际代价值G’子模块，终止条件判断；根据构图工作地的地图，判断移动机器人下一个点移动是否障碍，是否穿过三维构图的分割线，是否爬坡，是否转弯；构建能耗实际代价值模型，获得能耗约束下在具有凹凸地形中移动机器人避障作业路线和显示总能耗。它概念简单，实现方便，鲁棒性高，利于满足移动机器人作业的避障和能耗管理的现实需求。

Description

一种能耗约束下在具有凹凸地形中移动机器人避障方法

技术领域

本发明涉及移动机器人避障方法技术领域，尤其涉及一种能耗约束下在具有凹凸地形中移动机器人避障方法。

背景技术

机器人避障方法是根据任务要求及环境避障限制等约束，对机器人在笛卡尔空间或关节空间进行轨迹及运动规律的作业轨迹规划规划；此外，机器人作业过程中普遍需要较高的能耗，故能耗策略研究成为许多工业机器人制造商和学术研究团体的公同关注课题。

目前，研究能耗约束下在移动机器人避障方法，如遗传算法、基于梯度的优化算法、克隆算法、快速扩展随机树法、人工势场法等不同方法，这些具有不同的适用范围和求解特性。但存在移动机器人实际工作场景被理想化，不考虑实际中的具有凹凸地形耗能，不考虑实际移动过程转弯耗能，致使能耗约束下在移动机器人避障方法偏离实际情况，使得能耗约束参考价值降低。

发明内容

为了克服现有研究能耗约束下在移动机器人避障方法研究问题时，移动机器人实际工作场景被理想化，没有考虑实际中的具有凹凸地形耗能和实际移动过程转弯耗能，致使能耗约束下在移动机器人避障方法偏离实际情况，使得能耗约束参考价值降低等缺陷，本发明提供一种能耗约束下在具有凹凸地形中移动机器人避障方法。

本发明具体解决其技术问题所采用的技术解决方案是：

一种能耗约束下在具有凹凸地形中移动机器人避障方法，包括电机功率运行能耗，高度差造成的能耗，摩擦阻力造成的能耗，速度变化造成的能耗，转弯造成的能耗，加载移动机器人工作地的地图，定义移动机器人工作地的地图中障碍，绘出地图，遍历出A*星算法总代价值最小点设为下一个点的子模块，计算起点到当前点的新实际代价值G’子模块，终止条件判断。一种能耗约束下在具有凹凸地形中移动机器人避障方法，构建能耗实际代价值模型，基于A*星算法总代价值F＝G+H进行每个移动方向进行枚举，其中能耗实际代价值为G，从当前点到终点的预估代价值为H，最终获得能耗约束下在具有凹凸地形中移动机器人避障作业路线和显示总能耗。

能耗实际代价值模型如下：

G＝Ep+Ev+Ef+Ek1+Ek2

其中，Ep＝p*(Scn/(vn-vc))，记Ep表示电机功率运行能耗，p为电机功率,Scn为当前点到下个点行走的距离，vn为更新相应的下一个点速度，vc为当前点速度；

Ev＝m*g*Δh，记Ev表示高度差造成的能耗，Δh根据构图工作地的地图构图情况分别取Hv、Hv1、Hv2，Hv为计算当前点到下个点的高度差，Hv1为当前点的高度和前点与下个点中间的高度之间高度差，Hv2为前点与下个点中间的高度和下个点的高度之间高度差，在构图工作地的地图构图无分割线情况时取Δh＝Hv；在构图工作地的地图构图有分割线情况时，分割线前半段取Δh＝Hv1，分割线后半段取Δh＝Hv2；

Ef＝u*m*g*cos(slope)*Scn，记Ef表示摩擦阻力造成的能耗，其中slope为凹凸坡度角；u为地形中地面摩擦系数，m为移动机器人质量，g为重力加速度；

Ek1＝0.5*m*(vn²-vc²)，记Ek1表示速度变化造成的能耗；

Ek2＝0.5*I*(wn²-wc²)，记Ek2表示转弯造成的能耗，I为转动惯量，wn为更新相应的下一个点角速度，wc为当前点角速度。

能耗约束下在具有凹凸地形中移动机器人避障方法流程如下：

步骤1.1：初始化，加载移动机器人工作地的地图；

步骤1.2：定义移动机器人工作地的地图中障碍；

步骤1.3：基本参数地面摩擦系数、机器人电机功率、openlist开放表、closelist禁忌表设定，绘出地图；

步骤1.4：Num为当前搜索值，初始化无穷大inf，设置Num＝inf；

步骤1.5：遍历出A*星算法总代价值最小点设为下一个点的子模块；

步骤1.6：标记为已访问，即openlist(Nextpoint(1),Nextpoint(2))＝1；

步骤1.7：能耗约束下在具有凹凸地形中移动机器人避障方法终止条件判断，当下一点坐标(Nextpoint(1),Nextpoint(2))和终点坐标(Spoint(1),Spoint(2))重合，即Nextpoint(1)＝＝Spoint(1)且Nextpoint(2)＝＝Spoint(2)，或者无法抵达终点，即Num＝inf，则能耗约束下在具有凹凸地形中移动机器人避障方法终止，绘出约束条件的移动路线，程序终止；否则继续往下执行；

步骤1.8：计算一个点移动的实际代价值，下一个点移动，对横坐标Nextpoint(1)有-1,0,1三种情况，对纵坐标Nextpoint(2)有-1,0,1三种情况，横坐标和纵坐标的变化构成双重循环，取i＝1至3，j＝1至3，在每个移动方向进行枚举，设过渡参数dk，其值等于对应i和j下的起点到当前点的实际代价值，记为dk＝G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)；

步骤1.9：下一个点移动状态判断，如果下一个点移动是原来点，即i＝＝2且j＝＝2，或者下一个点移动是障碍点，即closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)＝＝1，则不更新起点到当前点的实际代价值，继续下一个点移动的每个移动方向进行枚举，否则继续往下执行；

步骤1.10：计算起点到当前点的新实际代价值G’子模块；

步骤1.11：对比原来的实际代价值dk和起点到当前点的新实际代价值G’，如果G’<dk，则更新实际代价值dk，即实际代价值dk＝G’，更新A*星算法总代价值F，即F＝dk+H，其中从当前点到终点的预估代价值为H，继续往下执行；否则继续下一个点移动的每个移动方向进行枚举；

步骤1.12：更新前继点的横坐标和纵坐标，即

parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)＝Nextpoint(1)

parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)＝Nextpoint(2)；

步骤1.13：判断下一个点移动的每个移动方向进行枚举是否完成，枚举完成结束条件是j>3且i>3，如果不满足枚举完成结束条件，则继续下一个点移动的每个移动方向进行枚举，直至满足枚举完成结束条件，然后直接跳转至步骤1.1，最后直至满足能耗约束下在具有凹凸地形中移动机器人避障方法的终止条件判断，绘出约束条件的移动全部路线，程序结束。

遍历A*星算法总代价值最小点设为下一个点的子模块流程如下：

步骤2.1：机器人开始从起点遍历在openlist开放表中，且不在clostlist禁忌表中每一个点；

步骤2.2：搜索当前点[p,q]，进行判断，当点[p,q]满足是openlist开放表中，即openlist(p,q)＝＝0，且点[p,q]满足不是clostlist禁忌表中，即closelist(p,q)～＝1时，则点[p,q]符合条件输出点不是障碍点，即Outpoint＝[p,q]；否则直接跳转至步骤2.4；

步骤2.3：设F为A*星算法总代价值，是起点到当前点的实际代价值和从当前点到终点的预估代价值之和，令起点到当前点的实际代价值为G，从当前点到终点的预估代价值为H，即F＝G+H,点[p,q]的总代价值记为F(p,q)，当总代价值大于或等于0，即F(p,q)>＝0，且总代价值小于当前搜索值Num，即Num>F(p,q)，则令当前阈值为总代价值，即Num＝F(p,q)，设下一点为当前点，即Nextpoint＝[p,q]，继续往下执行；否则直接执行步骤2.4；

步骤2.4：判断是否遍历所有点，如果已遍历所有点，则本子模块结束；否则返回至步骤2.1。

计算起点到当前点的新实际代价值G’子模块流程如下：

步骤3.1：判断移动机器人是否转弯，如果转弯角度theta＝0，则不转弯，继续往下执行；如果转弯角度theta>0，则转弯，跳转至步骤3.9；

步骤3.2：根据构图工作地的地图构图原理，判断是否穿过三维构图的分割线，如果左上或右下，即i＝1&&j＝1或i＝3&&j＝3，则表示穿过三维构图的分割线，则继续往下执行，否则跳转至步骤3.7；

步骤3.3：计算当前点到下个点的高度差Hv，当前点的高度和前点与下个点中间的高度之间高度差Hv1，前点与下个点中间的高度和下个点的高度之间高度差Hv2，当前点到下个点行走的距离Scn；

步骤3.4：判断分割线前半段是否爬坡，如果满足Hv1>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；如果不满足Hv1>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；

步骤3.5：判断分割线后半段是否爬坡，如果满足Hv2>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；如果不满足Hv2>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；

步骤3.6：计算高度差造成的能耗Ev，摩擦阻力造成的能耗Ef，跳转至步骤3.6；

步骤3.7：计算当前点到下个点的高度差Hv，当前点到下个点行走的距离Scn；

步骤3.8：判断是否爬坡，如果满足Hv>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1，摩擦阻力造成的能耗Ef，高度差造成的能耗Ev，跳转至步骤3.16；如果不满足Hv>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1，摩擦阻力造成的能耗Ef，高度差造成的能耗Ev，跳转至步骤3.16；

步骤3.9：根据构图工作地的地图构图原理，判断是否穿过三维构图的分割线，如果左上或右下，即i＝1&&j＝1或i＝3&&j＝3，则表示穿过三维构图的分割线，则继续往下执行，否则跳转至步骤3.14；

步骤3.10：计算当前点到下个点的高度差Hv，当前点的高度和前点与下个点中间的高度之间高度差Hv1，前点与下个点中间的高度和下个点的高度之间高度差Hv2，当前点到下个点行走的距离Scn，转弯角度theta；

步骤3.11：判断分割线前半段是否爬坡，如果满足Hv1>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；如果不满足Hv1>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；

步骤3.12：判断分割线后半段是否爬坡，如果满足Hv2>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；如果不满足Hv2>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；

步骤3.13：计算高度差造成的能耗Ev，摩擦阻力造成的能耗Ef，跳转至步骤3.16；

步骤3.14：计算当前点到下个点的高度差Hv，当前点到下个点行走的距离Scn，转弯角度theta；

步骤3.15：判断是否爬坡，如果满足Hv>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1，转弯造成的能耗Ek2，摩擦阻力造成的能耗Ef，高度差造成的能耗Ev，跳转至步骤3.16；如果不满足Hv>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1，转弯造成的能耗Ek2，摩擦阻力造成的能耗Ef，高度差造成的能耗Ev；

步骤3.16：计算起点到当前点的新实际代价值G’；则G’＝Ep+Ek1+Ek2+Ef+Ev。

本发明的有益效果是，采用能耗约束下在具有凹凸地形中移动机器人避障方法，通过A*星算法，解决了研究能耗约束下在移动机器人避障方法研究问题时，移动机器人实际工作场景被理想化，没有考虑实际中的具有凹凸地形耗能和实际移动过程转弯耗能，致使能耗约束下在移动机器人避障方法偏离实际情况，使得能耗约束参考价值降低等问题。它概念简单，实现方便，鲁棒性高，适应于移动机器人作业的避障和能耗管理的现实需求。

附图说明

图1为本发明能耗约束下在具有凹凸地形中移动机器人避障方法流程图；

图2为本发明能耗约束下在具有凹凸地形中移动机器人避障方法中遍历A*星算法总代价值最小点设为下一个点的子模块流程流程图；

图3为本发明能耗约束下在具有凹凸地形中移动机器人避障方法中计算起点到当前点的新实际代价值G’子模块流程图。

具体实施方式

下面结合附图和实施例对本发明专利作进一步地说明：

本发明所提供的一种能耗约束下在具有凹凸地形中移动机器人避障方法，包括电机功率运行能耗，高度差造成的能耗，摩擦阻力造成的能耗，速度变化造成的能耗，转弯造成的能耗，加载移动机器人工作地的地图，定义移动机器人工作地的地图中障碍，绘出地图，遍历出A*星算法总代价值最小点设为下一个点的子模块，计算起点到当前点的新实际代价值G’子模块，终止条件判断。一种能耗约束下在具有凹凸地形中移动机器人避障方法构建能耗实际代价值模型，基于A*星算法总代价值F＝G+H进行每个移动方向进行枚举，其中能耗实际代价值为G，从当前点到终点的预估代价值为H，获得能耗约束下在具有凹凸地形中移动机器人避障作业路线和显示总能耗。

能耗实际代价值模型如下：

G＝Ep+Ev+Ef+Ek1+Ek2

其中，Ep＝p*(Scn/(vn-vc))，记Ep表示电机功率运行能耗，p为电机功率,Scn为当前点到下个点行走的距离，vn为更新相应的下一个点速度，vc为当前点速度；

Ev＝m*g*Δh，记Ev表示高度差造成的能耗，Δh根据构图工作地的地图构图情况分别取Hv、Hv1、Hv2，Hv为计算当前点到下个点的高度差，Hv1为当前点的高度和前点与下个点中间的高度之间高度差，Hv2为前点与下个点中间的高度和下个点的高度之间高度差，在构图工作地的地图构图无分割线情况时取Δh＝Hv；在构图工作地的地图构图有分割线情况时，分割线前半段取Δh＝Hv1，分割线后半段取Δh＝Hv2；

Ef＝u*m*g*cos(slope)*Scn，记Ef表示摩擦阻力造成的能耗，其中slope为凹凸坡度角；u为地形中地面摩擦系数，m为移动机器人质量，g为重力加速度；

Ek1＝0.5*m*(vn²-vc²)，记Ek1表示速度变化造成的能耗；

Ek2＝0.5*I*(wn²-wc²)，记Ek2表示转弯造成的能耗，I为转动惯量，wn为更新相应的下一个点角速度，wc为当前点角速度。

能耗约束下在具有凹凸地形中移动机器人避障方法流程如下：

步骤1.1：初始化，加载移动机器人工作地的地图；

步骤1.2：定义移动机器人工作地的地图中障碍；

步骤1.3：基本参数地面摩擦系数、机器人电机功率、openlist开放表、closelist禁忌表设定，绘出地图；

步骤1.4：Num为当前搜索值，初始化无穷大inf，设置Num＝inf；

步骤1.5：遍历出A*星算法总代价值最小点设为下一个点的子模块；

步骤1.6：标记为已访问，即openlist(Nextpoint(1),Nextpoint(2))＝1；

步骤1.7：能耗约束下在具有凹凸地形中移动机器人避障方法终止条件判断，当下一点坐标(Nextpoint(1),Nextpoint(2))和终点坐标(Spoint(1),Spoint(2))重合，即Nextpoint(1)＝＝Spoint(1)且Nextpoint(2)＝＝Spoint(2)，或者无法抵达终点，即Num＝inf，则能耗约束下在具有凹凸地形中移动机器人避障方法终止，绘出约束条件的移动路线，程序终止；否则继续往下执行；

步骤1.8：计算一个点移动的实际代价值，下一个点移动，对横坐标Nextpoint(1)有-1,0,1三种情况，对纵坐标Nextpoint(2)有-1,0,1三种情况，横坐标和纵坐标的变化构成双重循环，取i＝1至3，j＝1至3，在每个移动方向进行枚举，设过渡参数dk，其值等于对应i和j下的起点到当前点的实际代价值，记为dk＝G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)；

步骤1.9：下一个点移动状态判断，如果下一个点移动是原来点，即i＝＝2且j＝＝2，或者下一个点移动是障碍点，即closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)＝＝1，则不更新起点到当前点的实际代价值，继续下一个点移动的每个移动方向进行枚举，否则继续往下执行；

步骤1.10：计算起点到当前点的新实际代价值G’子模块；

步骤1.11：对比原来的实际代价值dk和起点到当前点的新实际代价值G’，如果G’<dk，则更新实际代价值dk，即实际代价值dk＝G’，更新A*星算法总代价值F，即F＝dk+H，其中从当前点到终点的预估代价值为H，继续往下执行；否则继续下一个点移动的每个移动方向进行枚举；

步骤1.12：更新前继点的横坐标和纵坐标，即

parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)＝Nextpoint(1)

parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)＝Nextpoint(2)；

步骤1.13：判断下一个点移动的每个移动方向进行枚举是否完成，枚举完成结束条件是j>3且i>3，如果不满足枚举完成结束条件，则继续下一个点移动的每个移动方向进行枚举，直至满足枚举完成结束条件，然后直接跳转至步骤1.5，最后直至满足能耗约束下在具有凹凸地形中移动机器人避障方法的终止条件判断，绘出约束条件的移动全部路线，程序结束。

遍历A*星算法总代价值最小点设为下一个点的子模块流程如下：

步骤2.1：机器人开始从起点遍历在openlist开放表中，且不在clostlist禁忌表中每一个点；

步骤2.2：搜索当前点[p,q]，进行判断，当点[p,q]满足是openlist开放表中，即openlist(p,q)＝＝0，且点[p,q]满足不是clostlist禁忌表中，即closelist(p,q)～＝1时，则点[p,q]符合条件输出点不是障碍点，即Outpoint＝[p,q]；否则直接跳转至步骤2.4；

步骤2.3：设F为A*星算法总代价值，是起点到当前点的实际代价值和从当前点到终点的预估代价值之和，令起点到当前点的实际代价值为G，从当前点到终点的预估代价值为H，即F＝G+H,点[p,q]的总代价值记为F(p,q)，当总代价值大于或等于0，即F(p,q)>＝0，且总代价值小于当前搜索值Num，即Num>F(p,q)，则令当前阈值为总代价值，即Num＝F(p,q)，设下一点为当前点，即Nextpoint＝[p,q]，继续往下执行；否则直接执行步骤2.4；

步骤2.4：判断是否遍历所有点，如果已遍历所有点，则本子模块结束；否则返回至步骤2.1。

计算起点到当前点的新实际代价值G’子模块流程如下：

步骤3.1：判断移动机器人是否转弯，如果转弯角度theta＝0，则不转弯，继续往下执行；如果转弯角度theta>0，则转弯，跳转至步骤3.9；

步骤3.2：根据构图工作地的地图构图原理，判断是否穿过三维构图的分割线，如果左上或右下，即i＝1&&j＝1或i＝3&&j＝3，则表示穿过三维构图的分割线，则继续往下执行，否则跳转至步骤3.7；

步骤3.3：计算当前点到下个点的高度差Hv，当前点的高度和前点与下个点中间的高度之间高度差Hv1，前点与下个点中间的高度和下个点的高度之间高度差Hv2，当前点到下个点行走的距离Scn；

步骤3.4：判断分割线前半段是否爬坡，如果满足Hv1>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；如果不满足Hv1>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；

步骤3.5：判断分割线后半段是否爬坡，如果满足Hv2>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；如果不满足Hv2>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；

步骤3.6：计算高度差造成的能耗Ev，摩擦阻力造成的能耗Ef，跳转至步骤3.16；

步骤3.7：计算当前点到下个点的高度差Hv，当前点到下个点行走的距离Scn；

步骤3.8：判断是否爬坡，如果满足Hv>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1，摩擦阻力造成的能耗Ef，高度差造成的能耗Ev，跳转至步骤3.16；如果不满足Hv>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1，摩擦阻力造成的能耗Ef，高度差造成的能耗Ev，跳转至步骤3.16；

步骤3.9：根据构图工作地的地图构图原理，判断是否穿过三维构图的分割线，如果左上或右下，即i＝1&&j＝1或i＝3&&j＝3，则表示穿过三维构图的分割线，则继续往下执行，否则跳转至步骤3.14；

步骤3.10：计算当前点到下个点的高度差Hv，当前点的高度和前点与下个点中间的高度之间高度差Hv1，前点与下个点中间的高度和下个点的高度之间高度差Hv2，当前点到下个点行走的距离Scn，转弯角度theta；

步骤3.11：判断分割线前半段是否爬坡，如果满足Hv1>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；如果不满足Hv1>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；

步骤3.12：判断分割线后半段是否爬坡，如果满足Hv2>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；如果不满足Hv2>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；

步骤3.13：计算高度差造成的能耗Ev，摩擦阻力造成的能耗Ef，跳转至步骤3.6；

步骤3.14：计算当前点到下个点的高度差Hv，当前点到下个点行走的距离Scn，转弯角度theta；

步骤3.15：判断是否爬坡，如果满足Hv>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1，转弯造成的能耗Ek2，摩擦阻力造成的能耗Ef，高度差造成的能耗Ev，跳转至步骤3.16；如果不满足Hv>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1，转弯造成的能耗Ek2，摩擦阻力造成的能耗Ef，高度差造成的能耗Ev；

步骤1.6：计算起点到当前点的新实际代价值G’；则G’＝Ep+Ek1+Ek2+Ef+Ev。

Claims (3)

1.一种能耗约束下在具有凹凸地形中移动机器人避障方法，包括电机功率运行能耗，高度差造成的能耗，摩擦阻力造成的能耗，速度变化造成的能耗，转弯造成的能耗，加载移动机器人工作地的地图，定义移动机器人工作地的地图中障碍，绘出地图，遍历出A*星算法总代价值最小点设为下一个点的子模块，计算起点到当前点的新实际代价值G’子模块，终止条件判断，其特征在于，构建能耗实际代价值模型，基于A*星算法总代价值F＝G+H进行每个移动方向进行枚举，能耗实际代价值为G，从当前点到终点的预估代价值为H，获得能耗约束下在具有凹凸地形中移动机器人避障作业路线和显示总能耗；能耗实际代价值模型如下

G＝Ep+Ev+Ef+Ek1+Ek2

Ep＝p*(Scn/(vn-vc))，记Ep表示电机功率运行能耗，p为电机功率,Scn为当前点到下个点行走的距离，vn为更新相应的下一个点速度，vc为当前点速度；Ev＝m*g*Δh，记Ev表示高度差造成的能耗，Δh根据构图工作地的地图构图情况分别取Hv、Hv1、Hv2，Hv为计算当前点到下个点的高度差，Hv1为当前点的高度和前点与下个点中间的高度之间高度差，Hv2为前点与下个点中间的高度和下个点的高度之间高度差，在构图工作地的地图构图无分割线情况时取Δh＝Hv；在构图工作地的地图构图有分割线情况时，分割线前半段取Δh＝Hv1，分割线后半段取Δh＝Hv2；Ef＝u*m*g*cos(slope)*Scn，记Ef表示摩擦阻力造成的能耗，slope为凹凸坡度角；u为地形中地面摩擦系数，m为移动机器人质量，g为重力加速度；Ek1＝0.5*m*(vn²-vc²)，记Ek1表示速度变化造成的能耗；Ek2＝0.5*I*(wn²-wc²)，记Ek2表示转弯造成的能耗，I为转动惯量，wn为更新相应的下一个点角速度，wc为当前点角速度；能耗约束下在具有凹凸地形中移动机器人避障方法流程如下：

步骤1.1：初始化，加载移动机器人工作地的地图；

步骤1.2：定义移动机器人工作地的地图中障碍；

步骤1.3：基本参数地面摩擦系数、机器人电机功率、openlist开放表、closelist禁忌表设定，绘出地图；

步骤1.4：Num为当前搜索值，初始化无穷大inf，设置Num＝inf；

步骤1.5：遍历A*星算法总代价值最小点设为下一个点的子模块；

步骤1.6：标记为已访问，即openlist(Nextpoint(1),Nextpoint(2))＝1；

步骤1.7：能耗约束下在具有凹凸地形中移动机器人避障方法终止条件判断，当下一点坐标(Nextpoint(1),Nextpoint(2))和终点坐标(Spoint(1),Spoint(2))重合，即Nextpoint(1)＝＝Spoint(1)且Nextpoint(2)＝＝Spoint(2)，或者无法抵达终点，即Num＝inf，则能耗约束下在具有凹凸地形中移动机器人避障方法终止，绘出约束条件的移动路线，程序终止；否则继续往下执行；

步骤1.8：计算一个点移动的实际代价值，下一个点移动，对横坐标Nextpoint(1)有-1,0,1三种情况，对纵坐标Nextpoint(2)有-1,0,1三种情况，横坐标和纵坐标的变化构成双重循环，取i＝1至3，j＝1至3，在每个移动方向进行枚举，设过渡参数dk，其值等于对应i和j下的起点到当前点的实际代价值，记为dk＝G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)；

步骤1.9：下一个点移动状态判断，如果下一个点移动是原来点，即i＝＝2且j＝＝2，或者下一个点移动是障碍点，即closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)＝＝1，则不更新起点到当前点的实际代价值，继续下一个点移动的每个移动方向进行枚举，否则继续往下执行；

步骤1.10：计算起点到当前点的新实际代价值G’子模块；

步骤1.11：对比原来的实际代价值dk和起点到当前点的新实际代价值G’，如果G’<dk，则更新实际代价值dk，即实际代价值dk＝G’，更新A*星算法总代价值F，即F＝dk+H，其中从当前点到终点的预估代价值为H，继续往下执行；否则继续下一个点移动的每个移动方向进行枚举；

步骤1.12：更新前继点的横坐标和纵坐标，即

parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)＝Nextpoint(1)

parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)＝Nextpoint(2)；

步骤1.13：判断下一个点移动的每个移动方向进行枚举是否完成，枚举完成结束条件是j>3且i>3，如果不满足枚举完成结束条件，则继续下一个点移动的每个移动方向进行枚举，直至满足枚举完成结束条件，然后直接跳转至步骤1.5，最后直至满足能耗约束下在具有凹凸地形中移动机器人避障方法的终止条件判断，绘出约束条件的移动全部路线，程序结束。

2.根据权利要求1所述的一种能耗约束下在具有凹凸地形中移动机器人避障方法，其特征在于，所述遍历A*星算法总代价值最小点设为下一个点的子模块，流程如下

步骤2.1：机器人开始从起点遍历在openlist开放表中，且不在clostlist禁忌表中每一个点；

步骤2.2：搜索当前点[p,q]，进行判断，当点[p,q]满足是openlist开放表中，即openlist(p,q)＝＝0，且点[p,q]满足不是clostlist禁忌表中，即closelist(p,q)～＝1时，则点[p,q]符合条件输出点不是障碍点，即Outpoint＝[p,q]；否则直接跳转至步骤2.4；

步骤2.3：设F为A*星算法总代价值，是起点到当前点的实际代价值和从当前点到终点的预估代价值之和，令起点到当前点的实际代价值为G，从当前点到终点的预估代价值为H，即F＝G+H,点[p,q]的总代价值记为F(p,q)，当总代价值大于或等于0，即F(p,q)>＝0，且总代价值小于当前搜索值Num，即Num>F(p,q)，则令当前阈值为总代价值，即Num＝F(p,q)，设下一点为当前点，即Nextpoint＝[p,q]，继续往下执行；否则直接执行步骤2.4；

步骤2.4：判断是否遍历所有点，如果已遍历所有点，则本子模块结束；否则返回至步骤2.1。

3.根据权利要求1所述的一种能耗约束下在具有凹凸地形中移动机器人避障方法，其特征在于，所述计算起点到当前点的新实际代价值G’子模块，流程如下

步骤3.1：判断移动机器人是否转弯，如果转弯角度theta＝0，则不转弯，继续往下执行；如果转弯角度theta>0，则转弯，跳转至步骤3.9；

步骤3.2：根据构图工作地的地图构图原理，判断是否穿过三维构图的分割线，如果左上或右下，即i＝1&&j＝1或i＝3&&j＝3，则表示穿过三维构图的分割线，则继续往下执行，否则跳转至步骤3.7；

步骤3.3：计算当前点到下个点的高度差Hv，当前点的高度和前点与下个点中间的高度之间高度差Hv1，前点与下个点中间的高度和下个点的高度之间高度差Hv2，当前点到下个点行走的距离Scn；

步骤3.4：判断分割线前半段是否爬坡，如果满足Hv1>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；如果不满足Hv1>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；

步骤3.5：判断分割线后半段是否爬坡，如果满足Hv2>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；如果不满足Hv2>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；

步骤3.6：计算高度差造成的能耗Ev，摩擦阻力造成的能耗Ef，跳转至步骤3.16；

步骤3.7：计算当前点到下个点的高度差Hv，当前点到下个点行走的距离Scn；

步骤3.8：判断是否爬坡，如果满足Hv>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1，摩擦阻力造成的能耗Ef，高度差造成的能耗Ev，跳转至步骤3.16；如果不满足Hv>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1，摩擦阻力造成的能耗Ef，高度差造成的能耗Ev，跳转至步骤3.16；

步骤3.9：根据构图工作地的地图构图原理，判断是否穿过三维构图的分割线，如果左上或右下，即i＝1&&j＝1或i＝3&&j＝3，则表示穿过三维构图的分割线，则继续往下执行，否则跳转至步骤3.14；

步骤3.10：计算当前点到下个点的高度差Hv，当前点的高度和前点与下个点中间的高度之间高度差Hv1，前点与下个点中间的高度和下个点的高度之间高度差Hv2，当前点到下个点行走的距离Scn，转弯角度theta；

步骤3.11：判断分割线前半段是否爬坡，如果满足Hv1>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；如果不满足Hv1>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；

步骤3.12：判断分割线后半段是否爬坡，如果满足Hv2>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；如果不满足Hv2>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1；

步骤3.13：计算高度差造成的能耗Ev，摩擦阻力造成的能耗Ef，跳转至步骤3.16；

步骤3.14：计算当前点到下个点的高度差Hv，当前点到下个点行走的距离Scn，转弯角度theta；

步骤3.15：判断是否爬坡，如果满足Hv>0，则爬坡，更新相应的下一个点速度vn，当前点速度vc，计算电机功率p,及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1，转弯造成的能耗Ek2，摩擦阻力造成的能耗Ef，高度差造成的能耗Ev，跳转至步骤3.16；如果不满足Hv>0，则非爬坡，更新相应的下一个点速度vn，当前点速度vc，及相应电机恒功率或恒转矩运行消耗的能量Ep，速度变化造成的能耗Ek1，转弯造成的能耗Ek2，摩擦阻力造成的能耗Ef，高度差造成的能耗Ev；

步骤3.16：计算起点到当前点的新实际代价值G’；则G’＝Ep+Ek1+Ek2+Ef+Ev。

Images