一种四旋翼无人机动态避障轨迹生成方法及自主导航装置

摘要: 本发明涉及四旋翼无人机自主导航控制,具体涉及一种一种面向动态环境的无人机动态避障轨迹生成方法及装置,适用于室内、巷道、仓储、应急救援等包含动态障碍物且gps信号...
一种四旋翼无人机动态避障轨迹生成方法及自主导航装置

本发明涉及四旋翼无人机自主导航控制,具体涉及一种一种面向动态环境的无人机动态避障轨迹生成方法及装置,适用于室内、巷道、仓储、应急救援等包含动态障碍物且gps信号受限的复杂场景。

背景技术:

1、随着工业自动化、城市安防、应急救援、仓储物流及室内巡检等领域的快速发展,四旋翼无人机凭借结构紧凑、机动性强、起降灵活、成本低廉等优势,在室内、地下管廊、城市巷道、工业园区等复杂场景中得到广泛应用。在上述场景中,无人机作业环境普遍存在gps信号缺失、空间狭窄、障碍物分布密集、人员与移动设备频繁运动等特点,对无人机的自主感知、环境建模、实时规划与动态避障能力提出了极高要求。

2、(一)动态障碍物感知方面的不足

3、现有技术主要分为基于视觉的方法与基于点云的方法。基于深度图像的udepth等方法计算量轻,但在低纹理、弱光照、物体边缘模糊及遮挡场景下容易出现检测偏差,边界框精度较低;基于点云聚类的dbscan等方法对噪声敏感,在复杂结构环境中易产生误分割与漏检。部分方案引入深度学习目标检测网络,虽然在公开数据集上精度较高,但依赖大规模标注数据,模型参数量大、推理耗时高,难以在嵌入式机载平台实现实时运行。同时,多数检测方法仅输出静态目标框,缺乏连续跟踪与运动状态估计能力,无法为轨迹规划提供速度、加速度等关键信息,导致动态避障滞后、安全性不足。

4、(二)环境建图方面的不足

5、传统占据栅格地图与体素地图均基于环境静态假设构建,无法区分静态结构与动态障碍物。当场景中出现行人、移动机器人等动态目标时,其运动轨迹会在地图中形成持续残留的伪影,造成地图污染,使环境模型与真实场景产生偏差,直接误导路径规划,显著提升碰撞风险。尽管部分研究提出动态占据地图与动态目标过滤方法,但大多计算复杂度高、内存占用大,难以在资源受限平台部署;同时,动态区域清理机制不完善,易出现清理不彻底、过度清理、静态结构被破坏等问题,无法兼顾静态环境建模精度与动态区域时效性。

6、(三)轨迹规划方面的不足

7、传统方法如a*、rrt*、人工势场法等存在明显局限。采样类算法生成轨迹不平滑,易造成无人机振动与控制抖动;人工势场法易陷入局部最优,在狭窄空间与密集障碍环境中易出现规划失败;基于欧氏符号距离场的方法依赖静态地图,缺乏对动态障碍物运动趋势的预测,无法实现前瞻避障。更为关键的是,现有方法大多将空间路径与时间分配拆分为两个独立阶段,属于时空解耦规划,难以在统一优化框架内同时处理静态约束、动态约束与无人机动力学约束,导致轨迹出现速度突变、加速度超限、加加速度不连续等问题,轨迹可行性差、飞行效率低、安全性不足。

8、(四)机载系统实时性方面的不足

9、无人机受载荷、功耗与体积限制,嵌入式计算平台资源有限,现有动态导航系统普遍存在规划周期长、重规划频率低、系统延迟高等问题,难以应对动态障碍物快速移动的场景。同时,视觉惯性定位、环境感知、地图构建、轨迹规划各模块独立运行,缺乏紧耦合协同设计,数据时序不一致、信息传递滞后、模块间冗余计算较多,进一步降低系统整体效率,导致无人机在动态环境中响应迟缓、避障不及时、自主运行可靠性不足。

10、综上所述,现有四旋翼无人机自主导航技术在动态复杂环境中仍面临感知精度不足、地图易污染、轨迹规划难以兼顾动态避障与动力学约束、系统实时性差等一系列技术瓶颈,亟待研究一种适用于资源受限嵌入式平台、集感知、建图、规划于一体的动态环境自主导航装置及方法。

技术实现思路

1、本发明的目的在于克服现有技术中动态障碍物感知鲁棒性差、环境地图易受动态目标污染、轨迹规划无法兼顾动态避障与动力学约束、机载系统实时性不足等缺陷,提供一种面向动态环境的四旋翼无人机自主导航装置及动态避障轨迹生成方法,实现动态障碍物高精度检测跟踪、无污染稀疏体素地图构建、基于微分平坦与minco轨迹的梯度优化动态避障规划,整套系统轻量化、低功耗、高实时性,可在嵌入式平台稳定运行,适用于各类复杂动态环境自主导航作业。

2、本发明通过以下技术方案实现上述目的:

3、技术方案一:面向动态环境的四旋翼无人机自主导航装置

4、所述自主导航装置包括机载感知模块、嵌入式计算模块、飞控与执行模块、通信交互模块、电源管理模块和机械结构集成模块,各模块通过硬件接口与软件协议实现数据交互与协同工作,共同完成动态环境下自主感知、地图构建、轨迹规划与飞行控制任务。

5、机载感知模块采用rgb-d深度相机,为系统提供彩色图像、深度图像、惯性测量数据与三维点云多模态感知数据,输出内容包括校正后的彩色图像、校正后的深度图像、相机内参矩阵、imu原始数据与三维点云数据,为视觉惯性定位、动态障碍物检测与环境地图构建提供统一数据源。

6、嵌入式计算模块为装置的核心运算单元,负责运行视觉惯性里程计、动态障碍物检测跟踪、动态环境建图、动态避障轨迹规划全部算法,并实现与飞控的数据交互。嵌入式计算模块运行的软件栈包括:相机驱动与数据预处理节点、视觉惯性里程计节点、动态障碍物检测与跟踪节点、动态稀疏体素地图构建节点、动态避障轨迹规划节点、通信协议转换节点。

7、飞控与执行模块负责接收轨迹指令、完成无人机姿态控制、位置控制、电机驱动与故障保护。

8、通信交互模块实现装置内各单元之间的稳定数据传输,机载感知模块与嵌入式计算模块之间采用usb 3.1 type-c接口,传输图像、imu及点云数据;嵌入式计算模块与飞控模块之间采用usb-ttl串口,使用mavlink 2.0协议,满足实时导航需求。

9、电源管理模块采用多路供电结构,具备过压、过流、反接、短路四重保护功能,实时采集电池电压、电流与剩余电量,当电压低于预设阈值时触发低压保护,控制无人机自动降落。

10、机械结构集成模块采用轻量化碳纤维机架,深度相机安装于机身正前方水平朝前,嵌入式计算模块安装于机身中心上方,飞控模块安装于机身中心下方并配备减震装置,整机重心居中,传感器视场无遮挡,线缆牢固整洁。

11、技术方案二:面向动态环境的四旋翼无人机动态避障轨迹生成方法

12、(一)无人机微分平坦特性建模

13、四旋翼无人机为典型微分平坦非线性系统,本发明选取平坦输出为其中,为无人机世界坐标系三维位置,为无人机偏航角。基于微分平坦特性,无人机全部12维状态向量(位置、速度、加速度、姿态角、角速度)与4维控制输入(总推力、三轴力矩)均可由平坦输出及其前三阶导数解析表示,将高维复杂欠驱动系统优化问题转化为低维全驱动平坦输出空间优化问题,大幅降低计算复杂度。

14、由微分平坦特性推导的关系如下:

15、第一,速度、加速度由位置的一阶、二阶导数直接获得。

16、第二,机体姿态由加速度向量与重力向量合成推导。定义合成推力向量后,依次推导机体坐标系z轴方向、中间坐标系x轴方向、机体坐标系y轴方向、机体坐标系x轴方向,最终构建旋转矩阵,并由旋转矩阵分解得到滚转角与俯仰角。

17、第三,机体角速度由姿态导数与旋转矩阵推导,机体坐标系相对于世界坐标系的角速度通过旋转矩阵导数关系解析获得。

18、第四,总推力由无人机质量与合成加速度模值计算,三轴力矩由转动惯量与角加速度计算。

19、通过微分平坦变换,轨迹规划无需直接处理复杂非线性动力学方程,仅在平坦输出空间完成优化,即可保证生成轨迹满足无人机动力学可行性。具体如下:

20、(1)速度、加速度由位置一阶、二阶导数直接获得

21、

22、其中为位置向量。

23、(2)机体姿态由加速度与重力向量合成

24、首先定义中间变量:

25、

26、其中为重力加速度。

27、机体坐标系轴方向:

28、

29、中间坐标系轴方向:

30、

31、机体坐标系轴方向:

32、

33、机体坐标系轴方向:

34、

35、旋转矩阵:

36、

37、该旋转矩阵可以表示为欧拉角的形式,因此滚转角和俯仰角可以通过上述表达式计算得到。

38、(1)机体角速度由姿态导数与旋转矩阵推导

39、机体坐标系相对于世界坐标系的角速度可以表示为:

40、

41、通过旋转矩阵的导数关系,可以得到角速度的具体表达式:

42、

43、进一步推导可得:

44、

45、(2)总推力与力矩由质量、转动惯量与加速度、角加速度解析计算

46、总推力:

47、

48、三轴力矩:

49、

50、其中,,为无人机的转动惯量。

