CN113467461A - 移动机器人非结构化环境下的人机协作式路径规划方法 - Google Patents

Abstract

本发明提供一种移动机器人非结构化环境下的人机协作式路径规划方法，其包括以下步骤：S1、操纵员引导路径规划器；S2、机器人自主路径规划器；S3、对步骤S1得到的操纵员规划的路径以及步骤S2得到的机器人自主规划的路径进行混合滤波，将操纵员修改的路径与机器人自主规划的路径进行合成；S4、触觉反馈：将规划路径与机器人实际行驶的路径位置偏差与速度偏差以力/触觉的形式反馈至操纵员的手柄处。本发明的方法将基于力/触觉的遥操纵技术与自主规划技术结合在一起，既克服了移动机器人无法自主完成野外复杂环境规划的缺点，也减轻了操纵员的操纵负担，提高了工作效率。

Description

移动机器人非结构化环境下的人机协作式路径规划方法

技术领域

本发明涉及路径规划和遥操纵技术领域，具体涉及一种移动机器人非结构化环境下的人机协作式路径规划方法。

背景技术

尽管移动机器人系统智能化水平取得了巨大的进步，但是由于其认知能力的局限性，在非结构化等极具挑战性的环境下的自主规划问题还是不能完全交由机器人系统。比如针对灾后等非结构化环境下的搜救工作，一方面由于灾难环境太危险或者人类根本无法进入，而对于自主机器人而言该场景又过于复杂而无法自主作业，因此该场景下人机协作至关重要。另一方面，灾难场景下驾驶机器人对于操作员而言是一项高度紧张和困难的工作，操作员很大一部分工作仅仅是操纵机器人和躲避障碍物等低级任务，而在覆盖搜索区域和成功找到遇难者等方面的表现能力下降。因此，考虑将人类高级决策与机器智能决策进行融合，以实现更高效的搜救工作。

发明内容

为了解决上述现有技术的不足，本发明提出了一种移动机器人非结构化环境下的人机协作式路径规划方法，其在机器人自主规划路径的基础上，让操纵员在线协助修改路径，并将机器人实时行驶的路径与期望路径偏差通过力/触觉提示的形式反馈至操纵员操纵端，以此来实现人机协作式路径规划。

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

具体地，本发明提供一种移动机器人非结构化环境下的人机协作式路径规划方法，所述方法涉及的内容包括：

S1、操纵员引导路径规划器，具体包括以下子步骤：

S11、操纵员基于B样条曲线对路径进行规划；

S12、操纵员通过操纵手柄对路径进行修改：

S2、机器人自主路径规划器：机器人根据传感器提供的环境信息，进行基于势场的局部路径规划；

S3、对步骤S1得到的操纵员规划的路径以及步骤S2得到的机器人自主规划的路径进行混合滤波，将操纵员修改的路径与机器人自主规划的路径进行合成，该步骤具体包括以下子步骤：

S31、将机器人自主移动和操纵员的控制信号融合并生成机器人的行驶路径，即：

S32、通过式(2)的混合项实现二者的融合：

其中，I_2n表示单位矩阵，

表示J的广义逆矩阵，如式(3)所示，γ(x(t),s(t))为路径点的位置，

为路径的切矢量，

为路径的曲率；

S4、触觉反馈：将规划路径与机器人实际行驶的路径位置偏差与速度偏差以力/触觉的形式反馈至操纵员的手柄处。

优选地，B样条曲线是B样条基函数的线性组合，其曲线表达式为：

γ_S(x)＝{γ(x,s)∈R²|s∈S} (4)

其中，

表示控制点向量，s表示B样条曲线节点，集合S表示节点向量。

优选地，步骤S12具体包括以下步骤：

S121、建立操纵手柄的构型向量，公式为：

其中，q表示操纵手柄的构型向量，M(q)表示惯性矩阵，

表示科氏和离心项，τ表示反馈至触觉手柄的力，τ_h表示操纵者给手柄的力；

S122、将操纵手柄的构型向量映射至路径控制点向量：

其中，

