一种基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法与流程

本发明属于机器人轨迹规划领域，涉及一种基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法。

背景技术：

常用的机器人运动轨迹规划方法有图论方法、人工势场法、搜索类方法、快速扩展随机树方法、最优控制方法等。相比于提及的其他几类方法，最优控制方法由于基于机器人的运动学(或动力学)方程，使得规划的轨迹具有天然的可行性。此外，最优控制方法具有简单的数学结构，控制饱和、障碍规避等各类约束可以在统一的框架下进行考虑。机器人实际的作业环境中可能存在着众多的障碍，利用最优控制方法进行求解时将导致非线性程度极高的优化问题，使得求解过程的鲁棒性与效率严重依赖于初始猜测的品质。由于机器人在轨迹规划完成前必须处于精致的等待状态，若求解效率慢或失败，将影响整个作业任务的进程。这一客观事实限制了最优控制方法在实际机器人轨迹规划求解问题中的应用。为此，有必要发展高效的初始化策略，以提高最优控制方法求解复杂障碍环境下机器人轨迹规划问题的性能。

技术实现要素：

为了解决上述技术问题，本发明利用同伦的思想，提出了一种基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法。本方法将复杂障碍环境下机器人的轨迹规划形成的原始非线性最优控制问题，转化为一系列相对容易求解的子问题的迭代。在初代子问题中，障碍被缩小到较小的尺寸，随着子问题代数的增加，障碍尺寸逐渐增大，并在末代子问题达到障碍物的真实尺寸。每一代子问题的解被用作下一代子问题的初解，相当于构造了一种持续的“热启动”方式，保证了最优控制问题求解的鲁棒性与效率。

为了达到上述目的，本发明采用的技术方案为：

一种基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法，首先，根据机器人的结构特性建立相应的运动方程、约束条件，用特征圆描述其轮廓。其次，根据作业环境中的障碍信息，用特征圆描述障碍并设定安全距离，从而建立机器人作业过程中的避障条件。第三，确定机器人轨迹规划问题对应的初始边界条件与终端边界条件。第四，根据运动方程、约束条件、初始条件、运动时间，将轨迹规划问题转化为以最小化能量消耗的最优控制问题。最后，利用同伦策略将构造的最优控制问题转化为一系列子问题，每个子问题中障碍的尺寸相比于前一个子问题不断增大，并使用当前子问题的解作为下一个子问题的初始猜测。包括以下步骤：

步骤1：构建机器人相关信息

步骤1-1：建立机器人的运动方程

建立描述机器人运动状态的状态空间x，并确定对应的控制变量u，从而建立如下的机器人运动方程(可以是运动学方程或动力学方程)：

其中，t为时间变量。

步骤1-2：确定机器人作业过程中的状态变量和控制变量的约束

机器人作业过程中出于安全考虑，可能对状态变量施加特定的约束条件，本发明将相关的状态变量约束写作如下不等式形式：

cs(x,t)≤0(2)

同时，为防止控制饱和，应对控制变量施加特定的约束，本发明将相关的控制变量约束写作如下不等式形式：

cc(u,t)≤0(3)

步骤1-3：用特征圆描述机器人的轮廓

记机器人特征圆的圆心为o(xc,yc)，半径为rc，则特征圆可以用如下方程表示：

其中，x和y为分别为横纵坐标。

步骤2：建立障碍规避条件

步骤2-1：用特征圆描述作业环境中的障碍

记作业环境中一共有n个障碍，对于第k个障碍，记其特征圆的圆心为ok(xk,yk)，半径为rk，则特征圆可以用如下方程表示

步骤2-2：设定安全距离以建立避障条件

设定机器人与作业环境中每个障碍的最小安全距离为rs，则机器人与第k个障碍的避障条件表示为

将公式(6)中n个约束中不等号左端的表达式写作如下的矩阵：

则全部n个避障条件可以表示为如下的矩阵形式：

h(x,t)≤0(8)

为利用障碍尺寸同伦策略实现对轨迹规划问题的稳定高效求解，在矩阵h(x,t)的基础上构造如下的弱化避障约束矩阵：

其中，参数ε∈(0，1]为一缩放系数，代表同伦策略中的同伦因子。显然，

步骤3：确定轨迹规划问题的边界条件

记轨迹规划问题的初始时刻和终端时刻分别为ts与tf，机器人在初始时刻与终端时刻的状态分别为xs与xf，则轨迹规划问题的边界条件为：

x(ts)＝xs,x(tf)＝xf(10)

步骤4：建立最优控制模型

根据公式(10)中的边界条件、公式(1)中的系统方程以及公式(2)、(3)、(6)中的约束条件，建立如下的能量最优控制问题：

其中，j为最优控制问题的性能指标；表示机器人运动过程中的能量消耗；

步骤5：利用同伦策略求解最优控制问题

步骤5-1：设置同伦因子列表为满足0＜α1＜α2＜…＜αm＝1，m为同伦策略实施过程中采用的同伦因子的个数。

步骤5-2：求解过程初始化

记p为同伦因子索引，并初始化同伦因子指标p＝0。记最优控制问题状态变量和控制变量的初始猜测分别为x^[0]与u^[0]。

步骤5-3：更新同伦因子索引为p＝p+1。

步骤5-4：构造子问题并求解

构造如下的子问题：

利用x^[p-1]与u^[p-1]作为状态变量与控制变量的初始猜测对问题进行求解，记状态变量与控制变量的收敛解分别为x^[p]与u^[p]。

步骤5-5：流程判断

若p＜m，则返回步骤5-3继续进行迭代；若p＝m，则问题等价于问题p，即x^[p]与u^[p]则为能量最优意义下机器人的运动轨迹与相应的控制输入，求解结束。

