一种无人机导航控制方法及系统与流程

本发明属于无人机飞行控制，涉及无人机导航控制技术，具体为一种无人机导航控制方法及系统。

背景技术：

1、无人机航点飞行是一种让无人机按照用户预先规划好的路线自动飞行的智能模式，航点是用户预先在电子地图或实时图传画面上设定的一系列点，每个航点不仅仅是一个位置，还包含丰富的指令，比如飞行高度、速度、机头朝向、云台俯仰角度。因此，无人机航点飞行是工业级无人机开展巡检、测绘、物资配送等作业任务的主流飞行模式。

2、无人机航点飞行中的航点通常由悬停航点、巡航航点和盘旋航点构成，悬停航点用于目标精细化观测，巡航航点用于远距离快速通行，盘旋航点用于环绕式侦察。这三类航点对飞控底层控制律的约束条件存在显著差异，悬停工况要求位置环具备高刚度锁定性能，巡航工况要求速度前馈跟踪控制，盘旋工况则要求圆周轨迹跟踪引导。因此，无人机航点飞行中的航点一般是混合类型标识航点，包括悬停标识、巡航标识和盘旋标识。在无人机航点飞行的航线规划时要考虑混合类型标识航点。

3、传统的无人机航线规划主要采用b样条、最小加速度等全局轨迹重规划算法，全局轨迹重规划算法为保证轨迹曲率连续，会对用户预先设定的原始航点坐标进行微调，导致规划航点与实际到达位置存在系统性偏差，难以满足精准巡检与测绘作业的定位要求。

4、为了克服全局轨迹重规划算法难以满足精准巡检与测绘作业的定位要求的缺陷，采用上层任务调度架构进行航点规划，上层任务调度架构依赖mavlink协议对航点任务消息进行承载，上层任务调度器串行解析航点任务消息的命令标识后，再调用对应的底层控制接口将航点任务下发至飞控模型。但是在上层任务调度架构下，航点本身缺乏直接驱动底层控制律重构的能力，无法利用类型标识对速度规划和控制策略切换等多个模块进行并行驱动，严重制约了航点切换的响应速度与协同效率。因此，上层任务调度架构无法满足无人机航点飞行时，利用多类型标识控制具体动作的要求，导致航点切换存在响应速度慢以及无法进行速度前瞻性调节的技术问题。

技术实现思路

1、针对上述背景技术中所描述的利用上层任务调度架构进行无人机航点飞行时，存在响应速度慢以及无法进行速度前瞻性调节的技术问题，针对该技术问题，本发明提出了一种无人机导航控制方法及系统。

2、为了解决上述技术问题，本发明采用如下技术方案：

3、一种无人机导航控制方法，包括以下步骤：

4、构建无人机航线上的航点，航点的数据结构包括位置参数、类型标识以及与类型标识绑定的专属控制参数；

5、无人机飞控系统遍历无人机航线上的所有航点，根据航点的位置参数计算相邻航段间的转弯角度，并基于相邻航段间的转弯角度和当前航点的类型标识，为当前航段末端预先赋值目标速度，形成速度衔接规划表；

6、在当前航段飞行中，基于当前位置与当前航点之间的剩余距离、当前速度、当前航段末端目标速度和预设减速度判断无人机是否达到减速启动条件：

7、若否，则继续判断；

8、若是，则以预设减速度执行减速，直至速度锁定为所述当前航段末端目标速度，所述当前航段末端目标速度是根据速度衔接规划表确定的；

9、当无人机抵达当前航点时，获取当前航点对应的类型标识，基于当前航点对应的类型标识对当前航点进行控制策略切换；

10、将下一航点设为当前航点，循环执行上述步骤，直至所有航点执行完成、无人机飞行任务暂停或无人机飞行任务终止。

11、进一步限定，所述类型标识包括悬停、巡航和盘旋；

12、与所述悬停绑定的悬停专属控制参数包括悬停期望偏航角和悬停持续时间；

13、与所述巡航绑定的巡航专属控制参数包括巡航期望速度；

14、与所述盘旋绑定的盘旋专属控制参数包括盘旋半径、盘旋圈数、盘旋期望速度和盘旋方向。

15、进一步限定，所述无人机飞控系统遍历无人机航线上的所有航点，根据航点的位置参数计算相邻航段间的转弯角度，并基于相邻航段间的转弯角度和当前航点的类型标识，为当前航段末端预先赋值目标速度，形成速度衔接规划表具体包括：

