CN108663929A - 一种基于路径规划的无人机刹车改进方法 - Google Patents

Abstract

本发明公开了一种基于路径规划的无人机刹车改进方法，包括路径规划模块、位置控制模块、位置控制模块、位置控制模块以及刚体动力学模块，本发明针对飞行器在刹车过程中出现的刹车姿态跳变、震荡等异常情况，对无人机的刹车功能进行相应的路径规划。本发明基于路径规划的思想，对飞行器在刹车过程中的姿态变化过程进行规划，解决了飞行器在高速飞行急刹过程中出现的姿态跳变震荡、回拉等异常现象，很大程度上改善了飞行器急刹掉高现象。通过使用本发明，能够提高摇杆回中时无人飞行器刹车功能的稳定性，而且还能够提高用户的操作便利性和体验感。本发明作为一种无人飞行器刹车功能的改进方法及***可广泛用于无人飞行器的领域中。

Description

一种基于路径规划的无人机刹车改进方法

技术领域

本发明涉及无人机领域，具体涉及一种基于路径规划的无人机刹车改进方法。

背景技术

在目前主流的开源飞控中，例如PX4，针对无人机在位置控制模式下的刹车功能的处理过程主要是将遥控器杆量转换为速度设定值，再计算出位置设定值；当摇杆居中时，进入刹车模块，此时杆量快速变为零，对应的速度设定值也跳变为零。但由于刹车时刻速度设定值的跳变为零，从而位置设定值也出现跳变，进而使得飞机的刹车姿态出现跳变，从而出现刹车姿态震荡、回拉现象、急刹掉高等异常情况，给使用者带来非常差的用户体验。尤其在逆风环境中高速飞行状态下的突然刹车，甚至会出现飞机姿态超调侧翻进而导致炸机的危险。

发明内容

本发明所要解决的技术问题是提供一种基于路径规划的无人机刹车改进方法，利用路径规划的思想，对无人飞行器刹车过程的姿态变化过程进行规划，使飞行器由前飞阶段先经过姿态过渡阶段再进入刹车阶段，当速度降到位置控制临界速度时，飞行器完成姿态调整进入P模式控制(位置控制，定点模式)。

本发明是通过以下技术方案来实现的：一种基于路径规划的无人机刹车改进方法，包括路径规划模块、位置控制模块、位置控制模块、位置控制模块以及刚体动力学模块，

由基于姿态规划的路径规划模块计算得到刹车过程中飞行器的位置设定值，并输出给位置控制模块；

由位置控制模块计算得到刹车过程中飞行器的姿态设定值和推力设定值，并分别输出给姿态控制模块和电机控制模块；

由姿态控制模块计算得到刹车过程中飞行器的力矩设定值，并输出给电机控制模块；

由电机控制模块计算得到刹车过程中飞行器的电机转速，并输出给飞行器的刚体动力学模块；

由刚体动力学模块计算得到刹车过程中飞行器的位置、速度信息和角度、角速度信息，并分别输出给飞控的位置控制模块和姿态控制模块，使飞行器进行位置回路和姿态回路的PID控制。

作为优选的技术方案，所述位置控制模块：

式中，是NED坐标系下飞行器三个方向的加速度设定值；φ_des、θ_des、ψ_T分别为飞行器的滚转角设定值、俯仰角设定值和偏航角设定值；g是重力加速度常数；m为飞行器质量；u₁为Z方向飞行器推力，由位置控制模块计算得到的Pitch和Roll的姿态角设定值分别为

对于悬停控制，水平方向的速度设定值都为零：p_des＝0，q_des＝0、r_des＝0，Z方向推力由PD控制器计算得到：

作为优选的技术方案，所述姿态控制模块：

式中，φ、θ、ψ分别为飞行器的滚转角、俯仰角、偏航角；p、q、r为三个姿态角对应的角速度；φ_des、θ_des、ψ_des分别为滚转角设定值、俯仰角设定值、偏航角设定值；p_des、q_des、r_des为三个姿态角设定值对应的姿态角速度设定值；k_p,φ、k_p,θ、k_p,ψ、k_d,φ、k_d,θ、k_d,ψ为PD控制参数。

作为优选的技术方案，还包括保直线方法、XY同步刹车方法。

作为优选的技术方案，所述保直线方法具体步骤如下：

1、当无人飞行器由飞行状态进入刹车状态时，首先记录此时的刹车初始速度init_vel方向向量；

2、每次计算相邻时刻的位置设定值差值向量时，

delta_L＝pos_sp-pos_sp_pre，都将delta_L向量往刹车初始速度方向作投影，使得每次计算得到的位置设定值都在init_vel方向上，从而达到保直线的目的，其中，投影的夹角由公式计算得到。

作为优选的技术方案，所述XY同步刹车方法具体步骤如下：

1、当无人飞行器由飞行状态进入刹车状态时，首先记录此时的刹车初始速度init_vel方向向量；

2、每次对刹车初始速度方向设计合适的刹车姿态角设定值angle_sp，再将刹车姿态角分别沿XY方向投影，计算XY方向对应的刹车姿态角angle_x、angle_y，此时，init_vel分解到XY方向的速度分量会对应不同的刹车姿态角，理论上XY方向对应的速度会同时减速到零，从而达到XY同步刹车的目的，其中，XY方向对应的刹车姿态角angle_x、angle_y由公式计算得到。

本发明的有益效果是：1、本发明针对飞行器在刹车过程中出现的刹车姿态跳变、震荡等异常情况，对无人机的刹车功能进行相应的路径规划；

2、本发明基于路径规划(Planning)的思想，对飞行器在刹车过程中的姿态变化过程进行规划，解决了飞行器在高速飞行急刹过程中出现的姿态跳变震荡、回拉等异常现象，很大程度上改善了飞行器急刹掉高现象；

3、通过使用本发明，能够提高摇杆回中时无人飞行器刹车功能的稳定性，而且还能够提高用户的操作便利性和体验感；

4、本发明作为一种无人飞行器刹车功能的改进方法及***可广泛用于无人飞行器的领域中。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为基于路径规划的无人机控制回路示意图；

图2为刹车过程中无人飞行器飞行姿态角的变化示意图；

图3为刹车过程中无人飞行器飞行姿态角的变化示意图；

图4为刹车过程中保直线策略和XY同步刹车策略的示意图；

图5为低速飞行时刹车过程中速度与姿态角的变化情况示意图；

图6为高速飞行时刹车过程中速度与姿态角的变化情况示意图。

具体实施方式

本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合。

本说明书(包括任何附加权利要求、摘要和附图)中公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换。即，除非特别叙述，每个特征只是一系列等效或类似特征中的一个例子而已。

如图1所示，包括路径规划模块、位置控制模块、位置控制模块、位置控制模块以及刚体动力学模块，

由基于姿态规划的路径规划模块计算得到刹车过程中飞行器的位置设定值，并输出给位置控制模块；

由位置控制模块计算得到刹车过程中飞行器的姿态设定值和推力设定值，并分别输出给姿态控制模块和电机控制模块；

由姿态控制模块计算得到刹车过程中飞行器的力矩设定值，并输出给电机控制模块；

由电机控制模块计算得到刹车过程中飞行器的电机转速，并输出给飞行器的刚体动力学模块；

由刚体动力学模块计算得到刹车过程中飞行器的位置、速度信息和角度、角速度信息，并分别输出给飞控的位置控制模块和姿态控制模块，使飞行器进行位置回路和姿态回路的PID控制。

为了提高无人飞行器刹车功能的稳定性及操作者的用户体验，本发明对飞行器的刹车过程进行姿态规划，将刹车过程中无人飞行器的姿态角变化情况分为四个阶段：飞行阶段、姿态过渡阶段、刹车阶段和姿态调整阶段。飞行器的二维平面飞行姿态角变化示意图如图2所示。

如图2所示：

①飞行阶段：即飞行器接收遥控器摇杆信号保持飞行姿态角向前飞行。以水平方向Pitch杆满杆飞行为例，飞行器Pitch方向速度由零逐步加速到水平最大飞行速度后保持匀速飞行。

