CN114625152A - 一种四旋翼无人机快速碰撞检测与恢复方法 - Google Patents

Abstract

本发明涉及一种四旋翼无人机快速碰撞检测与恢复方法，首先，使用非线性干扰观测器对碰撞产生的外力和力矩进行检测；其次，将检测到的外力和力矩与导纳控制器结合，根据碰撞外力和力矩实时的调整无人机的位置和航向角；最后，提出使用倾斜‑旋转分离姿态控制器，将无人机姿态误差划分为水平姿态误差和航向误差，为水平姿态误差分配更高的增益，从而增强了无人机水平姿态恢复能力，以上所提出方法可实现四旋翼无人机的碰撞快速检测以及碰撞后无人机的状态恢复，可用于视觉退化，狭小空间等无人机感知避障失效场景下四旋翼无人机的安全控制。

Description

一种四旋翼无人机快速碰撞检测与恢复方法

技术领域

本发明涉及一种基于干扰观测器的四旋翼无人机快速碰撞检测与状态恢复方法，可实现四旋翼无人机的碰撞快速检测以及碰撞后无人机的状态恢复，可用于视觉退化，狭小空间等无人机感知避障失效场景下四旋翼无人机的安全控制。

背景技术

四旋翼无人机具有垂直起降、机动灵活的优点，这些特点使得无人机在军事、民用领域得到了广泛的应用，四旋翼无人机应用要处理的很重要的一个问题就是避障，虽然，目前基于视觉感知和规划的研究在无人机避障方面取得了一定进展，但是随着无人机的任务和面对的环境越来越复杂，例如狭小空间陌生、非结构化环境或飞行过程中出现不可预知的干扰影响等，无人机仍然很容易受到碰撞的影响，无人机碰撞对于无人机结构损伤是不可逆的，因此碰撞的检测与处理对于无人机安全控制至关重要。

目前对于四旋翼无人机碰撞处理主要有两种思路，一种是在机身选材上进行研究，部分研究者选用柔性材料制造无人机机身，在碰撞过程中依靠机体吸收一部分碰撞能量，但是由于机身柔韧性，无人机不方便挂载重物。第二种思路是采用软件算法进行碰撞检测和处理，其中碰撞检测部分采用物理传感器的方式，如霍尔传感器，这种方式传感器依赖于安装位置，无法进行360°大范围碰撞检测，或者将碰撞产生的外力和力矩作为无人机的状态，采用滤波或者优化算法进行碰撞检测，但是基于滤波或者优化的估计算法需要一定时间收敛，无法满足检测实时性，在碰撞处理阶段，大部分研究采用了碰撞即悬停，碰撞即沿着碰撞法线方向飞行一定轨迹等策略，以上处理方式没有考虑碰撞能量的卸载，以及无人机机体垂直方向自由度即航向的柔性特性。

自然界中飞行动物具有碰撞检测与处理的能力，据观察，果蝇在高速碰撞墙壁以后会反弹回来，蜜蜂在通过尺寸小于尺寸的狭小通道的时候，会调整姿态，侧身穿越通道。受到飞行动物启发，旋翼无人机在经历碰撞过程后也可以有这些行为，从而更加智能的执行作业任务。

发明内容

本发明的技术解决问题是：针对无人机在碰撞以后安全问题，解决无人机快速碰撞检测与状态恢复问题，提供一种四旋翼无人机快速碰撞检测与恢复方法，实现四旋翼无人机的碰撞快速检测以及碰撞后无人机的状态恢复，用于视觉退化，狭小空间等无人机感知避障失效场景下四旋翼无人机的安全控制。

本发明的技术解决方案为：分为以下三个方面内容：基于非线性干扰观测器的碰撞检测方法、基于导纳控制器智能碰撞处理方法、基于倾斜-旋转分离的无人机快速姿态恢复方法。

第一步，将四旋翼无人机受到的碰撞作为外部干扰，利用非线性干扰观测器对碰撞外力和外力力矩进行估计，并进行碰撞检测，得到碰撞检测结果；

第二步，根据第一步估计的碰撞外力和外力矩以及碰撞检测结果，利用导纳控制器对碰撞后四旋翼无人机的位置和航向进行实时动态调整；

第三步，四旋翼无人机在碰撞以后往往存在很大的倾角误差，调整这些倾角误差所需力矩会超过电机饱和限制，而四旋翼无人机在姿态恢复过程中，航向角调整对于无人机姿态恢复影响很小，但是调整航向角误差所需电机能力却很大，因此在电机所能提供的可用力矩的情况下，优先调整水平姿态有利于四旋翼无人机状态的恢复，提出倾斜-旋转分离的姿态控制方法，用于四旋翼无人机水平姿态的快速恢复，将四旋翼无人机的姿态误差拆分为水平姿态误差和航向误差，在姿态控制器设计过程中为水平姿态误差设置更大的增益，从而实现水平姿态的快速镇定，避免四旋翼无人机失稳。

具体实施步骤如下：

第一步，将碰撞对无人机的影响看做的无人机受到的外部干扰，利用如下非线性干扰观测器对碰撞外力进行估计：

式子中

为估计的碰撞外力，c_f为干扰观测器增益，R为无人机本体坐标系到惯性系的旋转矩阵，e_w＝[0 0 1]^T代表重力矢量在惯性系下的方向，f_des为位置控制器输出控制量，z_f是中间变量。

利用如下非线性干扰观测器对碰撞产生外力矩进行估计：

式子中

为估计的碰撞外力矩，c_τ为干扰观测器增益，J为无人机转动惯量，w为无人机角速度，τ_des为姿态控制器输出控制量，z_τ是中间变量。

通常旋翼无人机在飞行过程中，由于传感器噪声、外部风扰等因素都会对无人机产生外部力和力矩的影响，这与碰撞产生的外力和力矩是混合在一起的，实验证明，碰撞产生的外力和力矩幅值往往比其他因素产生的外力和力矩都要大，为了将其他因素对无人机外力和力矩产生的影响排除，本方案采用如下策略进行碰撞检测：

η是碰撞检测标志位，当η为1的时候，表示检测到碰撞，当η为0的时候代表没有碰撞，κ_f和κ_τ分别为位置通道和航向通道碰撞检测阈值。其中κ_f为0.8N,κ_τ为0.1Nm。

第二步，当检测到碰撞以后，无人机根据非线性干扰观测器估计碰撞外力

和外力矩

进行位置或者航向的柔性调节，本发明构建一个导纳控制器如下：

式子中X_r＝[p_ref ψ_ref]^T为根据碰撞外力生成参考轨迹，X_d＝[p_des ψ_des]^T表示碰撞前无人机参考的路径和航向角，M表示虚拟的惯性系数矩阵，D代表了弹性系数矩阵，K代表了刚性系数矩阵。

为第二步中非线性干扰观测器估计的外力和外力矩。由上式可以看到，当有碰撞产生碰撞外力和外力矩以后，可以根据外力矩实时调整无人机期望的位置和航向角。从而避免无人机受到碰撞冲击能量的持续伤害。

第三步，当碰撞发生以后，旋翼无人机由于碰撞冲击会在俯仰、横滚和航向角等三个姿态角上产生很大的倾角，无人机姿态中俯仰和横滚角对姿态的稳定具有比较重要的作用，航向角对于无人机的稳定影响不大，无人机在大倾角下姿态调整所需的力矩往往超出了电机输出的限幅值，在电机饱和情况下，优先把有限资源(控制器力矩)用于无人机水平姿态调整将会提升无人机姿态恢复能力，本发明提出了倾斜-旋转分离的姿态控制方法，用于无人机水平姿态的快速恢复，具体为：将无人机的姿态误差分为水平姿态误差和航向误差，在姿态控制器设计的时候为水平姿态误差(俯仰-横滚角误差)设置更大的增益，从而给实现水平姿态的快速镇定，避免了无人机失稳。俯仰和横滚角的调整惯性系下无人机的z_w轴方向和位置环下无人机生成的期望z_w,des轴方向，当知道位置环生成的期望加速度a_des以后，z_w,des可以计算为：

z_w可以计算为：

z_w＝Re_w

在z_w和z_w,des之间的水平姿态误差可以用旋转轴

和旋转角α来表示，其中：

α＝cos^-1(z_w ^Tz_w,des)

因此用四元数表示的水平姿态误差为：

为了尽可能快的对齐z_w和z_w,des，q_e,xy的最后一个元素可以设置为0，因此水平姿态误差可以表示为：

对齐z_w和z_w,des以后，现在仍然存在一个航向角误差需要修正，航向角误差可以表示为：

其中q_e为期望的总姿态误差；

在分离出水平姿态误差和航向角误差以后，电机输出期望的力矩可以表示为：

其中

和

分别为水平姿态四元数误差q_e,xy和航向误差q_e,z的虚部，k_q,xy、k_q,z和k_w为控制器增益，w_e和

分别为角速度误差和角速度微分误差，它们可以通过以下式子进行计算：

式子中R_des为期望姿态角误差，

为期望姿态角误差微分。

本发明与现有技术相比的优点在于：

(1)本发明将碰撞外力和外力矩看做是一种干扰，基于干扰观测器进行碰撞外力和外力矩的检测，能够拓展碰撞检测范围，同时提升碰撞检测的速度。

