一种微型水下探测机器人自主导航系统

摘要

本发明公开了一种微型水下探测机器人自主导航系统，属于AUV水下导航技术领域，包括：第一处理器；避障声呐传感单元，用于探测水下地貌和在水下机器人进行自主导航时扫描水下物体进行避碰；磁力计，用于获取磁场强度和方向；惯性测量单元，用于获取三轴姿态角、角速率以及加速度；数据记录仪，用于将避障声呐传感单元、磁力计、惯性测量单元获取测量结果存储；模数转换器，用于将模拟信号转换成数字信号；开入开出模块，用于输入输出数据；该微型水下探测机器人自主导航系统，不依赖外部传感器、不需布置声波通讯定位节点、不需浮出水面组合GNSS信号，实现长时、独立、自主式水下导航目的。

权利要求

1.一种微型水下探测机器人自主导航系统，其特征在于：包括：第一处理器；

避障声呐传感单元，用于探测水下地貌和在水下机器人进行自主导航时扫描水下物体进行避碰；

磁力计，用于获取磁场强度和方向；

惯性测量单元，用于获取三轴姿态角、角速率以及加速度；

数据记录仪，用于将所述避障声呐传感单元、磁力计、惯性测量单元获取测量结果存储；

模数转换器，用于将模拟信号转换成数字信号；

开入开出模块，用于输入输出数据；

第二处理器；

与第二处理器连接的晶振；

与第二处理器连接的复位模块；

其中，所述第二处理器与所述第一处理器通过EMIF总线连接；

第三处理器；

与第三处理器连接的晶振；

与第三处理器连接的复位模块；

其中，所述第一处理器与所述第三处理器通过EMIF总线连接；

所述导航系统还包括最小方差估计模块，所述最小方差估计模块用于通过滤波器为系统误差提供线性最小方差估计，并通过误差的估计值修正导航系统；所述滤波器的输入值获取包括：通过磁位公式获取各地磁场的标量磁位，式中，a为地球半径，r为地球径向距离，g、h为地磁场球谐系数， 为伴随勒让德函数，i、j为函数的阶次， 、 分别为余纬和经度，=90‑λ，λ为纬度，k为模型的截断水平，n是地磁模型类型阶数，选取1‑15之间；

通过地磁公式获取地磁分量；

根据机器人目前所处的经纬高得出当地的地磁分量MN，ME,MH同时从地理坐标系转换至载体坐标系，可通过以下旋转矩阵：式中 、、为机器人的滚转角，俯仰角，航向角，通过上式即可得到安装在机器人上的磁力计的理论地磁值，同时磁力计获得实际测量值，将此值作为观测输入量输入滤波器中；

所述导航系统通电时先进行初始化程序，检测应用层程序的完整性，若应用层程序完整，则进入应用层程序与监控软件数据交互并给第一处理器发送配置参数，否则进入底层BOOT循环等待监控软件发送正确应用层程序，所述第一处理器在通电后等待DSP发送端口配置参数，则接收到应用层程序的配置参数时则建立数据连接；

所述滤波器在通电时，执行初始化，并初始对准指令，初始对准完毕，检测导航解算标志是否有效，若有效捷联导航解算，计算滤波器状态转移矩阵，同时计算噪声矩阵，并离散化系统和噪声矩阵，再检测滤波器更新标志是否有效，若有效则计算量测量，更新滤波器数据，否则更新滤波器时间。

2.根据权利要求1所述的一种微型水下探测机器人自主导航系统，其特征在于：所述第二处理器通过共享RAM与第三处理器交互数据。

3.根据权利要求1所述的一种微型水下探测机器人自主导航系统，其特征在于：所述第二处理器和第三处理器为DSP架构，所述第一处理器为FPGA架构。

4.根据权利要求3所述的一种微型水下探测机器人自主导航系统，其特征在于：所述第一处理器通过SPI协议分别与惯性测量单元和磁力计连接，通过UART总线与避障声呐传感单元数据连接。

说明书

一种微型水下探测机器人自主导航系统

技术领域

[0001] 本发明属于AUV水下导航技术领域，具体涉及微型水下探测机器人自主导航系统。

背景技术

[0002] 近年来，AUV水下导航成为水下定位技术发展的重点和难点，经过各国学者的深入研究已经初见成果。科研人员对AUV自主导航进行了深入研究，发展出多种水下导航定位技术，主要有：惯性导航系统（Inertial Navigation System，INS）、全球卫星定位系统（Global Navigation Satellite System，GNSS）、声学定位系统（Acoustic Positioning System，APS）、多普勒测速仪（Doppler Velocity Log，DVL）、地球物理导航，这些导航设备各有其优缺点；

[0003] 1、惯性导航，无需外部设备，误差随时间积累；

[0004] 2、卫星导航，精度高，误差不随时间积累，在水下无法接收卫星信号；

[0005] 3、多普勒计程仪，无需外部设备，需要航向和垂直基准信息；

[0006] 4、声学导航，精度高，误差不随时间积累，需要预衔在海中布放定位基阵；

[0007] 5、地球物理导航、无需外部设备，误差不随时间积累，需要预先建立信息数据库，仅用于辅助导航；

[0008] 惯性导航通常包含惯性传感器陀螺仪和加速度计，利用惯性器件基准方向和初始位置、速度信息推算出载体的位置、速度和姿态；惯性导航优势显著，首先其使用环境不受任何限制，空中、陆地、水中都可以实现，同时能稳定输出高频位置、速度和姿态等全面导航信息；但误差会随着时间积累导致定位精度发散，而高精度惯性导航又相对昂贵；综合考虑各方面因素，因此，需要研发一种新的导航系统来解决现有的问题。

发明内容

[0009] 本发明的目的在于提供一种微型水下探测机器人自主导航系统，以解决现有导航系统的误差会随着时间积累导致定位精度发散的问题。

[0010] 为实现上述目的，本发明提供如下技术方案：一种微型水下探测机器人自主导航系统，包括：

[0011] 第一处理器；

[0012] 避障声呐传感单元，用于探测水下地貌和在水下机器人进行自主导航时扫描水下物体进行避碰；

[0013] 磁力计，用于获取磁场强度和方向；

[0014] 惯性测量单元，用于获取三轴姿态角、角速率以及加速度；

[0015] 数据记录仪，用于将所述避障声呐传感单元、磁力计、惯性测量单元获取测量结果存储；

[0016] 模数转换器，用于将模拟信号转换成数字信号；

[0017] 开入开出模块，用于输入输出数据；

[0018] 优选的，所述导航系统还包括：

[0019] 第二处理器；

[0020] 与第二处理器连接的晶振；

[0021] 与第二处理器连接的复位模块；

[0022] 其中，所述第二处理器与所述第一处理器通过EMIF总线连接。

[0023] 优选的，所述导航系统还包括：

[0024] 第三处理器；

[0025] 与第三处理器连接的晶振；

[0026] 与第三处理器连接的复位模块；

[0027] 其中，所述第一处理器与所述第三处理器通过EMIF总线连接。

[0028] 优选的，所述第二处理器通过共享RAM与第三处理器交互数据。

[0029] 优选的，所述导航系统还包括最小方差估计模块，所述最小方差估计模块用于通过滤波器为系统误差提供线性最小方差估计，并通过误差的估计值修正导航系统。

[0030] 优选的，所述滤波器的输入值获取包括：通过磁位公式获取各地磁场的标量磁位，[0031]

[0032] 式中，a为地球半径，r为地球径向距离，g、h为地磁场球谐系数， 为伴随勒让德函数，i、j为函数的阶次，、 分别为余纬和经度， =90‑λ，λ为纬度，k为模型的截断水平；

[0033] 通过地磁公式获取地磁分量；

[0034]

[0035]

[0036]

[0037] 根据机器人目前所处的经纬高得出当地的地磁分量MN，ME,MH

[0038] 同时从地理坐标系转换至载体坐标系，可通过以下旋转矩阵：

[0039]

[0040] 式中 、、为机器人的滚转角，俯仰角，航向角，通过上式即可得到安装在机器人上的磁力计的理论地磁值，同时磁力计获得实际测量值，将此值作为观测输入量输入滤波器中。

[0041] 优选的，所述导航系统通电时先进行初始化程序，检测应用层程序的完整性，若应用层程序完整，则进入应用层程序与监控软件数据交互并给第一处理器发送配置参数，否则进入底层BOOT循环等待监控软件发送正确应用层程序，所述第一处理器在通电后等待DSP发送端口配置参数，则接收到应用层程序的配置参数时则建立数据连接。

