Trajectory Control of Quadrotor With Cable-Suspended Load via Dynamic Feedback Linearization
-
摘要: 三维空间下的四旋翼吊挂运输系统是一种欠驱动、强耦合、多变量的非线性系统. 根据系统的动力学特点, 将系统分解为双质点系绳连接子系统和四旋翼姿态控制子系统. 选择与系统自由度维数相同的广义坐标并基于虚位移原理计算对应的广义力, 从而建立系统的拉格朗日动力学方程. 利用微分平滑特性证明了运输系统存在平凡零动态, 因此可通过动态反馈转化为线性和能控系统. 经过2次动态扩展和变量代换, 原系统扩展为总相对阶等于系统状态维度的线性能控系统. 基于赫尔维茨稳定性判据, 设计了跟踪误差指数收敛的动态反馈控制律. 该方法可作为一类非线性系统控制器设计的标准方法. 最后以三维空间的螺旋曲线及水平面内频率变化的圆周曲线为参考轨迹进行仿真, 仿真结果验证了控制系统的有效性.Abstract: A quadrotor with cable-suspended load in 3-D space is considered, which is underactuated, strongly coupling and nonlinear. According to the dynamic feature, the system is decoupled into quadrotor attitude control subsystem and double points link subsystem. The generalized coordinate with the same dimension of the system freedom is selected and the generalized force is calculated based on the principle of virtual displacement, then the Lagrange dynamic equation of the system is established. The quadrotor-load system is proved to have ordinary zero dynamics based on differentially flat property, so it can be transformed into linear and controllable system by dynamic feedback. After 2 dynamic expansions and variable substitutions, the original system is extended to a linear controllable system whose total relative orders are equal to the system state dimensions. Based on Hurwitz stability criterion, a dynamic feedback controller with exponential convergence of position error is designed. This method can be used as a standard method for a class of nonlinear systems. Finally, the spiral curve in 3-D space and the circular curve in the horizontal plane with varying frequency are taken as the reference trajectory for simulation. Simulation results demonstrate the effectiveness of the proposed method.
-
Key words:
- Quadrotor /
- transport /
- zero dynamics /
- dynamic feedback
-
四旋翼无人飞行器与单旋翼直升机类似,具有灵活的运动机能和垂直起降的特点,但机械结构更简单, 能源利用率和安全性更高, 具有广泛的应用前景[1]. 与此同时, 基于四旋翼飞行器的无人运输系统作为一种便捷的航空运输手段, 也出现在军用和民用的各个领域, 比如物资补给或灾区救援. 然而由于运输可靠性和飞行安全性方面面临的困难, 目前这些应用都还处于小范围的试用阶段. 因此, 设计稳定可靠的四旋翼运输控制系统具有重要的理论和应用价值.
四旋翼运送载荷最初采用安装运输装置的方式. 但这种装置会增加飞行器的附加惯性, 导致其姿态响应缓慢. 一种替代方法是使用系绳吊挂结构, 吊挂结构不需要考虑飞行器与载荷外形之间的匹配问题及装载容积的限制问题, 同时保留飞行器姿态操作灵活性, 适应性更好. 吊挂结构还能使飞行器与载荷之间保持一定距离, 特别适合于一些对飞行器机体造成安全威胁的环境, 如地震灾区救援或雷区探测扫描.
四旋翼吊挂运输系统具有欠驱动, 强耦合和非线性的特点, 近年来针对该类系统的控制问题开展了大量研究. 文献[2]基于拉格朗日方程设计了迭代型线性二次最优控制器. 该控制器将模型线性化为不考虑载荷影响的独立飞行器模式和考虑载荷影响的运输系统模式, 实现精确跟踪的同时能有效拟制吊绳振荡. 但这种局部线性化方法仅在平衡点附近可以较好地描述系统特性, 当系统偏离平衡点严重时难以保证控制精度. 文献[1]通过相平面内的几何分析, 设计了两个轴向上分段式加速度轨迹. 然后基于反步法设计了非线性轨迹跟踪控制器. 文献[3-4] 针对吊挂飞行系统的位置控制及负载摆动抑制问题, 基于能量法设计了非线性控制器, 并对未知载荷质量和空气阻尼进行了参数估计. 但文献[1,3]中的动力学模型和控制器均局限于二维平面. 扩展到三维空间后, 系统状态变量增加4 维, 非线性项更加复杂, 原来的设计方法很难进行扩展. 因此, 在保留系统非线性的前提下将控制系统扩展到三维空间就成为当前重点研究方向. 文献[5] 证明了系统是以载荷位置和飞行器偏航角为平滑输出的微分平滑系统, 然后设计了三维空间下的非线性几何控制器, 控制器将系统分为四旋翼姿态, 系绳方位和载荷位置内外三个控制通道, 设计外通道时假设内通道状态已经确定, 这种控制器虽然可以使跟踪误差接近全局指数收敛. 但省略了内外通道之间的非线性耦合关系, 未考虑同时变化的情况. 文献[6] 重点考察了起飞过程. 将其分解为起飞, 拉伸和上升三种离散状态.通过一系列与状态相关的路径点来生成轨迹, 然后设计了非线性控制器来跟踪这一轨迹, 但未涉及运输过程中的轨迹跟踪问题. 文献[7] 在三维空间下设计了时间上分段控制的非线性控制器. 将吊绳振荡抑制设计为快动态, 轨迹跟踪设计为慢动态. 当运输过程中产生较大振荡时, 控制系统先对吊绳振荡进行抑制, 待振荡消除后再执行轨迹跟踪. 因此该控制器仅适合于执行无时效性要求的普通运输任务.
本文研究四旋翼吊挂运输系统执行紧急任务时的轨迹控制问题. 所设计的轨迹跟踪控制器能实现三维空间下光滑曲线轨迹的跟踪, 同时允许吊绳在运输过程中发生较大角度的振荡. 本文创新点包括: 1) 推导出形式更为简单的动力学模型. 在求解模型时, 选择系统质心位置作为广义坐标, 而不是文献[4, 7]中选择的载荷位置, 这样将系统平动和转动解耦, 使获得的动力学方程形式上更简单; 2) 提出一种判断非线性系统可动态反馈线性化的简单方法. 经典的非线性控制理论中, 判断一个系统能否实现动态反馈线性化, 需要将系统方程转化为正则形式[8], 对于非线性项较复杂的多变量系统, 这个转化过程通常是比较困难的. 本文基于微分平滑性提出了一种较简单的判断方法; 3)提出一种实现多变量微分平滑系统反馈线性化的标准算法. 文献[5]中证明了四旋翼系绳运输系统是微分平滑的, 但对于如何实现微分平滑系统的反馈线性化, 相关文献并未给出方法. 对于较简单的系统尚可通过观察和尝试的方法进行, 当系统维数较多时(如本文涉及的吊挂运输系统), 反馈线性化较难实现. 本文将动态扩展算法应用于微分平滑系统, 给出了适合计算机编程实现的标准算法, 该算法可扩展应用于一类具有微分平滑性的非线性系统.
本文内容安排如下: 第1节基于广义拉格朗日方程建立系统的动力学模型. 第2节基于微分平滑和动态反馈线性化理论设计轨迹跟踪控制系统. 第3节通过仿真验证控制系统的有效性. 第4节对本文进行总结, 并对后续研究进行展望.
1. 系统模型建立
考察如图1所示的四旋翼系绳运输系统, 假设系绳系于四旋翼飞行器质心位置并且系绳不可伸缩, 因此绳上张力相对四旋翼质心的力矩为0, 四旋翼姿态控制与系绳方位控制独立. 根据此特点, 整个运输系统可以分解为[9]四旋翼姿态控制子系统
$ {\Sigma _1} $ 和双质点系绳连接子系统$ {\Sigma _2} $ .两个子系统通过四旋翼合升力
$ {u_o} $ 进行耦合. 在子系统$ {\Sigma _1} $ 中,$ {u_o} $ 的方位对应飞行器期望的俯仰角和横滚角(偏航角由用户自由设定), 通过与飞行器的实际姿态比较, 产生姿态误差信号. 四旋翼姿态控制器根据这一误差信号, 得到三个轴向的控制力矩. 四旋翼四个螺旋桨的转速可以通过控制力矩和$ {u_o} $ 的幅值求出, 其计算式见文献[10]. 因此, 只要得到$ {u_o} $ 的值, 结合四旋翼姿态控制器即可确定飞行器输入信号. 由于四旋翼姿态控制问题已在相关文献中进行研究[11], 本文不再赘述. 本文的控制目标是在子系统$ {\Sigma _2} $ 中设计合适的$ {u_o} $ , 使载荷跟踪期望轨迹.以子系统
$ {\Sigma _2} $ 质心O为原点建立运动坐标系$ Ox'y'z' $ , 如图1所示. 用$ {m_q} $ ,$ {m_l} $ 分别表示四旋翼和载荷的质量, 用$ {l_1} $ ,$ {l_2} $ 分别表示四旋翼和载荷质心到O点的距离. 当系绳张紧时, 子系统$ {\Sigma _2} $ 的自由度为5, 选择广义坐标$ q (x,y,z,\alpha ,\beta ) $ , 其中,$ {{X}} = {\left[ {\begin{array}{c} x\;\;y \;\; z \\ \end{array} } \right]^{\rm{T}}} $ $\in {{{{\bf{R}}}}^3}$ 是O点在惯性系的坐标,$ \alpha $ 是系绳在$ Ox'y' $ 平面投影与$ Ox' $ 轴正向的夹角,$ \beta $ 是系绳与$ Oz' $ 轴正向的夹角.下面基于拉格朗日广义动力学方程建立系统模型.注1. 本文建立模型时已假定系绳处于张紧状态, 在系统起飞阶段, 系绳会经历从松弛到张紧的过渡过程. 这个过程可以分解为起飞、拉伸、上升三个阶段分别进行控制, 详细说明参见文献[6].
1.1 拉格朗日函数
系统的拉格朗日函数由平动动能, 转动动能和势能组成, 当选择系统质心O为参考点时, 系统的平动动能和势能可以很容易的得到[12]
$$\begin{split} &{T_{{\rm{trans}}}} = \frac{1}{2}m{{\dot X}^{\rm{T}}} \dot X \\ &V = mgz \end{split}$$ (1) 其中,
$ {m = m_q+m_l} $ 表示系统总质量, 势能V以地面为0势能面. 为了求出系统的转动动能, 考虑先求飞行器质心的线速度, 再求系绳转动角速度$ \omega $ , 最后求转动动能. 用$ {{{X}} _q} $ 表示四旋翼质心位置, 首先写出其关于广义坐标的表达式$$ {{{X}} _q} = \left[ {\begin{array}{c} {{x_q}} \\ {{y_q}} \\ {{z_q}} \\ \end{array} } \right] = \left[ {\begin{array}{c} {x + {l_1}{S_\beta }{C_\alpha }}\\ {y + {l_1}{S_\beta }{S_\alpha }} \\ {z + {l_1}{C_\beta }} \\ \end{array} } \right] $$ (2) 其中,
${S_x} = \sin x,\;{C_x} = \cos x$ . 由于转动动能与质心O的位置无关, 不考虑式中的质心位置, 对 $ {{{X}}_q} $ 求导有$$ {{\omega}} = { \frac{{\dot {{X}}}_q}{l_1} }= \left[ {\begin{array}{c} {\dot \beta {C_\beta }{C_\alpha } - \dot \alpha {S_\beta }{S_\alpha }} \\ {\dot \beta {C_\beta }{S_\alpha } + \dot \alpha {S_\beta }{C_\alpha }} \\ { - \dot \beta {S_\beta }} \\ \end{array} } \right] $$ (3) 于是系统转动动能为
$$ {T_{{\rm{rot}}}} = \frac{1}{2}{{I}}{ {{\omega}} ^T}{{\omega}} = \frac{1}{2}{{I}}({{\dot \beta }^2} + {{\dot \alpha }^2}S_\beta ^{^2}) $$ (4) 其中, I为系绳绕质心O的转动惯量, 且
${I} = {m_q}l_1^2 +$ $ {m_l}l_2^2 = {m_q}{l_1}l $ , 联立式(1)和式(4), 得到系统的拉格朗日函数为$$ \begin{split} L =& \;{T_{{\rm{trans}}}} + {T_{{\rm{rot}}}} - V = \\& \frac{1}{2}m{ {\dot {{X}}}^{\rm{T}}} {{X}}+ \frac{1}{2}{m_q}{l_1}l({{\dot \beta }^2} + {{\dot \alpha }^2}S_\beta ^{\rm{2}}){\rm{ - }}mgz \end{split} $$ (5) 1.2 广义力
下面利用虚功原理求各广义坐标对应的广义力, 首先写出子系统
$ {\Sigma _2} $ 所受主动力做的虚功$$ {A_{{u_o}}} = {u_{ox}}\delta {x_q} + {u_{oy}}\delta {y_q} + {u_{oz}}\delta {z_q} $$ (6) 其中,
$ (\delta {x_q},\delta {y_q},\delta {z_q}) $ 表示$ {X_q} $ 笛卡尔坐标的变分, 根据式(2)写出它们关于广义坐标的表达式$$ \begin{split} &\delta {x_q} =\; \delta x + {l_1}{C_\beta }{C_\alpha } \times \delta \beta - {l_1}{S_\beta }{S_\alpha } \times \delta \alpha \\ & \delta {y_q} =\delta y + {l_1}{C_\beta }{S_\alpha } \times \delta \beta + {l_1}{S_\beta }{C_\alpha } \times \delta \alpha \\ & \delta {z_q} =\delta z - {l_1}{S_\beta } \times \delta \beta \end{split} $$ (7) 令
$ \delta \alpha = \delta x = \delta y = \delta z = 0,\delta \beta \ne 0 $ , 联立式(6)和式(7), 得$$ {Q_\beta } = \frac{{{A_{{u_o}}}}}{{\delta \beta }} = {l_1}({u_{ox}}{C_\beta }{C_\alpha } + {u_{oy}}{C_\beta }{S_\alpha } - {u_{oz}}{S_\beta }) $$ (8) 其中,
$ {Q_\beta } $ 为广义坐标$ {\beta} $ 对应的广义力. 同理, 令$ \delta \beta = $ $\delta x = \delta y = \delta z = 0,\delta \alpha \ne 0 ,$ 得$$ {Q_\alpha } = {l_1}( - {u_{ox}}{S_\beta }{S_\alpha } + {u_{oy}}{S_\beta }{C_\alpha }) $$ (9) 类似地, 可以求出坐标
$ {x,y,z} $ 对应的广义力$$ \begin{array}{c} {Q_{_x}^{} = {u_{ox}},} \;\;{Q_{_y}^{} = {u_{oy}},} \;\;{Q_{_z}^{} = {u_{oz}}} \\ \end{array} $$ (10) 注2. 求广义力时, 不需考虑系统所受的保守力, 因此子系统
$ {\Sigma _2} $ 仅有唯一的主动力$ {u_{o}} $ , 具体参见文献[12].1.3 拉格朗日方程
将式(5)和式(8)~(10)代入拉格朗日动力学方程
$$ \frac{\rm{d}}{{{\rm{d}}t}}\frac{{\partial L}}{{\partial {{\dot q}_i}}} - \frac{{\partial L}}{{\partial {q_i}}} = {Q_{{q_i}}} $$ 其中,
$ {q_i} $ 表示第i个广义坐标. 整理得$$ \left\{ {\begin{aligned} & {\ddot x = \frac{u_{ox}}m} \\ & {\ddot y = \frac{u_{oy}}m} \\ & {\ddot z =\frac {u_{oz}}m + g} \\ & {\ddot \beta = \dfrac{{{u_{ox}}{C_\beta }{C_\alpha } + {u_{oy}}{C_\beta }{S_\alpha } - {u_{oz}}{S_\beta }}}{{{m_1}l}} + {{\dot \alpha }^2}{S_\beta }{C_\beta }} \\ & {\ddot \alpha = \dfrac{{ - {u_{ox}}{S_\alpha } + {u_{oy}}{C_\alpha }}}{{{m_1}l{S_\beta }}} - \dfrac{{2\dot \alpha \dot \beta {C_\beta }}}{{{S_\beta }}}} \\[-15pt]\end{aligned} } \right. $$ (11) 式(11)即为子系统
$ {\Sigma _2} $ 的动力学微分方程, 求出$ ({{X}},\alpha ,\beta ) $ 后, 载荷轨迹为$$ {{{X}}_l} = \left[ {\begin{array}{c} {{x_l}} \\ {{y_l}} \\ {{z_l}} \\ \end{array} } \right] = \left[ {\begin{array}{c} {x - {l_2}{S_\beta }{C_\alpha }} \\ {y - {l_2}{S_\beta }{S_\alpha }} \\ {z - {l_2}{C_\beta }} \\ \end{array} } \right] $$ (12) 式(11)和式(12)分别构成系统的状态方程和输出方程, 轨迹跟踪控制系统设计的目标是找到合适的
$ {u_o} $ , 使载荷轨迹$ {{{X}}_l} $ 稳定跟踪期望轨迹$ {{{X}}_l^*} $ .2. 控制系统设计
2.1 线性化与能控性
对于式(11)和式(12)表示的非线性系统, 首先提出的问题是它是否是能控的. 下面的理论[13-15]给出了分析非线性系统能控性的一条途径.
考虑如下仿射形式的多变量非线性系统
$$ \left\{ {\begin{aligned} & {{\dot {{\xi}}} = f( {{{\xi}} }) + \sum\limits_{i = 1}^m {{g_i}( {{{\xi}}} ){u_i}} } \\ & {{\rho _i} = {h_i}( {{\xi}} ),\;\;\;i = 1, \cdots, m} \\ \end{aligned} } \right. $$ (13) 其中,
$ f( {{\xi}} ),{g_i}({{\xi}} ) $ 是光滑向量场,$ {u_i} $ ,$ {{\rho _i}} $ 分别是系统输入和输出.定义 1(向量相对阶). 对式(13)表示的多变量非线性系统, 在
$ {{{\xi}} _0} $ 处有一个向量相对阶$ ({r_1}, \cdots ,{r_m}) $ , 当且仅当在$ {{{\xi}} _0} $ 的邻域内满足对所有$k < {r_i} - 1,$ $1 \le i \le m,$ $ 1 \le j \le m $ 有$$ {L_{{ {{{g_j}}}}}}L_{ {{f}}}^k{h_i}({{\xi}} ) = 0 $$ (14) 同时
$ m \times m $ 阶矩阵$$ E = \left[ {\begin{array}{*{20}{c}} {{L_{{{{g_1}}}}}L_{{f}}^{{r_1} - 1}{h_1}({{\xi}} )}& \cdots &{{L_{{{{g_m}}}}}L_{{f}}^{{r_1} - 1}{h_1}({{\xi}} )}\\ \vdots &{{\ddots}}& \vdots \\ {{L_{{{{g_m}}}}}L_{{f}}^{{r_m} - 1}{h_m}({{\xi}} )}& \cdots &{{L_{{{{g_m}}}}}L_{{f}}^{{r_m} - 1}{h_m}({{\xi}} )} \end{array}} \right] $$ (15) 在
$ { {{{\xi}}} _0} $ 处可逆, 式中$ L( \cdot ) $ 为李导数算子.定义2(零动态与平凡零动态). 对于定义在
$ t = 0 $ 邻域内的所有$ t $ , 如果存在由一个初始状态和一个输入函数组成的对, 它们使系统的输出在$ t = 0 $ 邻域内恒为零, 这种情况下系统的动态称为零动态. 如果系统的零动态对应唯一的常量状态和常量输入, 则称系统具有平凡零动态.非线性系统的零动态是线性系统零点概念的推广. 简单来说, 通过控制输入使输出恒为0时系统的内动态即称为零动态. 它描述了系统“不可观测”的那一部分性质. 下面的引理提供了通过系统零动态考察系统能控性的方法.
引理1. 对于式(13)描述的系统, 如果它有平凡零动态, 那么它可通过动态扩展算法得到一个可正则化的扩展, 并经过动态反馈与坐标变换转换成线性和能控的系统[8].
定理1. 由式(11)和式(12)描述的系统具有平凡零动态, 因此它可通过动态扩展算法转换成一个线性和能控的系统.
证明. 由于四旋翼吊挂运输系统是以载荷位置为平滑输出的微分平滑系统[5, 16], 这意味着系统的状态和输入均可表示成平滑输出及其导数的形式, 如果能得到它们之间的解析表达式, 根据定义2, 只要令表达式中的平滑输出及其导数为0, 即可得到系统的零动态, 下面基于这一思路求系统零动态.
设系绳上张力为
$ {{T}} $ ($ {{T}} $ 为矢量, 且${{T}}\!=\! [{{T_x}}\;\;{{T_y}}\;\;{{T_z}} ]^{\rm{T}})$ , 在地面系, 对$ {m_l} $ 应用牛顿第二定律有$$ {{T}} = m_l \left[ {\begin{array}{*{20}c} {\ddot x_l } \\ {\ddot y_l } \\ {\ddot z_l - g} \\ \end{array}} \right] $$ (16) 当系绳张紧时,
$ {{T}} $ 所在方向即绳的方向, 因此方位角$ \beta $ 可由下式求得$$ \beta = \arccos \left(\frac{{{{\ddot z}_l} - g}}{{\sqrt {{{({{\ddot x}_l})}^2} + {{({{\ddot y}_l})}^2} + {{({{\ddot z}_l} - g)}^2}} }} \right) $$ (17) 式(17)为
$ \beta $ 关于${\ddot { X}_L}$ 的函数. 令${\ddot { X}_L}= 0$ , 得$\beta = 0.$ 代入输出方程(12), 并令${{ X_L}} = 0$ , 得$x = 0, y = 0, z = {l_2}.$ 再代入状态方程(11), 得${u_{ox}} = {u_{oy}} = 0, {u_{oz}} = - mg .$ 下面考察
$ \alpha $ , 根据$ {T} $ 的方向矢量得到关系$$ \alpha = \arctan \left(\frac{{{{\ddot y}_l}}}{{{{\ddot x}_l}}}\right) $$ (18) 当
$ {\ddot X_L} = 0 $ 时, 右边分式中分子分母均为0, 无法求出$ \alpha $ 的值. 联系系统的物理含义,$ \alpha $ 定义为系绳在$ x'oy' $ 平面的投影与$ Ox' $ 轴正向的夹角, 当$ {\beta} = 0 $ 时, 系绳处于竖直方向, 系绳在$ x'oy' $ 平面的投影退化到一点. 在这种情况下,$ \alpha $ 无定义. 事实上, 系统在此时由5维退化到4维, 变量$ \beta = 0,x = 0,y = 0,z = {l_2} $ 已经确定了系统的常量状态, 此状态即为系统的平凡零动态. 根据定义2和引理1可知, 系统可通过动态扩展算法转换成一个线性和能控的系统. □上面的证明过程表明, 对于微分平滑系统, 由于系统状态和输入均可表示成平滑输出及其导数的函数, 当平滑输出及其导数为0时, 如果系统状态和输入有定义, 那么它一定是一个常量. 因此系统有平凡零动态, 可以通过动态扩展算法转换成线性和能控的系统, 我们把这个结论归纳如下.
推论1. 如果一个系统是微分平滑的, 那么它可通过动态扩展算法转换成线性和能控的系统.
2.2 动态扩展算法
下面运用动态扩展算法[17-18]对系统(11)和(12)进行转换. 由于系统的非线性项主要集中在状态方程(11)的第4个和第5个子式, 为简化表达式, 进行如下变量代换:
$$ \left\{ {\begin{aligned} & {\dfrac{{ - {u_{ox}}{S_\alpha } + {u_{oy}}{C_\alpha }}}{{{m_q}l{S_\beta }}} - 2\dot \alpha \dot \beta {C_\beta }{S_\beta } = {u_1}} \\ & {\dfrac{{{u_{ox}}{C_\beta }{C_\alpha } + {u_{oy}}{C_\beta }{S_\alpha } + {u_{oz}}{S_\beta }}}{{2{m_q}l{S_\beta }{C_\beta }}} = {u_2}} \\ & {\dfrac{{{u_{ox}}{C_\beta }{C_\alpha } + {u_{oy}}{C_\beta }{S_\alpha } - {u_{oz}}{S_\beta }}}{{2{m_q}l{S_\beta }{C_\beta }}} = {u_3}} \\ \end{aligned} } \right. $$ (19) 这样, 状态方程变为
$$ \left\{ {\begin{aligned} & {\dfrac{{\ddot x}}{{{l_2}}} = ({u_2} + {u_3}){S_\beta }{C_\alpha } - {u_1}{S_\beta }{S_\alpha } - 2\dot \alpha \dot \beta {C_\beta }{S_\alpha }{\rm{ }}} \\& {\dfrac{{\ddot y}}{{{l_2}}} = ({u_2} + {u_3}){S_\beta }{S_\alpha } + {u_1}{S_\beta }{C_\alpha } + 2\dot \alpha \dot \beta {C_\beta }{C_\alpha }{\rm{ }}} \\ & {\dfrac{{\ddot z}}{{{l_2}}} = ({u_2} - {u_3}){C_\beta } +\dfrac g{l_2}{\rm{ }}} \\ & {\ddot \beta = 2{u_3}{S_\beta }{C_\beta } + {{\dot \alpha }^2}{S_\beta }{C_\beta }{\rm{ }}} \\ & {\ddot \alpha = {u_1}{\rm{ }}} \\ \end{aligned} } \right. $$ (20) 取
$$ \begin{split} {{{\xi}}} =& {\left[ {\begin{array}{c} {{\xi _1},} \; {{\xi _2},} \; {{\xi _3},} \; {{\xi _4},} \; {{\xi _5},} \; {{\xi _6},} \; {{\xi _7},} \; {{\xi _8},} \; {{\xi _9},} \; {{\xi _{10}}} \end{array} } \right]^{\rm{T}}} = \\ & {\left[ {\begin{array}{c} {x,}\;{\dot x,}\;{y,}\;{\dot y,}\;{z,}\;{\dot z,}\;{\beta ,}\;{\dot \beta ,}\;{\alpha ,}\;{\dot \alpha } \end{array}} \right]^{\rm{T}}} \end{split} $$ (21) 则状态方程和输出方程变为如下的形式
$$ {\dot {{\xi}}} = f({{{\xi}}} ) + { {{{{{g}}_1}}}}( {{{\xi}}} ){u_1} + { {{{{{g}}_2}}}}( {{{\xi}}} ){u_2} + { {{{{{g}}_3}}}}( {{{\xi}}} ){u_3} $$ (22) $$ \left\{ {\begin{aligned} & {{h_1} = {x_l} = {\xi _1} - {l_2}{S_{{\xi _7}}}{C_{{\xi _9}}}} \\ & {{h_2} = {y_l} = {\xi _3} - {l_2}{S_{{\xi _7}}}{S_{{\xi _9}}}} \\ & {{h_3} = {z_l} = {\xi _5} - {l_2}{C_{{\xi _7}}}} \end{aligned} } \right. $$ (23) 其中
$${{f}} = \left[ {\begin{array}{*{20}{c}} {{\xi _2}}\\ { - 2{l_2}{\xi _8}{\xi _{10}}{C_{{\xi _7}}}{S_{{\xi _9}}}}\\ {{\xi _4}}\\ {2{l_2}{\xi _8}{\xi _{10}}{C_{{\xi _7}}}{C_{{\xi _9}}}}\\ {{\xi _6}}\\ g\\ {{\xi _8}}\\ {\xi _{10}^2{S_{{\xi _7}}}{C_{{\xi _7}}}}\\ {{\xi _{10}}}\\ 0 \end{array}} \right],\;\;{{{g}}_1} = \left[ {\begin{array}{*{20}{c}} 0\\ { - {l_2}{S_{{\xi _7}}}{S_{{\xi _9}}}}\\ 0\\ {{l_2}{S_{{\xi _7}}}{C_{{\xi _9}}}}\\ 0\\ 0\\ 0\\ 0\\ 0\\ 1 \end{array}} \right]$$ $$ {{{g}}_2} = \left[ {\begin{array}{c} 0 \; \\ {{l_2}{S_{{\xi _7}}}{C_{{\xi _9}}}} \; \\ 0\; \\ {{l_2}{S_{{\xi _7}}}{S_{{\xi _9}}}} \\ 0 \\ {{l_2}{C_{{\xi _7}}}} \\ 0 \\ 0\; \\ 0 \; \\ 0 \; \\ \end{array} } \right],\;\;\;\;\;\;\;\;\;\;\;\;\;{ {{g}}_3} = \left[ {\begin{array}{c} 0 \\ {{l_2}{S_{{\xi _7}}}{C_{{\xi _9}}}} \\ 0 \\ {{l_2}{S_{{\xi _7}}}{S_{{\xi _9}}}} \\ 0 \\ { - {l_2}{C_{{\xi _7}}}} \\ 0 \\ {2{S_{{\xi _7}}}{C_{{\xi _7}}}} \\ 0 \\ 0 \\ \end{array} } \right] \\ $$ 下面利用动态扩展算法求系统相对阶, 算法基本步骤为:
步骤1. 从低阶到高阶依次求平滑输出
$ {h_1},{h_2},{h_3} $ 关于系统向量场$ {{f}} $ 和${{g}}_1, {{g}}_2, {{g}}_3$ 的李导数, 直到某个控制变量前的系数非0.步骤2. 判断控制输入变量前的系数矩阵是否可逆, 如果可逆, 则求解过程结束; 否则进入步骤3.
步骤3. 按照文献[5]中的迭代方式引入新的状态变量和坐标变换公式, 将原系统的向量场扩展为新的形式, 然后回到步骤1.
重复上述过程, 直到系数矩阵可逆的情况出现. 具体求解过程如下:
1)第1次求相对阶
对
$ {h_1} $ 第1次求导得$$ \begin{split} &{L_{{{{g_1}}}}}{h_1} = \;0,\;\;{L_{{ {{g_2}}}}}{h_1} = 0,\;\;{L_{{{{g_3}}}}}{h_1} = 0 \\ &{L_{{f}}}{h_1} = {\xi _2} + {l_2}{\xi _{10}}{S_{{\xi _7}}}{S_{{\xi _9}}} - {l_2}{\xi _8}{C_{{\xi _7}}}{C_{{\xi _9}}} \end{split} $$ 第2次求导得
$$ \begin{split} &{L_{ {{{g_1}}}}}{L_{ {{f}}}}{h_1} = 0,\;\;{L_{{ {{g_2}}}}}{L_{{f}}}{h_1} = {l_2}{S_{{\xi _7}}}{C_{{\xi _9}}} \\ & {L_{{ {{g_3}}}}}{L_{{{f}}}}{h_1} = - {l_2}{S_{{\xi _7}}}{C_{2{\xi _7}}}{C_{{\xi _9}}} \end{split} $$ 出现不等于0的系数, 因此
$ {h_1} $ 可能的相对阶为2.同理, 对
$ {h_2}, {h_3} $ 求相对阶得$$ \begin{split} & {L_{{ {{g_1}}}}}{h_2} = 0,\;{L_{{ {{g_2}}}}}{h_2} = 0,\;{L_{{ {{g_3}}}}}{h_2} = 0 \\ & {L_{{ {{g_1}}}}}{L_{{f}}}{h_2} = 0,\; {L_{{ {{g_2}}}}}{L_{{f}}}{h_2} = {l_2}{S_{{\xi _7}}}{S_{{\xi _9}}} \\ & {L_{ {{{g_3}}}}}{L_{{f}}}{h_2} = - {l_2}{S_{{\xi _7}}}{C_{2{\xi _7}}}{S_{{\xi _9}}} \\ & {L_{{ {{g_1}}}}}{h_3} = 0,\;{L_{{ {{g_2}}}}}{h_3} = 0,\;{L_{ {{{g_3}}}}}{h_3} = 0 \\ & {L_{{ {{g_1}}}}}{L_{{f}}}{h_3} = 0 ,\;{L_{{ {{g_2}}}}}{L_{{f}}}{h_3} = {l_2}{C_{{\xi _7}}} \\ & {L_{{ {{g_3}}}}}{L_{{f}}}{h_3} = - {l_2}{C_{{\xi _7}}}{C_{2{\xi _7}}} \end{split} $$ 即
$ {h_2}, {h_3} $ 经过2次求导, 出现控制输入, 它们可能的相对阶都是2, 矩阵E为$$ E = \left[ {\begin{array}{c} \;{0} \; \;\;\;{{l_2}{S_{{\xi _7}}}{C_{{\xi _9}}}} \;\;\;\; { - {l_2}{S_{{\xi _7}}}{C_{2{\xi _7}}}{C_{{\xi _9}}}} \\ {0} \;\;\;\; {{l_2}{S_{{\xi _7}}}{S_{{\xi _9}}}} \;\;\;\; { - {l_2}{S_{{\xi _7}}}{C_{2{\xi _7}}}{S_{{\xi _9}}}} \\ {0} \quad \;\; {{l_2}{C_{{\xi _7}}}} \quad \quad \quad { - {l_2}{C_{{\xi _7}}}{C_{2{\xi _7}}}} \;\; \end{array} } \right] $$ 观察
$ E $ 的第2列和3列是线性相关的, 因此$ Rank(E) = 1 $ .注3. 推导过程涉及求多变量函数的李导数, 手工计算难度较大, 本文利用
${\rm{MATLAB }}$ 的符号运算功能辅助设计.2)动态扩展第1次迭代
令
$ L_{{f}}^2{h_3} + E(3,2){u_2} + E(3,3){u_3} = {\eta _1} $ , 则输入变为$$ \left\{ {\begin{aligned} & {{u_2} = \dfrac{{{\eta _1} - L_{{f}}^2{h_3}}}{{E(3,2)}} - \dfrac{{E(3,3){v_3}}}{{E(3,2)}}} \\ & {{u_1} = {v_1}} \\ & {{u_3} = {v_3}} \\ \end{aligned} } \right. $$ (24) 扩充状态
$ {\dot \eta _1} = {v_2} $ , 把$ {u_2} $ 代入式(22), 系统向量场变为$$ \left\{ {\begin{aligned} &{{\tilde {{\xi}}} = {{\left[ {{\xi _1},}\; {{\xi _2},} \; {{\xi _3},} \; {{\xi _4},} \; {{\xi _5},} \; {{\xi _6},} \; {{\xi _7},} \; {{\xi _8},} \; {{\xi _9},} \; {{\xi _{10}},} \; {{\eta _1}} \right]}^{\rm{T}}}} \\ & {\tilde { f} = {{\left[ {f + {{{{g}}_2}}\frac{{\xi _{11}} - L_{{f}}^2{h_3}}{E(3,2)},} \; 0 \right]}^{\rm{T}}}} \\ & {{{{{\tilde g}}}_1} = {{\left[ {{{{{g}}}_1,}} \; 0 \right]}^{\rm{T}}} } \\ & { {{{{\tilde g}}}_2} = {{\left[ {0,} \; {0,} \; {0,} \; {0,} \; {0,} \; {0,} \; {0,} \; {0,} \; {0,} \; {0,} \; 1 \right]}^{\rm{T}}} } \\ & { {{{{\tilde g}}}_3} = {{\left[ \frac{{{ {{g}}_3} }- {{ {{g}}_2}}E(3,3)}{E(3,2)},\; 0 \right]}^{\rm{T}}} } \end{aligned} } \right. $$ (25) 3)第2次求相对阶
对
$ {h_1} $ 第1次求导得$$ {L_{ {{\tilde g}_1}}}{h_1} = 0,\;{L_{ {{\tilde g}_2}}}{h_1} = 0,\;{L_{{{\tilde g}_3}}}{h_1} = 0 $$ 第2次求导得
$$ {L_{ {{\tilde g}_1}}}{L_{\tilde { f}}}{h_1} = 0,\;{L_{ {{\tilde g}_2}}}{L_{\tilde { f}}}{h_1} = 0,\;{L_{ {{\tilde g}_3}}}{L_{\tilde { f}}}{h_1} = 0 $$ 第3次求导得
$$ {L_{{ {\tilde g}_1}}}L_{\tilde { f}}^2{h_1} = 0,\;{L_{{\tilde { g}_2}}}L_{\tilde { f}}^2{h_1} = \tan {\xi _7}{C_{{\xi _9}}},\;{L_{{ {\tilde g}_3}}}L_{\tilde { f}}^2{h_1} = 0 $$ 出现不等于0的系数, 因此
$ {h_1} $ 可能的相对阶为3.同理, 对
$ {h_2}, {h_3} $ 有$$ \begin{split} & {L_{ {{\tilde g}_1}}}{h_2} = {L_{ {{\tilde g}_2}}}{h_2} = {L_{ {{\tilde g}_3}}}{h_2} = 0 \\ & {L_{ {{\tilde g}_1}}}{L_{\tilde { f}}}{h_2} = {L_{ {{\tilde g}_2}}}{L_{\tilde { f}}}{h_2} = {L_{{{\tilde g}_3}}}{L_{\tilde { f}}}{h_2} = 0 \\ &{L_{ {{\tilde g}_1}}}L_{_{\tilde { f}}}^2{h_2} = {L_{ {{\tilde g}_3}}}{L_{\tilde { f}}}{h_2} = 0\\ &{L_{{{\tilde g}_2}}}L_{_{\tilde { f}}}^2{h_2} = \tan {\xi _7}{S_{{\xi _9}}} \\ &{L_{{{\tilde g}_1}}}{h_3} = {L_{ {{\tilde g}_2}}}{h_3} = {L_{ {{\tilde g}_3}}}{h_3} = 0 \\ &{L_{ {{\tilde g}_1}}}{L_{\tilde { f}}}{h_3} = {L_{ {{\tilde g}_2}}}{L_{\tilde { f}}}{h_3} = {L_{ {{\tilde g}_3}}}{L_{\tilde { f}}}{h_3} = 0 \\ &{L_{ {{\tilde g}_1}}}L_{_{\tilde { f}}}^2{h_3} = {L_{ {{\tilde g}_3}}}{L_{\tilde { f}}}{h_3} = 0\\ &{L_{ {{\tilde g}_2}}}L_{_{\tilde {f}}}^2{h_3} = 1 \end{split} $$ 即对
$ {h_2}, {h_3} $ 求导3次, 出现控制输入, 因此它们可能的相对阶都是3, 矩阵E为$$ E = \left[ {\begin{array}{c} {0} \;\;\; {\tan {\xi _7}{C_{{\xi _9}}}} \;\;\; 0 \\ {0}\;\; \; {\tan {\xi _7}{S_{{\xi _9}}}}\;\; \; 0\\ {0}\!\! \quad\quad \quad{1} \quad\quad \;0 \\ \end{array} } \right] $$ 显然矩阵E仍然不可逆, 且
$ Rank(E) = 1 $ .4)动态扩展第2次迭代
令
$ {v_2} = {\eta _2} $ , 则输入变为$$ \left\{ {\begin{aligned} & {{v_2} = {\eta _2}} \\& {{v_1} = {w_1}} \\& {{v_3} = {w_3}} \\ \end{aligned} } \right. $$ (26) 扩充状态为
$$ {\dot \eta _2} = {w_2} $$ 把
$ {v_2} $ 代入式(25), 系统新的状态变量和向量场为$$ \left\{ {\begin{aligned} & { {\bar \xi} = {{{\left[ {{\xi _1},} \; {{\xi _2},} \; {{\xi _3},} \; {{\xi _4},} \; {{\xi _5},} \;{{\xi _6},} \;{{\xi _7},} \;{{\xi _8},} \;{{\xi _9},} \; {{\xi _{10}},} \; {{\eta _1},} \; {{\eta _2}} \right]}^{\rm{T}}}}}\\ & { {\bar f} = \left[ {\begin{array}{*{20}c} { {\tilde f} + { g_2}{\eta _2}} \\ 0 \end{array}}\right]} \\ & {\bar { g_1} = \left[ {\begin{array}{*{20}c} { {{{{\tilde g}}}_1}} \\ 0 \end{array}}\right]} \\ & {\bar{{ g}_2} = {{\left[ {0,} \; {0,} \; {0,} \; {0,} \; {0,} \; {0,} \; {0,} \; {0,} \; {0,} \; {0,} \; {0,} \; 1 \right]}^{\rm{T}}}} \\ & {{{\bar g}_3} = \left[ {\begin{array}{*{20}c} {{{{{\tilde g}}}_3}} \\ 0 \end{array}} \right]} \end{aligned} } \right. $$ (27) 5)第3次求相对阶
再次对
$ {h_1}, {h_2}, {h_3} $ 求导(省略求导过程), 第4次求导时出现控制输入, 对应的矩阵E为$$ \left[\!\!\! {\begin{array}{c} {\dfrac{{{\xi _8}{C_{{\xi _9}}}\! -\! {\xi _{10}}{C_{{\xi _7}}}{S_{{\xi _7}}}{S_{{\xi _9}}}}}{{C_{{\xi _7}}^2}}} \;\; {\tan {\xi _7}{C_{{\xi _9}}}}\;\; {2\tan {\xi _7}{C_{{\xi _9}}}({\eta _1} \!+\! g)} \\ {\dfrac{{{\xi _8}{S_{{\xi _9}}} \!+\! {\xi _{10}}{C_{{\xi _7}}}{S_{{\xi _7}}}{C_{{\xi _9}}}}}{{C_{{\xi _7}}^2}}} \;\; {\tan {\xi _7}{S_{{\xi _9}}}} \;\; {2\tan {\xi _7}{S_{{\xi _9}}}({\eta _1} \!+\! g)} \\ \quad 0 \quad\quad\quad\quad\quad \quad\quad 1 \quad\quad\quad\quad\quad\quad 0 \\ \end{array} } \!\!\!\right] $$ 矩阵E的行列式为
$$ \det (E) = 2{\xi _{10}}({\eta _1} + g){\tan ^2}{\xi _7} $$ 当
${\xi _{10}} \ne 0,{\eta _1} \ne - g,{\xi _7} \ne {{\text{π}} }/{2}$ 时,$ \det(E)\ne0 $ , 矩阵E可逆. 因此系统非奇异点集为${ {{{\xi}} ^q}} = \{ {\xi} |{\xi _7} \ne {{\text{π}} }/{2}, {\xi _{10}} \ne 0,$ $ {\xi _{11}} \ne - g\} $ [15]. 这就是说以$ ( {\xi} ,{\eta _{\rm{1}}},{\eta _{\rm{2}}}) $ 为新状态变量, 以$ ({w_1},{w_2},{w_3}) $ 为新输入, 以$ {\bar f}, {{{{\bar g}}}_1}, {{{{\bar g}}}_2},{{{{\bar g}}}_3}$ 为系统向量场的扩展系统, 在非奇异点集上对输出$ ({x_l},{y_l},{z_l}) $ 有不变向量相对阶$ {r_1} = {r_2} = {r_3} = 4 $ , 并且相对阶满足$$ {r_1} + {r_2} + {r_3} = n = 12 $$ 2.3 轨迹跟踪控制器
对于扩展系统(27), 由于矩阵E可逆, 可解出反馈输入
$$ \left[ {\begin{array}{c} {{w_1}} \\ {{w_2}} \\ {{w_3}} \\ \end{array} } \right] = {E^{ - 1}} \left(\left[ {\begin{array}{c} {x_l^{(4)} - L_{ {\bar f}}^4{h_1}} \\ {y_l^{(4)} - L_{ {\bar f}}^4{h_2}} \\ {z_l^{(4)} - L_{ {\bar f}}^4{h_3}} \\ \end{array} } \right]\right) $$ (28) 引入跟踪误差
$$ \left\{ {\begin{aligned} & {{e_1} = {x_l}(t) - {x_l}^*(t)} \\ & {{e_2} = {y_l}(t) - y_l^*(t)} \\ & {{e_3} = {z_l}(t) - z_l^*(t)} \\ \end{aligned} } \right. $$ 其中,
$ {x_l}^*(t),{y_l}^*(t),{z_l}^*(t) $ 表示参考轨迹. 令$$ \left\{ {\begin{aligned} & {x_l^{(4)} = x{{_l^*}^{(4)}} - \sum\limits_{j = 0}^3 {{k_{1,j}}} e_1^{(i)}} \\ & {y_l^{(4)} = y{{_l^*}^{(4)}} - \sum\limits_{j = 0}^3 {{k_{2,j}}} e_2^{(i)}} \\ & {z_l^{(4)} = z{{_l^*}^{(4)}} - \sum\limits_{j = 0}^3 {{k_{3,j}}} e_3^{(i)}} \\ \end{aligned} } \right. $$ (29) 选择系数
${k_{i,j}}\;(i = 1,2,3,j = 0,1,2,3)$ 使多项式$$ {p_i}(s) = {s^4} + {k_{i,3}}{s^3} + {k_{i,2}}{s^2} + {k_{i,1}}s + {k_{i,0}} $$ 为HURITZ多项式, 则式(27)和式(28)构成的闭环系统是跟踪误差指数收敛的系统[17].
系统扩展后控制结构如图2所示, 图中载荷的参考轨迹
$ {{X}}_L^* $ 由轨迹规划器产生, 从运输系统的输出中采集${{{X}}_L}$ 及其高阶导数$ {{X}}_L^{(1)},{{X}}_L^{(2)},{{X}}_L^{(3)} $ , 代入线性反馈式(29)和式(28), 得到$ {w_1},{w_2},{w_3} $ . 其中,$ {w_2} $ 经过2个积分器, 联合$ {w_3} $ , 代入式(24)得到$ {u_2} $ . 而$ {w_1},{w_3} $ 即$ {u_1},{u_3} $ , 再经过变量代换式(19), 得到实际的控制输入$ {u_{ox}},{u_{oy}},{u_{oz}} $ . 从图中可以看出, 在控制输入$ {u_2} $ 前叠加了2个积分器, 这意味着系统的控制输入不仅与系统当前状态有关, 还与$ {u_2} $ 的一、二阶导数有关, 控制量本身是受微分方程控制的动态过程, 这就是动态反馈的特点.注4. 与文献[6]中的控制结构不同, 上图给出的控制结构是单级的. 即给出系统的实际状态和参考轨迹以后, 控制系统只需通过一次精确的数学计算即可得到控制输入. 只是为便于对这个过程的理解, 上图把控制输入的计算过程按照逻辑关系分解为几个步骤, 这是数学计算的分解而不是控制环节的分解. 而在文献[6]中, 对载荷位置和系绳方位的控制是分两级的, 在控制系绳方位时, 假定载荷位置已经完成了控制. 因此省略了它们之间的非线性耦合关系, 未考虑同时变化的情况.
3. 仿真
为验证本文所提方法的有效性, 选择下面两种典型曲线作为参考轨迹进行MATLAB仿真, 检验控制器的跟踪效果, 仿真中使用的参数如表1.
表 1 仿真中使用的模型参数Table 1 Model parameters in the simulations变量 参数 单位 mq 0.4 kg ml 0.1 kg l1 0.8 m l2 0.2 m g −9.8 m·s−2 另外, 控制系统需要不断获取载荷位置的高阶导数, 仿真中采用的算法如下:
1)每次系统产生新状态后, 将载荷最近6个轨迹点数据与相应的时间点存放于
$ 4 \times 6 $ 二维数组中, 数组的每列由位置坐标和对应的时间构成, 表示为${[ x, \; y, \; z, \; t \\ ]^{\rm{T}}}$ , 代表一个轨迹点;2)取第4行时间值和第1行
$ x $ 坐标值, 对6个点数据进行5次多项式拟合. 同理, 对$ y, z $ 坐标的轨迹进行拟合;3)对拟合曲线求1到3阶导数, 即得到当前时刻载荷的高阶导数值.
注5. 在实际应用环境下, 控制系统需要获取系统质心位置, 系绳方位角等状态变量. 由于载荷位置是系统的平滑输出, 而系统状态变量都可以用平滑输出及其导数的函数表示. 因此, 只需设计传感器采集载荷位置, 再结合上述拟合曲线求导算法, 就能得到需要的系统状态变量. 关于载荷位置的观测参见文献[19].
3.1 跟踪螺旋曲线
在灾区救援时, 载荷可能需要围绕建筑物做螺旋上升运动, 开展近距离探测或喷洒作业. 针对此种应用场景, 设计了三维空间的一条螺旋曲线作为参考轨迹考察控制系统的跟踪效果, 螺旋曲线方程为
$$ \left\{ {\begin{aligned} & {{x_l}(t) = 10\cos (t)} \\ & {{y_l}(t) = 10\sin (t)} \\ & {{z_l}(t) = t} \\ \end{aligned} } \right. $$ (30) 选择下面的
${\rm{Hurwitz }}$ 多项式$$ {p_i}(s) = {(s + 3)^4}{\rm{ }} $$ (31) 展开上式得反馈系数集为
$$ \{ {k_{i,0}},{k_{i,1}}{\rm{,}}{k_{i,2}},{k_{i,3}}\} {\rm{ = }}\{ {\rm{81}},{\rm{108}},{\rm{54}},{\rm{12}}\} $$ (32) 其中,
$ i = 1,2,3 $ . 为检验误差收敛效果, 初始位置选择$ (x,y,z,\alpha ,\beta ) = ({\rm{11, 1, 0, 0}}{\rm{.5, 0}}) $ , 即载荷初始位置在$ x, y $ 方向与参考轨迹各有1个单位的误差. 增加的系统状态$ {\eta _1} = {\eta _2} = 1 $ . 需要注意的是, 这里选择的初始状态避开了系统奇异点. 时间步长值设置为0.01 s, 仿真时间为10 s. 另外, 将本文所用方法与文献[5]中方法进行比较, 得到的跟踪轨迹和轨迹误差收敛情况如图3和 图4所示.
图 3 两种控制方法下螺旋曲线跟踪轨迹对比((a)动态反馈控制方法; (b)几何控制方法)Fig. 3 Track a spiral curve via two control methods. ((a) Dynamic feedback control; (b) Geometry control)
图 4 两种控制方法下跟踪螺旋曲线误差收敛情况对比((a)动态反馈控制方法; (b)几何控制方法)Fig. 4 Position errors convergence when tracking a spiral curve via two control methods. ((a) Dynamic feedback control; (b) Geometry control)图3中, 细实线是螺旋上升的参考轨迹, 粗实线线是实际的载荷轨迹. 图中还给出了飞行器, 载荷和系绳在部分中间时刻的相对位置. 从图中可以看出, 载荷在初始误差下能逐渐趋近并稳定跟踪参考轨迹. 误差的收敛曲线如图4所示, 采用本文动态反馈方法大约经过4 s, 载荷在
$ x, y $ 轴方向的轨迹误差收敛到原点附近, 见图4(a). 文献[5]中几何控制方法误差收敛到原点的时间大约是6 s, 见图4(b). 从对比情况看, 在同样初始误差的情况下, 本文设计的控制器误差收敛更快. 它的代价是在跟踪过程中要不断采集控制变量本身的值和其导数值, 控制变量不仅依赖系统此刻的状态, 还依赖它的历史数据.另外, 从圆周运动规律可知, 要牵引载荷做圆周运动, 提供向心力的飞行器必须位于圆心方向, 因此图3中的飞行器轨迹位于载荷轨迹内侧. 图5和图6分别是轨迹跟踪曲线在
$ xoy $ 平面和$ xoz $ 平面的投影, 它们更加清晰地反映了此特点. 螺旋曲线投影到$ xoy $ 平面变成圆周曲线(见图5), 投影到$ xoz $ 平面变成正弦曲线(见图6). 在这两种投影方式下, 飞行器在初始时刻都位于曲线外侧, 跟踪过程开始以后, 飞行器迅速从外侧移动到内侧(圆心方向), 从而为载荷的曲线运动提供向心力. 控制输入曲线见图7, 跟踪过程中最大的控制力出现在最初的2 s内, 大约为20 N. 这对应飞行器从外侧移动到内侧的过程.跟踪过程中系绳的振荡曲线如图8所示, 图中给出了系绳与竖直方向夹角
$ \beta $ 及其角速度$ \beta^{(1)} $ 的变化曲线. 经过初始阶段的调整之后,$ \beta $ 角稳定在$0.7\;{\rm{rad}}$ (约40度)附近, 其角速度稳定在0附近. 此时系绳扫过的曲面近似于以纵轴为中心夹角为40度的圆锥曲面. 系绳在如此大角度运动的情况下, 采用局部线性化逼近的方法是不适合的, 而本文设计的控制器在此情况下仍保持了良好的跟踪效果.
图 8 螺旋曲线跟踪过程中系绳振荡角曲线Fig. 8 Curve of swing angel on the cable when tracking a spiral curve3.2 跟踪频率变化圆周曲线
由于本文设计的控制器允许吊绳发生较大角度的振荡, 因此当参考轨迹的曲线特征发生变化时, 能较快地实现跟踪. 文献[7]中基于分段控制方法设计的控制器, 当参考轨迹发生变化时, 控制器首先抑制吊绳上产生的振荡, 待振荡消除后再执行轨迹跟踪, 因此其轨迹收敛速度较慢. 为了说明这一点, 选择频率变化的圆周曲线作为参考轨迹对两种控制方法的跟踪效果进行验证. 曲线定义为
$$ \left\{ {\begin{aligned} & {{x_l}(t) = 5\cos \left(\dfrac{{2{\text{π}} }}{{T}}t\right)} \\ & {{y_l}(t) = 5\sin \left(\dfrac{{2{\text{π}} }}{{T}}t\right)} \\ & {{z_l}(t) = 0} \\ \end{aligned} } \right. $$ (33) 其中,T是圆周运动的周期, 仿真中T从
$ 2\pi $ 切换到$ \pi $ , 对应的角频率从$1\;{\rm{rad/s}}$ 到$2\;{\rm{rad/s}}$ . 载荷的初始状态为$ (x,y,z,\alpha ,\beta ) = ({\rm{5, 1, 0, 1, 0}}) $ , 其他仿真参数不变, 轨迹跟踪情况如图9所示.
图 9 两种控制方法跟踪圆周曲线轨迹对比(a)动态反馈控制方法; (b)分段控制方法Fig. 9 Track a circle via two control methods. (a) Dynamic feedback control; (b) Two-time-scale control图9(a)是本文动态反馈控制方法得到的跟踪轨迹, 图9(b)是文献[7]中分段控制方法得到的跟踪轨迹. 其中细实线是参考轨迹, 粗实线是载荷实际轨迹. 两图中载荷轨迹逐渐趋近并跟踪参考轨迹, 都能实现轨迹跟踪的效果. 但分段控制方法产生的轨迹重合点滞后一些, 这说明其实现轨迹跟踪需要的过程较长. 这一点也可以从两种控制方法得到的误差收敛曲线看出来. 图10(a), 图10(b)分别是两种控制方法轨迹误差收敛曲线. 从图中可以看出, 随着参考轨迹频率的改变, 轨迹误差经过了两次收敛过程, 本文动态反馈控制方法误差收敛时间大约在3 s 左右, 而分段控制方法误差收敛时间则接近5 s, 这说明本文设计的控制器在参考轨迹特征发生变化时能更快地实现轨迹跟踪. 这个特点在吊挂运输系统执行紧急任务时是很有必要的.
图 10 两种控制方法跟踪圆周曲线误差收敛情况对比((a)动态反馈方法; (b)分段控制方法)Fig. 10 Position errors convergence when tracking a circle curve via two control methods. ((a) Dynamic feedback control; (b) Two-time-scale control)图11 是跟踪过程中飞行器轨迹的变化曲线, 从图中可以看出, 飞行器
$ x,y $ 坐标是正余弦曲线. 参考轨迹频率切换后, 正余弦曲线的幅值减小, 频率增加, 同时z坐标也减小. 这说明随着参考轨迹频率的增大,飞行器的频率同步增加, 而飞行器轨道半径和竖直方向的高度同步减小. 这是因为随着载荷频率增加, 运转角速度增大, 需要的向心力更大. 这个更大的向心力通过增加绳与竖直方向的夹角来提供. 图12中$ \beta $ 角变化曲线也反映出这一点. 频率切换后, 绳与竖直方向夹角从0.5 rad 增加到1 rad. 在载荷$ z $ 坐标不变的情况下,$ \beta $ 角增加就意味着飞行器高度降低.4. 结论与展望
本文研究了四旋翼吊挂运输系统在三维空间下的轨迹跟踪问题. 得到的结论有: 1)运输系统可以分解为两个相互耦合的子系统, 基于广义拉格朗日方法可以获得系统精确的动力学模型; 2)运输系统具有平凡零动态, 因此是可线性化和能控的; 3)采用标准的动态扩展算法可以将原系统等价转化为总相对阶与系统维数相等的线性能控系统; 4)仿真结果表明所设计的控制器误差收敛时间短, 对参考轨迹的频率变化具有自适应性, 能较好地实现系绳大角度振荡情况下载荷轨迹的跟踪.
对于一个非线性多变量系统, 需要满足什么条件以及采用什么方法来实现精确反馈线性化, 经典非线性控制理论中针对这些问题给出的解决方法比较复杂, 在实际应用中较难实现. 本文结合微分平滑和动态反馈线性化理论提出了一套简单可行的判断和实现方法. 该方法适合于一类具有微分平滑性的多变量非线性系统. 后续研究将会考虑将此方法扩展应用于多四旋翼协同运输问题, 这是解决单四旋翼有效载荷不足的有效途径.
-
图 3 两种控制方法下螺旋曲线跟踪轨迹对比((a)动态反馈控制方法; (b)几何控制方法)
Fig. 3 Track a spiral curve via two control methods. ((a) Dynamic feedback control; (b) Geometry control)
图 4 两种控制方法下跟踪螺旋曲线误差收敛情况对比((a)动态反馈控制方法; (b)几何控制方法)
Fig. 4 Position errors convergence when tracking a spiral curve via two control methods. ((a) Dynamic feedback control; (b) Geometry control)
图 8 螺旋曲线跟踪过程中系绳振荡角曲线
Fig. 8 Curve of swing angel on the cable when tracking a spiral curve
图 9 两种控制方法跟踪圆周曲线轨迹对比(a)动态反馈控制方法; (b)分段控制方法
Fig. 9 Track a circle via two control methods. (a) Dynamic feedback control; (b) Two-time-scale control
图 10 两种控制方法跟踪圆周曲线误差收敛情况对比((a)动态反馈方法; (b)分段控制方法)
Fig. 10 Position errors convergence when tracking a circle curve via two control methods. ((a) Dynamic feedback control; (b) Two-time-scale control)
表 1 仿真中使用的模型参数
Table 1 Model parameters in the simulations
变量 参数 单位 mq 0.4 kg ml 0.1 kg l1 0.8 m l2 0.2 m g −9.8 m·s−2 
下载: 导出CSV
-
[1] 梁潇, 方勇纯, 孙宁. 平面四旋翼无人飞行器运送系统的轨迹规划与跟踪控制器设计. 控制理论与应用, 2015, 32(11): 1430−1438Liang Xiao, Fang Yong-Chun, Sun Ning. Trajectory planning and tracking controller design for a planar quadrotor unmanned aerial vehicle transportation system. Control Theory and Applications, 2015, 32(11): 1430−1438 [2] Alothman Y, Gu D B. Quadrotor transporting cablesuspended load using iterative Linear Quadratic regulator (iLQR) optimal control. In: Proceedings of the 8th IEEE Conference on Computer Science and Electronic Engineering. Colchester, UK: IEEE, 2017. 168−173 [3] Qian L H, Liu H H T. Dynamics and control of a quadrotor with a cable suspended payload. In: Proceedings of the 30th IEEE Canadian Conference on Electrical and Computer Engineering. Windsor, Canada: IEEE, 2017. 1−4 [4] 王诗章, 鲜斌, 杨森. 无人机吊挂飞行系统的减摆控制设计. 自动化学报, 2018, 44(10): 45−54Wang Shi-Zhang, Xian Bin, Yang Sen. Anti-swing controller design for an unmanned aerial vehicle with a slung-load. Acta Automatica Sinica, 2018, 44(10): 45−54 [5] Cruz P J, Oishi M, Fierro R. Lift of a cable-suspended load by a quadrotor: A hybrid system approach. In: Proceedings of the 2015 IEEE American Control Conference. Chicago, IL, USA: IEEE, 2015. 1887−1892 [6] Cruz P J, Fierro R. Cable-suspended load lifting by a quadrotor UAV: Hybrid model, trajectory generation, and control. Autonomous Robots, 2017, 41(8): 1629−1643 doi: 10.1007/s10514-017-9632-2 [7] de Angelis E L, Giulietti F, Pipeleers G. Two-time-scale control of a multirotor aircraft for suspended load transportation. Aerospace Science and Technology, 2019, 84(2019): 193−203 [8] Isidori A, Wang B, Zhuang S X. Nonlinear Control System I. Beijing: Electronics Industry Press, 2012. [9] 梁晓, 胡欲立. 基于微分平滑的四旋翼运输系统轨迹跟踪控制. 控制理论与应用, 2019, 36(4): 525−532Liang Xiao, Hu Yu-Li. Trajectory control of a quadrotor with a cable-suspended load based on differential Flatness. Control Theory and Applications, 2019, 36(4): 525−532 [10] Lee T, Leok M, McClamroch N H. Geometric tracking control of a quadrotor UAV on SE(3). In: Proceedings of the 49th IEEE Conference on Decision and Control (CDC). Atlanta, GA, USA: IEEE, 2010. 5420−5425 [11] 王宁, 王永. 基于模糊不确定观测器的四旋翼飞行器自适应动态面轨 迹跟踪控制. 自动化学报, 2018, 44(4): 685−695Wang Ning, Wang Yong. Fuzzy uncertainty observer based adaptive dynamic surface control for trajectory tracking of a quadrotor. Acta Automatica Sinica, 2018, 44(4): 685−695 [12] 陈乐生, 王以伦. 多刚体动力学基础. 哈尔滨: 哈尔滨工程大学出版社, 1995.Chen Le-Sheng, Wang Yi-Lun. Multi-body System Dynamics. Harbin: Harbin Engineering University Press, 1995. [13] 赵杰梅, 胡忠辉. 基于动态反馈的AUV水平面路径跟踪控制. 浙江大学学报(工学版), 2018, 52(18): 1467−1481Zhao Jie-Mei, Hu Zhong-Hui. Path following control of AUV in horizontal plane based on dynamic feedback control. Journal of Zhejiang University (Engineering Science), 2018, 52(18): 1467−1481 [14] 苏善伟, 朱波, 向锦武, 林岩. 非线性非最小相位系统的控制研究综 述. 自动化学报, 2015, 41(1): 9−21Su Shan-Wei, Zhu Bo, Xiang Jin-Wu, Lin Yan. A survey on the control of nonlinear non-minimum phase systems. Acta Automatica Sinica, 2015, 41(1): 9−21 [15] 易国, 毛建旭, 王耀南, 郭斯羽, 缪志强. 非完整移动机器人目标环绕动态反馈 线性化控制. 控制理论与应用, 2017, 34(7): 895−902 doi: 10.7641/CTA.2017.60962Yi Guo, Mao Jian-Xu, Wang Yao-Nan, Guo Si-Yu, Miao Zhi-Qiang. Circumnavigation of a target with nonholonomic mobile robots via dynamic feedback linearization. Control Theory and Applications, 2017, 34(7): 895−902 doi: 10.7641/CTA.2017.60962 [16] Fliess M. Flatness and defect of non-linear systems: Introductory theory and examples. International Journal of Control, 1995, 61(6): 1327−1361 doi: 10.1080/00207179508921959 [17] Taniguchi T, Eciolaza L, Sugeno M. Quadrotor control using dynamic feedback linearization based on piecewise bilinear models. In: Proceedings of the 2014 IEEE Symposium on Computational Intelligence in Control and Automation (CICA). Orlando, USA: IEEE, 2014. 1−7 [18] Choi I H, Bang H C. Quadrotor-tracking controller design using adaptive dynamic feedback-linearization method. Proceedings of the Institution of Mechanical Engineers, Part G: Journal of Aerospace Engineering, 2013, 228(12): 2329−2342 [19] Rego B S, Raffo G V. Suspended load path tracking control based on zonotopic state estimation using a tilt-rotor UAV. In: Proceedings of the 19th IEEE International Conference on Intelligent Transportation Systems (ITSC). Rio de Janeiro, Brazil: IEEE, 2016. 1445−1451 期刊类型引用(13)
1. 马伟杰，郭玉英. 基于视觉伺服的四旋翼吊挂抗摆控制方法. 兵器装备工程学报. 2024(04): 268-277 . 
百度学术2. 莫兰，王延凯，魏铭宏，陈提. 无人机四绳吊挂运输系统动力学与控制. 振动工程学报. 2024(10): 1747-1757 . 百度学术3. 陈谋，刘伟，张鹏. 性能约束下的四旋翼无人机协同吊挂系统分布式避碰跟踪控制. 自动化学报. 2024(12): 2392-2406 . 本站查看4. 林旭梅，陈一戈，苗芳荣，邵怡文. 基于给定收敛律的四旋翼高阶滑模控制器设计. 计算机仿真. 2023(01): 38-42+187 . 百度学术5. 杨小强，郭玉英. 基于BPNN参数整定的四旋翼吊挂自抗扰飞行控制. 飞行力学. 2023(03): 47-53+60 . 百度学术6. 范云生，陈欣宇，赵永生，宋保健. 基于扩张状态观测器的四旋翼吊挂飞行系统非线性控制. 自动化学报. 2023(08): 1758-1770 . 本站查看7. 刘肩山，唐毅，谢志明. 基于补偿函数观测器的无人机吊挂飞行控制. 现代信息科技. 2023(16): 62-65+70 . 百度学术8. 余志凯，黄子豪，傅瑾瑜，蒋宪鑫，辛颖. 基于嵌套饱和的四旋翼无人机吊挂负载控制器设计. 飞控与探测. 2023(03): 37-43 . 百度学术9. 郗厚印，张栋，杨云霄，周涛. 四旋翼吊挂变质量负载建模与控制方案设计. 计算机仿真. 2022(01): 60-65+101 . 百度学术10. 赵永生，陈欣宇，范云生. 基于干扰观测器的四旋翼吊挂系统非线性控制. 计算机仿真. 2022(10): 18-25 . 百度学术11. 焦海林，郭玉英，朱正为. 基于加速度补偿的无人机吊挂飞行抗摆控制. 计算机应用. 2021(02): 604-610 . 百度学术12. 石智梁，林旭梅，刘帅. 基于超扭曲算法的鲁棒自适应四旋翼控制器. 电光与控制. 2021(06): 64-67 . 百度学术13. 李佳琪，刘晨，黄明，夏天，丁佶辉. 四旋翼无人机位置与姿态控制研究发展综述. 南方农机. 2021(12): 25-27 . 百度学术其他类型引用(18)
-
下载:
计量
- 文章访问数: 1734
- HTML全文浏览量: 152
- PDF下载量: 230
- 被引次数: 31
