一种基于改进人工势场的足式机器人牵引控制方法及系统与流程

本发明涉及机器人运动控制领域，具体涉及一种基于改进人工势场的足式机器人牵引控制方法及系统。

背景技术：

足式机器人目前主要靠遥控的方式进行操作，无疑降低了机器人的工作效率，操控人员操控机器人时采用人眼作为视觉反馈，由于人眼视觉反馈无法转化为精确的机器人位姿控制量，因此存在机器人反复进行动作调整的现象，大大造成了人员的操控疲劳，操控效率较低。

目前已经开展了足式机器人自主行走的研究，比较常见的路径规划算法为A*全局搜索算法和遗传算法，其中，A*搜索算法受启发函数选取的影响，很难找到全局最优解；遗传算法结构相对复杂，计算任务艰巨。人工势场是早期Khatib提出的机器人移动轨迹规划理论，被广泛应用于轮式机器人控制，用于提升机器人的作业效率，但传统的人工势场理论存在机器人在局部区域内无法到达终点的缺陷。

技术实现要素：

为解决上述现有技术中的不足，本发明的目的是提供一种基于改进人工势场的足式机器人牵引控制方法及系统，克服了传统的人工势场理论存在机器人在局部区域内无法到达终点的缺陷。

本发明的目的是采用下述技术方案实现的：

本发明提供一种基于改进人工势场的足式机器人牵引控制方法，其改进之处在于：

构建机器人目标引力势能场和障碍物斥力势能场函数；

根据势能场计算牵引控制的目标和障碍物虚拟力，并计算环境产生的虚拟合力；

根据环境产生的虚拟合力，计算机器人的运动控制信号；

根据机器人的运动控制信号，计算机身位姿控制器的输入。

进一步地，在构建机器人目标引力势能场和障碍物斥力势能场函数之前，还包括提取机器人作业环境信息；所述作业环境信息通过机器人搭载的感知系统实时对环境进行识别，包括障碍物的识别和目标的识别。

进一步地，所述构建机器人目标引力势能场函数，包括：

实时估计机器人在世界坐标系下的位置；

依靠感知系统实时获取目标的位置信息；

实时计算机器人与目标距离，并引入机器人距离目标的安全距离阈值及引力场比例因子；

依据机器人与目标实时距离和安全距离的关系，构建机器人目标势能场函数；

优选的，采用机载惯性测量单元实时获取机器人在世界坐标系下的平动加速度，利用加速度二次积分的方法实时估计机器人在世界坐标系下的位置；

优选的，依靠机载感知系统对目标进行识别，提取目标特征信息和获取目标的深度信息，从而获得目标相对感知系统坐标系下的位置pc，根据机器人在世界坐标系下的位置p，以及感知系统相对于机器人坐标系的位置prc，进而得到目标在世界坐标系下的位置p^*，p^*＝pc+p+prc；

优选的，所述机器人目标势能场函数为：

其中：p为机器人在世界坐标系下的位置，p^*为目标的位置信息，d(p,p^*)为机器人与目标的欧式距离d(p,p^*)＝||p-p^*||，d^*为机器人距离目标的安全距离阈值，ζ为引力场比例因子，Ua(p)为机器人目标势能场函数。

进一步地，所述构建机器人障碍物斥力势能场函数，包括：

依靠感知系统实时获取障碍物的位置信息；

实时计算机器人与障碍物的距离，并引入机器人距离障碍物的斥力有效阈值及斥力场比例因子；

依据机器人与障碍物的实时距离和斥力有效阈值的关系，构建机器人障碍物势能场函数；

优选的，所述机器人障碍物势能场函数为：

其中：pi为实时获取障碍物的位置信息，dr(p,pi)为计算机器人与障碍物的距离，所述距离为欧式距离dr(p,pi)＝||p-pi||，Q^*为机器人距离障碍物的斥力有效阈值，η为斥力场比例因子，Uri(p)为障碍物势能场函数。

进一步地，所述计算目标和障碍物虚拟力，包括：

求取引力势能场的梯度▽Ua(p)，并将引力势能场的负梯度方向-▽Ua(p)作为目标对机器人的引力Fa(p)；

