[发明专利]一种微型水下探测机器人自主导航系统

权利要求书

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

第一处理器；

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

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

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

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

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

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

第二处理器；

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

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

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

第三处理器；

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

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

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

所述导航系统还包括最小方差估计模块，所述最小方差估计模块用于通过滤波器为系统误差提供线性最小方差估计，并通过误差的估计值修正导航系统；所述滤波器的输入值获取包括：通过磁位公式获取各地磁场的标量磁位，

式中，a为地球半径，r为地球径向距离，g、h为地磁场球谐系数，为伴随勒让德函数，i、j为函数的阶次，、分别为余纬和经度，=90-λ，λ为纬度，k为模型的截断水平，n是地磁模型类型阶数，选取1-15之间；

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

根据机器人目前所处的经纬高得出当地的地磁分量M_N，M_E,M_H

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

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

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

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

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

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

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