本發(fā)明涉及運(yùn)動(dòng)控制領(lǐng)域，特別是涉及一種運(yùn)動(dòng)控制方法及裝置、運(yùn)動(dòng)軌跡規(guī)劃方法。

背景技術(shù)：

在電機(jī)伺服系統(tǒng)中，一般把位置作為控制目標(biāo),利用當(dāng)前位置和目標(biāo)位置的差值為輸入量，采用比例-積分-導(dǎo)數(shù)(proportion-integral-derivative，PID)的控制方式，實(shí)現(xiàn)電壓輸出脈沖寬度調(diào)制(Pulse Width Modulation，PWM)的占空比控制。由于系統(tǒng)參數(shù)變化和外部干擾的存在,PID控制方法往往使系統(tǒng)產(chǎn)生較大的超調(diào)量甚至不穩(wěn)定。

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

本發(fā)明主要解決的技術(shù)問(wèn)題是提供一種運(yùn)動(dòng)控制方法及裝置、運(yùn)動(dòng)軌跡規(guī)劃方法，能夠解決現(xiàn)有技術(shù)中PID控制不穩(wěn)定的問(wèn)題。

為了解決上述技術(shù)問(wèn)題，本發(fā)明采用的一個(gè)技術(shù)方案是：提供一種運(yùn)動(dòng)控制方法，包括：獲取控制對(duì)象在當(dāng)前時(shí)刻的當(dāng)前位置及當(dāng)前速度；根據(jù)當(dāng)前位置及當(dāng)前速度計(jì)算當(dāng)前加速度，當(dāng)前加速度與當(dāng)前位置與目標(biāo)位置之差正相關(guān)，且與當(dāng)前速度負(fù)相關(guān)；利用當(dāng)前加速度控制驅(qū)動(dòng)控制對(duì)象的電機(jī)；重復(fù)執(zhí)行上述步驟直至控制對(duì)象到達(dá)目標(biāo)位置。

其中，根據(jù)當(dāng)前位置及當(dāng)前速度計(jì)算當(dāng)前加速度包括：當(dāng)前加速度a(t)＝K²(s₀-s(t))-2Kv(t)，其中a(·)為加速度，K為調(diào)節(jié)系數(shù)，s₀為目標(biāo)位置，s(·)為位置，v(·)為速度，t為當(dāng)前時(shí)刻。

其中，利用當(dāng)前加速度控制驅(qū)動(dòng)控制對(duì)象的電機(jī)包括：利用當(dāng)前加速度及當(dāng)前速度計(jì)算下一時(shí)刻速度v(t+1)＝v(t)+h*a(t)，其中t+1為下一時(shí)刻，h為迭代步長(zhǎng)，即下一時(shí)刻與當(dāng)前時(shí)刻之間的時(shí)間間隔；根據(jù)下一時(shí)刻速度控制電機(jī)。

其中，獲取控制對(duì)象在當(dāng)前時(shí)刻的當(dāng)前位置及當(dāng)前速度包括：利用位置傳感器獲取控制對(duì)象的當(dāng)前位置，至少利用當(dāng)前位置及前一時(shí)刻的位置進(jìn)行微分以獲取當(dāng)前速度。

為了解決上述技術(shù)問(wèn)題，本發(fā)明采用的另一個(gè)技術(shù)方案是：提供一種運(yùn)動(dòng)軌跡規(guī)劃方法，包括：獲取當(dāng)前時(shí)刻的當(dāng)前位置及當(dāng)前速度；根據(jù)當(dāng)前位置及當(dāng)前速度計(jì)算當(dāng)前加速度，其中當(dāng)前加速度與當(dāng)前位置與目標(biāo)位置之差正相關(guān)，且與當(dāng)前速度負(fù)相關(guān)；利用當(dāng)前位置及當(dāng)前速度計(jì)算下一時(shí)刻位置，利用當(dāng)前加速度及當(dāng)前速度計(jì)算下一時(shí)刻速度，下一時(shí)刻與當(dāng)前時(shí)刻的時(shí)間間隔為迭代步長(zhǎng)；重復(fù)執(zhí)行上述步驟直至到達(dá)目標(biāo)位置，以獲取運(yùn)動(dòng)軌跡。