②姿态过渡阶段：即飞行器接收遥控器摇杆回中信号(即刹车信号)由前飞姿态角过渡到刹车姿态角的姿态过渡阶段。由刚体动力学理论可知，飞行器只有将飞行姿态调整到刹车姿态时，飞机才会产生一个和速度方向相反的反向加速度使得飞行器达到减速刹车的效果。

③刹车阶段：即飞行器保持刹车姿态角将飞行速度降到位置控制临界速度的阶段。不同的刹车姿态角会产生不同大小的反向加速度，从而导致飞行器的刹车时间和刹车距离都有所不同。在实际操作中，可以通过调节刹车姿态角参数来实现不同飞行速度下合理的刹车距离和刹车效果。

④姿态调整阶段：即飞行器由刹车姿态角调整到水平悬停姿态，进而进入位置控制模式的阶段。当飞行器飞行速度降到位置控制临界速度时，此时飞行速度已减速到很小的值，此时将飞行器姿态调整到水平姿态有利于飞行器更合理平滑的过渡到位置控制模式。

综上所述，刹车过程中无人飞行器飞行姿态角和飞行速度的变化示意图如图3所示。

位置控制与姿态控制

无人飞行器***的位置控制模块：

式中，是NED坐标系下飞行器三个方向的加速度设定值；φ_des、θ_des、ψ_T分别为飞行器的滚转角设定值、俯仰角设定值和偏航角设定值；g是重力加速度常数；m为飞行器质量；u₁为Z方向飞行器推力。由位置控制模块计算得到的Pitch和Roll的姿态角设定值分别为

对于悬停控制，水平方向的速度设定值都为零：p_des＝0，q_des＝0、r_des＝0。Z方向推力由PD控制器计算得到：

基于PD控制无人飞行器***的姿态控制模块：

式中，φ、θ、ψ分别为飞行器的滚转角、俯仰角、偏航角；p、q、r为三个姿态角对应的角速度；φ_des、θ_des、ψ_des分别为滚转角设定值、俯仰角设定值、偏航角设定值；p_des、q_des、r_des为三个姿态角设定值对应的姿态角速度设定值；k_p,φ、k_p,_θ、k_p,ψ、k_d,φ、k_d,θ、k_d,ψ为PD控制参数。

刹车功能的进一步改进：

(1)保直线方法：

在实际无人飞行器的刹车过程中，由于非常容易受到外部风力的影响，导致刹车的轨迹并不一定是直线。为了进一步改进刹车效果，对刹车的轨迹采取了保直线的策略，具体步骤如下：

①、当无人飞行器由飞行状态进入刹车状态时，首先记录此时的刹车初始速度init_vel方向向量，如图4所示；

②、每次计算相邻时刻的位置设定值差值向量时，delta_L＝pos_sp-pos_sp_pre，都将delta_L向量往刹车初始速度方向作投影，使得每次计算得到的位置设定值都在init_vel方向上，从而达到保直线的目的。其中，投影的夹角由下列公式计算得到：

(2)XY同步刹车方法：

在实际无人飞行器的刹车过程中，控制器通常是将速度分别沿XY两个方向进行刹车处理，会导致由于XY刹车不同步而导致不良的用户体验，如两次电机刹车声音。为了进一步改进刹车功能，对刹车的过程采取了XY同步刹车的策略，具体步骤如下：

①、当无人飞行器由飞行状态进入刹车状态时，首先记录此时的刹车初始速度init_vel方向向量，如图4所示；

②每次对刹车初始速度方向设计合适的刹车姿态角设定值angle_sp，再将刹车姿 态角分别沿XY方向投影，计算XY方向对应的刹车姿态角angle_x、angle_y。此时，init_vel 分解到XY方向的速度分量会对应不同的刹车姿态角，理论上XY方向对应的速度会同时减速 到零，从而达到XY同步刹车的目的。其中，XY方向对应的刹车姿态角angle_x、angle_y由下 列公式计算得到：

实际飞行测试

将前面对无人飞行器刹车功能的改进算法添加到基于C++的飞控源码中，在位置控制模式下，分别对飞行器不同飞行速度下的刹车效果进行测试。

低速飞行刹车测试