表示对应于期望路径γ_S(x_h)的控制点向量，通过映射矩阵Q(x_h)进行在线修改，K表示正增益的对角矩阵；

其中，映射矩阵Q(x_h)决包括旋转、平移以及按比例扩大缩小三类变换，三类变换对应的矩阵分别为：

Q₁(x_h)＝I_2n (7)

其中，Q₁表示平移变换，I_2n表示2n×2n的单位矩阵，Q₂表示按比例扩大或者缩小变换，I_n表示n×n的单位矩阵，

表示克罗内克积运算，

表示变换的基准点，Q₃表示旋转变换，

diag(·)表示对角分块矩阵；

S123、操纵员端输出最终的路径控制信号：

其中，k_h为大于0的系数，其大小根据经验值获得。

优选地，步骤S2中机器人自主路径规划路径需要满足硬约束和软约束，机器人自主路径规划器输出的控制信号表示为：

其中，每一项约束都均能够通过不同的势能函数实现：

首先，式(11)中U_a，o是为了保证所规划路径无障碍物，其具体表达式为：

其中，

表示的是伪逆矩阵，o表示的是障碍物位置，

表示的是针对障碍物的斥力势场函数，

表示障碍物的影响范围外，

表示到达障碍物的边缘，其表达式为：

其次，式(11)中U_a，R是为了保证所规划路径可行，即路径曲线无奇点，其表达式为：

其中，

表示可以使路径奇异的控制点，S_i表示该控制点所对应节点的集合，针对奇异控制点设计的势场函数

在[0,R_R]上是单调的，其表达式如下：

最后，式(11)中U_a，I是为了实现软约束中保证移动机器人有效地执行所规划路径，并且所规划的路径尽可能地靠近兴趣点，其具体表达式为：

其中，

表示集合γ_S(x)中距离兴趣点r最近的点，其表达式为：

表示的是针对目标点设置的引力势场函数，因为此项作为软约束，优先权低于硬约束，所以势场函数存在上界U_I，该函数在[0,R_I]上严格单调，表达式如下：

与现有技术相比，本发明具有以下有益效果：

本发明将力/触觉反馈用在移动机器人在复杂非结构化环境下的路径规划上。首先，相机雷达等传感器感知机器人周围环境，并将信息通过视觉/听觉形式传给操纵员。然后，将环境建模为障碍物点和感兴趣的目标点，并传至机器人自主路径规划器，借助不同的势场函数进行避障规划，生成相应的路径控制信号；之后，操纵员根据环境信息进行基于B样条的路径修改，生成相应的路径控制信号；再将自主规划器和操纵员引导的路径信号经过混合滤波器进行加权生成机器人最终的行驶路径信号。最后，将机器人的实际行驶路径与规划的路径偏差通过触觉反馈模块以力/触觉的形式传至操纵手柄处，以实现人在回路的路径规划。本发明所述方法将基于力/触觉的遥操纵技术与自主规划技术结合在一起，既克服了移动机器人无法自主完成野外复杂环境规划的缺点，也减轻了操纵员的操纵负担，提高了工作效率。

附图说明

图1为本发明的流程示意图；

图2为本发明一种移动机器人非结构化环境下的人机协作式路径规划方法技术路线示意框图；

图3为本发明操纵员通过操纵手柄基于修改控制点以实现路径平移示意图；

图4为本发明操纵员通过操纵手柄基于修改控制点以实现路径旋转示意图；

图5为本发明操纵员通过操纵手柄基于修改控制点以实现路径按一定比例扩大或者缩小示意图。

具体实施方式

以下将参考附图详细说明本发明的示例性实施例、特征和方面。附图中相同的附图标记表示功能相同或相似的元件。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。

具体地，本发明提供一种移动机器人非结构化环境下的人机协作式路径规划方法，如图1所示，其包括以下步骤：

S1、操纵员引导路径规划器，具体包括以下子步骤：

S11、操纵员基于B样条曲线对路径进行规划；

S12、操纵员通过操纵手柄对路径进行修改：

S2、机器人自主路径规划器：机器人根据传感器提供的环境信息，进行基于势场的局部路径规划；

S3、对步骤S1得到的操纵员规划的路径以及步骤S2得到的机器人自主规划的路径进行混合滤波，将操纵员修改的路径与机器人自主规划的路径进行合成，该步骤具体包括以下子步骤：

S31、将机器人自主移动和操纵员的控制信号融合并生成机器人的行驶路径，即：

S32、通过式(2)的混合项实现二者的融合：

其中，I_2n表示单位矩阵，

表示J的广义逆矩阵，如式(3)所示，γ(x(t),s(t))为路径点的位置，

为路径的切矢量，

为路径的曲率；

S4、触觉反馈：将规划路径与机器人实际行驶的路径位置偏差与速度偏差以力/触觉的形式反馈至操纵员的手柄处。

优选地，B样条曲线是B样条基函数的线性组合，其曲线表达式为：

γ_S(x)＝{γ(x,s)∈R²|s∈S} (4)

其中，

表示控制点向量，s表示B样条曲线节点，集合S表示节点向量。

优选地，步骤S12具体包括以下步骤：

S121、建立操纵手柄的构型向量，公式为：

其中，q表示操纵手柄的构型向量，M(q)表示惯性矩阵，

表示科氏和离心项，τ表示反馈至触觉手柄的力，τ_h表示操纵者给手柄的力；

S122、将操纵手柄的构型向量映射至路径控制点向量：

其中，

表示对应于期望路径γ_S(x_h)的控制点向量，通过映射矩阵Q(x_h)进行在线修改，K表示正增益的对角矩阵；

其中，映射矩阵Q(x_h)决包括旋转、平移以及按比例扩大缩小三类变换，三类变换对应的矩阵分别为：

Q₁(x_h)＝I_2n (7)

其中，Q₁表示平移变换，I_2n表示2n×2n的单位矩阵，Q₂表示按比例扩大或者缩小变换，I_n表示n×n的单位矩阵，

表示克罗内克积运算，

表示变换的基准点，Q₃表示旋转变换，

diag(·)表示对角分块矩阵；

S123、操纵员端输出最终的路径控制信号：

其中，k_h为大于0的系数，其大小根据经验值获得。

优选地，步骤S2中机器人自主路径规划路径需要满足硬约束和软约束，机器人自主路径规划器输出的控制信号表示为：

其中，每一项约束都均能够通过不同的势能函数实现：

首先，式(11)中U_a，o是为了保证所规划路径无障碍物，其具体表达式为：

其中，

表示的是伪逆矩阵，o表示的是障碍物位置，

表示的是针对障碍物的斥力势场函数，

表示障碍物的影响范围外，

表示到达障碍物的边缘，其表达式为：

其次，式(11)中U_a，R是为了保证所规划路径可行，即路径曲线无奇点，其表达式为：

其中，

表示可以使路径奇异的控制点，S_i表示该控制点所对应节点的集合，针对奇异控制点设计的势场函数

在[0,R_R]上是单调的，其表达式如下：

最后，式(11)中U_a，I是为了实现软约束中保证移动机器人有效地执行所规划路径，并且所规划的路径尽可能地靠近兴趣点，其具体表达式为：

其中，

表示集合γ_S(x)中距离兴趣点r最近的点，其表达式为：

表示的是针对目标点设置的引力势场函数，因为此项作为软约束，优先权低于硬约束，所以势场函数存在上界U_I，该函数在[0,R_I]上严格单调，表达式如下：

具体实施例：

参见图2，其示出了本发明实施例中一种移动机器人非结构化环境下的人机协作式路径规划方法的流程图，该方法包括：

操纵员引导路径规划器：

本发明的操纵员引导路径规划是基于B样条曲线实现的，B样条曲线是B样条基函数的线性组合，由贝塞尔曲线改进而来，弥补了贝塞尔曲线不能进行局部修改的缺陷，曲线表达式为：

γ_S(x)＝{γ(x,s)∈R²|s∈S} (1)

其中，

表示控制点向量，s表示B样条曲线节点，集合S表示节点向量。

操纵员对路径的修改是通过操纵手柄完成的：

其中，q表示操纵手柄的构型向量，M(q)表示惯性矩阵，