其中，根據(jù)當(dāng)前位置及當(dāng)前速度計(jì)算當(dāng)前加速度包括：當(dāng)前加速度a(t)＝K²(s₀-s(t))-2Kv(t)，其中a(·)為加速度，K為調(diào)節(jié)系數(shù)，s(·)為位置，v(·)為速度，t為當(dāng)前時(shí)刻。

其中，利用當(dāng)前位置及當(dāng)前速度計(jì)算下一時(shí)刻位置包括：下一時(shí)刻位置s(t+1)＝s(t)+h*v(t)，其中h為迭代步長(zhǎng)；利用當(dāng)前加速度及當(dāng)前速度計(jì)算下一時(shí)刻速度包括：下一時(shí)刻速度v(t+1)＝v(t)+h*a(t)。

其中，運(yùn)動(dòng)軌跡包括位置軌跡和速度軌跡，其中速度軌跡連續(xù)不出現(xiàn)跳變，且在起始位置和目標(biāo)位置處速度均為0。

為了解決上述技術(shù)問(wèn)題，本發(fā)明采用的另一個(gè)技術(shù)方案是：提供一種運(yùn)動(dòng)控制裝置，包括：處理器、傳感器和驅(qū)動(dòng)器，處理器連接傳感器及驅(qū)動(dòng)器；處理器用于通過(guò)傳感器獲取控制對(duì)象在當(dāng)前時(shí)刻的當(dāng)前位置及當(dāng)前速度；根據(jù)當(dāng)前位置及當(dāng)前速度計(jì)算當(dāng)前加速度，當(dāng)前加速度與當(dāng)前位置與目標(biāo)位置之差正相關(guān)，且與當(dāng)前速度負(fù)相關(guān)；利用當(dāng)前加速度通過(guò)驅(qū)動(dòng)器控制驅(qū)動(dòng)控制對(duì)象的電機(jī)；重復(fù)執(zhí)行上述步驟直至控制對(duì)象到達(dá)目標(biāo)位置。

其中，處理器用于根據(jù)當(dāng)前位置及當(dāng)前速度計(jì)算當(dāng)前加速度a(t)＝K²(s₀-s(t))-2Kv(t)，其中a(·)為加速度，K為調(diào)節(jié)系數(shù)，s₀為目標(biāo)位置，s(·)為位置，v(·)為速度，t為當(dāng)前時(shí)刻；利用當(dāng)前加速度及當(dāng)前速度計(jì)算下一時(shí)刻速度v(t+1)＝v(t)+h*a(t)，其中t+1為下一時(shí)刻，h為迭代步長(zhǎng)，即下一時(shí)刻與當(dāng)前時(shí)刻之間的時(shí)間間隔；向驅(qū)動(dòng)器輸出下一時(shí)刻速度以控制電機(jī)。

本發(fā)明的有益效果是：利用當(dāng)前位置及當(dāng)前速度計(jì)算當(dāng)前加速度，并利用當(dāng)前加速度控制電機(jī)，可以實(shí)現(xiàn)電機(jī)的平穩(wěn)加速和平穩(wěn)減速，軌跡平滑，震動(dòng)較小，沒(méi)有超調(diào)，穩(wěn)定性更好。

附圖說(shuō)明

圖1是本發(fā)明運(yùn)動(dòng)控制方法第一實(shí)施例的流程圖；

圖2是本發(fā)明運(yùn)動(dòng)軌跡規(guī)劃方法第一實(shí)施例的流程圖；

圖3是本發(fā)明運(yùn)動(dòng)軌跡規(guī)劃方法第一實(shí)施例一個(gè)例子中位置軌跡的仿真圖；

圖4是本發(fā)明運(yùn)動(dòng)軌跡規(guī)劃方法第一實(shí)施例一個(gè)例子中速度軌跡的仿真圖；

圖5是本發(fā)明運(yùn)動(dòng)控制裝置第一實(shí)施例的結(jié)構(gòu)示意圖。

具體實(shí)施方式

如圖1所示，本發(fā)明運(yùn)動(dòng)控制方法第一實(shí)施例包括：

S11：獲取控制對(duì)象在當(dāng)前時(shí)刻的當(dāng)前位置及當(dāng)前速度。

一般利用傳感器獲取當(dāng)前位置及當(dāng)前速度。由于速度是位置對(duì)時(shí)間的微分，加速度是速度對(duì)時(shí)間的微分，可以使用位置傳感器、速度傳感器、加速度傳感器中的至少一種，結(jié)合必要的微分和/或積分運(yùn)算來(lái)獲取當(dāng)前位置及當(dāng)前速度。

例如，可以只利用位置傳感器直接獲取當(dāng)前位置，由于速度是位置對(duì)時(shí)間的微分，當(dāng)?shù)介L(zhǎng)，即兩次測(cè)量之間的時(shí)間間隔足夠小時(shí)，對(duì)速度公式進(jìn)行離散化，則當(dāng)前速度可以表示為當(dāng)前位置與前一時(shí)刻位置之差除以迭代步長(zhǎng)。也可以只利用加速度傳感器獲取加速度，對(duì)加速度進(jìn)行積分得到當(dāng)前速度，對(duì)速度進(jìn)行積分得到當(dāng)前位置。以此類推，可以得到其他情況。

S12：根據(jù)當(dāng)前位置及當(dāng)前速度計(jì)算當(dāng)前加速度。

當(dāng)前加速度與當(dāng)前位置與目標(biāo)位置之差正相關(guān)，且與當(dāng)前速度負(fù)相關(guān)，使得控制對(duì)象的運(yùn)動(dòng)速度連續(xù)不出現(xiàn)跳變，在整個(gè)運(yùn)動(dòng)過(guò)程的初期逐漸加速，后期逐漸減速，符合正常運(yùn)動(dòng)習(xí)慣。當(dāng)前位置與目標(biāo)位置之差可以是指兩者之間的直線距離，也可以是指兩者之間的運(yùn)動(dòng)軌跡長(zhǎng)度。

在本發(fā)明一個(gè)實(shí)施例中，當(dāng)前加速度表示為：

a(t)＝K²(s₀-s(t))-2Kv(t) (1)

其中a(·)為加速度，K為調(diào)節(jié)系數(shù)，s₀為目標(biāo)位置，s(·)為位置，v(·)為速度，t為當(dāng)前時(shí)刻，且控制對(duì)象在起始位置和目標(biāo)位置處速度均為0。調(diào)整K的大小可以改變加速度的大小，從而改變加減速的快慢進(jìn)而改變運(yùn)動(dòng)時(shí)間。

S13：利用當(dāng)前加速度控制驅(qū)動(dòng)控制對(duì)象的電機(jī)。

可以直接根據(jù)當(dāng)前加速度來(lái)控制電機(jī)，即將當(dāng)前加速度輸出至電機(jī)的驅(qū)動(dòng)器。

也可以利用當(dāng)前加速度計(jì)算及當(dāng)前速度計(jì)算得到下一時(shí)刻速度：

v(t+1)＝v(t)+h*a(t) (2)

其中v(·)為速度，a(·)為加速度，t為當(dāng)前時(shí)刻，t+1為下一時(shí)刻，h為迭代步長(zhǎng)，將下一時(shí)刻速度輸出至電機(jī)的驅(qū)動(dòng)器以控制電機(jī)。電機(jī)的驅(qū)動(dòng)器可以根據(jù)輸入量來(lái)調(diào)節(jié)電機(jī)的電壓大小、頻率等參數(shù)，從而調(diào)節(jié)電機(jī)的轉(zhuǎn)速。調(diào)節(jié)h的大小也可以改變速度變化的劇烈程度，h越大，速度變化越劇烈。

S14：判斷控制對(duì)象是否到達(dá)目標(biāo)位置。

一般將控制對(duì)象的當(dāng)前位置與目前位置進(jìn)行比較，若兩者完全相同或者誤差小于預(yù)設(shè)閾值，則判定控制對(duì)象到達(dá)目標(biāo)位置。

若是，則結(jié)束流程；若否，則跳轉(zhuǎn)至步驟S11以重復(fù)執(zhí)行上述步驟。

通過(guò)上述實(shí)施例的實(shí)施，利用當(dāng)前位置及當(dāng)前速度計(jì)算當(dāng)前加速度，并利用當(dāng)前加速度控制電機(jī)，可以實(shí)現(xiàn)電機(jī)的平穩(wěn)加速和平穩(wěn)減速，軌跡平滑，震動(dòng)較小，沒(méi)有超調(diào)，穩(wěn)定性更好。

如圖2所示，本發(fā)明運(yùn)動(dòng)軌跡規(guī)劃方法第一實(shí)施例包括：

S21：獲取當(dāng)前時(shí)刻的當(dāng)前位置及當(dāng)前速度。

S22：根據(jù)當(dāng)前位置及當(dāng)前速度計(jì)算當(dāng)前加速度。

當(dāng)前加速度與當(dāng)前位置與目標(biāo)位置之差正相關(guān)，且與當(dāng)前速度負(fù)相關(guān)，使得控制對(duì)象的運(yùn)動(dòng)速度連續(xù)不出現(xiàn)跳變，在整個(gè)運(yùn)動(dòng)過(guò)程的初期逐漸加速，后期逐漸減速，符合正常運(yùn)動(dòng)習(xí)慣。當(dāng)前位置與目標(biāo)位置之差可以是指兩者之間的直線距離，也可以是指兩者之間的運(yùn)動(dòng)軌跡長(zhǎng)度。

在本實(shí)施例中，當(dāng)前加速度表示為：

a(t)＝K²(s₀-s(t))-2Kv(t) (1)

其中a(·)為加速度，K為調(diào)節(jié)系數(shù)，s₀為目標(biāo)位置，s(·)為位置，v(·)為速度，t為當(dāng)前時(shí)刻，且控制對(duì)象在起始位置和目標(biāo)位置處速度均為0。調(diào)整K的大小可以改變加速度的大小，從而改變加減速的快慢進(jìn)而改變運(yùn)動(dòng)時(shí)間。在其他實(shí)施例中，加速度也可以用其他公式表示。

S23：利用當(dāng)前位置及當(dāng)前速度計(jì)算下一時(shí)刻位置，利用當(dāng)前加速度及當(dāng)前速度計(jì)算下一時(shí)刻速度。

速度為位置對(duì)時(shí)間的微分，進(jìn)行離散化之后代入當(dāng)前位置和當(dāng)前速度當(dāng)前加速度可得下一時(shí)刻位置：

s(t+1)＝s(t)+h*v(t) (3)

其中s(·)為位置，t為當(dāng)前時(shí)刻，t+1為下一時(shí)刻，h為迭代步長(zhǎng)，即下一時(shí)刻與當(dāng)前時(shí)刻的時(shí)間間隔。

加速度為速度對(duì)時(shí)間的微分，進(jìn)行離散化之后代入當(dāng)前速度和當(dāng)前加速度可得下一時(shí)刻速度：

v(t+1)＝v(t)+h*a(t)＝v(t)+h*(K²(s₀-s(t))-2Kv(t)) (4)

其中v(·)為速度，a(·)為加速度，t為當(dāng)前時(shí)刻，t+1為下一時(shí)刻，h為迭代步長(zhǎng)，即下一時(shí)刻與當(dāng)前時(shí)刻的時(shí)間間隔。

本步驟計(jì)算得到的下一時(shí)刻速度和下一時(shí)刻位置可以作為下一循環(huán)中步驟S21獲取的速度和位置。

S24：判斷是否到達(dá)目標(biāo)位置。

