CN115185273A - 一种空地异构多机器人轨迹跟踪平台 - Google Patents

Abstract

本发明公开了一种空地异构多机器人轨迹跟踪平台，所述多机器人轨迹跟踪平台包括轮式移动机器人系统和四旋翼飞行系统，其特征在于：所述轮式移动机器人系统和四旋翼飞行系统均通过位置‑速度双闭环控制框架下的外环位置控制模块和内环速度控制模块实现系统的轨迹跟踪；本发明基于超宽带测距，设计积分滑模控制器，实现对协同系统的轨迹跟踪控制，提高了系统对于位置和环境的自主性和适应性，增强了系统的鲁棒性与容错性，应用范围更加广泛。

Description

一种空地异构多机器人轨迹跟踪平台

技术领域：

本发明属于空地协同轨迹跟踪控制技术领域，具体涉及一种空地异构多机器人轨迹跟踪平台。

背景技术：

机器人作为多个综合性学科的集成体，具备一定环境感知、信息融合以及任务执行的能力。在一些简单的重复的任务场景中，单个机器人可以有效地满足人们的需求。然而，对于在一些大型复杂的任务环境与空间中，单个机器人的行为决策能力还是十分有限，于是人们提出利用多个机器人的协同来弥补单个机器人的不足。

地面移动机器人与空中飞行机器人组成的空地异构多机器人系统，以其广泛的应用需求，其不可取代的显著优势，得到了众多控制与机器人领域学者的青睐。空地异构多机器人系统通过空中和地面的协作和互补，能够完成单独种类机器人编队无法完成的任务，如大片区域排查、地理勘测、跟踪追逃、协同定位、救援和运输。协作系统在很多领域都有着重要的应用意义。空地协作系统在军用方面用于险区作战、敌情侦察、态势感知等；在民用方面，可用于地形地貌侦测、农业信息采集、交通信息釆集控制等方面。应用于危险环境搜救能够有效减少作业的人员伤亡并且提高作业效率。

因此，提出的基于双闭环控制的空地异构多机器人系统，不仅确保轮式移动机器人在复杂环境下运动的准确性，而且实现了四旋翼飞行器与轮式移动机器人协同控制。

发明内容：

本发明的目的是针对地面移动机器人与四旋翼飞行器组成的空地异构多机器人系统轨迹跟踪控制技术存在的不足，提出了一种基于超宽带定位的位置-速度双闭环控制框架下的协同轨迹跟踪控制方法，使空地异构多机器人系统能够精确地跟踪期望轨迹。

本发明的技术解决方案为：

一种空地异构多机器人轨迹跟踪平台，所述多机器人轨迹跟踪平台包括轮式移动机器人系统和四旋翼飞行系统，所述轮式移动机器人系统和四旋翼飞行系统均通过位置-速度双闭环控制框架下的外环位置控制模块和内环速度控制模块实现系统的轨迹跟踪；其中：所述轮式移动机器人系统与所述四旋翼飞行系统通过如下步骤实现空地异构多机器人的协同轨迹跟踪控制：

所述轮式移动机器人系统的外环位置控制模块通过两轮差速特性建立如下运动学模型；

其中，m＝(x_m,y_m,ε_m)^T表示轮式移动机器人的姿态位置和方向位置，

表示轮式移动机器人的线速度和角速度信息；

所述轮式移动机器人系统的内环速度控制模块使用PI控制方法对运动学模型计算生成轮式移动机器人系统的运动学控制器，即G_K(t)；

所述四旋翼飞行系统的外环位置控制模块通过飞行器位置控制的响应速度建立四旋翼飞行系统的积分滑模控制器；

所述四旋翼飞行系统的内环速度控制使用PID控制算法生成四旋翼动力学模型。

进一步，所述轮式移动机器人系统的外环位置控制模块通过两轮差速特性建立运动学模型过程：

根据论述移动机器人两个轮子之间的距离是d，驱动轮的直径是2r计算左右车轮通过电机驱动接口输出的角转速

和

则线速度v_l(t)和v_r(t)为：

根据两驱动轮中心连线的中点为机器的基点O_p,瞬时线速度为v_m(t)，瞬时角速度

姿态角ε_m即为v_m(t)与X轴夹角，小车的瞬时线速度为v_m(t)可以表示为：

根据左右轮及基点所处位置在该圆周运动中的角速度相同

则机器的瞬时角速度

可以表示为：

联立两式，利用v_r(t)和v_l(t)求出机器转动半径：

在驱动轮与地面接触运动为纯滚动无滑动情况下，机器的运动学模型可以表示为：

进而运动学模型可以写成如下形式：

其中，m＝(x_m,y_m,ε_m)^T表示轮式移动机器人的姿态位置和方向位置，

表示轮式移动机器人的线速度和角速度信息。

进一步，所述轮式移动机器人系统的运动学控制器生成过程：

