CN106444787B - 水上机器人智能定速巡航控制方法及系统 - Google Patents
水上机器人智能定速巡航控制方法及系统 Download PDFInfo
- Publication number
- CN106444787B CN106444787B CN201611088440.2A CN201611088440A CN106444787B CN 106444787 B CN106444787 B CN 106444787B CN 201611088440 A CN201611088440 A CN 201611088440A CN 106444787 B CN106444787 B CN 106444787B
- Authority
- CN
- China
- Prior art keywords
- speed
- cruise
- value
- angle
- mode
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 22
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 72
- 238000004364 calculation method Methods 0.000 claims abstract description 14
- 230000001133 acceleration Effects 0.000 claims description 46
- 238000007514 turning Methods 0.000 claims description 30
- 230000002093 peripheral effect Effects 0.000 claims description 17
- 230000000694 effects Effects 0.000 claims description 16
- 230000002706 hydrostatic effect Effects 0.000 claims description 10
- 238000012360 testing method Methods 0.000 claims description 7
- 230000009467 reduction Effects 0.000 claims description 6
- 230000004888 barrier function Effects 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 238000001514 detection method Methods 0.000 claims description 3
- 230000004048 modification Effects 0.000 abstract description 4
- 238000012986 modification Methods 0.000 abstract description 4
- 238000005516 engineering process Methods 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 5
- 230000005484 gravity Effects 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 241000208340 Araliaceae Species 0.000 description 2
- 235000005035 Panax pseudoginseng ssp. pseudoginseng Nutrition 0.000 description 2
- 235000003140 Panax quinquefolius Nutrition 0.000 description 2
- 230000002411 adverse Effects 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000011217 control strategy Methods 0.000 description 2
- 238000005183 dynamical system Methods 0.000 description 2
- 235000008434 ginseng Nutrition 0.000 description 2
- 230000035772 mutation Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000003252 repetitive effect Effects 0.000 description 2
- 230000007812 deficiency Effects 0.000 description 1
- 239000000446 fuel Substances 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 239000003643 water by type Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
本发明涉及机器人技术领域，特别涉及一种水上机器人智能定速巡航控制方法及系统；所述方法包括设置目标巡航速度等动力参数；获取水流速度；根据水流速度判断是否满足定速巡航要求；在满足定速巡航要求的情形下,可以一键进入定速巡航模式；在定速巡航模式下，基于水上机器人主控器航姿参考系统的地面速度向量等参等周期性动态计算油门动力，保证速度恒定；在定速巡航模式下，支持修改巡航速度，自动执行动力判断和继续作业；在不满足定速巡航的情形下，可执行最近最快的自动返回智能保护策略。
Description
技术领域
本发明涉及机器人技术领域，特别涉及一种水上机器人智能定速巡航控制方法及系统。
背景技术
定速巡航是源自汽车的一种速度控制系统，其作用是按司机要求的速度合开关之后，不用踩油门踏板就自动地保持车速，使车辆以固定的速度行驶。采用了这种技术，当在高速公路上长时间行车后，司机就不用再去控制油门踏板，减轻了疲劳，同时减少了不必要的车速变化，可以节省燃料。
相比汽车，水上机器人的环境更复杂，定速巡航考虑因素更多、系统复杂度更高。比如，水质采样等特殊服务场景要求水上机器人在作业时保持一定的速度(比如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、计算目标航向角Cy；
其中，Clng表示当前位置点的经度，Clat表示当前位置点的纬度，Nlng表示下一个航标位置点的经度，Nlat表示下一个航标位置点的纬度；
S302、通过目标航向角Cy计算当前位置到下一个航标位置点的直线距离Lcn：
Lcn＝2×R×sin(Cy/2)
其中，R是航迹半径，通常取值地球半径；
S303、计算上一个航标位置点与当前位置点所形成的向量与当前位置与下一个航标位置点所形成的向量之间的夹角a；
夹角a的计算可采用向量间夹角计算的常用手段，不再赘述。
S304、计算横向加速度
lat_acc＝ground_speed×ground_speed/Lcn×sin(a)；
其中，ground_speed为参考地面速度。
优选地，所述在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变，包括如果系统有智能避障功能，采用以下方式更新横向加速度：
lat_acc1＝lat_acc+(turn_angle/C_A)×turn_max_g
其中，lat_acc表示更新前横向加速度，lat_acc1表示更新后横向加速度，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，其中Cy是前面计算的目标偏航角：
turn_ratio＝((100-T_Gain)×(Cy-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、计算目标航向角Cy；
其中，Clng表示当前位置点的经度，Clat表示当前位置点的纬度，Nlng表示下一个航标位置点的经度，Nlat表示下一个航标位置点的纬度；
S302、通过目标航向角Cy计算当前位置到下一个航标位置点的直线距离Lcn：
Lcn＝2×R×sin(Cy/2)
其中，R是航迹半径，通常取值地球半径；
S303、计算上一个航标位置点与当前位置点所形成的向量与当前位置与下一个航标位置点所形成的向量之间的夹角a；
夹角a的计算可采用向量间夹角计算的常用手段，不再赘述。
S304、计算横向加速度
lat_acc＝ground_speed×ground_speed/Lcn×sin(a)；
其中，ground_speed为参考地面速度。
优选地，所述动力调整单元所述在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变，包括如果系统有智能避障功能，采用以下方式更新横向加速度：
lat_acc1＝lat_acc+(turn_angle/C_A)×turn_max_g
其中，lat_acc表示更新前横向加速度，lat_acc1表示更新后横向加速度，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，其中Cy是前面计算的目标偏航角：
turn_ratio＝((100-T_Gain)×(Cy-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对应的期望油门值。
以上所举实施例，对本发明的目的、技术方案和优点进行了进一步的详细说明，所应理解的是，以上所举实施例仅为本发明的优选实施方式而已，并不用以限制本发明，凡在本发明的精神和原则之内对本发明所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (7)
1.一种水上机器人智能定速巡航控制方法，其特征在于，包括：
S101、设置系统参数；设置包括目标巡航速度参数在内的动力参数；
S102、获取水流速度；
S103、根据水流速度判断是否满足定速巡航要求，如果不满足要求，执行最近最快的返回智能保护；
S104、在满足定速巡航要求的情形下，进入定速巡航模式；
S105、在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变,包括计算横向加速度,包括以下两种方式任意一种：
如果是手动控制航向模式，通过遥控器输入的航行控制信号PWM值计算横向加速度lat_acc：
lat_acc＝g_force×(pwm_to_angle/C_A)
g_force＝ground_speed×ground_speed/turn_radius
其中，C_A是计算常量；g_force表示最大的转弯加速度，turn_radius为转弯半径,其值由系统设置，ground_speed为参考地面速度，其值从主控器的状态记录中获取；pwm_to_angle表示第一转弯角度，其值是由将遥控器输入的航行控制信号PWM值转换成角度值而得到的；
所述将遥控器输入的航行控制信号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是遥控器扩大量程的最小值；
或者
如果是按规定的路线自主航行模式，采用以下方式计算横向加速度lat_acc：
S301、计算目标航向角Cy；
其中，Clng表示当前位置点的经度，Clat表示当前位置点的纬度，Nlng表示下一个航标位置点的经度，Nlat表示下一个航标位置点的纬度；
S302、通过Cy计算当前位置到下一个航标位置点的直线距离Lcn：
Lcn＝2×R×sin(Cy/2)
R是航迹半径；
S303、计算上一个航标位置点与当前位置点所形成的向量与当前位置与下一个航标位置点所形成的向量之间的夹角a；
S304、计算横向加速度
lat_acc＝ground_speed×ground_speed/Lcn×sin(a)；
其中，ground_speed为参考地面速度。
2.根据权利要求1所述水上机器人智能定速巡航控制方法，其特征在于，所述获取水流速度包括：
S201、主控器通过驱动探测判断是否搭载水流测速传感设备，如果有则进入S202，否则进入S203；
S202、直接读取水流速度；
S203、估计水流速度，具体包括
S203-1、调整航向平行水流；
S203-2、主控器后台自动设置一个距离当前位置距离L的航向位置点Waypoint；
S203-3、记录以静水条件下最大航行速度Vmax航行到Waypoint的耗时t；
S203-4、计算水流速度V＝Vmax-L/t。
3.根据权利要求1所述水上机器人智能定速巡航控制方法，其特征在于，所述根据水流速度判断是否满足定速巡航要求，包括：若静水条件下最大航行速度与水流速度的差值大于所述目标巡航速度，则满足定速巡航要求，否则不满足定速巡航要求。
4.根据权利要求1所述水上机器人智能定速巡航控制方法，其特征在于，所述在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变，包括如果系统有智能避障功能，采用以下方式更新横向加速度：
lat_acc1＝lat_acc+(turn_angle/C_A)×turn_max_g
其中，lat_acc表示更新前横向加速度，lat_acc1表示更新后横向加速度，turn_angle表示获取的第二转弯角度，turn_max_g是根据船体类型设置的最大转弯加速度，单位是重力加速度，C_A是计算常量。
5.根据权利要求1所述水上机器人智能定速巡航控制方法，其特征在于，在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变，包括基于横向加速度计算油门实际输出值，包括：
S401、根据橫向加速度计算转弯比率；
S402、根据系统限制的最大转弯角度以及限制的最大转弯油门增益计算转弯限制系数；
S403、根据转弯比率和转弯限制系数计算油门降速比率；
S404、根据油门降速比率修正目标巡航速度；
S405、将修正后的目标巡航速度与参考地面速度的差进行PID调节，得到油门偏差；
S406、根据油门偏差修正期望油门值，得到油门实际输出值。
6.水上机器人智能定速巡航控制系统，包括分别与主控器相连接的系统参数设置单元、模式控制单元、水流速度获取单元和动力调整单元，其特征在于：
所述参数设置单元用于设置系统参数；设置包括目标巡航速度参数在内的动力参数；
所述水流速度获取单元用于获取水流速度；
所述主控器用于根据水流速度判断是否满足定速巡航要求；如果不满足要求，执行最近最快的返回智能保护；
所述模式控制单元用于模式判断，包括手动控制航向模式、按规定的路线自主航行模式和定速巡航模式；在满足定速巡航要求的情形下进入定速巡航模式；
所述动力调整单元用于在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变。
7.根据权利要求6所述水上机器人智能定速巡航控制系统，其特征在于：
所述动力调整单元在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变，包括计算横向加速度，包括如果是手动控制航向模式，通过遥控器输入的航行控制信号PWM值计算横向加速度lat_acc：
lat_acc＝g_force×(pwm_to_angle/C_A)
g_force＝ground_speed×ground_speed/turn_radius
其中，C_A是计算常量；g_force表示最大的转弯加速度，turn_radius为转弯半径,其值由系统设置，ground_speed为参考地面速度，其值从主控器的状态记录中获取；pwm_to_angle表示第一转弯角度，其值是由将遥控器输入的航行控制信号PWM值转换成角度值而得到的；
或者
所述动力调整单元在定速巡航模式下进行周期性调整油门输出，确保在各种外围动力影响因素存在的情况下维持航行速度不变，包括计算横向加速度，包括如果是按规定的路线自主航行模式，采用以下方式计算横向加速度lat_acc：
S301、计算目标航向角Cy；
其中，Clng表示当前位置点的经度，Clat表示当前位置点的纬度，Nlng表示下一个航标位置点的经度，Nlat表示下一个航标位置点的纬度；
S302、通过Cy计算当前位置到下一个航标位置点的直线距离Lcn：
Lcn＝2×R×sin(Cy/2)
R是航迹半径；
S303、计算上一个航标位置点与当前位置点所形成的向量与当前位置与下一个航标位置点所形成的向量之间的夹角a；
S304、计算横向加速度
lat_acc＝ground_speed×ground_speed/Lcn×sin(a)；
其中，ground_speed为参考地面速度。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611088440.2A CN106444787B (zh) | 2016-12-01 | 2016-12-01 | 水上机器人智能定速巡航控制方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611088440.2A CN106444787B (zh) | 2016-12-01 | 2016-12-01 | 水上机器人智能定速巡航控制方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106444787A CN106444787A (zh) | 2017-02-22 |
| CN106444787B true CN106444787B (zh) | 2019-03-26 |
Family
ID=58223538
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201611088440.2A Active CN106444787B (zh) | 2016-12-01 | 2016-12-01 | 水上机器人智能定速巡航控制方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106444787B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107422736B (zh) * | 2017-08-03 | 2020-03-13 | 大连海事大学 | 一种无人船自主返航控制方法 |
| CN113232783B (zh) * | 2020-12-25 | 2022-09-27 | 北京航空航天大学合肥创新研究院 | 一种无人船速度控制方法及存储介质 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101462590A (zh) * | 2008-12-30 | 2009-06-24 | 大连海事大学 | 船舶操纵与主推进联合控制方法及其模拟系统 |
| CN101624093A (zh) * | 2008-07-09 | 2010-01-13 | Mtu腓特烈港有限责任公司 | 船速控制方法 |
| CN101833335A (zh) * | 2010-05-10 | 2010-09-15 | 珠海云洲智能科技有限公司 | 小型水面机器人装置及自适应水流优化导航方法 |
| CN102331717A (zh) * | 2011-10-10 | 2012-01-25 | 哈尔滨工程大学 | 一种船舶航速智能控制方法 |
| CN202574212U (zh) * | 2012-04-09 | 2012-12-05 | 孙朝煜 | 一种多功能定速巡航系统 |
| CN102963505A (zh) * | 2012-11-30 | 2013-03-13 | 安徽工业大学 | 水面垃圾自动清扫船 |
| CN203705951U (zh) * | 2014-01-23 | 2014-07-09 | 宁波海韦斯智能技术有限公司 | 一种船用推进器控制装置 |
| CN105159296A (zh) * | 2015-09-09 | 2015-12-16 | 广州睿航电子科技有限公司 | 一种无人船抗风浪巡航控制方法及系统 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2016169691A (ja) * | 2015-03-13 | 2016-09-23 | ヤマハ発動機株式会社 | ジェット推進艇及びその制御方法 |
-
2016
- 2016-12-01 CN CN201611088440.2A patent/CN106444787B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101624093A (zh) * | 2008-07-09 | 2010-01-13 | Mtu腓特烈港有限责任公司 | 船速控制方法 |
| CN101462590A (zh) * | 2008-12-30 | 2009-06-24 | 大连海事大学 | 船舶操纵与主推进联合控制方法及其模拟系统 |
| CN101833335A (zh) * | 2010-05-10 | 2010-09-15 | 珠海云洲智能科技有限公司 | 小型水面机器人装置及自适应水流优化导航方法 |
| CN102331717A (zh) * | 2011-10-10 | 2012-01-25 | 哈尔滨工程大学 | 一种船舶航速智能控制方法 |
| CN202574212U (zh) * | 2012-04-09 | 2012-12-05 | 孙朝煜 | 一种多功能定速巡航系统 |
| CN102963505A (zh) * | 2012-11-30 | 2013-03-13 | 安徽工业大学 | 水面垃圾自动清扫船 |
| CN203705951U (zh) * | 2014-01-23 | 2014-07-09 | 宁波海韦斯智能技术有限公司 | 一种船用推进器控制装置 |
| CN105159296A (zh) * | 2015-09-09 | 2015-12-16 | 广州睿航电子科技有限公司 | 一种无人船抗风浪巡航控制方法及系统 |
Non-Patent Citations (2)
| Title |
|---|
| 船舶定速巡航控制策略;王炳轩 等;《机电设备》;20180115;第41-45页 |
| 船速复合误差调节器半实物仿真及稳定性分析;张桂臣 等;《系统仿真学报》;20100208;第22卷(第2期);第353-356页 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106444787A (zh) | 2017-02-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106314423B (zh) | 车辆行驶控制装置 | |
| US11052914B2 (en) | Automated driving systems and control logic using maneuver criticality for vehicle routing and mode adaptation | |
| US10365653B2 (en) | Personalized autonomous vehicle ride characteristics | |
| JP6651642B2 (ja) | 車両制御装置 | |
| RU2640919C2 (ru) | Способ и система для оптимизации энергопотребления в транспортном средстве | |
| EP3173324B1 (en) | Watercraft control method and watercraft control system | |
| JP4952268B2 (ja) | 走行制御計画生成装置 | |
| CN108834425B (zh) | 用于控制自动驾驶车辆进入自动驾驶模式的方法及系统 | |
| WO2005058691A1 (ja) | 自動操舵制御装置および自動操舵装置 | |
| CN104483970B (zh) | 一种基于卫星定位系统和移动通信网络的控制无人驾驶系统航行的方法 | |
| CN105573340B (zh) | 一种固定翼无人机抗侧风的飞行控制方法 | |
| US9610942B2 (en) | Device and method for operating a vehicle | |
| KR20110113608A (ko) | 조종가능 플랫폼용의 자율 항해 시스템 및 방법 | |
| GB2596709A (en) | Passage planning and navigation systems and methods | |
| US20180345968A1 (en) | Vehicle controller, vehicle control method, and storage medium | |
| JP2021181301A (ja) | 船舶の自動誘導方法、船舶の自動誘導プログラム、船舶の自動誘導システム、及び船舶 | |
| KR102467138B1 (ko) | 운항체의 자율운항시스템 및 그 제어방법 | |
| CN108973991A (zh) | 车辆控制装置、车辆控制方法及存储介质 | |
| JP4640760B2 (ja) | 到来波浪の波形予測法および波浪中の航走体の運転制御方法 | |
| CN106444787B (zh) | 水上机器人智能定速巡航控制方法及系统 | |
| CN109689458A (zh) | 停车辅助方法和装置 | |
| CN105387858A (zh) | 一种游艇智能导航系统及工作方法 | |
| WO2018123948A1 (ja) | 船舶の自動操縦システム、船舶、及び船舶の自動操縦方法 | |
| JP2004355105A (ja) | 移動体の制御方法及び制御装置 | |
| JP2009166722A (ja) | 車両制御装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |