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

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

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