通过轮式移动真实机器人按照误差方程建立虚拟机器人运动模型，

其中：v_rr(t)和

分别是虚拟线速度和角速度；

根据虚拟机器人运动模型建立全局坐标系{X_L,O,Y_L}下的位置和方向误差方程为：

通过坐标变换将全局坐标系{X_L,O,Y_L}下的位置和方向误差方程式转换为局部坐标系{X_M,O_C,Y_M}；

通过计算上式中位置和方向误差方程的导数，得到：

对于上式跟踪误差系统，运动控制器设计为：

其中：v_K(t)和

分别是与给定参考轨迹保持一致的理想线速度和角速度，k₀，k₁和k₂是正可调参数。

进一步，所述四旋翼飞行系统的内环速度控制使用PID控制算法生成四旋翼动力学模型过程：

根据牛顿第二定律、欧拉方程对于四旋翼飞行器进行建模分析，推导出四旋翼动力学模型；

为了简化计算，令

其中，s(·)，c(·)分别为sin(·)，cos(·)的缩写；(x(t),y(t),z(t))，

分别表示四旋翼无人机的位置和三种姿态角(横滚、俯仰、偏航)；k_t表示线性阻力；g为重力加速度；m为重力加速度；I_x,I_y,I_z分别为沿x,y,z轴的转动惯量；u_i(t)(i＝1,2,3,4)为控制输入；u₁(t)表示z方向上的总升力，u₂(t)、u₃(t)、u₄(t)表示无人机在不同轴向的三个转动力矩。

进一步，所述四旋翼飞行系统的外环位置控制模块通过飞行器位置控制的响应速度建立四旋翼飞行系统的积分滑模控制器过程：

(h_x(t),h_y(t))表示位置期望值，e_xq(t)和e_yq(t)是期望位置与实际位置的偏差，存在：

本策略以X方向为例，所设计的积分滑模面为：

式中，γ₁是正可调参数，对滑模面求导可得

其中：

积分滑模控制器为

式中，γ₂和γ₃都是正的可调参数。

有益效果

本发明与现有技术相比有如下优点：

(1)搭建了空地异构多机器人系统，提高了系统对于位置和环境的自主性和适应性，有效改善了传统单机器人功能的缺失，提高了工作效率，增强了系统的鲁棒性与容错性，应用范围更加广泛。

(2)采用的超宽带技术可以穿透墙壁和森林，相较于传统的窄带射频信号，时间分辨率高，且该平台传输数据简单，对机器人处理数据能力要求低，有利于真正实现协同系统的自主控制。

(3)传统的PID控制算法鲁棒性差、抗干扰能力弱，在外界干扰下不能保证控制的速度与精度。而本发明方法基于位置-速度双闭环控制框架，通过引入非线性函数改进了经典PID控制算法，提高了控制器的控制精度与响应速度，使控制性能更好。

附图说明：

图1为空地异构多机器人协同控制系统模型图；

图2为轮式移动机器人运动学模型图；

图3为四旋翼飞行器动力学模型；

图4为空地异构多机器人协同控制轨迹跟踪图；

图5为轮式移动机器人位置、角度误差图；

图6为四旋翼飞行器位置误差图。

具体实施方式：

以下结合附图和具体实施例对本发明进行详细说明。

如图1给出了整个系统的控制结构图。本发明基于超宽带定位的位置-速度双闭环控制框架下的协同轨迹跟踪控制，实现系统的轨迹跟踪控制。其中，所述位置-速度双闭环控制框架由外环位置控制模块和内环速度控制模块构成；对于轮式移动机器人，所述外环位置控制模块由运动学控制器构成；所述内环速度控制使用PI控制算法；对于四旋翼飞行器，所述外环位置控制模块由积分滑模控制器构成；所述内环速度控制使用PID控制算法。所述位置-速度双闭环控制框架。本发明通过位置-速度双闭环控制框架，实现空地异构多机器人的协同轨迹跟踪控制，控制过程：

首先，建立轮式移动机器人的运动学模型；

其次，为了使轮式移动机器人达到期望位置，外环采用运动学控制器，内环采用PI控制方法；

然后，建立四旋翼飞行器的位置系统模型，为了实现轨迹跟踪的协同控制，外环采用积分滑模控制器，提高其精度，内环采用PID控制方法；

最终，实现空地异构多机器人轨迹跟踪协同控制。具体过程：

步骤1：根据两轮差速特性，对于轮式移动机器人进行运动学建模：

如图2所示，移动机器人两个轮子之间的距离是d，驱动轮的直径是2r。左右车轮通过电机驱动接口输出的角转速

和

则线速度v_l(t)和v_r(t)为：

两驱动轮中心连线的中点为机器的基点O_p,瞬时线速度为v_m(t)，瞬时角速度