求取斥力势能场的梯度▽Uri(p)，并将斥力势能场的负梯度方向-▽Uri(p)作为障碍物对机器人的斥力Fri(p)；

优选的，所述引力势能场的梯度为：

所述斥力势能场的梯度为：

其中：▽Ua(p)为引力势能场的梯度，Fa(p)为目标对机器人的引力，p为机器人在世界坐标系下的位置，p^*为目标的位置信息，d(p,p^*)为机器人与目标距离，d^*为机器人距离目标的安全距离阈值，ζ为引力场比例因子，Ua(p)为机器人目标势能场函数；-▽Uri(p)为斥力势能场的梯度，pi为实时获取障碍物的位置信息，dr(p,pi)为计算机器人与障碍物的距离，Q^*为机器人距离障碍物的斥力有效阈值，η为斥力场比例因子，Uri(p)为障碍物势能场函数；Fri(p)为障碍物对机器人的斥力；i表示障碍物的序号，表示第i个障碍物。

进一步地，所述环境产生的虚拟合力为：

F＝Fr(p)+Fa(p)

其中：Fa(p)为目标对机器人的引力，Fr(p)为所有障碍物对机器人产生的合力，且这里，Fri(p)表示第i个障碍物对机器人产生的斥力。

进一步地，所述计算机器人的运动控制信号包括计算机身纵向运动控制信号和机身转向速度控制信号；

优选的，所述机身纵向运动信号为：

其中：为机身纵向运动信号，kf为增益因子，Fa(p)为目标对机器人的引力，Fr(p)为所有障碍物对机器人产生的合力；

优选的，所述计算机身运动方向控制信号，包括：

根据环境产生的合力分力，求取合力对机器人作用的方向；

根据合力对机器人作用的方向设计机身转向速度控制信号；

优选的，合力对机器人作用的方向为：

θgoal＝atan2(Fy,Fx)

优选的，机身转向速度控制信号为：

其中：atan2(y,x)为反三角函数，该函数返回点(x,y)和原点(0,0)之间直线的倾斜角，θgoal为合力对机器人作用的方向，(Fx,Fy)为世界坐标系下的合力，Fx,Fy分别表示x轴的分力和y轴的分力，kθ为增益因子，θd为机器人实时的期望偏航姿态，为机身运动方向速度控制信号。

进一步地，所述计算机身位姿控制器输入，包括：

根据产生的机身纵向运动速度控制信号，并根据机身实时的位置信息，引入机身纵向运动控制器比例和微分因子，设计机身纵向运动控制器输入；

根据产生的机身运动方向速度控制信号，并根据机身实时的方向信息，引入机身运动方向控制器比例和微分因子，设计机身运动方向控制器输入；

优选的，机身纵向运动控制器输入为：

优选的，机身运动方向控制器输入为：

其中：为期望的机身纵向运动速度，x为实际的机身纵向位置，为实际的机身纵向速度；kpx为机身纵向运动控制器比例因子、kvx为机身纵向运动控制器微分因子，μx(t)为机身纵向运动控制器输入；为期望的机身运动方向变化速度，θ为实际的机身运动方向，为实际的机身运动方向变化速度；kpθ为机身运动方向控制器比例因子，kvθ为机身运动方向的微分因子，μθ(t)为机身运动方向控制器输入。

本发明还提供一种基于改进人工势场的足式机器人牵引控制系统，其改进之处在于：

构建模块，用于构建机器人目标引力势能场和障碍物斥力势能场函数；

第一计算模块，用于根据势能场计算目标和障碍物虚拟力，并计算环境产生的虚拟合力；

第二计算模块，用于根据环境产生的虚拟合力，计算机器人的运动控制信号；

第三计算模块，用于根据机器人的运动控制信号，计算机身位姿控制器的输入。

进一步地：还包括：提取模块，用于在构建机器人目标引力势能场和障碍物斥力势能场函数之前，提取机器人作业环境信息；所述作业环境信息通过机器人搭载的感知系统实时对环境进行识别，包括障碍物的识别和目标的识别；

优选的，所述构建模块，进一步包括：

目标引力势能场函数构建模块和障碍物斥力势能场函数构建模块；

优选的，所述目标引力势能场函数构建模块，进一步包括：