(2)本发明提出使用导纳控制器用于无人机碰撞处理，能够根据碰撞外力或者外力矩实时调整无人机的位置或者航向，避免无人机的坠毁。

(3)本发明基于倾斜-旋转分离思路设计姿态控制器，使得无人机在电机饱和情况下优先输出合理的力矩用于保证水平姿态的平稳，用于无人机在失稳情况下姿态的快速恢复。

附图说明

图1为本发明一种四旋翼无人机快速碰撞检测与恢复方法的流程图；

图2为无人机在碰撞以后位置通道柔性调节示意图；

图3为无人机在碰撞以后航向通道柔性调节示意图。

具体实施方式

下面结合附图对本发明进一步详细说明。

如图1所示，本发明的具体实现步骤如下：

第一步，将碰撞对无人机的影响看做的无人机受到的外部干扰，利用如下非线性干扰观测器对碰撞外力进行估计：

式子中

为估计的碰撞外力，c_f为干扰观测器增益，R为无人机本体坐标系到惯性系的旋转矩阵，e_w＝[0 0 1]^T代表重力矢量在惯性系下的方向，v为无人机速度，f_des为位置控制器输出控制量，z_f是中间变量。

利用如下非线性干扰观测器对碰撞产生外力矩进行估计：

式子中

为估计的碰撞外力矩，c_τ为干扰观测器增益，J为无人机转动惯量，w为无人机角速度，τ_des为姿态控制器输出控制量，z_τ是中间变量。

通常旋翼无人机在飞行过程中，由于传感器噪声、外部风扰等因素都会对无人机产生外部力和力矩的影响，这与碰撞产生的外力和力矩是混合在一起的，实验证明，碰撞产生的外力和力矩幅值往往比其他因素产生的外力和力矩都要大，为了将其他因素对无人机外力和力矩产生的影响排除，本方案采用如下策略进行碰撞检测：

η是碰撞检测标志位，当η为1的时候，表示检测到碰撞，当η为0的时候代表没有碰撞，κ_f和κ_τ分别位置通道和航向通道碰撞检测阈值。

第二步，当检测到碰撞以后，无人机根据非线性干扰观测器估计碰撞外力

和外力矩

进行位置或者航向的柔性调节，本方案设计的导纳控制器如下：

式子中X_r＝[p_ref ψ_ref]^T为根据碰撞外力生成参考轨迹，X_d＝[p_des ψ_des]^T表示碰撞前无人机参考的路径和航向角，M表示虚拟的惯性系数矩阵，D代表了弹性系数矩阵，K代表了刚性系数矩阵。

为第二步中非线性干扰观测器估计的外力和外力矩。由上式可以看到，当有碰撞产生碰撞外力和外力矩以后，可以根据外力矩实时调整无人机期望的位置和航向角。从而避免无人机受到碰撞冲击能量的持续伤害。

第三步，当碰撞发生以后，旋翼无人机由于碰撞冲击会在俯仰、横滚和航向角等三个姿态角上产生很大的倾角，无人机姿态中俯仰和横滚角对姿态的稳定具有比较重要的作用，航向角对于无人机的稳定影响不大，无人机在大倾角下姿态调整所需的力矩往往超出了电机输出的限幅值，在电机饱和情况下，优先把有限资源(控制器力矩)用于无人机水平姿态调整将会提升无人机姿态恢复能力，本发明提出了倾斜-旋转分离的姿态控制方法，用于无人机水平姿态的快速恢复，具体为：将无人机的姿态误差分为水平姿态误差和航向误差，在姿态控制器设计的时候为水平姿态误差(俯仰-横滚角误差)设置更大的增益，从而给实现水平姿态的快速镇定。避免了无人机失稳。俯仰和横滚角的调整惯性系下无人机的z_w轴方向和位置环下无人机生成的期望z_w,des轴方向，当知道位置环生成的期望加速度a_des以后，z_w,des可以计算为：

z_w可以计算为：

z_w＝Re_w

在z_w和z_w,des之间的水平姿态误差可以用旋转轴

和旋转角α来表示，其中：

α＝cos^-1(z_w ^Tz_w,des)

因此用四元数表示的水平姿态误差为：

为了尽可能快的对齐z_w和z_w,des，q_e,xy的最后一个元素可以设置为0，应此水平姿态误差可以表示为：

对齐z_w和z_w,des以后，现在仍然存在一个航向角误差需要修正，航向角误差可以表示为：

其中q_e为期望的总姿态误差；

在分离出水平姿态误差和航向角误差以后，电机输出期望的力矩可以表示为：

其中

和

分别为水平姿态四元数误差q_e,xy和航向误差q_e,z的虚部，k_q,xy、k_q,z和k_w为控制器增益，w_e和

