CN112683261B - 一种基于速度预测的无人机鲁棒性导航方法 - Google Patents

Abstract

本发明公开了一种基于速度预测的无人机鲁棒性导航方法，涉及无人机导航领域，该方法根据速度来确定自身的位置，主要是根据回声神经网络来预测导航中所需要的速度，通过无人机的自身参数去预测速度。相比于惯性导航预测速度而言，本发明所设计的速度预测器不会因时间的累积而使速度误差增大并且精度也比较高。这是因为，无人机的速度是由最近的输入序列决定的，速度估计误差会被逐渐遗忘。回声状态网路的遗忘特性正适合于学习这种特性，这也是本发明速度估计器能有效工作的物理基础。

Description

一种基于速度预测的无人机鲁棒性导航方法

技术领域

本发明涉及无人机导航领域，尤其涉及一种基于速度预测的无人机鲁棒性导航方法。

背景技术

无人机通常用GPS和惯导组合导航的方式去导航，但当飞机在城市建筑群或者偏远山区飞行时，GPS信号比较弱，此时可以用惯导去导航。惯导一般是由陀螺仪和加速度计组成，在得到位置信息时有累计误差，从而不能获得精确的位置。本发明主要应用于在山区或者城市建筑群这类弱GPS信号的地区，通过对无人机本身参数的辨识和传感器稳定测量从而去预测无人机飞行的速度，这种方式是一种新的思路，无人机本身的系统参数不会随着飞行的外界因素改变而改变，同时也不会有用传感器测量带来的误差，稳定性、精确性能得到保障。在无人机导航领域中能够得到广泛运用。

在无人机导航中，惯导是一种不受外部信号强弱影响的一种导航方式，因此它的应用很广，但是惯导中的速度信息是通过加速度积分获得。加速度数据主要是通过加速度计传感器测量。这样就会出现两个问题，第一，测量的误差通过积分后会放大；第二，积分的过程本身会带来误差。

发明内容

本发明的目的在于提出一种新的导航思路，导航需要的一个重要数据是速度，本发明提出了通过对系统本身参数的测量去预测速度信息，从而导航。解决了用加速度计测量时积分带来的误差问题，提高了精度。

为了解决测量误差积分放大以及积分过程算法误差这两个问题，本发明提出了利用无人机本身的系统参数，系统参数测量过程中不会出现上述两个问题，同时也不会受到外部环境的影响。将无人机的机体视为刚体，刚体运动学模型如式(1)所示，本发明中的无人机为四旋翼无人机：

其中v、ω分别为b系中的速度和角速度，F表示合力，J表示惯性矩阵，M表示合力矩，m表示物体的质量，

表示速度的变化率，

表示角速度的变化率。由上式可知，无人机的速度可以由系统所受合力、系统质量以及稳定测量角速度获得，精确度较高。

由式(1)可知，速度的变化率与角速度和所受合力以及质量相关，角速度的值可以通过陀螺仪测量，质量可以预先测量，合力主要由以下公式(2)计算可得：

其中g是重力加速度，为已知量，

表示从b系到n系的坐标转换矩阵，其中b系表示载体坐标系，n系表示导航坐标系，如公式(3)(4)所示；F_z表示气动主动力，如公式(5)所示；

其中ω_x、ω_y、ω_z分别为角速度在b系中的投影分量，四元数q＝[q₀,q₁,q₂,q₃]^T中的q₀,q₁,q₂,q₃表示四元数预先定义的四个数，描述的基本依据是，对空间中的任意两个原点重合的笛卡尔坐标系，总存在一个过原点的轴使得一个坐标系绕轴旋转一定角度后与另一个坐标系重合；如式(3)所示，为四元数表示的旋转矩阵，与欧拉角表示的旋转矩阵可以相互转化；式(4)描述了四元数表示的姿态微分方程，该微分方程的元素不会因俯仰角等于90°而出现奇异；

F_Z＝(w_r1 ²+w_r2 ²+w_r3 ²+w_r4 ²) (5)