姿态角ε_m即为v_m(t)与X轴夹角，小车的瞬时线速度为v_m(t)可以表示为：

左右轮及基点所处位置在该圆周运动中的角速度相同

但是到基点的半径不同，存在

则机器的瞬时角速度

可以表示为：

联立两式，利用v_r(t)和v_l(t)求出机器转动半径：

理想情况下即为左右轮转动时做圆周运动的线速度。

在驱动轮与地面接触运动为纯滚动无滑动情况下，机器的运动学模型可以表示为：

进而运动学模型可以写成如下形式：

其中，m＝(x_m,y_m,ε_m)^T表示轮式移动机器人的姿态位置和方向位置，

表示轮式移动机器人的线速度和角速度信息。

步骤2，所述轮式移动机器人的运动学控制器，根据轮式移动机器人的运动学模型设计：

在实际情况中，虚拟机器人和真实机器人之间存在跟踪位置误差。定义m_r＝(x_r(t),y_r(t),ε_r(t))^T表示虚拟机器人的位置姿态，也就是期望的运动姿态。

虚拟机器人的运动满足下面的方程：

其中v_rr(t)和

分别是虚拟线速度和角速度。然后，获得全局坐标系{X_M,O_C,Y_M}下的位置和方向误差方程为：

为了方便运动控制器设计，通过坐标变换将全局坐标系{X_L,O,Y_L}下的位置和方向误差方程式转换为局部坐标系{X_M,O_C,Y_M}。

通过计算上式中位置和方向误差方程的导数，得到：

对于上式跟踪误差系统，运动控制器设计为：

其中：v_K(t)和

分别是与给定参考轨迹保持一致的理想线速度和角速度，k₀，k₁和k₂是正可调参数。

步骤3，所述四旋翼动力学系统模型；

如图3所示，四旋翼无人机的动力学模型可以表示为：

其中，s(·)，c(·)分别为sin(·)，cos(·)的缩写；(x(t),y(t),z(t))，

分别表示四旋翼无人机的位置和三种姿态角(横滚、俯仰、偏航)；k_t表示线性阻力；g为重力加速度；m为重力加速度；I_x,I_y,I_z分别为沿x,y,z轴的转动惯量；u_i(t)(i＝1,2,3,4)为控制输入。u₁(t)表示z方向上的总升力，u₂(t)、u₃(t)、u₄(t)表示无人机在不同轴向的三个转动力矩。为了简化计算，令

步骤4，所述积分滑模控制器模型，根据协同系统的位置偏差设计：

(h_x(t),h_y(t))表示位置期望值，e_xq(t)和e_yq(t)是期望位置与实际位置的偏差，存在：

本策略以X方向为例，所设计的积分滑模面为：

式中，γ₁是正可调参数，对滑模面求导可得

其中：

外环积分滑模控制器设计为

式中，γ₂和γ₃都是正的可调参数。

实施例

本发明基于自主搭建的空地异构多机器人协同系统作为实验平台，该平台主要由一个基于STM32的轮式差速机器人、一个基于STM32的飞控和框架为X250的四旋翼无人机、WFLY系列遥控器、UWB超宽带定位、PC地面站组成。

实验区域设为8m*8m，飞行高度设为1.30m。选择了单个轮式移动机器人和单个四旋翼飞行器的方案来验证所提控制器的有效性。首先放置3个UWB基站，组成8m*8m的XOY二维地面坐标系，另外2个标签分别放置于轮式移动机器人和四旋翼飞行器，轮式移动机器人在开启电源后开始沿半径为1.5m的圆形轨迹运动。同时通过无线通信系统将轮式移动机器人的位置实时发送给四旋翼飞行器。然后通过积分滑模控制器跟踪轮式移动机器人的轨迹，得到如图4所示的轨迹跟踪图。轮式移动机器人和四旋翼飞行器的所有信号都被传输并保存到地面站进行显示和处理。

在图4中，轮式移动机器人在起始点为(4m,2m)时，沿半径为1.5m的圆形轨迹运动。通过上位机观察到轮式移动机器人的实际轨迹基本与参考轨迹吻合，说明UWB正常工作且四旋翼飞行器能够接收到轮式移动机器人的位置信息。此时，四旋翼飞行器在收到来自遥控器的启动信号后开始跟踪轮式移动机器人的轨迹。跟随者保持1.3米的相对高度距离。如图4所示，四旋翼飞行器的目标轨迹能够跟踪轮式移动机器人的实时轨迹，异构机器人的X和Y坐标基本吻合。

对于轮式移动机器人系统，位置跟踪误差结果如图5所示。由图得出轮式移动机器人能够基本跟踪目标轨迹，误差在一定范围内上下波动，跟踪误差小于0.2m；角度误差也在合理的范围内波动。对于四旋翼飞行器系统，位置跟踪误差结果如图6所示。由图6得出四旋翼飞行器能够精确跟踪目标位置，跟踪误差小于0.2m。

