[发明专利]基于波浪评估的多旋翼无人飞船水面起飞控制方法

权利要求书

1.一种基于波浪评估的多旋翼无人飞船水面起飞控制方法，用于控制多旋翼无人飞船，其特征在于，所述控制方法包括以下步骤：

S1、根据多旋翼无人飞船的状态估计波浪特征；

其中，所述多旋翼无人飞船状态包括：多旋翼无人飞船的竖向加速度、多旋翼无人飞船的竖向速度及多旋翼无人飞船的竖向位移；所述波浪特征包括：波浪的波峰、波浪的波谷及波浪波峰的均值；

S2、收到遥控端发送的起飞指令后，根据估计的波浪特征，选择合适的开始起飞时刻；

S3、以预定的起飞方式控制多旋翼无人飞船起飞；

其中，所述步骤S1中，根据多旋翼无人飞船的状态估计波浪特征的过程如下：

S101、获取多旋翼无人飞船沿机体坐标系三个坐标轴方向运动的加速度及多旋翼无人飞船绕机体坐标系三个坐标轴旋转的角速度；

其中，所述机体坐标系是与多旋翼无人飞船固定连接，没有相对运动的坐标系；机体坐标系的坐标原点为多旋翼无人飞船的重心，机体坐标系的X_b、Y_b、Z_b三轴分别指向多旋翼无人飞船的前、左、上三个方向，其中下标b为body的缩写，代表机体坐标系；

所述多旋翼无人飞船沿机体坐标系X_b、Y_b、Z_b三个坐标轴方向运动的加速度由多旋翼无人飞船搭载的加速度传感器测量获得，所述多旋翼无人飞船绕机体坐标系X_b、Y_b、Z_b三个坐标轴旋转的角速度由多旋翼无人飞船搭载的陀螺仪测量获得；

S102、利用多旋翼无人飞船绕机体坐标系三个坐标轴旋转的角速度将多旋翼无人飞船沿机体坐标系三个坐标轴运动的加速度投影至大地坐标系中；

其中，所述大地坐标系是指一个三轴指向固定、为多旋翼无人飞船提供方位参考的坐标系，令大地坐标系的X_e、Y_e、Z_e三轴分别指向地理的北、东、地或地理的东、北、天，其中下标e为earth的缩写，代表大地坐标系；

用一个旋转矩阵来表示机体坐标系到大地坐标系的旋转关系，即：

利用四元数Q来表示旋转矩阵用一单位四元数：

表示绕矢量v旋转θ角的这样一个旋转，式中q₀为四元数Q的实部，q₁、q₂、q₃为四元数Q的虚部系数，四元数Q与旋转矩阵的对应关系为：

利用步骤S101中获得的绕X_b、Y_b、Z_b轴旋转的角速度ω_xb、ω_yb、ω_zb对四元数Q＝q₀+q₁i+q₂j+q₃k的四个系数q₀、q₁、q₂、q₃进行迭代更新，以获得最新的旋转矩阵更新公式为：

其中dt为两次更新的间隔时间，q₀、q₁、q₂、q₃为上一次的四元数系数，q′₀、q′₁、q′₂、q′₃为更新后的四元数系数，q₀、q₁、q₂、q₃的初始值分别为：q₀＝1、q₁＝0、q₂＝0、q₃＝0；

求出大地坐标系下多旋翼无人飞船沿X_e、Y_e、Z_e三轴运动的加速度a_xe、a_ye、a_ze：

S103、将大地坐标系Z_e轴方向的加速度经过高通滤波后得到多旋翼无人飞船的竖向加速度，过程如下：

对测量值a_ze进行高通滤波得到a_zef，a_zef即为多旋翼无人飞船的竖向加速度；

S104、将多旋翼无人飞船的竖向加速度经过积分、高通滤波获得多旋翼无人飞船的竖向速度，过程如下：

对a_zef积分得到v_ze，对v_ze高通滤波得到v_zef，v_zef即为所述多旋翼无人飞船的竖向速度；

S105、将多旋翼无人飞船的竖向速度经过积分、高通滤波获得多旋翼无人飞船的竖向位移，过程如下：

对v_zef积分得到s_ze；对s_ze高通滤波得到s_zef，s_zef即为所述多旋翼无人飞船的竖向位移；

其中，当多旋翼无人飞船位于水平面时，所述多旋翼无人飞船的竖向位移等于零，当多旋翼无人飞船位于水平面之上时，所述多旋翼无人飞船的竖向位移大于零，当多旋翼无人飞船位于水平面之下时，所述多旋翼无人飞船的竖向位移小于零；

S106、根据多旋翼无人飞船的竖向速度的过零点及过零点前后两点的多旋翼无人飞船的竖向速度的大小关系来估计波浪的波峰和波谷；

S107、计算波浪波峰均值，该波浪波峰均值用来衡量多旋翼无人飞船附近水域波浪幅度的平均大小，计算公式如下：

式中，n代表当前为第n个点，peak_n为第n个波峰，mean_n-1为前n-1个波峰的均值，mean_n为n个波峰的均值；

其中，所述步骤S2中，收到遥控端发送的起飞指令后，根据估计的波浪特征，选择合适的开始起飞时刻的过程如下：

S201、未选择到合适的开始起飞时刻前，多旋翼无人飞船所有旋翼以预设转速旋转；

其中，所述预设转速小于多旋翼无人飞船在空中悬停时的转速；所述悬停时的转速为能使多旋翼无人飞船在空中悬停的一个旋翼转速的估计值，理想情况下，在悬停时多旋翼无人飞船的竖向速度为零；

S202、判断当前多旋翼无人飞船的竖向位移是否大于波浪波峰均值，若大于则进行步骤S203，否则重复步骤S202；

S203、若多旋翼无人飞船的横滚角和俯仰角均小于设定的阈值，则选择当前时刻为开始起飞时刻，否则重复步骤S202；

其中，所述多旋翼无人飞船的横滚角为多旋翼无人飞船的机体坐标系的Y_b轴与大地坐标系X_eY_e平面的夹角；所述多旋翼无人飞船的俯仰角为多旋翼无人飞船的机体坐标系X_b轴与大地坐标系X_eY_e平面的夹角；

其中，所述多旋翼无人飞船的横滚角和俯仰角由多旋翼无人飞船绕机体坐标系X_b、Y_b、Z_b三个坐标轴旋转的角速度经过计算得到。