基于反步法的多移载工装协同作业编队控制策略

doi:10.16183/j.cnki.jsjtu.2021.325

基于反步法的多移载工装协同作业编队控制策略

刘禹铭¹, 赵勇^,¹, 董正建², 王平², 姬煜琦²

1.上海交通大学上海市复杂薄板结构数字化制造重点实验室,上海 200240

2.上海飞机制造有限公司航空制造技术研究所,上海 201324

Formation Control Strategy of Multiple Mobile Robots Cooperative Operation Based on Backstepping Method

LIU Yuming¹, ZHAO Yong^,¹, DONG Zhengjian², WANG Ping², JI Yuqi²

1. Shanghai Key Laboratory of Digital Manufacture for Thin-Walled Structures, Shanghai Jiao Tong University, Shanghai 200240, China

2. Shanghai Aircraft Manufacturing Co., Ltd., Shanghai 201342, China

通讯作者: 赵勇,副教授,博士生导师,电话(Tel.):021-34206288;E-mail:zhaoyong@sjtu.edu.cn.

责任编辑: 陈晓燕

收稿日期: 2021-08-20 修回日期: 2021-10-8

基金资助:

国家科技重大专项04专项子课题(2018ZX04006001-009)

Received: 2021-08-20 Revised: 2021-10-8

作者简介 About authors

刘禹铭(1997-),硕士生,从事传感器数据融合、多智能体协同作业研究.

摘要

研究了一种基于反步法的多移载工装协同作业控制策略,提出了基于改进人工势场法和纯轨迹跟踪法的期望运动状态规划方案,并根据自适应蒙特卡洛定位方法确定了实际运动状态的估计值,设计了反步法与虚拟领航跟随法结合的队列控制器.构建了基于机器人操作系统(ROS)环境的仿真模型,并开展仿真验证.结果表明:提出的队形误差计算方法能够提高误差估计精度,该编队控制策略能够使得队形误差在6.2 s内收敛,所设计的队形控制器能够满足多移载工装的作业要求.

关键词： 编队控制; 反步法; 运动状态; 状态估计

Abstract

A multi-shift tooling coordinated operation control strategy based on the backstepping method is investigated, a desired motion state planning scheme based on the improved artificial potential field method and the pure trajectory tracking method is proposed, and the actual situation is determined according to the adaptive Monte Carlo positioning method. For the estimated value of the motion state, a queue controller in combination the anti-stepping method and the virtual pilot following method is designed. A simulation model based on the robot operation system (ROS) environment is constructed, and the simulation is verified. The results show that the proposed formation error calculation method can improve the accuracy of error estimation. The formation control strategy can make the formation error converge within 6.2 s, and the designed formation controller can meet the operation requirements of multi-shift tooling.

Keywords： formation control; backstepping method; movement state; state estimation

PDF (9418KB) 元数据多维度评价相关文章导出 EndNote| Ris| Bibtex 收藏本文

本文引用格式

刘禹铭, 赵勇, 董正建, 王平, 姬煜琦. 基于反步法的多移载工装协同作业编队控制策略[J]. 上海交通大学学报, 2023, 57(1): 103-115 doi:10.16183/j.cnki.jsjtu.2021.325

LIU Yuming, ZHAO Yong, DONG Zhengjian, WANG Ping, JI Yuqi. Formation Control Strategy of Multiple Mobile Robots Cooperative Operation Based on Backstepping Method[J]. Journal of Shanghai Jiaotong University, 2023, 57(1): 103-115 doi:10.16183/j.cnki.jsjtu.2021.325

飞机大部件装配作为飞机部装的核心环节,安全高效的部件运输以及自动化高精度的工装对接是整体工作完成的重要保障^[1].但是传统的固定平台装配导致工厂场地利用率低,整体装配过程灵活性低,并且装配设备的成本较高,若使用移载工装进行协同装配,则能让工厂的场地利用率和装配效率提高^[2].目前大多采用物流作业小车作为作业的移载工装,但是其运动精度较低.当各移载工装由于误差原因导致实际运动位姿偏离期望运动位姿时,多移载工装易使搬运件产生较大内力进而影响装配结果和产品最终的服役性能.

合理的编队控制器能够有效降低队列偏差所导致的作业过程中的内力.由于作业环境的扰动和移载工装的运动误差影响,由多移载工装组成的多智能体系统鲁棒性和稳定性较低,并且具有非线性的特点,而反步法适用于非线性不确定系统的控制,其采取分布式的控制器策略,对多智能体中的每个子系统设计运动控制器^[3].Tian等^[4]提出了干扰情况下二阶多智能系统的反步法控制方法.Liu等^[5]在经典的反步法基础之上,提出有限时间触发方法,减小了通信负担.Xue等^[6]则利用滑模控制将状态观测与反步法进行结合,设计了一种自适应反步滑模控制方法.

期望运动状态和实际运动状态是反步法控制策略下的核心变量.对于多智能体的期望运动状态设计依赖于编队方法的设计,目前常用的编队方法主要包括领航跟随法^[7]、人工势场法^[8]、基于行为控制法^[9]、虚拟结构法^[10]和虚拟领航者法^[11].而其中能够保持队列刚性结构的方法主要为领航跟随法和虚拟结构法.领航跟随法将多机器人作业过程中的编队问题转化为领航机器人和跟随机器人的期望角度与距离之间的关系问题.虚拟结构法的主要理念是将多机器人的队形假设为虚拟的刚性结构进行控制规律设计.该编队方法下整体队形轻便,形成稳定队列的收敛时间快,但系统稳定性难以保证,鲁棒性和灵活性较差^[12].基于多移载工装进行协同装配时的刚性队列要求和不同工厂环境下模块化装配的需要,将虚拟结构法和虚拟领航者法相结合,以虚拟结构的参考点作为虚拟领航者进行作业运动规划.常用的运动规划算法有人工势场法^[13]、A^*算法^[14]、遗传算法^[15]、粒子群优化算法^[16]等.人工势场法的运动规划兼容编队控制和避障规划的要求,且具有计算量小、规划路径平滑易于设计轨迹追踪器的优点,被广泛运用于工程实践中.编队控制的另一个重要环节是实际运动状态的估计,通过测量设备的定位与滤波算法的结合,实时校正估计实际位置.而针对移动机器人的定位领域,基于粒子滤波算法进行定位最为广泛.粒子滤波以其具有多模态、统计特征随机分布的特性,适用于位姿估计和实时目标追踪.但是粒子滤波过程中重采样会导致粒子退化的问题,针对这一问题,赵国旗等^[17]将模糊理论引入粒子滤波来防止重采样中出现的多模态失效的问题.Li等^[18]针对粒子滤波结合引入基于库尔贝克-莱布勒散度(Kullback-Leibler Divergence, KLD)进行采样的方法,有效解决了粒子退化问题.

