一種機器人的運動控制方法及控制系統(tǒng)的制作方法
【專利摘要】本發(fā)明提供一種機器人的運動控制方法及控制系統(tǒng)，包括如下步驟：S20根據(jù)輸入的地圖信息、規(guī)劃路徑及實時位置信息，判斷機器人當前所處位置是否在規(guī)劃路徑上，計算出機器人在規(guī)劃路徑上的目標位置信息；S30若機器人當前所處位置不在規(guī)劃路徑上，則根據(jù)實時位置信息和目標位置信息，使得機器人朝向目標位置；否則，直接跳轉(zhuǎn)至步驟S40；S40根據(jù)實時位置信息和目標位置信息，計算出機器人的線速度、角速度和實時朝向后，控制機器人運動。本發(fā)明可通過計算規(guī)劃路徑上距離機器人最近的點作為目標位置，以及使得機器人朝向目標位置，不僅使得機器人在最短時間回到規(guī)劃路徑上，還能控制機器人按照任意規(guī)劃路徑快速、精準地行走。
【專利說明】
一種機器人的運動控制方法及控制系統(tǒng)
技術(shù)領域
[0001]本發(fā)明涉及機器人技術(shù)領域，尤指是機器人的運動控制方法及控制系統(tǒng)。
【背景技術(shù)】
[0002]隨著科技不斷進步，機器人在當今社會中扮演著重要角色，由于機器人在某些方面可以自主完成各種任務，例如清潔機器人、滅火機器人，從而達到節(jié)省人力資源、減小消防人員安危等目的。
[0003]其中，控制機器人按照規(guī)劃路徑行走是機器人自主完成各種任務的關(guān)鍵。目前，通常是采用非完整控制系統(tǒng)來控制機器人運動，其非完整控制系統(tǒng)是:通過某種控制輸入的作用，使得機器人快速、精確地移動到運動空間的某一位置或規(guī)劃路徑上。
[0004]在現(xiàn)實中，由于系統(tǒng)物理特性的限制(例如機器人自身性能的限制)，或者安全因素(例如路況不平坦等安全因素)，實際系統(tǒng)的控制輸入都會受到飽和約束。在非完整控制系統(tǒng)中利用兩個輸入量控制三個輸出量，存在較大的難度;再考慮飽和約束，更難以控制機器人按照規(guī)劃路徑快速、精確地行走。

【發(fā)明內(nèi)容】

