本發(fā)明涉及機(jī)器人技術(shù)領(lǐng)域，特別涉及一種水上機(jī)器人智能定速巡航控制方法及系統(tǒng)。

背景技術(shù)：

定速巡航是源自汽車的一種速度控制系統(tǒng)，其作用是按司機(jī)要求的速度合開關(guān)之后，不用踩油門踏板就自動地保持車速，使車輛以固定的速度行駛。采用了這種技術(shù)，當(dāng)在高速公路上長時間行車后，司機(jī)就不用再去控制油門踏板，減輕了疲勞，同時減少了不必要的車速變化，可以節(jié)省燃料。

相比汽車，水上機(jī)器人的環(huán)境更復(fù)雜，定速巡航考慮因素更多、系統(tǒng)復(fù)雜度更高。比如，水質(zhì)采樣等特殊服務(wù)場景要求水上機(jī)器人在作業(yè)時保持一定的速度(比如2米/秒)，完成作業(yè)后能以其他速度(通常是全速)返航。由于水流等復(fù)雜因素導(dǎo)致操作員在陸地上無法估測航行速度以及無法通過搖桿給出比較精確的油門控制信號。因此迫切需要水上機(jī)器人能自主控制航速，操作員只需要通過遙控器或者地面基站調(diào)整水上機(jī)器人的航向即可。

現(xiàn)有的水上機(jī)器人作業(yè)時僅支持自主巡航和手動控制，自主巡航不可以控制方向，不滿足特殊作業(yè)時操控需求，且在有的小型水域可能無法規(guī)劃出合理的作業(yè)路線；手動控制時，操作員試探性的調(diào)整油門主觀判斷達(dá)到預(yù)期的速度并讓遙控器搖桿保持以達(dá)到穩(wěn)定在某一個速度，由于水流等因素導(dǎo)致實(shí)際航行速度與期望的巡航速度可能存在較大的偏差，且操作不便。

技術(shù)實(shí)現(xiàn)要素：

為了克服上述現(xiàn)有技術(shù)的不足，本發(fā)明提供了一種水上機(jī)器人智能定速巡航控制方法及系統(tǒng)，提出“半自動”定速巡航模式，實(shí)現(xiàn)了目標(biāo)巡航速度控制更精準(zhǔn)、安全以及操作員更易用。

本發(fā)明一種水上機(jī)器人智能定速巡航控制方法，包括：

S101、設(shè)置系統(tǒng)參數(shù)；設(shè)置包括目標(biāo)巡航速度參數(shù)在內(nèi)的動力參數(shù)；

S102、獲取水流速度；

S103、根據(jù)水流速度判斷是否滿足定速巡航要求，如果不滿足要求，執(zhí)行最近最快的返回智能保護(hù)；

S104、在滿足定速巡航要求的情形下，進(jìn)入定速巡航模式；

S105、在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出，確保在各種外圍動力影響因素存在的情況下維持航行速度不變。

本發(fā)明水上機(jī)器人智能定速巡航控制系統(tǒng)，包括分別與主控器相連接的系統(tǒng)參數(shù)設(shè)置單元、模式控制單元、水流速度獲取單元和動力調(diào)整單元：

所述參數(shù)設(shè)置單元用于設(shè)置系統(tǒng)參數(shù)；設(shè)置包括目標(biāo)巡航速度參數(shù)在內(nèi)的動力參數(shù)；

所述水流速度獲取單元用于獲取水流速度；

所述主控器用于根據(jù)水流速度判斷是否滿足定速巡航要求；如果不滿足要求，執(zhí)行最近最快的返回智能保護(hù)；

所述模式控制單元用于進(jìn)行模式判斷，包括手動控制航向模式、按規(guī)定的路線自主航行模式和定速巡航模式，在滿足定速巡航要求的情形下進(jìn)入定速巡航模式；

所述動力調(diào)整單元用于在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出，確保在各種外圍動力影響因素存在的情況下維持航行速度不變。

與現(xiàn)有技術(shù)相比，本發(fā)明將定速巡航功能在水上機(jī)器人在實(shí)現(xiàn)，在更復(fù)雜的環(huán)境實(shí)現(xiàn)了動力預(yù)估、自主航行和手動控制航向等，能更準(zhǔn)確、更安全的實(shí)現(xiàn)速度控制，而且將定速巡航作為一種獨(dú)立的工作模式、快速的的進(jìn)入和退出、速率可調(diào)整、可以自動的全速返航，使得本發(fā)明更實(shí)用、更易用。