估计单元，用于实时估计机器人在世界坐标系下的位置；

第一获取单元，用于依靠感知系统实时获取目标的位置信息；

第一计算单元，用于实时计算机器人与目标距离，并引入机器人距离目标的安全距离阈值及引力场比例因子；

目标势能场函数构建单元，用于依据机器人与目标实时距离和安全距离的关系，构建机器人目标势能场函数；

优选的，障碍物斥力势能场函数构建模块，进一步包括：

第二获取单元，用于依靠感知系统实时获取障碍物的位置信息；

第二计算单元，用于实时计算机器人与障碍物的距离，并引入机器人距离障碍物的斥力有效阈值及斥力场比例因子；

障碍物势能场函数构建单元，用于依据机器人与障碍物的实时距离和斥力有效阈值的关系，构建机器人障碍物势能场函数；

优选的，所述第一计算模块，还用于：

求取引力势能场的梯度；

取引力势能场的负梯度方向作为目标对机器人的引力；

求取斥力势能场的梯度；

取斥力势能场的负梯度方向作为障碍物对机器人的斥力；

优选的，所述第三计算模块，进一步包括：

机身纵向运动控制信号计算模块和机身转向速度控制信号计算模块。

与最接近的现有技术相比，本发明提供的技术方案具有的有益效果是：

(1)采用改进的人工势场理论，克服了机器人在局部区域内无法到达终点的缺陷；

(2)根据作业环境对机器人产生的虚拟作用力，设计的机器人运动控制信号，能够使机器人成功避开障碍物，减少机器人的损伤；

(3)设计的机器人运动控制信号，能够使机器人高效到达目标，提高机器人的任务执行效率；

(4)该控制方法简单、计算量小，规划的自主行走路径平稳可靠。

附图说明

图1是本发明提供的基于改进人工势场的足式机器人牵引控制方法的流程图；

图2是本发明提供的系统工作框图；

图3是本发明提供的作业环境作用力示意图；

图4是本发明提供的机器人控制信号示意图；

图5是本发明提供的基于改进人工势场的足式机器人牵引控制系统的结构框图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步的详细说明。

以下描述和附图充分地示出本发明的具体实施方案，以使本领域的技术人员能够实践它们。其他实施方案可以包括结构的、逻辑的、电气的、过程的以及其他的改变。实施例仅代表可能的变化。除非明确要求，否则单独的组件和功能是可选的，并且操作的顺序可以变化。一些实施方案的部分和特征可以被包括在或替换其他实施方案的部分和特征。本发明的实施方案的范围包括权利要求书的整个范围，以及权利要求书的所有可获得的等同物。在本文中，本发明的这些实施方案可以被单独地或总地用术语“发明”来表示，这仅仅是为了方便，并且如果事实上公开了超过一个的发明，不是要自动地限制该应用的范围为任何单个发明或发明构思。

实施例一、

本发明提供一种基于改进人工势场的足式机器人牵引控制方法，其流程图如图1所示，该方法包括以下步骤：

S1，提取机器人作业环境信息；

S2，构建机器人目标引力势能场；

S3，构建机器人障碍物斥力势能场；

S4，建立目标及障碍物虚拟力；

S5，根据环境产生的虚拟合力，设计机身纵向运动控制信号；

S6，根据环境产生的虚拟引力与斥力，设计机身运动方向控制信号；

S7，根据期望的机身运动信号，设计机身位姿控制器。

如图2所示，S1中，提取机器人作业环境信息。具体实现过程为：机身前方搭载感知系统，感知系统包括激光雷达、双目摄像头等设备，可实现360度的全景感知，机器人行走过程中，感知系统实时对环境进行识别，包括障碍物的识别、目标的识别。

图3为环境对机器人产生的虚拟作用力信息示意图，包括目标对机器人产生的引力以及障碍物对机器人产生的斥力，具体包括以下步骤：

S2，构建机器人目标势能场。具体包括：

S21：实时估计机器人在世界坐标系下的位置p。

优选的，采用机载惯性测量单元实时获取机器人在世界坐标系下的平动加速度，利用加速度二次积分的方法实时估计机器人在世界坐标系下的位置；

S22：依靠感知系统实时获取目标的位置信息p^*；

