CN116719332B - 一种基于倾转旋翼无人机位置和姿态的调控系统及方法 - Google Patents

Abstract

本发明公开了一种基于倾转旋翼无人机位置和姿态的调控系统及方法，包括以下步骤：步骤1：根据无人机的位置和姿态误差得到虚拟控制量；步骤2：以耗能最小构建代价函数，引入约束；根据代价函数、不等式约束和步骤1得到的虚拟控制量得到拉格朗日方程；步骤3：求解步骤2中的拉格朗日方程得到最优解；步骤4：根据基于倾转旋翼无人机动力学模型和步骤3得到的最优解计算得到倾转旋翼无人机实际位置和姿态；分别求解实际位置和姿态与期望位置和姿态的误差，若误差在阈值范围内则退出，若否则返回步骤1；本发明成功规避奇异点，并且分配结果迅速且稳定。

Description

一种基于倾转旋翼无人机位置和姿态的调控系统及方法

技术领域

本发明涉及电子技术领域，具体涉及一种基于倾转旋翼无人机位置和姿态的调控系统及方法。

背景技术

倾转旋翼无人机由于具有灵活的飞行能力和姿态全向性等优点而成为一个新兴的研究领域。虽然已有许多可靠性较好的控制分配算法，但倾转旋翼无人机处于两种奇异位置时，算法会无法生成稳定解而无法产生力或力矩，导致机动性损失。

倾转旋翼无人机遇到奇点的情况有两种：第一种发生在瞬时分配矩阵经历降秩时。与之对应的两种情况分别为期望力方向与机体Z框架正交令分配矩阵的秩降为4，或期望力方向位于机体Z平面令分配矩阵秩降为5。分配矩阵降秩导致分配结果不稳定，系统在至少一个自由度(DOF)上失去可控性。第二种情况为控制器得到的期望力方向对准其中一个无人机臂的轴，无人机臂无法向预期方向提供推力。

目前，采用对偶四元数构建跟踪控制器，用于倾转旋翼无人机的控制。该方法主要由四个部分组成：姿态估计、位置估计、控制器设计和系统仿真。对偶四元数作为一种扩展了复数的数学工具，可以同时描述旋转和平移运动。该控制器采用基于对偶四元数的控制器，实现了对无人机的自主控制和运动跟踪。仿真结果表明，该方法具有较高的精度和稳定性，可以更加准确地描述无人机的运动状态。利用对偶四元数构建PID跟踪控制器只能有效进行对无人机的轨迹跟踪，无法解决倾转旋翼无人机执行器奇异点问题。且涉及对偶四元数的运算，计算复杂度高可能会影响实时性能，尤其是在实际控制中需要频繁计算。

基于模型参考自适应控制的控制方法，将参考模型与实际模型的输出进行比较，并通过自适应控制算法对系统进行调节，以实现期望的控制效果。将期望的六自由度控制输入(例如，期望的姿态角度和推力)转换为六个电机输出，从而实现期望的飞行动力学响应。通过求解一个线性方程组来计算每个电机的推力指令，以实现期望的飞行动力学响应。这个线性方程组包括两个矩阵：一个是转换矩阵，用于将期望的六自由度控制输入转换为电机输出；另一个是电机模型矩阵，用于描述电机的推力输出与电机输入电压之间的关系。通过求解这个线性方程组，可以计算出每个电机的推力指令，从而实现期望的飞行动力学响应。当面对无人机奇异点时，通过关闭两个转子并将力和力矩映射到剩余四个转子上，在垂直位置时缺失两个转子会导致高度损失，此时又需要尽快切回常规分配。这种方法无法根本解决奇异点问题，只能通过对临界位置的检测尽可能实现无人机的正常运行，当计算量过大无人机会不稳定并会影响实时性能。

发明内容

本发明针对现有技术存在的问题提供一种能实现在存在奇点的情况下保持稳定轨迹跟踪能力的基于倾转旋翼无人机位置和姿态的调控系统及方法。

本发明采用的技术方案是：

一种基于倾转旋翼无人机位置和姿态的调控方法，包括以下步骤：

步骤1：根据无人机的位置和姿态误差得到虚拟控制量；

步骤2：以耗能最小构建代价函数，引入约束；根据代价函数、不等式约束和步骤1得到的虚拟控制量得到拉格朗日方程；

步骤3：求解步骤2中的拉格朗日方程得到最优解；

步骤4：根据基于倾转旋翼无人机动力学模型和步骤3得到的最优解计算得到倾转无人机实际位置和姿态；分别求解实际位置和姿态与期望位置和姿态的误差，若误差在阈值范围内则退出，若否则返回步骤1。

进一步的，所述步骤1中通过PID控制算法(比例积分微分控制算法)得到虚拟控制量f_c和τ_c；

式中：f_c为虚拟力向量，τ_c为力矩向量，为从机体坐标系到世界坐标系的旋转矩阵，ξ_e为世界坐标系下，倾转旋翼无人机在x，y，z方向上的位置误差，θ_e为世界坐标系下，无人机在横滚角、俯仰角和偏航角下的姿态误差，K_p、K_i、K_d为常数；/>为ξ_e的一阶微分，/>为θ_e的一阶微分。

进一步的，所述步骤2中的代价函数如下：

式中：J(u)为代价函数，u为输入变量，α_i为倾转机构角度，ω_j为舵机角速度，k_i和k_j均为权重系数。

进一步的，所述步骤2中约束包括无人机动力学模型带来的等式约束和倾转旋翼无人机的机械机构给舵机角速度和倾转机构角度带来的限制造成的不等式约束；在拉格朗日方程中等式约束通过拉格朗日乘子λ引入，不等式约束通过障碍函数引入；拉格朗日方程如下：

式中：v_c为虚拟控制量组成的虚拟向量矩阵，Ψ(t，∈，u)＝Au为分配矩阵A和输入变量u的乘积；∈为由多旋翼倾转旋翼无人机的位置与姿态组成的状态向量。

进一步的，所述步骤3中拉格朗日方程求解过程中通过如下所示的控制分配算法计算最优解；

对输入变量和拉格朗日乘子λ求偏导，满足偏导同时为0时对应的倾转机构角度、舵机角速度和拉格朗日乘子即为最优解集；

式中：△为关于黑塞矩阵的表达式，为黑塞矩阵，/>为输入变量u的一阶微分，/>为λ的一阶微分，h_ff为中间表达式，其为关于输入变量，拉格朗日乘子，虚拟向量等参量的复杂表达式。

进一步的，所述步骤4中倾转旋翼无人机动力学模型如下：

式中：ξ为世界坐标系下，倾转旋翼无人机在x，y，z方向上的位置，θ为世界坐标系下，无人机在横滚角、俯仰角和偏航角下的姿态，为从机体坐标系到世界坐标系下的旋转矩阵，f为基体坐标系下无人机产生的升力，τ为机体坐标系下无人机产生的力矩，I为力矩惯量，g为重力加速度；/>为ξ的一阶微分，/>为θ的一阶微分。

一种基于倾转旋翼无人机位置和姿态的调控系统，包括PID模块、控制器模块和倾转旋翼无人机模块；PID模块用于根据无人机的位置和姿态误差得到虚拟控制量；控制器模块用于通过控制分配算法求解拉格朗日方程，得到最优的倾转机构角度、舵机角速度；倾转旋翼无人机模块用于构建基于倾转旋翼无人机动力学模型，根据控制器模块计算得到的最优的倾转机构角度、舵机角速度计算得到飞机实际的位置和姿态。

本发明的有益效果是：

(1)本发明通过障碍函数和拉格朗日函数，将奇点情况下的约束纳入其中；

(2)本发明采用动态自适应控制分配算法无需在每个时刻求解，计算简单并适应无人机的快速动态，提高了倾转旋翼无人机在6自由度下的稳定性。

附图说明

图1为本发明方法流程示意图。

具体实施方式

下面结合附图和具体实施例对本发明做进一步说明。

如图1所示，一种基于倾转旋翼无人机位置和姿态的调控方法，包括以下步骤：

步骤1：根据无人机的位置和姿态误差得到虚拟控制量；

通过PID控制算法(比例积分微分控制算法)得到虚拟控制量f_c和τ_c；

式中：f_c为虚拟力向量，τ_c为力矩向量，为从机体坐标系到世界坐标系的旋转矩阵，ξ_e为世界坐标系下，倾转旋翼无人机在x，y，z方向上的位置误差，θ_e为世界坐标系下，无人机在横滚角、俯仰角和偏航角下的姿态误差，K_p、K_i、K_d为常数；/>为ξ_e的一阶微分，/>为θ_e的一阶微分。