附圖說明

圖1為本發(fā)明水上機(jī)器人智能定速巡航控制方法優(yōu)選實(shí)施例流程示意圖；

圖2為本發(fā)明獲取水流速度的流程示意圖；

圖3為本發(fā)明橫向加速度計算的流程示意圖；

圖4為本發(fā)明獲得油門實(shí)際輸出的流程示意圖；

圖5為本發(fā)明水上機(jī)器人智能定速巡航控制系統(tǒng)優(yōu)選實(shí)施例結(jié)構(gòu)示意圖。

具體實(shí)施方式

為使本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)表達(dá)得更加清楚明白，下面結(jié)合附圖及具體實(shí)施案例對本發(fā)明做進(jìn)一步詳細(xì)說明。

本發(fā)明水上機(jī)器人智能定速巡航控制方法，如圖1所示，包括

S101、設(shè)置系統(tǒng)參數(shù)；包括設(shè)置目標(biāo)巡航速度和最大轉(zhuǎn)彎油門增益等動力參數(shù)。

S102、獲取水流速度；

S103、根據(jù)水流速度判斷是否滿足定速巡航要求；如果不滿足要求，執(zhí)行最近最快的返回智能保護(hù)；

S104、在滿足定速巡航要求的情形下進(jìn)入定速巡航模式；

S105、在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出，確保在各種外圍動力影響因素存在的情況下維持航行速度不變；

本發(fā)明設(shè)定目標(biāo)巡航速度，系統(tǒng)自動的進(jìn)行動力判斷，在動力滿足的情況自動繼續(xù)完成當(dāng)前作業(yè)，在動力不滿足的情況下執(zhí)行最近最快的返回智能保護(hù)；

S101所述設(shè)置目標(biāo)巡航速度，可通過地面基站軟件可視化菜單設(shè)置，或者通過主控器腳本文件修改。

從操作來看，在設(shè)置目標(biāo)巡航速度之后，可一鍵指令進(jìn)入定速巡航工作模式，優(yōu)選在遙控器上設(shè)置自定義撥碼開關(guān)并在主控器側(cè)適配，也可以通過地面基站軟件在便于操作的位置設(shè)置按鈕一鍵操作。

作為一種可實(shí)現(xiàn)的方式，所述獲取水流速度包括：直接讀取水流測速傳感設(shè)備的水流速度值。

優(yōu)選地，作為另一種可實(shí)現(xiàn)的方式，所述獲取水流速度包括：

水流速度V＝Vmax-L/t

其中，Vmax表示靜水條件下最大航行速度；L表示自動設(shè)置的航向前方GPS坐標(biāo)點(diǎn)Waypoint與當(dāng)前位置GPS坐標(biāo)點(diǎn)的距離；t表示航行到Waypoint的耗時。

作為一種可實(shí)現(xiàn)方式，本發(fā)明所述獲取水流速度，如圖2所示，包括

S201、主控器通過驅(qū)動探測判斷是否搭載水流測速傳感設(shè)備，如果有則進(jìn)入S202，否則進(jìn)入S203

S202、直接讀取水流速度；

S203、估計水流速度，具體包括

S203-1、調(diào)整航向平行水流，優(yōu)選逆流；

S203-2、主控器后臺自動設(shè)置一個航航向前方GPS坐標(biāo)點(diǎn)Waypoint，距離當(dāng)前位置距離L；

S203-3、記錄以靜水條件下最大航行速度Vmax航行到Waypoint的耗時t，

S203-4、計算水流速度；

V＝Vmax-L/t

優(yōu)選地，可以重復(fù)操作多次，測試幾組數(shù)據(jù)并通過合并算法處理得到相對更準(zhǔn)確的水流速度值。

優(yōu)選地，作為一種可實(shí)現(xiàn)的方式，所述根據(jù)水流速度判斷是否滿足定速巡航要求，包括：若靜水條件下最大航行速度與水流速度的差值大于目標(biāo)巡航速度，則滿足定速巡航要求，否則不滿足定速巡航要求。

從操作來看，如果航速動力系統(tǒng)不滿足要求，則通過無線信號向地面基站軟件發(fā)送警告以及上報水流速度，以便操作員調(diào)整控制策略。

優(yōu)選地，系統(tǒng)支持應(yīng)急情況下返回智能保護(hù)。當(dāng)水流速度超過靜水條件下最大航行速度時，水上機(jī)器人有可能被沖走，需要自動執(zhí)行最短距離靠岸。具體方法是根據(jù)估算的水流速度以及記錄的岸邊出發(fā)點(diǎn)的距離，本實(shí)施例利用三角函數(shù)等原理計算出逆流航行的航向角，可以保證靠岸位置距離出發(fā)點(diǎn)最近。

優(yōu)選地，作為一種可實(shí)現(xiàn)的方式，所述在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出，確保在各種外圍動力影響因素存在的情況下維持航行速度不變，包括計算橫向加速度lat_acc，包括如果是手動控制航向模式，通過遙控器輸入的航行控制信號PWM值計算橫向加速度lat_acc：

lat_acc＝g_force×(pwm_to_angle/C_A)

其中：C_A是一個計算常量，優(yōu)選地，為了計算方便，其值可采用等邊直角三角形的夾角，即45度，乘以100，即C_A＝45×100；g_force＝ground_speed×ground_speed/turn_radius，g_force表示最大的轉(zhuǎn)彎加速度，turn_radius為轉(zhuǎn)彎半徑,其值由系統(tǒng)設(shè)置，ground_speed為參考地面速度，其值從主控器的狀態(tài)記錄中獲??；pwm_to_angle表示第一轉(zhuǎn)彎角度，其是將遙控器輸入的航行控制信號PWM值轉(zhuǎn)換成角度值；

優(yōu)選地，pwm_to_angle表示第一轉(zhuǎn)彎角度，其是將遙控器輸入的航行控制信號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)時插入一個時間間隔，該時間間隔根據(jù)不同的遙控器設(shè)置，_high是遙控器的標(biāo)準(zhǔn)最大輸入,_low是遙控器的標(biāo)準(zhǔn)最小輸入，rc_max是遙控器擴(kuò)大量程的最大值，rc_min是遙控器擴(kuò)大量程的最小值。

可選地，根據(jù)驅(qū)動檢測是否有航行控制信號PWM輸入判斷是否是手動控制航向，若有航行控制信號PWM輸入則為手動控制航向模式。

優(yōu)選地，作為另一種可實(shí)現(xiàn)的方式，所述在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出，確保在各種外圍動力影響因素存在的情況下維持航行速度不變，包括計算橫向加速度，包括如果是按規(guī)定的路線自主航行模式，采用以下方式計算橫向加速度，如圖3所示：

S301、計算目標(biāo)航向角C_y；

其中，C_lng表示當(dāng)前位置點(diǎn)的經(jīng)度，C_lat表示當(dāng)前位置點(diǎn)的緯度，N_lng表示下一個航標(biāo)位置點(diǎn)的經(jīng)度，N_lat表示下一個航標(biāo)位置點(diǎn)的緯度；

S302、通過目標(biāo)航向角C_y計算當(dāng)前位置到下一個航標(biāo)位置點(diǎn)的直線距離Lcn：

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

其中，R是航跡半徑，通常取值地球半徑；

S303、計算上一個航標(biāo)位置點(diǎn)與當(dāng)前位置點(diǎn)所形成的向量與當(dāng)前位置與下一個航標(biāo)位置點(diǎn)所形成的向量之間的夾角a；

夾角a的計算可采用向量間夾角計算的常用手段，不再贅述。

S304、計算橫向加速度

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

其中，ground_speed為參考地面速度。

優(yōu)選地，所述在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出，確保在各種外圍動力影響因素存在的情況下維持航行速度不變，包括如果系統(tǒng)有智能避障功能，采用以下方式更新橫向加速度：

lat_acc＝lat_acc+(turn_angle/C_A)×turn_max_g

其中，turn_angle表示獲取的第二轉(zhuǎn)彎角度，turn_max_g是根據(jù)船體類型設(shè)置的最大轉(zhuǎn)彎加速度，單位是重力加速度。

