一种基于多推进器的水下航行器控制方法与流程

本发明涉及机器人控制技术领域，具体涉及一种基于多推进器的水下航行器控制方法。

背景技术：
随着科技的发展，人类已经进入了开发、利用海洋资源的时代。而水下航行器是开发利用海洋资源的重点装备。水下航行器不仅在开发利用海洋方面起着重大作用，同时也是水下探测、打捞、救护的重要工具。要保证下航行器能有效地完成上述工作，就要能很好的控制航行器的动作和控位精度。由于水下航行器工作环境的复杂，加上本身形状的不规则性，往往很难建立良好的数学模型，难以设计出适应性良好的控制方法。若想控制水下航行器从A点位置到B点位置，由于海里的水动力环境复杂，故从A点到达B点的路径为非直线运动，在行驶过程中，需要进行多种前进、后退和转弯等的组合动作。然而，目前大多水下航行器的推进器配置方式大多采用左右对称方式排布，仅能实现前进、后退和转弯功能。其可供选择的运动轨迹较少，进而增加了控制难度。此外，由于水下航行器的转弯半径较大，当水下航行器接近B点位置时，很难实现精确的到达该位置点B，即精度较低。

技术实现要素：
有鉴于此，本发明提供了一种基于多推进器的水下航行器控制方法，能够灵活的控制水下航行器的运动，实现水下航行器的前进后退、左右平移、原地旋转或任意半径的转弯。进而提高了灵活性和机动性能，而且，任意半径的转弯，提高了精度。一种基于多推进器的水下航行器控制方法：所述水下航行器包括七个推进器，其中四个推进器安装在水下航行器的水平安装面上，另外三个推进器与所述水平安装面呈一定角度安装固定；所述安装在水平安装面上的四个推进器的排布方式为：以水平安装面的中心位置为原点o，建立平面直角坐标系，定义水下航行器艏艉方向作为y轴，水平安装面内与y轴垂直的方向作为x轴，形成xoy平面；x轴和y轴将所述水平安装面划分为四个象限，形成四个安装区；定义推进器螺旋桨所在端为尾部，其对立端为头部；第一推进器、第二推进器、第三推进器和第四推进器对应地固定在第二、一、四和三象限上，且第三推进器的头部朝向x轴，尾部朝向y轴摆放；第四推进器的头部朝向y轴，尾部朝向x轴摆放；第二推进器与第三推进器呈x轴对称；第一推进器与第四推进器呈x轴对称；z轴方向与x轴和y轴方向均垂直，形成三维直角坐标系；在xoz平面内固定两个推进器，两个推进器呈z轴对称，且两个推进器的头部朝向原点o安装；另一个推进器与水平安装面垂直、且头部朝向xoy平面安装在水下航行器的艉部。所述水平安装面上的四个推进器用于控制水下航行器的前进、后退、左移、右移、原地旋转和任意半径的转弯：根据水平安装面上的四个推进器的头尾安装方向以及水下航行器的移动方向，进行力学分析，确定每个推进器所对应电机的转动方向，之后，根据电机的转动方向和水下航行器的移动方向，设定电机的转速，进而实现四个电机转动后的合力方向与水下航行器的移动方向一致；与所述的水平安装面呈一定角度的三个推进器用于控制水下航行器垂直上浮或下潜；当控制水下航行器垂直上浮时：安装在xoz平面内的两个推进器的电机等速正转；另一个推进器的电机根据水下航行器的纵倾姿态角，自动调节电机的转速和转向；当控制水下航行器垂直下潜时：安装在xoz平面内的两个推进器的电机等速反转；另一个推进器的电机根据水下航行器的纵倾姿态角，自动调节电机的转速和转向。较佳地，水平安装面上的四个推进器控制前进、后退、左移、右移、原地旋转和任意半径的转弯的具体方法为：第一推进器、第二推进器、第三推进器和第四推进器对应的电机分别为第一电机、第二电机、第三电机和第四电机；前进：第一电机、第二电机、第三电机和第四电机均保持正转，同时第一电机和第二电机转速相等，第三电机和第四电机转速相等；后退：第一电机、第二电机、第三电机和第四电机均保持反转，同时第一电机和第二电机转速相等，第三电机和第四电机转速相等；左移：第一电机和第三电机反转，第二电机和第四电机正转，同时第一电机和第四电机转速相等，第二电机和第三电机转速相等；右移：第一电机和第三电机正转，第二电机和第四电机反转，同时第一电机和第四电机转速相等，第二电机和第三电机转速相等；原地左旋转：第一电机和第四电机反转，第二电机和第三电机正转，第一电机、第二电机、第三电机和第四电机的转速相同；原地右旋转：第一电机和第四电机正转，第二电机和第三电机反转，第一电机、第二电机、第三电机和第四电机的转速相同；左转弯：第一电机和第四电机反转，第二电机和第三电机正转，第一电机、第二电机、第三电机和第四电机的转速相同；并调整转速；右转弯：第一电机和第四电机正转，第二电机和第三电机反转，第一电机、第二电机、第三电机和第四电机的转速相同；并调整转速。有益效果：本发明采用七个推进器，分别以相互对称的方式排布在水下航行器上，通过改变每个推进器的电机的转速和方向，以实现力的相互叠加或抵消，进而实现水下航行器的前进后退、左右平移、原地旋转或任意半径的转弯，同时由于潜航器在设计加工中难以保证其水中的阻力对称平衡，这样潜航器在航行过程中难以直线航行，如果推进器采用对称布置控制方式，各个方向的不平衡性难以同时解决，采用本方案的控制方式可以较好的克服上述问题。附图说明图1(a)为本发明结构示意图；图1(b)为本发明结构初始情况下的受力分析示意图；图2(a)为前进时手柄操作与电机动作的关系图；图2(b)为前进时电机受力分析示意图；图3(a)为后退时手柄操作与电机动作的关系图；图3(b)为后退时电机受力分析示意图；图4(a)为左移时手柄操作与电机动作的关系图；图4(b)为左移时电机受力分析示意图；图5(a)为右移时手柄操作与电机动作的关系图；图5(b)为右移时电机受力分析示意图；图6(a)为左旋转时手柄操作与电机动作的关系图；图6(b)为左旋转时电机受力分析示意图；图7(a)为右旋转时手柄操作与电机动作的关系图；图7(b)为右旋转时电机受力分析示意图；图8(a)为上浮时手柄操作与电机动作的关系图；图8(b)为上浮时电机受力分析示意图；图9(a)为下潜时手柄操作与电机动作的关系图；图9(b)为下潜时电机受力分析示意图；具体实施方式下面结合附图并举实施例，对本发明进行详细描述。本发明提供了一种基于多推进器的水下航行器控制方法；本发明共采用七个推进器，其中四个推进器安装在水下航行器的水平安装面上，另外三个推进器与所述的水平安装面呈一定角度安装固定。其中，选取水下航行器的下表面作水平安装面。其中，安装在水平安装面上的推进器的排布为：如图1(a)所示的方式，固定推进器，采用现有技术中的手柄控制方式，通过控制推进器的正或反转，即可实现水下航行器的前进后退或左右平移如图1(a)和1(b)所示，以水平安装面的中心位置为原点o，建立平面直角坐标系，定义水下航行器艏艉方向作为y轴，水平安装面内与其垂直的方向作为x轴，形成xoy平面；x轴和y轴将所述水平安装面划分为四个象限，其中，定义右上为第一象限，逆时针顺次排序；形成四个安装区；定义推进器螺旋桨所在端为尾部，其对立端为头部；第一推进器、第二推进器、第三推进器和第四推进器对应地固定在第二、一、四和三象限上，且第一推进器头部的安装位置与第二推进器头部的安装位置呈y轴对称；第二推进器头部的安装位置与第三推进器尾部的安装位置呈x轴对称；第三推进器尾部的安装位置和第四推进器尾部的安装位置呈y轴对称；这样一来，通过改变推进器的正或反转以及推进器所受的转速，便可实现对水下航行器的左右横移或前进后退。当手柄1向前推时，如图2(a)所示的手柄操作与电机动作的关系图，第一电机、第二电机、第三电机和第四电机均保持正转，同时第一电机和第二电机转速相等，第三电机和第四电机转速相等；这时，如图2(b)所示，分析第一电机、第二电机、第三电机和第四电机的受力会发现，将产生前进合力，控制水下航行器前进动作，前进速度与手柄量程正比例关系。当手柄1向后推时，如图3(a)所示的手柄操作与电机动作的关系图，第一电机、第二电机、第三电机和第四电机均保持反转，同时第一电机和第二电机转速相等，第三电机和第四电机转速相等；这时，如图3(b)所示，分析第一电机、第二电机、第三电机和第四电机的受力会发现，将产生后退合力，控制水下航行器后退动作，后退速度与手柄量成正比例关系。当手柄1水平向左推时，如图4(a)所示的手柄操作与电机动作的关系图，第一电机和第三电机反转，第二电机和第四电机正转，同时第一电机和第四电机转速相等，第二电机和第三电机转速相等，这时，如图4(b)所示，分析第一电机、第二电机、第三电机和第四电机的受力会发现，将产生左移合力，控制水下航行器水平左移。左移速度与手柄量成正比例关系。当手柄1水平向右推时，如图5(a)所示的手柄操作与电机动作的关系图，第一电机和第三电机正转，第二电机和第四电机反转，同时第一电机和第四电机转速相等，第二电机和第三电机转速相等，这时，如图5(b)所示，分析第一电机、第二电机、第三电机和第四电机的受力会发现，将产生右移合力，控制水下航行器水平右移。右移速度与手柄量成正比例关系。当实现前进转弯、后退转弯、左移转弯或右移动转弯时，只需对应的根据上述四种操作，通过改变电机的转速，即可实现；当手柄2水平向左推时，如图6(a)所示的手柄操作与电机动作的关系图，第一电机和第四电机反转，第二电机和第三电机正转，第一电机、第二电机、第三电机和第四电机的转速相同；这时，如图6(b)所示，分析第一电机、第二电机、第三电机和第四电机的受力会发现，控制水下航行器原地左旋转。当转速不同时，即可实现左旋转，其半径跟转速相关。当手柄2水平向右推时，如图7(a)所示的手柄操作与电机动作的关系图，第一电机和第四电机正转，第二电机和第三电机反转，第一电机、第二电机、第三电机和第四电机的转速相同；这时，如图7(b)所示，分析第一电机、第二电机、第三电机和第四电机的受力会发现，控制水下航行器原地右旋转。当转速不同时，即可实现右旋转，其半径跟转速相关。综上所述可知，这样做的好处在于：一旦给定其中两个相邻的推进器推力(即转速)相同时，如果电机转向相同，则在水平方向和垂直方向产生合力，如果一个电机正转，一个电机反转，则在其中一个方向上的力将会抵消，另一个方向上的力将会产生合力。这样一来，只需通过控制电机的转向以及相邻两个电机的转速，便能够实现左右横移或前进后退，而且，转速的大小还能够对应的获得推进器的推力。较佳地，为保证水下航行器在各方向的行驶速度较为均匀，即为防止由于推进器的安装角度而导致推进器受力分解后的横轴方向与纵轴方向的力不相等，进而导致合力较大的方向的运动速度较大，合力较小的方向运动速度较小，本发明将推进器与水平安装面呈45度角安装；以实现各方向最佳的航向速度。与水平安装面呈一定角度固定在水平安装面的具体排布方法为：在xoy平面基础上，再以水平安装面的中心位置为原点o，定义与x轴方向和y轴方向均垂直的方向作为z轴，建立三维直角坐标系；在xoz平面内固定两个推进器，且两个推进器的头部朝向原点o安装；另一个推进器与水平安装面垂直、且头部朝向xoy平面安装在水下航行器的艉部。其好处在于：1、一旦水下航行器下方安装有设备，此种布置便于推进器排水。2、可以在x轴方向产生分力，进而保证水下航行器平稳上升或下降。3、双推进器模式在保证潜航器平稳性的同时极大的提高了下潜上浮速度。将另一个推进器与水平安装面垂直安装在水下航行器的艉部；依据上述排布方式，采用现有技术中的手柄控制方式，通过控制推进器的正或反转，即可实现水下航行器原地旋转和任意半径的转弯；设定推进器5和推进器6固定在xoz平面内的任意位置；推进器7与水平安装面垂直的、且头部朝向xoy平面安装在水下航行器的艉部。推进器5、推进器6和推进器7对应的电机分别为电机5、电机6和电机7；当手柄2前推时，如图8(a)所示的手柄操作与电机动作的关系图，电机5和电机6等速正转，这时，如图8(b)所示，分析电机5和电机6的受力会发现，产生上浮合力，控制水下航行器垂直上浮。当手柄2后推时，如图9(a)所示的手柄操作与电机动作的关系图，电机5和电机6等速反转，这时，如图9(b)所示，分析电机5和电机6的受力会发现，从而产生下潜合力，控制水下航行器垂直下潜。所述电机7不受操控；电机7根据水下航行器的纵倾姿态角，自动调节电机的转速和转向，以保持平衡。综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。