水上机器人智能定速巡航控制方法及系统与流程

本发明涉及机器人技术领域，特别涉及一种水上机器人智能定速巡航控制方法及系统。

背景技术：

定速巡航是源自汽车的一种速度控制系统，其作用是按司机要求的速度合开关之后，不用踩油门踏板就自动地保持车速，使车辆以固定的速度行驶。采用了这种技术，当在高速公路上长时间行车后，司机就不用再去控制油门踏板，减轻了疲劳，同时减少了不必要的车速变化，可以节省燃料。

相比汽车，水上机器人的环境更复杂，定速巡航考虑因素更多、系统复杂度更高。比如，水质采样等特殊服务场景要求水上机器人在作业时保持一定的速度(比如2米/秒)，完成作业后能以其他速度(通常是全速)返航。由于水流等复杂因素导致操作员在陆地上无法估测航行速度以及无法通过摇杆给出比较精确的油门控制信号。因此迫切需要水上机器人能自主控制航速，操作员只需要通过遥控器或者地面基站调整水上机器人的航向即可。

现有的水上机器人作业时仅支持自主巡航和手动控制，自主巡航不可以控制方向，不满足特殊作业时操控需求，且在有的小型水域可能无法规划出合理的作业路线；手动控制时，操作员试探性的调整油门主观判断达到预期的速度并让遥控器摇杆保持以达到稳定在某一个速度，由于水流等因素导致实际航行速度与期望的巡航速度可能存在较大的偏差，且操作不便。

技术实现要素：

为了克服上述现有技术的不足，本发明提供了一种水上机器人智能定速巡航控制方法及系统，提出“半自动”定速巡航模式，实现了目标巡航速度控制更精准、安全以及操作员更易用。

本发明一种水上机器人智能定速巡航控制方法，包括：

S101、设置系统参数；设置包括目标巡航速度参数在内的动力参数；

S102、获取水流速度；

S103、根据水流速度判断是否满足定速巡航要求，如果不满足要求，执行最近最快的返回智能保护；

S104、在满足定速巡航要求的情形下，进入定速巡航模式；

S105、在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变。

本发明水上机器人智能定速巡航控制系统，包括分别与主控器相连接的系统参数设置单元、模式控制单元、水流速度获取单元和动力调整单元：

所述参数设置单元用于设置系统参数；设置包括目标巡航速度参数在内的动力参数；

所述水流速度获取单元用于获取水流速度；

所述主控器用于根据水流速度判断是否满足定速巡航要求；如果不满足要求，执行最近最快的返回智能保护；

所述模式控制单元用于进行模式判断，包括手动控制航向模式、按规定的路线自主航行模式和定速巡航模式，在满足定速巡航要求的情形下进入定速巡航模式；

所述动力调整单元用于在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变。

与现有技术相比，本发明将定速巡航功能在水上机器人在实现，在更复杂的环境实现了动力预估、自主航行和手动控制航向等，能更准确、更安全的实现速度控制，而且将定速巡航作为一种独立的工作模式、快速的的进入和退出、速率可调整、可以自动的全速返航，使得本发明更实用、更易用。

附图说明

图1为本发明水上机器人智能定速巡航控制方法优选实施例流程示意图；

图2为本发明获取水流速度的流程示意图；

图3为本发明横向加速度计算的流程示意图；

图4为本发明获得油门实际输出的流程示意图；

图5为本发明水上机器人智能定速巡航控制系统优选实施例结构示意图。

具体实施方式

为使本发明的目的、技术方案和优点表达得更加清楚明白，下面结合附图及具体实施案例对本发明做进一步详细说明。

本发明水上机器人智能定速巡航控制方法，如图1所示，包括

S101、设置系统参数；包括设置目标巡航速度和最大转弯油门增益等动力参数。

S102、获取水流速度；

S103、根据水流速度判断是否满足定速巡航要求；如果不满足要求，执行最近最快的返回智能保护；

S104、在满足定速巡航要求的情形下进入定速巡航模式；

S105、在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变；

本发明设定目标巡航速度，系统自动的进行动力判断，在动力满足的情况自动继续完成当前作业，在动力不满足的情况下执行最近最快的返回智能保护；

S101所述设置目标巡航速度，可通过地面基站软件可视化菜单设置，或者通过主控器脚本文件修改。

从操作来看，在设置目标巡航速度之后，可一键指令进入定速巡航工作模式，优选在遥控器上设置自定义拨码开关并在主控器侧适配，也可以通过地面基站软件在便于操作的位置设置按钮一键操作。

作为一种可实现的方式，所述获取水流速度包括：直接读取水流测速传感设备的水流速度值。

优选地，作为另一种可实现的方式，所述获取水流速度包括：

水流速度V＝Vmax-L/t

其中，Vmax表示静水条件下最大航行速度；L表示自动设置的航向前方GPS坐标点Waypoint与当前位置GPS坐标点的距离；t表示航行到Waypoint的耗时。

作为一种可实现方式，本发明所述获取水流速度，如图2所示，包括

S201、主控器通过驱动探测判断是否搭载水流测速传感设备，如果有则进入S202，否则进入S203

S202、直接读取水流速度；

S203、估计水流速度，具体包括

S203-1、调整航向平行水流，优选逆流；

S203-2、主控器后台自动设置一个航航向前方GPS坐标点Waypoint，距离当前位置距离L；

S203-3、记录以静水条件下最大航行速度Vmax航行到Waypoint的耗时t，

S203-4、计算水流速度；

V＝Vmax-L/t

优选地，可以重复操作多次，测试几组数据并通过合并算法处理得到相对更准确的水流速度值。

优选地，作为一种可实现的方式，所述根据水流速度判断是否满足定速巡航要求，包括：若静水条件下最大航行速度与水流速度的差值大于目标巡航速度，则满足定速巡航要求，否则不满足定速巡航要求。

从操作来看，如果航速动力系统不满足要求，则通过无线信号向地面基站软件发送警告以及上报水流速度，以便操作员调整控制策略。

优选地，系统支持应急情况下返回智能保护。当水流速度超过静水条件下最大航行速度时，水上机器人有可能被冲走，需要自动执行最短距离靠岸。具体方法是根据估算的水流速度以及记录的岸边出发点的距离，本实施例利用三角函数等原理计算出逆流航行的航向角，可以保证靠岸位置距离出发点最近。

优选地，作为一种可实现的方式，所述在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变，包括计算横向加速度lat_acc，包括如果是手动控制航向模式，通过遥控器输入的航行控制信号PWM值计算横向加速度lat_acc：

lat_acc＝g_force×(pwm_to_angle/C_A)

其中：C_A是一个计算常量，优选地，为了计算方便，其值可采用等边直角三角形的夹角，即45度，乘以100，即C_A＝45×100；g_force＝ground_speed×ground_speed/turn_radius，g_force表示最大的转弯加速度，turn_radius为转弯半径,其值由系统设置，ground_speed为参考地面速度，其值从主控器的状态记录中获取；pwm_to_angle表示第一转弯角度，其是将遥控器输入的航行控制信号PWM值转换成角度值；

优选地，pwm_to_angle表示第一转弯角度，其是将遥控器输入的航行控制信号PWM值转换成角度值，具体包括：

如果PWM大于rc_trim_high，则

pwm_to_angle＝_high×(PWM-rc_trim_high))/(rc_max-rc_trim_high)；

否则

pwm_to_angle＝_high×(PWM-rc_trim_low))/(rc_trim_low-rc_min)

其中，rc_trim_low是遥控器输入中值减去DeadZone，rc_trim_high是遥控器输入中值加上DeadZone,遥控器输入中值＝(_high+_low)/2，DeadZone表示在电平翻转时插入一个时间间隔，该时间间隔根据不同的遥控器设置，_high是遥控器的标准最大输入,_low是遥控器的标准最小输入，rc_max是遥控器扩大量程的最大值，rc_min是遥控器扩大量程的最小值。

可选地，根据驱动检测是否有航行控制信号PWM输入判断是否是手动控制航向，若有航行控制信号PWM输入则为手动控制航向模式。

优选地，作为另一种可实现的方式，所述在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变，包括计算横向加速度，包括如果是按规定的路线自主航行模式，采用以下方式计算横向加速度，如图3所示：

S301、计算目标航向角C_y；

其中，C_lng表示当前位置点的经度，C_lat表示当前位置点的纬度，N_lng表示下一个航标位置点的经度，N_lat表示下一个航标位置点的纬度；

S302、通过目标航向角C_y计算当前位置到下一个航标位置点的直线距离Lcn：

Lcn＝2×R×sin(C_y/2)

其中，R是航迹半径，通常取值地球半径；

S303、计算上一个航标位置点与当前位置点所形成的向量与当前位置与下一个航标位置点所形成的向量之间的夹角a；

夹角a的计算可采用向量间夹角计算的常用手段，不再赘述。

S304、计算横向加速度

lat_acc＝ground_speed×ground_speed/Lcn×sin(a)；

其中，ground_speed为参考地面速度。

优选地，所述在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变，包括如果系统有智能避障功能，采用以下方式更新横向加速度：

lat_acc＝lat_acc+(turn_angle/C_A)×turn_max_g

其中，turn_angle表示获取的第二转弯角度，turn_max_g是根据船体类型设置的最大转弯加速度，单位是重力加速度。

优选地，在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变，包括基于横向加速度计算油门实际输出值,如图4所示，具体包括：

S401、根据橫向加速度计算转弯比率turn_rate