優(yōu)選地，在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出，確保在各種外圍動力影響因素存在的情況下維持航行速度不變，包括基于橫向加速度計算油門實(shí)際輸出值,如圖4所示，具體包括：

S401、根據(jù)橫向加速度計算轉(zhuǎn)彎比率turn_rate

turn_rate＝lat_acc/turn_max_g×G，

其中，G是重力加速度。

S402、根據(jù)系統(tǒng)限制的最大轉(zhuǎn)彎角度Tmax(默認(rèn)取值是90，即允許最大90度轉(zhuǎn)彎)，以及系統(tǒng)限制的最大轉(zhuǎn)彎油門增益T_Gain(避免油門突變過大造成系統(tǒng)不穩(wěn)定，百分比乘以100),計算轉(zhuǎn)彎限制系數(shù)turn_ratio，其中C_y是前面計算的目標(biāo)偏航角：

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

系統(tǒng)限制的最大轉(zhuǎn)彎角度Tmax和系統(tǒng)限制的最大轉(zhuǎn)彎油門增益T_Gain由系統(tǒng)設(shè)定,yaw_sensor表示航向角，從主控器獲取。

S403、根據(jù)轉(zhuǎn)彎比率和轉(zhuǎn)彎限制系數(shù)計算油門降速比率turn_reduction

turn_reduction＝1.0-turn_rate×turn_ratio

S404、根據(jù)油門降速比率修正目標(biāo)巡航速度speed_cruise

speed_cruise＝speed_cruise×turn_reduction

S405、根據(jù)修正的目標(biāo)巡航速度與參考地面速度ground_speed的差值進(jìn)行PID調(diào)節(jié)，得到油門偏差throttle_err；

PID調(diào)節(jié)方法可采用現(xiàn)有技術(shù)，不再贅述。

S406、根據(jù)油門偏差修正期望油門值，得到油門實(shí)際輸出值。

throttle＝throttle_target+throttle_err

其中throttle_target是設(shè)定的speed_cruise對應(yīng)的期望油門值。

本發(fā)明在定速巡航模式下，基于水上機(jī)器人主控器航姿參考系統(tǒng)的地面速度向量等參等周期性動態(tài)計算油門動力，保證速度恒定；在定速巡航模式下，支持修改巡航速度，自動執(zhí)行動力判斷和繼續(xù)作業(yè)；在不滿足定速巡航的情形下，可執(zhí)行最近最快的自動返回智能保護(hù)策略。

本發(fā)明水上機(jī)器人智能定速巡航控制系統(tǒng)，如圖5所示，包括分別與主控器相連接的系統(tǒng)參數(shù)設(shè)置單元、模式控制單元、水流速度獲取單元和動力調(diào)整單元；

所述參數(shù)設(shè)置單元用于設(shè)置系統(tǒng)參數(shù)；包括設(shè)置目標(biāo)巡航速度和最大轉(zhuǎn)彎油門增益等動力參數(shù)。

所述水流速度獲取單元用于獲取水流速度；

所述主控器用于根據(jù)水流速度判斷是否滿足定速巡航要求；如果不滿足要求，執(zhí)行最近最快的返回智能保護(hù)；

所述模式控制單元用于判斷模式，包括手動控制航向模式、按規(guī)定的路線自主航行模式和定速巡航模式，在滿足定速巡航要求的情形下進(jìn)入定速巡航模式；

所述動力調(diào)整單元用于在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出，確保在各種外圍動力影響因素存在的情況下維持航行速度不變；

本發(fā)明設(shè)定目標(biāo)巡航速度，系統(tǒng)自動進(jìn)行動力判斷，在動力滿足的情況自動繼續(xù)完成當(dāng)前作業(yè)，在動力不滿足的情況下執(zhí)行最近最快的返回智能保護(hù)；

所述參數(shù)設(shè)置單元設(shè)置目標(biāo)巡航速度，可通過地面基站軟件可視化菜單設(shè)置，或者通過主控器腳本文件修改。

從操作來看，在設(shè)置目標(biāo)巡航速度之后，可一鍵指令進(jìn)入定速巡航工作模式，優(yōu)選在遙控器上設(shè)置自定義撥碼開關(guān)并在主控器側(cè)適配，也可以通過地面基站軟件在便于操作的位置設(shè)置按鈕一鍵操作。

作為一種可實(shí)現(xiàn)的方式，水流速度獲取單元所述獲取水流速度包括：直接讀取水流測速傳感設(shè)備的水流速度值。

優(yōu)選地，作為另一種可實(shí)現(xiàn)的方式，水流速度獲取單元所述獲取水流速度包括：

水流速度V＝Vmax-L/t

其中，Vmax表示靜水條件下最大航行速度；L表示自動設(shè)置的航向前方GPS坐標(biāo)點(diǎn)Waypoint與當(dāng)前位置GPS坐標(biāo)點(diǎn)的距離；t表示航行到Waypoint的耗時。

作為一種可實(shí)現(xiàn)方式，水流速度獲取單元所述獲取水流速度，如圖2所示，包括

S201、主控器通過驅(qū)動探測判斷是否搭載水流測速傳感設(shè)備，如果有則進(jìn)入S202，否則進(jìn)入S203

S202、直接讀取水流速度；

S203、估計水流速度，具體包括

S203-1、調(diào)整航向平行水流，優(yōu)選逆流；

S203-2、主控器后臺自動設(shè)置一個航航向前方GPS坐標(biāo)點(diǎn)Waypoint，距離當(dāng)前位置距離L；

S203-3、記錄以靜水條件下最大航行速度Vmax航行到Waypoint的耗時t，

S203-4、計算水流速度；

V＝Vmax-L/t

優(yōu)選地，可以重復(fù)操作多次，測試幾組數(shù)據(jù)并通過合并算法處理得到相對更準(zhǔn)確的水流速度值。

優(yōu)選地，作為一種可實(shí)現(xiàn)的方式，主控器所述根據(jù)水流速度判斷是否滿足定速巡航要求，包括：若靜水條件下最大航行速度與水流速度的差值大于目標(biāo)巡航速度，則滿足定速巡航要求，否則不滿足定速巡航要求。

從操作來看，如果航速動力系統(tǒng)不滿足要求，則通過無線信號向地面基站軟件發(fā)送警告以及上報水流速度，以便操作員調(diào)整控制策略。

優(yōu)選地，系統(tǒng)支持應(yīng)急情況下返回智能保護(hù)。當(dāng)水流速度超過靜水條件下最大航行速度時，水上機(jī)器人有可能被沖走，需要自動執(zhí)行最短距離靠岸。具體方法是根據(jù)估算的水流速度以及記錄的岸邊出發(fā)點(diǎn)的距離，本實(shí)施例利用三角函數(shù)等原理計算出逆流航行的航向角，可以保證靠岸位置距離出發(fā)點(diǎn)最近。

優(yōu)選地，作為一種可實(shí)現(xiàn)的方式，動力調(diào)整單元所述在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出，確保在各種外圍動力影響因素存在的情況下維持航行速度不變，包括計算橫向加速度lat_acc，包括如果是手動控制航向模式，通過遙控器輸入的航行控制信號PWM值計算橫向加速度lat_acc：

lat_acc＝g_force×(pwm_to_angle/C_A)

其中：C_A是一個計算常量，優(yōu)選地，為了計算方便，其值可采用等邊直角三角形的夾角，即45度，乘以100，即C_A＝45×100；g_force＝ground_speed×ground_speed/turn_radius，g_force表示最大的轉(zhuǎn)彎加速度，turn_radius為轉(zhuǎn)彎半徑,其值由系統(tǒng)設(shè)置，ground_speed為參考地面速度，其值從主控器的狀態(tài)記錄中獲??；pwm_to_angle表示第一轉(zhuǎn)彎角度，其是將遙控器輸入的航行控制信號PWM值轉(zhuǎn)換成角度值；

優(yōu)選地，pwm_to_angle表示第一轉(zhuǎn)彎角度，其是將遙控器輸入的航行控制信號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)時插入一個時間間隔，該時間間隔根據(jù)不同的遙控器設(shè)置，_high是遙控器的標(biāo)準(zhǔn)最大輸入,_low是遙控器的標(biāo)準(zhǔn)最小輸入，rc_max是遙控器擴(kuò)大量程的最大值，rc_min是遙控器擴(kuò)大量程的最小值。

可選地，根據(jù)驅(qū)動檢測是否有航行控制信號PWM輸入判斷是否是手動控制航向，若有航行控制信號PWM輸入則為手動控制航向模式。

優(yōu)選地，作為另一種可實(shí)現(xiàn)的方式，動力調(diào)整單元所述在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出，確保在各種外圍動力影響因素存在的情況下維持航行速度不變，包括計算橫向加速度，包括如果是按規(guī)定的路線自主航行模式，采用以下方式計算橫向加速度，如圖3所示：

S301、計算目標(biāo)航向角C_y；

其中，C_lng表示當(dāng)前位置點(diǎn)的經(jīng)度，C_lat表示當(dāng)前位置點(diǎn)的緯度，N_lng表示下一個航標(biāo)位置點(diǎn)的經(jīng)度，N_lat表示下一個航標(biāo)位置點(diǎn)的緯度；

S302、通過目標(biāo)航向角C_y計算當(dāng)前位置到下一個航標(biāo)位置點(diǎn)的直線距離Lcn：

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

其中，R是航跡半徑，通常取值地球半徑；

S303、計算上一個航標(biāo)位置點(diǎn)與當(dāng)前位置點(diǎn)所形成的向量與當(dāng)前位置與下一個航標(biāo)位置點(diǎn)所形成的向量之間的夾角a；

夾角a的計算可采用向量間夾角計算的常用手段，不再贅述。

S304、計算橫向加速度

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

其中，ground_speed為參考地面速度。

優(yōu)選地，所述動力調(diào)整單元所述在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出，確保在各種外圍動力影響因素存在的情況下維持航行速度不變，包括如果系統(tǒng)有智能避障功能，采用以下方式更新橫向加速度：

lat_acc＝lat_acc+(turn_angle/C_A)×turn_max_g

其中，turn_angle表示獲取的第二轉(zhuǎn)彎角度，turn_max_g是根據(jù)船體類型設(shè)置的最大轉(zhuǎn)彎加速度，單位是重力加速度。

優(yōu)選地，在定速巡航模式下進(jìn)行周期性調(diào)整油門輸出，確保在各種外圍動力影響因素存在的情況下維持航行速度不變，包括基于橫向加速度計算油門實(shí)際輸出值,如圖4所示，具體包括：

S401、根據(jù)橫向加速度計算轉(zhuǎn)彎比率turn_rate

turn_rate＝lat_acc/turn_max_g×G，

其中，G是重力加速度。

S402、根據(jù)系統(tǒng)限制的最大轉(zhuǎn)彎角度Tmax(默認(rèn)取值是90，即允許最大90度轉(zhuǎn)彎)，以及系統(tǒng)限制的最大轉(zhuǎn)彎油門增益T_Gain(避免油門突變過大造成系統(tǒng)不穩(wěn)定，百分比乘以100),計算轉(zhuǎn)彎限制系數(shù)turn_ratio，其中C_y是前面計算的目標(biāo)偏航角：

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

系統(tǒng)限制的最大轉(zhuǎn)彎角度Tmax和系統(tǒng)限制的最大轉(zhuǎn)彎油門增益T_Gain由系統(tǒng)設(shè)定，yaw_sensor表示航向角，從主控器獲取。

S403、根據(jù)轉(zhuǎn)彎比率和轉(zhuǎn)彎限制系數(shù)計算油門降速比率turn_reduction

turn_reduction＝1.0-turn_rate×turn_ratio

S404、根據(jù)油門降速比率修正目標(biāo)巡航速度speed_cruise

speed_cruise＝speed_cruise×turn_reduction

S405、根據(jù)修正的目標(biāo)巡航速度與參考地面速度ground_speed的差值進(jìn)行PID調(diào)節(jié)，得到油門偏差throttle_err；

PID調(diào)節(jié)方法可采用現(xiàn)有技術(shù)，不再贅述。

S406、根據(jù)油門偏差修正期望油門值，得到油門實(shí)際輸出值。

throttle＝throttle_target+throttle_err

其中throttle_target是設(shè)定的speed_cruise對應(yīng)的期望油門值。

以上所舉實(shí)施例，對本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)進(jìn)行了進(jìn)一步的詳細(xì)說明，所應(yīng)理解的是，以上所舉實(shí)施例僅為本發(fā)明的優(yōu)選實(shí)施方式而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)對本發(fā)明所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。