[发明专利]一种面向复杂环境的多异构机器人视觉伺服队形变换方法

权利要求书

1.一种面向复杂环境的多异构机器人视觉伺服队形变换方法，该方法包括：

(1)、构建基本队形库，完成各基本编队队形基本信息配置；基本队形库构建规则如下：

L＝{Γ₁,Γ₂,…,Γ_i,…,Γ_N}

其中，L表示基本队形库；Γ_i表示第i个基本队形信息，具体内容如下：

Γ_i＝{D_ix,D_iy,d_safe,d_width,d_length,n_i}

其中，D_ix表示第i个基本队形的X轴期望距离矩阵，D_iy表示第i个基本队形的Y轴期望距离矩阵；d_safe表示机器人间的安全距离，d_wid表示第i个基本队形的宽度，d_len表示第i个基本队形的长度，n_i表示第i个基本队形包含的机器人数量；

(2)、指定初始编队队形，给定任务目标；

(2.1)、设计编队控制律F₁和跟踪控制律F₂，完成无人机的视觉伺服跟踪；通过搭载在无人机的视觉相机获取地面编队的中心位姿信息，传回上位机，基于伺服跟踪控制算法计算无人机期望速度信号；再传回四旋翼板载控制器，将期望速度信号转换为飞控接收信号，控制无人机运动到编队中心的上空；

(2.2)、通过基于辅助标签的视觉算法解算地面机器人的相对位姿，传回上位机并计算各地面移动机器人的期望运动速度，通过蓝牙传送到各地面移动机器人终端，实现期望编队形状；

(3)、异构多机器人系统感知周围环境，检测机器人集群路径内环境约束；

(4)、计算编队变换系数，如下式：

其中，γ表示编队的变换系数；D_form表示当前多机器人编队的宽度；D_c表示复杂地形可通过的最大安全宽度；根据变换系数大小选择对应操作：

根据变换系数判断是否不需变换即可通过当前狭窄地形，如果γ小于等于1，则表明地形安全宽度大于当前编队宽度，不需要任何队形变换操作即可通过；如果γ大于1，则表明地形安全宽度小于当前编队宽度，即继续保持原队形无法通过当前环境，转到步骤(5)；

(5)、编队变换；

(5.1)、首先判断当前编队的基本队形能否穿越地形，如果基本队形满足安全约束，转到步骤(5.2)；若不能满足安全约束，则只能进行编队切换；编队切换评价函数f如下：

f＝k_tf_t+k_ff_df+k_sf_s

其中，k_t、k_f、e表示的权重系数；f表示编队队形变换策略评价函数；f_t表示编队变换所需要的时间；f_df表示当前队形与期望队形的差异；f_s表示安全系数，用以衡量期望队形的安全程度；f_t的计算方式如下：

其中，t_a表示编队变换的开始时刻；t_b表示编队变换的结束时刻；实际中可通过测量的方式获取不同队形间切换所需时间；f_df的计算方式如下：

其中，D_dx表示期望编队队形的X轴信息；D_dy表示期望编队队形的Y轴信息；D_cx表示当前编队队形的X轴信息；D_cy表示当前编队队形的Y轴信息；f_s的计算方式如下：

其中，α表示编队宽度的权重系数；β表示编队长度的权重系数；f_x表示编队宽度；f_y表示编队长度；在所有其他编队中选取评价函数最小的作为被切换编队队形，转到步骤(5.3)；

(5.2)、若当前编队的最小队形可安全通过当前区域，那么对编队收缩操作，根据经验选取最佳收缩变换系数γ^*，当步骤(4)中的编队变换系数满足γ≥γ^*时，直接进行切换操作，计算步骤如(5.1)所示，得出最佳切换编队队形，并转到步骤(5.3)；

当γ满足1＜γ＜γ^*时，考虑编队队形的收缩操作；选取1/γ为队形收缩系数，确定收缩后的编队形状；为弥补人为设置最佳收缩变换系数带来的不足，也即确保真正实现更优的编队变换操作，计算收缩评价函数，并在可收缩情况下计算选取编队切换操作的评价函数；通过比较收缩和切换评价函数的值，最终确定出优化过的期望编队队形，转到步骤(5.3)；

(5.3)、将期望编队队形设置为步骤(5.1)或(5.2)中通过编队变换策略选取的优化队形，然后根据给定的控制方法，确保多机器人系统完成编队变换操作；

(6)、保持切换后编队，通过当前狭窄区域；然后转到步骤(2)，检测机器人周围环境约束，再重复步骤(3)、(4)、(5)。