PS论文(2) | Cao Dong

论文笔记

发布日期: 2021-11-12

文章字数: 3.6k

阅读时长: 13 分

阅读次数:

论文地址：Fast Trajectory Optimization for Agile Quadrotor Maneuvers with a Cable-Suspended Payload

摘要

本文提出了一种新型的绳载负载的四旋翼系统的动力学模型和快速轨迹优化算法，贡献如下：

提出一个定义新型的悬挂负载行为的公式。其被建模为连接到四旋翼与两个转动关节和一个移动关节的组合，上述关节都是被动的。与当前的工作不同，我们不需要根据缆绳张力使用混合模式来确定模型。
提出了一种基于上述系统模型的快速轨迹优化算法。我们所提出的模型使我们能够将轨迹优化问题作为一个带互补约束的数学程序(MPCC， Mathematical Program with Complementarity Constraints)。在这个框架中，系统的期望行为(例如，避障)可以很容易地表述出来。

提出的框架的具体优势

在速度方面优于最先进的技术，而不限制轨道的复杂性;
提供参数化不同任务的简单方法;
根据系统动力学和控制输入边界，保证轨迹的可行性。

实验简述：

本文所提出的方法在计算速度方面优于目前的技术水平，并且在系统动力学和控制输入饱和方面保证了轨迹的可行性，同时使用更少的优化参数。

我们在一个真实的四旋翼飞行器上实验验证了我们的方法，表明我们的方法适用于各种任务，如飞行通过理想的航路点，同时避免障碍物，或向理想的目标投掷有效载荷。

据我们所知，这是第一次在绳载负载的四旋翼飞行器上实现了基于系统动力学的三维灵活运动，如图1所示。

我们的四旋翼飞行轨迹通过多个航路点，同时通过缆索悬挂运输负载。

1 引言

A 动机

部分或完全驱动的操作器已经被用于空中操纵，可以将它们安装在四旋翼上进行操作。虽然这种解决方案适用于精细操作和与环境的交互，但它有一些缺点(例如，能源损耗、整体平台的大小、额外的惯性)，降低了无人机的灵活性。相反，通过绳载负荷的方式显著降低了系统的机械复杂性和重量，并且不需要额外的驱动，因此所需的能量最小。与驱动机械臂方法相比，绳载系统在系统动力学建模和参数化特定行为以完成预期任务方面引入了各种挑战。

缆绳张力的变化极大地改变了动力学描述，因为载荷可以通过缆绳将力传递给车辆，这取决于缆绳是否绷紧。通常引入一种混合动力学模型来处理系统的变化结构，根据拉索张力的不同有不同的数学描述(也称为模式)。这为系统提供了准确的描述，但使轨迹优化难以求解。实际上，混合优化会使用非常昂贵的计算性能，并且需要通过轨迹预先确定所需的模式序列。此外，这种公式通常会导致在为关键的安全功能(如缆绳缠绕)指定凸约束时遇到困难，并需要复杂的特定任务参数调整。此外，考虑工作空间，如在一组约束条件下考虑障碍物是困难的，并给轨迹优化带来额外的挑战。

B 相关工作

针对无人机绳载负荷的最新技术大致可分为两种方法:

跟踪负载期望轨迹的反馈控制。(对象是负载位置)
绳载负荷的四旋翼飞行器的轨迹规划。(对象是四旋翼位置)

本文主要使用的是第二种方法。

一种可能的解决方案是使运输载荷的摆动行为最小化。减少有效载荷的摆动会导致次优结果，因为它没有能量效率(四旋翼必须抵消有效载荷的摆动运动)，而且它没有利用系统动力学，阻止了高速敏捷的运动。

利用系统摆动动力学的敏捷导航轨迹规划也已进行了研究。虽然已经取得了令人印象深刻的成果，但仍有一些挑战尚未解决。已经提出了快速的轨迹计算方法，但往往不采用最优准则，不允许系统通过轨迹的行为约束或表示手动计算的系统状态硬约束。诸如航路点导航或避障等任务通常需要大量的参数，这些参数必须由专家操作人员手工调整，并显著影响系统的性能和所需的计算时间(例如，为每个障碍引入一个整数变量会成倍地增加问题的复杂性)。混合模式通常需要事先确定准确的模式序列。这迫使优化器沿着可能不是最优的单模态序列限制运动，并需要耗时的优化才能达到令人满意的结果。保证闭环系统性能的一个关键方面是计算轨迹的可行性。微分平坦度常被用来规划平滑的多项式轨迹，使基于系统输入的成本函数最小化，但不保证电机输入饱和。最后，其目前工作实验结构结果仅在仿真中显示，或在现实世界中仅限于二维运动的实验中显示。(参考文献Trajectory generation and control of a quadrotor with a cable-suspended load-a differentially-flat hybrid system；Mixed Integer Quadratic Program trajectory generation for a quadrotor with a cable-suspended payload ；Lift of a cable-suspended load by a quadrotor: A hybrid system approach； Unified Motion Control for Dynamic Quadrotor Maneuvers Demonstrated on Slung Load and Rotor Failure Tasks )

2 问题公式化

A 系统动力学的公式

大多数在绳载负载的空中操作都采用一种混合方法，根据缆索的张力建立两种模式的系统模型。在第一种模式中，缆绳完全绷紧并在四旋翼和负载之间传递力;在第二种模式中，缆绳是松弛的，导致有效载荷服从自由落体动力学。这使得轨迹优化成为一个复杂的问题，因为需要混合轨迹优化，并且系统状态约束通常是非凸的。

相反地，我们引入了一种新颖而简单绳载负载的四旋翼系统参数化方法。我们通过一个由三个被动约束关节(两个转动关节和一个移动关节)组成的系统将负载建模为附着在四旋翼悬架基座上的质量点。通过利用关节限制的棱柱关节ρ模拟了绳载负载系统的混合模式。在ρ处于极限状态时，反作用力通过关节传输回悬挂基座。(好像是把缆绳和负载当做机器臂的关节处理)我们模型中的广义坐标如表1和图2所示：

$${q_{coord}} = {\left[ {x,y,z,\phi ,\theta ,\psi ,\eta ,\mu ,\rho } \right]^T} \tag{1}$$

动力学可以被描述为一个二阶系统或使用如Robotics | SpringerLink所述的欧拉-拉格朗日方程，得到类似Cartesian Impedance Control of a UAV with a Robotic Arm - ScienceDirect的公式:

$$H(q)\ddot q + C(q,\dot q) + G(q) = B(q)u \tag{2}$$

其中$q = \left[ {{q_{coord}},{{\dot q}_{coord}}} \right]$

模型描述和整体结构

表1 绳载负载的四旋翼系统的变量

B 轨迹优化问题

我们提出的轨迹优化问题如下：

$${\text{find }}\ddot q{\text{ subjiect to}}$$ $$H(q)\ddot q + C(q,\dot q) + G(q) = B(q)u \tag{3}$$ $$f(q,u) \geqslant 0 \tag{4}$$

该轨迹被划分为N个时间节点，其中系统动力学包括一个直接转化方式。我们决定使用直接方法(PDF) Direct Trajectory Optimization Using Nonlinear Programming and Collocation ，因为它在数值上优于其他技术，如射击方法Direct Trajectory Optimization of Rigid Body Dynamical Systems through Contact 。此外，尽管它们会导致更大的优化问题，但这些问题是稀疏的，可以非常快地解决。

我们使用一阶欧拉逼近将动力学化作时间步长q(n)和q(n+ 1)之间的等式约束。这种等式约束通过最小化缺陷变量${\zeta _n}$被定义为:

$${\zeta _n} = q(n + 1) - q(n) - dt(n)\dot q(n) \tag{5}$$

这种一阶近似是可能的最简单的积分方案，因此可以得到有效的计算。此外，它得到了一个定义良好的非线性问题的近似，可以提供给求解器及其梯度。

C 将绳载负载的四旋翼运动问题视作MPCC问题

我们将这个特殊问题作为一个带互补约束的数学程序(Mathematical Program
with Complementarity Constraint,MPCC),如下：

$$\eqalign{ & \min {\text{ }}L(q,\dot q,u,t){\text{ subject to}} \cr & H(q)\ddot q + C(q,\dot q) + G(q) = B(q)u + J{(q)^T}\lambda , \cr & f(q,u) \geqslant 0, \cr & \phi (q) = ({l_0} - \rho ) \geqslant 0, \cr & \lambda \geqslant 0, \cr & \phi (q)\lambda = 0 \cr} \tag{6}$$

接下来，我们介绍MPCC的成本函数和所有的约束。

成本函数公式：

我们基于状态、输入和时间构建成本函数如下
$$L(q,u,t) = {S_f}{t_f} + \int_t {q{{(t)}^T}Qq(t)} + u{(t)^T}Ru(t)dt \tag{7}$$ 其中， ${S_f}$为终止时间系数，Q和R是状态和输入对应的对角正定代价矩阵，维度分别为${\mathbb{R}^{18 \times 18}}$和${\mathbb{R}^{4 \times 4}}$。
通过将代价描述为具有终止时间代价的状态和输入的二次函数，我们减少了优化参数的数量，并利用了优化中的解析导数。(成本函数中的q和公式中的q有矛盾？)
线性互补约束：
用一个互补约束来模拟缆绳长度的限制，即把缆绳长度当作一个关节的限制。该约束公式与Direct Trajectory Optimization of Rigid Body Dynamical Systems through Contact | （这篇文章中的公式与本文中的很相似）中提出的约束公式类似，其形式如下：
$$\eqalign{ & \phi (q) = ({l_0} - \rho ) \ge 0 \cr & \lambda \ge 0, \cr & \phi (q)\lambda = 0 \cr} $$
$\phi (q)$为${l_0}$的突破约束；
λ为表示约束力的决定变量；
$\phi (q)\lambda = 0$为实现了两者的互补约束。
输入限制约束:
我们考虑到驱动系统的物理限制，对每个电机提供的推力施加一个上限和下限:
$${T_{\min }} \leqslant u \leqslant {T_{\max }} \tag{9}$$
关节限制约束：
限制了四旋翼和负载之间的最大角度以防止负载缆索进入旋翼。
${\alpha _{\min }} \leqslant \eta ,\mu \leqslant {\alpha _{\max }} \tag{10}$
避障：
为了简化避障，我们在工作空间中将障碍物建模为球体，并使用与CHOMP: Gradient optimization techniques for efficient motion planning相似但使用经过简化的带符号距离函数。因此，绳载负载的四旋翼系统与障碍物之间的距离可以约束为:
$$d(q(t)) = \sqrt {{{(p(t) - o)}^T}(p(t) - o)} - \rho (t) - r \tag{11}$$ 其中$p(t) = {\left[ {x(t),y(t),z(t)} \right]^T}$是四旋翼的位置，$o = {[{x_o},{y_o},{z_o}]^T}$为半径为*r*的障碍物位置。描述带符号距离的方法同样适用于无数个圆柱形障碍物，其中距离函数为: $$\eqalign{ & {r_{op}}(t) = (p(t) - o) - (p(t - o) \cdot n) \cdot n \cr & d(q(t)) = \sqrt {{r_{op}}{{(t)}^T}{r_{op}}(t)} - \rho (t) - r \cr} \tag{12}$$
其中n为对称轴。这个函数可以作为一个不等式约束，因此，执行一个保守但安全的轨迹，实现无人机本体、负载、缆绳和障碍物之间的碰撞。从而就不需要任何模态序列或混合模型。

D 实现特定任务

在本节中，我们将展示如何将优化问题的一般公式用于特定的任务，如航点导航和向预期目标投掷负载。

航点导航：
航点导航任务可以描述为前三个状态变量$p(n) = {\left[ {x(n),y(n),z(n)} \right]^T}$(n为系统的给点时间点)的边界框约束。为了算法的鲁棒性，我们不使用等式约束，而是允许一个路径点存在很小的容许误差δ，不等式约束如下:
$${p_{w,i}} - \delta \leqslant p(n) \leqslant {p_{w,i}} + \delta \tag{13}$$
此外，还可以设置负载的位置，因为它是在系统状态中是完全定义的。
负载投掷:
投掷运动的目标是：释放负载，使其落向给定的目标。释放后，负载将遵循一个弹道轨迹，其最终位置可以被限制在期望的目标位置周围的一个给定的阈值内。
${p_r} = \left[ {{x_r},{y_r},{z_r}} \right]$为释放负载瞬间的负载在世界坐标系下的负载位置，${{\dot p}_r}$为释放负载瞬间的负载在世界坐标系下的负载速度。${p_t} = \left[ {{x_t},{y_t},{z_t}} \right]$为目标位置。${p_h}$为无人机处于期望高度时在弹道轨迹中的位置。需要注意${p_r},{{\dot p}_r}$可以通过$q(t)$和正向运动学雅克比矩阵$J(q(t))$的乘积得到(${{\dot p}_r} = J(q(t))\dot q(t)$). $$\eqalign{ & {t_1} = \left\{ \matrix{ {{{{\dot z}_r}} \over g}{\rm{, }}{{\dot z}_r} > 0 \hfill \cr 0{\rm{, }}{{\dot z}_r} \le 0 \hfill \cr} \right. \cr & {z_{\max }} = {z_r} + {{\dot z}_r}{t_1} - g{{t_2^2} \over 2} \cr & {t_2} = {t_1} + \sqrt {{{2({z_{\max }} - {z_t})} \over g}} \cr & {p_h} = {p_r} + {{\dot p}_r}{t_2} + \left[ {0,0, - g} \right]{{t_2^2} \over 2} \cr} \tag{14}$$ 其中，${t_1}$为载体自由落体抛物运动达到峰值的时间，${t_2}$为载体到达期望高度的时间此外，我们施加以下约束: $$0 \le \left\| {{p_h} - {p_t}} \right\| \le {d_{\max }} \tag{15}$$
${d_{\max }}$为所允许的与目标位置的最大距离。这个公式的优点是，我们不限制释放的位置，而是让优化过程找到它的最佳值。