其中w_ri(i＝1,2,3,4)表示四旋翼无人机四个电机转动角速率，所述四个电机完全相同，升力系数K_f＝ρSr²C_Tstat。其中C_Tstat表示无量纲的升力系数、S表示旋翼面积、r表示旋翼半径、ρ表示空气密度。

由式(2)(3)(4)(5)可知，合力F可以由无人机的本身系数求出，由式(1)知，整个刚体模型中，速度的变化率与速度、合力和角速度相关，所以就可以用无人机已知的系统参数与陀螺仪测得的角速度去预测速度，采用回声状态网络预测速度。

相比于惯性导航预测速度而言，本发明所设计的速度预测器不会因时间的累积而使速度误差增大并且精度也比较高。这是因为，无人机的速度是由最近的输入序列决定的，速度估计误差会被逐渐遗忘。回声状态网路的遗忘特性正适合于学习这种特性，这也是本发明速度估计器能有效工作的物理基础。

附图说明

图1为本发明n系和b系坐标系定义示意图

图2为本发明回声状态网络结构图

图3为本发明速度预测器结构图

具体实施方式

下面结合附图对本发明的具体实施方式以及工作原理作进一步详细说明。

本发明无人机导航预测的主要思路是根据速度来确定自身的位置，主要是根据回声神经网络来预测导航中所需要的速度，通过无人机的自身参数去预测速度。其结构如图2所示。选择回声状态网络的原因是该网络有很高的预测精度，同时它有一条最重要的特性：回声特性，当网络参数的选取满足一定的约束条件时，回声状态网络的内部状态是逐渐衰减的，那么从系统的稳定性角度来看，回声状态网络是全局一致渐进稳定的。同时选取不同的学习算法时可以进行在线学习。本发明选取递推最小二乘算法。

为了便于描述四旋翼无人机在近地面空间中的运动学规律和动力学特性，需要建立适合其特性的坐标系。在四旋翼无人机研究中常涉及到如下四个坐标系：惯性系(i系)、导航坐标系(n系，北东地)、载体坐标系(b系)和WGS84坐标系；其中，惯性系(i系)，定义惯性坐标系是为了对四旋翼无人机进行受力分析，在惯性系中分析物体受力无需考虑惯性力的影响，而在非惯性系中受力分析需要考虑惯性力的影响。当地导航坐标系(n系，北东地)，原点位于无人机起飞点或者人为设定的一个参考点。载体坐标系(b系)中载体x轴指向真北时航向角为0°，如图1所示，图1(a)为onNED导航坐标系，即n系；图1(b)为oxyz载体坐标系，即b系，四旋翼无人机前进方向为大箭头所指方向，电机转向为曲线箭头方向。

为了解决测量误差积分放大以及积分过程算法误差这两个问题，本发明提出了利用无人机本身的系统参数，系统参数测量过程中不会出现上述两个问题，同时也不会受到外部环境的影响。将无人机的机体视为刚体，刚体运动学模型如式(1)所示，本发明中的无人机为四旋翼无人机：

其中v、ω分别为b系中的速度和角速度，F表示合力，J表示惯性矩阵，M表示合力矩，m表示物体的质量，

表示速度的变化率，

表示角速度的变化率。由上式可知，无人机的速度可以由系统所受合力、系统质量以及稳定测量角速度获得，精确度较高。

由式(1)可知，速度的变化率与角速度和所受合力以及质量相关。角速度的值可以通过陀螺仪测量，质量可以预先测量。合力主要由以下公式(2)计算可得：

其中g是重力加速度，为已知量，

表示从b系到n系的坐标转换矩阵，其中b系表示载体坐标系，n系表示导航坐标系，如公式(3)(4)所示。F_z表示气动主动力，如公式(5)所示。

其中ω_x、ω_y、ω_z分别为角速度在b系中的投影分量。四元数q＝[q₀,q₁,q₂,q₃]^T中的q₀,q₁,q₂,q₃表示四元数预先定义的四个数。描述的基本依据是，对空间中的任意两个原点重合的笛卡尔坐标系，总存在一个过原点的轴使得一个坐标系绕轴旋转一定角度后与另一个坐标系重合。如式(3)所示，为四元数表示的旋转矩阵，与欧拉角表示的旋转矩阵可以相互转化。式(4)描述了四元数表示的姿态微分方程，该微分方程的元素不会因俯仰角等于90°而出现奇异。

F_Z＝(w_r1 ²+w_r2 ²+w_r3 ²+w_r4 ²) (5)

其中w_ri(i＝1,2,3,4)表示四旋翼无人机四个电机转动角速率，所述四个电机完全相同，升力系数K_f＝ρSr²C_Tstat。其中C_Tstat表示无量纲的升力系数、S表示旋翼面积、r表示旋翼半径、ρ表示空气密度。

由式(2)(3)(4)(5)可知，合力F可以由无人机的本身系数求出，由式(1)知，整个刚体模型中，速度的变化率与速度、合力和角速度相关，所以就可以用无人机已知的系统参数与陀螺仪测得的角速度去预测速度，本发明将采用回声状态网络去预测速度，整个结构如图3所示，步骤如下：

步骤一：根据无量纲的升力系数C_Tstat与旋翼面积S、旋翼半径r、空气密度ρ可以求出升力系数K_f，再根据四个电机中每个电机的转动速率可以求出气动主动力，从而求出所需要的合力F。

步骤二：如图2所示，构建一个神经网络，其中W_in表示将外部输入传递到存储池当中，W表示内部状态的传递，W_out表示控制输出。存储池存储回声状态网络的内部状态向量x，内部状态向量x之间的联系由状态转移矩阵W决定。将合力F与陀螺仪测得机体角速度ω_x、ω_y、ω_z为输入，载体坐标系中的速度v_x、v_y、v_z为期望输出。输入的状态数目总计为5个(ω_x、ω_y、ω_z、F、m)，输出的状态数目总计为3个(v_x、v_y、v_z)。回声状态网络的数学形式如式(6)和(7)所示，

x(n+1)＝αx(n)+βf(W_inu(n+1)+Wx(n)) (6)

其中，n表示时刻n，u表示维数为K的输入向量，x表示维数为N的内部状态向量，y表示维数为L的输出向量，W_in将输入与内部状态关联起来，W表示内部状态的传递，W_back表示输出向量反馈矩阵，W_out表示控制输出矩阵。α和β均表示待定参数。

步骤三：在式(6)中有待定参数α和β，由式(8)可知只要确定了参数α即可确定参数β。对于输入矩阵选取为随机矩阵W_in，其值在-1到1之间。对于状态转移矩阵W，首选取一个随机矩阵W，’并计算该随机矩阵W’的奇异值，将该矩阵W’除以奇异值得到状态转移矩阵W。对于输出矩阵W_out，将使用如下所描述的回声状态训练网络算法进行训练。

|α|＜1,且β＝1-|α| (8)

回声状态训练网络算法：回声状态训练网络的学习算法目前主要有广义逆法、递推最小二乘算法。广义逆法只适合于批量处理数据，不适合于在线运行。因此，本发明推导适合于式(6)和(7)所确定回声状态训练网络的递推最小二乘算法。

步骤1随机生成值在-1到1矩阵W_in、W’和W_out，计算W’的奇异值σ_W，并将W’除以σ_W得到W。

步骤2从训练样本库中获取一个样本，根据式(9)计算期望输出y_d(n)与当前网络输出y(n)的误差向量δ。

步骤3选择适当的遗忘因子μ，根据式(10)递推估计输出矩阵W_out。

步骤4重复进行步骤2和3，直至将训练样本库完整遍历一次。

定义误差向量δ

δ＝y_d(n)-y(n) (9)

递推最小二乘算法如下，当前时刻n修正后的权值将在n+1时刻使用，故记当前时刻修正后的输出矩阵为W_out(n+1)。

其中μ表示遗忘因子，且0.95<μ≤1，P_RLS的初值可选为元素足够大的对角阵。L_RLS(n)表示一个中间变量。

