CN102004447A - 水下航行器组合导航与控制半实物仿真试验系统 - Google Patents

Abstract

本发明公开了一种水下航行器组合导航与控制半实物仿真试验系统，仿真计算机计算出水下航行器的位置、姿态、速度和航向信息；SINS子系统模拟水下航行器的姿态运动；卫星导航子系统模拟水下航行器的位置和速度信息；深度模拟子系统模拟水下航行器航行深度信息；DVL导航子系统模拟求解水下航行器的速度；虚拟线加速度计模拟水下航行器的比力信息；导航控制计算机采集上述信息完成惯性导航和控制系统解算，驱动舵机运动；舵机模拟动作，并由舵角反馈电位计将舵角信息反馈给仿真计算机。本发明实现了水下航行器组合导航控制系统的联调与测试。

Description

水下航行器组合导航与控制半实物仿真试验系统

技术领域

本发明涉及一种半实物仿真系统，具有捷联惯性导航+GPS/北斗(GPS/BD)+多普勒测速仪+深度传感器的组合导航与控制系统仿真功能，适用于多种水下航行器的组合导航控制系统的理论方法研究、工程应用技术开发和系统性能检测与验证。

背景技术

半实物仿真试验又称为硬件在回路中仿真，它是指在系统研究中，把数学模型、实体模型(物理效应模型)和系统的实际设备(实物)联系在一起运转。利用半实物仿真系统，可以检验系统中某些部件的性能，可以进一步校正系统的数学模型，从而在实验室内全面地检验和评定系统设计的合理性和各部件工作的协调性，在武器系统研制中发挥了重要作用，具有经济性和安全性的特点。

水下航行器导航技术与飞机、导弹等飞行器导航技术相比，具有工作时间长、环境复杂、信息源少、隐蔽性要求高等特点，因此水下导航具有更大的难度。在水下航行器导航与控制系统半实物仿真技术研究方面，卢淑娟等人在《水下航行器组合导航仿真系统设计与实现》中提出的自主水下航行器(AUV)SINS/DVL组合导航的实时仿真系统由载体计算机和导航计算机组成，其中AUV模型解算主要由载体计算机完成，导航计算机负责解算AUV导航系统的导航参数。严卫生在《水下航行器导航与控制实时仿真系统》一文中建立的仿真系统由两部分组成：导航与控制实时仿真系统和自动驾驶仪的导航控制计算机，其中实时仿真系统由两台联网的计算机和两个接口转换器组成，以模拟多普勒测速仪、罗经和深度传感器等外围设备信息，并将模拟的信息参数提供给导航控制计算机。以上仿真系统都是基于计算机的数字仿真，用计算机模拟传感器设备为控制系统提供虚拟信号，没有接入真实的导航与控制组件。

王彦恺在《基于SINS/GPS/DVL组合导航的AUV半实物仿真系统设计》中设计了一种AUV半实物仿真系统。从文中所列的AUV半实物仿真系统结构图和AUV导航与控制流程图可以看出，该系统的多普勒信号仿真器和GPS仿真器仍采用计算机数字信号模拟，而且在系统中没有接入水压仿真器的压力信号，未构成完整的闭环仿真系统。谢攀在《一种新型UUV半实物仿真系统设计与实现》中给出了GPS模拟子系统、多普勒测速模拟装置和水压仿真器子系统的测试结果，但没有给出闭环仿真系统的实验结果，没有解决捷联惯导系统的线加速度模拟问题以及模型解算和导航解算的同步问题。

专利200610011580.X提出了一种SINS/CNS/GPS组合导航半实物仿真系统，专利200610089437.2中提出了一种捷联惯导/天文组合导航半实物仿真系统。这两种半实物仿真系统利用轨迹发生器生成标称轨迹数据，然后叠加上SINS、CNS、GPS等器件的误差数据来研究组合导航系统的动态性能。但是这两种半实物仿真系统未考虑航行器的动力学和运动学模型，而且这两种半实物仿真系统没有构成导航控制系统的全闭环仿真试验，也没有深度模拟装置，不适合水下航行器的组合导航。

发明内容

现有的仿真系统没有解决惯导系统线加速度信号仿真问题，无法构成导航控制系统的全闭环仿真试验，本发明提供一种水下航行器组合导航与控制半实物仿真试验系统，实现了对IMU惯导组件、多普勒测速仪组件、深度传感器组件的性能测试，可进行导航控制算法的研究，全部实物与仿真设备连接构成半实物闭环仿真系统，实现了水下航行器组合导航控制系统的联调与测试。

本发明解决其技术问题所采用的技术方案是：仿真试验系统包括ADI实时仿真计算机、捷联惯性导航子系统(SINS子系统)、卫星导航子系统、导航控制计算机、舵机、多普勒(DVL)导航子系统、虚拟线加速度计和深度模拟子系统。

ADI实时仿真计算机解算水下航行器的动力学和运动学数学模型，计算出水下航行器的位置、姿态、速度和航向信息。

SINS子系统包括反射内存通讯板、角运动模拟器和IMU惯性测量装置。IMU惯性测量装置固定在角运动模拟器上，实时敏感水下航行器在载体坐标系下的三个角加速度和角速度，并送到导航控制计算机。角运动模拟器通用反射内存接收来自ADI实时仿真计算机的水下航行器姿态信息，模拟水下航行器的姿态运动，为惯性组件提供虚拟试验环境。

卫星导航子系统包括卫星导航模拟器和卫星导航接收机。卫星导航模拟器可实现对真实卫星导航系统的模拟——根据ADI实时仿真计算机发出的水下航行器的运动信息实时生成并发射GPS/BD信号载波，卫星导航接收机通过射频天线实时地接收卫星导航模拟器发射的卫星导航信号，然后把水下航行器所处的位置和在导航坐标下的速度信息通过RS232串口送给导航控制计算机。

深度模拟子系统包括深度传感器、I/O通讯模块、反射内存通讯板和深度模拟器。深度传感器安装在深度模拟器上，深度模拟器通过反射内存接收来自ADI实时仿真计算机发送的水下航行器航行深度信号，将其转换为相应的压力信号，深度传感器将敏感到深度信息通过I/O通讯模块输出至导航控制计算机。

DVL导航子系统包括DVL测速仿真装置、声对接装置和多普勒测速仪。ADI实时仿真计算机通过反射内存将水下航行器的速度信息传递给DVL测速模拟装置，DVL测速模拟装置为多普勒测速仪提供水声传播环境，并通过声对接装置与多普勒测速仪连接，多普勒测速仪根据发射的声波与经过反射回的声波之间的频率差求解水下航行器的速度，并传送给导航控制计算机。

虚拟线加速度计包括哥氏加速度计算装置、重力加速度计算装置和数据融合装置。哥氏加速度计算装置和重力加速度计算装置分别接收来自ADI实时仿真计算机的信号，并计算得到哥氏加速度和当地的重力加速度，通过数据融合装置得到水下航行器实时的比力信息，而后送至导航控制计算机。

导航控制计算机实时并行采集IMU惯导组件、卫星导航接收机、深度传感器、DVL测速仪输出的姿态、位置、深度、速度信息及虚拟线加速度计输出的比力信息，完成惯性导航和控制系统解算，输出水下航行器的操舵指令，驱动舵机运动。

舵机接收导航控制计算机输出的操作指令，模拟水下航行器垂直舵和水平舵的动作。舵机上的舵角反馈电位计将舵角信息反馈给ADI实时仿真计算机，构成整个系统的闭环反馈。

上述设备模拟了水下航行器在航行过程中的姿态、位置、速度、航行深度等信息，在实验室内构建了水下航行器的半实物仿真试验环境。

在本发明中，通过引入开机信号作为仿真开始信号实现了导航控制计算机的惯导解算与ADI实时仿真计算机的模型解算及虚拟加速度计的同步。开机信号由稳压电源提供，分别通过I/O通讯模块与ADI实时仿真计算机、导航控制计算机和虚拟线加速度计连接，确保导航控制计算机的惯导解算与ADI实时仿真计算机的模型解算及虚拟线加速度计的同步，保证了模型解算和导航控制计算机的积分时间相同，使积分误差达到最小。

