专利名称：一种多水下机器人半物理仿真系统及其控制方法
技术领域：
本发明涉及多水下机器人控制和仿真技术领域，具体的说是一种基于混合式硬件 体系结构的多水下机器人半物理仿真系统及其控制方法。
背景技术：
为了满足水下机器人在控制系统软硬件设计和关键技术前期验证的需求，国内外 学者构建了各种应用于水下机器人的仿真平台，主要包括数字仿真平台和半物理仿真平 台。半物理仿真，也称半实物、硬件在回路(HIL，hardware in loop)仿真，在先进水下 机器人系统设计与开发中具有极其重要的意义。尤其对于多机器人系统而言，具有系统复 杂、存在各种不可预见性的特点，因此研究相应的仿真技术并建立其仿真环境，是设计复杂 控制系统、研究其关键技术及验证控制系统鲁棒性不可或缺的手段。与数字仿真不同的是，半物理仿真将真实的机器人控制器软硬件系统置于仿真环 境中，全过程、全状态进行仿真，及时发现机器人控制器在真实海洋环境中可能存在的软硬 件漏洞，提高系统可靠性。而这种半物理仿真只能对单一的机器人进行模拟控制，对于多机 器人，则无法实现模拟控制。目前，在相关报道中尚未发现多水下机器人半物理仿真装置。
发明内容
针对现有技术中存在的无法实现对于多机器人模拟控制这一空白，本发明要解决 的技术问题是提供一种能够对各机器人系统的软硬件内部关系、外部耦合等复杂过程进行 验证的多水下机器人半物理仿真系统及其控制方法。为解决上述技术问题，本发明采用的技术方案是本发明多水下机器人半物理仿真系统包括视景显示计算机，虚拟环境计算机，电 气转接装置以及机载自动驾驶仪，其中视景显示计算机，与虚拟环境计算机进行网络通信连接，实现海底地形、障碍物、 涌流和声纳传感器数据的模拟，以及各水下机器人位姿的三维立体显示；虚拟环境计算机，存有各水下机器人的运动学、动力学模型，通过模型解算，生成 各种虚拟传感器设备信息，以实现水下机器人的自主闭环控制，同时完成模拟各种故障状 况，以及模拟水声信道实现多机器人协作信息的转发；电气转接装置，将虚拟环境计算机扩展卡的标准电缆接口转接为可与多水下机器 人机载自动驾驶仪方便连接的接口；机载自动驾驶仪为多套，为真实的水下机器人软硬件控制系统，通过电气转接装 置获取虚拟环境计算机的传感器数据，根据自身的运动规划和闭环控制系统生成执行器控 制量经由电气转接装置发送给虚拟环境计算机，同时根据使命需求通过虚拟环境计算机向 其它机器人系统发送协作信息。所述机载自动驾驶仪包括CPU单元、串口扩展单元、外围扩展单元、开关控制单元、电源控制/监测单元以及推进系统控制单元，其中CPU单元通过PC104总线连接串口扩 展单元、外围扩展单元连接，通过RS485总线连接开关控制单元、电源控制/监测单元以及 推进系统控制单元。电源控制/监测单元监测能源系统的电压、电流以及故障状态，接收 CPU单元的指令，对能源系统实施控制；推进系统控制单元监测驱动装置的运行状态，对驱 动装置实施使能、转速、转向控制；开关控制单元监测水下机器人的入水、漏水情况，对各机 载传感器、压载设备开关实施控制。所述虚拟环境计算机包括工控机、数字IO单元、模拟量输入单元、模拟量输出单 元以及串口单元，其中，工控机连接上述各单元；数字IO单元的数字量输入端在机载自动 驾驶仪的控制下向工控机输入多水下机器人的电源开关以及机器人压载设备的动作开关 的仿真信号，数字量输出端向机载自动驾驶仪输出入水检测及漏水和漏油报警及其它故障 信号；模拟量输入单元在工控机的控制下向工控机输入多水下机器人推进系统的模拟量输 出设备的仿真信号；模拟量输出单元在工控机的控制下通过串口单元向机载自动驾驶仪输 出各种虚拟传感器的仿真信号。所述电气转接装置一端与虚拟环境计算机扩展卡的标准电缆接口相连，另一端设 有方便与各水下机器人机载自动驾驶仪连接的电缆接口。所述机载自动驾驶仪为多台同构水下机器人控制装置，或者为多台异构水下机器 人控制装置。本发明多水下机器人半物理仿真方法包括以下步骤初始使命装载及路径规划；根据上述规划的路径进行仿真控制，具体为虚拟环境计算机向各机载自动驾驶仪发送仿真开始指令；各机载自动驾驶仪在接收到上述仿真开始指令后，按照规划的结果开始运行，通 过内部的自主闭环控制程序控制计算推进系统的控制量，通过模拟输出接口转由电气转接 装置发送给虚拟环境计算机的模拟输入卡；虚拟环境计算机将上述推进系统的控制量作为输入，通过各机载自动驾驶仪的运 动学、动力学模型解算获得各虚拟传感器信息和位姿信息，并将传感器信息通过对应串口 卡、模拟输出卡转由电气转接装置发送给各机载自动驾驶仪；同时虚拟环境计算机将解算 获得的各水下机器人的位姿信息通过网络接口发送给视景显示计算机进行三维显示。本发明明方法还包括以下步骤判断各机载自动驾驶仪是否需要发送协作信息；各机载自动驾驶仪如需发送协作信息，则通过对应的RS232接口经由电气转接装 置发送给虚拟环境计算机；由虚拟环境计算机将协作信息转发给指定的其他机载自动驾驶仪。如果机载自动驾驶仪不需发送协作信息，则接续判断各机载自动驾驶仪是否需要 发送协作信息步骤。所述初始使命装载及路径规划包括；虚拟环境计算机向各机载自动驾驶仪和视景显示计算机装载初始使命参数，并接 收返回的使命确认信息；各机载自动驾驶仪正确完成路径规划程序后，向虚拟环境计算机返回路径规划结果；虚拟环境计算机将路径规划结果发送给视景显示计算机进行二维显示。本发明具有以下有益效果及优点1.本发明采用模块化的设计思想，在最小水下机器人控制系统的配置下，可方便 扩展传感器和设备节点，系统扩展性好、可移植性高。2.本发明具有各种虚拟模型，实现了多水下机器人运动控制、协作导航、水声组 网、队形控制等关键技术涉及的虚拟传感器接口，便于配置各种类型的水下机器人。3.本发明具有各种故障模拟接口，可模拟各种水下机器人的故障状态，验证控制 器的完备性。
图1为本发明电气结构框图；图2为本发明系统中机载自动驾驶仪的电气结构框图；图2A为本发明中机载自动驾驶仪的电源控制/监测单元框图；图2B为本发明系统中机载自动驾驶仪的推进系统控制单元框图；图2C为本发明系统中机载自动驾驶仪的开关控制单元电气结构框图；图3为本发明系统中虚拟环境计算机的电气结构框图；图4为本发明系统中电气转接装置电气结构框图；图5为本发明方法流程图。
具体实施例方式如图1所示，本发明多水下机器人半物理仿真系统包括视景显示计算机1，虚拟 环境计算机2，电气转接装置3以及机载自动驾驶仪4，其中视景显示计算机1，与虚拟环 境计算机2进行网络通信连接，实现海底地形、障碍物、涌流和声纳传感器数据的模拟，以 及各水下机器人位姿的三维立体显示；虚拟环境计算机2，存有各水下机器人的运动学、动 力学模型，通过模型解算，生成各种虚拟传感器设备信息，以实现水下机器人的自主闭环控 制，同时完成模拟各种故障状况，以及模拟水声信道实现多机器人协作信息的转发；电气转 接装置3，将虚拟环境计算机2扩展卡的标准电缆接口转接为可与多台水下机器人机载自 动驾驶仪4方便连接的接口 ；机载自动驾驶仪4，为多套，为真实的水下机器人软硬件控制 系统，通过获取虚拟环境计算机2的传感器数据，根据自身的运动规划和闭环控制系统生 成执行器控制量发送给虚拟环境计算机2，从而实现多水下机器人的自主行为，同时根据使 命需求通过虚拟环境计算机2向其它机器人系统发送协作信息。与单体水下机器人仿真模拟系统不同的是，多水下机器人仿真既可以模拟多台 (本实施例仿真装置采用四套机载自动驾驶仪，即第1 4机载自动驾驶仪I IV)同构水 下机器人控制系统，也可以模拟多台异构水下机器人控制系统。在多水下机器人协同完成 使命的过程中，能够对各机器人系统的软硬件内部关系、外部耦合等复杂过程进行验证。如图2、2A C所示，机载自动驾驶仪4包括CPU单元、串口扩展单元、外围扩展 单元、开关控制单元、电源控制/监测单元以及推进系统控制单元，其中CPU单元通过PC104 总线分别连接串口扩展单元、外围扩展单元连接，通过485总线连接开关控制单元、电源控制/监测单元以及推进系统控制单元，电源控制/监测单元监测能源系统的电压、电流以及 故障状态，接收CPU单元的指令，对水下机器人的能源实施控制；推进系统控制单元检测水 下机器人驱动装置的运行状态，对驱动装置实施使能、转速、转向控制；开关控制单元监测 水下机器人的入水、漏水情况，对水下机器人的各种设备进行控制。如图3所示，虚拟环境计算机2包括工控机、数字IO单元、模拟量输入单元、模拟 量输出单元以及串口单元，其中，工控机连接上述各单元；数字IO单元的数字量输入端在 机载自动驾驶仪4的控制下向工控机输入多水下机器人上一些驱动装置、传感器以及机载 设备的电源开关的仿真信号，数字量输出端向机载自动驾驶仪4输出入水检测及漏水和漏 油报警仿真信号；模拟量输入单元在机载自动驾驶仪4的控制下向工控机输入多水下机器 人推进系统的模拟量输出设备的仿真信号；模拟量输出单元在工控机的控制下向机载自动 驾驶仪4输出多机器人的深度、角速度陀螺等传感器的仿真信号；串口单元在工控机的控 制下向机载机动驾驶仪4输出各种虚拟传感器的仿真信号。如图4所示，电气转接装置3的一端与虚拟环境计算机扩展卡包括串口卡、DA卡、 AD卡、IO卡的标准电缆接口相连，另一端设有方便与各水下机器人机载自动驾驶4连接的接口。虚拟环境计算机2和机载自动驾驶仪4就是通过电气转接装置3相连，实现了水 下机器人的自主闭环控制与导航，同时，虚拟环境计算机2通过电气转接装置3对机载自动 驾驶仪4实现使命装载和控制，并将实时水下机器人位姿信息通过以太网发送给视景显示 计算机1进行二维、三维显示，而视景显示计算机1通过三维软件的碰撞检测功能来模拟声 学和光学传感器，并将结果发送给虚拟环境计算机2。本发明的视景显示计算机1采用戴尔公司的高性能图形工作站!decision 690。 视景显示计算机通过TCP/IP协议与虚拟环境计算机进行环境数据和UUV载体状态信息的 交互，实现海底地形、障碍物、水流、各水下机器人位姿的三维立体显示。本实施例发明中，虚拟环境计算机2的工作机采用研华工控机IPC610，数字IO单 元采用研华公司的48路隔离数字量IO卡PCI-1751，模拟量输入单元采用研华公司的12位 32路A/D数据采集卡PCL-8i;3B，模拟量输出单元采用12路隔离模拟量输出卡PCL-727，串 口单元采用研华公司的PCL-858。数字IO单元的数字量输入用于模拟多水下机器人上一些 设备的电源开关，如无线电信标、声定位系统、无线电天线、避碰声纳、温盐仪等，以及潜水 器压载设备的动作开关，如抛载压铁；数字IO单元的数字量输出用于模拟入水检测及漏水 和漏油检测信号；模拟量输入单元主要用于模拟推进系统其它模拟量输出设备以接收控制 量输入；模拟量输出单元用于输出深度、角速度陀螺等传感器得仿真信号；串口单元在工 控机的控制下用于产生各种虚拟传感器设备的仿真信号，如无线电、数字罗盘、惯性测量单 元、多普勒测速、水声通讯机等。机载自动驾驶仪采用基于PC104和RS485网络的混合体系结构，既具有PC014嵌 入式工业计算机紧凑的栈接结构、外围模块丰富的特点，又具有RS485总线结构简单、易扩 展的特点，开发方便。机载自动驾驶仪的CPU单元采用盛博公司的6235-133-32M，串口扩展 单元盛博公司的MSP-8，外围扩展单元采用盛博公司的ADT-650。开关控制单元以单片机ATmegaS为核心，通过RS-485接口接收CPU单元的指令， 根据命令对各机载传感器、设备的电源进行开关控制，同时检测各个设备开关状态。
电源控制/监测单元以单片机ATmegaS为核心，负责检测电池的电压、电流、温度、 压力及漏水等状态，并通过RS-485接口将数据返回到CPU单元。推进系统控制单元以单片机ATmega8为核心，通过RS-485接口接收CPU单元的命 令，根据命令控制各电机的使能、方向，控制D/A驱动各电机驱动模块，同时检测各电机的 故障状态，通过A/D采集各电机工作电流、温度，并通过RS-485接口向CPU单元发送。如图5所示，本发明多水下机器人半物理仿真方法包括以下步骤1)初始使命装载及路径规划虚拟环境计算机向各机载自动驾驶仪和视景显示计算机装载初始使命参数，并接 收返回的使命确认信息；各机载自动驾驶仪正确完成路径规划程序后，向虚拟环境计算机返回路径规划结 果；虚拟环境计算机将路径规划结果发送给视景显示计算机进行二维显示；2)根据上述规划的路径进行仿真控制虚拟环境计算机向各机载自动驾驶仪发送仿真开始指令；各机载自动驾驶仪在接收到上述仿真开始指令后，按照规划的结果开始运行，通 过内部的闭环控制系统计算推进系统的控制量，通过模拟输出接口转由电气转接装置发送 给虚拟环境计算机的模拟输入卡；虚拟环境计算机将上述推进系统的控制量作为输入，通过各机载自动驾驶仪的运 动学、动力学模型解算获得各虚拟传感器信息和位姿信息，并将传感器信息通过对应串口 卡、模拟输出卡转由电气转接装置发送给各机载自动驾驶仪；同时虚拟环境计算机将解算 获得的各水下机器人的位姿信息通过网络接口发送给视景显示计算机进行三维显示；判断各机载自动驾驶仪是否需要发送协作信息；各机载自动驾驶仪如需发送协作信息，则通过对应的RS232接口经由电气转接装 置发送给虚拟环境计算机；由虚拟环境计算机转发给指定的其他机载自动驾驶仪。
权利要求
1.一种多水下机器人半物理仿真系统，其特征在于包括视景显示计算机(1)，虚拟环 境计算机O)，电气转接装置(3)以及机载自动驾驶仪，其中视景显示计算机(1)，与虚拟环境计算机( 进行网络通信连接，实现海底地形、障碍 物、涌流和声纳传感器数据的模拟，以及各水下机器人位姿的三维立体显示；虚拟环境计算机O)，存有各水下机器人的运动学、动力学模型，通过模型解算，生成各 种虚拟传感器设备信息，以实现水下机器人的自主闭环控制，同时完成模拟各种故障状况， 以及模拟水声信道实现多机器人协作信息的转发；电气转接装置(3)，将虚拟环境计算机( 扩展卡的标准电缆接口转接为可与多水下 机器人机载自动驾驶仪(4)方便连接的接口 ；机载自动驾驶仪(4)，为多套，为真实的水下机器人软硬件控制系统，通过电气转接装 置C3)获取虚拟环境计算机O)的传感器数据，根据自身的运动规划和闭环控制系统生成 执行器控制量经由电气转接装置C3)发送给虚拟环境计算机O)，同时根据使命需求通过 虚拟环境计算机O)向其它机器人系统发送协作信息。
2.按权利要求1所述的多水下机器人半物理仿真系统，其特征在于所述机载自动驾驶 仪(4)包括CPU单元、串口扩展单元、外围扩展单元、开关控制单元、电源控制/监测单元 以及推进系统控制单元，其中CPU单元通过PC104总线连接串口扩展单元、外围扩展单元连 接，通过RS485总线连接开关控制单元、电源控制/监测单元以及推进系统控制单元。电源 控制/监测单元监测能源系统的电压、电流以及故障状态，接收CPU单元的指令，对能源系 统实施控制；推进系统控制单元监测驱动装置的运行状态，对驱动装置实施使能、转速、转 向控制；开关控制单元监测水下机器人的入水、漏水情况，对各机载传感器、压载设备开关 实施控制。
3.按权利要求1所述的多水下机器人半物理仿真系统，其特征在于所述虚拟环境计算 机(2)包括工控机、数字IO单元、模拟量输入单元、模拟量输出单元以及串口单元，其中，工 控机连接上述各单元；数字IO单元的数字量输入端在机载自动驾驶仪(4)的控制下向工控 机输入多水下机器人的电源开关以及机器人压载设备的动作开关的仿真信号，数字量输出 端向机载自动驾驶仪(4)输出入水检测及漏水和漏油报警及其它故障信号；模拟量输入单 元在工控机的控制下向工控机输入多水下机器人推进系统的模拟量输出设备的仿真信号； 模拟量输出单元在工控机的控制下通过串口单元向机载自动驾驶仪(4)输出各种虚拟传 感器的仿真信号。
4.按权利要求1所述的多水下机器人半物理仿真系统，其特征在于所述电气转接装置 (3) —端与虚拟环境计算机扩展卡的标准电缆接口相连，另一端设有方便与各水下机器人 机载自动驾驶仪(4)连接的电缆接口。
5.按权利要求1所述的多水下机器人半物理仿真系统，其特征在于所述机载自动驾 驶仪(4)为多台同构水下机器人控制装置，或者为多台异构水下机器人控制装置。
6.一种多水下机器人半物理仿真方法，其特征在于包括以下步骤初始使命装载及路径规划；根据上述规划的路径进行仿真控制，具体为虚拟环境计算机向各机载自动驾驶仪发送仿真开始指令；各机载自动驾驶仪在接收到上述仿真开始指令后，按照规划的结果开始运行，通过内部的自主闭环控制程序控制计算推进系统的控制量，通过模拟输出接口转由电气转接装置 发送给虚拟环境计算机的模拟输入卡；虚拟环境计算机将上述推进系统的控制量作为输入，通过各机载自动驾驶仪的运动 学、动力学模型解算获得各虚拟传感器信息和位姿信息，并将传感器信息通过对应串口卡、 模拟输出卡转由电气转接装置发送给各机载自动驾驶仪；同时虚拟环境计算机2将解算获 得的各水下机器人的位姿信息通过网络接口发送给视景显示计算机进行三维显示。
7.按权利要求6所述的多水下机器人半物理仿真方法，其特征在于还包括以下步骤判断各机载自动驾驶仪是否需要发送协作信息；各机载自动驾驶仪如需发送协作信息，则通过对应的RS232接口经由电气转接装置发 送给虚拟环境计算机；由虚拟环境计算机将协作信息转发给指定的其他机载自动驾驶仪。
8.按权利要求7所述的多水下机器人半物理仿真方法，其特征在于 如果机载自动驾驶仪不需发送协作信息，则接续判断各机载自动驾驶仪是否需要发送 协作信息步骤。
9.按权利要求6所述的多水下机器人半物理仿真方法，其特征在于初始使命装载及 路径规划包括；虚拟环境计算机向各机载自动驾驶仪和视景显示计算机装载初始使命参数，并接收返 回的使命确认信息；各机载自动驾驶仪正确完成路径规划程序后，向虚拟环境计算机返回路径规划结果； 虚拟环境计算机将路径规划结果发送给视景显示计算机进行二维显示。
全文摘要
本发明涉及一种多水下机器人半物理仿真系统及其控制方法，包括视景显示计算机，与虚拟环境计算机进行网络通信连接；虚拟环境计算机存有各水下机器人的运动学、动力学模型，通过模型解算，生成各种虚拟传感器设备信息；电气转接装置将虚拟环境计算机扩展卡的标准电缆接口转接为可与多水下机器人机载自动驾驶仪方便连接的接口；机载自动驾驶仪为真实的水下机器人软硬件控制系统，根据自身的运动规划和闭环控制系统生成执行器控制量经由电气转接装置发送给虚拟环境计算机，根据使命需求通过虚拟环境计算机向其它机器人系统发送协作信息。本发明可方便扩展传感器和设备节点，系统扩展性好、可移植性高，便于配置各种类型的水下机器人。
文档编号G05D1/00GK102117071SQ20091024902
公开日2011年7月6日 申请日期2009年12月30日 优先权日2009年12月30日
发明者任申真, 曾俊宝, 李一平, 谭亮 申请人:中国科学院沈阳自动化研究所