CN114063621A - 一种轮式机器人编队跟踪与避障控制方法 - Google Patents

一种轮式机器人编队跟踪与避障控制方法 Download PDF

Info

Publication number
CN114063621A
CN114063621A CN202111383567.8A CN202111383567A CN114063621A CN 114063621 A CN114063621 A CN 114063621A CN 202111383567 A CN202111383567 A CN 202111383567A CN 114063621 A CN114063621 A CN 114063621A
Authority
CN
China
Prior art keywords
formation
wheeled robot
actual
obstacle
robot
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202111383567.8A
Other languages
English (en)
Other versions
CN114063621B (zh
Inventor
李丽
邝昌根
石浩达
李扬
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Yanshan University
Original Assignee
Yanshan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Yanshan University filed Critical Yanshan University
Priority to CN202111383567.8A priority Critical patent/CN114063621B/zh
Publication of CN114063621A publication Critical patent/CN114063621A/zh
Application granted granted Critical
Publication of CN114063621B publication Critical patent/CN114063621B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
    • G05D1/0289—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling with means for avoiding collisions between vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供一种轮式机器人编队跟踪与避障控制方法，即建立实际轮式机器人和虚拟机器人的运动学模型，采用领导‑跟随法得出实际轮式机器人和虚拟轮式机器人的编队跟踪误差模型；根据人工势场法，设计障碍物的斥力势场函数，求出障碍物对实际轮式机器人的斥力大小；利用障碍物对实际轮式机器人的合斥力大小对轮式机器人编队跟踪误差大小进行修正，得到一条无碰撞路径；对编队跟踪任务和编队避障任务设置优先级排序，同时根据输入输出反馈线性化方法设计分布式编队控制器，使轮式机器人编队不仅能够按照期望队形跟踪参考轨迹，还具备自主避障的能力。

Description

一种轮式机器人编队跟踪与避障控制方法
技术领域
本发明属于轮式机器人编队跟踪与避障控制领域，涉及一种轮式机器人编队跟踪与避障控制方法。
背景技术
随着人工智能技术的快速发展，机器人被越来越多地应用于智能驾驶、智能仓储物流、智能家居、工业自动化等领域。其中，轮式机器人因结构简单、操作灵活、控制简便、工作效率高等优点得到广泛应用。多机器人系统因相对单机器人具有系统性能稳定、自组织性好等优势,可以完成单机器人无法完成的复杂任务,并且提高工作的效率，也得到研究者的青睐。在多机器人系统中,编队是最基本的问题之一。此外，在实际应用场合中往往存在障碍物，这要求多机器人系统在执行编队跟踪任务的同时，又要具备编队避障的能力。因此，设计有效的编队控制策略同时实现编队跟踪任务和编队避障任务具有非常重要的研究价值和实际意义。
轮式机器人系统属于一类典型的欠驱动非线性控制系统，其满足非完整约束条件，这便给控制算法设计和稳定性分析带来一定的困难。目前，常见的轮式机器人编队轨迹跟踪控制方法有：反步控制、模型预测控制、滑模控制、PID控制、鲁棒控制、自适应控制、输入输出反馈线性化等。其中，输入输出反馈线性化方法能够将复杂非线性系统转化为线性系统，给系统控制设计和稳定性分析带来极大的便利，于是其经常被用于非线性系统控制设计。
路径规划是轮式机器人实现避障功能的主要方法。目前，常用的路径规划方法主要分为两类：全局路径规划和局部路径规划。
全局路径规划是指在已知的环境中给机器人规划一条无碰撞路径，规划路径的精度取决于获取环境信息的准确度，当环境发生变化时，该方法就不适用了。因此，全局路径规划是一种事前规划，对机器人系统的实时计算能力要求不高，但对环境模型的容错率和噪声干扰的鲁棒性较差。典型的全局路径规划方法有：A*算法、D*算法、快速搜索随机树(RRT)等。
局部路径规划是指当对环境要求完全未知或部分可知时，通过传感器对机器人的工作环境进行实时探测，以获取障碍物的信息，然后根据机器人当前的局部环境信息，为机器人实时规划一条无碰撞路径。局部路径规划需要搜索环境数据信息并对该环境模型进行动态更新和实时校正。因此，局部路径规划要求系统具有高速的数据信息处理能力和计算能力，对环境误差和噪声干扰具有较高的鲁棒性，但是由于缺乏全局环境信息，所以局部路径规划结果不一定是最优的，甚至可能找不到正确路径或完整路径。典型的局部路径规划方法有：人工势场法、元启发式算法、人工神经网络算法、模糊逻辑算法等。
发明内容
本发明的目的在于提供一种轮式机器人编队跟踪与避障控制方法。该方法通过障碍物作用于实际轮式机器人的合斥力大小，对编队跟踪误差大小进行实时修正以获得一条无碰撞路径，使轮式机器人编队不仅能够按照期望队形跟踪参考轨迹运动，还具备实时自主避障的能力。
为实现上述目的，本发明采用以下技术方案：一种轮式机器人编队跟踪与避障控制方法，其包括如下步骤：
S1、建立实际轮式机器人和虚拟轮式机器人的运动学模型，采用领导-跟随法得出实际轮式机器人和虚拟轮式机器人的编队跟踪误差模型；
S2、根据人工势场法，设计障碍物的斥力势场函数，求出障碍物对实际轮式机器人的斥力大小；
S3、利用障碍物对轮式机器人的合斥力大小对轮式机器人编队跟踪误差大小进行修正，得到一条无碰撞路径；
S4、根据输入输出反馈线性化方法设计分布式编队控制器，同时对编队跟踪任务和编队避障任务设置优先级排序，使轮式机器人编队不仅能够按照期望队形跟踪参考轨迹运动，还具备自主避障的能力。
本发明技术方案的进一步改进在于：步骤S1建立实际轮式机器人和虚拟轮式机器人的运动学模型，采用领导-跟随法得出实际轮式机器人和虚拟轮式机器人的编队跟踪误差模型，具体步骤如下：
S1.1、建立实际轮式机器人和虚拟轮式机器人的运动学模型；
Figure BDA0003366548980000031
其中，xi、yi、θi分别为实际轮式机器人i(i＝1,…,n)在全局坐标系下x坐标、y坐标和方向角的位姿信息；vi、ωi分别表示实际轮式机器人i的线速度和角速度。
Figure BDA0003366548980000032
其中，
Figure BDA0003366548980000033
分别为虚拟轮式机器人ir(i＝1,…,n)在全局坐标系下x坐标、y坐标和方向角的位姿信息；x0、y0、θ0分别为参考轨迹在全局坐标系下x坐标、y坐标和方向角的位姿信息；d表示轮式机器人的质心与其后轮连轴的距离；li、
Figure BDA00033665489800000311
分别为预先设计的虚拟机器人相对于参考轨迹的质心距离参数和方位角参数。
S1.2、通过坐标转换，得出实际轮式机器人和虚拟轮式机器人的编队跟踪误差模型；
Figure BDA0003366548980000034
Figure BDA0003366548980000035
Figure BDA0003366548980000036
其中，
Figure BDA0003366548980000037
表示虚拟轮式机器人位姿与实际轮式机器人位姿之间的偏差。
将上述编队跟踪误差模型求导得：
Figure BDA0003366548980000038
Figure BDA0003366548980000039
Figure BDA00033665489800000310
其中，v0、ω0分别为参考轨迹的线速度、角速度。
本发明所述步骤S2根据人工势场法，设计障碍物的斥力势场函数，求出障碍物对实际轮式机器人的斥力大小，具体方法如下：
S2.1、根据人工势场法设计障碍物j(j＝1,…,m)的斥力势场函数；
Figure BDA0003366548980000041
其中，Ui,j表示障碍物j作用于实际轮式机器人i的斥力势场函数；αj为大于0的可调斥力势场强度因子；Rj表示障碍物j的斥力势场区域半径；di,j为实际轮式机器人i与障碍物j的直线距离,即：
Figure BDA0003366548980000042
这里xj、yj分别表示障碍物j在全局坐标系下的x轴、y轴的坐标信息。
S2.2、根据障碍物j的斥力势场函数，计算出障碍物j对实际轮式机器人i的斥力大小；
Figure BDA0003366548980000043
这里，
Figure BDA0003366548980000044
其中，Fi,j为障碍物j对实际轮式机器人i的斥力大小；
Figure BDA0003366548980000045
分别为斥力势场函数Ui,j对变量x、y的偏导数；
Figure BDA0003366548980000046
分别表示在全局坐标系下x轴、y轴的方向向量。
本发明技术方案的进一步改进在于：步骤S3利用障碍物对实际轮式机器人i的合斥力大小对编队跟踪误差大小进行修正，得到一条无碰撞路径，具体步骤如下：
S3.1、根据力的叠加原理，计算出m个障碍物对轮式机器人i的合斥力大小如下:
Figure BDA0003366548980000047
Figure BDA0003366548980000051
其中，
Figure BDA0003366548980000052
分别表示合斥力在全局坐标系下x轴、y轴的分量大小。
S3.2、通过坐标变换，利用合斥力大小对轮式机器人编队跟踪误差大小修正为如下形式：
Figure BDA0003366548980000053
Figure BDA0003366548980000054
Figure BDA0003366548980000055
其中，
Figure BDA0003366548980000056
分别为修正后x轴、y轴的编队跟踪误差大小。
本发明技术方案的进一步改进在于：步骤S4对编队跟踪任务和编队避障任务设置优先级排序，同时根据输入输出反馈线性化方法设计分布式编队控制器，使轮式机器人编队不仅能够按照期望队形跟踪参考轨迹运动，还具备实时自主避障的能力，具体步骤如下：
S4.2、对编队跟踪任务和编队避障任务设置优先级排序；
若实际轮式机器人未进入障碍物的斥力势场区域，即：当di,j≥Rj时，实际轮式机器人的任务优先级为：编队跟踪任务>编队避障任务。此时，障碍物对实际轮式机器人的斥力大小为
Figure BDA0003366548980000057
无需对编队跟踪误差
Figure BDA0003366548980000058
Figure BDA0003366548980000059
进行修正；
若实际轮式机器人进入障碍物的斥力势场区域，即：当0<di,j<Rj时，实际轮式机器人的任务优先级为：编队避障任务>编队跟踪任务。此时，障碍物对实际轮式机器人的斥力大小为
Figure BDA00033665489800000510
需要对编队跟踪误差
Figure BDA00033665489800000511
进行修正。
S4.2、根据输入输出反馈线性化方法设计分布式编队控制器；
Figure BDA0003366548980000061
其中，
Figure BDA0003366548980000062
分别为实际轮式机器人i期望的线速度和角速度；kx、ky、kθ分别为可调的编队控制器参数，分别满足
Figure BDA0003366548980000063
Figure BDA0003366548980000064
kθ>0。这里，ε为任意大于d的常数，θE为辅助方向角变量。
本发明提出了一种基于输入输出反馈线性化方法和跟踪误差修正策略的轮式机器人编队跟踪与避障控制方法。通过对实际轮式机器人进行任务优先级排序，合理地给实际轮式机器人分配编队跟踪任务和编队避障任务。当实际轮式机器人优先执行编队避障任务时，利用障碍物对其的合斥力大小对编队跟踪误差进行实时修正以获得一条无碰撞路径，从而减少了传统路径规划方法耗时的问题。本发明根据输入输出反馈线性化方法设计的分布式编队控制器不仅保证了轮式机器人编队能够按照期望队形跟踪参考轨迹运动，还赋予实际轮式机器人实时自主避障的能力。因此，本发明更符合实际应用场景。
由于采用了上述技术方案，本发明取得的技术效果如下：
1、将人工势场法的应用对象由单个轮式机器人扩展到多个轮式机器人，并巧妙地利用了障碍物对轮式机器人编队的斥力来解决编队避障问题；
2、首次提出基于跟踪误差修正策略的轮式机器人编队避障方法，通过修正编队跟踪误差在线实时生成一条无碰撞路径，具有实时性强、对数据处理能力要求低、应用方便、低成本等优点；
3、通过对实际轮式机器人进行任务优先级排序，所设计的分布式编队控制器保证了轮式机器人编队能够顺利地执行编队跟踪任务和编队避障任务，更加符合实际应用要求。
附图说明
图1为轮式机器人编队跟踪示意图；
图2为本发明一种轮式机器人编队跟踪与避障控制方法的流程图；
图3为本发明实施例对圆形参考轨迹的编队跟踪与避障曲线图；
图4为本发明实施例编队x轴跟踪误差曲线图；
图5为本发明实施例编队y轴跟踪误差曲线图；
图6为本发明实施例编队方向角θ跟踪误差曲线图；
图7为本发明实施例轮式机器人编队受到的合斥力曲线图。
具体实施方式
下面结合附图和实施例对本发明轮式机器人编队跟踪与避障控制方法进行详细说明。需要说明的是，可以对此处公开的实施例做出各种修改，因此，说明书中公开的实施例不应该视为对本发明的限制，而仅是作为实施例的范例，其目的是使本发明的特征显而易见。
本发明公开的一种轮式机器人编队跟踪与避障控制方法，旨在轮式机器人编队既要完成编队跟踪任务，又要具备实时自主避障的能力，具体步骤为：
S1、建立实际轮式机器人和虚拟轮式机器人的运动学模型，采用领导-跟随法得出实际轮式机器人和虚拟轮式机器人的编队跟踪误差模型。
本发明首先建立实际轮式机器人和虚拟轮式机器人的运动学模型，然后，通过坐标转换，建立轮式机器人编队跟踪误差模型。将轮式机器人编队跟踪任务转换为实际轮式机器人与虚拟轮式机器人的位置误差最小问题。
具体步骤如下：
S1.1、建立实际轮式机器人和虚拟轮式机器人的运动学模型。
图1为轮式机器人编队跟踪示意图。如图1所示，全局坐标系{X,O,Y}是绝对的、静止的坐标系，局部坐标系{X′,O′,Y′}表示实际轮式机器人主体的方向和位置。实际轮式机器人要跟踪的期望轨迹由虚拟轮式机器人产生，而虚拟轮式机器人的运动轨迹由参考轨迹产生，而参考轨迹由计算机产生。
实际轮式机器人i(i＝1,…,n)的运动学模型如下：
Figure BDA0003366548980000071
其中，qi＝[xi,yi,θi]T表示实际轮式机器人i在全局坐标系下的x坐标、y坐标、方向角的位姿信息；d表示实际轮式机器人的质心与其后轮连轴的距离；vi、ωi分别表示实际轮式机器人i的线速度、角速度。
虚拟轮式机器人ir(i＝1,…,n)的运动学模型如下：
Figure BDA0003366548980000081
其中，
Figure BDA0003366548980000082
为虚拟轮式机器人ir在全局坐标系下的x坐标、y坐标、方向角的位姿信息；x0、y0、θ0分别为参考轨迹在全局坐标系下x坐标、y坐标和方向角的位姿信息；li、
Figure BDA0003366548980000083
分别为预先设计的虚拟轮式机器人ir相对于参考轨迹的质心距离参数和方位角参数。
S1.2、通过坐标转换，得出实际轮式机器人i和虚拟轮式机器人ir的编队跟踪误差模型。
从全局坐标系到局部坐标系的变换矩阵定义如下：
Figure BDA0003366548980000084
根据实际轮式机器人i的运动学模型(1)、虚拟轮式机器人ir的运动学模型(2)以及坐标转换矩阵(3)，定义
Figure BDA0003366548980000085
为实际轮式机器人i的编队跟踪误差，建立如下实际轮式机器人i和虚拟轮式机器人ir的编队跟踪误差模型：
Figure BDA0003366548980000086
Figure BDA0003366548980000087
Figure BDA0003366548980000088
其中，
Figure BDA0003366548980000089
表示虚拟轮式机器人位姿与实际轮式机器人位姿之间的偏差。
将编队跟踪误差模型(4)求导可得：
Figure BDA00033665489800000810
Figure BDA00033665489800000811
Figure BDA00033665489800000812
其中，
Figure BDA00033665489800000813
分别为
Figure BDA00033665489800000814
的导数；v0、ω0分别为参考轨迹的线速度、角速度。
S2、根据人工势场法，设计障碍物的斥力势场函数，求出障碍物对实际轮式机器人的斥力大小。
对于实际轮式机器人而言，不仅要避免与障碍物的碰撞，同时还要避免编队内部轮式机器人之间的碰撞。因此，实际轮式机器人的邻居机器人同环境障碍物一样，也被视为它的障碍物，后面叙述将统称为障碍物。
具体方法如下：
S2.1、根据人工势场法设计障碍物j(j＝1,…,m)的斥力势场函数；
Figure BDA0003366548980000091
其中，Ui,j表示障碍物j作用于实际轮式机器人i的斥力势场函数；αj为大于0的可调斥力势场强度因子；Rj表示障碍物j的斥力势场区域半径；di,j为实际轮式机器人i与障碍物j的直线距离,即：
Figure BDA0003366548980000092
这里xj、yj分别表示障碍物j在全局坐标系下的x轴、y轴的坐标信息。
S2.2、根据障碍物j的斥力势场函数(6)，计算出障碍物j对实际轮式机器人i的斥力大小；
Figure BDA0003366548980000093
这里，
Figure BDA0003366548980000094
其中，Fi,j为障碍物j对实际轮式机器人i的斥力大小，斥力方向为实际轮式机器人到障碍物方向的反方向；
Figure BDA0003366548980000095
分别为斥力势场函数Ui,j对变量x、y的偏导数，即：斥力Fi,j在全局坐标系下x轴、y轴上的斥力分量；
Figure BDA0003366548980000096
分别表示在全局坐标系下x轴、y轴的方向向量。
S3、利用障碍物对实际轮式机器人的合斥力大小对编队跟踪误差大小进行修正，得到一条无碰撞路径。
编队跟踪误差为虚拟轮式机器人位姿与当前实际轮式机器人位姿的差值。因为当前实际轮式机器人位姿由传感器实时测量所得，无法更改，所以利用合斥力修正编队跟踪误差实质上是对虚拟轮式机器人的位姿进行了修正。因此，在实际轮式机器人执行编队避障任务时，所得到的无碰撞路径本质上为修正后的虚拟轮式机器人的运动轨迹。
具体步骤如下：
S3.1、根据力的叠加原理，计算出m个障碍物对实际轮式机器人i的合斥力大小如下:
Figure BDA0003366548980000101
Figure BDA0003366548980000102
其中，
Figure BDA0003366548980000103
分别表示合斥力在全局坐标系下x轴、y轴的分量大小。
S3.2、通过坐标变换，利用合斥力大小对轮式机器人编队跟踪误差大小修正为如下形式：
Figure BDA0003366548980000104
Figure BDA0003366548980000105
Figure BDA0003366548980000106
其中，
Figure BDA0003366548980000107
分别为修正后x轴、y轴的编队跟踪误差大小。
本发明所述步骤S4对编队跟踪任务和编队避障任务设置优先级排序，同时根据输入输出反馈线性化方法设计分布式编队控制器，使轮式机器人编队不仅能够按照期望队形跟踪参考轨迹运动，还具备自主避障的能力。
具体步骤如下：
S4.1、对编队跟踪任务和编队避障任务设置优先级排序；
若实际轮式机器人未进入障碍物的斥力势场区域，即：当di,j≥Rj时，实际轮式机器人的任务优先级为：编队跟踪任务>编队避障任务。此时，障碍物对实际轮式机器人的斥力大小为
Figure BDA0003366548980000111
无需对编队跟踪误差
Figure BDA0003366548980000112
Figure BDA0003366548980000113
进行修正；
若实际轮式机器人进入障碍物的斥力势场区域，即：当0<di,j<Rj时，实际轮式机器人的任务优先级为：编队避障任务>编队跟踪任务。此时，障碍物对实际轮式机器人的斥力大小为
Figure BDA0003366548980000114
需要对编队跟踪误差
Figure BDA0003366548980000115
进行修正。
S4.2、根据输入输出反馈线性化方法设计分布式编队控制器；
Figure BDA0003366548980000116
其中，
Figure BDA0003366548980000117
分别为实际轮式机器人i期望的线速度和角速度；kx、ky、kθ分别为可调的编队控制器参数，分别满足
Figure BDA0003366548980000118
Figure BDA0003366548980000119
kθ>0。这里，ε为任意大于d的常数，θE为辅助方向角变量。
另外，通过李雅普诺夫稳定理论对该分布式编队控制器进行稳定性分析。这里，假设实际轮式机器人的速度能够完美跟踪上期望速度，即：
Figure BDA00033665489800001110
Figure BDA00033665489800001111
稳定性分析分以下两种情形：
情形一：当实际轮式机器人未进入障碍物的斥力势场区域时，编队跟踪任务优先级大于编队避障任务。障碍物对实际轮式机器人的斥力大小为
Figure BDA00033665489800001112
无需对编队跟踪误差
Figure BDA00033665489800001113
进行修正，即：
Figure BDA00033665489800001114
选取李亚普诺夫函数为
Figure BDA00033665489800001115
并对其求导可得：
Figure BDA0003366548980000121
将(5)、(8)式代入上式并化简后得：
Figure BDA0003366548980000122
由李雅普诺夫稳定理论可知，所设计的分布式编队控制器能够保证编队跟踪误差趋于零。
情形二：当实际轮式机器人进入障碍物的斥力势场区域时，编队避障任务优先级大于编队跟踪任务。障碍物对实际轮式机器人的斥力大小为
Figure BDA0003366548980000123
需要对编队跟踪误差
Figure BDA0003366548980000124
进行修正，即:
Figure BDA0003366548980000125
Figure BDA0003366548980000126
选取李亚普诺夫函数为
Figure BDA0003366548980000127
并对其求导可得：
Figure BDA0003366548980000128
因为编队避障任务的优先级高于编队跟踪任务，所以令v0＝0、ω0＝0，并将(5)、(8)式代入上式并化简后得：
Figure BDA0003366548980000129
构造辅助方向角变量为：
Figure BDA0003366548980000131
于是，
Figure BDA0003366548980000132
通过杨氏不等式放缩技巧，(9)式可以写成如下形式：
Figure BDA0003366548980000133
根据李雅普诺夫稳定理论可知，要想保证所设计的分布式编队控制器的稳定性，则必须要有
Figure BDA0003366548980000134
恒成立。因此，令(10)式小于0求得控制参数的取值范围为：
Figure BDA0003366548980000135
kθ>0、ε>d。
实施例
为验证本发明提出的轮式机器人编队跟踪与避障控制方法的有效性，给出了实际实验结果作为验证，说明了轮式机器人编队不仅能够按照期望队形跟踪参考轨迹运动，还具备实时自主避障的能力。实际轮式机器人的给定参数如下：
步骤S1.1、参考轨迹为圆形，运动方向为逆时针、半径为1.5m、线速度为v0＝0.45m/s、角速度为ω0＝0.3rad/s。轮式机器人的编队参数为：l1＝0m、l2＝0.8m、l3＝0.8m、
Figure BDA0003366548980000136
d＝0.1m；
步骤S2.1、障碍物(包括邻居机器人)的斥力势场强度因子为αj＝2.0，其斥力势场区域半径Rj＝1.0m；
步骤S4.2、分布式编队控制器的给定参数下表所示。
轮式机器人 k<sub>x</sub> k<sub>y</sub> k<sub>θ</sub>
1号机器人 1.2 4.5 2.5
2号机器人 1.5 2.5 0.4
3号机器人 1.0 3.8 1.3
图3为圆形参考编队跟踪与避障曲线图，实际轮式机器人初始位置为：q1＝[1.2,1.2,0]T、q2＝[0.5,1.7,0]T、q3＝[0.6,0.6,0]T。由图可知，当实际轮式机器人在障碍物斥力势场区域外时，则优先执行编队跟踪任务；当实际轮式机器人进入障碍物斥力势场区域时，则优先执行编队避障任务。因此，本发明所提出的一种轮式机器人编队跟踪与避障控制方法既实现了编队跟踪任务，又能完成编队避障任务。
图4、图5、图6分别为x轴、y轴、方向角θ的编队跟踪误差曲线图，在t＝5s内，轮式机器人编队优先执行编队跟踪任务，编队跟踪误差
Figure BDA0003366548980000141
均收敛到零，
Figure BDA0003366548980000142
收敛到一个定常值；在t＝6.5s～11.0s时间段内，轮式机器人编队位于障碍物的斥力势场区域，则优先执行编队避障任务；在t＝11.0s后，轮式机器人编队驶离障碍物的斥力势场区域，则优先执行编队跟踪任务。因此，本发明所设置的编队任务优先级方案在实际应用中是有效的。
图7为障碍物对实际轮式机器人的合斥力大小曲线图。由图可知，当实际轮式机器人在障碍物的斥力势场区域外时，合斥力为0；反之，合斥力不为0。
本发明未尽事宜为公知技术。
上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所作的等效变化或修饰，都应涵盖在本发明的保护范围之内。

Claims (5)

1.一种轮式机器人编队跟踪与避障控制方法，其特征在于：其包括如下步骤：
S1、建立实际轮式机器人和虚拟轮式机器人的运动学模型，采用领导-跟随法得出实际轮式机器人和虚拟轮式机器人的编队跟踪误差模型；
S2、根据人工势场法，设计障碍物的斥力势场函数，求出障碍物对实际轮式机器人的斥力大小；
S3、利用障碍物对实际轮式机器人的合斥力大小对轮式机器人编队跟踪误差大小进行修正，得到一条无碰撞路径；
S4、对编队跟踪任务和编队避障任务设置优先级排序，同时根据输入输出反馈线性化方法设计分布式编队控制器，使轮式机器人编队不仅能够按照期望队形跟踪参考轨迹运动，还具备实时自主避障的能力。
2.根据权利要求1所述的一种轮式机器人编队跟踪与避障控制方法，其特征在于：所述步骤S1建立实际轮式机器人和虚拟轮式机器人的运动学模型，采用领导-跟随法得出实际轮式机器人和虚拟轮式机器人的编队跟踪误差模型，具体步骤如下：
S1.1、建立实际轮式机器人和虚拟轮式机器人的运动学模型；
Figure FDA0003366548970000011
其中，xi、yi、θi分别为实际轮式机器人i(i＝1,…,n)在全局坐标系下x坐标、y坐标和方向角的位姿信息；vi、ωi分别表示实际轮式机器人i的线速度和角速度；
Figure FDA0003366548970000012
其中，
Figure FDA0003366548970000013
分别为虚拟轮式机器人ir(i＝1,…,n)在全局坐标系下x坐标、y坐标和方向角的位姿信息；x0、y0、θ0分别为参考轨迹在全局坐标系下x坐标、y坐标和方向角的位姿信息；d表示轮式机器人的质心与其后轮连轴的距离；li、
Figure FDA0003366548970000021
分别为预先设计的虚拟机器人相对于参考轨迹的质心距离参数和方位角参数；
S1.2、通过坐标转换，得出实际轮式机器人和虚拟轮式机器人的编队跟踪误差模型；
Figure FDA0003366548970000022
Figure FDA0003366548970000023
Figure FDA0003366548970000024
其中，
Figure FDA0003366548970000025
表示虚拟轮式机器人位姿与实际轮式机器人位姿之间的偏差；
将上述编队跟踪误差模型求导得：
Figure FDA0003366548970000026
Figure FDA0003366548970000027
Figure FDA0003366548970000028
其中，v0、ω0分别为参考轨迹的线速度、角速度。
3.根据权利要求1所述的一种轮式机器人编队跟踪与避障控制方法，其特征在于：所述步骤S2中根据人工势场法设计障碍物的斥力势场函数，求出障碍物对实际轮式机器人的斥力函数，具体方法如下：
S2.1、根据人工势场法设计障碍物j(j＝1,…,m)的斥力势场函数；
Figure FDA0003366548970000029
其中，Ui,j表示障碍物j作用于实际轮式机器人i的斥力势场函数；αj为大于0的可调斥力势场强度因子；Rj表示障碍物j的斥力势场区域半径；di,j为实际轮式机器人i与障碍物j的直线距离,即：
Figure FDA00033665489700000210
这里xj、yj分别表示障碍物j在全局坐标系下的x轴、y轴的坐标信息；
S2.2、根据障碍物j的斥力势场函数，计算出障碍物j对实际轮式机器人i的斥力大小；
Figure FDA0003366548970000031
这里，
Figure FDA0003366548970000032
其中，Fi,j为障碍物j对轮式机器人i的斥力大小；
Figure FDA0003366548970000033
分别为斥力势场函数Ui,j对变量x、y的偏导数；
Figure FDA0003366548970000034
分别表示在全局坐标系下x轴、y轴的方向向量。
4.根据权利要求1所述的一种轮式机器人编队跟踪与避障控制方法，其特征在于：所述步骤S3利用障碍物对实际轮式机器人的合斥力大小对编队跟踪误差大小进行修正的具体步骤如下：
S3.1、根据力的叠加原理，计算出m个障碍物对实际轮式机器人i的合斥力大小如下：
Figure FDA0003366548970000035
Figure FDA0003366548970000036
其中，
Figure FDA0003366548970000037
分别表示合斥力在全局坐标系下x轴、y轴的分量大小；
S3.2、通过坐标变换，利用合斥力大小对轮式机器人编队跟踪误差大小修正为如下形式：
Figure FDA0003366548970000038
Figure FDA0003366548970000039
Figure FDA00033665489700000310
其中，
Figure FDA00033665489700000311
分别为修正后的编队x轴、y轴的跟踪误差大小。
5.根据权利要求1所述的一种轮式机器人编队跟踪与避障控制方法，其特征在于：所述步骤S4对编队跟踪任务和编队避障任务设置优先级排序，同时根据输入输出反馈线性化方法设计分布式编队控制器，使轮式机器人编队不仅能够按照期望队形跟踪参考轨迹运动，还具备自主避障的能力，具体步骤如下：
S4.1、对编队跟踪任务和编队避障任务设置优先级排序；
若实际轮式机器人未进入障碍物的斥力势场区域，即：当di,j≥Rj时，实际轮式机器人的任务优先级为：编队跟踪任务>编队避障任务；此时，障碍物对实际轮式机器人的斥力大小为
Figure FDA0003366548970000041
无需对编队跟踪误差
Figure FDA0003366548970000042
Figure FDA0003366548970000043
进行修正；
若实际轮式机器人进入障碍物的斥力势场区域，即：当0<di,j<Rj时，实际轮式机器人的任务优先级为：编队避障任务>编队跟踪任务；此时，障碍物对实际轮式机器人的斥力大小为
Figure FDA0003366548970000044
需要对编队跟踪误差
Figure FDA0003366548970000045
进行修正；
S4.2、根据输入输出反馈线性化方法设计分布式编队控制器；
Figure FDA0003366548970000046
其中，
Figure FDA0003366548970000047
分别为实际轮式机器人i期望的线速度和角速度；kx、ky、kθ分别为大于0的可调编队控制器参数。
CN202111383567.8A 2021-11-22 2021-11-22 一种轮式机器人编队跟踪与避障控制方法 Active CN114063621B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111383567.8A CN114063621B (zh) 2021-11-22 2021-11-22 一种轮式机器人编队跟踪与避障控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111383567.8A CN114063621B (zh) 2021-11-22 2021-11-22 一种轮式机器人编队跟踪与避障控制方法

Publications (2)

Publication Number Publication Date
CN114063621A true CN114063621A (zh) 2022-02-18
CN114063621B CN114063621B (zh) 2023-10-24

Family

ID=80278799

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111383567.8A Active CN114063621B (zh) 2021-11-22 2021-11-22 一种轮式机器人编队跟踪与避障控制方法

Country Status (1)

Country Link
CN (1) CN114063621B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115469665A (zh) * 2022-09-16 2022-12-13 广东工业大学 一种适应于动态环境的智能轮椅目标跟踪控制方法和系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101605994B1 (ko) * 2015-07-07 2016-03-24 중앙대학교 산학협력단 미지의 스키딩과 슬리핑을 갖는 이동 로봇을 위한 적응적 선도-추종 군집 제어 방법 및 그 장치
CN108459612A (zh) * 2017-02-21 2018-08-28 北京航空航天大学 基于人工势场法的无人机编队控制方法及装置
CN110109351A (zh) * 2019-04-08 2019-08-09 广东工业大学 一种基于指定性能的多智能体一致性控制方法
CN110703692A (zh) * 2019-07-30 2020-01-17 浙江工业大学 一种基于虚拟结构法的多移动机器人分布式预测控制方法
CN112731942A (zh) * 2020-12-30 2021-04-30 哈尔滨工程大学 一种基于改进领航者虚拟结构法的多auv编队控制方法
CN113110495A (zh) * 2021-05-08 2021-07-13 广东省智能机器人研究院 一种考虑外部干扰下移动机器人的编队控制方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101605994B1 (ko) * 2015-07-07 2016-03-24 중앙대학교 산학협력단 미지의 스키딩과 슬리핑을 갖는 이동 로봇을 위한 적응적 선도-추종 군집 제어 방법 및 그 장치
CN108459612A (zh) * 2017-02-21 2018-08-28 北京航空航天大学 基于人工势场法的无人机编队控制方法及装置
CN110109351A (zh) * 2019-04-08 2019-08-09 广东工业大学 一种基于指定性能的多智能体一致性控制方法
CN110703692A (zh) * 2019-07-30 2020-01-17 浙江工业大学 一种基于虚拟结构法的多移动机器人分布式预测控制方法
CN112731942A (zh) * 2020-12-30 2021-04-30 哈尔滨工程大学 一种基于改进领航者虚拟结构法的多auv编队控制方法
CN113110495A (zh) * 2021-05-08 2021-07-13 广东省智能机器人研究院 一种考虑外部干扰下移动机器人的编队控制方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
LEONARD N E，FIORELLI E: "Adaptive image-baesed trajectory tracking control of wheeled mobile robots with an uncalibrated fixed camera", IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, vol. 23, no. 6, pages 2266 - 2282, XP011586916, DOI: 10.1109/TCST.2015.2411627 *
李芸等: "领航跟踪法和势函数组合的船舶编队控制", 控制理论与应用, vol. 33, no. 8, pages 1259 - 1264 *
桑雷;吕强;: "基于人工势场法的多机器人编队与避障", 信息系统工程, no. 03, pages 139 - 145 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115469665A (zh) * 2022-09-16 2022-12-13 广东工业大学 一种适应于动态环境的智能轮椅目标跟踪控制方法和系统

Also Published As

Publication number Publication date
CN114063621B (zh) 2023-10-24

Similar Documents

Publication Publication Date Title
CN112757306B (zh) 一种机械臂逆解多解选择和时间最优轨迹规划算法
Zhang et al. Real-time kinematic control for redundant manipulators in a time-varying environment: Multiple-dynamic obstacle avoidance and fast tracking of a moving object
CN106625666B (zh) 冗余机械臂的控制方法及装置
Peng et al. Leader–follower formation control of nonholonomic mobile robots based on a bioinspired neurodynamic based approach
CN113601512B (zh) 一种机械臂奇异点的通用规避方法与系统
Xu et al. Dynamic neural networks based kinematic control for redundant manipulators with model uncertainties
CN109491389B (zh) 一种具有速度约束的机器人轨迹跟踪方法
CN111665855B (zh) 一种机器人预设性编队控制方法
Resende et al. A nonlinear trajectory tracking controller for mobile robots with velocity limitation via fuzzy gains
CN112578671B (zh) 一种基于u模型优化smc的agv轨迹跟踪控制方法
CN114721275B (zh) 一种基于预设性能的视觉伺服机器人自适应跟踪控制方法
CN112975938B (zh) 一种基于零空间的机械臂速度层轨迹规划方法
Yuan et al. Consistent path planning for on-axle-hitching multisteering trailer systems
CN116061191B (zh) 仿生蛇形机器人主动避障的模型预测控制方法
He et al. Collaboration of multiple SCARA robots with guaranteed safety using recurrent neural networks
CN114063621B (zh) 一种轮式机器人编队跟踪与避障控制方法
Yan et al. Adaptive and intelligent control of a dual-arm space robot for target manipulation during the post-capture phase
Yang et al. Pose control of constrained redundant arm using recurrent neural networks and one-iteration computing algorithm
Jin et al. Consensus based attractive vector approach for formation control of nonholonomic mobile robots
Xie et al. A fuzzy neural controller for model-free control of redundant manipulators with unknown kinematic parameters
Xiao et al. NA-OR: A path optimization method for manipulators via node attraction and obstacle repulsion
Galicki Control-based solution to inverse kinematics for mobile manipulators using penalty functions
Wu et al. Reinforcement learning approach to the control of heavy material handling manipulators for agricultural robots
Low Adaptable virtual structure formation tracking control design for nonholonomic tracked mobile robots, with experiments
Vergara et al. Generating reactive approach motions towards allowable manifolds using generalized trajectories from demonstrations

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant