CN113741491A - 一种基于故障诊断结果的x型四旋翼无人机控制方法 - Google Patents

Abstract

本发明公开了一种基于故障诊断结果的X型四旋翼无人机控制方法，其步骤包括：步骤一，建立X型四旋翼无人机的运动学模型；步骤二，进行位姿PID控制器设计；步骤三，建立执行机构故障模型；步骤四，进行PID控制器参数智能调节。本控制方法通过将PID控制器与强化学习调参技术相结合，在四旋翼无人机出现执行机构故障的情况下，基于强化学习，根据四旋翼无人机自身状态和故障诊断结果，对姿态控制和位置控制的PID控制参数自整定，实现了四旋翼无人机位姿系统的稳定控制，保证了四旋翼无人机位姿系统的可靠性和安全性，实现了位姿的到达与稳定，提高了其自主飞行的安全性。

Description

一种基于故障诊断结果的X型四旋翼无人机控制方法

技术领域

本发明涉及一种基于故障诊断结果的X型四旋翼无人机控制方法，属于无人机智能控制技术领域。

背景技术

随着人工智能技术的飞速发展，传统控制框架与人工智能相结合的技术在航空航天领域越来越受到重视。四旋翼无人机凭借其结构特点，可以通过更简单的控制方式飞行。但由于其四输入、六自由度的特性，系统具有非线性、欠驱动、强耦合等特点，是一种典型的不确定非线性系统，同时在控制系统设计时必须满足尽可能减少人员的干预，加强其抗干扰能力。

对于四旋翼无人机的位姿控制，常见的有PID控制方法、滑膜控制方法、反步控制方法、神经网络控制方法和模糊逻辑控制方法。这些方法能够实现位姿的稳定，但同时也存在以下局限性：1)在自主飞行的过程中，四旋翼无人机遭受系统崩溃等故障造成的打击是致命的，需加入故障容错控制系统来保证系统能够自发处理突发故障，但现有的控制方法中，控制器中控制增益的人工整定较为繁杂，阻碍了控制性能的提升，无法满足四旋翼无人系统自发处理突发故障的要求，自主飞行的安全性得不到保障；2)对于安全性要求很高的航空航天控制领域，现有的控制方法难以对系统预期功能进行精确的描述，并且缺乏对系统鲁棒性的保证。

发明内容

针对上述现有技术存在的问题，本发明提供一种基于故障诊断结果的X型四旋翼无人机控制方法，该控制方法基于四旋翼无人机的故障诊断信息，将传统PID控制器与强化学习调参技术结合，保证四旋翼无人机位姿系统的可靠性和安全性，实现位姿的到达与稳定，提高其自主飞行的安全性，为系统鲁棒性提供保证。

为了实现上述目的，本发明提供一种基于故障诊断结果的X型四旋翼无人机控制方法，包括如下步骤:

步骤一，建立X型四旋翼无人机的运动学模型,其具体形式如下：

其中，

为滚转角，θ为俯仰角，ψ为偏航角，

分别为三轴角加速度，

分别为三轴角速度，

分别为三轴线加速度，

分别为三轴线速度，I_x,I_y,I_z为机身在x,y,z三个方向上的转动惯量，J_r为转动惯量，K₁,…,K₆为空气阻力系数，l为电机到质心的臂长，m为机身重量，g为重力加速度；U₁,…,U₄,Ω存在如下关系：

其中，U₁为总升力，U₂为滚转力矩，U₃为俯仰力矩，U₄为偏航力矩，b为升力系数，d为阻力系数，Ω₁,…,Ω₄分别为1至4号电机的转速；

步骤二，进行位姿PID控制器设计：

1)姿态控制回路为：

其中，k1...k9为姿态控制器参数，俯仰角误差

滚转角误差e_θ＝θ_c-θ，偏航角误差e_ψ＝ψ_c-ψ，

为目标姿态角，

为当前姿态角，

分别为三轴目标角加速度，

分别为俯仰角误差速度、滚转角误差速度、偏航角误差速度；对应电机的转速表示如下：

其中，C_T为升力系数，C_Q为阻力系数；

2)位置控制回路为：

其中，k1...k9为位置控制器参数，x轴方向位置误差e_x＝x_c-x，y轴方向位置误差e_y＝y_c-y，z轴方向位置误差e_z＝z_c-z，[x_c,y_c,z_c]^T为目标位置，[x,y,z]^T为当前位置，

分别为x轴、y轴、z轴方向误差速度，

为x轴、y轴、z轴方向目标加速度；设ψ_c已知，可得：

步骤三，建立执行机构故障模型：

其中，控制输入u＝[U₁,U₂,U₃,U₄]，u^F为受电机故障影响的真实的控制器输入，

为不确定的故障；

步骤四，进行PID控制器参数智能调节：

基于步骤三中的故障诊断信息，采用强化学习DDPG算法，通过搭建动作神经网络和评价神经网络，输入四旋翼无人机当前四旋翼的位置、速度、姿态角和角速度的状态，输出最优的控制器参数，实现PID控制器参数的智能调节，具体的DDPG算法过程如下：

1)随机初始化当前评价网络Q(S,A|θ^Q)和当前动作网络μ(S|θ^μ)，以及评价网络参数θ^Q和动作网络参数θ^μ，其中S为状态，A为动作；

2)初始化目标评价网络Q′和目标动作网络μ′，并将网络参数赋值为θ^Q′←θ^Q,θ^μ′←θ^μ；

3)初始化经验回放池，以用于存放过往数据和提高数据利用率并降低数据相关性；

4)for episode＝1 to M进行迭代，其中，M为最大训练回合数：

①为动作选择初始化一个随机过程N；

②获得初始状态S；

③在当前动作网络基于状态S得到动作A＝μ(S|θ^μ)+N；

④执行动作A，得到新状态S′、奖励R和终止状态的标志end；

⑤将{S,A,S′,R,end}五元组存入经验回放池；

⑥当S＝S′，从经验回放池中采样128个样本{S_j,A_j,S′_j,R_j,end_j},j＝1,2,...,128，计算当前目标Q值y_j：

⑦使用均方损失误差

通过神经网络的梯度反向传播来更新当前评价网络的网路参数θ^Q；使用

更新当前动作网络的网络参数θ^μ；

⑧每隔100个episode，更新目标评价网络和目标动作网络的网络参数：

⑨若S′为终止状态，则当前轮迭代完毕，否则跳转步骤③，进行下一轮迭代。

进一步地，步骤四中的奖励，分为稀疏奖励和reward shaping奖励两种；其中，稀疏奖励为：在一个迭代回合中，当四旋翼均达到目标点时给予智能体奖励，其余时刻奖励为0，直至回合结束，每当四旋翼到达目标点获得奖励R＝1，如果四旋翼越界，则给予惩罚R＝-1，并结束回合；reward shaping奖励为：在一个迭代回合中，四旋翼每一步有一个基础奖励，并将四旋翼当前位置与目标位置的距离作为惩罚项，直至回合结束，每多一步给予奖励R＝10，再减去四旋翼距目标点的距离作为惩罚，如果四旋翼越界，则给予惩罚R＝-100，并结束回合。

本控制方法通过将PID控制器与强化学习调参技术相结合，在四旋翼无人机出现执行机构故障的情况下，基于强化学习，根据四旋翼无人机自身状态和故障诊断结果，对姿态控制和位置控制的PID控制参数自整定，实现了四旋翼无人机位姿系统的稳定控制，保证了四旋翼无人机位姿系统的可靠性和安全性，实现了位姿的到达与稳定，提高了其自主飞行的安全性，对四旋翼无人机系统鲁棒性提供了保证。

附图说明

图1是本发明故障下四旋翼无人机俯仰角的响应图；

图2是本发明故障下四旋翼无人机滚转角的响应图；

图3是本发明故障下四旋翼无人机偏航角的响应图；

图4是本发明故障下四旋翼无人机x轴距离的响应图；

图5是本发明故障下四旋翼无人机y轴距离的响应图；

图6是本发明故障下四旋翼无人机z轴距离的响应图。

具体实施方式

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

一种基于故障诊断结果的X型四旋翼无人机控制方法，包括如下步骤:

步骤一，建立X型四旋翼无人机的运动学模型,其具体形式如下：

其中，

为滚转角，θ为俯仰角，ψ为偏航角，

分别为三轴角加速度，

分别为三轴角速度，

分别为三轴线加速度，

分别为三轴线速度，I_x,I_y,I_z为机身在x,y,z三个方向上的转动惯量，J_r为转动惯量，K₁,…,K₆为空气阻力系数，l为电机到质心的臂长，m为机身重量，g为重力加速度；U₁,…,U₄,Ω存在如下关系：

其中，U₁为总升力，U₂为滚转力矩，U₃为俯仰力矩，U₄为偏航力矩，b为升力系数，d为阻力系数，Ω₁,…,Ω₄分别为1至4号电机的转速；

步骤二，进行位姿PID控制器设计：

1)姿态控制回路为：

其中，k1...k9为姿态控制器参数，俯仰角误差

滚转角误差e_θ＝θ_c-θ，偏航角误差e_ψ＝ψ_c-ψ，

为目标姿态角，

为当前姿态角，

分别为三轴目标角加速度，

分别为俯仰角误差速度、滚转角误差速度、偏航角误差速度；对应电机的转速表示如下：

其中，C_T为升力系数，C_Q为阻力系数；

