[发明专利]一种三维地形下轮式机器人作业路径平滑的改进lazy theta方法

权利要求书

1.一种三维地形下轮式机器人作业路径平滑的改进lazy theta方法，包括E_p表示电机效率损失的能量、E_h表示地面起伏时的势能、E_f表示滚动摩擦阻力消耗的能量、E_k表示速度变化和转弯消耗的能量，从起始节点到n节点的实际代价g(n)，从n节点到目标节点的预估代价h(n)，对于评估节点n的估价函数f(n)，有f(n)＝g(n)+h(n)，轮式机器人作业从起始节点到n节点的实际代价g(n)的计算能耗模型

其中，表示轮式移动机器人在作业过程中能耗受约于因素包括电机效率损失的能量，E_h＝mgΔh表示地面起伏时的势能，E_f＝μmgcosθS_cn表示滚动摩擦阻力消耗的能量，表示速度变化和转弯消耗的能量；η表示电机效率；P表示电机功率；S_cn表示从n节点的父节点到n节点之间的路程；v_n表示n节点的速度；v_n-1表示n节点的父节点的速度；m表示轮式移动机器人的质量；g表示重力加速度；Δh表示当前点到下个点的高度差；μ表示摩擦系数；θ表示坡道与水平面之间的夹角；I表示转动惯量；ω表示角速度，(x1,y1)和(x2,y2)表示两点坐标，open表中存储待被搜索的节点，close表中存储已被搜索过的节点，过(x1,y1)和(x2,y2)两点直线方程l，其特征在于，初始化当前n节点时，为其设立正确的父节点，祖父节点是指当前n节点的父节点的父节点；当搜索当前n节点时，检查当前n节点与其祖父节点之间是否存在障碍物，若不存在，则计算从其祖父节点到当前n节点的实际代价g1(n)，并与当前n节点本身的实际代价g(n)比较，若实际代价g1(n)g(n)，则将当前n节点的父节点替换为其祖父节点；若存在障碍物，则查询障碍物的端点，取距离当前n节点、当前n节点的祖父节点距离之和近的一端作为转折点，选择转折点周围8个节点中与当前n节点、当前n节点的祖父节点之间均无障碍物的点作为备选点，计算每个备选点到当前n节点的实际代价g1(n)，取备选点中实际代价g1(n)最小的点，若实际代价g1(n)g(n)，则将当前n节点的父节点替换为其祖父节点，否则不作替换，一种三维地形下轮式机器人作业路径平滑的改进lazy theta方法具体流程如下：

步骤一：初始化，当前节点n选择，遍历出在open表且不在close表中f(n)值最小的点设为当前节点n；

步骤二：判断当前节点是否有祖父节点，若当前节点是有祖父节点，继续往下执行，否则跳转至步骤九；

步骤三：计算当前节点n与其祖父节点两点直线距离上经过的栅格坐标，方法如下：

依据直线方程l，计算出这两点的连线在平面经过了哪些栅格，每个栅格的坐标是其左下角的坐标，栅格的坐标(x,y)为：

(x,y)＝(x1+b,y1+αa-β)

式中：令dx＝x2-x1，dy＝y2-y1，dxdy≠0

当dxdy0时，α＝1，β＝1

当dxdy0时，α＝-1，β＝0

a∈[1,|y2-y1|]，a∈正整数

(a-1)(|x2-x1|/|y2-y1|)ba(|x2-x1|/|y2-y1|)，且b∈正整数

三维地形中，地表距离不能直接按照欧几里得距离计算，将路线划分为多段，计算每段的长度并求和，得出地表距离，投影在XOY平面内的路线经过的栅格的坐标，将坐标代入方程l，从而求出轮式机器人克服摩擦力做的功，由高度差求出轮式机器人克服重力势能做的功，最终求出轮式机器人在该段路线能耗模型中消耗的能量；

步骤四：判断这些栅格中是否有障碍物，若栅格中是有障碍物，继续往下执行，否则计算n节点的祖父节点到当前n节点的实际代价g1(n)值，跳转至步骤六；

步骤五：重新选择祖父节点，查询障碍物的端点，取距离当前节点n与祖父节点距离之和近的一端作为转折点，选择转折点周围8个点中与当前节点n、祖父节点之间均无障碍物的点作为备选节点；

步骤六：计算各备选点到当前节点n的实际代价g1(n)，取g1(n)最小的备选点作当前节点n的祖父节点；

步骤七：将当前节点n的实际代价g(n)与g1(n)比较，若g(n)g1(n)，继续往下执行，否则跳转至步骤九；

步骤八：更新当前节点n的实际代价g(n)与评估函数f(n)，当前节点n的父节点替换为其祖父节点；

步骤九：将当前节点n放入close表中；

步骤十：判断是否满足算法结束条件，若当前节点n是目标节点，则寻路成功，或若open表已为空表，则寻路失败，算法程序结束；否则继续往下执行；

步骤十一：依次搜寻点n的8个邻近节点，如果某邻近节点，既没有在open表，也不在close表中，将其放入open表，计算出该邻近节点的评估函数f(n)值，并设父节点为n节点；如果已在open表中，判断其实际代价g(n)值是否更小，若是，则用更小的实际代价g(n)值替换原来的实际代价g(n)值，同时重新计算预估代价h(n)值和评估函数f(n)值，并设父节点为n节点，否则查看其它临近节点；

步骤十二：所有临近节点查看完毕后，跳回步骤一。