[0005]本發(fā)明為了解決機器人快速、精確行走的問題，提供一種機器人的運動控制方法及控制裝置。該控制方法及控制裝置可控制機器人按照任意規(guī)劃路徑快速、精準地行走。
[0006]為了實現(xiàn)本發(fā)明以上發(fā)明目的，本發(fā)明是通過以下技術(shù)方案實現(xiàn)的:
[0007]本發(fā)明提供一種機器人的運動控制方法，包括如下步驟:S20根據(jù)輸入的地圖信息、規(guī)劃路徑及實時位置信息，判斷所述機器人當前所處位置是否在所述規(guī)劃路徑上，計算出機器人在所述規(guī)劃路徑上的目標位置信息；S30若所述機器人當前所處位置不在所述規(guī)劃路徑上，則根據(jù)所述實時位置信息和目標位置信息，使得所述機器人朝向所述目標位置；否則，直接跳轉(zhuǎn)至步驟S40; S40根據(jù)所述實時位置信息和目標位置信息，計算出所述機器人的線速度、角速度和實時朝向后，控制所述機器人運動。
[0008]進一步，所述步驟S30包括:S31根據(jù)所述機器人的實時位置信息、目標位置信息，計算出所述實時位置和目標位置所在的直線段與坐標系上橫軸所形成的目標夾角，所述坐標系設置在所述地圖上；S32獲取所述機器人的當前朝向與所述坐標系上橫軸所形成的當前夾角；S33判斷所述當前夾角與所述目標夾角是否相同；S34若所述當前夾角與所述目標夾角不同，則所述機器人以預設角速度旋轉(zhuǎn)后，跳轉(zhuǎn)至步驟S33;否則，所述機器人保持當前朝向。
[0009]進一步，本機器人的運動控制方法，還包括如下步驟:S41根據(jù)所述機器人的目標位置信息，計算出所述目標位置的相鄰位置信息；S42根據(jù)所述相鄰位置信息，計算出所述機器人所在路徑的當前曲率值;S43判斷所述當前曲率值與第一曲率閾值的大小;S44若所述當前曲率值大于所述第一曲率閾值，則進一步判斷所述機器人的當前速度與第一速度閾值的大小；否則，跳轉(zhuǎn)至步驟S46; S45若所述機器人的當前速度大于第一速度閾值，則所述機器人的當前速度以第一預設閾值進行減小后，跳轉(zhuǎn)至步驟S44;否則，跳轉(zhuǎn)至步驟S46; S46判斷所述機器人是否到達所述目標位置;S47若所述機器人到達所述目標位置，則進一步判斷所述機器人的當前朝向與所述目標位置的規(guī)劃朝向之間的差值是否滿足第一閾值；否貝1J，跳轉(zhuǎn)至步驟S41;S48若所述機器人的當前朝向與所述目標位置的規(guī)劃朝向之間的差值不滿足第一閾值，則所述機器人以預設角速度旋轉(zhuǎn)后，跳轉(zhuǎn)至步驟S47。
[0010]進一步，本機器人的運動控制方法，還包括如下步驟:S50判斷所述目標位置是否為所述規(guī)劃路徑上的終點位置;S60若所述目標位置不是所述規(guī)劃路徑上的終點位置，則跳轉(zhuǎn)至步驟S20;否則，所述機器人的當前速度以第二預設閾值進行減小。
[0011]進一步，本機器人的運動控制方法，還包括如下步驟:S70判斷所述機器人是否到達所述終點位置;S80若所述機器人到達所述終點位置，則進一步判斷所述機器人的當前朝向與所述終點位置的規(guī)劃朝向是否相同；否則，跳轉(zhuǎn)至步驟S60;S90若所述機器人的當前朝向與所述終點位置的規(guī)劃朝向不同，所述機器人以預設角速度旋轉(zhuǎn)后，跳轉(zhuǎn)至步驟S80;否則，所述機器人保持當前朝向。
[0012]進一步，所述步驟S20中的地圖呈柵格狀，所述步驟S20中的目標位置信息是指:所述規(guī)劃路徑上的所有規(guī)劃點中距離所述機器人當前所處位置最近的規(guī)劃點信息，或所述規(guī)劃路徑上距離所述機器人當前所處位置最近的點位置信息。
[0013]進一步，所述步驟S40是指:根據(jù)所述實時位置信息和目標位置信息，在極坐標模型下，利用Lyapunov函數(shù)，利用Lyapunov直接法設計跟蹤控制器，計算出所述機器人的線速度、角速度和實時朝向。
[0014]本發(fā)明還提供一種機器人的運動控制系統(tǒng)，包括:判斷模塊，所述判斷模塊根據(jù)輸入的地圖信息、規(guī)劃路徑及實時位置信息，判斷機器人當前所處位置是否在所述規(guī)劃路徑上;計算模塊，所述計算模塊與所述判斷模塊電連接，用于計算出所述機器人在所述規(guī)劃路徑上的目標位置信息;朝向控制模塊，所述朝向控制模塊分別與所述判斷模塊、計算模塊電連接，所述朝向控制模塊接收所述判斷模塊輸出的所述機器人當前所處位置不在所述規(guī)劃路徑上，所述朝向控制模塊根據(jù)所述計算模塊輸出的所述實時位置信息和目標位置信息，使得所述機器人朝向所述目標位置;運動控制模塊，所述運動控制模塊分別與所述判斷模塊、朝向控制模塊電連接，所述運動控制模塊根據(jù)所述實時位置信息和目標位置信息，計算出所述機器人的線速度、角速度和實時朝向后，控制所述機器人運動。
[0015]進一步，本機器人的運動控制系統(tǒng)，還包括:所述計算模塊還根據(jù)所述機器人的實時位置信息、目標位置信息，計算出所述實時位置和目標位置所在的直線段與坐標系上橫軸所形成的目標夾角，所述坐標系設置在所述地圖上;所述獲取模塊用于獲取所述機器人的當前朝向與所述坐標系上橫軸所形成的當前夾角；所述判斷模塊還分別與所述計算模塊、獲取模塊電連接，所述判斷模塊接收所述計算模塊輸出的目標夾角，所述判斷模塊還接收所述獲取模塊輸出的當前夾角，判斷所述當前夾角與所述目標夾角是否相同；所述朝向控制模塊與所述判斷模塊電連接，所述朝向控制模塊接收所述判斷模塊輸出的所述當前夾角與所述目標夾角不同，則所述機器人以預設角速度旋轉(zhuǎn)；否則，所述機器人保持當前朝向。
[0016]進一步，本機器人的運動控制系統(tǒng)，還包括:所述計算模塊還根據(jù)所述機器人的目標位置信息，計算出所述目標位置的相鄰位置信息;所述計算模塊還根據(jù)所述相鄰位置信息，計算出所述機器人所在路徑的當前曲率值;所述判斷模塊接收所述計算模塊輸出的當前曲率值，判斷所述當前曲率值與第一曲率閾值的大小;若所述當前曲率值大于所述第一曲率閾值，所述判斷模塊進一步判斷所述機器人的當前速度與第一速度閾值的大小;速度控制模塊，所述速度控制模塊與所述判斷模塊電連接，所述速度控制模塊接收所述判斷模塊輸出的所述機器人的當前速度大于第一速度閾值，則所述機器人的當前速度以第一預設閾值進行減小;所述判斷模塊還判斷所述機器人是否到達所述目標位置;若所述機器人到達所述目標位置，所述判斷模塊進一步判斷所述機器人的當前朝向與所述目標位置的規(guī)劃朝向之間的差值是否滿足第一閾值;所述朝向控制模塊接收所述判斷模塊輸出的所述機器人的當前朝向與所述目標位置的規(guī)劃朝向之間的差值不滿足第一閾值，所述機器人以預設角速度旋轉(zhuǎn)。
[0017]本發(fā)明至少具有以下有益效果之一:
[0018]1、本發(fā)明中機器人在任意初始位置時，通過計算出規(guī)劃路徑上距離機器人最近的點作為目標位置，以及使得機器人朝向目標位置，可讓機器人盡快移動到規(guī)劃路徑上，從而增大機器人的可移動范圍。同時，機器人因不確定外界因素偏離路徑后，可通過計算規(guī)劃路徑上距離機器人最近的點作為目標位置，以及使得機器人朝向目標位置，不僅使得機器人在最短時間回到規(guī)劃路徑上，還能控制機器人按照任意規(guī)劃路徑快速、精準地行走。
[0019]2、本發(fā)明中機器人由于一些不確定因素超過預設閾值后，可自動調(diào)節(jié)機器人的實時速度，不僅可以滿足機器人的最大速度要求，還使得機器人結(jié)合實際路況，更為準確地按照規(guī)劃路徑行走，提高了機器人靈活性、工作效率。
[0020]3、本發(fā)明中還通過遍歷機器人的目標位置前后一定范圍路徑的曲率，根據(jù)比較當前曲率與第一曲率閾值，來調(diào)整實時速度，使得機器人滿足飽和輸入條件下的路徑跟蹤控制，實現(xiàn)對任意路徑的跟蹤，降低了對路徑規(guī)劃的要求，極大的提高了路徑跟蹤的精確度，使機器人可在狹窄區(qū)域完成路徑跟蹤。
【附圖說明】
[0021 ]下面結(jié)合附圖和【具體實施方式】對本發(fā)明作進一步詳細說明:
[0022]圖1為一種機器人的運動控制方法的流程示意圖；
[0023]圖2為機器人的運動控制方法的一部分流程示意圖；
[0024]圖3為機器人的運動控制方法的另一部分流程示意圖；
[0025]圖4為另一種機器人的運動控制方法的流程示意圖；
[0026]圖5為一種機器人的運動控制系統(tǒng)的組成結(jié)構(gòu)示意圖；
[0027]圖6為機器人的運動控制系統(tǒng)的一部分組成結(jié)構(gòu)示意圖；
[0028]圖7為機器人的運動控制系統(tǒng)的另一部分組成結(jié)構(gòu)示意圖；
[0029]圖8為再一種機器人的運動控制方法的流程示意圖；
[0030]圖9為機器人的運動控制方法的再一部分流程示意圖；
[0031 ]圖10為機器人的運動控制方法的又一部分流程示意圖；
[0032]圖11為機器人的運動控制方法的又又一部分流程示意圖；
[0033]圖12為機器人的運動控制方法的又又又一部分流程示意圖；
[0034]圖13a為機器人初始位置不在規(guī)劃路徑上的一種的運動示意圖；
[0035]圖13b為機器人初始位置不在規(guī)劃路徑上的另一種的運動示意圖；
[0036]圖14a為機器人偏離規(guī)劃路徑的一種的運動示意圖；
[0037]圖14b為機器人偏離規(guī)劃路徑的另一種的運動示意圖；
[0038]圖15a為機器人偏離規(guī)劃路徑后回到規(guī)劃路徑的一種的運動示意圖；
[0039]圖15b為機器人偏離規(guī)劃路徑后回到規(guī)劃路徑的另一種的運動示意圖；
[0040]圖16a為機器人轉(zhuǎn)彎時的一種的運動不意圖；
[0041]圖16b為機器人轉(zhuǎn)彎時的另一種的運動不意圖；
[0042]圖17a為規(guī)劃路徑上規(guī)劃點的一種設置示意圖；
[0043]圖17b為規(guī)劃路徑上規(guī)劃點的另一種設置示意圖；
[0044]圖中:
[0045]10-計算模塊 n-獲取模塊20-判斷模塊
[0046]30-朝向控制模塊40-運動控制模塊 41-速度控制模塊；
[0047]100-規(guī)劃路徑 100’、100”_實際行走路徑
[0048]10、10 ’ -走到規(guī)劃路徑的實際行走路徑
[0049]23、23’_偏離規(guī)劃路徑的實際行走路徑
[0050]34、34’、35、35’_回歸至規(guī)劃路徑的實際行走路徑
[0051]101-初始位置 102、103、_規(guī)劃路徑上距離機器人最近的點[0052 ]104、106、1011、1015-規(guī)劃路徑上的規(guī)劃點
[0053]105-偏離位置 A-機器人B-墻壁。
【具體實施方式】
[0054]為了更清楚地說明本發(fā)明實施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對實施例或現(xiàn)有技術(shù)描述中所需要使用的附圖作簡單地介紹，顯而易見地，以下說明和附圖對于本發(fā)明是示例性的，并且不應被理解為限制本發(fā)明。以下說明描述了眾多具體細節(jié)以方便對本發(fā)明理解。然而，在某些實例中，熟知的或常規(guī)的細節(jié)并未說明，以滿足說明書簡潔的要求。
[0055]本發(fā)明機器人的運動控制方法可以通過java語言進行編寫，同理也可以使用C#，VB，C++/C等其他編程語言進行實現(xiàn)。
[0056]如圖1所述，根據(jù)本發(fā)明的一個實施例，一種機器人的運動控制方法，包括如下步驟:
[0057]SlO輸入的地圖信息、規(guī)劃路徑，以及機器人的實時位置信息，所述地圖呈柵格狀，每個柵格的大小可以根據(jù)具體情況進行設置，所述規(guī)劃路徑經(jīng)過若干個柵格，所述規(guī)劃點的選取:間隔選取設定距離的柵格為規(guī)劃點，例如每隔五個柵格，設置一個規(guī)劃點；或者每隔十個柵格，設置一個規(guī)劃點；
[0058]S20根據(jù)輸入的地圖信息、規(guī)劃路徑及實時位置信息，判斷機器人當前所處位置是否在所述規(guī)劃路徑上;此處，可以判斷機器人的初始位置是否在規(guī)劃路徑上;也可以判斷機器人在行走過程中，是否在規(guī)劃路徑上行走；
[0059]計算出所述機器人在所述規(guī)劃路徑上的目標位置信息；實時位置是指:機器人當前所處位置;當所述機器人在規(guī)劃路徑上時，目標位置是指:所述規(guī)劃路徑上的所有規(guī)劃點中距離所述機器人當前所處位置最近的規(guī)劃點；當所述機器人不在規(guī)劃路徑上時，目標位置是指:所述規(guī)劃路徑上距離所述機器人當前所處位置最近的點；
[0060]S30若所述機器人當前所處位置不在所述規(guī)劃路徑上，則根據(jù)所述實時位置信息和目標位置信息，使得所述機器人朝向所述目標位置;否則，直接跳轉(zhuǎn)至步驟S40;
[0061 ] S40根據(jù)所述實時位置信息和目標位置信息，計算出所述機器人的線速度、角速度和實時朝向后，控制所述機器人運動;優(yōu)選的，根據(jù)所述實時位置信息和目標位置信息，在極坐標模型下，利用Lyapunov函數(shù)，利用Lyapunov直接法設計跟蹤控制器，計算出所述機器人的線速度、角速度和實時朝向。
[0062]具體的，機器人的初始位置101未在規(guī)劃路徑上時，如圖13a、13b所示，100是規(guī)劃路徑，機器人從初始位置101行走到已知規(guī)劃路徑100上的最近點位置102時，方法中不包括步驟S30(根據(jù)所述實時位置信息和目標位置信息，使得所述機器人朝向所述目標位置)時，機器人的行走路徑為12;方法中包括步驟S30時，機器人的行走路徑為12’。比較行走路徑12和行走路徑12’后，發(fā)現(xiàn)圖13b中機器人的行走路徑12’更短，并且機器人可以在狹窄區(qū)域完成行走。
[0063]如圖1、圖2所示，根據(jù)本發(fā)明的另一個實施例，一種機器人的運動控制方法，包括如下步驟:
[0064]SlO輸入的地圖信息、規(guī)劃路徑，以及機器人的實時位置信息，所述地圖呈柵格狀，每個柵格的大小可以根據(jù)具體情況進行設置，所述規(guī)劃路徑經(jīng)過若干個柵格，所述規(guī)劃點的選取:間隔選取設定距離的柵格為規(guī)劃點，例如每隔五個柵格，設置一個規(guī)劃點；或者每隔十個柵格，設置一個規(guī)劃點；
[0065]S20根據(jù)輸入的地圖信息、規(guī)劃路徑及實時位置信息，判斷機器人當前所處位置是否在所述規(guī)劃路徑上;此處，可以判斷機器人的初始位置是否在規(guī)劃路徑上;也可以判斷機器人在行走過程中，是否在規(guī)劃路徑上行走；
[0066]計算出所述機器人在所述規(guī)劃路徑上的目標位置信息；實時位置是指:機器人當前所處位置;當所述機器人在規(guī)劃路徑上時，目標位置是指:所述規(guī)劃路徑上的所有規(guī)劃點中距離所述機器人當前所處位置最近的規(guī)劃點；當所述機器人不在規(guī)劃路徑上時，目標位置是指:所述規(guī)劃路徑上距離所述機器人當前所處位置最近的點；
[0067]S30若所述機器人當前所處位置不在所述規(guī)劃路徑上，則根據(jù)所述實時位置信息和目標位置信息，使得所述機器人朝向所述目標位置;否則，直接跳轉(zhuǎn)至步驟S40:
[0068]S31根據(jù)所述機器人的實時位置信息、目標位置信息，計算出所述實時位置和目標位置所在的直線段與坐標系上橫軸所形成的目標夾角，所述坐標系設置在所述地圖上；
[0069]S32獲取所述機器人的當前朝向與所述坐標系上橫軸所形成的當前夾角；
[0070]S33判斷所述當前夾角與所述目標夾角是否相同；
[0071]S34若所述當前夾角與所述目標夾角不同，則所述機器人以預設角速度旋轉(zhuǎn)后，跳轉(zhuǎn)至步驟S33;
[0072]S35若所述當前夾角與所述目標夾角相同，則所述機器人保持當前朝向；跳轉(zhuǎn)至步驟 S40;
[0073]這里的夾角計算僅用在算規(guī)劃路徑上距離機器人最近的點，一旦計算出最近點，把最近點作為第一個目標點，接著機器人運動到達第一個目標，然后機器人向下一個目標點運動，之后不用計算所述實時位置和目標位置所在的直線段與坐標系上橫軸所形成的目標夾角，直到機器人如果偏離已知規(guī)劃路徑一定范圍，需要重新計算最近點，此時重復上述過程；
[0074]S40根據(jù)所述實時位置信息和目標位置信息，計算出所述機器人的線速度、角速度和實時朝向后，控制所述機器人運動;優(yōu)選的，根據(jù)所述實時位置信息和目標位置信息，在極坐標模型下，利用Lyapunov函數(shù)，利用Lyapunov直接法設計跟蹤控制器，計算出所述機器人的線速度、角速度和實時朝向。
[0075]具體的，機器人在行走過程中(在規(guī)劃路徑100上最近點102至規(guī)劃點104這段距離時)，由于一些不確定因素(路面不平、輪子打滑等)，造成機器人偏離已知規(guī)劃路徑100，如圖14a、14b所示。23是機器人非正常情況下的行走路徑。求取已知規(guī)劃路徑上的最近點103作為目標位置，最近點是指:規(guī)劃路徑上距離機器人最近的點，最近點可以是規(guī)劃路徑所經(jīng)過的一個柵格，也可以是規(guī)劃點；使機器人最快趨近已知規(guī)劃路徑100。圖14a為不計算最近點作為目標位置104時，機器人的行走路徑34;圖14b為計算最近點103作為目標位置時，機器人的行走路徑34’。比較行走路徑34和行走路徑34’后，發(fā)現(xiàn)機器人按照行走路徑34’行走，會最快回到規(guī)劃路徑上。
[0076]如圖1、圖3所示，根據(jù)本發(fā)明的再一個實施例，一種機器人的運動控制方法，包括如下步驟:
[0077]SlO輸入的地圖信息、規(guī)劃路徑，以及機器人的實時位置信息，所述地圖呈柵格狀，每個柵格的大小可以根據(jù)具體情況進行設置，所述規(guī)劃路徑經(jīng)過若干個柵格，所述規(guī)劃點的選取:間隔選取設定距離的柵格為規(guī)劃點，例如每隔五個柵格，設置一個規(guī)劃點；或者每隔十個柵格，設置一個規(guī)劃點；
[0078]S20根據(jù)輸入的地圖信息、規(guī)劃路徑及實時位置信息，判斷機器人當前所處位置是否在所述規(guī)劃路徑上;此處，可以判斷機器人的初始位置是否在規(guī)劃路徑上;也可以判斷機器人在行走過程中，是否在規(guī)劃路徑上行走；
[0079]計算出所述機器人在所述規(guī)劃路徑上的目標位置信息；實時位置是指:機器人當前所處位置;當所述機器人在規(guī)劃路徑上時，目標位置是指:所述規(guī)劃路徑上的所有規(guī)劃點中距離所述機器人當前所處位置最近的規(guī)劃點；當所述機器人不在規(guī)劃路徑上時，目標位置是指:所述規(guī)劃路徑上距離所述機器人當前所處位置最近的點；
[0080]S30若所述機器人當前所處位置不在所述規(guī)劃路徑上，則根據(jù)所述實時位置信息和目標位置信息，使得所述機器人朝向所述目標位置;否則，直接跳轉(zhuǎn)至步驟S40;
[0081 ] S40根據(jù)所述實時位置信息和目標位置信息，計算出所述機器人的線速度、角速度和實時朝向后，控制所述機器人運動;優(yōu)選的，根據(jù)所述實時位置信息和目標位置信息，在極坐標模型下，利用Lyapunov函數(shù)，利用Lyapunov直接法設計跟蹤控制器，計算出所述機器人的線速度、角速度和實時朝向；
[0082]S41根據(jù)所述機器人的目標位置信息，計算出所述目標位置的相鄰位置信息;相鄰位置是指:目標位置周側(cè)的預設位置；
[0083]S42根據(jù)所述相鄰位置信息，計算出所述機器人所在路徑的當前曲率值；
[0084]S43判斷所述當前曲率值與第一曲率閾值的大小，所述第一曲率閥值是根據(jù)機器人的性能，而得到的轉(zhuǎn)彎路徑的最大曲率值;判斷機器人是否能夠轉(zhuǎn)過此彎；
[0085]S44若所述當前曲率值大于所述第一曲率閾值，則進一步判斷所述機器人的當前速度與第一速度閾值的大小，所述第一速度閾值是根據(jù)機器人自身的性能，而得到的轉(zhuǎn)彎時的最大速度值，判斷機器人當前速度是否滿足在轉(zhuǎn)彎時的最大速度；否則，跳轉(zhuǎn)至步驟S46；
[0086]S45若所述機器人的當前速度大于第一速度閾值，則所述機器人的當前速度以第一預設閾值進行減小后，跳轉(zhuǎn)至步驟S44;否則，跳轉(zhuǎn)至步驟S46;
[0087]S46判斷所述機器人是否到達所述目標位置；
[0088]S47若所述機器人到達所述目標位置，則進一步判斷所述機器人的當前朝向與所述目標位置的規(guī)劃朝向之間的差值是否滿足第一閾值;允許機器人轉(zhuǎn)彎時，有適當誤差;否貝1J，跳轉(zhuǎn)至步驟S41;
[0089]S48若所述機器人的當前朝向與所述目標位置的規(guī)劃朝向之間的差值不滿足第一閾值，則所述機器人以預設角速度旋轉(zhuǎn)后，跳轉(zhuǎn)至步驟S47。
[0090]具體的，在飽和輸入條件下，即在有速度、加速度限制條件下，機器人從當前位置105行走到已知規(guī)劃路徑100上最近點103，如圖15a、15b所示，如圖16a、16b所示。不計算路徑曲率時，機器人的行走路徑35、100;計算路徑曲率時，機器人的行走路徑35’、100’。比較行走路徑35、100和行走路徑35 ’、100 ’后，發(fā)現(xiàn)計算路徑曲率后，機器人行走路徑更為精確，不會因速度、加速度的限制，而造成機器人速度不能到達控制輸入值，使得機器人沖出目標點。
[0091]如圖4所示，根據(jù)本發(fā)明的又一個實施例，一種機器人的運動控制方法，包括如下步驟:
[0092]SlO輸入的地圖信息、規(guī)劃路徑，以及機器人的實時位置信息，所述地圖呈柵格狀，每個柵格的大小可以根據(jù)具體情況進行設置，所述規(guī)劃路徑經(jīng)過若干個柵格，所述規(guī)劃點的選取:間隔選取設定距離的柵格為規(guī)劃點，例如每隔五個柵格，設置一個規(guī)劃點；或者每隔十個柵格，設置一個規(guī)劃點；
[0093]S20根據(jù)輸入的地圖信息、規(guī)劃路徑及實時位置信息，判斷機器人當前所處位置是否在所述規(guī)劃路徑上;此處，可以判斷機器人的初始位置是否在規(guī)劃路徑上;也可以判斷機器人在行走過程中，是否在規(guī)劃路徑上行走；
[0094]計算出所述機器人在所述規(guī)劃路徑上的目標位置信息；實時位置是指:機器人當前所處位置;當所述機器人在規(guī)劃路徑上時，目標位置是指:所述規(guī)劃路徑上的所有規(guī)劃點中距離所述機器人當前所處位置最近的規(guī)劃點；當所述機器人不在規(guī)劃路徑上時，目標位置是指:所述規(guī)劃路徑上距離所述機器人當前所處位置最近的點；
[0095]S30若所述機器人當前所處位置不在所述規(guī)劃路徑上，則根據(jù)所述實時位置信息和目標位置信息，使得所述機器人朝向所述目標位置;否則，直接跳轉(zhuǎn)至步驟S40;
[0096]S40根據(jù)所述實時位置信息和目標位置信息，計算出所述機器人的線速度、角速度和實時朝向后，控制所述機器人運動;優(yōu)選的，根據(jù)所述實時位置信息和目標位置信息，在極坐標模型下，利用Lyapunov函數(shù)，利用Lyapunov直接法設計跟蹤控制器，計算出所述機器人的線速度、角速度和實時朝向；
[0097]S50判斷所述目標位置是否為所述規(guī)劃路徑上的終點位置；
[0098]S60若所述目標位置不是所述規(guī)劃路徑上的終點位置，則跳轉(zhuǎn)至步驟S20;否則，所述機器人的當前速度以第二預設閾值進行減??；
[0099]S70判斷所述機器人是否到達所述終點位置；
[0100]S80若所述機器人到達所述終點位置，則進一步判斷所述機器人的當前朝向與所述終點位置的規(guī)劃朝向是否相同，若對機器人到達終點位置時，沒有朝向要求，可省略此處機器人朝向判斷及機器人后續(xù)朝向調(diào)整步驟;否則，跳轉(zhuǎn)至步驟S60;
[0101]S90若所述機器人當前朝向與所述終點位置的規(guī)劃朝向不同，所述機器人以預設角速度旋轉(zhuǎn)后，跳轉(zhuǎn)至步驟S80;
[0102]S91若所述機器人當前朝向與所述終點位置的規(guī)劃朝向相同，則所述機器人保持當前朝向。
[0103]具體的，在選取機器人在規(guī)劃路徑上的規(guī)劃點時，規(guī)劃點之間的距離較大，其最大速度較大，如圖17a所示；規(guī)劃點之間的距離較小，其最大速度較小，如圖17b所示；即圖17a中機器人最大速度大于圖17b中機器人最大速度，規(guī)劃點的適當選取，可以滿足對機器人最大速度的要求。
[0104]如圖5、圖6、圖7所示，根據(jù)本發(fā)明的一個實施例，一種機器人的運動控制系統(tǒng)，包括:
[0105]判斷模塊20，所述判斷模塊20根據(jù)輸入的地圖信息、規(guī)劃路徑及實時位置信息，判斷機器人當前所處位置是否在所述規(guī)劃路徑上；
[0106]計算模塊10，所述計算模塊10與所述判斷模塊20電連接，計算出所述機器人在所述規(guī)劃路徑上的目標位置信息；
[0107]朝向控制模塊30，所述朝向控制模塊30分別與所述判斷模塊20、計算模塊10電連接，所述朝向控制模塊30接收所述判斷模塊20輸出的所述機器人當前所處位置不在所述規(guī)劃路徑上，所述朝向控制模塊30根據(jù)所述計算模塊10輸出的所述實時位置信息和目標位置信息，使得所述機器人朝向所述目標位置；
[0108]運動控制模塊40，所述運動控制模塊40分別與所述判斷模塊20、朝向控制模塊30電連接，所述運動控制模塊40根據(jù)所述實時位置信息和目標位置信息，計算出所述機器人的線速度、角速度和實時朝向后，控制所述機器人運動。
[0109]優(yōu)選的，所述計算模塊10還根據(jù)所述機器人的實時位置信息、目標位置信息，計算出所述實時位置和目標位置所在的直線段與坐標系上橫軸所形成的目標夾角，所述坐標系設置在所述地圖上；
[0110]優(yōu)選的，還包括獲取模塊11，所述獲取模塊11用于獲取所述機器人的當前朝向與所述坐標系上橫軸所形成的當前夾角；
[0111]優(yōu)選的，所述判斷模塊20還分別與所述計算模塊10、獲取模塊11電連接，所述判斷模塊20接收所述計算模塊10輸出的目標夾角，所述判斷模塊20還接收所述獲取模塊11輸出的當前夾角，判斷所述當前夾角與所述目標夾角是否相同；
[0112]優(yōu)選的，所述朝向控制模塊30與所述判斷模塊20電連接，所述朝向控制模塊30接收所述判斷模塊20輸出的所述當前夾角與所述目標夾角不同，則所述機器人以預設角速度旋轉(zhuǎn);否則，所述機器人保持當前朝向；
[0113]優(yōu)選的，所述計算模塊10還根據(jù)所述機器人的目標位置信息，計算出所述目標位置的相鄰位置信息；
[0114]優(yōu)選的，所述計算模塊10還根據(jù)所述相鄰位置信息，計算出所述機器人所在路徑的當前曲率值；
[0115]優(yōu)選的，所述判斷模塊20接收所述計算模塊10輸出的當前曲率值，判斷所述當前曲率值與第一曲率閾值的大小；
[0116]優(yōu)選的，若所述當前曲率值大于所述第一曲率閾值，所述判斷模塊20進一步判斷所述機器人的當前速度與第一速度閾值的大小；
[0117]優(yōu)選的，還包括速度控制模塊41，所述速度控制模塊41與所述判斷模塊20電連接，所述速度控制模塊41接收所述判斷模塊20輸出的所述機器人的當前速度大于第一速度閾值，則所述機器人的當前速度以第一預設閾值進行減??；
[0118]優(yōu)選的，所述判斷模塊20還判斷所述機器人是否到達所述目標位置;若所述機器人到達所述目標位置，所述判斷模塊20進一步判斷所述機器人的當前朝向與所述目標位置的規(guī)劃朝向之間的差值是否滿足第一閾值；
[0119]優(yōu)選的，所述朝向控制模塊30接收所述判斷模塊20輸出的所述機器人的當前朝向與所述目標位置的規(guī)劃朝向之間的差值不滿足第一閾值，所述機器人以預設角速度旋轉(zhuǎn)。
[0120]如圖5、圖6、圖7所示，根據(jù)本發(fā)明的另一個實施例，一種機器人的運動控制系統(tǒng)，包括:
[0121]優(yōu)選的，還包括輸入模塊，用于輸入的地圖信息、規(guī)劃路徑，以及機器人的實時位置信息，所述地圖呈柵格狀，每個柵格的大小可以根據(jù)具體情況進行設置，所述規(guī)劃路徑經(jīng)過若干個柵格，所述規(guī)劃點的選取:間隔選取設定距離的柵格為規(guī)劃點，例如每隔五個柵格，設置一個規(guī)劃點;或者每隔十個柵格，設置一個規(guī)劃點；
[0122]判斷模塊20，用于根據(jù)輸入的地圖信息、規(guī)劃路徑及實時位置信息，判斷機器人當前所處位置是否在所述規(guī)劃路徑上;此處，可以判斷機器人的初始位置是否在規(guī)劃路徑上；也可以判斷機器人在行走過程中，是否在規(guī)劃路徑上行走；
[0123]計算模塊10，用于計算出所述機器人在所述規(guī)劃路徑上的目標位置信息；實時位置是指:機器人當前所處位置；當所述機器人在規(guī)劃路徑上時，目標位置是指:所述規(guī)劃路徑上的所有規(guī)劃點中距離所述機器人當前所處位置最近的規(guī)劃點；當所述機器人不在規(guī)劃路徑上時，目標位置是指:所述規(guī)劃路徑上距離所述機器人當前所處位置最近的點；；
[0124]朝向控制模塊30，若所述機器人當前所處位置不在所述規(guī)劃路徑上，則根據(jù)所述實時位置信息和目標位置信息，使得所述機器人朝向所述目標位置；
[0125]優(yōu)選的，所述計算模塊10還根據(jù)所述機器人的實時位置信息、目標位置信息，計算出所述實時位置和目標位置所在的直線段與坐標系上橫軸所形成的目標夾角，所述坐標系設置在所述地圖上；
[0126]優(yōu)選的，還包括獲取模塊11，用于獲取所述機器人的當前朝向與所述坐標系上橫軸所形成的當前夾角；
[0127]優(yōu)選的，所述判斷模塊20還判斷所述當前夾角與所述目標夾角是否相同；
[0128]優(yōu)選的，若所述當前夾角與所述目標夾角不同，所述朝向控制模塊30控制所述機器人以預設角速度旋轉(zhuǎn)；
[0129]優(yōu)選的，若所述當前夾角與所述目標夾角相同，所述朝向控制模塊30控制所述機器人保持當前朝向；
[0130]所述運動控制模塊40還根據(jù)所述實時位置信息和目標位置信息，計算出所述機器人的線速度、角速度和實時朝向后，控制所述機器人運動;優(yōu)選的，根據(jù)所述實時位置信息和目標位置信息，在極坐標模型下，利用Lyapunov函數(shù)，利用Lyapunov直接法設計跟蹤控制器，計算出所述機器人的線速度、角速度和實時朝向；
[0131 ]優(yōu)選的，所述計算模塊10還根據(jù)所述機器人的目標位置信息，計算出所述目標位置的相鄰位置信息;相鄰位置是指:目標位置周側(cè)的預設位置；
[0132]優(yōu)選的，所述計算模塊10還根據(jù)所述相鄰位置信息，計算出所述機器人所在路徑的當前曲率值；
[0133]優(yōu)選的，所述判斷模塊20還判斷所述當前曲率值與第一曲率閾值的大小，所述第一曲率閥值是根據(jù)機器人的性能，而得到的轉(zhuǎn)彎路徑的最大曲率值;判斷機器人是否能夠轉(zhuǎn)過此彎；
[0134]優(yōu)選的，若所述當前曲率值大于所述第一曲率閾值，所述判斷模塊20還進一步判斷所述機器人的當前速度與第一速度閾值的大小，所述第一速度閾值是根據(jù)機器人自身的性能，而得到的轉(zhuǎn)彎時的最大速度值，判斷機器人當前速度是否滿足在轉(zhuǎn)彎時的最大速度；
[0135]優(yōu)選的，還包括速度控制模塊41，若所述機器人的當前速度大于第一速度閾值，則所述機器人的當前速度以第一預設閾值進行減??；
[0136]優(yōu)選的，所述判斷模塊20還判斷所述機器人是否到達所述目標位置；
[0137]優(yōu)選的，若所述機器人到達所述目標位置，所述判斷模塊20還進一步判斷所述機器人的當前朝向與所述目標位置的規(guī)劃朝向之間的差值是否滿足第一閾值;允許機器人轉(zhuǎn)彎時，有適當誤差；
[0138]優(yōu)選的，若所述機器人的當前朝向與所述目標位置的規(guī)劃朝向之間的差值不滿足第一閾值，所述朝向控制模塊30還控制所述機器人以預設角速度旋轉(zhuǎn)；
[0139]優(yōu)選的，所述判斷模塊20還判斷所述目標位置是否為所述規(guī)劃路徑上的終點位置；
[0140]優(yōu)選的，若所述目標位置是所述規(guī)劃路徑上的終點位置，所述速度控制模塊41還所述機器人的當前速度以第二預設閾值進行減小；
[0141 ]優(yōu)選的，所述判斷模塊20還判斷所述機器人是否到達所述終點位置；
[0142]優(yōu)選的，若所述機器人到達所述終點位置，所述判斷模塊20還進一步判斷所述機器人的當前朝向與所述終點位置的規(guī)劃朝向是否相同，若對機器人到達終點位置時，沒有朝向要求，可省略此處機器人朝向判斷及機器人后續(xù)朝向調(diào)整步驟；
[0143]優(yōu)選的，若所述機器人當前朝向與所述終點位置的規(guī)劃朝向不同，所述朝向控制模塊30還控制所述機器人以預設角速度旋轉(zhuǎn)；否則，所述朝向控制模塊30還控制所述機器人保持當前朝向。
[0144]如圖8、圖9、圖10、圖11和圖12所不，根據(jù)本發(fā)明的另一個實施例，一種機器人的運動控制方法，包括:
[0145]S1:輸入的信息包括柵格地圖、規(guī)劃好的路徑、機器人的實時位置信息，將實際地圖轉(zhuǎn)換為柵格地圖，每個柵格為5cm X 5cm;
[0146]S2:計算已知規(guī)劃路徑上距離機器人最近的點，遍歷已知規(guī)劃路徑上所有目標點，計算與當前位置距離，距離最小的即為最近點，并將最近記為目標點，如圖7所示；
[0147]S3:機器人姿態(tài)判斷模塊，如圖9所示；
[0148]S3.1:連接機器人當前位置點和最近點，計算所得連線與全局坐標系橫軸的夾角，判斷機器人當前姿態(tài)是否與所得夾角相同；不同轉(zhuǎn)S3.2，相同轉(zhuǎn)S4;
[0149]S3.2:機器人以給定角速度原地旋轉(zhuǎn)一定角度，轉(zhuǎn)S3.1;
[0150]S4:根據(jù)機器人當前姿態(tài)信息、目標點姿態(tài)信息，把對路徑的跟蹤看作是機器人依次到達路徑上的坐標點，在極坐標誤差模型下，利用Lyapunov直接法設計跟蹤控制器，計算出控制輸入量:機器人線速度、角速度，并計算出機器人實時姿態(tài)，轉(zhuǎn)S5;
[0151]S5:判斷目標點是否為已知規(guī)劃路徑的終點，否則轉(zhuǎn)S6，是則轉(zhuǎn)S8;
[0152]S6:計算曲率模塊，如圖10所示；
[0153]S6.1:遍歷機器人實時位置前后一定范圍，計算出路徑曲率轉(zhuǎn)，S6.2;
[0154]S6.2:判斷計算所得曲率值與閾值曲率的大小，小于閾值轉(zhuǎn)S6.5，大于閾值轉(zhuǎn)S6.3；
[0155]S6.3:判斷機器人當前時刻速度與閾值速度大小，小于閾值轉(zhuǎn)S6.5，大于閾值轉(zhuǎn)S6.4；
[0156]S6.4:機器人速度以某一閾值減小，轉(zhuǎn)S6.3;
[0157]S6.5:判斷機器人是否到達目標點，否則轉(zhuǎn)S6.1，是則轉(zhuǎn)S6.6;
[0158]S6.6:判斷機器人當前姿態(tài)是否滿足閾值，否則轉(zhuǎn)S6.7，是則轉(zhuǎn)S7;
[0159]S6.7:機器人以給定角速度旋轉(zhuǎn)一定角度，轉(zhuǎn)S6.6;
[0160]S7:計算機器人下一個目標點，機器人目標點的選取可以滿足對機器人最大速度的要求，轉(zhuǎn)S4;
[0161 ] S8:判斷機器人速度模塊，如圖11所示；
[0162]S8.1:判斷機器人當前時刻速度與閾值速度大小，小于閾值轉(zhuǎn)S9，大于閾值轉(zhuǎn)S8.2；
[0163]S8.2:機器人速度以某一閾值減小，轉(zhuǎn)S8.1 ；
[0164]S9:判斷機器人是否到達終點，否則轉(zhuǎn)Sl0.1，是則轉(zhuǎn)SlO;
[0165]SlO:機器人終點姿態(tài)判斷模塊，如圖12所示；
[0166]S10.1:判斷機器人當前姿態(tài)是否與終點姿態(tài)相同，否則轉(zhuǎn)S10.2，是則轉(zhuǎn)Sll;
[0167]S10.2:機器人以給定角速度原地旋轉(zhuǎn)一定角度，轉(zhuǎn)Sl0.1;
[0168]S11:輸出路徑跟蹤完畢信息，結(jié)束。
[0169]應當說明的是，上述實施例均可根據(jù)需要自由組合。以上所述僅是本發(fā)明的優(yōu)選實施方式，應當指出，對于本技術(shù)領域的普通技術(shù)人員來說，在不脫離本發(fā)明原理的前提下，還可以做出若干改進和潤飾，這些改進和潤飾也應視為本發(fā)明的保護范圍。
【主權(quán)項】
1.一種機器人的運動控制方法，其特征在于，包括如下步驟: S20根據(jù)輸入的地圖信息、規(guī)劃路徑及實時位置信息，判斷所述機器人當前所處位置是否在所述規(guī)劃路徑上，計算出機器人在所述規(guī)劃路徑上的目標位置信息； S30若所述機器人當前所處位置不在所述規(guī)劃路徑上，則根據(jù)所述實時位置信息和目標位置信息，使得所述機器人朝向所述目標位置;否則，直接跳轉(zhuǎn)至步驟S40; S40根據(jù)所述實時位置信息和目標位置信息，計算出所述機器人的線速度、角速度和實時朝向后，控制所述機器人運動。2.根據(jù)權(quán)利要求1所述的機器人的運動控制方法，其特征在于，所述步驟S30包括: S31根據(jù)所述機器人的實時位置信息、目標位置信息，計算出所述實時位置和目標位置所在的直線段與坐標系上橫軸所形成的目標夾角，所述坐標系設置在所述地圖上； S32獲取所述機器人的當前朝向與所述坐標系上橫軸所形成的當前夾角； S33判斷所述當前夾角與所述目標夾角是否相同； S34若所述當前夾角與所述目標夾角不同，則所述機器人以預設角速度旋轉(zhuǎn)后，跳轉(zhuǎn)至步驟S33;否則，所述機器人保持當前朝向。3.根據(jù)權(quán)利要求1所述的機器人的運動控制方法，其特征在于，還包括如下步驟: S41根據(jù)所述機器人的目標位置信息，計算出所述目標位置的相鄰位置信息； S42根據(jù)所述相鄰位置信息，計算出所述機器人所在路徑的當前曲率值； S43判斷所述當前曲率值與第一曲率閾值的大??； S44若所述當前曲率值大于所述第一曲率閾值，則進一步判斷所述機器人的當前速度與第一速度閾值的大小;否則，跳轉(zhuǎn)至步驟S46; S45若所述機器人的當前速度大于第一速度閾值，則所述機器人的當前速度以第一預設閾值進行減小后，跳轉(zhuǎn)至步驟S44;否則，跳轉(zhuǎn)至步驟S46; S46判斷所述機器人是否到達所述目標位置； S47若所述機器人到達所述目標位置，則進一步判斷所述機器人的當前朝向與所述目標位置的規(guī)劃朝向之間的差值是否滿足第一閾值;否則，跳轉(zhuǎn)至步驟S41; S48若所述機器人的當前朝向與所述目標位置的規(guī)劃朝向之間的差值不滿足第一閾值，則所述機器人以預設角速度旋轉(zhuǎn)后，跳轉(zhuǎn)至步驟S47。4.根據(jù)權(quán)利要求1所述的機器人的運動控制方法，其特征在于，還包括如下步驟: S50判斷所述目標位置是否為所述規(guī)劃路徑上的終點位置； S60若所述目標位置不是所述規(guī)劃路徑上的終點位置，則跳轉(zhuǎn)至步驟S20;否則，所述機器人的當前速度以第二預設閾值進行減小。5.根據(jù)權(quán)利要求4所述的機器人的運動控制方法，其特征在于，還包括如下步驟: S70判斷所述機器人是否到達所述終點位置； S80若所述機器人到達所述終點位置，則進一步判斷所述機器人的當前朝向與所述終點位置的規(guī)劃朝向是否相同；否則，跳轉(zhuǎn)至步驟S60; S90若所述機器人的當前朝向與所述終點位置的規(guī)劃朝向不同，所述機器人以預設角速度旋轉(zhuǎn)后，跳轉(zhuǎn)至步驟S80;否則，所述機器人保持當前朝向。6.根據(jù)權(quán)利要求1?5中任意一項所述的機器人的運動控制方法，其特征在于: 所述步驟S20中的地圖呈柵格狀，所述步驟S20中的目標位置信息包括:所述規(guī)劃路徑上的所有規(guī)劃點中距離所述機器人當前所處位置最近的規(guī)劃點位置信息，或所述規(guī)劃路徑上距離所述機器人當前所處位置最近的點位置信息。7.根據(jù)權(quán)利要求1?5中任意一項所述的機器人的運動控制方法，其特征在于: 所述步驟S40是指:根據(jù)所述實時位置信息和目標位置信息，在極坐標模型下，利用Lyapunov函數(shù)，利用Lyapunov直接法設計跟蹤控制器，計算出所述機器人的線速度、角速度和實時朝向。8.—種應用在如權(quán)利要求1?7中任意一項所述的機器人的運動控制方法的控制系統(tǒng)，其特征在于，包括: 判斷模塊，所述判斷模塊根據(jù)輸入的地圖信息、規(guī)劃路徑及實時位置信息，判斷機器人當前所處位置是否在所述規(guī)劃路徑上； 計算模塊，所述計算模塊與所述判斷模塊電連接，用于計算出所述機器人在所述規(guī)劃路徑上的目標位置信息； 朝向控制模塊，所述朝向控制模塊分別與所述判斷模塊、計算模塊電連接，所述朝向控制模塊接收所述判斷模塊輸出的所述機器人當前所處位置不在所述規(guī)劃路徑上，所述朝向控制模塊根據(jù)所述計算模塊輸出的所述實時位置信息和目標位置信息，使得所述機器人朝向所述目標位置； 運動控制模塊，所述運動控制模塊分別與所述判斷模塊、朝向控制模塊電連接，所述運動控制模塊根據(jù)所述實時位置信息和目標位置信息，計算出所述機器人的線速度、角速度和實時朝向后，控制所述機器人運動。9.根據(jù)權(quán)利要求8所述的機器人的運動控制系統(tǒng)，其特征在于，還包括: 所述計算模塊還根據(jù)所述機器人的實時位置信息、目標位置信息，計算出所述實時位置和目標位置所在的直線段與坐標系上橫軸所形成的目標夾角，所述坐標系設置在所述地圖上； 所述獲取模塊用于獲取所述機器人的當前朝向與所述坐標系上橫軸所形成的當前夾角； 所述判斷模塊還分別與所述計算模塊、獲取模塊電連接，所述判斷模塊接收所述計算模塊輸出的目標夾角，所述判斷模塊還接收所述獲取模塊輸出的當前夾角，判斷所述當前夾角與所述目標夾角是否相同； 所述朝向控制模塊與所述判斷模塊電連接，所述朝向控制模塊接收所述判斷模塊輸出的所述當前夾角與所述目標夾角不同，則所述機器人以預設角速度旋轉(zhuǎn);否則，所述機器人保持當前朝向。10.根據(jù)權(quán)利要求8所述的機器人的運動控制系統(tǒng)，其特征在于，還包括: 所述計算模塊還根據(jù)所述機器人的目標位置信息，計算出所述目標位置的相鄰位置信息; 所述計算模塊還根據(jù)所述相鄰位置信息，計算出所述機器人所在路徑的當前曲率值； 所述判斷模塊接收所述計算模塊輸出的當前曲率值，判斷所述當前曲率值與第一曲率閾值的大小； 若所述當前曲率值大于所述第一曲率閾值，所述判斷模塊進一步判斷所述機器人的當前速度與第一速度閾值的大??； 速度控制模塊，所述速度控制模塊與所述判斷模塊電連接，所述速度控制模塊接收所述判斷模塊輸出的所述機器人的當前速度大于第一速度閾值，則所述機器人的當前速度以第一預設閾值進行減??； 所述判斷模塊還判斷所述機器人是否到達所述目標位置； 若所述機器人到達所述目標位置，所述判斷模塊進一步判斷所述機器人的當前朝向與所述目標位置的規(guī)劃朝向之間的差值是否滿足第一閾值； 所述朝向控制模塊接收所述判斷模塊輸出的所述機器人的當前朝向與所述目標位置的規(guī)劃朝向之間的差值不滿足第一閾值，所述機器人以預設角速度旋轉(zhuǎn)。
【文檔編號】G05D1/02GK105867379SQ201610244271
【公開日】2016年8月17日
【申請日】2016年4月13日
【發(fā)明人】左嘉欣, 張小*, 章征貴
【申請人】上海物景智能科技有限公司