另外，本发明建立了仿真模型进行仿真，具体参数为K_f＝6.20566×10^-6，α＝0.996，内部状态数目为50，角速度用陀螺仪测得数据。如表1所示，其描述了传感器噪声对速度估测的影响，表明速度预测器在传感器噪声条件下能正常工作。表1中的标准噪声是当α＝0.996时取得的值，这是通过对α连续取值后得到的最优值，后面几项是指将该项的传感器噪声设置为0时，将其它传感器噪声取为标准噪声时的值，这样可以反映该传感器器噪声对训练和预测的影响。影响值可以由该噪声值减去标准噪声再除以标准噪声计算得到。由表1可知加速度噪声和速度噪声对预测RMSE(均方根误差)为0时的值很接近标准噪声，相对于标准噪声值产生0.1％的影响，在实际中可以忽略速度和加速度噪声对速度预测器的影响。角速度是速度预测器的输入，由表1知，角速度噪声与标准噪声预测RMSE之差为0.002383。对预测RMSE的影响幅度为4.8％，但是在无角速度噪声的情况反而稍有劣化。由表1可知位置噪声对预测RMSE的影响幅度为30.6％，这是因为位置噪声极大地改变了无人机运动控制，所以根据表1可以推测在不同运动轨迹之下，速度估计器的估计性能将有较大的差异。

表1噪声对训练RMSE和预测RMSE的影响(N＝50，α＝0.996)

为了保证速度预测有良好泛化能力，选择悬停+螺线+直线作为训练轨迹。表2为不同轨迹下条件速度预测RMSE(均方根误差)和MRE(平均相对误差)值。由表2可知在标准噪声条件下，首先进行横向比较：v_x、v_y的均方根误差与平均相对误差均小于v_z，因为在z轴方向的速度测量受到重力影响，但相差不大，三个方向的速度的均方根误差与平均相对误差都很小。从竖向比较发现螺线飞行的时候，均方根误差与平均相对误差大于其它两种飞行情况。因为螺线飞行工况复杂，带有z轴方向速度的测量。从表中可得x，y轴的速度在悬停和直线情况下精度大于94％。悬停，螺线和直线所有工况下，速度预测精度大于91.7％，预测RMSE值小于0.04m/s，表明即使在复杂飞行情况下，预测结果精度也很高。

表2不同轨迹不同噪声条件下，速度预测性能(η_MRE＝0.05)

注：η_MRE＝0.05表明小于0.05m/s的速度不纳入平均相对误差统计范围内。

由上述表可以得出结论：相比于惯性导航预测速度而言，本发明所设计的速度预测器不会因时间的累积而使速度误差增大并且精度也比较高。这是因为，无人机的速度是由最近的输入序列决定的，速度估计误差会被逐渐遗忘。回声状态网路的遗忘特性正适合于学习这种特性，这也是本发明速度估计器能有效工作的物理基础。

以上所述，仅为本发明的具体实施方式，本说明书中所公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换；所公开的所有特征、或所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以任何方式组合；本领域的技术人员根据本发明技术方案的技术特征所做出的任何非本质的添加、替换，均属于本发明的保护范围。

Claims (2)

1.一种基于速度预测的无人机鲁棒性导航方法，所述无人机为四旋翼无人机，该方法包括以下步骤：

S1)将无人机的机体视为刚体，刚体运动学模型如式(1)所示：

其中v、ω分别为b系中的速度和角速度，b系表示载体坐标系，F表示合力，J表示惯性矩阵，M表示合力矩，m表示物体的质量，

表示速度的变化率，

表示角速度的变化率，由式(1)可知，无人机的速度由系统所受合力、系统质量以及稳定测量角速度获得；

S2)由式(1)可知，速度的变化率与角速度和所受合力以及质量相关，角速度的值通过陀螺仪测量，质量能够预先测量，合力由以下公式(2)计算得到：

其中g是重力加速度，为已知量，

表示从b系到n系的坐标转换矩阵，其中b系表示载体坐标系，n系表示导航坐标系，如公式(3)(4)所示；F_z表示气动主动力，如公式(5)所示；