turn_rate＝lat_acc/turn_max_g×G，

其中，G是重力加速度。

S402、根据系统限制的最大转弯角度Tmax(默认取值是90，即允许最大90度转弯)，以及系统限制的最大转弯油门增益T_Gain(避免油门突变过大造成系统不稳定，百分比乘以100),计算转弯限制系数turn_ratio，其中C_y是前面计算的目标偏航角：

turn_ratio＝((100-T_Gain)×(C_y-yaw_sensor)/Tmax)/100

系统限制的最大转弯角度Tmax和系统限制的最大转弯油门增益T_Gain由系统设定,yaw_sensor表示航向角，从主控器获取。

S403、根据转弯比率和转弯限制系数计算油门降速比率turn_reduction

turn_reduction＝1.0-turn_rate×turn_ratio

S404、根据油门降速比率修正目标巡航速度speed_cruise

speed_cruise＝speed_cruise×turn_reduction

S405、根据修正的目标巡航速度与参考地面速度ground_speed的差值进行PID调节，得到油门偏差throttle_err；

PID调节方法可采用现有技术，不再赘述。

S406、根据油门偏差修正期望油门值，得到油门实际输出值。

throttle＝throttle_target+throttle_err

其中throttle_target是设定的speed_cruise对应的期望油门值。

本发明在定速巡航模式下，基于水上机器人主控器航姿参考系统的地面速度向量等参等周期性动态计算油门动力，保证速度恒定；在定速巡航模式下，支持修改巡航速度，自动执行动力判断和继续作业；在不满足定速巡航的情形下，可执行最近最快的自动返回智能保护策略。

本发明水上机器人智能定速巡航控制系统，如图5所示，包括分别与主控器相连接的系统参数设置单元、模式控制单元、水流速度获取单元和动力调整单元；

所述参数设置单元用于设置系统参数；包括设置目标巡航速度和最大转弯油门增益等动力参数。

所述水流速度获取单元用于获取水流速度；

所述主控器用于根据水流速度判断是否满足定速巡航要求；如果不满足要求，执行最近最快的返回智能保护；

所述模式控制单元用于判断模式，包括手动控制航向模式、按规定的路线自主航行模式和定速巡航模式，在满足定速巡航要求的情形下进入定速巡航模式；

所述动力调整单元用于在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变；

本发明设定目标巡航速度，系统自动进行动力判断，在动力满足的情况自动继续完成当前作业，在动力不满足的情况下执行最近最快的返回智能保护；

所述参数设置单元设置目标巡航速度，可通过地面基站软件可视化菜单设置，或者通过主控器脚本文件修改。

从操作来看，在设置目标巡航速度之后，可一键指令进入定速巡航工作模式，优选在遥控器上设置自定义拨码开关并在主控器侧适配，也可以通过地面基站软件在便于操作的位置设置按钮一键操作。

作为一种可实现的方式，水流速度获取单元所述获取水流速度包括：直接读取水流测速传感设备的水流速度值。

优选地，作为另一种可实现的方式，水流速度获取单元所述获取水流速度包括：

水流速度V＝Vmax-L/t

其中，Vmax表示静水条件下最大航行速度；L表示自动设置的航向前方GPS坐标点Waypoint与当前位置GPS坐标点的距离；t表示航行到Waypoint的耗时。

作为一种可实现方式，水流速度获取单元所述获取水流速度，如图2所示，包括

S201、主控器通过驱动探测判断是否搭载水流测速传感设备，如果有则进入S202，否则进入S203

S202、直接读取水流速度；

S203、估计水流速度，具体包括

S203-1、调整航向平行水流，优选逆流；

S203-2、主控器后台自动设置一个航航向前方GPS坐标点Waypoint，距离当前位置距离L；

S203-3、记录以静水条件下最大航行速度Vmax航行到Waypoint的耗时t，

S203-4、计算水流速度；

V＝Vmax-L/t

优选地，可以重复操作多次，测试几组数据并通过合并算法处理得到相对更准确的水流速度值。

优选地，作为一种可实现的方式，主控器所述根据水流速度判断是否满足定速巡航要求，包括：若静水条件下最大航行速度与水流速度的差值大于目标巡航速度，则满足定速巡航要求，否则不满足定速巡航要求。

从操作来看，如果航速动力系统不满足要求，则通过无线信号向地面基站软件发送警告以及上报水流速度，以便操作员调整控制策略。

优选地，系统支持应急情况下返回智能保护。当水流速度超过静水条件下最大航行速度时，水上机器人有可能被冲走，需要自动执行最短距离靠岸。具体方法是根据估算的水流速度以及记录的岸边出发点的距离，本实施例利用三角函数等原理计算出逆流航行的航向角，可以保证靠岸位置距离出发点最近。