优选的，依靠机载感知系统对目标进行识别，提取目标特征信息和获取目标的深度信息，从而获得目标相对感知系统坐标系下的位置pc，根据机器人在世界坐标系下的位置p，以及感知系统相对于机器人坐标系的位置prc，进而得到目标在世界坐标系下的位置p^*，p^*＝pc+p+prc；

S23：实时计算机器人与目标的距离d(p,p^*)，并引入机器人距离目标的安全距离阈值d^*及引力场比例因子ζ。

S24：依据机器人与目标的实时距离与安全距离的关系，构建机器人目标势能场函数Ua(p)。

S3，机器人障碍物斥力势能场。具体包括：

S31：依靠感知系统实时获取障碍物的位置信息pi。

S32：实时计算机器人与障碍物的距离dr(p,pi)，并引入机器人距离障碍物的斥力有效阈值Q^*及斥力场比例因子η。

S33：依据机器人与障碍物的实时距离与斥力有效阈值的关系，构建机器人障碍物势能场函数Uri(p)。

S4，建立目标及障碍物虚拟力。具体包括：

S41：求取引力势能场的梯度▽Ua(p)，首先，对目标与机器人之间的欧式距离关于机器人的位置向量元素pj进行一阶偏导，其次，根据引力势能场函数的定义表达式，对引力势能场函数关于机器人的位置向量元素pj进行一阶偏导，进而得到引力势能场函数关于p的梯度函数。对于机器人位置p＝[p1,x,pn]^T，目标位置机器人与目标之间的欧式距离表达式对d(p,p^*)关于机器人的位置向量元素pj进行一阶偏导，进而得到d(p,p^*)关于p的梯度，即)；

基于上述表达式，对引力势能场函数关于机器人的位置向量元素pj进行一阶偏导，进而得到引力势能场函数关于p的梯度函数。

引力势能场的梯度为：

S42：取引力势能场的负梯度方向作为目标对机器人的引力Fa(p)。

S43：求取斥力势能场的梯度▽Uri(p)：

对于机器人位置p＝[p1,…,pn]^T，第i个障碍物位置pi＝[pi1,…,pin]^T，机器人与第i个障碍物之间的欧式距离表达式对dr(p,pi)关于机器人的位置向量元素pj进行一阶偏导，进而得到dr(p,pi)关于p的梯度，即：

基于上述表达式，对斥力势能场函数关于机器人的位置向量元素pj进行一阶偏导，进而得到斥力势能场函数关于p的梯度函数。

S44：取斥力势能场的负梯度方向作为障碍物对机器人的斥力Fri(p)。

其中：▽Ua(p)为引力势能场的梯度，Fa(p)为目标对机器人的引力，p为机器人在世界坐标系下的位置，p^*为目标的位置信息，d(p,p^*)为机器人与目标距离，d^*为机器人距离目标的安全距离阈值，ζ为引力场比例因子，Ua(p)为机器人目标势能场函数；▽Uri(p)为斥力势能场的梯度，pi为实时获取障碍物的位置信息，dr(p,pi)为计算机器人与障碍物的距离，Q^*为机器人距离障碍物的斥力有效阈值，η为斥力场比例因子，Uri(p)为障碍物势能场函数；Fri(p)为障碍物对机器人的斥力；i表示障碍物的序号，表示第i个障碍物。

图4为机器人根据环境对其虚拟作用力而产生的控制信号示意图，包括机器人纵向运动控制信号及运动方向控制信号，具体包括以下步骤：

S5，根据环境产生的虚拟合力，设计机身纵向运动控制信号，具体包括：

S51：根据目标对机器人产生的引力以及障碍物对机器人产生的斥力，构建环境对机器人作用的合力F。

F＝Fr(p)+Fa(p) (5)

S52：根据环境对机器人作用的合力，引入增益因子kf，设计机身纵向运动信号

S6，根据环境产生的合力分力，设计机身运动方向控制信号。具体包括：

S61：根据环境产生的合力分力(Fx,Fy)，求取合力对机器人作用的方向θgoal：

θgoal＝atan2(Fy,Fx) (7)

S62：引入增益因子kθ，并结合机器人实时的期望偏航姿态θd，设计机身转向速度控制信号