2)位置控制回路为：

其中，k1...k9为位置控制器参数，x轴方向位置误差e_x＝x_c-x，y轴方向位置误差e_y＝y_c-y，z轴方向位置误差e_z＝z_c-z，[x_c,y_c,z_c]^T为目标位置，[x,y,z]^T为当前位置，

分别为x轴、y轴、z轴方向误差速度，

为x轴、y轴、z轴方向目标加速度；设ψ_c已知，可得：

步骤三，建立执行机构故障模型：

其中，控制输入u＝[U₁,U₂,U₃,U₄]，u^F为受电机故障影响的真实的控制器输入，

为不确定的故障；

步骤四，进行PID控制器参数智能调节：

基于步骤三中的故障诊断信息，采用强化学习DDPG算法，通过搭建动作神经网络和评价神经网络，输入四旋翼无人机当前四旋翼的位置、速度、姿态角和角速度的状态，输出最优的控制器参数，实现PID控制器参数的智能调节，具体的DDPG算法过程如下：

1)随机初始化当前评价网络Q(S,A|θ^Q)和当前动作网络μ(S|θ^μ)，以及评价网络参数θ^Q和动作网络参数θ^μ，其中S为状态，A为动作；

2)初始化目标评价网络Q′和目标动作网络μ′，并将网络参数赋值为θ^Q′←θ^Q,θ^μ′←θ^μ；

3)初始化经验回放池，以用于存放过往数据和提高数据利用率并降低数据相关性；

4)for episode＝1 to M进行迭代，其中，M为最大训练回合数：

①为动作选择初始化一个随机过程N，选择高斯过程，提高探索率；

②获得初始状态S；

③在当前动作网络基于状态S得到动作A＝μ(S|θ^μ)+N；

④执行动作A，得到新状态S′、奖励R和终止状态的标志end；

⑤将{S,A,S′,R,end}五元组存入经验回放池；

⑥当S＝S′，从经验回放池中采样128个样本{S_j,A_j,S′_j,R_j,end_j},j＝1,2,...,128，计算当前目标Q值y_j：

⑦使用均方损失误差

通过神经网络的梯度反向传播来更新当前评价网络的网路参数θ^Q；使用

更新当前动作网络的网络参数θ^μ；

⑧每隔100个episode，更新目标评价网络和目标动作网络的网络参数：

⑨若S′为终止状态，则当前轮迭代完毕，否则跳转步骤③，进行下一轮迭代。

具体的DDPG算法参数设置如表1所示：

表1 DDPG算法参数设置

参数	设置值
		最大训练回合数M	1×10<sup>4</sup>
学习率	2×10<sup>-3</sup>
		衰减因子γ	0.99
经验回放池大小	1000
		评价网络隐含层神经元个数	[300,400]
动作网络隐含层神经元个数	[300,400]

优选地，步骤四中的奖励，分为稀疏奖励和reward shaping奖励两种；其中，稀疏奖励为：在一个迭代回合中，当四旋翼均达到目标点时给予智能体奖励，其余时刻奖励为0，直至回合结束，每当四旋翼到达目标点获得奖励R＝1，如果四旋翼越界，则给予惩罚R＝-1，并结束回合；reward shaping奖励为：在一个迭代回合中，四旋翼每一步有一个基础奖励，并将四旋翼当前位置与目标位置的距离作为惩罚项，直至回合结束，每多一步给予奖励R＝10，再减去四旋翼距目标点的距离作为惩罚，如果四旋翼越界，则给予惩罚R＝-100，并结束回合。

实施例：

针对X型四旋翼无人机位姿系统，具体的仿真参数如下：惯性矩阵参数分别为I_x＝1.75×10^-2kg·m²，I_y＝1.75×10^-2kg·m²，I_z＝3.18×10^-2kg·m²；质量为1.5kg，螺旋桨升力系数为1.11×10^-5N/(rad/s)²，螺旋桨扭力系数为1.49×10^-7N·m/(rad/s)²，电机转速比例参数为646rad/s，电机转速偏执参数为166rad/s，转子的转动惯量为9.9×10^-5kg·m²。

仿真实验环境设置如下，姿态初值为[0,0,0]rad，位置初值为[0,0,5]m，目标位置为[0,0,5]m，给定初始扰动，即初始角速度[0.5,0.5,0.5]rad/s，初始速度为[0.5,0.5,0.5]m/s，

仿真过程中，根据X型四旋翼无人机自身状态和故障诊断，本发明通过位姿PID控制器结合强化学习调参技术，对四旋翼无人机的姿态和位置控制的PID控制参数进行了自整定，故障下的姿态角度响应如图1至图3所示，在惯性坐标下的位置响应如图4至图6所示，最终实现了目标位姿的到达与稳定。