步骤2：以耗能最小构建代价函数，引入约束；根据代价函数、不等式约束和步骤1得到的虚拟控制量得到拉格朗日方程；

代价函数如下：

式中：J(u)为代价函数，u为输入变量，α_i为倾转机构角度，ω_j为舵机角速度，k_i和k_j均为权重系数。当代价函数最小时，对应的倾转机构角度和舵机角速度为方程的最优解。

设定代价函数后，需要引入约束，在倾转旋翼无人机模型中约束包括无人机动力学模型带来的等式约束和倾转旋翼无人机的机械机构给舵机角速度和倾转机构角度带来的限制造成的不等式约束；在拉格朗日方程中等式约束通过拉格朗日乘子引入λ，不等式约束通过障碍函数引入；拉格朗日方程如下：

式中：v_c为虚拟控制量组成的虚拟向量矩阵，Ψ(t，∈，u)＝Au为分配矩阵A和输入变量u的乘积；∈为由多旋翼倾转旋翼无人机的位置与姿态组成的状态向量。v_c＝[f_c τ_c]^T表示由虚拟向量f_c和τ_c组成的虚拟向量矩阵。

步骤3：求解步骤2中的拉格朗日方程得到最优解；

A矩阵的形式如下：

机体x-y平面上机体x轴与手臂i的夹角用γ表示，β表示机体x-y平面与手臂i的夹角。转子的推力系数为c_f，c_d为转子阻力系数。s_i用于表示附在臂i上的转子的转子旋转方向

无人机机械结构对舵机角速度和倾转机构角度带来的限制如下：

-π＜α_i＜π，ω_min≤ω≤ω_max

对于上述不等式约束的情况，引入如下障碍函数，n₁、n₂、n₃、n₄、n₅和n₆表示障碍系数

对于拉格朗日方程来说，对输入变量和拉格朗日乘子λ求偏导，满足偏导同时为0时对应的倾转机构角度、舵机角速度和拉格朗日乘子即为最优解集；

式中：△为关于黑塞矩阵的表达式，为黑塞矩阵，/>为输入变量u的一阶微分，/>为λ的一阶微分，h_ff为关于输入变量，拉格朗日乘子，虚拟向量等参量的复杂表达式。

式中：f(t，∈)和g(t，∈)为描述无人机动力学模型的函数，通过控制分配算法使输入不断更新，直至求出最优解。

步骤4：根据基于倾转旋翼无人机动力学模型和步骤3得到的最优解计算得到倾转无人机实际位置和姿态；分别求解实际位置和姿态与期望位置和姿态的误差，若误差在阈值范围内则退出，若否则返回步骤1。

结合分配矩阵可以计算得出倾转旋翼无人机实际的位置和姿态，与期望位置和姿态左茶，求出误差后返回步骤l继续调节，由此形成闭环回路。

一种基于倾转旋翼无人机位置和姿态的调控系统，包括PID模块、控制器模块和倾转旋翼无人机模块；PID模块用于根据无人机的位置和姿态误差得到虚拟控制量；控制器模块用于通过控制分配算法求解拉格朗日方程，得到最优的倾转机构角度、舵机角速度；倾转旋翼无人机模块用于构建基于倾转旋翼无人机动力学模型，根据控制器模块计算得到的最优的倾转机构角度、舵机角速度计算得到飞机实际的位置和姿态。

现有的控制分配方法在保证系统的稳定性和通用性方面是有效的，但它们大多依赖于开环和静态方法，而这些方法并不能保证系统的稳定性和通用性，并且考虑到系统的实际运行情况，完全将控制分配与执行机构状态分离可能会导致大范围的不确定性或严重误差。本发明使用障碍函数和拉格朗日函数，将奇异情况下的约束纳入其中。动态自适应控制分配算法无需在每个时刻求解，实现了计算简单并适应了无人机的快速动态，提高了倾转旋翼无人机在6自由度下的稳定性。该算法是一致全局稳定的，对倾转旋翼无人机6自由度奇异点轨迹跟踪的仿真结果验证了基于倾转旋翼无人机模型的算法的可行性和性能，保证了无人机的全向性和稳定性。在存在奇点的情况下保持稳定轨迹跟踪的能力也得到了证明。

