Customized Interior-point Method for Cooperative Trajectory Planning of Multiple Unmanned Aerial Vehicles
-
摘要: 为提高多无人机(Unmanned aerial vehicles, UAV)协同轨迹规划(Cooperative trajectory planning, CTP)效率, 在解耦序列凸优化(Sequential convex programming, SCP)方法基础上, 提出一种高效求解凸优化子问题的定制内点法. 首先引入松弛变量, 构建子问题的等价描述形式, 并推导该形式下的子问题最优性条件. 然后在预测−校正原对偶内点法的框架下, 构建一套高效求解最优性条件方程组的计算流程以降低子问题计算复杂度, 并利用约束矩阵特征提出一种快速计算原对偶搜索方向的方法以提高规划效率. 仿真结果表明, 在解耦序列凸优化框架下, 定制内点法可将协同轨迹规划耗时降低一个数量级, 达到秒级.Abstract: To improve the efficiency of cooperative trajectory planning (CTP) for multiple unmanned aerial vehicles (UAVs), a customized interior-point method is proposed for efficiently solving convex subproblems within the decoupled sequential convex programming (SCP) method. Firstly, slack variables are introduced to construct an equivalent form of the subproblem, and the optimality conditions of the subproblem in this equivalent form are derived. Then, under the frame of the predictor-corrector primal-dual interior-point method, an efficient computation procedure for solving the equations of optimality conditions is developed. And through exploring the matrix characteristics of constraints to reduce the complexity of solving subproblems, a fast generation method for primal-dual search direction is proposed to improve the planning efficiency. Simulation results demonstrate that the customized interior-point method can decrease the computation times of CTP by one order of magnitude to several seconds.
-
在21世纪, 社会发展伴随着快速增长的资源需求, 为开采丰富的海洋资源, 海洋工程领域日益受到全球各国关注. 在这种情况下, 积极发展海洋装备设施技术至关重要. 深海起重机在海洋工程中扮演着重要角色, 例如海底隧道的搭建、跨海桥梁等项目中水下吊装作业和水下补给等任务. 因此, 针对深海起重机的研究具有非常重要的实际意义.
然而, 由于起重机的固有结构, 其所吊运负载的运动无法直接驱动, 只能通过台车运动间接控制, 从而产生欠驱动自由度(即负载摆角). 这种待控自由度多于控制输入的系统, 被称作欠驱动系统[1-4]. 欠驱动系统在工业生产中普遍存在, 如欠驱动机械臂[5]、下肢外骨骼[6]、无人机系统[7]、欠驱动TORA(Translational oscillators with rotating actuator)系统[8]以及欠驱动起重机系统[9-10], 其中欠驱动起重机系统又可细分为桥式起重机系统[11-13]、塔式起重机系统[14-15]、桅杆式起重机系统[16]、伴有双摆特性的起重机系统[17-18]等. 本文研究的深海起重机系统是一种在水下环境运输负载的欠驱动桥式起重机系统. 一方面, 系统本身存在复杂的欠驱动特性、非线性与耦合性; 另一方面, 在特殊的应用环境下, 负载运输过程还会受到水流作用力的影响. 海水作用力的存在也使系统各状态量间耦合关系更为复杂. 因此, 深海起重机系统依然存在许多悬而未决的控制难题.
近年来, 轨迹规划[19-22]方法成为解决欠驱动系统控制问题的常用方法, 用轨迹规划解决起重机控制问题[23-24]也逐渐一般化, 此类方法可看作从系统输入到输出的一种解, 通过考虑具体的控制目标, 将已知参数与期望轨迹作为系统输入, 并将可驱动状态量(如桥式起重机的台车位移、速度、加速度)的时间序列作为系统输出. 根据实际需求, 规划所得轨迹可对系统能耗、运输效率等指标进行优化, 同时还能够保证吊运的平稳性和准确性. 迄今为止, 研究人员针对陆地起重机系统(陆地桥式起重机、塔式起重机)提出了基于轨迹规划与跟踪的控制方法. 具体而言, 对双摆起重机系统, 陈鹤等[25]结合高斯伪谱法, 将双摆吊车的控制问题转化为函数的优化问题, 提出了一种时间最优的消摆轨迹规划方法; Sun等[26]设计了一种能量最优的轨迹规划方法, 可在整个运输过程中保证台车精确定位, 降低系统的能量能耗, 并有效抑制负载的残余摆动; Boscario等[27]提出了一种轨迹规划算法, 可增强轨迹对系统参数的鲁棒性, 有效抑制负载的残余摆动; Zhang等[28]提出了一种适用于非零初始负载摆角的误差跟踪控制方法. 对塔式起重机系统, Liu等[29]为起重臂和台车规划了一条轨迹, 此轨迹不仅能实现有效载荷的运输, 还可改善系统的瞬态性能.
然而, 上述现有的针对陆地起重机系统的轨迹规划方法均需离线进行, 无法很好地实现对系统的实时规划/控制, 因此, 在一些对实时性要求较高的场合, 现有离线规划方法便不再适用. 以桥式起重机系统为例, 当台车目标位置发生变化时, 需重新离线计算轨迹, 使作业效率受到影响. 为满足实时性要求, 一些学者进一步针对陆地起重机系统提出了基于在线轨迹规划的控制方法. 具体而言, 对桥式起重机系统, Otto等[30]提出了一种基于伺服约束的实时轨迹控制方法, 并通过理论分析与实际实验证明了该方法的实时性和鲁棒性; 通过对台车运动过程离散化, Li等[31]提出了一种基于载荷摆动能量的在线规划方法, 可最小化台车的运行时间.
尽管如此, 上述现有轨迹规划方法均是针对陆地起重机系统设计的, 在水下环境中(存在水流影响)往往难以直接适用. 除此之外, 大多现有的轨迹规划方法均将复杂非线性系统的模型线性化, 当负载摆动较大导致线性化条件不成立时便无法保证控制性能. 因此, 本文在未对复杂非线性模型线性化的前提下, 考虑水流作用, 为深海起重机系统设计了一种结构简单、参数易于调整的新型轨迹. 本文主要贡献如下: 1) 针对深海起重机系统, 提出了一种新型实时轨迹规划方法, 该方法基于非线性系统模型, 无需线性化近似; 2) 通过理论分析与仿真结果, 均证明了所提方法可使台车精确定位, 有效抑制负载摆动, 且对外部干扰有较强的鲁棒性, 同时也具有实时性, 可根据不同目标位置在线调整规划轨迹; 3) 该方法结合深海起重机系统在水下的作业环境, 考虑了在水流干扰下负载(如核燃料棒)呈现出的柔性特性.
本文结构安排如下: 第1节介绍深海起重机系统的动力学模型, 描述系统的控制目标. 在第2节中, 提出一种实时轨迹规划方法, 并从理论上分析该轨迹的良好性能. 接下来, 第3节对所提方法进行一系列仿真, 以验证其控制性能. 最后, 第4节对本文所做工作进行总结, 并对今后的研究进行展望.
1. 相关问题描述
本节将简述深海起重机系统的动力学模型, 并结合模型介绍相应的控制任务.
1.1 深海起重机系统动力学模型
图1是二维空间中深海起重机系统示意图, 其中在惯性坐标系
$ iOj $ 中,$ x(t) $ 表示台车位移,$ u(t) $ 表示驱动台车的控制输入.$ I $ 表示截面惯性矩. 定义柔性负载与台车连接端在$ i $ 方向上的位置为$ i = 0 $ , 负载自然状态下原长为$ l $ ,$ w(y,t) $ 表示在时刻$ t $ 及$ i $ 方向上$ i = y $ 处负载元的横向偏差(即负载摆动),$ f_w(y,t) $ 表示负载对应位置受到的水作用力. 系统参数如表1所示.表 1 系统参数Table 1 System parameters参数 物理意义 单位 $m_r$ 负载质量 kg $m_t$ 台车质量 kg $d$ 负载截面直径 m $l$ 负载长度 m $E$ 杨氏模量 GPa $c$ 粘性阻尼系数 N·s/m $\rho_w$ 水密度 kg/m3 $C_a$ 附加质量系数 — $C_d$ 阻力系数 — 水作用力
$ f_w(y,t) $ 表示为如下两部分的和[32]:$$ f_w(y,t) = f_m(y,t)+f_d(y,t) $$ 其中
$$ \begin{split} &f_m(y,t) = \frac{\pi}{4}\rho_wC_a(y)d^2\left(\ddot x(t)+\ddot w(y,t)\right) \\ &f_d(y,t) = \frac{1}{2}\rho_wC_d(y)d\left(\dot x(t)+\dot w(y,t)\right)\times \\ & \qquad\qquad\;\;|\left(\dot x(t)+\dot w(y,t)\right)| \end{split} $$ $ f_m(y,t) $ 与$ f_d(y,t) $ 分别表示水作用力$ f_w(y,t) $ 中的惯性部分与阻力部分.深海起重机系统的动力学方程为[32]
$$ (m_t+m_r)\ddot x(t)+m\int_{0}^{l}\ddot w(y,t){\rm{d}}y = u(t) $$ (1) $$\begin{split} &EIw''''(y,t)+m\ddot w(y,t)+c\dot w(y,t) =\;\;\;\;\;\\ &\qquad-m\ddot x(t) -f_d(y,t) \end{split}$$ (2) 其中
$$ m = \frac{m_r}{l}+\frac{\pi}{4}\rho_wC_a(y)d^2 $$ $ m $ 表示单位长度负载质量与被负载所排开流体质量的和. 记$ w'(y,t) $ 表示$ w(y,t) $ 对位置$ y $ 的一阶偏微分, 相应地,$ w''''(y,t) $ 表示$ w(y,t) $ 对位置$ y $ 的四阶偏微分.1.2 控制目标
针对深海起重机系统, 本文的控制目标是利用可驱动与不可驱动状态量之间的非线性耦合关系, 为可驱动的台车运动在线规划一条轨迹, 在实现台车准确快速定位的同时, 还能够在水下作业环境下充分抑制柔性负载的摆动. 具体所要实现的控制任务概括如下:
1) 设计一条参考轨迹
$ \ddot x_r(t) $ 使台车快速准确地到达目标位置;2) 在轨迹中设计一个消摆环节
$ \ddot x_e(t) $ 以消除负载摆动.因此, 为满足上述两部分要求, 本文将加速度轨迹
$ \ddot x(t) $ 规划如下:$$ \ddot x(t) = \ddot x_r(t)+\ddot x_e(t) $$ (3) 实时轨迹规划的过程如图2所示.
2. 轨迹规划及分析
为实现上述控制目标, 本节将提出具体的实时轨迹规划方法, 并给出相应的理论分析.
2.1 轨迹规划
基于式(2)所示的深海柔性起重机系统状态变量间的耦合关系, 首先考虑设计消摆轨迹
$ \ddot x_e(t) $ . 根据实际情况, 负载末端点在所有点元中的摆动幅度最大, 若末端点摆动范围满足要求, 则其他点的摆动幅度也可保证在要求范围内. 本节中, 仅考虑负载末端点位置($ y = l $ 处)的动力学. 相应地,$ C_a(y) $ ,$ C_d(y) $ 可简记为$ C_a $ ,$ C_d $ ,$ w(y,t) $ 可记为$ w(t) $ ,$ f_d(y,t) $ 记为$ f_d(t) $ . 令$ \ddot x_r(t) = 0 $ ,$ f_d(t) $ 作为$ w(t) $ ,$ \dot w(t) $ 的高阶项可忽略不计, 那么式(2)可表示为$$ EIw''''(t)+m\ddot w(t)+c\dot w(t) = -m\ddot x(t) $$ (4) 在式(4)的基础上, 为方便后续系统的有界性与收敛性分析, 构造李雅普诺夫候选函数如下:
$$ V(t) = \frac {1}{2}EIw''^2(t)+\frac {1}{2}m\dot w^2(t)\ge 0 $$ (5) 对式(5)进行求导, 并将结果代入式(4), 整理可得
$$ \begin{split} \dot V(t) =\;& \dot w(t)(EIw''''(t)+m\ddot w(t)) =\\ & -\dot w(t)\left(c\dot w(t)+m\ddot x(t)\right) =\\ & -c\dot w^2(t)-m\dot w(t)\ddot x(t) \end{split} $$ (6) 设计消摆轨迹
$ \ddot x_e(t) $ 为如下形式:$$ \ddot x_e(t) = k\dot w(t) $$ (7) 其中,
$k\in {\bf R}^+$ 为正增益. 考虑设定条件$\ddot x_r(t) = 0 ,$ 即$ \ddot x(t) = \ddot x_r(t)+\ddot x_e(t) = \ddot x_e(t) $ . 将式(7)代入式(6)进行整理, 得$$ \dot V(t) = -(c+km)\dot w^2(t)\le 0 $$ 根据拉塞尔不变性定理, 可以得出如下结论:
$$ \dot w(t)\rightarrow 0, \;\ddot w(t)\rightarrow 0 $$ (8) 考虑到仅有
$ \ddot x_e(t) $ 不能实现台车的精准定位, 还需要选择定位参考轨迹$ \ddot x_r(t) $ , 使其满足如下条件:1) 为模拟实际情况下台车的起始运动, 设轨迹的初始条件为
$$ x_r(0) = 0,\;\;\dot x_r(0) = 0 $$ (9) 2) 考虑到深海起重机的实际工作需求,
$ x_r(t) $ 要在有限时间$ t_p $ 内收敛到目标位置$ p_d $ , 且无超调, 即$$ \lim\limits_{t \to t_p}x_r(t) = p_d $$ (10) 3)
$ \dot x_r(t) $ ,$ \ddot x_r(t) $ 和$ \dddot x_r(t) $ 应该满足如下条件:$$ 0\le \dot x_r(t)\le v_m,\ |\ddot x_r(t)|\le a_m,\ |\dddot x_r(t)|\le j_m $$ (11) $$ {\lim\limits_{t \to +\infty}}\dot x_r(t) = 0,{\lim\limits_{t \to +\infty}}\ddot x_r(t) = 0 $$ (12) 其中,
$v_m ,$ $ a_m $ 和$ j_m $ 分别代表所选参考轨迹的速度、加速度和加加速度的上界.只需满足式(9)~(12)所示的条件, 定位参考轨迹
$ x_r(t) $ 便可根据实际需要任意选择, 结合式(7), 最终规划轨迹可表示为$$ \ddot x(t) = \ddot x_r(t)+\ddot x_e(t) = \ddot x_r(t)+k\dot w(t) $$ (13) 其中, 增益
$k\;(k\in {\bf R}^+)$ 还应满足如下条件:$$ km+c-1\ge 0 $$ (14) 通过对式
$ (13) $ 积分, 可分别求得台车速度与台车位移的表达式为$$ \dot x(t) = \dot x_r(t)+\dot x_e(t) = \dot x_r(t)+kw(t) $$ (15) $$ \begin{split} x(t) = \;&x_r(t)+x_e(t) =\\ & x_r(t)+k\int_{0}^{t} w(\tau_t){\rm{d}}\tau_t+Q(k) \end{split} $$ (16) 其中
$$ \begin{split} Q(k)\; =\; &\frac {k}{km+c} \;\times \\ &\left(mp_d+\frac {1}{2}\rho_wC_dd\int_{0}^{\infty}\int_{0}^{t}\dot x_r^2(\tau_t){\rm{d}}\tau_t{\rm{d}}t\right) \end{split} $$ (17) 其中,
$ Q(k) $ 为只关于$ k $ 的函数, 相对时间$ t $ 为常数, 因此, 式(13), (15)和(16)中的等式关系成立. 其中, 关于$ Q(k) $ 函数特性的证明过程将在第2.2节中给出.2.2 性能分析
为便于理论分析, 本小节利用定理1证明设计轨迹的良好性能.
定理 1. 如式(13)所示的轨迹
$ x(t) $ 及其导数光滑且一致连续. 在沿该轨迹运行时, 深海起重机系统的各状态量均能收敛, 具体如下所示:1) 负载摆动及其前两阶导数均收敛至零, 即
$$ {\lim\limits_{t \to +\infty}}[w(t)\ \ \dot w(t)\ \ \ddot w(t)]^{\rm{T}} = [0\ \ 0\ \ 0]^{\rm{T}} $$ (18) 2) 台车速度、加速度收敛到零, 且台车准确到达目标位置
$ p_d $ , 即$$ {\lim\limits_{t \to +\infty}}[x(t)\ \ \dot x(t)\ \ \ddot x(t)]^{\rm{T}} = [p_d\ \ 0\ \ 0]^{\rm{T}} $$ (19) 证明. 为使证明过程更清晰, 特分为两个步骤证明定理1. 其中, 步骤1将证明式(18), 即负载摆动及其前两阶导数的收敛性, 接着对应式(19), 步骤2将证明台车位移、速度、加速度的收敛性.
步骤 1. 将式(13)代入式(6), 并整理可得
$$ \dot V(t) = -(km+c)\dot w^2(t)-m\dot w(t)\ddot x_r(t) $$ (20) 根据式(14), 并借助均值不等式进行放缩, 式(20)可整理为
$$ \begin{split} \dot V(t) \le &-(km+c)\dot w^2(t)+2\times|\frac{1}{2}m\ddot x_r(t)|\times|\dot w(t)|\le\\ & \frac {1}{4}m^2\ddot x_r^2(t)+\dot w^2(t)-(km+c)\dot w^2(t)=\\ & \frac {1}{4}m^2\ddot x_r^2(t)-(km+c-1)\dot w^2(t)\\[-15pt] \end{split} $$ (21) 式(21)对时间求积分, 可以得到
$$ \begin{split} V(t)\le &\frac {1}{4}m^2\int_{0}^{t}\ddot x_r^2(\tau_t){\rm{d}}\tau_t\;-\\ &(km+c-1)\int_{0}^{t}\dot w^2(\tau_t){\rm{d}}\tau_t+V(0) \end{split} $$ (22) 由式(9)和式(11), 通过分部积分, 式(22)中的第1项满足如下不等式:
$$ \begin{split} &\frac {1}{4}m^2\int_{0}^{t}\ddot x_r^2(\tau_t){\rm{d}}\tau_t \le m^2\int_{0}^{t}\ddot x_r^2(\tau_t){\rm{d}}\tau_t=\\ &\qquad m^2\left[\ddot x_r(\tau_t)\dot x_r(\tau_t)|_{0}^{t}-\int_{0}^{t}\dot x_r(\tau_t)\dddot x_r(\tau_t){\rm{d}}\tau_t\right]\le\\ &\qquad m^2\left[\ddot x_r(t)\dot x_r(t)+j_m\int_{0}^{t}\dot x_r(\tau_t){\rm{d}}\tau_t\right]=\\ & \qquad m^2\left[\ddot x_r(t)\dot x_r(t)+j_mx_r(t)\right]\in L_{\infty} \\[-10pt]\end{split} $$ (23) 根据式(14), 式(22)中第2项非正, 即
$$ -(km+c-1)\int_{0}^{t}\dot w^2(\tau_t){\rm{d}}\tau_t\le 0 $$ (24) 通过式(22)~(24), 可以得到
$$ V(t)\in L_{\infty} $$ 再结合式(5), (11)和(20), 可以推出如下结论:
$$ \begin{split} &V(t)\in L_{\infty} \Rightarrow w''(t)\in L_{\infty}, \\ &\dot w(t)\in L_{\infty} \Rightarrow \dot V(t)\in L_{\infty} \end{split} $$ (25) 根据式(22), (23)和(25), 可推出
$$ \begin{split} (km+c&-1)\int_{0}^{t}\dot w^2(\tau_t){\rm{d}}\tau_t \le \\ &\frac {1}{4}m^2\int_{0}^{t}\ddot x_r^2(\tau_t){\rm{d}}\tau_t+\\ & V(0)-V(t)\ \in L_{\infty} \Rightarrow \\& \dot w(t)\in L_{2} \end{split} $$ (26) 将规划后的轨迹
$ \ddot x(t) $ 代入耦合关系(4), 得$$ \begin{split} m\ddot w(t) = & -EIw''''(t)-c\dot w(t)-km\dot w(t)-m\ddot x_r(t)-\\ & \frac {1}{2}\rho_wC_dd(\dot x_r(t)+\dot w(t))^2\\[-10pt] \end{split} $$ (27) 由
$ \dot w(t),\ w''''(t),\ \dot x_r(t),\ \ddot x_r(t)\in L_{\infty} $ 可得$$ \ddot w(t)\in L_{\infty} $$ (28) 又因为
$ \dot w(t)\in L_{2}\bigcap L_{\infty} $ 并有$ \ddot w(t)\in L_{\infty} $ , 利用芭芭拉定理可推出$$ {\lim\limits_{t \to +\infty}}\dot w(t) = 0 $$ (29) 此外, 还需对
$ w(t) $ ,$ \dot w(t) $ 进行证明. 首先通过对式(27)进行整理, 可以得到$$ \begin{split} \ddot w(t) = & -\frac {EI}{m}w''''(t)-\\ & \left[\left(k+\frac {c}{m}\right)\dot w(t)+\ddot x_r(t)\right]-\\ & \left[\frac {1}{2}\frac {\rho_wC_dd}{m}\left(\dot x_r(t)+\dot w(t)\right)^2\right] \end{split} $$ 将
$ \ddot w(t) $ 写为两函数相加的形式$\ddot w(t) = \varphi_1(t)+ $ $ \varphi_2(t)$ , 其中$$ \begin{split} \varphi_1(t) = & -\frac {EI}{m}w''''(t) \\ \varphi_2(t) = & -\left[\left(k+\frac {c}{m}\right)\dot w(t)+\ddot x_r(t)\right]-\\ & \left[\frac {1}{2}\frac {\rho_wC_dd}{m}\left(\dot x_r(t)+\dot w(t)\right)^2\right] \end{split} $$ 利用式(12), (25)和(29), 可推得
$$ \begin{split} & \dot \varphi_1(t) = -\frac {EI}{m}\dot w''''(t)\in L_{\infty}\\ & {\lim\limits_{t \to +\infty}}\varphi_2(t) = 0 \end{split} $$ (30) 结合式(29)和式(30), 应用扩展芭芭拉定理[33], 可推出
$$ \begin{split} & {\lim\limits_{t \to +\infty}}\varphi_1(t) = -\frac {EI}{m}{\lim\limits_{t \to +\infty}}w''''(t) = 0\\ & {\lim\limits_{t \to +\infty}}\ddot w(t) = 0 \end{split} $$ (31) 式(31)中,
$ w''''(t) $ 可看作$ w(t) $ 的加减运算式, 由此可以得到$$ {\lim\limits_{t \to +\infty}}w(t) = 0 $$ (32) 至此, 以上过程证明了本定理中的式(18). 接下来, 证明式(19).
步骤 2. 式(13)对时间
$ t $ 的导数为$$ \dddot x(t) = \dddot x_r(t)+k\ddot w(t) $$ (33) 根据式(13), (15)和(33), 可知
$x(t) ,$ $\dot x(t),$ $ \ddot x(t) $ 均连续可导. 根据式(11)的约束条件, 可知$$ \dot x_r(t), \ddot x_r(t), \dddot x_r(t)\in L_{\infty} $$ 另外, 考虑实际情况
$ w(t) $ 有界, 再结合式(25)和式(28), 可得$$ w(t), \dot w(t), \ddot w(t)\in L_{\infty} $$ 因此, 根据式(13), (15)和(33), 可推知
$$ \dot x(t),\ \ddot x(t),\ \dddot x(t)\in L_{\infty} $$ (34) 即
$x(t) ,$ $\dot x(t),$ $ \ddot x(t) $ 均一致连续.接着, 将式(13)代入式(27)可得
$$ m\ddot x(t) = -c\dot w(t)-m\ddot w(t)-\frac {1}{2}\rho_wC_dd\dot x_r^2(t) $$ (35) 考虑式(12), (29)和(31), 则式(35)关于时间的极限为
$$ \begin{split} {\lim\limits_{t \to +\infty}}\ddot x(t) = & -\frac {1}{m}\left(c{\lim\limits_{t \to +\infty}}\dot w(t)+m{\lim\limits_{t \to +\infty}}\ddot w(t)\right)-\\ &\frac {1}{m}\left(\frac {1}{2}\rho_wC_dd{\lim\limits_{t \to +\infty}}\dot x_r^2(t)\right)= 0 \\[-10pt]\end{split} $$ (36) 将式(12)和式(32)代入式(15), 整理可得
$$ {\lim\limits_{t \to +\infty}}\dot x(t) = {\lim\limits_{t \to +\infty}}\left(\dot x_r(t)+kw(t)\right) = 0 $$ (37) 接下来将证明
$ x(t) $ 的收敛性. 通过对式(35)求积分, 并代入式(15), 计算得到$$ \begin{split} (km+c)w(t) = &-m\dot w(t)-m\dot x_r(t)-\\ &\frac {1}{2}\rho_wC_dd\int_{0}^{t}\dot x_r^2(\tau_t){\rm{d}}\tau_t \end{split} $$ (38) 继而对式(38)求关于时间的积分, 当
$ t $ $ \rightarrow $ $ +\infty $ 时, 式(38)可写为$$ \begin{split} (km+c)&\int_{0}^{+\infty}w(\tau_t){\rm{d}}\tau_t = -m{\lim\limits_{t \to +\infty}}\left(w(t)+x_r(t)\right)-\\ & \frac {1}{2}\rho_wC_dd\int_{0}^{\infty}\int_{0}^{t}\dot x_r^2(\tau_t){\rm{d}}\tau_tdt\\[-15pt] \end{split} $$ (39) 再根据式(17)和式(32), 整理式(39), 可以得到
$$ \begin{split} k\int_{0}^{+\infty}&w(\tau_t){\rm{d}}\tau_t = -\frac {k}{km+c}\left(mp_d\right)-\frac {k}{km+c}\times \\ & \left(\frac {1}{2}\rho_wC_dd\int_{0}^{\infty}\int_{0}^{t}\dot x_r^2(\tau_t){\rm{d}}\tau_t{\rm{d}}t\right) = -Q(k) \\[-10pt]\end{split} $$ (40) 其中, 记
$$ P = \int_{0}^{\infty}\int_{0}^{t}\dot x_r^2(\tau_t){\rm{d}}\tau_t{\rm{d}}t $$ (41) 对式(41)进行分部积分, 计算可得
$$ \begin{split} P =& \int_{0}^{\infty}\left[ x_r(\tau_t)\dot x_r(\tau_t)|_{0}^{t}-\int_{0}^{t}x_r(\tau_t)\ddot x_r(\tau_t){\rm{d}}\tau_t\right] {\rm{d}}t=\\ & \int_{0}^{\infty}x_r(t)\dot x_r(t){\rm{d}}t-\int_{0}^{\infty}\int_{0}^{t}x_r(\tau_t)\ddot x_r(\tau_t){\rm{d}}\tau_t{\rm{d}}t=\\ & \frac{1}{2}x_r^2(t)|_{0}^{\infty}-\int_{0}^{\infty}\int_{0}^{t}x_r(\tau_t)\ddot x_r(\tau_t){\rm{d}}\tau_t{\rm{d}}t=\\ & \frac{1}{2}p_d^2-\int_{0}^{\infty}\int_{0}^{t}x_r(\tau_t)\ddot x_r(\tau_t){\rm{d}}\tau_t{\rm{d}}t \\[-15pt]\end{split} $$ (42) 式(42)中, 记
$$ T = \int_{0}^{\infty}\int_{0}^{t}x_r(\tau_t)\ddot x_r(\tau_t){\rm{d}}\tau_t{\rm{d}}t $$ (43) 利用二重积分的区域可加性质整理式(43), 拆解过程如下:
$$ \begin{split} T = & \int_{0}^{t_p}\int_{0}^{t}x_r(\tau_t)\ddot x_r(\tau_t){\rm{d}}\tau_t{\rm{d}}t\;+\\ & \int_{t_p}^{\infty}\int_{t_p}^{t}x_r(\tau_t)\ddot x_r(\tau_t){\rm{d}}\tau_t{\rm{d}}t\;+\\ & \int_{0}^{t_p}\int_{t_p}^{\infty}x_r(\tau_t)\ddot x_r(\tau_t){\rm{d}}\tau_t{\rm{d}}t \end{split} $$ (44) 当
$ \tau_t>t_p $ 时,$\ddot x_r(\tau_t)\equiv 0 ,$ 故$x_r(\tau_t)\ddot x_r(\tau_t) = 0,$ 即式(44)中后两项均为0. 而在式(44)第1项中,$\int_{0}^{t}x_r(\tau_t)\ddot x_r(\tau_t){\rm{d}}\tau_t$ 为关于时间$ t $ 的确定函数, 此函数在有限区间$ [0, t_p] $ 上的积分值为常数, 故第1项为常值. 因此,$ T $ 为常值. 将式(43)代入式(42)可得$$ P = \frac{1}{2}p_d^2-T $$ (45) 根据对式(44)的分析以及式(45)可知,
$ P $ 为常值, 故$ Q(k) $ 与时间$ t $ 无关, 仅为$ k $ 的函数. 整理式(40), 有$$ k\int_{0}^{+\infty}w(\tau_t){\rm{d}}\tau_t+Q(k) = 0 $$ (46) 将式(46)代入式(16), 整理得到
$$ \begin{split} {\lim\limits_{t \to +\infty}}x(t)& = {\lim\limits_{t \to +\infty}}x_r(t)+k\int_{0}^{+\infty}w(\tau_t){\rm{d}}\tau_t+Q(k) = p_d \end{split} $$ (47) 因此, 由式(47)可知,
$ x(t) $ 最终收敛到$p_d ,$ 本定理的第2部分证毕. 综合步骤1与步骤2的证明过程, 可得所设计轨迹具有良好的定位和消摆性能. □3. 仿真结果与分析
为验证所提轨迹规划方法的有效性, 本节使用MATLAB进行数值仿真, 首先说明了应用本文所提方法前后负载振动幅度的变化, 接着测试了在有外部扰动(初始扰动与中间扰动)、多次变换目标位置
$ p_d $ 、与已有轨迹规划方法相比较三种设定情况下的轨迹跟踪消摆性能. 仿真参数及其取值如表2所示.表 2 系统参数仿真值Table 2 Simulation values of system parameters参数 取值 单位 $m_r$ 0.37 kg $m_t$ 38.0 kg $d$ 0.008 m $l$ 1 m $E$ 248.3 GPa $c$ 0.6 N·s/m $\rho_w$ 1000 kg/m3 $C_a$ 0.93 — $C_d$ 1.28 — 梯形速度轨迹是轨迹规划方法中最基本、也是最常用的一种轨迹, 对控制方法的好坏有一定标称作用, 因此, 本文选择如下所示经平滑处理的梯形速度轨迹作为定位参考轨迹:
$$ \ddot x_r(t) = \begin{cases} \dfrac{50}{9}p_dt ,&0\le t<0.1\\ \dfrac{5}{9}p_d ,& 0.1\le t\le 0.9 \\ -\dfrac{50}{9}p_dt+\dfrac{50}{9}p_d ,&0.9<t\le 1\\ 0 ,&1<t<2\\ -\dfrac{50}{9}p_dt+\dfrac{100}{9}p_d ,&2\le t<2.1\\ -\dfrac{5}{9}p_d ,& 2.1\le t\le 2.9 \\ \dfrac{50}{9}p_dt-\dfrac{50}{3}p_d, &2.9<t\le 3\\ 0 ,&t>3 \end{cases} $$ (48) 设式(48)中目标位置为
$ p_d = 1.2\ {\rm{m}} $ , 则定位参考轨迹的位移、速度、加速度轨迹如图3所示, 由图3可知, 所选定位参考轨迹可满足式(9)~(12)的全部条件. 通过多次调试, 式(14)中参数$ k $ 取为1.
图 3 参考位移、速度、加速度轨迹Fig. 3 The reference displacement, velocity, and acceleration trajectories值得说明的是, 图4中粗点线与点划线分别代表设定的目标位置与负载摆动进入相对稳定状态的设定边界值. 另外, 结合仿真结果, 考虑量化负载摆动幅度(负载在台车运动方向反方向的最大摆动值)、反向摆动幅度(负载在台车运动方向的最大摆动值)、进入相对稳态时间(负载摆动值到达并保持在
$ \pm0.024\ {\rm{m}} $ 内所需的最短时间)三个指标来定量分析比较, 从而说明本文所提方法良好的控制性能.无外部扰动的仿真对比结果如图4所示, 同时, 表3列出了量化指标的具体数值. 经由表3数据计算可得, 相比未考虑消摆的定位参考轨迹, 本文实时轨迹规划方法使负载摆动幅度降低了48.92%, 反向摆动幅度降低了25%, 进入相对稳态时间缩短了0.6
$ {\rm{s}}, $ 三个量化指标均优于对比情形; 再结合图4的直观结果, 本文所提方法可在实现台车精准定位的同时, 快速有效地抑制负载的摆动幅度, 具有良好的消摆控制性能.表 3 无外部扰动时量化指标对比结果Table 3 Comparison results of quantitative indices without external disturbance正向摆动
幅度 (m)反向摆动
幅度 (m)进入相对稳态
时间 (s)定位参考轨迹 –0.278 0.032 3.7 本文规划轨迹 –0.142 0.024 3.1 图5为无外部扰动时柔性负载摆动的三维仿真图, 表示负载横向偏差
$ w(y,t) $ 随位置$ y $ 与时间$ t $ 变化的三维曲线, 直观显示了负载各位置摆动幅度随时间的变化情况. 同样地, 图6 ~ 9中粗点线代表设定的目标位置; 图9中点划线代表负载摆动进入相对稳定状态的设定边界值.
图 8 验证所提方法实时性的仿真结果Fig. 8 Simulation results to verify the real-time performance of the proposed method为验证所提消摆轨迹对外部扰动的鲁棒性, 并结合实际起重机的起重过程, 本文设计了加入初始扰动(
$ t = 0\ {\rm{s}} $ 时, 在负载末端加入幅度为$ -0.1\ {\rm{m}} $ 的扰动)与中间扰动($ t = 6.5\ {\rm{s}} $ 时, 在负载末端加入幅度为$ -0.1\ {\rm{m}} $ 的扰动)的仿真测试(采用比例分配的方式将扰动施加在柔性负载上), 仿真结果分别如图6和图7所示. 由图6和图7可知, 在外部扰动存在的情况下, 相比未考虑消摆的参考轨迹, 本文规划轨迹可使系统更快稳定, 并快速有效地减小负载的摆动幅度, 具有较强的鲁棒性.此外, 为说明所提轨迹规划方法的实时性(即规划轨迹随选择目标点的不同而不同, 且无需离线计算), 本文模拟在操作过程中目标位置多次变化的情况, 设计了一组驱动台车依次到达三个目标位置(分别为
$ p_{d1} = 1.2\ {\rm{m}}, $ $ p_{d2} = 2.4\ {\rm{m}} ,$ $ p_{d3} = 4\ {\rm{m}}) $ 的仿真, 仿真结果如图8所示. 由图8可知, 本文所提轨迹规划方法可使台车运行轨迹跟踪到达设定目标位置, 且实时调整, 不需离线计算; 其次, 负载摆动幅度明显减小(虽目标位置不同, 但与定位参考轨迹相比, 负载摆动幅度均降低了50%左右), 系统更快稳定, 也验证了所提方法良好的控制性能.最后, 为验证方法的有效性, 选择改进的输入整形方法[32]作为对比方法, 设定目标位置为
$ p_d = $ $ 1.2\ {\rm{m}} $ , 运送时间为$ 3\ {\rm{s}} $ , 得到如图9所示的仿真结果与表4所列的量化指标数值. 由图9与表4可知, 与输入整形方法比较, 本文所提轨迹规划方法可有效抑制负载摆动(负载摆动幅度降低了22.4%), 超调更小(反向摆动幅度降低了53.85%)且更快速平稳地回到自然无摆状态(进入相对稳态时间缩短了1.2$ {\rm{s}} $ ), 具有较好的定位消摆性能, 体现了所提方法的有效性.表 4 与输入整形方法的量化指标对比结果Table 4 Comparison results of quantitative indices with input shaping method正向摆动
幅度 (m)反向摆动
幅度 (m)进入相对稳态
时间 (s)输入整形方法 –0.183 0.052 4.3 本文规划方法 –0.142 0.024 3.1 由上述一系列仿真结果可知, 本文所提实时消摆轨迹可在使台车快速、准确到达目标位置的同时, 较好地抑制柔性负载摆动.
4. 总结
基于深海柔性起重机系统可驱变量与不可驱变量间的非线性耦合关系, 本文提出一种实时轨迹规划方法, 该方法不仅能实现台车精确定位, 有效抑制负载摆动, 而且对外部干扰有较强的鲁棒性, 同时也可根据不同目标位置实时调整规划轨迹, 具有实时性. 经过理论分析, 证明了系统各变量的收敛性, 且仿真结果也验证了所设计轨迹良好的消摆控制性能. 在今后的研究中, 将进一步缩小定位参考轨迹的选择范围(例如可以优先选择结构形式简单、参数数量少、阶次低的表达式作为定位参考轨迹), 并将实时规划方法推广到其他欠驱动系统.
-
图 1 多无人机协同轨迹规划问题示意图
Fig. 1 Illustration of multi-UAV cooperative trajectory planning problems
图 4 原对偶搜索方向计算复杂度变化图
Fig. 4 Variations of computation complexity for primal-dual search direction
图 7 编队重构协同轨迹规划二维结果图
Fig. 7 Cooperative trajectories in 2D for formation reconfiguration
图 8 编队重构协同轨迹规划三维结果图
Fig. 8 Cooperative trajectories in 3D for formation reconfiguration
图 9 编队集结任务无人机间最短距离变化曲线
Fig. 9 Variation curve of minimum distance between UAVs for formation rendezvous
图 10 编队重构任务无人机间最短距离变化曲线
Fig. 10 Variation curve of minimum distance between UAVs for formation reconfiguration
图 13 基于SeDuMi的编队集结轨迹规划结果
Fig. 13 Trajectory planning results using SeDuMi for formation rendezvous
图 14 基于定制内点法的编队集结轨迹规划结果
Fig. 14 Trajectory planning results using customized interior-point method for formation rendezvous
图 15 基于SeDuMi的编队重构轨迹规划结果
Fig. 15 Trajectory planning results using SeDuMi for formation reconfiguration
图 16 基于定制内点法的编队重构轨迹规划结果
Fig. 16 Trajectory planning results using customized interior-point method for formation reconfiguration
表 1 原对偶搜索方向计算复杂度
Table 1 Computation complexity for primal-dual search direction
计算方法 计算成本 方法 I 0.33$n_{{X}}^3$+ 3$n_A^3$+$n_{{X}}^2{n_A}$+${n_{{X}}}n_A^2$ 方法 II 2.67$n_{{X}}^3$+ 3$n_A^3$+$n_{{X}}^2{n_A}$+${n_{{X}}}n_A^2$ 方法III 2.67 (${n_{{X}}}$+2${n_A}$)3 
下载: 导出CSV
-
[1] 沈林成, 牛轶峰, 朱华勇. 多无人机自主协同控制理论与方法. 北京: 国防工业出版社, 2013. 1−9Shen Lin-Cheng, Niu Yi-Feng, Zhu Hua-Yong. Theories and Methods of Autonomous Cooperative Control for Multiple UAVs. Beijing: National Defense Industry Press, 2013. 1−9 [2] Liu X F, Lu P, Pan B F. Survey of convex optimization for aerospace applications. Astrodynamics, 2017, 1(1): 23-40 doi: 10.1007/s42064-017-0003-8 [3] Boyd S, Vandenberghe L. Convex Optimization. Cambridge: Cambridge University Press, 2004. 1−15 [4] Liu X F, Lu P. Solving nonconvex optimal control problems by convex optimization. Journal of Guidance, Control, and Dynamics 2014, 37(3): 750-765 doi: 10.2514/1.62110 [5] Schulman J, Duan Y, Ho J, Lee A, Awwal I, Bradlow H, et al. Motion planning with sequential convex optimization and convex collision checking. The International Journal of Robotics Research, 2014, 33(9): 1251-1270 doi: 10.1177/0278364914528132 [6] Misra G, Bai X L. Optimal path planning for free-flying space manipulators via sequential convex programming. Journal of Guidance, Control, and Dynamics, 2017, 40(11): 3019-3026 doi: 10.2514/1.G002487 [7] 刘延杰, 朱圣英, 崔平远. 序列凸优化的小天体附着轨迹优化. 宇航学报, 2018, 39(2): 177-183 doi: 10.3873/j.issn.1000-1328.2018.02.008Liu Yan-Jie, Zhu Sheng-Ying, Cui Ping-Yuan. Soft landing trajectory optimization on asteroids by convex programming. Journal of Astronautics, 2018, 39(2): 177-183 doi: 10.3873/j.issn.1000-1328.2018.02.008 [8] 王劲博, 崔乃刚, 郭继峰, 徐大富. 火箭返回着陆问题高精度快速轨迹优化算法. 控制理论与应用, 2018, 35(3): 389-398 doi: 10.7641/CTA.2017.70730Wang Jin-Bo, Cui Nai-Gang, Guo Ji-Feng, Xu Da-Fu. High precision rapid trajectory optimization algorithm for launch vehicle landing. Control Theory & Applications, 2018, 35(3): 389-398 doi: 10.7641/CTA.2017.70730 [9] Zhang Z, Li J X, Wang J. Sequential convex programming for nonlinear optimal control problems in UAV path planning. Aerospace Science and Technology, 2018, 76: 280-290 doi: 10.1016/j.ast.2018.01.040 [10] Zhou D, Zhang Y Q, Li S L. Receding horizon guidance and control using sequential convex programming for spacecraft 6-DOF close proximity. Aerospace Science and Technology, 2019, 87: 459-477 doi: 10.1016/j.ast.2019.02.041 [11] Augugliaro F, Schoellig A P, D＇Andrea R. Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. Vilamoura-Algarve, Portugal: IEEE, 2012. 1917−1922 [12] Chen Y F, Cutler M, How J P. Decoupled multiagent path planning via incremental sequential convex programming. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Seattle, USA: IEEE, 2015. 5954−5961 [13] Morgan D, Subramanian G P, Chung S J, Hadaegh F. Swarm assignment and trajectory optimization using variable-swarm, distributed auction assignment and sequential convex programming. The International Journal of Robotics Research, 2016, 35(10): 1261-1285 doi: 10.1177/0278364916632065 [14] 王祝, 刘莉, 龙腾, 温永禄. 基于罚函数序列凸规划的多无人机轨迹规划. 航空学报, 2016, 37(10): 3149-3158Wang Zhu, Liu Li, Long Teng, Wen Yong-Lu. Trajectory planning for multi-UAVs using penalty sequential convex programming. Acta Aeronautica et Astronautica Sinica 2016, 37(10): 3149-3158 [15] Wang Z, Liu L, Long T. Minimum-time trajectory planning for multi-unmanned-aerial-vehicle cooperation using sequential convex programming. Journal of Guidance, Control, and Dynamics, 2017, 40(11): 2976-2982 doi: 10.2514/1.G002349 [16] Wang Z, Liu L, Long L, Xu G T. Efficient unmanned aerial vehicle formation rendezvous trajectory planning using Dubins path and sequential convex programming. Engineering Optimization, 2019, 51(8): 1412-1429 doi: 10.1080/0305215X.2018.1524461 [17] Sturm J F. Using SeDuMi 1.02, a matlab toolbox for optimization over symmetric cones. Optimization Methods and Software, 1999, 11(1-4): 625-653 doi: 10.1080/10556789908805766 [18] Andersen E D, Roos C, Terlaky T. On implementing a primal-dual interior-point method for conic quadratic optimization. Mathematical Programming, 2003, 95(2): 249-277 doi: 10.1007/s10107-002-0349-3 [19] Dueri D, Açıkmeşe B, Scharf D P, Harris M W. Customized real-time interior-point methods for onboard powered-descent guidance. Journal of Guidance, Control, and Dynamics, 2017, 40(2): 197-212 doi: 10.2514/1.G001480 [20] Xu G T, Long T, Wang Z, Cao Y. Matrix structure driven interior point method for quadrotor real-time trajectory planning. IEEE Access, 2019, 7: 90941-90953 doi: 10.1109/ACCESS.2019.2926782 [21] Mehrotra S. On the implementation of a primal-dual interior point method. SIAM Journal on Optimization, 1992, 2(4): 575-601 doi: 10.1137/0802028 [22] Duff I S. MA57-A code for the solution of sparse symmetric definite and indefinite systems. ACM Transactions on Mathematical Software, 2004, 30(2): 118-144 doi: 10.1145/992200.992202 [23] Mehrotra S. Asymptotic convergence in a generalized predictor-corrector method. Mathematical Programming, 1996, 74(1): 11-28. doi: 10.1007/BF02592143 [24] Mao Y Q, Szmuk M, Açıkmeşe B. Successive convexification of non-convex optimal control problems and its convergence properties. In: Proceedings of the 55th IEEE Conference on Decision and Control (CDC). Las Vegas, USA: IEEE, 2016. 3636−3641 期刊类型引用(5)
1. Jing SUN，Guangtong XU，Zhu WANG，Teng LONG，Jingliang SUN. Safe flight corridor constrained sequential convex programming for efficient trajectory generation of fixed-wing UAVs. Chinese Journal of Aeronautics. 2025(01): 542-555 . 
必应学术2. 王祝，张振鹏，张梦通，徐广通. 安全走廊约束的无人机轨迹序列凸优化方法. 系统仿真学报. 2025(01): 134-144 . 百度学术3. 冯子鑫，薛文超，张冉，齐洪胜. 可回收火箭大气层内动力下降的多阶段鲁棒优化制导方法. 自动化学报. 2024(03): 505-517 . 本站查看4. 王子恒，李伊陶，熊兴中. 基于分布式模型预测控制的实时可交互无人机群编队方法. 计算机应用研究. 2024(12): 3600-3606 . 百度学术5. 郑纪彬，杨志伟，杨洋，刘宏伟，张田仓，杨娟. 集群无人机载雷达阵列波束合成. 雷达科学与技术. 2023(01): 24-34+39 . 百度学术其他类型引用(3)
-
下载:
计量
- 文章访问数: 1782
- HTML全文浏览量: 737
- PDF下载量: 306
- 被引次数: 8