如图5所示，无人飞行器在低速飞行状态进入刹车模块姿态角和速度的变化情况。实测结果表明，低速飞行状态进入刹车模块时，飞行器在刹车过程中姿态过渡平稳，没有出现姿态超调震荡、回拉等异常现象，急刹掉高现象也得到明显改善，刹车效果和用户体验都大大提高。

高速飞行刹车测试

如图6所示，无人飞行器在高速飞行状态进入刹车模块姿态角和速度的变化情况。实测结果表明，高速飞行状态进入刹车模块时，飞行器在刹车过程中姿态过渡平稳，没有出现姿态超调震荡、回拉等异常现象，急刹掉高现象也得到明显改善，刹车效果和用户体验都大大提高。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何不经过创造性劳动想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书所限定的保护范围为准。

Claims (6)

1.一种基于路径规划的无人机刹车改进方法，其特征在于：包括路径规划模块、位置控制模块、位置控制模块、位置控制模块以及刚体动力学模块，

由基于姿态规划的路径规划模块计算得到刹车过程中飞行器的位置设定值，并输出给位置控制模块；

由位置控制模块计算得到刹车过程中飞行器的姿态设定值和推力设定值，并分别输出给姿态控制模块和电机控制模块；

由姿态控制模块计算得到刹车过程中飞行器的力矩设定值，并输出给电机控制模块；

由电机控制模块计算得到刹车过程中飞行器的电机转速，并输出给飞行器的刚体动力学模块；

由刚体动力学模块计算得到刹车过程中飞行器的位置、速度信息和角度、角速度信息，并分别输出给飞控的位置控制模块和姿态控制模块，使飞行器进行位置回路和姿态回路的PID控制。

2.如权利要求1所述的基于路径规划的无人机刹车改进方法，其特征在于：所述位置控制模块：

式中，是NED坐标系下飞行器三个方向的加速度设定值；φ_des、θ_des、ψ_T分别为飞行器的滚转角设定值、俯仰角设定值和偏航角设定值；g是重力加速度常数；m为飞行器质量；u₁为Z方向飞行器推力，由位置控制模块计算得到的Pitch和Roll的姿态角设定值分别为

对于悬停控制，水平方向的速度设定值都为零：p_des＝0，q_des＝0、r_des＝0，Z方向推力由PD控制器计算得到：

3.如权利要求1所述的基于路径规划的无人机刹车改进方法，其特征在于：所述姿态控制模块：

式中，φ、θ、ψ分别为飞行器的滚转角、俯仰角、偏航角；p、q、r为三个姿态角对应的角速度；φ_des、θ_des、ψ_des分别为滚转角设定值、俯仰角设定值、偏航角设定值；p_des、q_des、r_des为三个姿态角设定值对应的姿态角速度设定值；k_p,φ、k_p,θ、k_p,ψ、k_d,φ、k_d,θ、k_d,ψ为PD控制参数。

4.如权利要求1所述的基于路径规划的无人机刹车改进方法，其特征在于：还包括保直线方法、XY同步刹车方法。

5.如权利要求4所述的基于路径规划的无人机刹车改进方法，其特征在于：所述保直线方法具体步骤如下：

1、当无人飞行器由飞行状态进入刹车状态时，首先记录此时的刹车初始速度init_vel方向向量；

2、每次计算相邻时刻的位置设定值差值向量时，

delta_L＝pos_sp-pos_sp_pre，都将delta_L向量往刹车初始速度方向作投影，使得每次计算得到的位置设定值都在init_vel方向上，从而达到保直线的目的，其中，投影的夹角由公式计算得到。

6.如权利要求4所述的基于路径规划的无人机刹车改进方法，其特征在于：所述XY同步刹车方法具体步骤如下：

1、当无人飞行器由飞行状态进入刹车状态时，首先记录此时的刹车初始速度init_vel方向向量；

2、每次对刹车初始速度方向设计合适的刹车姿态角设定值angle_sp，再将刹车姿态角分别沿XY方向投影，计算XY方向对应的刹车姿态角angle_x、angle_y，此时，init_vel分解到XY方向的速度分量会对应不同的刹车姿态角，理论上XY方向对应的速度会同时减速到零，从而达到XY同步刹车的目的，其中，XY方向对应的刹车姿态角angle_x、angle_y由公式计算得到。