其中：θgoal为合力对机器人作用的方向，(Fx,Fy)为世界坐标系下的合力，Fx,Fy分别表示x轴的分力和y轴的分力，kθ为增益因子，θd为机器人实时的期望偏航姿态，为机身转向速度控制信号。

S7，根据期望的机身运动控制信号，设计机身位姿控制器。具体包括：

S71：根据产生的期望机身纵向运动速度控制信号并根据机身实时的位置信息x，引入控制器比例kpx、微分因子kvx，搭建机身纵向运动控制器输入μx(t)。

S72：根据产生的期望机身运动方向速度控制信号并根据机身实时的方向信息θ，引入控制器比例kpθ、微分因子kvθ，搭建机身运动方向控制器输入μθ(t)。

其中：为期望的机身纵向运动速度，x为实际的机身纵向位置，为实际的机身纵向速度；kpx为机身纵向运动控制器比例因子、kvx为机身纵向运动控制器微分因子，μx(t)为机身纵向运动控制器输入；为期望的机身运动方向变化速度，θ为实际的机身运动方向，为实际的机身运动方向变化速度；kpθ为机身运动方向控制器比例因子，kvθ为机身运动方向的微分因子，μθ(t)为机身运动方向控制器输入。

实施例二、

基于同样的发明构思，本发明还提供一种基于改进人工势场的足式机器人牵引控制系统，其结构框图如图5所示，包括：

构建模块201，用于构建机器人目标引力势能场和障碍物斥力势能场函数；

第一计算模块202，用于根据势能场计算目标和障碍物虚拟力，并计算环境产生的虚拟合力；

第二计算模块203，用于根据环境产生的虚拟合力，计算机器人的运动控制信号；

第三计算模块204，用于根机器人的运动控制信号，计算机身位姿控制器的输入。

系统还包括：提取模块，用于在构建机器人目标引力势能场和障碍物斥力势能场函数之前，提取机器人作业环境信息；所述作业环境信息通过机器人搭载的感知系统实时对环境进行识别，包括障碍物的识别和目标的识别；

优选的，所述构建模块，进一步包括：

目标引力势能场函数构建模块和障碍物斥力势能场函数构建模块；

优选的，所述目标引力势能场函数构建模块，进一步包括：

估计单元，用于实时估计机器人在世界坐标系下的位置；

第一获取单元，用于依靠感知系统实时获取目标的位置信息；

第一计算单元，用于实时计算机器人与目标距离，并引入机器人距离目标的安全距离阈值及引力场比例因子；

目标势能场函数构建单元，用于依据机器人与目标实时距离和安全距离的关系，构建机器人目标势能场函数；

优选的，障碍物斥力势能场函数构建模块，进一步包括：

第二获取单元，用于依靠感知系统实时获取障碍物的位置信息；

第二计算单元，用于实时计算机器人与障碍物的距离，并引入机器人距离障碍物的斥力有效阈值及斥力场比例因子；

障碍物势能场函数构建单元，用于依据机器人与障碍物的实时距离和斥力有效阈值的关系，构建机器人障碍物势能场函数；

优选的，所述第一计算模块，还用于：

求取引力势能场的梯度；

取引力势能场的负梯度方向作为目标对机器人的引力；

求取斥力势能场的梯度；

取斥力势能场的负梯度方向作为障碍物对机器人的斥力；

优选的，所述第三计算模块，进一步包括：

机身纵向运动控制信号计算模块和机身转向速度控制信号计算模块。

本发明提供的一种基于改进人工势场的足式机器人牵引控制方法，采用改进的人工势场理论，通过机载感知系统对目标以及障碍物的识别并提取特征信息，构建目标及障碍物势能场，利用势能场的梯度获取目标与障碍物对机器人本体产生的虚拟牵引力，从而转化为机身控制信号送入机身位姿控制器，实现机器人的高效自主牵引控制，该控制方法简单、计算量小，规划的自主行走路径平稳可靠。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作S以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的S。

以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员依然可以对本发明的具体实施方式进行修改或者等同替换，这些未脱离本发明精神和范围的任何修改或者等同替换，均在申请待批的本发明的权利要求保护范围之内。