上述的时间同步连接在使用时包括以下步骤：

(1)ADI仿真主机、角运动模拟器、深度模拟器、多普勒测速模拟装置和导航控制计算机设置仿真初值；

(2)角运动模拟器、深度模拟器分别运动到设定的姿态角位置和深度，导航控制计算机接收IMU组件、GPS接收机和深度传感器信息，多普勒测速仪仿真装置接收水下航行器的速度信号，各设备均稳定在初值状态，等待开机信号；

(3)打开开机信号，各设备构成闭环进行仿真运行：开机信号由稳压电源提供，分别通过I/O通讯模块与ADI实时仿真计算机、导航控制计算机和虚拟线加速度计连接，实现了导航控制计算机的惯导解算与ADI实时仿真计算机的模型解算及虚拟线加速度计的同步，保证了模型解算和导航控制计算机的积分时间相同，使积分误差达到最小。

(4)仿真时间到，角运动模拟器、深度模拟器退出仿真模式，然后导航控制计算机、GPS模拟器、多普勒测速仪模拟装置和ADI仿真机退出，本次仿真结束。

本发明的有益效果是：本发明将更多的水下航行器实物设备(卫星导航接收机、多普勒测速仪、深度传感器等)直接接入仿真回路，可对导航控制系统各传感器组件功能进行检验，还可通过模型和实物之间的切换，进一步校准数学模型。通过引入开机信号，实现了惯导解算与水下航行器模型解算的同步性，减小了半实物仿真系统的误差，提高了系统的仿真精度。虚拟线加速度计满足半实物仿真试验的需求，使试验能在实验室条件下顺利进行，相比车载惯性测量器件更具实用性，且不受天气、路况等外部因素的影响。全闭环半实物仿真试验系统能够设计、分析和优化水下航行器的弹道轨迹；可以检验水下航行器组合导航与控制系统软硬件功能，完成系统测试，进行导航控制系统的方案论证和误差分析，对水下航行器进行性能评估；为水下航行器的研制提供了有力的技术支持，有助于缩短研制时间，节约研究经费。

下面结合附图和实施例对本发明进一步说明。

附图说明

图1为本发明的结构组成示意图；

图2为本发明的SINS子系统的组成结构图；

图3为本发明的深度模拟子系统的组成结构图；

图4为本发明的DVL导航子系统的组成结构图；

图5为虚拟线加速度计的结构图；

图6为本发明的信号连接图；

图7为水下航行器在SINS+DVL组合导航情况下，惯导解算的纬度与真实值对比曲线；

图8为水下航行器在SINS+DVL组合导航情况下，惯导解算的经度与真实值对比曲线；

图9为水下航行器在SINS+DVL组合导航情况下，航向角随时间的变化曲线；

图10为水下航行器在SINS+DVL组合导航情况下，北向速度随时间的变化曲线；

图11为水下航行器在SINS+DVL组合导航情况下，东向速度随时间的变化曲线；

图12为水下航行器在SINS+DVL组合导航情况下，深度随时间的变化曲线；

图13为水下航行器在SINS+GPS卫星导航情况下，纬度误差随时间的变化曲线；

图14为水下航行器在SINS+GPS卫星导航情况下，经度误差随时间的变化曲线；

具体实施方式

利用本发明完成了水下航行器导航控制系统半实物仿真实验。

(1)系统组成

水下航行器组合导航与控制半实物仿真试验系统由IMU惯性测量装置、多普勒测速仪、深度传感器、卫星导航接收机和舵机组件等组件实物和ADI实时仿真计算机、多普勒测速模拟装置、深度模拟器、角运动模拟器、卫星导航模拟器、虚拟线加速度计等仿真设备以及导航控制计算机组成。水下航行器在水下航行时，采用SINS+DVL组合导航方式；在水面航行时，采用SINS+GPS/北斗(GPS/BD)组合导航方式。

系统的工作原理是：

