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

文档序号:12270175阅读:来源:国知局

技术特征:

1.一种水上机器人智能定速巡航控制方法,其特征在于,包括:

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

S102、获取水流速度;

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

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

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

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所述水上机器人智能定速巡航控制方法,其特征在于,所述在定速巡航模式下进行周期性调整油门输出,确保在各种外围动力影响因素存在的情况下维持航行速度不变,包括计算横向加速度,包括如果是手动控制航向模式,通过遥控器输入的航行控制信号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值转换成角度值而得到的。

5.根据权利要求4所述水上机器人智能定速巡航控制方法,其特征在于,所述将遥控器输入的航行控制信号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是遥控器扩大量程的最小值。

6.根据权利要求1所述水上机器人智能定速巡航控制方法,其特征在于,所述在定速巡航模式下进行周期性调整油门输出,确保在各种外围动力影响因素存在的情况下维持航行速度不变,包括计算横向加速度,包括如果是按规定的路线自主航行模式,采用以下方式计算横向加速度lat_acc:

S301、计算目标航向角Cy

<mrow> <msub> <mi>C</mi> <mi>y</mi> </msub> <mo>=</mo> <mi>a</mi> <mi>r</mi> <mi>c</mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>(</mo> <msub> <mi>N</mi> <mrow> <mi>ln</mi> <mi>g</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>C</mi> <mrow> <mi>ln</mi> <mi>g</mi> </mrow> </msub> <mo>)</mo> <mo>&times;</mo> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mo>(</mo> <msub> <mi>N</mi> <mrow> <mi>l</mi> <mi>a</mi> <mi>t</mi> </mrow> </msub> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>N</mi> <mrow> <mi>l</mi> <mi>a</mi> <mi>t</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>C</mi> <mrow> <mi>l</mi> <mi>a</mi> <mi>t</mi> </mrow> </msub> <mo>)</mo> </mrow> </mfrac> <mo>)</mo> </mrow> </mrow>

其中,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为参考地面速度。

7.根据权利要求4-6任一所述水上机器人智能定速巡航控制方法,其特征在于,所述在定速巡航模式下进行周期性调整油门输出,确保在各种外围动力影响因素存在的情况下维持航行速度不变,包括如果系统有智能避障功能,采用以下方式更新横向加速度lat_acc:

lat_acc=lat_acc+(turn_angle/C_A)×turn_max_g

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

8.根据权利要求1所述水上机器人智能定速巡航控制方法,其特征在于,在定速巡航模式下进行周期性调整油门输出,确保在各种外围动力影响因素存在的情况下维持航行速度不变,包括基于横向加速度计算油门实际输出值,包括:

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

S402、根据系统限制的最大转弯角度以及限制的最大转弯油门增益计算转弯限制系数;

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

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

S405、将修正后的目标巡航速度与参考地面速度的差进行PID调节,得到油门偏差;

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

9.水上机器人智能定速巡航控制系统,包括分别与主控器相连接的系统参数设置单元、模式控制单元、水流速度获取单元和动力调整单元,其特征在于:

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

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

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

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

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

10.根据权利要求9所述水上机器人智能定速巡航控制系统,其特征在于:

所述动力调整单元在定速巡航模式下进行周期性调整油门输出,确保在各种外围动力影响因素存在的情况下维持航行速度不变,包括计算横向加速度,包括如果是手动控制航向模式,通过遥控器输入的航行控制信号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

<mrow> <msub> <mi>C</mi> <mi>y</mi> </msub> <mo>=</mo> <mi>a</mi> <mi>r</mi> <mi>c</mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>(</mo> <msub> <mi>N</mi> <mrow> <mi>ln</mi> <mi>g</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>C</mi> <mrow> <mi>ln</mi> <mi>g</mi> </mrow> </msub> <mo>)</mo> <mo>&times;</mo> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mo>(</mo> <msub> <mi>N</mi> <mrow> <mi>l</mi> <mi>a</mi> <mi>t</mi> </mrow> </msub> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>N</mi> <mrow> <mi>l</mi> <mi>a</mi> <mi>t</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>C</mi> <mrow> <mi>l</mi> <mi>a</mi> <mi>t</mi> </mrow> </msub> <mo>)</mo> </mrow> </mfrac> <mo>)</mo> </mrow> </mrow>

其中,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 2 3 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1