51、通过微分平坦变换,轨迹规划无需直接处理复杂非线性动力学方程,仅需在位置与偏航角构成的平坦空间内完成优化,即可保证生成轨迹满足无人机动力学可行性。

52、(二)minco轨迹参数化

53、本发明采用minco(最小控制能量轨迹)作为参数化表示,实现轨迹时空解耦、稀疏参数、高阶连续、平滑最优表征。minco轨迹基于三阶积分链系统构建,优化目标为最小化轨迹加加速度平方积分,使轨迹平滑性最优,降低机身振动与能耗。轨迹由两组稀疏参数唯一确定:航点向量与时间段向量。轨迹为分段五次多项式,满足段间位置连续、速度连续、加速度连续,具有稀疏参数、计算量小;时空完全解耦、可独立优化路径形状与飞行速度;局部调整不影响全局、适配动态环境实时重规划;天然满足动力学连续性等优势。具体的:

54、minco轨迹基于三阶积分链系统构建,优化目标为最小化轨迹加加速度平方积分:

55、

56、使轨迹平滑性最优,降低机身振动与能耗。

57、轨迹由两组稀疏参数唯一确定:航点向量,时间段向量。

58、轨迹为分段五次多项式,满足段间位置连续、段间速度连续、段间加速度连续、全局加加速度最优

59、minco轨迹具有稀疏参数,计算量小;时空完全解耦,可独立优化路径形状与飞行速度;局部调整不影响全局,适配动态环境实时重规划;天然满足动力学连续性,无需额外约束等优势。

60、(三)统一时空优化目标函数

61、本发明构建融合动力学可行性约束、静态避障代价、动态避障代价的统一加权优化目标函数。其中:

62、动力学可行性代价限制轨迹速度、加速度、加加速度不超过无人机物理极限;

63、静态避障代价采用引导点算法构建距离场,将轨迹碰撞点投影至无碰撞区域,计算轨迹与静态障碍物的距离惩罚;

64、动态避障代价采用后退视界距离场方法,基于动态障碍物预测轨迹构建锥形安全区域,随预测时间线性收缩安全半径,量化未来碰撞风险。

65、各项代价以加权形式融入统一目标函数,权重系数可根据场景动态调整。具体的:

66、统一加权优化目标函数表达式为:

67、

68、其中,动力学可行性代价限制轨迹速度、加速度、加加速度不超过无人机物理极限:

69、静态避障代价采用引导点算法构建距离场,将轨迹点投影至无碰撞区域,计算轨迹与静态障碍物距离惩罚:

70、动态避障代价采用后退视界距离场方法,基于动态障碍物预测轨迹构建锥形安全区域,随预测时间线性收缩安全半径,量化未来碰撞风险:

71、为各项权重系数,可根据场景动态调整。

72、(四)引导点算法与静态代价计算

73、引导点算法用于快速近似静态障碍物的碰撞代价与梯度,包括以下步骤:检测轨迹中碰撞点集;通过路径搜索生成无碰撞局部路径;将碰撞点投影至安全路径生成引导点;以引导点构建法向距离场,计算静态代价与梯度。该算法无需构建稠密距离场,计算量小、精度高,适配嵌入式平台实时运行。

74、(五)后退视界距离场与动态代价计算

75、后退视界距离场实现对动态障碍物的前瞻避障,步骤如下:由动态障碍物跟踪模块获取当前位置、速度、加速度;采用恒定加速度模型预测未来时域内障碍物轨迹;以当前位置为起点构建线性收缩锥形安全区域;计算轨迹点到锥形安全区域的有向距离;距离小于零时产生惩罚代价,实现动态避障。该方法可有效预测动态障碍物运动趋势,避免被动避障的滞后性,提升动态环境飞行安全性。

76、本发明的有益效果如下:

77、其一,针对单一检测器在复杂场景下鲁棒性不足的问题,提出udepth与dbscan异构检测融合架构与双向iou匹配策略,有效综合两类方法的互补优势,显著提升动态障碍物检测精度与环境适应性;

78、其二,针对地图易受动态目标污染的问题,提出基于对数几率概率模型的稀疏体素地图与自适应动态区域清理机制,实现高精度无污染的动态环境建模,为轨迹规划提供可靠的静态结构信息;

79、其三,针对现有轨迹规划无法同时处理动态避障与动力学约束的问题,基于微分平坦特性与minco轨迹参数化,构建引导点算法与后退视界距离场联合的统一时空优化框架,实现动态避障前瞻性与轨迹动力学可行性的同步保障;

80、其四,整套系统轻量化、低功耗、高实时性,单帧轨迹规划时间小于10ms,cpu占用率低于45%,可在资源受限嵌入式平台上稳定运行,适用于室内、地下管廊、仓储、应急救援等各类复杂动态环境自主导航作业。