[0042] 优选的，所述滤波器在通电时，执行初始化，并初始对准指令，初始对准完毕，检测导航解算标志是否有效，若有效捷联导航解算，计算滤波器状态转移矩阵，同时计算噪声矩阵，并离散化系统和噪声矩阵，再检测滤波器更新标志是否有效，若有效则计算量测量，更新滤波器数据，否则更新滤波器时间。

[0043] 优选的，所述第二处理器和第三处理器为DSP架构，所述第一处理器为FPGA架构。

[0044] 优选的，所述第一处理器通过SPI协议分别与惯性测量单元和磁力计连接，通过UART总线与避障声呐传感单元数据连接。

[0045] 本发明的技术效果和优点：该微型水下探测机器人自主导航系统，不依赖外部传感器、小型化、低成本的导航系统，不需布置声波通讯定位节点、不需浮出水面组合GNSS信号，实现长时、独立、自主式水下导航目的；通过低成本INS传感器和地磁的导航传感方案，替代传统的光纤陀螺和加计的INS导航方案；针对水下机器人导航与控制系统小型化需求，设计基于双DSP和FPGA架构的一体化系统架构，结合INS芯片和地磁模块，从导航控制系统的架构上进行创新，硬件体积小、重量轻、价格低廉；

[0046] 本发明相对于海洋体积小、重量轻，受到海流的影响很大，而导航系统是实现自主导航定位与运动控制的核心，需要具备高精度的导航定位系统、丰富的通讯与控制接口以及高性能的计算处理单元，且具备低成本、小型化、高可靠性等特征，从而实现产品化、实用化；

[0047] 本方案高集成度、高扩展性，基于FPGA+双DSP架构，包含传感器采集、导航解算、对外通讯及电气控制、电源转换功能，其中双DSP分别作为导航定位解算、给控制系统提供运算载体；该系统具备以下特点：

[0048] 1、具备惯性单元传感器、磁力计传感器采集传感功能，通过滤波算法组合导航解算，实现姿态、位置、速度等导航信息的实时输出，供控制运算单元使用；

[0049] 2、具备避障声呐传感器采集传感功能，实时输出避障声呐采集结果，供控制运算单元使用；

[0050] 3、硬件上拥有丰富的对外通讯接口、对外控制开关接口、模拟量采集接口，可根据不同的任务搭载不同的载荷，满足不同的运动控制、任务控制需求；

[0051] 4、具备智能电源转换硬件，可实现控制台电源、主电池电源、应急电池电源切换功能；在导航系统结构上，划分为独立模块，采用叠层方式进行互联，模块之间连接器板间连接器实现，方便后续装联、装配及生产、调试和维护工作。

附图说明

[0052] 图1为本发明的原理图；

[0053] 图2为本发明系统执行示意图；

[0054] 图3为本发明系统架构示意图；

[0055] 图4为本发明的运行流程图。

具体实施方式

[0056] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

[0057] 本发明提供了如图1中所示的一种微型水下探测机器人自主导航系统，包括：

[0058] 第一处理器；

[0059] 避障声呐传感单元，用于探测水下地貌和在水下机器人进行自主导航时扫描水下物体进行避碰；

[0060] 磁力计，用于获取磁场强度和方向；

[0061] 惯性测量单元，用于获取三轴姿态角、角速率以及加速度；

[0062] 数据记录仪，用于将所述避障声呐传感单元、磁力计、惯性测量单元获取测量结果存储；

[0063] 模数转换器，用于将模拟信号转换成数字信号；

[0064] 开入开出模块，用于输入输出数据。

[0065] 第二处理器；

[0066] 与第二处理器连接的晶振；

[0067] 与第二处理器连接的复位模块；

[0068] 其中，所述第二处理器与所述第一处理器通过EMIF总线连接。

[0069] 第三处理器；

[0070] 与第三处理器连接的晶振；

[0071] 与第三处理器连接的复位模块；

[0072] 其中，所述第一处理器与所述第三处理器通过EMIF总线连接。

[0073] 本实施例中，所述第二处理器为主DSP，第三处理器为从DSP，所述第一处理器为FPGA