本发明在倾转旋翼无人机的俯仰角(pitch angle)接近于90°奇异点时，成功规避先前因分配矩阵无法降秩而出现的奇异点问题。通过运动分配方法，无人机的偏航角(yawangle)和滚动角(roll angle)能够正常运行且稳定在期望位置，在奇异点附近的轨迹跟踪也表现出稳定运行而不损失机动性。输入相应的无人机姿态信号，无人机可运用分配矩阵迅速分配力和力矩并用信号传递给各个转子，从而做到迅速响应完成目标姿态。同时该过程中各俯仰角(pitch angle)和转子转速均在可实现的限制范围内，使得飞行具有稳定性和可行性。

Claims (4)

1.一种基于倾转旋翼无人机位置和姿态的调控方法，其特征在于，包括以下步骤：

步骤1：根据无人机的位置和姿态误差得到虚拟控制量；

步骤2：以耗能最小构建代价函数，引入约束；根据代价函数、不等式约束和步骤1得到的虚拟控制量得到拉格朗日方程；

代价函数如下：

式中：J(u)为代价函数，u为输入变量，α_i为倾转机构角度，ω_j为舵机角速度，k_i和k_j均为权重系数；

约束包括无人机动力学模型带来的等式约束和倾转旋翼无人机的机械机构给舵机角速度和倾转机构角度带来的限制造成的不等式约束；在拉格朗日方程中等式约束通过拉格朗日乘子λ引入，不等式约束通过障碍函数引入；拉格朗日方程如下：

式中：υ_c为虚拟控制量组成的虚拟向量矩阵，Ψ(t，∈，u)＝Au为分配矩阵A和输入变量u的乘积；∈为由多旋翼倾转旋翼无人机的位置与姿态组成的状态向量；

无人机机械结构对舵机角速度和倾转机构角度带来的限制如下：

-π＜α_i＜π，ω_min≤ω≤ω_max

对于上述不等式约束的情况，引入如下障碍函数，n₁、n₂、n₃、n₄、n₅和n₆表示障碍系数

步骤3：求解步骤2中的拉格朗日方程得到最优解；拉格朗日方程求解过程中通过如下所示的控制分配算法计算最优解；

对输入变量和拉格朗日乘子λ求偏导，满足偏导同时为0时对应的倾转机构角度、舵机角速度和拉格朗日乘子即为最优解集；

式中：Δ为关于黑塞矩阵的表达式，为黑塞矩阵，/>为输入变量u的一阶微分，/>为λ的一阶微分，h_ff为中间表达式；

步骤4：根据基于倾转旋翼无人机动力学模型和步骤3得到的最优解计算得到倾转无人机实际位置和姿态；分别求解实际位置和姿态与期望位置和姿态的误差，若误差在阈值范围内则退出，若否则返回步骤1。

2.根据权利要求1所述的一种基于倾转旋翼无人机位置和姿态的调控方法，其特征在于，所述步骤1中通过PID控制算法(比例积分微分控制算法)得到虚拟控制量f_c和τ_c；

式中：f_c为虚拟力向量，τ_c为力矩向量，为从机体坐标系到世界坐标系的旋转矩阵，ξ_e为世界坐标系下，倾转旋翼无人机在x，y，z方向上的位置误差，θ_e为世界坐标系下，无人机在横滚角、俯仰角和偏航角下的姿态误差，K_p、K_i、K_d为常数；/>为ξ_e的一阶微分，/>为θ_e的一阶微分。

3.根据权利要求1所述的一种基于倾转旋翼无人机位置和姿态的调控方法，其特征在于，所述步骤4中倾转旋翼无人机动力学模型如下：

式中：ξ为世界坐标系下，倾转旋翼无人机在x，y，z方向上的位置，θ为世界坐标系下，无人机在横滚角、俯仰角和偏航角下的姿态，为从机体坐标系到世界坐标系下的旋转矩阵，f为基体坐标系下无人机产生的升力，τ为机体坐标系下无人机产生的力矩，I为力矩惯量，g为重力加速度；/>为ξ的一阶微分，/>为θ的一阶微分。

4.一种如权利要求1～3任一项所述的基于倾转旋翼无人机位置和姿态的调控方法的调控系统，其特征在于，包括PID模块、控制器模块和倾转旋翼无人机模块；PID模块用于根据无人机的位置和姿态误差得到虚拟控制量；控制器模块用于通过控制分配算法求解拉格朗日方程，得到最优的倾转机构角度、舵机角速度；倾转旋翼无人机模块用于构建基于倾转旋翼无人机动力学模型，根据控制器模块计算得到的最优的倾转机构角度、舵机角速度计算得到飞机实际的位置和姿态。