首先，由ADI实时仿真计算机给角运动模拟器、深度模拟器、卫星导航模拟器和多普勒测速模拟装置设置初值，待所有仿真设备达到初值后，启动开机信号，仿真系统开始运行。ADI实时仿真计算机实时解算水下航行器的动力学、运动学方程，并将解算出的三个姿态角、深度及速度信息分别送至角运动模拟器、深度模拟器和多普勒测速模拟装置，同时将水下航行器的位置(包括经度、纬度、深度)和速度信息通过网络送至卫星导航模拟器。IMU惯性测量装置、深度传感器、卫星导航接收机和多普勒测速仪将采集到的水下航行器的姿态角速率、深度信息、位置信息及速度信息分别输出至导航控制计算机。导航控制计算机通过导航解算得出操舵信号，控制舵机运动，舵反馈信号送入ADI实时仿真计算机，从而构成一个闭环的半实物仿真系统。

系统的工作流程是：

①对系统进行初始化，包括对各仿真设备和参试实物的初值设置、软件的初始化；

②在检测到开机信号发出后，虚拟线加速度计输出水下航行器的线加速度，同时，模型解算模块解算出水下航行器的运动信息；

③通过水下航行器是否上浮判断是SINS+DVL组合导航还是SINS+卫星组合导航，如是前者，则导航控制计算机通过RS422串口接收DVL测速仪送出的水下航行器的速度信息，同时通过RS232串口接收ADI实时仿真计算机解算出的水下航行器的运动信息及虚拟线加速度计送出的水下航行器的线加速度，解算后送出操舵指令；否则确定是否进行GPS/BD位置修正，若是，则接收卫星信号接收机的数据，进行GPS/BD星座定位解算，导航控制计算机通过SINS/卫星组合滤波校正惯导系统的导航误差，控制水下航行器回归设定航迹；

④舵机在操舵指令的控制下，驱动舵机动作，舵角反馈电位计将实时舵角信息送到模型解算模块，从而完成了整个半实物仿真试验系统的闭环。

⑤当水下航行器到达指定区域，或满足其它结束条件(如航行时间到)时，系统结束运行。

(2)虚拟线加速度计

惯性导航系统是以陀螺和加速度计为敏感元件的导航参数解算系统。在水下航行器导航控制半实物仿真试验系统中，由于IMU惯性测量装置固定在三轴转台上，所以加速度计无法提供比力信息。本发明设计的虚拟线加速度计，利用水下航行器的经纬度、深度、位置信息，采用公式(1)计算出水下航行器的比力信息，通过RS232串口送入导航控制计算机，从而使惯性导航实验能够在实验室条件下进行。