Claims (5)

1.一种空地异构多机器人轨迹跟踪平台，所述多机器人轨迹跟踪平台包括轮式移动机器人系统和四旋翼飞行系统，其特征在于：所述轮式移动机器人系统和四旋翼飞行系统均通过位置-速度双闭环控制框架下的外环位置控制模块和内环速度控制模块实现系统的轨迹跟踪；其中：所述轮式移动机器人系统与所述四旋翼飞行系统通过如下步骤实现空地异构多机器人的协同轨迹跟踪控制：

所述轮式移动机器人系统的外环位置控制模块通过两轮差速特性建立如下运动学模型；

其中，m＝(x_m,y_m,ε_m)^T表示轮式移动机器人的姿态位置和方向位置，

表示轮式移动机器人的线速度和角速度信息；

所述轮式移动机器人系统的内环速度控制模块使用PI控制方法对运动学模型计算生成轮式移动机器人系统的运动学控制器，即G_K(t)；

所述四旋翼飞行系统的外环位置控制模块通过飞行器位置控制的响应速度建立四旋翼飞行系统的积分滑模控制器；

所述四旋翼飞行系统的内环速度控制使用PID控制算法生成四旋翼动力学模型。

2.根据权利要求1所述的一种空地异构多机器人轨迹跟踪平台，其特征在于：所述轮式移动机器人系统的外环位置控制模块通过两轮差速特性建立运动学模型过程：

根据论述移动机器人两个轮子之间的距离是d，驱动轮的直径是2r计算左右车轮通过电机驱动接口输出的角转速

和

则线速度v_l(t)和v_r(t)为：

根据两驱动轮中心连线的中点为机器的基点O_p,瞬时线速度为v_m(t)，瞬时角速度

姿态角ε_m即为v_m(t)与X轴夹角，小车的瞬时线速度为v_m(t)可以表示为：

根据左右轮及基点所处位置在该圆周运动中的角速度相同

则机器的瞬时角速度

可以表示为：

联立两式，利用v_r(t)和v_l(t)求出机器转动半径：

在驱动轮与地面接触运动为纯滚动无滑动情况下，机器的运动学模型可以表示为：

进而运动学模型可以写成如下形式：

其中，m＝(x_m,y_m,ε_m)^T表示轮式移动机器人的姿态位置和方向位置，

表示轮式移动机器人的线速度和角速度信息。

3.根据权利要求1所述的一种空地异构多机器人轨迹跟踪平台，其特征在于：所述轮式移动机器人系统的运动学控制器生成过程：

通过轮式移动真实机器人按照误差方程建立虚拟机器人运动模型，

其中：v_rr(t)和

分别是虚拟线速度和角速度；

根据虚拟机器人运动模型建立全局坐标系{X_L,O,Y_L}下的位置和方向误差方程为：

通过坐标变换将全局坐标系{X_L,O,Y_L}下的位置和方向误差方程式转换为局部坐标系{X_M,O_C,Y_M}；

通过计算上式中位置和方向误差方程的导数，得到：

对于上式跟踪误差系统，运动控制器设计为：

其中：v_K(t)和

分别是与给定参考轨迹保持一致的理想线速度和角速度，k₀，k₁和k₂是正可调参数。

4.根据权利要求1所述的一种空地异构多机器人轨迹跟踪平台，其特征在于：所述四旋翼飞行系统的内环速度控制使用PID控制算法生成四旋翼动力学模型过程：

根据牛顿第二定律、欧拉方程对于四旋翼飞行器进行建模分析，推导出四旋翼动力学模型；

为了简化计算，令

其中，s(·)，c(·)分别为sin(·)，cos(·)的缩写；(x(t),y(t),z(t))，

分别表示四旋翼无人机的位置和三种姿态角(横滚、俯仰、偏航)；k_t表示线性阻力；g为重力加速度；m为重力加速度；I_x,I_y,I_z分别为沿x,y,z轴的转动惯量；u_i(t)(i＝1,2,3,4)为控制输入；u₁(t)表示z方向上的总升力，u₂(t)、u₃(t)、u₄(t)表示无人机在不同轴向的三个转动力矩。

5.根据权利要求1所述的一种空地异构多机器人轨迹跟踪平台，其特征在于：所述四旋翼飞行系统的外环位置控制模块通过飞行器位置控制的响应速度建立四旋翼飞行系统的积分滑模控制器过程：

(h_x(t),h_y(t))表示位置期望值，e_xq(t)和e_yq(t)是期望位置与实际位置的偏差，存在：

本策略以X方向为例，所设计的积分滑模面为：

式中，γ₁是正可调参数，对滑模面求导可得

其中：

则积分滑模控制器为

式中，γ₂和γ₃都是正的可调参数。

Images