以上仿真验证了本发明控制方法的有效性和对四旋翼无人机系统鲁棒性的保证。

Claims (2)

1.一种基于故障诊断结果的X型四旋翼无人机控制方法，其特征在于，包括如下步骤:

步骤一，建立X型四旋翼无人机的运动学模型,其具体形式如下：

其中，

为滚转角，θ为俯仰角，ψ为偏航角，

分别为三轴角加速度，

分别为三轴角速度，

分别为三轴线加速度，

分别为三轴线速度，I_x,I_y,I_z为机身在x,y,z三个方向上的转动惯量，J_r为转动惯量，K₁,…,K₆为空气阻力系数，l为电机到质心的臂长，m为机身重量，g为重力加速度；U₁,…,U₄,Ω存在如下关系：

其中，U₁为总升力，U₂为滚转力矩，U₃为俯仰力矩，U₄为偏航力矩，b为升力系数，d为阻力系数，Ω₁,…,Ω₄分别为1至4号电机的转速；

步骤二，进行位姿PID控制器设计：

1)姿态控制回路为：

其中，k1...k9为姿态控制器参数，俯仰角误差

滚转角误差e_θ＝θ_c-θ，偏航角误差e_ψ＝ψ_c-ψ，

为目标姿态角，

为当前姿态角，

分别为三轴目标角加速度，

分别为俯仰角误差速度、滚转角误差速度、偏航角误差速度；对应电机的转速表示如下：

其中，C_T为升力系数，C_Q为阻力系数；

2)位置控制回路为：

其中，k1...k9为位置控制器参数，x轴方向位置误差e_x＝x_c-x，y轴方向位置误差e_y＝y_c-y，z轴方向位置误差e_z＝z_c-z，[x_c,y_c,z_c]^T为目标位置，[x,y,z]^T为当前位置，

分别为x轴、y轴、z轴方向误差速度，

为x轴、y轴、z轴方向目标加速度；设ψ_c已知，可得：

步骤三，建立执行机构故障模型：

其中，控制输入u＝[U₁,U₂,U₃,U₄]，u^F为受电机故障影响的真实的控制器输入，

为不确定的故障；

步骤四，进行PID控制器参数智能调节：

基于步骤三中的故障诊断信息，采用强化学习DDPG算法，通过搭建动作神经网络和评价神经网络，输入四旋翼无人机当前四旋翼的位置、速度、姿态角和角速度的状态，输出最优的控制器参数，实现PID控制器参数的智能调节，具体的DDPG算法过程如下：

1)随机初始化当前评价网络Q(S,A|θ^Q)和当前动作网络μ(S|θ^μ)，以及评价网络参数θ^Q和动作网络参数θ^μ，其中S为状态，A为动作；

2)初始化目标评价网络Q′和目标动作网络μ′，并将网络参数赋值为θ^Q′←θ^Q,θ^μ′←θ^μ；

3)初始化经验回放池，以用于存放过往数据和提高数据利用率并降低数据相关性；

4)for episode＝1 to M进行迭代，其中，M为最大训练回合数：

①为动作选择初始化一个随机过程N；

②获得初始状态S；

③在当前动作网络基于状态S得到动作A＝μ(S|θ^μ)+N；

④执行动作A，得到新状态S′、奖励R和终止状态的标志end；

⑤将{S,A,S′,R,end}五元组存入经验回放池；

⑥当S＝S′，从经验回放池中采样128个样本{S_j,A_j,S_j′,R_j,end_j},j＝1,2,...,128，计算当前目标Q值y_j：

⑦使用均方损失误差

通过神经网络的梯度反向传播来更新当前评价网络的网路参数θ^Q；使用

更新当前动作网络的网络参数θ^μ；

⑧每隔100个episode，更新目标评价网络和目标动作网络的网络参数：

⑨若S′为终止状态，则当前轮迭代完毕，否则跳转步骤③，进行下一轮迭代。

2.根据权利要求1所述的一种基于故障诊断结果的X型四旋翼无人机控制方法，其特征在于，步骤四中的奖励，分为稀疏奖励和reward shaping奖励两种；其中，稀疏奖励为：在一个迭代回合中，当四旋翼均达到目标点时给予智能体奖励，其余时刻奖励为0，直至回合结束，每当四旋翼到达目标点获得奖励R＝1，如果四旋翼越界，则给予惩罚R＝-1，并结束回合；reward shaping奖励为：在一个迭代回合中，四旋翼每一步有一个基础奖励，并将四旋翼当前位置与目标位置的距离作为惩罚项，直至回合结束，每多一步给予奖励R＝10，再减去四旋翼距目标点的距离作为惩罚，如果四旋翼越界，则给予惩罚R＝-100，并结束回合。

Images