表示科氏和离心项，τ表示反馈至触觉手柄的力，τ_h表示操纵者给手柄的力。

考虑到机器人空间与操纵手柄空间的不同性，本发明将操纵手柄的构型向量映射至路径控制点向量：

其中，

表示对应于期望路径γ_S(x_h)的控制点向量，可通过映射矩阵Q(x_h)进行在线修改。K表示正增益的对角矩阵。

映射矩阵Q(x_h)决定了操纵员如何通过操纵触觉设备来影响路径，分为旋转、平移以及按比例扩大缩小这三类变换，如图2-4所示。对应的矩阵分别为：

Q₁(x_h)＝I_2n (4)

其中，Q₁表示平移变换，I_2n表示2n×2n的单位矩阵，Q₂表示按比例扩大或者缩小变换，I_n表示n×n的单位矩阵，

表示克罗内克积运算，

表示变换的基准点，Q₃表示旋转变换，

diag(·)表示对角分块矩阵。

最后，由操纵员端给出的路径控制信号为：

其中，k_h为大于0的系数，其大小可根据经验值获得。

机器人自主路径规划器

机器人自主路径规划器设计需要满足两方面的约束，一方面是硬约束，首先要保证无障碍物，其次要保证所规划的路径是可行的，即无奇点。另一方面为了保证所规划路径的高效性，本发明加入了软约束，即所规划的路径在一定范围内趋向于兴趣点。因此，由机器人自主路径规划器给出的控制信号可表示为：

其中，每一项约束都是通过不同的势能函数来实现的：

首先，式(8)第一项是为了保证所规划路径是无障碍物的：

其中，

表示的是伪逆矩阵，o表示的是障碍物位置，

表示的是针对障碍物的斥力势场函数，

表示障碍物的影响范围外，

表示到达障碍物的边缘，其表达式为：

其次，式(8)第二项是为了保证所规划路径可行，即路径曲线无奇点：

其中，

表示可以使路径奇异的控制点，S_i表示该控制点所对应节点的集合，针对奇异控制点设计的势场函数

在[0,R_R]上是单调的，其表达式如下：

最后，式(8)第三项是为了保证移动机器人更为有效地执行所规划路径，加入了软约束，即所规划的路径要尽可能地靠近兴趣点：

其中，

表示集合γ_S(x)中距离兴趣点r最近的点，其表达式为：

表示的是针对目标点设置的引力势场函数，因为此项作为软约束，优先权低于硬约束，所以势场函数存在上界U_I，该函数在[0,R_I]上严格单调，表达式如下：

混合滤波器

将机器人自主移动和操纵员的控制信号融合并最终生成机器人的行驶路径，即：

考虑到融合之后路径的可行性，本发明设计了式(16)的混合项以实现二者的融合：

其中，I_2n表示单位矩阵，

表示J的广义逆矩阵，如式(17)所示，雅可比矩阵J考虑了路径的一些局部几何性质以保证融合之后的路径依然可行，比如路径点的位置γ(x(t),s(t))，路径的切矢量

路径的曲率

等等。

触觉反馈

触觉反馈模块主要是计算移动机器人实际行驶路径与上述规划路径之间的偏差信号，以此来作为操纵员的触觉提示。本发明设计了速度偏差

和位置偏差e_x两部分：

最终反馈至操纵手柄的力为：

其中，B表示正定阻尼矩阵，K^*表示增益的正定对角矩阵，K_M表示非负对角矩阵，用于提供对零指令速度距离的感知，k＞0根据经验值设定。

本发明的方法将基于力/触觉的遥操纵技术与自主规划技术结合在一起，通过基于B样条曲线的操纵员在线修改路径和基于势场概念的机器人局部规划路径，并将机器人的规划路径与实际行驶路径差值反馈至操纵员处，以实现人类高级决策与机器智能的有效融合，既克服了移动机器人无法自主完成野外复杂环境规划的缺点，也减轻了操纵员的操纵负担，提高了工作效率。

最后应说明的是：以上所述的各实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或全部技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (4)

