基于CLF与CBF的移动机器人在线导航与避障方法

技术领域

本发明属于人工智能与机器人领域的一种移动机器人导航与避障控制方法，具体涉及一种基于控制李雅普诺夫函数的导航控制与基于控制障碍函数的避障控制方法。

背景技术

机器人技术已经得到了快速的发展，在物流、自动驾驶以及营救作业等多种场合得到了广泛的应用。因此，为移动机器人设计一个安全关键控制器十分重要，如果机器人的控制器不能保证机器人在作业中的安全操作，那么会带来不可逆转的毁灭影响。安全关键控制器，代表控制器将保证机器人的作业安全凌驾于其他指标之上，如轨迹跟踪等等，始终将机器人安全作业为首要控制目标。

机器人的安全保证在实际应用中主要体现为避免与障碍物发生碰撞，常用的方法包括模型预测控制(Model Predictive Control,MPC)、人工势场法、可达性分析和神经网络等方法。最近，基于控制障碍函数(Control Barrier Function,CBF)的方法在安全关键控制中得到了广泛的应用。该类方法往往通过二次规划(Quadratic Program，QP)来构建一个连续时域内的优化控制问题，并将CBF以及控制李雅普诺夫函数(Control LyapunovFunction,CLF)作为约束加入到该优化控制问题中(CLF-CBF-QP)。通过快速求解QP优化控制问题可以在线获得移动机器人的最优控制指令，实现关键安全控制。

但是基于上述优化控制问题形式的现有方法难以直接应用到单车模型的移动机器人上，因为安全关键控制器往往不能直接控制所有的控制量来实现移动机器人的安全行驶，往往只能控制移动机器人的线速度，而不能控制其角速度。因此一些研究基于高阶控制障碍函数(High Order Control Barrier Function,HOCBF)来设计控制器，但是HOCBF的引入导致只能控制角速度来实现避障，线速度在这个情况下需要变为固定值。还有一些方法将CBF与MPC结合来克服连续时域下的缺陷，在离散时域内求解移动机器人的最优控制指令，但是当障碍物数目变多时，在离散时域中求解难以满足实时性的需求。因此，实现单车模型的移动机器人的在线安全关键控制仍然是一个充满挑战性的问题。

发明内容

有鉴于此，为了解决背景技术中存在的问题，本发明提出了一种基于CLF与CBF的移动机器人在线导航与避障方法，通过基于单车模型特性的移动机器人的后轴轴心来设计CBF函数，实现优化控制问题(CLF-CBF-QP)能在连续时域内同时控制移动机器人的线速度和角速度进行避障控制，并最终行驶到机器人的终点位置处。

为了实现以上目的，本发明采用的技术方案如下：

步骤1：收集环境内所有障碍物的信息并保存在移动机器人的地图信息内，以及确定移动机器人的当前位置与终点位置；

步骤2：确定移动机器人模型，并将移动机器人模型转换为仿射控制形式；

步骤3：根据移动机器人的当前位置与终点位置以及地图内的障碍物信息，分别构建保证机器人向终点移动的CLF约束和保证移动机器人避免与障碍物碰撞的CBF约束；

步骤4：确定优化控制问题的目标函数，结合保证机器人向终点移动的CLF约束、保证移动机器人避免与障碍物碰撞的CBF约束以及控制量的物理约束，构建获得优化控制问题；

步骤5：求解优化控制问题后，获得最优控制量并发送给移动机器人的控制系统，结合仿射控制形式的移动机器人模型，控制移动机器人进行运动；

步骤6：检测移动机器人是否到达终点，如果未到达终点，则重复步骤3-步骤5，直至移动机器人抵达终点。

所述步骤1中，所有障碍物均为圆形障碍物，每个障碍物的信息包括其圆心位置、半径以及移动速度；移动机器人的形状为圆形，将移动机器人的后轴轴心坐标作为移动机器人的位置。

所述步骤2中，移动机器人模型的公式如下：

其中，x

移动机器人模型对应的仿射控制系统公式如下：

u＝[v，w]

其中，

所述步骤3具体为：

步骤3.1：基于移动机器人的当前位置与终点位置构建控制李雅普诺夫函数V(x)，再基于控制李雅普诺夫函数V(x)构建保证机器人向终点移动的CLF约束；

步骤3.2：基于移动机器人的当前位置与地图内的障碍物信息构建控制障碍函数，然后基于控制障碍函数构建保证移动机器人避免与障碍物碰撞的CBF约束。

所述步骤3.1中，控制李雅普诺夫函数V(x)的公式如下：

e＝[x

其中，e代表当前位置和终点位置之间的差异，T表示转置操作，R为误差权重矩阵，a

所述保证机器人向终点移动的约束的公式如下：

其中，f(x)表示控制状态函数，g(x)表示控制输入函数，u为控制量，γ代表类K函数。

所述步骤3.2中，控制障碍函数的公式如下：

其中，h

所述步骤3.2中，当所述障碍物为静态障碍物时，保证移动机器人避免与第i个障碍物O

其中，f(x)表示控制状态函数，g(x)表示控制输入函数，u为控制量，α代表类K

所述步骤3.2中，当障碍物为动态障碍物时，保证移动机器人避免与第i个障碍物O

其中，h

所述步骤4中，优化控制问题的公式如下：

u

其中，H表示控制量的权重矩阵，p表示松弛变量的权重系数，δ为松弛变量，R

所述步骤6中，如果移动机器人超过最大控制时间还未到达终点，则移动机器人原路返回至起点。

本发明的有益效果为：

本发明基于CLF-CBF-QP的优化控制问题形式，可以保证在线快速求解该优化控制问题，保证了实时性；同时本发明基于机器人的后轴轴心来构建CBF函数，可以保证同时控制线速度和角速度来实现碰撞避免；本发明为了实现移动机器人的导航功能，还设计了一个含有位置信息误差和航向角误差之间交叉项的CLF函数，保证在任何情况下控制器均能同时控制线速度和角速度来实现导航功能。

附图说明

图1为本发明方法的流程图。

图2为本发明所针对的单车模型的移动机器人示意图。

图3为本发明在存在静态障碍物下的导航仿真结果。

图4为本发明在存在动态障碍物下的导航仿真结果。

具体实施方式

下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。

图1表示了本发明方法的流程图，下面将对该流程图进行详细的解释。

步骤1：收集环境内所有障碍物的信息(包括障碍物的尺寸、移动速度等)并保存在移动机器人的地图信息内，以及确定移动机器人的当前位置与终点位置，与地图信息一起发送给移动机器人的控制器；设定所有障碍物均为圆形障碍物，每个障碍物的信息包括其圆心位置、半径大小以及移动速度等信息。同样设定移动机器人的形状为圆形，将移动机器人的后轴轴心坐标作为移动机器人的位置。即给定的移动机器人的当前位置(x

X

y

其中，x

步骤2：确定移动机器人的单车模型，并将移动机器人的单车模型转换为仿射控制形式；

步骤2中，移动机器人的单车模型的公式如下：

其中，x

移动机器人的单车模型对应的仿射控制系统公式如下：

u＝[v，w]

其中，

步骤3：根据移动机器人的当前位置与终点位置以及地图内的障碍物信息，分别构建保证机器人向终点移动的CLF约束和保证移动机器人避免与障碍物碰撞的CBF约束；

步骤3具体为：

步骤3.1：基于移动机器人的当前位置与终点位置构建控制李雅普诺夫函数(Control Lyapunov Function,CLF)V(x)，再基于控制李雅普诺夫函数V(x)构建保证机器人向终点移动的CLF约束；

步骤3.2：基于移动机器人的当前位置与地图内的障碍物信息(具体是基于机器人中心位置与障碍物位置通过欧式距离)构建控制障碍函数(Control Barrier Function,CBF)h

步骤3.1中，本发明添加了关于位置坐标误差和航向角误差之间的两个交叉项b

e＝[x

其中，e代表当前位置和终点位置之间的差异，T表示转置操作，P为误差权重矩阵，要求其正定，a

保证机器人向终点移动的约束的公式如下：

其中，γ代表类K函数，这里取其为正实数。只要保证控制量能够满足上述的约束，那么便能保证机器人向终点移动。

步骤3.2中，控制障碍函数的公式如下：

其中，h

因此只要保证h

其中，α代表类K

同时本发明可以实现移动机器人与动态障碍物之间的碰撞避免，本发明使用

其中，

本发明通过基于后轴轴心来表示机器人的圆心位置，继而通过欧式距离来与障碍物构建CBF，可以保证基于CBF构建的约束能够同时控制机器人的线速度和角速度来实现碰撞避免。

同时本发明构造的CBF函数可以避免奇异性，即能保证

步骤4：确定优化控制问题的目标函数，结合保证机器人向终点移动的CLF约束、保证移动机器人避免与障碍物碰撞的CBF约束以及添加控制量的物理约束，构建获得优化控制问题(CLF-CBF-QP)；

当障碍物为静态障碍物时，优化控制问题的形式如下：

u

其中，H表示控制量的权重矩阵，p表示松弛变量的权重系数，δ为松弛变量，且δ∈R，控制量、松弛变量共同组成优化变量，R

[v

步骤5：求解优化控制问题后，获得最优控制量并发送给移动机器人的控制系统，确定仿真步长，结合仿射控制形式的移动机器人的单车模型，控制移动机器人进行运动；本发明实际上将整体的优化控制问题拆分为多个QP问题进行在线求解。

步骤6：检测移动机器人是否到达终点，如果未到达终点，则重复步骤3-步骤5，直至移动机器人抵达终点。

如果移动机器人超过最大控制时间还未到达终点，则移动机器人原路返回至起点。具体实施中，移动机器人超过最大控制时间还未到达终点，则终止控制过程，仿真结束。

本发明经由仿真实验进行了验证，设定最大控制时间为30s，仿真步长为0.1s，仿真结果如图3、图4的(a)-图4的(d)所示，其中图3是针对环境中只有静态的障碍物，图4是针对环境中存在动态的障碍物。障碍物均使用边缘为虚线的黑色圆形来表示，边缘为实线的黑色圆形表示当前机器人的位置，灰色的圆形表示机器人的起始和目标终点位置，黑色的实线代表了机器人的过去运动轨迹，动态障碍物的运动轨迹使用黑色的虚线表示。图3的仿真结果显示本发明的方法可以实现由线速度和角速度控制的单车模型的移动机器人与静态障碍物的碰撞避免并导航移动机器人到其终点位置；图4的仿真结果展示了整个控制过程中四个不同时刻的仿真结果，通过观察四个时刻的仿真结果可以发现本发明实现了移动机器人与动态障碍物的碰撞避免，并控制移动机器人行驶到了终点。

上述实施例用来解释说明本发明，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明做出的任何修改和改变，都落入本发明的保护范围。