$f_{\mathrm{ib}}^n={\stackrel{\·}{V}}_{\mathrm{en}}^n+(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×V_{\mathrm{en}}^n-g^n+{\▿}_i---\left(1\right)$

式中，——比力在导航坐标系的投影；

——惯性坐标系相对于地球坐标系的角速率在导航坐标系的投影；

——地球坐标系相对于导航坐标系的角速率在导航坐标系的投影；

——地球坐标系相对于导航坐标系的速率度在导航坐标系的投影；

gⁿ——重力加速度在导航坐标系的投影；

——噪声干扰；

工作原理为，通过ADI实时仿真计算机的模型结算模块得到水下航行器的加速度信息

经积分后得到其速度信息由水下航行器所处地理位置的纬度L、实时的航行深度h可以计算得到当地的重力加速度gⁿ；由水下航行器所处地理位置的经度λ和纬度L信息，可以计算出地球坐标系到导航坐标系的位置转换矩阵，进而求出由于水下航行器相对于地球运动(相对运动)和地球旋转(牵连运动)引起的哥氏加速度

通过计算地球速率和矩阵变换得到由于水下航行器保持在地球表面运动(圆周运动)引起的对地向心加速度对上述信息进行综合、加入噪声干扰后得到虚拟线加速度计的输出

(3)陀螺仪积分时间同步技术

根据捷联惯导系统的基本原理可知，陀螺仪和虚拟线加速度计送出的加速度信息需要经过积分运算才能得到水下航行器的速度信息，在半实物仿真系统中各仿真设备都有自己的时钟系统，如果导航控制计算机与ADI仿真主机不同步，惯导解算就会产生较大的积分累积误差，甚至引起系统的发散。所以如何确定积分开始时间，即实现导航控制计算机与ADI机的同步时间关系到半实物仿真系统的精确性和实时性。在本发明中，通过引入开机信号作为仿真开始信号实现了导航控制计算机的惯导解算与ADI实时仿真计算机的模型解算及虚拟线加速度计的同步。如图6中的开机信号同时送至导航控制计算机、ADI实时仿真计算机及虚拟线加速度计，这就保证了ADI实时仿真计算机的模型解算和导航控制计算机的积分时间相同，使积分误差达到最小。

(4)水下SINS+DVL组合导航半实物实时仿真试验

进行水下航行器在水下航行时导航与控制半实物仿真试验，模拟水下航行器从初始位置运动到设定位置，接入的实物有IMU惯性测量装置、深度传感器和舵机组件等。

ADI实时仿真计算机通过反射内存网络与深度模拟器、角运动模拟器、多普勒测速模拟装置连接，对其初值进行设置，仿真初值设置如表1所示。在所有仿真设备到达初始位置后，由外部稳压电源送出开机信号，启动半实物实时仿真系统。仿真开始，ADI实时仿真机解算水下航行器的动力学和运动学模型，角运动模拟器接收水下航行器姿态，模拟水下航行器的姿态运动，深度模拟器接收水下航行器深度信号，为深度传感器提供相应的压力信号，多普勒测速仪仿真装置接收水下航行器的速度信号，模拟多普勒测速仪回波信号。导航控制计算机通过RS232串口采集IMU惯性测量装置、多普勒测速仪和虚拟线加速度计敏感到的水下航行器姿态、速度和加速度信息，通过D/A接口采集深度传感器敏感到的深度信息，进行导航和控制系统解算，得到水下航行器操舵指令，控制舵机运动，舵机反馈信号通过A/D接口送入ADI实时仿真计算机，从而构成闭环仿真系统。图7-图12为仿真结果曲线。

表1  仿真初值设置

参数	数值
		初始位置(经度、纬度、深度)	N30°0′0″，E120°0′0″，-10m
初始速度(北向、天向、东向)	-11.0m/s，0.2m/s，3.9m/s
		初始姿态角	200°，0°，0°
初始位置误差	0.0m，0.0m，0.0m
		初始速度误差	0.0m/s，0.0m/s，0.0m/s
初始失准角	0′，0′，0′，
		设定深度	100.0m
目标位置	N29°59′55″，E119°51′21″，-100m
		弹道类型	直航+导航
直航时间	1600s
		深度控制方式	定深
惯导解算周期	10ms

(2)近水面SINS+卫星组合导航半实物实时仿真试验

水下航行器近水面航行时采用SINS+卫星组合导航方式，通过仿真检验卫星辅助导航校正的精度。接入的实物有IMU惯性测量装置、卫星导航接收机、深度传感器和舵机组件等。

ADI实时仿真计算机通过反射内存网络对深度模拟器、角运动模拟器、卫星导航模拟器进行初值设置，仿真初值设置如表2所示。在所有仿真设备到达初始位置后，由外部稳压电源送出开机信号，启动半实物实时仿真系统。仿真开始，ADI实时仿真机解算水下航行器的动力学和运动学模型，角运动模拟器接收水下航行器姿态，模拟水下航行器的姿态运动，深度模拟器接收水下航行器深度信号，为深度传感器提供相应的压力信号，卫星模拟器接收水下航行器位置信息，模拟GPS/BD卫星信号。导航控制计算机通过串口同时采集IMU惯性测量装置、卫星信号接收机敏感到的水下航行器姿态、速度信息和虚拟线加速度计送出的加速度信息，通过D/A接口采集深度传感器敏感到的水下航行器的航行深度信息，通过导航控制解算得到水下航行器的操舵指令，控制舵机运动，舵角反馈电位计将舵机反馈信号通过A/D接口送入ADI实时仿真计算机，从而构成闭环仿真系统。

表2  仿真初值设置

参数	数值
		初始位置(经度、纬度、深度)	N30°0′0″，E120°0′0″，-3m
初始速度(北向、天向、东向)	-0.0m/s，0.0m/s，0.0m/s
		初始姿态角	90°0′0″，0°0′0″，0°0′0″
初始位置误差	0.0m，0.0m，0.0m
		初始速度误差	0.0m/s，0.0m/s，0.0m/s
初始失准角	0′，0′，0′，
		目标位置	N30°0′0″，E119°51′21″，-0m
GPS输出频率	10Hz

图13和图14为纬度误差随时间的变化曲线和经度误差随时间的变化曲线。从图中可以看出水下航行器经过卫星辅助导航校正之后，纬度定位误差小于1.25m，经度定位误差在±5m范围内，精度较高。

Claims (2)

1.一种水下航行器组合导航与控制半实物仿真试验系统，包括ADI实时仿真计算机、捷联惯性导航子系统、卫星导航子系统、导航控制计算机、舵机、多普勒导航子系统、虚拟线加速度计和深度模拟子系统，其特征在于：

ADI实时仿真计算机解算水下航行器的动力学和运动学数学模型，计算出水下航行器的位置、姿态、速度和航向信息；

SINS子系统包括反射内存通讯板、角运动模拟器和IMU惯性测量装置，IMU惯性测量装置固定在角运动模拟器上，实时敏感水下航行器在载体坐标系下的三个角加速度和角速度，并送到导航控制计算机，角运动模拟器通用反射内存接收来自ADI实时仿真计算机的水下航行器姿态信息，模拟水下航行器的姿态运动，为惯性组件提供虚拟试验环境；

卫星导航子系统包括卫星导航模拟器和卫星导航接收机，卫星导航模拟器可实现对真实卫星导航系统的模拟——根据ADI实时仿真计算机发出的水下航行器的运动信息实时生成并发射GPS/BD信号载波，卫星导航接收机通过射频天线实时地接收卫星导航模拟器发射的卫星导航信号，然后把水下航行器所处的位置和在导航坐标下的速度信息通过RS232串口送给导航控制计算机；

深度模拟子系统包括深度传感器、I/O通讯模块、反射内存通讯板和深度模拟器，深度传感器安装在深度模拟器上，深度模拟器通过反射内存接收来自ADI实时仿真计算机发送的水下航行器航行深度信号，将其转换为相应的压力信号，深度传感器将敏感到深度信息通过I/O通讯模块输出至导航控制计算机；

DVL导航子系统包括DVL测速仿真装置、声对接装置和多普勒测速仪，ADI实时仿真计算机通过反射内存将水下航行器的速度信息传递给DVL测速模拟装置，DVL测速模拟装置为多普勒测速仪提供水声传播环境，并通过声对接装置与多普勒测速仪连接，多普勒测速仪根据发射的声波与经过反射回的声波之间的频率差求解水下航行器的速度，并传送给导航控制计算机；

虚拟线加速度计包括哥氏加速度计算装置、重力加速度计算装置和数据融合装置，哥氏加速度计算装置和重力加速度计算装置分别接收来自ADI实时仿真计算机的信号，并计算得到哥氏加速度和当地的重力加速度，通过数据融合装置得到水下航行器实时的比力信息，而后送至导航控制计算机；

导航控制计算机实时并行采集IMU惯导组件、卫星导航接收机、深度传感器、DVL测速仪输出的姿态、位置、深度、速度信息及虚拟线加速度计输出的比力信息，完成惯性导航和控制系统解算，输出水下航行器的操舵指令，驱动舵机运动；

舵机接收导航控制计算机输出的操作指令，模拟水下航行器垂直舵和水平舵的动作。舵机上的舵角反馈电位计将舵角信息反馈给ADI实时仿真计算机，构成整个系统的闭环反馈。

2.根据权利要求1所述的水下航行器组合导航与控制半实物仿真试验系统，其特征在于：所述的ADI实时仿真计算机、导航控制计算机和虚拟线加速度计分别通过I/O通讯模块连接由稳压电源提供的开机信号，确保导航控制计算机的惯导解算与ADI实时仿真计算机的模型解算及虚拟线加速度计的同步。

Images