本文以反步法作为编队控制策略的基础,将虚拟结构法和虚拟领航者法相结合,通过改进的人工势场法和纯轨迹跟踪法设计虚拟领航者的期望运动位姿,解决了路径规划陷入局部最优的问题,进一步根据队形几何关系推导得到实际跟随者的期望运动位姿.结合雷达测量设备和自适应蒙特卡洛定位(Adaptive Monte Carlo Localization, AMCL)算法估计实际运动位姿,降低了每个移载工装的定位误差值,基于跟随误差设计了队形控制器,完成了队列的协同作业控制.

1 基于反步法的编队控制器设计

1.1 结合虚拟结构法和虚拟领航者法的队形设计

在协同搬运机翼的过程中,移载工装需要保持编队队形的刚性结构,以减小队形变形所产生的误差进而降低内力.编队结构的队列模型如图1所示.

图1

新窗口打开| 下载原图ZIP| 生成PPT

图1 虚拟领航者引领下的刚性结构队形

Fig.1 Rigid structure formation guided by virtual navigator

考虑以期望队形的几何中心作为编队参考点,在该位置添加一台同类型的虚拟移载工装,即虚拟领航者.实际的移载工装作为跟随者,进行整体的编队控制,领航跟随的关系如图2所示.其中,C为小车质心,w为角速度.

图2

新窗口打开| 下载原图ZIP| 生成PPT

图2 领航者与跟随者编队位姿误差分析

Fig.2 Pose error analysis of leader and follower formation

以虚拟领航者的起始后轮轴中心作为全局坐标系的坐标原点O,并根据坐标原点选取全局坐标系下的X_g轴与Y_g轴.设计其中领航者位姿信息为机器人 i,表明第i台领航者,其包含距离全局坐标原点的距离信息ρ_i和与全局X_g正方向的夹角θ_i.跟随者实际位姿为机器人 j,表明第j台跟随者,其包含距离全局坐标原点的距离信息ρ_j,与领航者的距离差L_ij和期望转角角度差φ_ij.期望位姿为机器人j(d) (d为期望的相对位姿),其包含距离全局坐标原点的距离信息 ${ρ^{d}}_{j}$ ,与领航者的距离差 ${L^{d}}_{i j}$ 和期望转角角度差 ${φ^{d}}_{i j}$ .通过几何关系计算跟随者期望位姿,并与当前实际位姿进行比对,得到队形误差量.

假设领航者机器人i在全局坐标系中对应的坐标为(x_i, y_i),则由图示的几何关系可以得到全局坐标的变换关系:

$\begin{array}{l} {x^{d}}_{j} = x_{i} + {L^{d}}_{i j} c o s ({φ^{d}}_{i j} + θ_{i}) \\ {y^{d}}_{j} = y_{i} + {L^{d}}_{i j} s i n ({φ^{d}}_{i j} + θ_{i}) \\ {θ^{d}}_{j} = θ_{i} \end{array}\}$

(1)

$\begin{array}{l} {x^{t}}_{j} = x_{i} + L_{i j} c o s (φ_{i j} + θ_{i}) \\ {y^{t}}_{j} = y_{i} + L_{i j} s i n (φ_{i j} + θ_{i}) \\ {θ^{t}}_{j} = θ_{j} \end{array}\}$

(2)

式中: ${x^{d}}_{j}$ 、 ${y^{d}}_{j}$ 以及 ${θ^{d}}_{j}$ 分别为每时每刻变化下的跟随者机器人 j的期望全局坐标; ${x^{t}}_{j}$ 、 ${y^{t}}_{j}$ 以及 ${θ^{t}}_{j}$ 分别为每时每刻变化下的跟随者机器人 j的实际全局坐标.

由式(1)与(2)可以得到移载工装期望位姿以及实际位姿.将两式相减,得到误差的相应关系式并将其对时间求导,进一步获得误差关于时间的微分方程,从而根据队列的运动误差,设计队形控制器.

$[\begin{array}{l} x_{e} \\ y_{e} \\ θ_{e} \end{array}]$

$[\begin{array}{l} {L^{d}}_{i j} c o s ({φ^{d}}_{i j} + θ_{i} - θ_{j}) - L_{i j} c o s (φ_{i j} + θ_{i} - θ_{j}) \\ {L^{d}}_{i j} s i n ({φ^{d}}_{i j} + θ_{i} - θ_{j}) - L_{i j} s i n (φ_{i j} + θ_{i} - θ_{j}) \\ θ_{i} - θ_{j} \end{array}]$

(3)

$[\begin{array}{l} {\overset{\cdot}{x}}_{e} \\ {\overset{\cdot}{y}}_{e} \\ {\overset{\cdot}{θ}}_{e} \end{array}]$