若是，則獲取運(yùn)動(dòng)軌跡，結(jié)束流程；若否，則跳轉(zhuǎn)至步驟S21以重復(fù)執(zhí)行上述步驟。運(yùn)動(dòng)軌跡包括位置軌跡和速度軌跡，其中速度軌跡連續(xù)不出現(xiàn)跳變，且在起始位置和目標(biāo)位置處速度均為0。

舉例說(shuō)明，取初始位置為0，目標(biāo)位置為5米，初始速度為0，在2.5kHz頻率控制系統(tǒng)下,取迭代步長(zhǎng)＝0.0004秒,調(diào)節(jié)系數(shù)K取2，通過(guò)仿真得到位置軌跡如圖3所示，速度軌跡如圖4所示。從圖中可以看出，位置軌跡和速度軌跡均比較平滑，效率高,耗能少。

如圖5所示，本發(fā)明運(yùn)動(dòng)控制裝置第一實(shí)施例包括：處理器110、傳感器120和驅(qū)動(dòng)器130，處理器110分別連接傳感器120及驅(qū)動(dòng)器130。

處理器110控制運(yùn)動(dòng)控制裝置的操作，處理器110還可以稱為CPU(Central Processing Unit，中央處理單元)。處理器110可能是一種集成電路芯片，具有信號(hào)的處理能力。處理器110還可以是通用處理器、數(shù)字信號(hào)處理器(DSP)、專用集成電路(ASIC)、現(xiàn)成可編程門(mén)陣列(FPGA)或者其他可編程邏輯器件、分立門(mén)或者晶體管邏輯器件、分立硬件組件。通用處理器可以是微處理器或者該處理器也可以是任何常規(guī)的處理器等。

傳感器120用于檢測(cè)控制對(duì)象和/或驅(qū)動(dòng)控制對(duì)象的電機(jī)的參數(shù)。

驅(qū)動(dòng)器130用于控制電機(jī)，可以根據(jù)輸入量來(lái)調(diào)節(jié)電機(jī)的電壓大小、頻率等參數(shù)，從而調(diào)節(jié)電機(jī)的轉(zhuǎn)速。

運(yùn)動(dòng)控制裝置可以進(jìn)一步包括存儲(chǔ)器(圖中未畫(huà)出)，存儲(chǔ)器用于存儲(chǔ)處理器110工作所必需的指令及數(shù)據(jù)。

在某些實(shí)施例中，處理器110和驅(qū)動(dòng)器130可以集成在一起。

處理器110用于通過(guò)傳感器120獲取控制對(duì)象在當(dāng)前時(shí)刻的當(dāng)前位置及當(dāng)前速度；根據(jù)當(dāng)前位置及當(dāng)前速度計(jì)算當(dāng)前加速度，當(dāng)前加速度與當(dāng)前位置與目標(biāo)位置之差正相關(guān)，且與當(dāng)前速度負(fù)相關(guān)；利用當(dāng)前加速度通過(guò)驅(qū)動(dòng)器130控制驅(qū)動(dòng)控制對(duì)象的電機(jī)；重復(fù)執(zhí)行上述步驟直至控制對(duì)象到達(dá)目標(biāo)位置。

可選的，處理器用于根據(jù)當(dāng)前位置及當(dāng)前速度計(jì)算當(dāng)前加速度a(t)＝K²(s₀-s(t))-2Kv(t)，其中a(·)為加速度，K為調(diào)節(jié)系數(shù)，s₀為目標(biāo)位置，s(·)為位置，v(·)為速度，t為當(dāng)前時(shí)刻；利用當(dāng)前加速度及當(dāng)前速度計(jì)算下一時(shí)刻速度v(t+1)＝v(t)+h*a(t)，其中t+1為下一時(shí)刻，h為迭代步長(zhǎng)，即下一時(shí)刻與當(dāng)前時(shí)刻之間的時(shí)間間隔；向驅(qū)動(dòng)器輸出下一時(shí)刻速度以控制電機(jī)。調(diào)節(jié)系數(shù)和目標(biāo)位置可以是存儲(chǔ)在存儲(chǔ)器中的，也可以是通過(guò)輸入模塊(未畫(huà)出)，例如鍵盤(pán)、觸摸屏，麥克風(fēng)等獲取的，當(dāng)然也可以是通過(guò)通信電路從其他裝置獲取的。

本發(fā)明運(yùn)動(dòng)控制裝置各實(shí)施例中各部分的具體功能可參考本發(fā)明運(yùn)動(dòng)控制方法實(shí)施例中的對(duì)應(yīng)描述，在此不再重復(fù)。

在本發(fā)明所提供的幾個(gè)實(shí)施例中，應(yīng)該理解到，所揭露的運(yùn)動(dòng)控制裝置和方法，可以通過(guò)其它的方式實(shí)現(xiàn)。例如，以上所描述的運(yùn)動(dòng)控制裝置實(shí)施方式僅僅是示意性的，例如，所述模塊或單元的劃分，僅僅為一種邏輯功能劃分，實(shí)際實(shí)現(xiàn)時(shí)可以有另外的劃分方式，例如多個(gè)單元或組件可以結(jié)合或者可以集成到另一個(gè)系統(tǒng)，或一些特征可以忽略，或不執(zhí)行。另一點(diǎn)，所顯示或討論的相互之間的耦合或直接耦合或通信連接可以是通過(guò)一些接口，裝置或單元的間接耦合或通信連接，可以是電性，機(jī)械或其它的形式。

所述作為分離部件說(shuō)明的單元可以是或者也可以不是物理上分開(kāi)的，作為單元顯示的部件可以是或者也可以不是物理單元，即可以位于一個(gè)地方，或者也可以分布到多個(gè)網(wǎng)絡(luò)單元上。可以根據(jù)實(shí)際的需要選擇其中的部分或者全部單元來(lái)實(shí)現(xiàn)本實(shí)施方式方案的目的。

另外，在本發(fā)明各個(gè)實(shí)施例中的各功能單元可以集成在一個(gè)處理單元中，也可以是各個(gè)單元單獨(dú)物理存在，也可以兩個(gè)或兩個(gè)以上單元集成在一個(gè)單元中。上述集成的單元既可以采用硬件的形式實(shí)現(xiàn)，也可以采用軟件功能單元的形式實(shí)現(xiàn)。

所述集成的單元如果以軟件功能單元的形式實(shí)現(xiàn)并作為獨(dú)立的產(chǎn)品銷(xiāo)售或使用時(shí)，可以存儲(chǔ)在一個(gè)計(jì)算機(jī)可讀取存儲(chǔ)介質(zhì)中?；谶@樣的理解，本發(fā)明的技術(shù)方案本質(zhì)上或者說(shuō)對(duì)現(xiàn)有技術(shù)做出貢獻(xiàn)的部分或者該技術(shù)方案的全部或部分可以以軟件產(chǎn)品的形式體現(xiàn)出來(lái)，該計(jì)算機(jī)軟件產(chǎn)品存儲(chǔ)在一個(gè)存儲(chǔ)介質(zhì)中，包括若干指令用以使得一臺(tái)計(jì)算機(jī)設(shè)備(可以是個(gè)人計(jì)算機(jī)，服務(wù)器，或者網(wǎng)絡(luò)設(shè)備等)或處理器(processor)執(zhí)行本發(fā)明各個(gè)實(shí)施方式所述方法的全部或部分步驟。而前述的存儲(chǔ)介質(zhì)包括：U盤(pán)、移動(dòng)硬盤(pán)、只讀存儲(chǔ)器(ROM，Read-Only Memory)、隨機(jī)存取存儲(chǔ)器(RAM，Random Access Memory)、磁碟或者光盤(pán)等各種可以存儲(chǔ)程序代碼的介質(zhì)。

以上所述僅為本發(fā)明的實(shí)施方式，并非因此限制本發(fā)明的專利范圍，凡是利用本發(fā)明說(shuō)明書(shū)及附圖內(nèi)容所作的等效結(jié)構(gòu)或等效流程變換，或直接或間接運(yùn)用在其他相關(guān)的技術(shù)領(lǐng)域，均同理包括在本發(fā)明的專利保護(hù)范圍內(nèi)。