分别为角速度误差和角速度微分误差，它们可以通过以下式子进行计算：

式子中R_des为期望姿态角误差，

为期望姿态角误差微分。

如图2所示，当无人机遇到尺寸比其自身尺寸大的时候，无人机将根据非线性观测器估计的外力重新规划一条路径，无人机跟踪该路径并快速进行姿态调整。

如图3所示，当无人机遇到尺寸比自身尺寸小的时候，无人机将根据非线性观测观测估计的外力矩实时规划新的航向角，无人机调整航向角，柔性的调整姿态通过障碍物。

综合图2和3，可以看到，本发明提出的碰撞检测和处理方法能够很好的检测出碰撞，并且实时调整无人机的行为，避免了无人机的损伤。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (4)

1.一种四旋翼无人机快速碰撞检测与恢复方法，其特征在于，包括以下步骤：

第一步，将四旋翼无人机受到的碰撞作为外部干扰，利用非线性干扰观测器对碰撞外力和外力力矩进行估计，并进行碰撞检测，得到碰撞检测结果；

第二步，根据第一步估计的碰撞外力和外力矩以及碰撞检测结果，利用导纳控制器对碰撞后四旋翼无人机的位置和航向进行实时动态调整；

第三步，提出倾斜-旋转分离的姿态控制方法，用于四旋翼无人机水平姿态的快速恢复，将四旋翼无人机的姿态误差拆分为水平姿态误差和航向误差，在姿态控制器设计过程中为水平姿态误差设置更大的增益，从而实现水平姿态的快速镇定，避免四旋翼无人机失稳。

2.根据权利要求1所述的四旋翼无人机碰撞快速检测与恢复方法，其特征：所述第一步具体实现如下：

(1)利用如下非线性干扰观测器对碰撞外力进行估计：

式子中

为估计的碰撞外力，c_f为干扰观测器增益，R为无人机本体坐标系到惯性系的旋转矩阵，e_w＝[0 0 1]^T代表重力矢量在惯性系下的方向，v为无人机速度，f_des为位置控制器输出控制量，z_f是中间变量。

(2)利用如下非线性干扰观测器对碰撞产生外力矩进行估计：

式子中

为估计的碰撞外力矩，c_τ为干扰观测器增益，J为无人机转动惯量，w为无人机角速度，τ_des为姿态控制器输出控制量，z_τ是中间变量。

(3)为了将其他因素对无人机外力和力矩产生的影响排除，采用如下策略进行碰撞检测：

η是碰撞检测标志位，当η为1的时候，表示检测到碰撞，当η为0的时候代表没有碰撞，κ_f和κ_τ分别为位置通道和航向通道上的碰撞检测阈值。

3.根据权利要求1所述的四旋翼无人机快速碰撞检测与恢复方法，其特征：所述第二步中，导纳控制器如下：

式子中X_r＝[p_ref ψ_ref]^T为根据碰撞外力生成参考轨迹，X_d＝[p_des ψ_des]^T表示碰撞前无人机参考的路径和航向角，M表示虚拟的惯性系数矩阵，D代表了弹性系数矩阵，K代表刚性系数矩阵，

为非线性干扰观测器估计的外力和外力矩；当有碰撞产生碰撞外力和外力矩以后，根据外力矩实时调整无人机期望的位置和航向角，避免无人机受到碰撞冲击能量的持续伤害。

4.根据权利要求1所述的四旋翼无人机碰撞快速检测与恢复方法，其特征：所述第三步中，倾斜-旋转分离的姿态控制方法为：俯仰和横滚角的调整惯性系下无人机的z_w轴方向和位置环下无人机生成的期望z_w,des轴方向，当确定位置环生成的期望加速度a_des后，z_w,des计算为：

z_w计算为：

z_w＝Re_w

在z_w和z_w,des之间的水平姿态误差用旋转轴

和旋转角α来表示，其中：

α＝cos^-1(z_w ^Tz_w,des)

采用四元数表示的水平姿态误差为：

为了尽可能快的对齐z_w和z_w,des，q_e,xy的最后一个元素设置为0，水平姿态误差表示为：

对齐z_w和z_w,des以后，航向角误差需要修正，航向角误差表示为：

其中q_e为期望的总姿态误差；

在分离出水平姿态误差和航向角误差以后，电机输出期望的力矩表示为：

其中

和

分别为水平姿态四元数误差q_e,xy和航向误差q_e,z的虚部，k_q,xy、k_q,z和k_w为控制器增益，w_e和

分别为角速度误差和角速度微分误差，它们通过以下式子进行计算：

式子中R_des为期望姿态角误差，

为期望姿态角误差微分。

Images