技术领域
[0001] 本
发明涉及无人机领域，具体是指一种基于三种探鱼模式飞行控制算法的智能钓鱼无人机方法。
背景技术
[0002] 目前将无人机应用在人们生产和生活的各个领域已成为主要趋势；将无人机与传统的钓鱼尤其时海钓活动结合能使钓鱼更加智能化和专业化，同时使得钓鱼人能够更加方便智能的监测钓鱼活动，民用消费类四旋翼无人机已广泛的应用于航拍和测绘等领域，因其价格低廉，携带方便，操控方便已成为很多消费玩家的首选，其缺点是
载荷能
力有限，在海钓过程中不易携带偏重的钓饵和钓钩。
[0003] 相对较大型的工业级无人机则主要用于电力巡检，
农药喷洒，农田植保以及警用消防等领域，这类无人机的特点是可携带较重的载荷，续航时间相对长，
稳定性好，但是体积大，携带不方便，而且价格昂贵，并不适用于大量的钓鱼人长期使用。
[0004] 因此，设计出一种基于三种探鱼模式飞行控制算法的智能钓鱼无人机方法势在必行。
发明内容
[0005] 本发明要解决的技术问题是目前的民用消费类四旋翼无人机载荷能力有限，在海钓过程中不易携带偏重的钓饵和钓钩，较大型的工业级无人
机体积大，携带不方便，而且价格昂贵，并不适用于大量的钓鱼人长期使用，且目前缺少一种适用于钓鱼无人机使用的控制算法。
[0006] 为解决上述技术问题，本发明提供的技术方案为：一种基于三种探鱼模式飞行控制算法的智能钓鱼无人机方法，包括以下步骤：
[0007] S1、建立钓鱼无人机的数学模型；
[0008] S2、设计钓鱼无人机的
电机推力控制策略和控制分配方法；
[0009] S3、设计钓鱼无人机的串级闭环PID飞行控制方法；
[0010] S4、规划三种不同钓鱼无人机的探鱼路线并进行智能抛钩抛饵，探鱼模式包括多点探鱼、快速探鱼和区域探鱼三种。
[0011] 本发明与
现有技术相比的优点在于：解决现有消费应用类无人机中存在的不足，提供一种基于不同探鱼模式的飞行控制算法的智能钓鱼无人机方法，所述方法将串级闭环PID控制算法用于无人机的海上探鱼，采用
位置控制和
姿态控制的方法对无人机的探鱼路线进行实时计算和修正，保证钓鱼无人机沿着既定的探鱼线路飞行，进行实时监测，采用串级闭环PID控制算法，实现钓鱼无人机的位置、高度和姿态控制，同时通过位置和高度控制方法实现钓鱼无人机在不同探鱼模式下的实时精准探鱼。
[0012] 作为改进，S1步骤在钓鱼无人机的机体
坐标系和惯性坐标系下，建立无人机的位置和姿态运动学和动力学方程，具体包括：(1)钓鱼无人机的位置运动学方程；
[0013] 钓鱼无人机
重心在惯性系下坐标为[xio,yio,zio]，无人机重心初始时刻在惯性系下的坐标为 [xio,yio,zio]，在惯性系和机体系下的速度分量分别为[ui,vi,wi]和[u,v,w]，无人机飞行过程中的
滚转角、
俯仰角和
偏航角分别为 则无人机的位置运动学方程为：
[0014]
[0015] 其中，Rbi为钓鱼无人机惯性系到机体系的坐标转换矩阵，t0,t分别表示钓鱼无人机飞行过程中的初始时刻和当前时刻；
[0016] 钓鱼无人机的姿态运动学方程；
[0017] 钓鱼无人机在惯性系和机体系下的姿态运动学方程为：其中， 为无人机机体系相对惯性系的姿态
角变化率，[ωxb,ωyb,ωzb]为无人机机体系相对惯性系在机体系下的
角速度分量；
[0018] 钓鱼无人机的位置动力学方程；
[0019] 钓鱼无人机的位置动力学方程包括高度和
水平控
制动力学方程，具体的动力学方程形式如下：
[0020] 其中，Fb为四个电机在机体系下的总升力，沿obzb的负方向；F1b,F2b,F3b,F4b为四个电机各自产生的在机体系下的升力，沿obzb的负方向；p,q,r为飞机在机体系下的沿obxb,obyb,obzb方向的角速度，分别为滚转角速度，俯仰角速度和偏航角速度；Ω1,Ω2,Ω3,Ω4为1,2,3,4号电机的转速，单位为rpm(转每分)；g为重力
加速度，单位为 m/s2；Rbi为惯性系到机体系的转换矩阵；m为钓鱼无人机的
质量； 为钓鱼无人机在机体系下各轴速度变化量；
[0021] 钓鱼无人机的姿态动力学方程；
[0022] 钓鱼无人机的在滚转、俯仰和偏航三个方向的姿态动力学方程如下所示：
[0023] 其中，[Mx,My,Mz]为钓鱼无人机所受的滚转、俯仰和偏航力矩；[F1,F2,F3,F4]分别为1,2,3,4号电机和螺旋桨产生的向上的升力； [Ixx,Iyy,Izz]分别为钓鱼无人机的机体系下各个方向的
转动惯量；Ixz为钓鱼无人机机体系下obxbzb平面惯性积；[x1,y1]为1号电机在机体系中的水平坐标；[-x2,-y2]为2号电机在机体系中的水平坐标；
[0024] 作为改进，S2步骤中钓鱼无人机的四个电机升力具体控制分配策略和控制方法如下：
[0025] 钓鱼无人机的四个电机和螺旋桨产生的升力方程：
[0026] 其中，KT为电机和螺旋桨综合的升力系数；F为四个电机产生的总升力；
[0027] 钓鱼无人机的四个电机和螺旋桨产生的力矩方程：
[0028] 其中，Jr为每个螺旋桨自身的转动惯量；KM为电机和螺旋桨的反
扭矩系数；
[0029] 钓鱼无人机的四个电机和螺旋桨的控制分配方程：
[0030] 其中，U1(N)为高度控制回路输出的高度控制量；U2(N·m)为滚转控制回路输出的滚转控制量；U3(N·m)为俯仰控制回路输出的俯仰控制量；U4(N ·m)为航向控制回路输出的偏航控制量；
[0031] 作为改进，根据所建的钓鱼无人机数学模型和电机控制分配策略，设计钓鱼无人机的串级闭环PID飞行控制算法，具体方法如下：
[0032] 钓鱼无人机的高度控制回路；
[0033] 钓鱼无人机的高度控制回路包含内环垂直速度控制和外环高度回路控制，具体控制
框图如
附图2所示，计算公式为：
[0034]
[0035] 其中，U1为高度控制回路输出的高度控制量；Hc,H(m)：惯性系下的期望高度和实际高度；Kph,Kih,Kdh分别为高度控制回路的比例、积分和微分系数；wc(m/s)为机体系下垂直期望速度；w(m/s)为机体系下飞机实际垂直速度；Kpw为垂直速度控制回路的比例系数； (m/s2)为惯性系下期望垂直加速度；
[0036] 钓鱼无人机的姿态控制回路；
[0037] 钓鱼无人机的姿态控制回路包含三个通道(滚转、俯仰和偏航)的内环姿态角速度控制回路和外环姿态角控制回路，具体方程：
[0038] 其中，U2,U3,U4分别为智能钓鱼无人机的滚转、俯仰和偏航控制量；Kpp,Kpq,Kpr分别为滚转、俯仰和偏航通道的内环角速度控制回路的比例系数； 分别为滚转、俯仰和偏航通道的
外环姿态角控制回路的比例系数； 分别为滚转、俯仰和偏航通道的外环姿态角
控制回路的积分系数； 分别为滚转、俯仰和偏航通道的外环姿态角控制回路的
微分系数； 分别为目标滚转角、目标俯仰角和目标偏航角； 分别为实际滚转
角、实际俯仰角和实际偏航角；
[0039] (3)钓鱼无人机的位置控制回路；
[0040] 钓鱼无人机的位置控制回路包含水平北向位置控制回路和东向位置控制回路方程，水平位置控制回路又包含内环水平速度控制回路和外环水平位置控制回路，钓鱼无人机在机体系的obzb轴上产生的升力在惯性系下的水平oixi和oiyi轴上产生的升力为：
[0041] 其中，[xi,yi]为钓鱼无人机实际飞行中在惯性系下输出的水平北向和东向位移量，由于钓鱼无人机在水平方向的滚转和俯仰的姿态变化很小，故可近似认为滚转角 和俯仰角θ均为小量，则上述方程可简化为：
[0042]
[0043] 钓鱼无人机在机体系下的水平速度u和v转换到惯性系下为：
[0044] 其中，[ui,vi]为钓鱼无人机在惯性系下的水平北向和东向速度分量；
[0045] 钓鱼无人机的水平北向和东向位置控制回路方程为：
[0046] 其中， 分别为钓鱼无人机在惯性系下的水平北向和东向速度分量的变化量；[Kpx,Kpy]分别为水平北向和东向位置控制回路的比例系数；[Kix,Kiy]分别为水平北向和东向位置控制回路的积分系数；[Kdx,Kdy]分别为水平北向和东向位置控制回路的微分系数；[Kpu,Kpv]分别为水平北向和东向内环速度控制回路的比例系数；
[0047] 作为改进，S4步骤根据设计的钓鱼无人机的串级闭环PID飞行控制算法，规划钓鱼无人机的三种探鱼模式路线，具体如下：
[0048] (1)多点探鱼模式的高度和水平位置航迹规划；
[0049] 多点探鱼模式下，钓鱼无人机的高度先上升，飞行一段时间后，再降落到一定的高度，再次飞行一段时间，然后上升到之前的高度，飞行一段时间后到达某个点，再次降落，循环往复，钓鱼无人机的水平北向位移轨迹为一条直线，在无人机上升或下降过程中北向位置保持恒定，钓鱼无人机的水平东向位移轨迹初始时刻会有略微变化，待无人机运动到相应的位置点后不再东向位移轨迹不再变化；
[0050] (2)快速探鱼模式的高度和水平位置航迹规划；
[0051] 快速探鱼模式下，钓鱼无人机的期望高度先直线上升，待飞行到目标点后，开始降落，然后进行探鱼任务，直至探鱼任务结束，高度上升，然后返回的初始点，钓鱼无人机先飞行到既定的目标高度后，在水平方向做圆环运动，执行快速探鱼任务，水平北向和东向位置不断变化；
[0052] 区域探鱼模式的高度和水平位置航迹规划；
[0053] 区域探鱼模式下，钓鱼无人机的期望高度变化情况与快速探鱼完全一致，钓鱼无人机先飞行到既定的目标高度后，在水平方向不断前进，并来回的扫描，执行区域探鱼任务，水平北向位置不断增大，水平东向位置来回变化。
附图说明
[0054] 图1是本发明的整体控制框图和控制策略。
[0055] 图2是本发明的高度控制回路原理框图。
[0056] 图3是本发明的滚转姿态角控制回路原理框图。
[0057] 图4是本发明的俯仰姿态角控制回路原理框图。
[0058] 图5是本发明的偏航姿态角控制回路原理框图。
[0059] 图6是本发明的水平位置控制回路原理框图。
[0060] 图7是本发明的水平速度控制回路原理框图。
[0061] 图8是本发明的高度变化情况。
[0062] 图9是本发明的水平北向位移轨迹变化情况。
[0063] 图10是本发明的水平东向位移轨迹变化情况。
[0064] 图11是本发明的快速区域探鱼模式下的高度变化情况。
[0065] 图12是本发明的快速探鱼模式下的水平北向位移轨迹变化情况。
[0066] 图13是本发明的快速探鱼模式下的水平东向位移轨迹变化情况。
[0067] 图14是本发明的区域探鱼模式下的水平北向位移轨迹变化情况。
[0068] 图15是本发明的区域探鱼模式下的水平东向位移轨迹变化情况。
[0069] 图16是本发明的多点探鱼模式的期望和实际高度对比图。
[0070] 图17是本发明的多点探鱼模式的水平期望和实际轨迹对比图。
[0071] 图18是本发明的快速探鱼模式的期望和实际高度对比图。
[0072] 图19是本发明的快速探鱼模式的水平期望和实际轨迹对比图。
[0073] 图20是本发明的区域探鱼模式的期望和实际高度对比图。
[0074] 图21是本发明的区域探鱼模式的水平期望和实际轨迹对比图。
具体实施方式
[0075] 下面结合附图对本发明做进一步的详细说明。
[0076] 本发明在具体实施时，一种基于三种探鱼模式飞行控制算法的智能钓鱼无人机方法，包括以下步骤：
[0077] S1、建立钓鱼无人机的数学模型；
[0078] S2、设计钓鱼无人机的电机推力控制策略和控制分配方法；
[0079] S3、设计钓鱼无人机的串级闭环PID飞行控制方法；
[0080] S4、规划三种不同钓鱼无人机的探鱼路线并进行智能抛钩抛饵，探鱼模式包括多点探鱼、快速探鱼和区域探鱼三种。
[0081] 所述的S1步骤在钓鱼无人机的机体坐标系和惯性坐标系下，建立无人机的位置和姿态运动学和动力学方程，具体包括：(1)钓鱼无人机的位置运动学方程；
[0082] 钓鱼无人机重心在惯性系下坐标为[xio,yio,zio]，无人机重心初始时刻在惯性系下的坐标为 [xio,yio,zio]，在惯性系和机体系下的速度分量分别为[ui,vi,wi]和[u,v,w]，无人机飞行过程中的滚转角、俯仰角和偏航角分别为 则无人机的位置运动学方程为：
[0083]
[0084] 其中，Rbi为钓鱼无人机惯性系到机体系的坐标转换矩阵，t0,t分别表示钓鱼无人机飞行过程中的初始时刻和当前时刻；
[0085] (2)钓鱼无人机的姿态运动学方程；
[0086] 钓鱼无人机在惯性系和机体系下的姿态运动学方程为：其中， 为无人机机体系相对惯性系的姿态
角变化率，[ωxb,ωyb,ωzb]为无人机机体系相对惯性系在机体系下的角速度分量；
[0087] (3)钓鱼无人机的位置动力学方程；
[0088] 钓鱼无人机的位置动力学方程包括高度和水平控制动力学方程，具体的动力学方程形式如下：
[0089] 其中，Fb为四个电机在机体系下的总升力，沿obzb的负方向；F1b,F2b,F3b,F4b为四个电机各自产生的在机体系下的升力，沿obzb的负方向；p,q,r为飞机在机体系下的沿obxb,obyb,obzb方向的角速度，分别为滚转角速度，俯仰角速度和偏航角速度；Ω1,Ω2,Ω3,Ω4为1,2,3,4号电机的转速，单位为rpm(转每分)；g为
重力加速度，单位为m/s2；Rbi为惯性系到机体系的转换矩阵；m为钓鱼无人机的质量； 为钓鱼无人机在机体系下各轴速度变化量；
[0090] (4)钓鱼无人机的姿态动力学方程；
[0091] 钓鱼无人机的在滚转、俯仰和偏航三个方向的姿态动力学方程如下所示：
[0092] 其中，[Mx,My,Mz]为钓鱼无人机所受的滚转、俯仰和偏航力矩；[F1,F2,F3,F4]分别为1,2,3,4号电机和螺旋桨产生的向上的升力； [Ixx,Iyy,Izz]分别为钓鱼无人机的机体系下各个方向的转动惯量；Ixz为钓鱼无人机机体系下obxbzb平面惯性积；[x1,y1]为1号电机在机体系中的水平坐标；[-x2,-y2]为2号电机在机体系中的水平坐标；
[0093] 所述的S2步骤中钓鱼无人机的四个电机升力具体控制分配策略和控制方法如下：
[0094] (1)钓鱼无人机的四个电机和螺旋桨产生的升力方程：
[0095] 其中，KT为电机和螺旋桨综合的升力系数；F为四个电机产生的总升力；
[0096] (2)钓鱼无人机的四个电机和螺旋桨产生的力矩方程：
[0097] 其中，Jr为每个螺旋桨自身的转动惯量；KM为电机和螺旋桨的反扭矩系数；
[0098] (3)钓鱼无人机的四个电机和螺旋桨的控制分配方程：
[0099] 其中，U1(N)为高度控制回路输出的高度控制量； U2(N·m)为滚转控制回路输出的滚转控制量；U3(N·m)为俯仰控制回路输出的俯仰控制量；U4(N ·m)为航向控制回路输出的偏航控制量；
[0100] 根据所建的钓鱼无人机数学模型和电机控制分配策略，设计钓鱼无人机的串级闭环PID飞行控制算法，具体方法如下：
[0101] (1)钓鱼无人机的高度控制回路；
[0102] 钓鱼无人机的高度控制回路包含内环垂直速度控制和外环高度回路控制，具体控制框图如附图2所示，计算公式为：
[0103]
[0104] 其中，U1为高度控制回路输出的高度控制量；Hc,H(m)：惯性系下的期望高度和实际高度；Kph,Kih,Kdh分别为高度控制回路的比例、积分和微分系数；wc(m/s)为机体系下垂直期望速度；w(m/s)为机体系下飞机实际垂直速度；Kpw为垂直速度控制回路的比例系数； (m/s2)为惯性系下期望垂直加速度；
[0105] (2)钓鱼无人机的姿态控制回路；
[0106] 钓鱼无人机的姿态控制回路包含三个通道(滚转、俯仰和偏航)的内环姿态角速度控制回路和外环姿态角控制回路，具体方程：
[0107] 其中，U2,U3,U4分别为智能钓鱼无人机的滚转、俯仰和偏航控制量；Kpp,Kpq,Kpr分别为滚转、俯仰和偏航通道的内环角速度控制回路的比例系数； 分别为滚转、俯仰和偏航通道的外环姿态角控制回路的
比例系数； 分别为滚转、俯仰和偏航通道的外环姿态角控制回路的积分系数；
分别为滚转、俯仰和偏航通道的外环姿态角控制回路的微分系数； 分
别为目标滚转角、目标俯仰角和目标偏航角； 分别为实际滚转角、实际俯仰角和实际偏航角；
[0108] (3)钓鱼无人机的位置控制回路；
[0109] 钓鱼无人机的位置控制回路包含水平北向位置控制回路和东向位置控制回路方程，水平位置控制回路又包含内环水平速度控制回路和外环水平位置控制回路，钓鱼无人机在机体系的obzb轴上产生的升力在惯性系下的水平oixi和oiyi轴上产生的升力为：
[0110] 其中，[xi,yi]为钓鱼无人机实际飞行中在惯性系下输出的水平北向和东向位移量，由于钓鱼无人机在水平方向的滚转和俯仰的姿态变化很小，故可近似认为滚转角 和俯仰角θ均为小量，则上述方程可简化为：
[0111]
[0112] 钓鱼无人机在机体系下的水平速度u和v转换到惯性系下为：
[0113] 其中，[ui,vi]为钓鱼无人机在惯性系下的水平北向和东向速度分量；
[0114] 钓鱼无人机的水平北向和东向位置控制回路方程为：
[0115] 其中， 分别为钓鱼无人机在惯性系下的水平北向和东向速度分量的变化量；[Kpx,Kpy]分别为水平北向和东向位置控制回路的比例系数；[Kix,Kiy]分别为水平北向和东向位置控制回路的积分系数；[Kdx,Kdy]分别为水平北向和东向位置控制回路的微分系数；[Kpu,Kpv]分别为水平北向和东向内环速度控制回路的比例系数；
[0116] 所述的S4步骤根据设计的钓鱼无人机的串级闭环PID飞行控制算法，规划钓鱼无人机的三种探鱼模式路线，具体如下：
[0117] (1)多点探鱼模式的高度和水平位置航迹规划；
[0118] 多点探鱼模式下，钓鱼无人机的高度先上升，飞行一段时间后，再降落到一定的高度，再次飞行一段时间，然后上升到之前的高度，飞行一段时间后到达某个点，再次降落，循环往复，钓鱼无人机的水平北向位移轨迹为一条直线，在无人机上升或下降过程中北向位置保持恒定，钓鱼无人机的水平东向位移轨迹初始时刻会有略微变化，待无人机运动到相应的位置点后不再东向位移轨迹不再变化；
[0119] (2)快速探鱼模式的高度和水平位置航迹规划；
[0120] 快速探鱼模式下，钓鱼无人机的期望高度先直线上升，待飞行到目标点后，开始降落，然后进行探鱼任务，直至探鱼任务结束，高度上升，然后返回的初始点，钓鱼无人机先飞行到既定的目标高度后，在水平方向做圆环运动，执行快速探鱼任务，水平北向和东向位置不断变化；
[0121] 区域探鱼模式的高度和水平位置航迹规划；
[0122] 区域探鱼模式下，钓鱼无人机的期望高度变化情况与快速探鱼完全一致，钓鱼无人机先飞行到既定的目标高度后，在水平方向不断前进，并来回的扫描，执行区域探鱼任务，水平北向位置不断增大，水平东向位置来回变化。
[0123] 本发明的工作原理：
[0125] 多点探鱼，快速探鱼和区域探鱼模式下的期望高度、水平北向及东向期望位移单位均为m，时间量的单位均为s；
[0126] 多点探鱼模式下智能钓鱼无人机的期望高度和水平北向及东向期望位移变化情况如下所示：
[0127]
[0128]
[0129]
[0130] 快速探鱼模式下智能钓鱼无人机的期望高度和水平北向及东向期望位移变化情况如下所示：
[0131]
[0132]
[0133]
[0134] 区域探鱼模式下智能钓鱼无人机的期望高度和水平北向及东向期望位移变化情况如下所示：
[0135]
[0136]
[0137]
[0138] 智能钓鱼无人机通过高度、位置和姿态控制回路来实现多点探鱼，快速探鱼和区域探鱼任务，水平方向的期望探鱼轨迹和实际探鱼轨迹的对比简图如附图9～14所示；
[0139] 由附图可知，智能钓鱼无人机通过串级PID控制算法能够实现多点探鱼，快速探鱼和区域探鱼三种不同模式下的探鱼任务，期望探鱼轨迹和实际探鱼轨迹基本吻合，满足探鱼要求。
[0140] 此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征，在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具本的限定。
[0141] 在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
[0142] 在本发明中，除非另有明确的规定和限定，第一特征在第二特征之“上”或之“下”可以包括第一和第二特征直接
接触，也可以包括第一和第二特征不是直接接触而是通过它们之间的另外的特征接触。而且，第一特征在第二特征“之上”、“上方”和“上面”包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度小于第二特征。
[0143] 在本
说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”，“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0144] 尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、
修改、替换和变型。
