1.一種水上機(jī)器人智能定速巡航控制方法,其特征在于,包括:
S101、設(shè)置系統(tǒng)參數(shù);設(shè)置包括目標(biāo)巡航速度參數(shù)在內(nèi)的動(dòng)力參數(shù);
S102、獲取水流速度;
S103、根據(jù)水流速度判斷是否滿足定速巡航要求,如果不滿足要求,執(zhí)行最近最快的返回智能保護(hù);
S104、在滿足定速巡航要求的情形下,進(jìn)入定速巡航模式;
S105、在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出,確保在各種外圍動(dòng)力影響因素存在的情況下維持航行速度不變。
2.根據(jù)權(quán)利要求1所述水上機(jī)器人智能定速巡航控制方法,其特征在于,所述獲取水流速度包括:
S201、主控器通過驅(qū)動(dòng)探測判斷是否搭載水流測速傳感設(shè)備,如果有則進(jìn)入S202,否則進(jìn)入S203;
S202、直接讀取水流速度;
S203、估計(jì)水流速度,具體包括
S203-1、調(diào)整航向平行水流;
S203-2、主控器后臺自動(dòng)設(shè)置一個(gè)距離當(dāng)前位置距離L的航向位置點(diǎn)Waypoint;
S203-3、記錄以靜水條件下最大航行速度Vmax航行到Waypoint的耗時(shí)t;
S203-4、計(jì)算水流速度V=Vmax-L/t。
3.根據(jù)權(quán)利要求1所述水上機(jī)器人智能定速巡航控制方法,其特征在于,所述根據(jù)水流速度判斷是否滿足定速巡航要求,包括:若靜水條件下最大航行速度與水流速度的差值大于所述目標(biāo)巡航速度,則滿足定速巡航要求,否則不滿足定速巡航要求。
4.根據(jù)權(quán)利要求1所述水上機(jī)器人智能定速巡航控制方法,其特征在于,所述在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出,確保在各種外圍動(dòng)力影響因素存在的情況下維持航行速度不變,包括計(jì)算橫向加速度,包括如果是手動(dòng)控制航向模式,通過遙控器輸入的航行控制信號PWM值計(jì)算橫向加速度lat_acc:
lat_acc=g_force×(pwm_to_angle/C_A)
g_force=ground_speed×ground_speed/turn_radius
其中,C_A是計(jì)算常量;g_force表示最大的轉(zhuǎn)彎加速度,turn_radius為轉(zhuǎn)彎半徑,其值由系統(tǒng)設(shè)置,ground_speed為參考地面速度,其值從主控器的狀態(tài)記錄中獲??;pwm_to_angle表示第一轉(zhuǎn)彎角度,其值是由將遙控器輸入的航行控制信號PWM值轉(zhuǎn)換成角度值而得到的。
5.根據(jù)權(quán)利要求4所述水上機(jī)器人智能定速巡航控制方法,其特征在于,所述將遙控器輸入的航行控制信號PWM值轉(zhuǎn)換成角度值包括:
如果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表示在電平翻轉(zhuǎn)時(shí)插入一個(gè)時(shí)間間隔,該時(shí)間間隔根據(jù)不同的遙控器設(shè)置,_high是遙控器的標(biāo)準(zhǔn)最大輸入,_low是遙控器的標(biāo)準(zhǔn)最小輸,rc_max是遙控器擴(kuò)大量程的最大值,rc_min是遙控器擴(kuò)大量程的最小值。
6.根據(jù)權(quán)利要求1所述水上機(jī)器人智能定速巡航控制方法,其特征在于,所述在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出,確保在各種外圍動(dòng)力影響因素存在的情況下維持航行速度不變,包括計(jì)算橫向加速度,包括如果是按規(guī)定的路線自主航行模式,采用以下方式計(jì)算橫向加速度lat_acc:
S301、計(jì)算目標(biāo)航向角Cy;
其中,Clng表示當(dāng)前位置點(diǎn)的經(jīng)度,Clat表示當(dāng)前位置點(diǎn)的緯度,Nlng表示下一個(gè)航標(biāo)位置點(diǎn)的經(jīng)度,Nlat表示下一個(gè)航標(biāo)位置點(diǎn)的緯度;
S302、通過Cy計(jì)算當(dāng)前位置到下一個(gè)航標(biāo)位置點(diǎn)的直線距離Lcn:
Lcn=2×R×sin(Cy/2)
R是航跡半徑;
S303、計(jì)算上一個(gè)航標(biāo)位置點(diǎn)與當(dāng)前位置點(diǎn)所形成的向量與當(dāng)前位置與下一個(gè)航標(biāo)位置點(diǎn)所形成的向量之間的夾角a;
S304、計(jì)算橫向加速度
lat_acc=ground_speed×ground_speed/Lcn×sin(a);
其中,ground_speed為參考地面速度。
7.根據(jù)權(quán)利要求4-6任一所述水上機(jī)器人智能定速巡航控制方法,其特征在于,所述在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出,確保在各種外圍動(dòng)力影響因素存在的情況下維持航行速度不變,包括如果系統(tǒng)有智能避障功能,采用以下方式更新橫向加速度lat_acc:
lat_acc=lat_acc+(turn_angle/C_A)×turn_max_g
其中,turn_angle表示獲取的第二轉(zhuǎn)彎角度,turn_max_g是根據(jù)船體類型設(shè)置的最大轉(zhuǎn)彎加速度,單位是重力加速度,C_A是計(jì)算常量。
8.根據(jù)權(quán)利要求1所述水上機(jī)器人智能定速巡航控制方法,其特征在于,在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出,確保在各種外圍動(dòng)力影響因素存在的情況下維持航行速度不變,包括基于橫向加速度計(jì)算油門實(shí)際輸出值,包括:
S401、根據(jù)橫向加速度計(jì)算轉(zhuǎn)彎比率;
S402、根據(jù)系統(tǒng)限制的最大轉(zhuǎn)彎角度以及限制的最大轉(zhuǎn)彎油門增益計(jì)算轉(zhuǎn)彎限制系數(shù);
S403、根據(jù)轉(zhuǎn)彎比率和轉(zhuǎn)彎限制系數(shù)計(jì)算油門降速比率;
S404、根據(jù)油門降速比率修正目標(biāo)巡航速度;
S405、將修正后的目標(biāo)巡航速度與參考地面速度的差進(jìn)行PID調(diào)節(jié),得到油門偏差;
S406、根據(jù)油門偏差修正期望油門值,得到油門實(shí)際輸出值。
9.水上機(jī)器人智能定速巡航控制系統(tǒng),包括分別與主控器相連接的系統(tǒng)參數(shù)設(shè)置單元、模式控制單元、水流速度獲取單元和動(dòng)力調(diào)整單元,其特征在于:
所述參數(shù)設(shè)置單元用于設(shè)置系統(tǒng)參數(shù);設(shè)置包括目標(biāo)巡航速度參數(shù)在內(nèi)的動(dòng)力參數(shù);
所述水流速度獲取單元用于獲取水流速度;
所述主控器用于根據(jù)水流速度判斷是否滿足定速巡航要求;如果不滿足要求,執(zhí)行最近最快的返回智能保護(hù);
所述模式控制單元用于模式判斷,包括手動(dòng)控制航向模式、按規(guī)定的路線自主航行模式和定速巡航模式;在滿足定速巡航要求的情形下進(jìn)入定速巡航模式;
所述動(dòng)力調(diào)整單元用于在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出,確保在各種外圍動(dòng)力影響因素存在的情況下維持航行速度不變。
10.根據(jù)權(quán)利要求9所述水上機(jī)器人智能定速巡航控制系統(tǒng),其特征在于:
所述動(dòng)力調(diào)整單元在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出,確保在各種外圍動(dòng)力影響因素存在的情況下維持航行速度不變,包括計(jì)算橫向加速度,包括如果是手動(dòng)控制航向模式,通過遙控器輸入的航行控制信號PWM值計(jì)算橫向加速度lat_acc:
lat_acc=g_force×(pwm_to_angle/C_A)
g_force=ground_speed×ground_speed/turn_radius
其中,C_A是計(jì)算常量;g_force表示最大的轉(zhuǎn)彎加速度,turn_radius為轉(zhuǎn)彎半徑,其值由系統(tǒng)設(shè)置,ground_speed為參考地面速度,其值從主控器的狀態(tài)記錄中獲?。籶wm_to_angle表示第一轉(zhuǎn)彎角度,其值是由將遙控器輸入的航行控制信號PWM值轉(zhuǎn)換成角度值而得到的;
或者
所述動(dòng)力調(diào)整單元在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出,確保在各種外圍動(dòng)力影響因素存在的情況下維持航行速度不變,包括計(jì)算橫向加速度,包括如果是按規(guī)定的路線自主航行模式,采用以下方式計(jì)算橫向加速度lat_acc:
S301、計(jì)算目標(biāo)航向角Cy;
其中,Clng表示當(dāng)前位置點(diǎn)的經(jīng)度,Clat表示當(dāng)前位置點(diǎn)的緯度,Nlng表示下一個(gè)航標(biāo)位置點(diǎn)的經(jīng)度,Nlat表示下一個(gè)航標(biāo)位置點(diǎn)的緯度;
S302、通過Cy計(jì)算當(dāng)前位置到下一個(gè)航標(biāo)位置點(diǎn)的直線距離Lcn:
Lcn=2×R×sin(Cy/2)
R是航跡半徑;
S303、計(jì)算上一個(gè)航標(biāo)位置點(diǎn)與當(dāng)前位置點(diǎn)所形成的向量與當(dāng)前位置與下一個(gè)航標(biāo)位置點(diǎn)所形成的向量之間的夾角a;
S304、計(jì)算橫向加速度
lat_acc=ground_speed×ground_speed/Lcn×sin(a);
其中,ground_speed為參考地面速度。