16、无人机飞控系统遍历无人机航线上的所有航点，确定航点、航点和航点的位置参数；

17、基于航点、航点和航点的位置参数，计算航点与航点对应航段的方向角以及航点与航点对应航段的方向角；

18、基于方向角和方向角确定航点的转弯角度，根据航点的转弯角度确定航点的最大协调转弯速度；

19、根据当前航点的类型标识和最大协调转弯速度形成速度衔接规划表。

20、进一步限定，所述在当前航段飞行中，基于当前位置与当前航点之间的剩余距离、当前速度、当前航段末端目标速度和预设减速度判断无人机是否达到减速启动条件具体包括：

21、无人机在当前航段飞行中，无人机飞控系统实时获取无人机的当前位置和无人机的当前速度；

22、在每个控制周期，根据无人机的当前位置和无人机的当前航点位置计算剩余距离，并根据当前速度、当前航段末端目标速度和预设减速度计算制动距离；

23、基于剩余距离和制动距离建立减速启动条件，根据减速启动条件判断无人机是否达到减速条件：

24、若否，则继续判断；

25、若是，则以预设减速度执行减速，并在减速过程中继续按控制周期更新剩余距离与制动距离，动态调整减速过程，直至速度锁定为当前航段末端目标速度，所述当前航段末端目标速度是根据速度衔接规划表确定的。

26、进一步限定，所述当无人机抵达当前航点时，获取当前航点对应的类型标识，基于当前航点对应的类型标识对当前航点进行控制策略切换具体包括：

27、构建控制策略库并预先存储于无人机飞控系统的存储器中，所述控制策略库包括与类型标识绑定的专属控制参数；

28、当无人机抵达当前航点时，保存切换前当前活跃控制器的控制输出值至存储模块的预留存储区；

29、根据当前航点的类型标识，从所述控制策略库中以内存拷贝方式加载对应的专属控制参数至当前活跃控制器，以对当前航点进行控制策略切换；同时以所保存的切换前当前活跃控制器的控制输出值为目标，反解确定切换后当前活跃控制器的积分初值，以确保切换瞬间控制输出连续。

30、进一步限定，所述根据当前航点的类型标识，从所述控制策略库中以内存拷贝方式加载对应的专属控制参数至当前活跃控制器，以对当前航点进行控制策略切换具体包括：

31、若当前航点的类型标识为悬停，则将当前航点的控制策略切换为位置环锁定策略，目标位置锁定为当前航点的位置，从所述控制策略库中以内存拷贝方式将当前航点的悬停期望偏航角赋值给无人机控制系统的当前活跃控制器，并将悬停持续时间赋值给无人机控制系统的悬停计时器；

32、若当前航点的类型标识为巡航，则将当前航点的控制策略切换为速度环前馈策略，从所述控制策略库中以内存拷贝方式将当前航点的巡航期望速度赋值给无人机控制系统的当前活跃控制器；

33、若当前航点的类型标识为盘旋，则将当前航点的控制策略切换为圆周运动引导律策略，从所述控制策略库中以内存拷贝方式将当前航点的盘旋半径、盘旋圈数、盘旋期望速度和盘旋方向赋值给无人机控制系统的当前活跃控制器。

34、进一步限定，所述对当前航点进行控制策略切换还包括盘旋退出航向对准：

35、若当前航点对应的类型标识为盘旋，则在盘旋圈数完成后，利用盘旋固有的圆周运动引导律策略使无人机航向调整至与下一航段入航方向对齐，再触发切换退出盘旋并进入下一航段。

36、进一步限定，所述盘旋退出航向对准具体包括：

37、若当前航点对应的类型标识为盘旋，则在盘旋圈数完成后，利用盘旋固有的圆周运动引导律策略使无人机保持圆周运动，圆周运动过程中实时监测无人机的当前航向角、无人机的当前角速度、无人机的当前角加速度和无人机的当前盘旋速度；

38、基于无人机的当前航向角、无人机的当前角速度及无人机的当前角加速度建立航向角预测模型；

39、基于无人机的当前盘旋速度和下一航段的期望速度计算速度过渡时间；

40、将所述速度过渡时间代入所述航向角预测模型中，计算经过速度过渡时间后的航向角，并判断航向角与下一航段入航方向角的偏差是否小于或等于预设航向角阈值：

41、若是，则无人机航向与下一航段入航方向对齐，在速度过渡时间届满时触发切换退出盘旋并进入下一航段；