[0074] 参见图3，本实施例中，所述第二处理器和第三处理器为DSP架构，所述第一处理器为FPGA架构。所述第一处理器通过SPI协议分别与惯性测量单元和磁力计连接，通过UART总线与避障声呐传感单元数据连接。

[0075] 参见图4，所述导航系统通电时先进行初始化程序，检测应用层程序的完整性，若应用层程序完整，则进入应用层程序与监控软件数据交互并给第一处理器发送配置参数，否则进入底层BOOT循环等待监控软件发送正确应用层程序，所述第一处理器在通电后等待DSP发送端口配置参数，则接收到应用层程序的配置参数时则建立数据连接。

[0076] 本实施例中，所述第二处理器通过共享RAM与第三处理器交互数据。

[0077] 最小方差估计模块，所述最小方差估计模块用于通过滤波器为系统误差提供线性最小方差估计，并通过误差的估计值修正导航系统。

[0078] 所述滤波器的输入值获取包括：通过磁位公式获取各地磁场的标量磁位，[0079]

[0080] 式中，a为地球半径，r为地球径向距离，g、h为地磁场球谐系数， 为伴随勒让德函数，i、j为函数的阶次， 、 分别为余纬和经度， =90‑λ，λ为纬度，k为模型的截断水平；

[0081] 通过地磁公式获取地磁分量；

[0082]

[0083]

[0084]

[0085] 根据机器人目前所处的经纬高得出当地的地磁分量MN，ME,MH

[0086] 同时从地理坐标系转换至载体坐标系，可通过以下旋转矩阵：

[0087]

[0088] 式中 、、为机器人的滚转角，俯仰角，航向角，通过上式即可得到安装在机器人上的磁力计的理论地磁值，同时磁力计获得实际测量值，将此值作为观测输入量输入滤波器中。

[0089] 参见图2，所述滤波器在通电时，执行初始化，并初始对准指令，初始对准完毕，检测导航解算标志是否有效，若有效捷联导航解算，计算滤波器状态转移矩阵，同时计算噪声矩阵，并离散化系统和噪声矩阵，再检测滤波器更新标志是否有效，若有效则计算量测量，更新滤波器数据，否则更新滤波器时间；

[0090] 地磁场根据覆盖范围可以分为全球磁场和区域磁场，目前，权威的全球地磁场主要包括IGRF模型、WMM模型、NGDC‑720模型、EMM2010模型、CGRF模型，其中IGRF模型是国际地磁学和高空物理学会执行委员会以及世界地磁测量部门同意，作为国际通用地磁场标准，是一系列关于地球主磁场及其年变率的数学模型，其模型精度在我国大体为水平分量114.2nT，北向分量104.0nT，垂直分量157.3nT，地球磁场的值大小为50000nT左右，因此该模型能够很好的诠释我国地磁场值，并作为组合导航的输入；

[0091] 本实施例中，DSP选用TI公司的TMS320C6746，具有375MHz超高主频，BGA361封装，支持32位整型，SP，具有IEEE单精度/32位和DP具有IEEE双精度、64位浮点数，3个64位通用定时器，每个通用定时器配置为2个32位定时器，32KB 的 L1P 程序 RAM缓存、32KB 的 L1D 数据 RAM缓存、256KB 的 L2 统一映射 RAM/缓存，1个64位通用定时器或看门狗定时器，还可以配置为2个32位定时器，等级为工业级；

[0092] FPGA使用ALTERA公司的EP4CGX30, 具有290个IO资源，BGA484封装，29440个逻辑门，1080Kbit内存，最大工作频率200MHz，等级为工业级；

[0093] DSP与FPGA之间通过EMIF总线进行互联；主处理器通过共享RAM与从处理器交互数据；最小系统对外接口通过FPGA实现，最大化的发挥FPGA并行处理优势，可扩展性强；DSP上电执行完初始化程序后首先检查应用层程序的完整性，若检查未通过则进入底层boot模式，等待上位机监控软件进行应用层程序更新；若通过则进入应用层程序，给FPGA装载发送端口参数，并与上位机监控软件进行交互，同时执行相关应用层功能；

[0094] FPGA上电后等待DSP发送端口配置参数，DSP发送端口配置参数后，则开始与DSP进行总线数据交互以及与外界数据交互。

[0095] 最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换,凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。