其中ω_x、ω_y、ω_z分别为角速度在b系中的投影分量，四元数q＝[q₀,q₁,q₂,q₃]^T中的q₀,q₁,q₂,q₃表示四元数预先定义的四个数，描述的基本依据是，对空间中的任意两个原点重合的笛卡尔坐标系，总存在一个过原点的轴使得一个坐标系绕轴旋转一定角度后与另一个坐标系重合；如式(3)所示，为四元数表示的旋转矩阵，与欧拉角表示的旋转矩阵相互转化；式(4)描述了四元数表示的姿态微分方程，该微分方程的元素不会因俯仰角等于90°而出现奇异；

F_Z＝(w_r1 ²+w_r2 ²+w_r3 ²+w_r4 ²) (5)

其中w_ri(i＝1,2,3,4)表示四旋翼无人机四个电机转动角速率，所述四个电机完全相同，升力系数K_f＝ρSr²C_Tstat，其中C_Tstat表示无量纲的升力系数、S表示旋翼面积、r表示旋翼半径、ρ表示空气密度；

S3)由式(2)(3)(4)(5)可知，合力F由无人机的本身系数求出，由式(1)知，整个刚体模型中，速度的变化率与速度、合力和角速度相关，所以采用无人机已知的系统参数与陀螺仪测得的角速度去预测速度，采用回声状态网络预测速度，采用回声状态网络预测速度的步骤如下：

S31)根据无量纲的升力系数C_Tstat与旋翼面积S、旋翼半径r、空气密度ρ求出升力系数K_f，再根据四个电机中每个电机的转动速率求出气动主动力，从而求出所需要的合力F；

S32)构建一个神经网络，其中W_in表示将外部输入传递到存储池当中，W表示内部状态的传递，W_out表示控制输出；存储池存储回声状态网络的内部状态向量x，内部状态向量x之间的联系由状态转移矩阵W决定；输入的状态数目总计为5个，分别为ω_x、ω_y、ω_z、F和m，输出的状态数目总计为3个，分别为v_x、v_y和v_z；回声状态网络的数学形式如式(6)和(7)所示，

x(n+1)＝αx(n)+βf(W_inu(n+1)+Wx(n)) (6)

其中，n表示时刻n，u表示维数为K的输入向量，x表示维数为N的内部状态向量，y表示维数为L的输出向量，W_in将输入与内部状态关联起来，W表示内部状态的传递，W_out表示控制输出矩阵，α和β均表示待定参数；

S33)在式(6)中有待定参数α和β，由式(8)可知只要确定了参数α即可确定参数β；对于输入矩阵选取为随机矩阵W_in，其值在-1到1之间；对于状态转移矩阵W，首选取一个随机矩阵W，’并计算该随机矩阵W’的奇异值，将矩阵W’除以奇异值得到状态转移矩阵W，对于输出矩阵W_out，将使用如下所描述的回声状态训练网络算法进行训练；

|α|＜1,且β＝1-|α| (8)

其中，回声状态训练网络算法采用适合于式(6)和(7)所确定回声状态训练网络的递推最小二乘算法，其包括如下步骤：

S331)随机生成值在-1到1矩阵W_in、W’和W_out，计算W’的奇异值σ_W，并将W’除以σ_W得到W；

S332)从训练样本库中获取一个样本，根据式(9)计算期望输出y_d(n)与当前网络输出y(n)的误差向量δ；

S333)选择适当的遗忘因子μ，根据式(10)递推估计输出矩阵W_out；

S334)重复进行步骤S332)和S333)，直至将训练样本库完整遍历一次；

定义误差向量δ

δ＝y_d(n)-y(n) (9)

递推最小二乘算法如下，当前时刻n修正后的权值将在n+1时刻使用，故记当前时刻修正后的输出矩阵为W_out(n+1)；

其中μ表示遗忘因子，且0.95<μ≤1，P_RLS的初值选为元素足够大的对角阵，L_RLS(n)表示一个中间变量。

2.根据权利要求1所述的基于速度预测的无人机鲁棒性导航方法，其特征在于，所述升力系数K_f＝6.20566×10^-6，待定参数α＝0.996，内部状态数目为N＝50。

Images