42、若否，则以预设航向角阈值为边界条件，将下一航段入航方向角与预设航向角阈值确定的边界航向角代入航向角预测模型中，计算满足偏差角小于或等于预设航向角阈值且不小于速度过渡时间的最小时间解，在最小时间解的时刻触发切换退出盘旋并进入下一航段。

43、进一步限定，所述无人机导航控制方法还包括监测异常：监测循环执行过程中是否存在航点跳变异常：

44、若否，则继续监测；

45、若是，则暂停无人机飞行任务，并改变类型标识为悬停，将控制策略切换为位置环锁定策略。

46、与上述无人机导航控制方法对应的无人机导航控制系统，包括：

47、航点构建模块：用于构建无人机航线上的航点，航点的数据结构包括位置参数、类型标识以及与类型标识绑定的专属控制参数；

48、航点解析模块：用于无人机飞控系统遍历无人机航线上的所有航点，根据航点的位置参数计算相邻航段间的转弯角度，并基于相邻航段间的转弯角度和当前航点的类型标识，为当前航段末端预先赋值目标速度，形成速度衔接规划表；

49、速度调节模块：用于在当前航段飞行中，基于当前位置与当前航点之间的剩余距离、当前速度、当前航段末端目标速度和预设减速度判断无人机是否达到减速启动条件：

50、若否，则继续判断；

51、若是，则以预设减速度执行减速，直至速度锁定为所述当前航段末端目标速度，所述当前航段末端目标速度是根据速度衔接规划表确定的；

52、切换策略模块：用于当无人机抵达当前航点时，获取当前航点对应的类型标识，基于当前航点对应的类型标识对当前航点进行控制策略切换；

53、以及循环执行模块：用于将下一航点设为当前航点，循环执行速度调节模块与切换策略模块，直至所有航点执行完成、无人机飞行任务暂停或无人机飞行任务终止。

54、与现有技术相比，本发明的有益效果在于：

55、1、本发明无人机导航控制方法，其在无人机飞控系统解析航点后形成速度衔接规划表，接着根据减速启动条件执行减速，当无人机的速度降至当前航段末端目标速度时，将该速度锁定为目标速度，从而实现了无人机在接近当前航点时主动减速，改善了传统技术缺少前置速度规划、航点邻域急减速带来的速度突变问题，使速度过渡更加平顺。利用类型标识为当前航段末端预先赋值目标速度，同时利用类型标识对当前航点进行控制策略切换，实现了利用类型标识对速度规划和无人机飞控系统的控制策略进行并行驱动，确保切换的响应速度与协同效率，从而克服了现有上层任务调度架构下无法进行速度前瞻性调节的技术问题，并有效缓解了切换响应滞后的缺陷。

56、2、本发明无人机导航控制方法，其在策略切换时，保存切换前活跃控制器的控制输出，并以该控制输出为目标反解确定切换后活跃控制器的积分初值，确保切换瞬间控制输出连续，避免了传统策略切换方式中因异构控制律间积分项不匹配而导致的控制量跳变和飞行姿态突变。

57、3、本发明还设置了控制策略库，在策略切换时，根据当前航点的类型标识从控制策略库中以内存拷贝方式加载当前航点的类型标识绑定的专属控制参数赋值给无人机控制系统的当前活跃控制器，无需动态内存分配，参数加载耗时确定且远小于控制周期，进一步提升异构控制律间切换过程的平滑性与连续性。

58、4、本发明无人机导航控制方法，其在当前航点对应的类型标识为盘旋时，在切换至下一航点时需要进行航向对准，即形成航向对准机制，利用盘旋阶段固有的圆周运动引导律特性，使无人机在完成盘旋圈数后的持续圆周运动中实时进行航向预测与主动对准，实现从圆周运动到直线运动的顺滑衔接与过渡，使得盘旋退出瞬间的控制量变化平稳，避免了传统技术中因缺乏航向预对准而在退出时需同时进行航向修正与速度调整所导致的控制量突变和姿态振荡。

59、5、本发明在各控制周期内的计算均为闭式计算：剩余距离与制动距离的计算合计约13次浮点运算，航向角预测与偏差角计算约10次浮点运算，速度过渡时间的计算为2次浮点运算，最小时间解的闭式求根不超过20次浮点运算，单控制周期合计不超过80次浮点乘加，且运算量与航线长度、航点数量无关；以主频168mhz且含硬件浮点运算单元的cortex-m4处理器为例，上述计算耗时约1μs至2μs，仅占2ms控制周期的0.1%以下，适于在低成本嵌入式mcu上实时运行。