优选地，作为一种可实现的方式，动力调整单元所述在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变，包括计算横向加速度lat_acc，包括如果是手动控制航向模式，通过遥控器输入的航行控制信号PWM值计算横向加速度lat_acc：

lat_acc＝g_force×(pwm_to_angle/C_A)

其中：C_A是一个计算常量，优选地，为了计算方便，其值可采用等边直角三角形的夹角，即45度，乘以100，即C_A＝45×100；g_force＝ground_speed×ground_speed/turn_radius，g_force表示最大的转弯加速度，turn_radius为转弯半径,其值由系统设置，ground_speed为参考地面速度，其值从主控器的状态记录中获取；pwm_to_angle表示第一转弯角度，其是将遥控器输入的航行控制信号PWM值转换成角度值；

优选地，pwm_to_angle表示第一转弯角度，其是将遥控器输入的航行控制信号PWM值转换成角度值，具体包括：

如果PWM大于rc_trim_high，则

pwm_to_angle＝_high×(PWM-rc_trim_high))/(rc_max-rc_trim_high)；

否则

pwm_to_angle＝_high×(PWM-rc_trim_low))/(rc_trim_low-rc_min)

其中，rc_trim_low是遥控器输入中值减去DeadZone，rc_trim_high是遥控器输入中值加上DeadZone,遥控器输入中值＝(_high+_low)/2，DeadZone表示在电平翻转时插入一个时间间隔，该时间间隔根据不同的遥控器设置，_high是遥控器的标准最大输入,_low是遥控器的标准最小输入，rc_max是遥控器扩大量程的最大值，rc_min是遥控器扩大量程的最小值。

可选地，根据驱动检测是否有航行控制信号PWM输入判断是否是手动控制航向，若有航行控制信号PWM输入则为手动控制航向模式。

优选地，作为另一种可实现的方式，动力调整单元所述在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变，包括计算横向加速度，包括如果是按规定的路线自主航行模式，采用以下方式计算横向加速度，如图3所示：

S301、计算目标航向角C_y；

其中，C_lng表示当前位置点的经度，C_lat表示当前位置点的纬度，N_lng表示下一个航标位置点的经度，N_lat表示下一个航标位置点的纬度；

S302、通过目标航向角C_y计算当前位置到下一个航标位置点的直线距离Lcn：

Lcn＝2×R×sin(C_y/2)

其中，R是航迹半径，通常取值地球半径；

S303、计算上一个航标位置点与当前位置点所形成的向量与当前位置与下一个航标位置点所形成的向量之间的夹角a；

夹角a的计算可采用向量间夹角计算的常用手段，不再赘述。

S304、计算横向加速度

lat_acc＝ground_speed×ground_speed/Lcn×sin(a)；

其中，ground_speed为参考地面速度。

优选地，所述动力调整单元所述在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变，包括如果系统有智能避障功能，采用以下方式更新横向加速度：

lat_acc＝lat_acc+(turn_angle/C_A)×turn_max_g

其中，turn_angle表示获取的第二转弯角度，turn_max_g是根据船体类型设置的最大转弯加速度，单位是重力加速度。

优选地，在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变，包括基于横向加速度计算油门实际输出值,如图4所示，具体包括：

S401、根据橫向加速度计算转弯比率turn_rate

turn_rate＝lat_acc/turn_max_g×G，

其中，G是重力加速度。

S402、根据系统限制的最大转弯角度Tmax(默认取值是90，即允许最大90度转弯)，以及系统限制的最大转弯油门增益T_Gain(避免油门突变过大造成系统不稳定，百分比乘以100),计算转弯限制系数turn_ratio，其中C_y是前面计算的目标偏航角：

turn_ratio＝((100-T_Gain)×(C_y-yaw_sensor)/Tmax)/100

系统限制的最大转弯角度Tmax和系统限制的最大转弯油门增益T_Gain由系统设定，yaw_sensor表示航向角，从主控器获取。

S403、根据转弯比率和转弯限制系数计算油门降速比率turn_reduction

turn_reduction＝1.0-turn_rate×turn_ratio

S404、根据油门降速比率修正目标巡航速度speed_cruise

speed_cruise＝speed_cruise×turn_reduction

S405、根据修正的目标巡航速度与参考地面速度ground_speed的差值进行PID调节，得到油门偏差throttle_err；

PID调节方法可采用现有技术，不再赘述。

S406、根据油门偏差修正期望油门值，得到油门实际输出值。

throttle＝throttle_target+throttle_err

其中throttle_target是设定的speed_cruise对应的期望油门值。

以上所举实施例，对本发明的目的、技术方案和优点进行了进一步的详细说明，所应理解的是，以上所举实施例仅为本发明的优选实施方式而已，并不用以限制本发明，凡在本发明的精神和原则之内对本发明所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。