$[\begin{array}{l} - v_{j} + v_{i} c o s θ_{e} + w_{j} y_{e} - w_{i} {L^{d}}_{i j} s i n ({φ^{d}}_{i j} + θ_{e}) \\ - w_{j} x_{e} + v_{i} s i n θ_{e} + w_{i} {L^{d}}_{i j} c o s ({φ^{d}}_{i j} + θ_{e}) - d' w_{j} \\ w_{i} - w_{j} \end{array}]$

(4)

式中:x_e、y_e及θ_e为跟随者机器人 j 在x方向、y方向以及角度θ上实际位姿与期望位姿之间的误差量; v_i、w_i、v_j及w_j分别为领航者机器人 i的行驶速度与角速度和跟随者机器人 j 的行驶速度与角速度; ${\overset{\cdot}{x}}_{e}$ 、 ${\overset{\cdot}{y}}_{e}$ 及 ${\overset{\cdot}{θ}}_{e}$ 为x_e、y_e及θ_e对时间求导的误差变化率;d'为单移载工装质心到后轮轮轴连线中心的距离.

1.2 基于反步法的跟随者队形控制器设计

针对编队中各个跟随者产生的运动误差,设计基于反步法的分布式队形控制器.反步法应用于非线性系统的一般形式为

$\overset{\cdot}{x}$ =f_x(x)+g_x(x)u_x(x)

(5)

式中:f_x(x)和g_x(x)为与输入变量x 相关的函数;u_x(x)为输入控制量.根据已得到的误差动态微分方程,将x_e、y_e两项误差值看作队形控制器的子系统,并递归寻找对应的虚拟控制,对于两项输入控制v_j和w_j,当选取v_j作为虚拟控制输入时:

v_j=k₁x_e+v_icos θ _e-w_i

${L^{d}}_{i j}$ sin(

${φ^{d}}_{i j}$ +θ_e)

(6)

式中:k₁为大于0的正比例系数.将式(5)代入式(6)中,则 ${\overset{\cdot}{x}}_{e}$ =-k₁x_e+w_jy_e,选定Lyapunov函数V₁= $\frac{1}{2} x_{e}^{2}$ ,可知满足李雅普诺夫第一性,又由于

${\overset{\cdot}{V}}_{1}$ =x_e

${\overset{\cdot}{x}}_{e}$ =-k₁

$x_{e}^{2}$ +w_jx_ey_e

(7)

当y_e=0时,V₁≤0,则x_e在有限时间收敛至0.对于下一个子系统微分方程的虚拟控制量w_j,设计取值为

w_j=

$\frac{1}{d'}$ (v_isin θ _e+w_iL_ijcos(

${φ^{d}}_{i j}$ +θ_e)+k₂y_e)

(8)

式中:k₂>0.选取Lyapunov函数V₂=V₁+ $\frac{1}{2} y_{e}^{2}$ ,计算得知其满足李雅普诺夫第一性,又由于

${\overset{\cdot}{V}}_{2}$ =x_e

${\overset{\cdot}{x}}_{e}$ +y_e

${\overset{\cdot}{y}}_{e}$ =-k₁

$x_{e}^{2}$ +y_e(v_isin θ _e+ w_i

${L^{d}}_{i j}$ cos(

${φ^{d}}_{i j}$ +θ_e)-dw_j)

(9)

将(8)代入式(9)中,得到V₂=-k₁ ${\overset{\cdot}{x}}_{e}^{2}$ -k₂ ${\overset{\cdot}{y}}_{e}^{2}$ .可以看出,x_e、y_e在递归控制下会随着时间收敛为0,则每辆跟随者的位置坐标和输入变量在反步法的控制下可以保持刚性编队要求,至此整体的运动控制器设计完毕.

2 移载工装的运动轨迹规划与运动状态估计

2.1 基于改进人工势场法的避障路线设计

从整体队列控制器的设计过程可知,期望的运动状态和实际的运动状态的求取是控制的两大关键.期望的运动状态需要满足避障、运动平滑、对不同作业环境规划适应性强的要求,而求取实际的运动状态需要满足实时性强、精度高的要求,通过两者决定的运动误差来设计可用的队列控制器.整体的设计流程图如图3所示.

图3

新窗口打开| 下载原图ZIP| 生成PPT

图3 整体编队控制器设计流程图

Fig.3 Designed flow chart of overall formation controller

根据编队控制器的设计流程,首先进行虚拟领航者的期望运动路径设计.基于实际工作环境扫描得到多移载工装的作业空间,在作业空间中引入人工势场,障碍物周围分布斥力场,目标点周围分布引力场,使得被控对象在合力作用下自主移动.设定移载工装所在位置为q(x₀, y₀),目标点位置为g(x_end, y_end),定义目标距离函数为ρ(q, g),q为起始点,g为目标点.在传统的人工势场中常用的引力与斥力场函数为

U_att=

$\frac{1}{2}$ ηρ(q, g)²

(10)

U_rep=

$\{\begin{array}{l} \frac{1}{2} ξ {(\frac{1}{ρ (q) - q_{0}})}^{2}, & ρ (q) < q_{0} \\ 0, & 其他 \end{array}$

(11)

式中:η、ξ为选取的引力系数与斥力系数;ρ(q)为移载工装距离障碍物最近点的距离;q₀为基于移载工装自身尺寸以及队形等要素所决定的安全距离.而斥力场仅在针对移载工装定义的安全距离内生效,以达到更短的避障路线规划.由于虚拟领航者位于队列几何中心,所以选取几何外接圆半径为安全距离的参考基准,能简洁地处理编队避障的问题,路径规划的场地适应性和实时性都能满足要求.

传统人工势场法有可能出现合力为0引起的局部最优问题,对这种陷入局部极小值的情况,本文使用模拟退火法进行处理^[19].模拟退火算法与人工势场法结合的重点是当陷入局部最优的情况时,在当前点x附近邻域内选取随机点x₁,计算两点在人工势场中的势场U(x₁)和U(x),当U(x₁)<U(x)时,选择随机点x₁作为下一个路径点.当随机点x₁不是势场更低点时,以一定概率选取其作为下一路径点,其初始化转移概率P= $e^{- \frac{Δ}{T}}$ ,其中T为随着随机点增加而衰减的温度值函数,Δ为随机点与当前的势能差,即Δ=U(x₁)-U(x).通过不断地按照衰减的概率选取当前随机点邻域上新的随机位置点,直到逃离局部最优值,生成到达最终目标点的路径.具体的算法流程图如图4所示,其中α为对应转移概率的选取判断阈值.

图4

新窗口打开| 下载原图ZIP| 生成PPT

图4 结合模拟退火法的人工势场法算法流程图

Fig.4 Algorithm steps of APF method in combination with simulated annealing

使用上述介绍的改进人工势场法,当虚拟领航者陷入局部最优导致运动震荡时,通过搜索下一路径点进行跳出,如图5所示.

图5

新窗口打开| 下载原图ZIP| 生成PPT

图5 模拟退火法逃出局部最优示意图

Fig.5 Diagram of escaping from local optimum by simulated annealing

图5(c)与5(f)中的箭头符号象征人工势场中的势场矢量,而图5(d)中的U为合势场的大小,没有对应的单位.可以看出,采用模拟退火法使得被控移载工装在全局坐标系中跳出了局部最优点,重新规划路径,最终到达了目标点.

2.2 移载工装预期轨迹的追踪控制器设计

使用改进人工势场法规划虚拟领航者的避障路径之后,需要设计对应的轨迹追踪控制器保障移载工装的循迹作业.设计轨迹跟踪器需要先构建移载工装的运动学模型.实际作业环境下的移载工装的运动可忽略竖直方向上的运动,因此将被控对象的运动空间设计成二维平面.基于实际工况,在控制移载工装的过程中,根据已有移载工装的实际参数和控制变量,输入对应的移载工装角速度和线速度,并将移载工装简化为图6所示的差速轮驱动模型,图中:X_L和Y_L为小车以质心C所建立的坐标系X轴与Y轴;r为小车差速轮半径;2R_a 为小车的宽度;p为小车车轮车轴的中心点,D为小车质心C离轮轴中心的距离.

图6

新窗口打开| 下载原图ZIP| 生成PPT

图6 移载工装简化的运动学模型示意图

Fig.6 Schematic of simplified kinematic model of transfer tooling

根据图6建立的运动学模型,假定移载工装为差速轮,其对应整体工装的控制量为[v w]^T,即运动速度v和决定方向的角速度w.而相对应的状态变量为[x y θ]^T,即全局坐标系下的x和y以及行驶方向与X轴正半轴的夹角.在图6中,移载工装的质心在平面坐标系下的速度为v_c,其在X, Y上的分量为v_x_c与v_y_c,则通过速度分解:

v_x_c cos θ+v_y_csin θ=v

(12)

通过简化模型几何关系可以得到差速轮模型的移载工装运动方程,写作整体系统方程形式:

$[\begin{array}{l} {\overset{\cdot}{x}}_{c} \\ {\overset{\cdot}{y}}_{c} \\ \overset{\cdot}{θ} \end{array}]$

$[\begin{array}{l} c o s θ & - d s i n θ \\ s i n θ & d c o s θ \\ 0 & 1 \end{array}] [\begin{array}{l} v \\ w \end{array}]$

(13)

式中: ${\overset{\cdot}{x}}_{c}$ 、 ${\overset{\cdot}{y}}_{c}$ 及 $\overset{\cdot}{θ}$ 为移载工装质心x、y方向上的位移速度以及角速度.基于已经建立的移载工装运动模型,对于理想的输入控制量进行设计,本文采用基于几何关系的纯轨迹跟踪控制器^[20],从简化的差速轮模型出发, 在规划路径上选取最接近预瞄距离的目标点作为下一时刻追踪的预瞄点P_y,其在自身质心坐标系中的坐标为(x, y),移载工装循迹过程中的几何关系如图7所示,图中:l_d 为移载工装质心到预瞄点P_y的距离;R为当前期望目标期望轨迹对应的曲率半径;d_x预瞄点与相应曲率中心在自身质心坐标系X方向上的距离根据图7的几何关系可得距离方程:

$\begin{array}{l} R^{2} = y^{2} + d_{x}^{2} \\ R = \frac{v}{w} \\ d_{x} = R - x \\ l_{d}^{2} = y^{2} + (R - d_{x})^{2} \end{array}\}$

(14)

图7

新窗口打开| 下载原图ZIP| 生成PPT

图7 纯轨迹跟踪法的几何关系示意图

Fig.7 Geometric relationship schematic of pure pursuit tracking method

并得到相关曲率的表达式和角速度控制方程:

$\begin{array}{l} γ = \frac{2 x}{l_{d}^{2}} \\ w = v γ \end{array}\}$

(15)

通过选取较为平缓的速度和合适的预瞄距离来选取相关的输入控制量,从而确保对运动轨迹较好的追踪效果,并得到虚拟领航者的规划运动状态,根据提出的队形结构的几何关系变化,进一步计算跟随者的期望运动状态.

2.3 基于自适应蒙特卡洛算法的位姿估计

移载工装的实际位姿是队形偏差计算的另一个关键元素.若直接采用测量设备进行定位获取实际位姿,传感器的误差会直接影响定位的结果,进而导致队形偏差计算失准.而结合滤波算法修正观测变量的定位方法可以提高实际运动状态的观测精度,解决定位精度不够的问题.本文构建移载工装的实时定位框架,采用自适应蒙特卡洛定位算法进行实际位姿估计.

蒙特卡洛定位(Monte Carlo Localization, MCL)算法是一种适用于室内较小地图情况下的全局定位算法.该算法基于概率进行定位计算,使用粒子表征移载工装所处位置的概率大小,初始时刻粒子平均分布在全局地图上,在移载工装运动作业时,粒子与移载工装同步运动,并根据移载工装的位姿筛选出与移载工装运动状态一致的粒子.在一段时间后粒子完成收敛时,当前粒子即表征移载工装的真实位姿估计,若粒子收敛后表征的位姿是错误的,该算法无法做到纠偏和恢复.当粒子数目较少时,丢失真实位姿附近的粒子的情况常有发生.而AMCL算法的引入有效地规避了这一问题.自适应蒙特卡洛定位算法是蒙特卡洛定位算法的一种改进.AMCL通过KLD重采样的方法注入了新的随机粒子,减小粒子运算量的同时,避免移载工装“绑架问题”的出现.

首先针对移载工装的运动状态和观测状态进行建模,其状态方程为

$\begin{array}{l} x_{k} = F_{k} x_{k - 1} + B_{k} U_{k} + Q_{k} \\ z_{k} = H_{k} x_{k} + R_{k} \end{array}\}$

(16)

式中:x_k为k时刻运动状态变量,即对应的[p_x p_y v φ w]^T;F_k为状态转移矩阵;U_k为输入量;B_k为相应的输入矩阵;Q_k为过程噪声;z_k为目标测量值;H_k为测量矩阵;R_k为测量噪声.AMCL算法引入带有权重的粒子集合S={ $x_{t}^{i}$ , $w_{t}^{i}$ },其中 $x_{t}^{i}$ 是从提议分布的q(x_t|x_t_-1, u_t_-1, z_t) 中选取的索引为i 的粒子; $w_{t}^{i}$ 代表粒子的重要性权重,且满足归一化条件 $\overset{n}{\sum_{i = 1}} w_{t}^{i}$ =1.而普通蒙特卡洛定位算法对粒子权重的计算方法如下:

$w_{t}^{i}$

$w_{t - 1}^{i} \frac{p (z_{t} | x^{i}) p (x^{i} | x_{t - 1}^{i}, u_{t - 1})}{q (x_{t}^{i} | x_{t - 1}^{i}, z_{t}, u_{t - 1})}$

(17)

根据p(z_t|xⁱ)与p(xⁱ| $x_{t - 1}^{i}$ , u_t_-1)分别为对应观测与运动方程计算得出的条件概率以及建立的移载工装动力学方程,可以得到表征移载工装运动的粒子提议分布:

$x_{t}^{i}$ |

$x_{t - 1}^{i}$ , z_t, u_t_-1)

$w_{t}^{i}$ =p(

$x_{t}^{i}$ |

$x_{t - 1}^{i}$ , u_t_-1)

(18)

而移载工装的运动状态的后验可靠性估计为

B_el(x_t)≈

$\overset{n}{\sum_{i = 1}} w_{t}^{i}$ δ(x_t-

$x_{t}^{i}$ )

(19)

式中:B_el(x_t)为该运动状态下的位姿后验可靠性;δ(x_t- $x_{t}^{i}$ )为相关差值的狄拉克函数值.为了解决粒子滤波重采样中粒子退化的问题,AMCL算法使用KLD重采样方法对粒子集进行处理,其中重采样的粒子集数目阈值的表征公式为

M_χ=

