水下机器人四螺旋桨推进器控制方法
技术领域
本发明属于水下机器人自动化控制技术领域,特别是涉及一种水下机器人的四螺旋桨推进器控制方法。
背景技术
随着我国海洋渔业的发展,人类自身的条件已经不能满足水下工作的需要,水下机器人的出现,在人类开发、探索海洋资源以及水产养殖方面都发挥着越来越重要的作用。
目前,国外已经有了一系列商业化的水下机器人产品,1960年世界上第一台水下机器人诞生,取名为“CURV1”,世界上第一台用于商业的水下机器人是美国的RCV-225,它主要应用于近海的开发作业,体积小,质量轻,有四个推进器可以实现潜体的前进、后退及上升、下降功能,但是水下机器人由于其复杂的水下环境,加上本身机器人形状的不规则性,往往很难建立准确的数学模型,加上水流等因素的影响,水下机器人的运动控制方法就需要较好的适应性。为了适应复杂的水下环境,通过上位机有效的控制下位机,就目前发展情况来看,仍需要建立一套比较有效的控制方法,通过控制动力大小来控制水下机器人在水下的运动方式,这样也可以提高水下机器人的控制精度和控制效率。
发明内容
本发明针对上述技术问题,提出了一种水下机器人四螺旋桨推进器控制方法,该方法能够实现上位机对下位机的动力大小以及方向控制,不仅提高了水下工作效率,而且可以让水下机器人在水下以任意方向运动,提高了水下监测的灵活高效性,很好的满足了操作人员的需要。
本发明的技术方案为:
和现有技术相比,本发明提供了一种水下机器人四螺旋桨推进器控制方法,包括如下步骤:
(1)水下机器人上位机检测三维摇杆的实时位置将其转化成相应的参数通过485通信发给下位机;
(2)下位机接收到上位机发来的控制信息后,判断三维摇杆的三维运动参数up_dow,fro_bac,lef_rig,当摇杆不操作时,处于中间位置,摇杆的三维运动参数均为0;
(3)根据三维摇杆的三维运动参数控制四个螺旋桨推进器的推力大小以及方向,利用左垂直推进器和右垂直推进器控制水下机器人的上升和下潜运动,利用左水平推进器和右水平推进器控制水下机器人直线前进、直线后退、左转弯、右转弯、左前方前进、右前方前进、左后方后退、右后方后退动作。基于上述方案,本发明还进行如下改进:
当摇杆从中间位置顺时针旋转到头的过程中,第一维运动参数up_dow的读数位从1到(N-1)均匀递增,当摇杆从中间位置逆时针旋转到头的过程中,读数位从N到(2N-1)均匀递增,下位机通过判断第一维运动参数up_dow,控制水下机器人的上升和下潜运动;如果第一维运动参数up_dow≤(N-1),此时左垂直推进器旋转方向为正,右垂直推进器旋转方向为反,实现下潜运动;如果第一维运动参数up_dow≥N,左垂直推进器旋转方向为反,右垂直推进器旋转方向为正,实现上升运动。
当摇杆从中间位置向前到头的过程中,第二维运动参数fro_bac的读数位从1到(N-1)均匀递增,当摇杆从中间位置向后到头的过程中,读数位从N到(2N-1)均匀递增,下位机判断第二维运动参数fro_bac,控制水下机器人的前进和后退运动;如果第二维运动参数fro_bac≤(N-1)成立,同时再判断第三维运动参数lef_rig=0,实现直线前进动作;如果第二维运动参数fro_bac≥N成立,同时再判断第三维运动参数lef_rig=0,实现直线后退动作。
当摇杆从中间位置向右到头的过程中,第三维运动参数lef_rig的读数位从1到(N-1)均匀递增,当摇杆从中间位置向左到头的过程中,读数位从N到(2N-1)均匀递增,下位机判断第三维运动参数lef_rig,控制水下机器人的左转弯和右转弯运动;如果第三维运动参数lef_rig≤(N-1)成立,同时再判断第二维运动参数fro_bac=0,实现右转弯运动;如果第三维运动参数lef_rig≥N成立,同时再判断第二维运动参数fro_bac=0,实现左转弯运动。
如果第二维运动参数fro_bac≤(N-1)成立,则fro=fro_bac,为前进命令;同时再判断第三维运动参数lef_rig,如果第三维运动参数lef_rig≥N成立,则lef=lef_rig-N,右水平推进器旋转方向为反,右水平推进器动力为[Max×(fro+lef)/N]r/min,当(fro+lef)≥N时,取(fro+lef)=N;左水平推进器动力为[Max×(|fro-lef|)/N]r/min,当(fro-lef)≥0时,左水平推进器旋转方向为正,当(fro-lef)<0时,左水平推进器旋转方向为反,实现左前方前进动作;如果第三维运动参数lef_rig≤(N-1)成立,则rig=lef_rig,左水平推进器旋转方向为正,左水平推进器动力为[Max×(fro+rig)/N]r/min,当(fro+rig)≥N时,取(fro+rig)=N;右水平推进器动力为[Max×(|fro-rig|)/N]r/min,当(fro-rig)≥0时右水平推进器旋转方向为反,当(fro-rig)<0时右水平推进器旋转方向为正,实现右前方前进动作。
如果第二维运动参数N<fro_bac<(N+40)成立,则bac=fro_bac-N,为转弯过渡状态或转弯命令区;同时再判断第三维运动参数,如果第三维运动参数lef_rig≥N成立,则lef=lef_rig-N,右水平推进器旋转方向为反,右水平推进器动力为[Max×(bac+lef)/N]r/min,当(bac+lef)≥N时,取(bac+lef)=N;左水平推进器动力为[Max×(|bac-lef|)/N]r/min,当(bac-lef)≥0时左水平推进器旋转方向为正,当(bac-lef)<0时左水平推进器旋转方向为反,实现左转弯过渡状态;如果lef_rig≤(N-1)成立,则rig=lef_rig,左水平推进器旋转方向为正,左水平推进器动力为[Max×(bac+rig)/N]r/min,当(bac+rig)≥N时,取(bac+rig)=N;右水平推进器动力为[Max×(|bac-rig|)/N]r/min,当(bac-rig)≥0时右水平推进器旋转方向为反,当(bac-rig)<0时右水平推进器旋转方向为正,实现右转弯过渡状态。
如果第二维运动参数fro_bac≥(N+40)成立,则bac=fro_bac-N,为后退命令;同时再判断第三维运动参数,如果第三维运动参数lef_rig≥N成立,则lef=lef_rig-N,右水平推进器旋转方向为正,右水平推进器动力为[Max×(bac+lef)/N]r/min,当(bac+lef)≥N时,取(bac+lef)=N;左水平推进器动力为[Max×(|bac-lef|)/N]r/min,当(bac-lef)≥0时左水平推进器旋转方向为反,当(bac-lef)<0时左水平推进器旋转方向为正,实现左后方后退动作;如果第三维运动参数lef_rig≤(N-1)成立,则rig=lef_rig,左水平推进器旋转方向为反,左水平推进器动力为[Max×(bac+rig)/N]r/min,当(bac+rig)≥N时,取(bac+rig)=N;右水平推进器动力为[Max×(|bac-rig|)/N]r/min,当(bac-rig)≥0时右水平推进器旋转方向为正,当(bac-rig)<0时右水平推进器旋转方向为反,实现右后方后退动作。
本发明的技术效果为:
本发明通过上位机检测三维摇杆的实时位置将其转化成相应的参数通过485通信发给下位机,下位机接收到上位机发来的控制信息后,判断三维摇杆的三维运动参数up_dow,fro_bac,lef_rig,根据三维摇杆的命令参数控制四个螺旋桨推进器的推力大小以及方向,从而实现水下机器人在水中的运动控制;本发明不仅提高了水下工作效率以及水下机器人的控制精度和控制效率,而且提高了水下监测的灵活高效性,很好的满足了操作人员的需要。
附图说明
图1为本发明结构组成以及运动方向图。
图2为本发明上位机三维摇杆控制方向图。
图3为本发明一维运动参数控制示意图。
图4为本发明二维运动参数控制示意图。
图5为本发明三维运动参数控制示意图。
具体实施方式
下面结合附图说明本发明的具体实施方式:
由图1-5所示,本发明提供了一种水下机器人四螺旋桨推进器控制方法,水下机器人5包括左水平推进器3、右水平推进器4和左垂直推进器1、右垂直推进器2,每个推进器由一个电机和一个螺旋桨组成。四个推进器采用一种电机,但采用两对螺旋桨,水平推进器用一对,垂直推进器用一对,其中每对螺旋桨同向安装,但桨叶方向相反。上位机通过操作三维摇杆6来控制水下机器人5的运动方式。水平推进器提供水平推进动力,垂直推进器提供上升下潜动力,水下机器人5水平向前推进的时候为水平螺旋桨提供动力的两个电机分别反向旋转,故两个螺旋桨提供的动力都为向前;当两个电机同向旋转的时候,两个螺旋桨提供的动力一个向前一个向后,实现水下机器人5的转向,并且分别给予两个电机不同的动力,实现了转弯角度大小的控制。水下机器人5的上升下潜控制时左垂直推进器1和右垂直推进器2的两个电机不进行差速控制,而是同时给予相同的动力,通过控制动力大小控制水下机器人5的上升和下潜速度。下位机通过判断三维摇杆6的第一维运动参数实现控制水下机器人5的上升和下潜运动,通过判断第二和第三维运动参数,实现水下机器人5的直线前进、直线后退、左转弯、右转弯、左前方前进、右前方前进、左后方后退、右后方后退的运动控制。
本发明提供了一种水下机器人四螺旋桨推进器控制方法,包括如下步骤:
(1)水下机器人5上位机检测三维摇杆6的实时位置将其转化成相应的参数通过485通信发给下位机;
(2)下位机接收到上位机发来的控制信息后,判断三维摇杆6的三维运动参数up_dow,fro_bac,lef_rig,当摇杆6不操作时,处于中间位置,摇杆6的三维运动实时参数均为0;
(3)根据三维摇杆6的三维运动参数控制四个螺旋桨推进器的推力大小以及方向,利用左垂直推进器1和右垂直推进器2控制水下机器人5的上升和下潜运动,利用左水平推进器3和右水平推进器4控制水下机器人5直线前进、直线后退、左转弯、右转弯、左前方前进、右前方前进、左后方后退、右后方后退动作。
当摇杆6从中间位置顺时针旋转到头的过程中,第一维运动参数up_dow的读数位从1到(N-1)均匀递增,当摇杆6从中间位置逆时针旋转到头的过程中,读数位从N到(2N-1)均匀递增,下位机通过判断第一维运动参数up_dow,控制水下机器人5的上升和下潜运动;如果第一维运动参数up_dow≤(N-1),此时左垂直推进器1旋转方向为正,右垂直推进器2旋转方向为反,实现下潜运动;如果第一维运动参数up_dow≥N,左垂直推进器1旋转方向为反,右垂直推进器2旋转方向为正,实现上升运动。
当摇杆6从中间位置向前到头的过程中,第二维运动参数fro_bac的读数位从1到(N-1)均匀递增,当摇杆6从中间位置向后到头的过程中,读数位从N到(2N-1)均匀递增,下位机判断第二维运动参数fro_bac,控制水下机器人5的前进和后退运动;如果第二维运动参数fro_bac≤(N-1)成立,同时再判断第三维运动参数lef_rig=0,实现直线前进动作;如果第二维运动参数fro_bac≥N成立,同时再判断第三维运动参数lef_rig=0,实现直线后退动作。
当摇杆6从中间位置向右到头的过程中,第三维运动参数lef_rig的读数位从1到(N-1)均匀递增,当摇杆6从中间位置向左到头的过程中,读数位从N到(2N-1)均匀递增,下位机判断第三维运动参数lef_rig,控制水下机器人5的左转弯和右转弯运动;如果第三维运动参数lef_rig≤(N-1)成立,同时再判断第二维运动参数fro_bac=0,实现右转弯运动;如果第三维运动参数lef_rig≥N成立,同时再判断第二维运动参数fro_bac=0,实现左转弯运动。
如果第二维运动参数fro_bac≤(N-1)成立,则fro=fro_bac,为前进命令;同时再判断第三维运动参数lef_rig,如果第三维运动参数lef_rig≥N成立,则lef=lef_rig-N,右水平推进器4旋转方向为反,右水平推进器4动力为[Max×(fro+lef)/N]r/min,当(fro+lef)≥N时,取(fro+lef)=N;左水平推进器3动力为[Max×(|fro-lef|)/N]r/min,当(fro-lef)≥0时,左水平推进器3旋转方向为正,当(fro-lef)<0时,左水平推进器3旋转方向为反,实现左前方前进动作;如果第三维运动参数lef_rig≤(N-1)成立,则rig=lef_rig,左水平推进器3旋转方向为正,左水平推进器3动力为[Max×(fro+rig)/N]r/min,当(fro+rig)≥N时,取(fro+rig)=N;右水平推进器4动力为[Max×(|fro-rig|)/N]r/min,当(fro-rig)≥0时右水平推进器4旋转方向为反,当(fro-rig)<0时右水平推进器4旋转方向为正,实现右前方前进动作。
如果第二维运动参数N<fro_bac<(N+40)成立,则bac=fro_bac-N,为转弯过渡状态或转弯命令区;同时再判断第三维运动参数,如果第三维运动参数lef_rig≥N成立,则lef=lef_rig-N,右水平推进器4旋转方向为反,右水平推进器4动力为[Max×(bac+lef)/N]r/min,当(bac+lef)≥N时,取(bac+lef)=N;左水平推进器3动力为[Max×(|bac-lef|)/N]r/min,当(bac-lef)≥0时左水平推进器3旋转方向为正,当(bac-lef)<0时左水平推进器3旋转方向为反,实现左转弯过渡状态;如果lef_rig≤(N-1)成立,则rig=lef_rig,左水平推进器3旋转方向为正,左水平推进器3动力为[Max×(bac+rig)/N]r/min,当(bac+rig)≥N时,取(bac+rig)=N;右水平推进器4动力为[Max×(|bac-rig|)/N]r/min,当(bac-rig)≥0时右水平推进器4旋转方向为反,当(bac-rig)<0时右水平推进器4旋转方向为正,实现右转弯过渡状态。
如果第二维运动参数fro_bac≥(N+40)成立,则bac=fro_bac-N,为后退命令;同时再判断第三维运动参数,如果第三维运动参数lef_rig≥N成立,则lef=lef_rig-N,右水平推进器4旋转方向为正,右水平推进器4动力为[Max×(bac+lef)/N]r/min,当(bac+lef)≥N时,取(bac+lef)=N;左水平推进器3动力为[Max×(|bac-lef|)/N]r/min,当(bac-lef)≥0时左水平推进器3旋转方向为反,当(bac-lef)<0时左水平推进器3旋转方向为正,实现左后方后退动作;如果第三维运动参数lef_rig≤(N-1)成立,则rig=lef_rig,左水平推进器3旋转方向为反,左水平推进器3动力为[Max×(bac+rig)/N]r/min,当(bac+rig)≥N时,取(bac+rig)=N;右水平推进器4动力为[Max×(|bac-rig|)/N]r/min,当(bac-rig)≥0时右水平推进器4旋转方向为正,当(bac-rig)<0时右水平推进器4旋转方向为反,实现右后方后退动作。