本发明相对于现有技术，有益效果为：本发明在含约束非线性最优控制的框架下，利用障碍尺寸同伦策略实现了复杂作业环境下机器人点对点轨迹规划问题的高效稳定求解，对于机器人实际作业中面临的轨迹规划问题求解具有重要意义。本发明中阐述的方法具有很强的可操作性和可行性，便于实际应用。

附图说明

图1为本发明的计算流程图。

图2为本发明实施例中的移动机器人的示意图。

图3为本发明实施例中的作业环境示意图。

图4为本发明实施例中的移动机器人横坐标随时间的变化规律。

图5为本发明实施例中的移动机器人纵坐标随时间的变化规律。

图6为本发明实施例中的移动机器人朝向角随时间的变化规律。

图7为本发明实施例中的移动机器人速度随时间的变化规律。

图8为本发明实施例中的移动机器人前轮转角随时间的变化规律。

图9为本发明实施例中的移动机器人加速度随时间的变化规律。

图10为本发明实施例中的移动机器人轨迹示意图。

具体实施方式

以下结合具体实施例对本发明做进一步说明。

本发明的方法中，首先构建机器人的运动方程、状态变量与控制变量的约束条件，并用特征圆描述机器人的轮廓。第二，根据机器人作业环境中的障碍信息，利用不等式约束描述机器人与障碍的避障条件。第三，根据机器人的当前状态确定轨迹规划问题的初始边界条件，并根据目标状态确定轨迹规划问题的终端边界条件。第四，建立以能量消耗最小为指标的最优控制问题。第五，利用同伦策略构建一系列子问题，实现对原始最优控制问题的迭代求解。本发明的计算流程图如图1所示。

图2为一具有圆形外壳的移动机器人的示意图，特征圆半径rc＝1m，圆心为o(xc,yc)。其后轮中心位于特征圆的圆心，轮距为l＝0.7m。规定机器人只能向前行进且速度上限为vmax＝1m/s，前轮转向角的上限为

图3为作业环境的示意图，其中分布着25个圆形障碍。对于第k(k＝1,2,…,25)个圆形障碍，其半径rk＝9m，圆心横坐标为xk＝25×(kmod5)-25，圆心纵坐标为其中mod为取余符号，为向下取整符号。令安全距离rs＝1m。

一种基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法，包括以下步骤：

步骤1：构建机器人相关信息

步骤1-1：建立机器人的运动方程

选取状态空间x＝[xc,yc,θ,v]^t，其中θ为机器人的朝向角，v为机器人的速度。选取控制变量u＝[u1,u2]^t，其中u1＝tanβ即前轮转角的正切值，u2为机器人的加速度。从而建立如下的机器人运动学方程

步骤1-2：确定机器人作业过程中的状态变量和控制变量的约束

将机器人运动过程中的状态变量的约束写作cs(x,t)≤0，其中

类似地，将控制变量的约束写作cc(u,t)≤0，其中

步骤1-3：用特征圆描述机器人的轮廓

机器人特征圆方程记作

(x-xc)²+(y-yc)²＝1²(16)

步骤2：建立障碍规避条件

步骤2-1：用特征圆描述作业环境中的障碍

作业环境中一共有25个障碍，对于第k个障碍，其特征圆方程为

(x-xk)²+(y-yk)²＝9²,k＝1,2,...,25(17)

步骤2-2：设定安全距离以建立避障条件

由于机器人与作业环境中每个障碍的最小安全距离为rs＝1m，所以机器人与第k个障碍的避障条件表示为

将上述25个约束中不等号左端的表达式写作如下的矩阵

则全部25个避障条件可以表示为如下的矩阵形式

h(x,t)≤0(20)

为利用障碍尺寸同伦策略实现对轨迹规划问题的稳定高效求解，在矩阵h(x,t)的基础上构造如下的弱化避障约束矩阵

步骤3：确定轨迹规划问题的边界条件

记轨迹规划问题的初始时刻和终端时刻分别为ts＝0s与tf＝210s，机器人在初始时刻与终端时刻的状态分别为xs＝[13,0,π/2,0]^t与xf＝[87,105,π/2,0]^t，则轨迹规划问题的边界条件为

x(ts)＝xs,x(tf)＝xf(22)

步骤4：建立最优控制模型

根据公式(22)中的边界条件、公式(13)中的系统方程以及公式(14)(15)(20)中的约束条件，建立如下的能量最优控制问题

步骤5：利用障碍尺寸同伦策略求解最优控制问题

步骤5-1：设置同伦因子列表

设置同伦因子列表为α＝{0.2,0.4,0.6,0.8,1}，此时m＝5。

步骤5-2：求解过程初始化

记p为同伦因子索引，并初始化同伦因子指标p＝0。记最优控制问题状态变量和控制变量的初始猜测分别为x^[0]与u^[0]。

步骤5-3：更新同伦因子索引

更新同伦因子索引为p＝p+1。

步骤5-4：构造子问题并求解

构造如下的子问题

利用x^[p-1]与u^[p-1]作为状态变量与控制变量的初始猜测对问题进行求解，记状态变量与控制变量的收敛解分别为x^[p]与u[^p]。

步骤5-5：流程判断

若p＜5，则返回步骤5-3进行迭代；若p＝5，问题等价于问题p，即x^[p]与u^[p]则为能量最优意义下机器人的运动轨迹与相应的控制输入，求解结束。

根据上述步骤，计算得到的移动机器人的横坐标、纵坐标、朝向角以及速度随时间的变化规律如图4～图7所示，计算得到的前轮转角和加速度如图8～图9所示，轨迹示意图如图10所示。

以上所述实施例仅表达本发明的实施方式，但并不能因此而理解为对本发明专利的范围的限制，应当指出，对于本领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些均属于本发明的保护范围。