1.一种移动机器人非结构化环境下的人机协作式路径规划方法，其特征在于，其包括以下步骤：

S1、操纵员引导路径规划器，具体包括以下子步骤：

S11、操纵员基于B样条曲线对路径进行规划；

S12、操纵员通过操纵手柄对路径进行修改：

S2、机器人自主路径规划器：机器人根据传感器提供的环境信息，进行基于势场的局部路径规划；

S3、对步骤S1得到的操纵员规划的路径以及步骤S2得到的机器人自主规划的路径进行混合滤波，将操纵员修改的路径与机器人自主规划的路径进行合成，该步骤具体包括以下子步骤：

S31、将机器人自主移动和操纵员的控制信号融合并生成机器人的行驶路径，即：

S32、通过式(2)的混合项实现二者的融合：

其中，I_2n表示单位矩阵，

表示J的广义逆矩阵，如式(3)所示，γ(x(t),s(t))为路径点的位置，

为路径的切矢量，

为路径的曲率；

S4、触觉反馈：将规划路径与机器人实际行驶的路径位置偏差与速度偏差以力/触觉的形式反馈至操纵员的手柄处。

2.根据权利要求1所述的移动机器人非结构化环境下的人机协作式路径规划方法，其特征在于，B样条曲线是B样条基函数的线性组合，其曲线表达式为：

γ_S(x)＝{γ(x,s)∈R²|s∈S} (4)

其中，

表示控制点向量，s表示B样条曲线节点，集合S表示节点向量。

3.根据权利要求2所述的移动机器人非结构化环境下的人机协作式路径规划方法，其特征在于，步骤S12具体包括以下步骤：

S121、建立操纵手柄的构型向量，公式为：

其中，q表示操纵手柄的构型向量，M(q)表示惯性矩阵，

表示科氏和离心项，τ表示反馈至触觉手柄的力，τ_h表示操纵者给手柄的力；

S122、将操纵手柄的构型向量映射至路径控制点向量：

其中，

表示对应于期望路径γ_S(x_h)的控制点向量，通过映射矩阵Q(x_h)进行在线修改，K表示正增益的对角矩阵；

其中，映射矩阵Q(x_h)决包括旋转、平移以及按比例扩大缩小三类变换，三类变换对应的矩阵分别为：

Q₁(x_h)＝I_2n (7)

其中，Q₁表示平移变换，I_2n表示2n×2n的单位矩阵，Q₂表示按比例扩大或者缩小变换，I_n表示n×n的单位矩阵，

表示克罗内克积运算，

表示变换的基准点，Q₃表示旋转变换，

diag(·)表示对角分块矩阵；

S123、操纵员端输出最终的路径控制信号：

其中，k_h为大于0的系数，其大小根据经验值获得。

4.根据权利要求1所述的移动机器人非结构化环境下的人机协作式路径规划方法，其特征在于，步骤S2中机器人自主路径规划路径需要满足硬约束和软约束，机器人自主路径规划器输出的控制信号表示为：

其中，每一项约束都均能够通过不同的势能函数实现：

首先，式(11)中U_a，o是为了保证所规划路径无障碍物，其具体表达式为：

其中，

表示的是伪逆矩阵，o表示的是障碍物位置，

表示的是针对障碍物的斥力势场函数，

表示障碍物的影响范围外，

表示到达障碍物的边缘，其表达式为：

其次，式(11)中U_a，R是为了保证所规划路径可行，即路径曲线无奇点，其表达式为：

其中，

表示可以使路径奇异的控制点，S_i表示该控制点所对应节点的集合，针对奇异控制点设计的势场函数

在[0,R_R]上是单调的，其表达式如下：

最后，式(11)中U_a，I是为了实现软约束中保证移动机器人有效地执行所规划路径，并且所规划的路径尽可能地靠近兴趣点，其具体表达式为：

其中，

表示集合γ_S(x)中距离兴趣点r最近的点，其表达式为：

表示的是针对目标点设置的引力势场函数，因为此项作为软约束，优先权低于硬约束，所以势场函数存在上界U_I，该函数在[0,R_I]上严格单调，表达式如下：

Images