$\frac{k' - 1}{2 ε} {[1 - \frac{2}{9 (k' - 1)} + \sqrt{\frac{2}{9 (k' - 1)}} β]}^{3}$

(20)

式中:M_χ为粒子集的数目阈值;k'为非空粒子状态空间的个数;ε与β分别为真实分布与预估分布的最大误差与正态分布的分位数.根据KLD采样的流程可以看出,当粒子对整体地图覆盖程度高时,采样的粒子集数目降低,可以减小实时定位过程中的运算量;而粒子对地图覆盖程度较低时,粒子数目提高,在误差协方差较低的情况下通过随机引入粒子的方法防止重采样过程中粒子退化的问题.如图8所示,AMCL算法运用于移载工装全局定位并最终收敛,收敛粒子表征移载工装的估计位姿.

图8

新窗口打开| 下载原图ZIP| 生成PPT

图8 AMCL算法的定位收敛过程

Fig.8 Location convergence process of AMCL algorithm

本文通过融合移载工装的里程计与激光雷达的传感器信息,将移载工装运动模型和观测模型代入AMCL算法,提出合理的粒子提议分布来逼近移载工装的位姿,其具体估计流程架构如图9所示.

图9

新窗口打开| 下载原图ZIP| 生成PPT

图9 AMCL算法运用于移载工装协同作业的定位流程

Fig.9 Positioning process of AMCL algorithm applied to collaborative operation of mobile robot

3 仿真验证

3.1 仿真环境搭建以及参数设计

通过在ROS-Gazebo中建立与现场环境相关的实际地图和差速轮移载工装模型,并进一步将实际地图二值化,得到如图10所示的场景图,将规划的路径点与控制信号发送给移载工装进行编队位移控制.并在ROS中构建在该场景下多移载工装的仿真模型.

图10

新窗口打开| 下载原图ZIP| 生成PPT

图10 仿真作业空间建模

Fig.10 Construction of workspace simulation environment

根据搭建好的二值地图,通过改进人工势场法来规划虚拟领航者和跟随者的期望路径.其中载工装的具体参数以及队列期望距离如表1所示,导航方式为激光定位与第一构建,并得到相应的虚拟结构刚性队列的几何中心路径图.

表1 移载工装技术参数

Tab.1 Technical parameter of transfer tooling

物理量	取值
作业空间/m	40,60
长、宽、高/mm	900,700,300
离地间隙/mm	20
雷达距离精度/cm	2
雷达距离误差协方差	0.001
雷达角度精度	<0.5^°
雷达角度误差协方差	0.0001
末端精度/cm	∓5
旋转半径/cm	0
队列距离/cm	25
速度误差协方差	0.01

新窗口打开| 下载CSV

在移载工装的运动空间中建立引力、斥力场,选取的引力系数η=1/70,斥力系数ξ=8/7,虚拟领航者与作业空间中障碍物的安全距离根据队列距离与自身尺寸设置为35 cm.作业空间内的总体引力场、斥力场以及总势场如图11所示,图中:U_rep为作业空间内的斥力;U_att为作业空间内的引力.

图11

新窗口打开| 下载原图ZIP| 生成PPT

图11 移载工装作业空间内势场变化图

Fig.11 Variation diagram of potential field in workspace of multiple mobile robots

通过给定参数的移载工装,在Simulink中建立符合标准的基于差速轮驱动的移载工装运动模型,并对期望轨迹通过纯跟踪法进行追踪,得到虚拟领航者相应的期望速度与角速度如图12所示,其中设定预瞄距离下限值为5 cm.

图12

新窗口打开| 下载原图ZIP| 生成PPT

图12 差速轮移载工装运动模型

Fig.12 Motion model of differential wheel transfer tooling

对于整体的测量系统,采用车载激光雷达的出厂误差作为测量误差,结合实时控制的运动方程产生的误差进行滤波从而对实际位姿与运动状态进行估计与修正.其中距离测量误差的初始提议分布为N(μ₁,σ₁) (μ₁为均值,σ₁为方差),角度测量误差的初始提议分布为 N(μ₂,σ₂) (μ₂为均值,σ₂为方差),均假设为高斯分布,协方差与均值由表1参考获得.得到实际跟随者移载工装的状态变量最优值后,再通过反步法设定的控制策略,进行队列的纠偏仿真.

3.2 仿真结果分析

在进行避障路径规划中使用传统人工势场法,即不引入模拟退火法逃离局部最优点时,在目标点靠近障碍物的过程里,移载工装会陷入势场中的局部最优点而小距离震荡,最终无法到达目标点.移载工装在势场中停滞情况与整体路径如图13所示.

图13

新窗口打开| 下载原图ZIP| 生成PPT

图13 传统人工势场法下失败的路径规划

Fig.13 Failed path planning in traditional artificial potential field method

结合模拟退火法对陷入局部最优的情况进行了改进.设定停滞震荡的距离判定范围为3 cm,并进行模拟退火法进行跳脱,其改进后路线如图14所示.

图14

新窗口打开| 下载原图ZIP| 生成PPT

图14 改进人工势场法成功规划目标路径

Fig.14 Target path planed by improved artificial potential field method

设定预定路径之后,使目标小车模型对路径进行了轨迹追踪,保持领航者速度10 cm/s匀速行驶进行编队控制,通过计算得到的输入控制量如图15所示,图中:T为控制时间.

图15

新窗口打开| 下载原图ZIP| 生成PPT

图15 虚拟领航者输入运动变量

Fig.15 Input movement variables of virtual pilot

确定系统输入控制量后,使用Simulink搭建的移载工装纯轨迹跟踪法模型进行轨迹追踪仿真,得到图16所示的追踪效果和对应误差图像,图中E为移载工装跟踪运动误差.仿真结果表明,输入控制的角速度变化较平缓,追踪偏差逐渐收敛于2.2 cm以内,满足追踪的需求.

图16

新窗口打开| 下载原图ZIP| 生成PPT

图16 纯轨迹跟踪下的期望轨迹与误差

Fig.16 Desired trajectory and error in pure trajectory tracking

在仿真环境中读取移载工装的实际位姿,以激光雷达作为测量设备.根据蒙特卡洛定位以及自适应蒙特卡洛定位方法估计移载工装的实际位姿,并与直接使用激光雷达测量的方法进行对比,结果如图17所示,图中:E_θ为角度追踪误差;E_dis为距离跟踪误差.对比可得,使用自适应蒙特卡洛定位的方法可以将角度误差降低到0.01° 以下,比直接使用测量设备的0.5° 测量误差有较大降低.距离方面也将误差收敛在0.8 cm以内,两者的误差协方差系数也相较于MCL定位算法和直接使用测量设备进行定位的误差协方差系数小.

图17

新窗口打开| 下载原图ZIP| 生成PPT

图17 自适应蒙特卡洛定位方法误差与其他方法误差对比

Fig.17 Comparison of error of adaptive Monte Carlo localization method and other methods

利用反步法设计的控制策略对整体队形进行保持.对于反步法中k₁和k₂参数的选取,由于4台实际跟随者关于虚拟领航者几何对称,基于误差的控制策略可选取相同参数,所以k₁和k₂参数只需要对单台跟随者进行设计,引入表征控制效果的指标:

$\sqrt{{(Δ x)}^{2} + {(Δ y)}^{2} + {(Δ θ)}^{2}}$ +

$\frac{T}{α}$

(21)

式中:Δx、Δy及Δθ为实际运动与期望运动的误差值;α为比例系数,取60,并将符合收敛的参数域中的指标进行归一化:

$\begin{array}{l} S_{i} = s_{i} / \overset{n}{\sum_{k = 1}} s_{k} \\ S = \overset{n}{\sum_{i = 1}} S_{i} \end{array}\}$

(22)

式中:S_i为第i台移载工装的评价指标归一化结果;s_i为第i台评价指标结果;S为多移载工装评价指标之和.通过仿真得到不同参数变化情况下,移载工装在跟随过程中的评价指标变化情况,如图18所示,并选取指标极大值位置的参数值为[1.47],即反步法中的控制参数[k₁, k₂], 红点为极大值.

图18

新窗口打开| 下载原图ZIP| 生成PPT

图18 反步法控制效果与参数选取关系

Fig.18 Relationship between backstepping control effect and parameter selection

参数选取完毕后,对多移载工装编队下的追踪效果进行仿真, 前后跟随者的期望追踪轨迹路线如图19所示,图19中引入了Z方向的全局坐标系Z_g来表征移载工装的竖直尺寸大小.图20所示为根据虚拟领航者与跟随者的位姿变换关系得到的跟随者理想运动轨迹以及控制过程中跟随者实际与期望位姿的误差曲线,其中E_f为表征误差的衡量指标,计算式为E_f= $\sqrt{{(Δ x)}^{2} + {(Δ y)}^{2} + {(Δ θ)}^{2}}$ .而表2表示运动过程中的误差收敛时间.

图19

新窗口打开| 下载原图ZIP| 生成PPT

图19 跟随者理想运动轨迹设计

Fig.19 Ideal movement trajectory design of follower

图20

新窗口打开| 下载原图ZIP| 生成PPT

图20 跟随者误差指标随时间变化曲线

Fig.20 Follower error metric versus time

表2 跟随机器人三方向追踪时间

Tab.2 Track time in three directions of robot follower

跟随机器人编号	追踪时间/s
跟随机器人编号	X方向	Y方向	角度
1	3.5021	5.5021	4.2345
2	4.2987	4.2987	6.1251
3	2.1342	2.1342	3.2134
4	3.2342	4.9234	3.4321

新窗口打开| 下载CSV

可以看出,在控制量有一定的扰动下,移载工装依然能够在6.2 s内收敛到期望的位姿,达到整体的编队要求.

4 结语

针对多移载工装协同作业的控制问题,提出了一种基于反步法的多移载工装协同作业编队控制策略.建立了移载工装的运动学模型,提出了基于改进人工势场法和纯轨迹追踪法的期望运动状态规划方法,以及基于自适应蒙特卡洛定位的实际运动状态估计方法,与直接使用测量设备相比,角度误差由 0.5° 降低至 0.01°,位置误差由2 cm降低至 0.8 cm,精度提高.构建了仿真模型进行验证,队形误差在6.2 s内达到收敛,满足多移载工装协同作业的条件.

参考文献

原文顺序

文献年度倒序

文中引用次数倒序

被引期刊影响因子

[1]

朱永国

. 飞机大部件自动对接若干关键技术研究[D]. 南京: 南京航空航天大学, 2012.

[本文引用: 1]

ZHU

Yongguo

. Research on some key techniques on aircraft large part automatic joining[D]. Nanjing: Nanjing University of Aeronautics and Astronautics, 2012.

[本文引用: 1]

[2]

CHEN

X H

, JIA

Y M

, MATSUNO

. Tracking control of nonholonomic mobile robots with velocity and acceleration constraints[C]// 2014 American Control Conference. Portland, USA: IEEE, 2014.

[本文引用: 1]

[3]

陈罡, 孟静, 高晓丁, 等.

基于Backstepping方法的移动机器人路径跟踪问题研究

[J]. 测控技术, 2016, 35(8): 52-56.

[本文引用: 1]

CHEN

Gang

, MENG

Jing

, GAO

Xiaoding

, et al.

Research on path tracking problem of the mobile robot based on backstepping

[J]. Measurement & Control Technology, 2016, 35(8): 52-56.

[本文引用: 1]

[4]

TIAN

X H

, LIU

H L

, LIU

H T

Robust finite-time consensus control for multi-agent systems with disturbances and unknown velocities

[J]. ISA Transactions, 2018, 80: 73-80.

DOI:S0019-0578(18)30288-X PMID:30086828 [本文引用: 1]

In this paper, we investigated the finite-time consensus tracking problem for multi-agent systems with external bounded disturbances and input bounded disturbances and unknown velocities. Based on the Lyapunov finite-time theorem, a novel finite-time consensus control is constructed by using the backstepping method. For unknown velocities, the high-gain observer is used to estimate the velocity information. It is proved that the consensus can be achieved in finite time. The consensus shows fast response and strong robustness to various disturbances. Finally, the effectiveness of the results is illustrated by numerical simulations.Copyright © 2018 ISA. Published by Elsevier Ltd. All rights reserved.

[5]

LIU

J H

, WANG

C L

, CAI

Global finite-time event-triggered consensus for a class of second-order multi-agent systems with the power of positive odd rational number and quantized control inputs

[J]. Neurocomputing, 2019, 360: 254-264.

DOI:10.1016/j.neucom.2019.05.065 URL [本文引用: 1]

[6]

XUE

L X

, WU

, XU

Y Z

, et al.

A simulation-based multi-objective optimization design method for pump-driven electro-hydrostatic actuators

[J]. Processes, 2019, 7(5): 274.

DOI:10.3390/pr7050274 URL [本文引用: 1]

[7]

SHAO

, XIE

, WANG

Leader-following formation control of multiple mobile vehicles

[J]. IET Control Theory & Applications, 2007, 1(2): 545-552.

DOI:10.1049/iet-cta:20050371 URL [本文引用: 1]

[8]

CHI

, ZHANG

C J

, SONG

, et al. A strategy of multi-robot formation and obstacle avoidance in unknown environment[C]//2016 IEEE International Conference on Information and Automation. Ningbo, China: IEEE, 2016: 1455-1460.

[本文引用: 1]

[9]

REN

, BEARD

R W

Decentralized scheme for spacecraft formation flying via the virtual structure approach

[J]. Journal of Guidance, Control, and Dynamics, 2004, 27(1): 73-82.

DOI:10.2514/1.9287 URL [本文引用: 1]

[10]

ZHOU

D J

, WANG

Z J

, SCHWAGER

Agile coordination and assistive collision avoidance for quadrotor swarms using virtual structures

[J]. IEEE Transactions on Robotics, 2018, 34(4): 916-923.

DOI:10.1109/TRO.2018.2857477 URL [本文引用: 1]

[11]

王树凤, 张钧鑫, 张俊友.

基于人工势场和虚拟领航者的智能车辆编队控制

[J]. 上海交通大学学报, 2020, 54(3): 305-311.

[本文引用: 1]

WANG

Shufeng

, ZHANG

Junxin

, ZHANG

Junyou

Intelligent vehicles formation control based on artificial potential field and virtual leader

[J]. Journal of Shanghai Jiao Tong University, 2020, 54(3): 305-311.

[本文引用: 1]

[12]

BENZERROUK

, ADOUANE

, MARTINET

Stable navigation in formation for a multi-robot system based on a constrained virtual structure

[J]. Robotics and Autonomous Systems, 2014, 62(12): 1806-1815.

DOI:10.1016/j.robot.2014.07.004 URL [本文引用: 1]

[13]

SARAVANAKUMAR

, ASOKAN

Multipoint potential field method for path planning of autonomous underwater vehicles in 3D space

[J]. Intelligent Service Robotics, 2013, 6(4): 211-224.

DOI:10.1007/s11370-013-0138-2 URL [本文引用: 1]

[14]

SINGH

, SHARMA

, SUTTON

, et al.

A constrained A^* approach towards optimal path planning for an unmanned surface vehicle in a maritime environment containing dynamic obstacles and ocean currents

[J]. Ocean Engineering, 2018, 169: 187-201.

DOI:10.1016/j.oceaneng.2018.09.016 URL [本文引用: 1]

[15]

LAMINI

, BENHLIMA

, ELBEKRI

Genetic algorithm based approach for autonomous mobile robot path planning

[J]. Procedia Computer Science, 2018, 127: 180-189.

DOI:10.1016/j.procs.2018.01.113 URL [本文引用: 1]

[16]

PANDEY

, SHUKLA

, TIWARI

Three-dimensional path planning for unmanned aerial vehicles using glowworm swarm optimization algorithm

[J]. International Journal of System Assurance Engineering and Management, 2018, 9(4): 836-852.

[本文引用: 1]

[17]

赵国旗, 杨明, 王冰, 等.

基于智能终端的移动机器人室内外无缝定位方法

[J]. 上海交通大学学报, 2018, 52(1): 13-19.

[本文引用: 1]

ZHAO

Guoqi

, YANG

Ming

, WANG

Bing

, et al.

Indoor and outdoor seamless positioning method of mobile robot based on intelligent terminal

[J]. Journal of Shanghai Jiao Tong University, 2018, 52(1): 13-19.

[本文引用: 1]

[18]

, SUN

, SATTAR

T P

Adapting sample size in particle filters through KLD-resampling

[J]. Electronics Letters, 2013, 49(12): 740-742.

DOI:10.1049/el.2013.0233 URL [本文引用: 1]

[19]

ANILY

, FEDERGRUEN

Simulated annealing methods with general acceptance probabilities

[J]. Journal of applied probability, 1987, 24(3): 657-667.

DOI:10.2307/3214097 URL [本文引用: 1]

[20]

COULTER

R C

. Implementation of the pure pursuit path tracking algorithm[R]. Pittsburgh, USA: Carnegie-Mellon UNIV Pittsburgh PA Robotics INST, 1992.

[本文引用: 1]

2012

... 飞机大部件装配作为飞机部装的核心环节,安全高效的部件运输以及自动化高精度的工装对接是整体工作完成的重要保障^[1].但是传统的固定平台装配导致工厂场地利用率低,整体装配过程灵活性低,并且装配设备的成本较高,若使用移载工装进行协同装配,则能让工厂的场地利用率和装配效率提高^[2].目前大多采用物流作业小车作为作业的移载工装,但是其运动精度较低.当各移载工装由于误差原因导致实际运动位姿偏离期望运动位姿时,多移载工装易使搬运件产生较大内力进而影响装配结果和产品最终的服役性能. ...

2012

2014

基于Backstepping方法的移动机器人路径跟踪问题研究

2016

... 合理的编队控制器能够有效降低队列偏差所导致的作业过程中的内力.由于作业环境的扰动和移载工装的运动误差影响,由多移载工装组成的多智能体系统鲁棒性和稳定性较低,并且具有非线性的特点,而反步法适用于非线性不确定系统的控制,其采取分布式的控制器策略,对多智能体中的每个子系统设计运动控制器^[3].Tian等^[4]提出了干扰情况下二阶多智能系统的反步法控制方法.Liu等^[5]在经典的反步法基础之上,提出有限时间触发方法,减小了通信负担.Xue等^[6]则利用滑模控制将状态观测与反步法进行结合,设计了一种自适应反步滑模控制方法. ...

基于Backstepping方法的移动机器人路径跟踪问题研究

2016

Robust finite-time consensus control for multi-agent systems with disturbances and unknown velocities

2018

Global finite-time event-triggered consensus for a class of second-order multi-agent systems with the power of positive odd rational number and quantized control inputs

2019

A simulation-based multi-objective optimization design method for pump-driven electro-hydrostatic actuators

2019

Leader-following formation control of multiple mobile vehicles

2007

... 期望运动状态和实际运动状态是反步法控制策略下的核心变量.对于多智能体的期望运动状态设计依赖于编队方法的设计,目前常用的编队方法主要包括领航跟随法^[7]、人工势场法^[8]、基于行为控制法^[9]、虚拟结构法^[10]和虚拟领航者法^[11].而其中能够保持队列刚性结构的方法主要为领航跟随法和虚拟结构法.领航跟随法将多机器人作业过程中的编队问题转化为领航机器人和跟随机器人的期望角度与距离之间的关系问题.虚拟结构法的主要理念是将多机器人的队形假设为虚拟的刚性结构进行控制规律设计.该编队方法下整体队形轻便,形成稳定队列的收敛时间快,但系统稳定性难以保证,鲁棒性和灵活性较差^[12].基于多移载工装进行协同装配时的刚性队列要求和不同工厂环境下模块化装配的需要,将虚拟结构法和虚拟领航者法相结合,以虚拟结构的参考点作为虚拟领航者进行作业运动规划.常用的运动规划算法有人工势场法^[13]、A^*算法^[14]、遗传算法^[15]、粒子群优化算法^[16]等.人工势场法的运动规划兼容编队控制和避障规划的要求,且具有计算量小、规划路径平滑易于设计轨迹追踪器的优点,被广泛运用于工程实践中.编队控制的另一个重要环节是实际运动状态的估计,通过测量设备的定位与滤波算法的结合,实时校正估计实际位置.而针对移动机器人的定位领域,基于粒子滤波算法进行定位最为广泛.粒子滤波以其具有多模态、统计特征随机分布的特性,适用于位姿估计和实时目标追踪.但是粒子滤波过程中重采样会导致粒子退化的问题,针对这一问题,赵国旗等^[17]将模糊理论引入粒子滤波来防止重采样中出现的多模态失效的问题.Li等^[18]针对粒子滤波结合引入基于库尔贝克-莱布勒散度(Kullback-Leibler Divergence, KLD)进行采样的方法,有效解决了粒子退化问题. ...

2016

Decentralized scheme for spacecraft formation flying via the virtual structure approach

2004

Agile coordination and assistive collision avoidance for quadrotor swarms using virtual structures

2018

基于人工势场和虚拟领航者的智能车辆编队控制

2020

基于人工势场和虚拟领航者的智能车辆编队控制

2020

Stable navigation in formation for a multi-robot system based on a constrained virtual structure

2014

Multipoint potential field method for path planning of autonomous underwater vehicles in 3D space

2013

A constrained A^* approach towards optimal path planning for an unmanned surface vehicle in a maritime environment containing dynamic obstacles and ocean currents

2018

Genetic algorithm based approach for autonomous mobile robot path planning

2018

Three-dimensional path planning for unmanned aerial vehicles using glowworm swarm optimization algorithm

2018

基于智能终端的移动机器人室内外无缝定位方法

2018

基于智能终端的移动机器人室内外无缝定位方法

2018

Adapting sample size in particle filters through KLD-resampling

2013

Simulated annealing methods with general acceptance probabilities

1987

... 传统人工势场法有可能出现合力为0引起的局部最优问题,对这种陷入局部极小值的情况,本文使用模拟退火法进行处理^[19].模拟退火算法与人工势场法结合的重点是当陷入局部最优的情况时,在当前点x附近邻域内选取随机点x₁,计算两点在人工势场中的势场U(x₁)和U(x),当U(x₁)<U(x)时,选择随机点x₁作为下一个路径点.当随机点x₁不是势场更低点时,以一定概率选取其作为下一路径点,其初始化转移概率P=

$e^{- \frac{Δ}{T}}$

,其中T为随着随机点增加而衰减的温度值函数,Δ为随机点与当前的势能差,即Δ=U(x₁)-U(x).通过不断地按照衰减的概率选取当前随机点邻域上新的随机位置点,直到逃离局部最优值,生成到达最终目标点的路径.具体的算法流程图如图4所示,其中α为对应转移概率的选取判断阈值. ...

1992

... 式中:

${\overset{\cdot}{x}}_{c}$

、

${\overset{\cdot}{y}}_{c}$

及

$\overset{\cdot}{θ}$

为移载工装质心x、y方向上的位移速度以及角速度.基于已经建立的移载工装运动模型,对于理想的输入控制量进行设计,本文采用基于几何关系的纯轨迹跟踪控制器^[20],从简化的差速轮模型出发, 在规划路径上选取最接近预瞄距离的目标点作为下一时刻追踪的预瞄点P_y,其在自身质心坐标系中的坐标为(x, y),移载工装循迹过程中的几何关系如图7所示,图中:l_d 为移载工装质心到预瞄点P_y的距离;R为当前期望目标期望轨迹对应的曲率半径;d_x预瞄点与相应曲率中心在自身质心坐标系X方向上的距离根据图7的几何关系可得距离方程: ...

〈

〉