專利名稱：逆運動學的制作方法
技術領域：
本發(fā)明涉及雅可比(Jacobian)矩陣的逆。這也稱為逆運動學問題(IK)。IK問題 與下述有關利用在用于游戲和動畫的計算機圖形中使用的任意數(shù)量的關節(jié)(joint)操縱 計算機產(chǎn)生的關節(jié)角色(articularted character)、關鍵幀(keyframing)、控制機器人、用 于控制控制力矩陀螺機制和許多其他應用的奇異回避方法。
背景技術：
在使用控制力矩陀螺(Control Moment Gyro, CMG)等的諸如機器人技術、計算機 動畫、航天器控制的應用中，作為例如(q)或時間的可變參數(shù)的函數(shù)并因此隨著時間改變 的可變矩陣將輸入空間連接到輸出空間。讓(χ)用來代表描述終端效果器(end effector)的位置和配置的笛卡兒坐標的 矢量，例如圖1中的14。對于三維笛卡兒空間，(χ)將具有一個或多個元素，以代表位置和 角度旋轉。終端效果器空間的大小(即，維數(shù))被表示為(m)。例如，如果終端效果器可僅 執(zhí)行三維位移，則(m = 3)。讓(q)用來描述關節(jié)坐標和它們的配置，例如，用于圖1中的關節(jié)11、12和13。每 個關節(jié)可具有一個或多個自由度。將(q)中的自由度的總數(shù)表示為(η)。例如，如果圖1中 的每個關節(jié)，即11、12和13，具有兩個自由度，則自由度的總數(shù)為(η = 6)。然后，可如下描述結構的模型、機器人或計算機動畫對象(object)χ = f (q)(la)在(等式la)中，f(q)是關節(jié)變量(q)的非線性函數(shù)。在許多應用中，需要對于希望終端效果器位移(X)的給定集合計算關節(jié)變量(q)。 這要求轉換(等式la)q = [f (x)]"-l(lb)在(等式lb)中，([f(X)]~-l)表示從(q)到(χ)的映射的反轉。解析(等式lb)是冗長的工作。用于求解(等式la)的數(shù)字方法涉及從兩側對 (等式la)求微分。這給出了運動學關系dx = J (q) dq(Ic)在(等式lc)中，(dq)定義輸出空間，(dx)定義輸入空間，而(J(q))是通過關于 (q)對(f(q))求微分計算出的參數(shù)依賴型雅可比矩陣。J(q) = diff(f(q))/diff(q)(Id)(等式Id)表示(f(q))關于(q)的微分。在關節(jié)圖的機器人技術和動畫中，(X)表示根據(jù)在笛卡爾空間中定義的角和線性 位移構造的矢量。(q)用于描述關節(jié)坐標和它們的配置。(dx)描述在笛卡爾空間中定義的 速度。(dq)描述關節(jié)速度。在使用控制力矩陀螺機制的姿勢控制中，如技術人員所知道的(χ)用于描述CMG 集群(cluster)的總角動量(angular momentum)。由萬向(gimbal)角來構成(q)，(dx)描述扭矩的分量。如技術人員所知道的，(dq)表示萬向速率。(χ)和(dx)的維度(即，維數(shù))為(m)。(q)和(dq)的維度是(η)。典型地，(m)小于或等于(η)。對于備用配置，(m)小于(η)。在本發(fā)明的一些實施例中，(m)和(η)可為任意大。圖1是例如具有3個關節(jié)11、12和13以及1個終端效果器14的機器人的結構的例子。關于終端效果器的目標軌跡被指定為如圖1中的15，并且在三維笛卡爾空間中被定 義。在圖1中，11、12和13是可按角度或線性地改變以提供自由度(η)并允許14追隨目 標15的關節(jié)機制。關節(jié)11、12和13的每個可具有一個或多個自由度，并且關節(jié)可具有不 同數(shù)目的自由度。圖2是在諸如計算機動畫的領域中使用的圖形結構。圖2中的結構的具體示例具 有 5 個終端效果器 211、214、215、213 和 212 和 10 個關節(jié) 21、22、23、24、25、26、27、28、29、 210。包括圖2中的結構僅為了說明目的，并且在本發(fā)明中描述的IK算法可工作于下述結構 上所述結構對于終端效果器具有(m)個自由度，且對于關節(jié)具有(η)個自由度，其中(m) 和(η)可為任意大。216、217、218、219和220是終端效果器必須追隨的、在笛卡爾空間中定 義的目標軌跡。在機器人技術中，電動機或其他機制可用于提供線性和或角動量，以產(chǎn)生位移并 改變(q)。在計算機圖形中，動畫軟件重新畫圖，以產(chǎn)生(q)中的位移。由于(等式Ic)中的雅可比依賴于(q)，所以傳感器被用于測量(q)或數(shù)學模型被 用于計算、導出或估計(q)。在本發(fā)明的該說明書中，(dxd)被用于表示對于(dx)的希望值的矢量。假設已知 (dxd)和(q)，并且通過數(shù)學模型或傳感器來提供(dxd)和(q)。對于希望軌跡(dxd)的給定集合，逆運動學問題(IK)是求解(等式Ic)的逆的問 題，這用于給定矢量(dxd)如何導出(dq)。dq = ij(q)dxd(2)在等式2中，(ij(q))表示(等式1)中的雅可比的逆。如果(m)嚴格地小于(η), 即，(m)不等于(η)，則偽逆方法被用于計算(ij(q))。在計算機圖形和機器人中，(dxd)表示終端效果器的希望速度。在使用CMG的姿勢控制中，(dxd)表示扭矩的希望分量。作為本發(fā)明的主題的逆運動學的問題，是如何選擇關節(jié)變量，即在動畫應用中如 何操縱電動機或如何重畫結構，以使得(dx)追隨希望的目標軌跡(dxd)。圖3表示逆運動學塊，即，處理系統(tǒng)，其確定產(chǎn)生目標軌跡(dxd)所需要的關節(jié)變 量(dq)。該塊的輸入是(dxd)并且從該塊的輸出是(dq)。逆運動學塊必須對給定的(dxd) 給出(dq)，使得在每次迭代時(dx)與(dxd)相同。當給定(dxd)并且(J(q))是時變參數(shù)依賴型雅可比矩陣時，對于(dq)求解(等 式2)是計算機動畫、機器人的控制、航天器的控制以及其他應用中的一個關鍵的元素或步
馬聚ο
實時求解(等式2)是冗長的并且計算密集的任務。本發(fā)明一些實施例的目的是得到用于求解(等式2)中定義的逆運動學問題的計 算有效方法。由于(q)中的較高數(shù)量的自由度(n)，所以實時求解(等式2)中的IK問題是數(shù)字 密集的任務。例如，增加用關節(jié)連接的對象的動畫中的細節(jié)和自由度導致動作的改進視覺表 現(xiàn)。但是，由于需要對大量(η)實時計算(等式2)中的IK問題，所以導致高密集計算。 因為由于(等式Ic)和(等式2)中的雅可比(J(q))依賴于參數(shù)(q)而是時變函 數(shù)(在特定的配置中被稱為奇異(singular)狀態(tài))，所以(J(q))變?yōu)橹忍?，并且結果，(等 式2)中的逆可導致對于給定軌跡(dxd)，(dq)任意大的值。這是與逆運動學問題的計算相 關聯(lián)的另一復雜因素。導出(ij(q))、即雅可比的逆的傳統(tǒng)IK方法是基于矩陣的操縱，其使得處理高度 計算密集，并且難以在并行處理器構架上運行。阻尼最小二乘法(DLS)(也稱為奇異魯棒(SR)算法)傳統(tǒng)上被用于求解(等 式 2)中 的 問 題(Y. Nakamura 禾口 H. Hanafusa, “Inverse kinematic solutionwith singularity robustness for robot manipulator control", Journal of Dynamicsystems, Measurements and Control, Vol. 108, Sep. 1986, C. W. Wampler and L. J. Leifer, "Application of damped least-squares methods to resolved-rate andresolved-acceleration control of manipulators", Journal of Dynamic Systems, Measurement, and Control,110(1998), pp. 31-98)。 在等式(3b)中，(Jt (q))是在(等式Id)中定義的雅可比的轉置。(I)是單位矩 陣，已知(k)為需要適應的阻尼因子，而Γ-1)代表逆運算符。當(k = 0)時，(等式3)變 化到偽逆方法。
在奇異狀態(tài)，雅可比變?yōu)橹忍潱⑶医Y果，(等式4a)中的逆不存在，并且其不能使 用(等式4a)的算術公式生成。此外，當(J(q))接近奇異狀態(tài)時，基于(等式4b)的解導致對于(dq)的過分大的 值。阻尼因子(k)因此被用作解的精確性和解的可行性之間的折中。當(k = 0)(等式3a) 時變化到(等式4a)；當配置遠離奇異時，(k)通常被設置為(k = 0)。接近奇異(k > 0)。 因此，由于需要適應(k)，所以(等式3a)和(等式3b)中的IK方法更復雜。適應阻尼因子 (k)要求額外的計算，并且因此需要額外的處理功率。W. A. Wolovich 禾口 H.Elliott 在"A computational technique forinversekinematics，，，， 中，Proceedings ofthe 23rd Conference on Decision and Control,1984 禾口 A. Balestrino, G. De Maria 禾口 L. Sciavicco 在“Robust control of roboticmanipulators”，9th IFAC World Congress，1984 中，已經(jīng)提到了包括僅使用用于 機器人的控制的雅可比的轉置的反饋環(huán)的算法。但是，與在本發(fā)明中提到的方法不同，這些 算法不能避免或躲開奇異狀態(tài)并且當系統(tǒng)處于奇異狀態(tài)時不能給出解。

發(fā)明內容
本發(fā)明提供一種用于控制系統(tǒng)的實時方法，所述系統(tǒng)包括多個控制部件，每個具 有至少一個可變參數(shù)(dq)；和受控元件，具有由控制部件控制的軌跡，其中軌跡通過可變 矩陣與可變參數(shù)相關聯(lián)，所述方法包括定義將可變參數(shù)dq與軌跡dx相關聯(lián)的控制轉移矩 陣(K)，并且使用反饋環(huán)，在反饋環(huán)中，計算依賴于作為希望軌跡(dxd)和當前軌跡(dx)之 間的差異的誤差(e)的反饋項。可通過多次循環(huán)重復計算反饋項，使得當前軌跡到達希望軌跡。矩陣K的輸出(dq)具有被指定為(η)的維度，希望軌跡dxd可具有維度(m)，并且 (m)可以小于或等于(η)。所述方法可包括選擇具有維度為(m)乘以(η)的控制轉移矩陣(K)，以及依賴于系 統(tǒng)的屬性確定(K)形式和數(shù)值。對于給定(dxd)生成(dq)的數(shù)值算法可以以濾波器的形式。該算法可以被布置 為僅需要乘法和累加類型的指令?？稍趩蝹€處理器或在并行平臺上執(zhí)行所述方法。即使當(J(q))變?yōu)橹忍潟r，矩陣⑷也可以被布置為給出(dq)的解。在奇異狀態(tài)下，誤差(e)在奇異方向上增長，并且(K)的全滿結構可以被布置為生 成(dq)的非零解，其產(chǎn)生促使軌跡遠離奇異狀態(tài)的動作。所述系統(tǒng)可以是被布置為顯示可移動對象的圖像的顯示器，控制部件包括可移動 對象的關節(jié)，并且受控對象包括可移動對象的元素。所述系統(tǒng)可為包括機器人和控制系統(tǒng)的機器人系統(tǒng)，受控元件是機器人的元件， 并且控制部件包括機器人的關節(jié)?？刂撇考砂刂屏赝勇菹到y(tǒng)的陀螺。本發(fā)明還提供一種用于可移動系統(tǒng)的控制系統(tǒng)，所述控制系統(tǒng)被布置為根據(jù)本發(fā) 明的方法而操作。本發(fā)明還提供一種包含機器人和根據(jù)本發(fā)明的控制系統(tǒng)的機器人系統(tǒng)。本發(fā)明還包括包含多個陀螺和根據(jù)本發(fā)明的控制系統(tǒng)的控制力矩陀螺系統(tǒng)。本發(fā)明的一些實施例的目的是導出用于求解(等式2)中定義的逆運動學問題的 計算有效的、實時的數(shù)字方法。在本發(fā)明的一些附加實施例中，奇異魯棒性意味著即使對于雅可比矩陣(J(q)) 為秩虧或奇異時的情形也存在解。此外，在本發(fā)明的一些實施例中的算法不需要計算阻尼因子。因此，本發(fā)明提供了一種用于數(shù)字計算可變矩陣的逆的實時方法，其中所述方法 使用反饋環(huán)，在所述反饋環(huán)中，可能具有被指定為(m)的任意維度的希望軌跡(dxd)與當前軌跡(dx)相比較，以生成誤差(e)?？稍谟嬎隳孢\動學問題的每個循環(huán)進行所述比較。所述誤差(e)可用作對于控制轉移矩陣(K)的輸入，其中所述控制轉移矩陣(K)生成所需要 的輸出(dq)。(dq)可具有被指定為(η)的任意維度，(m)小于或等于(η)。矩陣可隨時間改變。例如，其可為時間依賴型或依賴于隨時間改變的另一參數(shù)。所述方法可為計算機實現(xiàn)的方法。逆運動學問題是計算時間可變和參數(shù)依賴型矩陣的逆的問題。其具有例如在機器 人控制、利用任意數(shù)目的關節(jié)以及終端效果器控制和操作計算機生成的關節(jié)角色、關鍵幀 應用、使用控制力矩陀螺機制等其他事物控制航天器中的應用。所述方法可包括用于選擇反饋補償(K)的方法，所述反饋補償(K)具有維度(m) 乘以(η)；以及用于依賴于針對其正在解決逆運動學問題的屬性和結構，確定(K)的形式和 數(shù)字值的方法。所述方法可使用對于給定(dxd)生成(dq)的算法的數(shù)字實現(xiàn)方式，并且可作為只 需要乘法和累加類型的指令的濾波器來執(zhí)行，其中可在單個處理器或并行平臺上運行所述 指令。在一些實施例中，即使當(J(q))變?yōu)橹忍潟r⑷也給出(dq)的解。在奇異狀態(tài) 下，誤差(e)可在奇異方向增長，并且(K)的全滿結構可對于(dq)生成非零解，該非零解產(chǎn) 生軌跡從奇異狀態(tài)遠離的動作。結果，可在所提出的算法中實現(xiàn)奇異性的避免。本發(fā)明因此可對于解決逆運動學問題提供計算有效和奇異魯棒的方法。本發(fā)明還提供用于控制系統(tǒng)的運動的方法，該方法包括本發(fā)明的方法。系統(tǒng)可為 諸如機器人胳膊的接合系統(tǒng)、或者諸如航空器的另一個可移動系統(tǒng)。本發(fā)明還包括用于可移動系統(tǒng)的控制系統(tǒng)，該控制系統(tǒng)被布置為根據(jù)本發(fā)明的方 法操作。本發(fā)明還提供生成可移動對象的圖形圖像的方法，所述方法包括在以上任意段落 中定義的方法。本發(fā)明還提供一種顯示系統(tǒng)，包括處理部件，被布置為執(zhí)行在以上任意段落中定 義的方法，從而生成圖像數(shù)據(jù)；以及顯示部件，被布置為顯示圖像。例如，所述系統(tǒng)可包含游 戲機或其他計算機系統(tǒng)?，F(xiàn)在將僅以示例的方式通過參考附圖來描述本發(fā)明的優(yōu)選實施例。


圖1是例如具有3個關節(jié)和1個終端效果器(end effector)位置的機器人結構 的示例。圖2是在諸如計算機動畫的領域中使用的圖形結構。圖3代表了逆運動學塊。圖4代表了根據(jù)本發(fā)明的第一實施例的逆運動學解的反饋形式。圖5是先前在圖4中描繪的反饋逆運動學定律的擴展版本。圖6包括實現(xiàn)在本發(fā)明中描述的逆運動學問題的算法的C代碼的示例。圖7是根據(jù)本發(fā)明的實施例的機器人和控制系統(tǒng)的示意圖。圖8是根據(jù)本發(fā)明的實施例的圖形顯示系統(tǒng)的示意圖
具體實施例方式在本發(fā)明的一個實施例中1)存在乘法部件。2)存在加法部件。3)存在減法部件。4)存在諸如可存儲雅可比的具有維度(m)乘以(η)的存儲塊的雅可比存儲部件。5)存在提供(dxd)的部件。6)存在諸如可提供(q)的數(shù)學模型或傳感器的部件。7)存在諸如可存儲參數(shù)(z(t-l))的具有維度(m)的存儲塊的參數(shù)存儲部件。8)存在諸如可存儲參數(shù)(dq(t-l))的具有維度(m)的存儲塊的參數(shù)存儲部件。8)存在諸如可存儲參數(shù)(dx)的具有維度(m)的存儲塊的參數(shù)存儲部件。9)存在諸如可存儲參數(shù)(tmp)的具有維度(m)的存儲塊的參數(shù)存儲部件。10)存在諸如可存儲(A)的具有維度(m)的存儲塊的參數(shù)存儲部件；如果A是乘 以標量(scalar)的單位矩陣，則存儲塊需要維度(1)來僅存儲標量。11)存在諸如可存儲⑵的具有維度(m)的存儲塊的參數(shù)存儲部件。通常通過例如個人計算機或微處理器或微控制器或數(shù)字信號處理器的通用處理 器來提供以上部件，或者以上部件可為專用構建算法和存儲塊。本發(fā)明的該實施例的目的是提供一種用于求解(等式2)的計算有效并且奇異魯 棒的實時算法，其不需要計算矩陣的逆和阻尼因子。用于求解的等式(2)中的雅可比可表 現(xiàn)機器人的運動學、計算機動畫角色，或者可與航空器的控制相關聯(lián)或者可與其他應用相 關聯(lián)，其中需要被稱為雅可比矩陣的矩陣的逆，所述雅可比矩陣依賴于可變參數(shù)，例如(q)。首先，提出并如圖4中所示構造反饋環(huán)。在每次循環(huán)時計算該反饋環(huán)并且因此其 實時運行。所生成的(dx)45和所要求的(dxd)44矢量之間的誤差47被用作控制定律(K)41 的輸入。該控制定律生成所需要的(dq) 43，使得促使誤差47為難以察覺的小的值，46是加 法器，48表示正號而49表示負號。圖4中所示的反饋環(huán)提供了用于對于給定的雅可比(J(q))從(dxd)導出(dq)的 機制。圖4中的反饋環(huán)實質上替代圖3中的IK塊。使用圖中的反饋環(huán)，可導出以下關系dq = K[JK+I]"-ldxd(5)在(等式5)中，⑷是作為本發(fā)明的本實施例的一部分導出的控制轉移 (transfer)矩陣或者控制定律，(J)是從(等式Id)導出的雅可比矩陣而Γ-1)代表逆運 算符。在本發(fā)明的該實施例和其他實施例中，重要的元素是(K)的導出和(K)的形式的選 擇，以提供奇異避免屬性。(K)也需要被適應于解決J(q)的可變屬性。(K)是轉移函數(shù)的全轉移矩陣，具有非零非對角元素，如技術人員所知，以便于在 環(huán)中提供奇異避免屬性。(K)具有(η)乘以(m)的維度。(K)是(q)的函數(shù)，即，如技術人員所知，其被適應于解決雅可比的時變屬性，并且 其依賴于(q)。當雅可比變?yōu)橹忍潟r，誤差(e)將在 一個方向上增長，這是因為(J(q))將在該特 定的方向上傳遞0輸出。(K)的非零元素將隨后生成必要輸出(dq)，(dq)將導致遠離奇異方向的軌跡，結果是難以察覺的小誤差(e)和目標的良好軌跡。圖4中的反饋環(huán)中的誤差47描述希望變量(dxd)和根據(jù)(等式Ic)計算的實際 變量之間的差異。e = dxd-J (q) dq(6)通過適當?shù)剡x擇(K)，可使得(等式6)中的誤差任意小。假設(K)已知，可根據(jù) (等式5)計算(dq)。對于固定采樣間隔的實時實現(xiàn)方式如果在規(guī)律的時間間隔需要執(zhí)行參數(shù)依賴型矩陣的雅可比(在此稱為雅可比矩 陣)的逆(典型的而不是僅有的應用是機器人控制和飛行器控制)，則可使用以下算法，其 中如技術人員所知，所述規(guī)律的時間間隔也稱為采樣間隔。在本發(fā)明中的該實施例中，以以下離散形式給出(K)的形式K :z(t) = Az(t-l)+e (7a)dq = Jt(q)Pz(t_l)(7b)(等式7a)和(等式7b)—起表示圖4和(等式5)中的反饋環(huán)和塊⑷。(等式7a)和(等式7b)—起將(等式6)中的誤差(e)與(dq)相連。(等式7a)和(等式7b)在每個循環(huán)一起實時運行，在所述循環(huán)求解逆運動學問 題，以找到dq的值，從而獲得dxd的希望結果。如技術人員所知，兩個連續(xù)采樣之間的時間 差，或IK算法的兩個連續(xù)執(zhí)行之間的時間為采樣時間(dt)。在(等式7a)中，㈧是具有維度(m)乘以(m)的負定對角矩陣。在(等式7a)中，⑵是具有維度(m)乘以(m)的正定滿矩陣。在(等式7b)中，(Jt (q))是具有維度(η)乘以(m)的在(等式Id)中導出的雅 可比的轉置。在(等式7b)中，在每次采樣時使用(q)的當前值重新計算雅可比的轉置 (Jt(Q))0通過傳感器或數(shù)學模型來提供(q)。在(等式7a)和(等式7b)中，(z (t))是根據(jù)維度(m)的狀態(tài)變量的矢量。在(等式7a)中，(z(t-l))是根據(jù)維度(m)的狀態(tài)變量的矢量。在(等式7a)和(等式7b)中，(z(t_l))被延遲等于(z(t))的(dt)值的一個采 樣周期。為了計算延遲值，需要根據(jù)維度(m)的存儲元件。這里將等于(dt)的一個采樣的 延遲表示為D (dt)。
在(等式7a)和(等式7b)中，(z(t_l))表示在先前迭代時根據(jù)(等式7a)計算 的(z(t))的值。(等式7a)和(等式7b)—起表示對于給定誤差(e)允許計算(dq)的濾波器。(等式7a)和(等式7b)以及(等式6)—起是本發(fā)明的該實施例的結果，并且提 供用于求解逆運動學問題的完整的算法。該算法總結如下。e = dxd-J (q)dq(t-l)(9a)z(t) = Az(t-l)+e(9b)dq = Jt(q)Pz(t_l)(9c)(等式9a)、(等式9b)和(等式9c)一起對于給定的希望軌跡(dxd)，允許計算 (dq)。
在圖5中以圖形形式表現(xiàn)(等式9a)、(等式9b)和(等式9c)，圖5為圖4的擴展版本。圖5反應了實時實現(xiàn)方式并示范了算法的濾波屬性。D(dt)51和511是延遲塊并 且代表其中存儲了對于(z(t))513的值和(dq)53的值的存儲器。在(等式16)中定義了 (A)512。在(等式20)中定義了 510中的P。510中的(Jt(q))是使用當前采樣的(q)的值 計算的雅可比的轉置。通過傳感器或數(shù)學模型來提供(q)。根據(jù)(等式％)計算(z(t))513， (J(q)) 52是使用當前采樣的(q)的值計算的雅可比。通過傳感器或數(shù)學模型來提供(q)。 (e)是要求軌跡(dxd)54和實際軌跡(dx) 55之間的誤差。56和515代表加法器塊，58、516 和517代表正號，而59代表負號。(z(t-l)) 514是當在先前采樣或迭代中計算的(z(t))513 的值。(dq(t_l))518是當在先前的采樣或迭代中計算的(dq)53的值。(等式9a)、(等式9b)和(等式9c)一起代替圖3中的IK塊。(等式9a)、(等式9b)和(等式9c)一起表示代替(等式2)的新算法。(等式9a)、(等式9b)和(等式9c)一起表示用于求解逆運動學(IK)問題的算 法。在每個循環(huán)實時執(zhí)行(等式9a)、(等式9b)和(等式9c)。(等式9a)、(等式9b)和(等式9c)執(zhí)行與濾波器的操作類似的操作，并且因此 只需要累加(accumulate)和乘法指令。在圖5中以濾波器形式示出了計算。在(等式9a)中，(dq(t-l))是延遲了等于(dt)的一個采樣周期的(dq(t))的值。dq(t-l) = D(dt)dq(10)對于可變采樣間隔的實時實現(xiàn)方式作為逆運動學的另一類應用，實時但不以固定的采樣速率執(zhí)行雅可比矩陣或參數(shù) 依賴型矩陣的逆。例如，在關節(jié)角色動畫的計算機圖形和動作合成中，以預定速率(有時稱 為幀速率)定義端點(或如圖1中的14和圖2中的211、212、213、214、215的終端效果器) 的新位置。在一幀(或時間間隔)中，順序地若干次解決逆運動學任務，直到端點或終端效 果器達到所需要的位置。有時在一幀中的這些順序求解被稱為迭代。在一幀中，執(zhí)行逆運 動學的若干次迭代，直到終端效果器或端點到達所需要的級別。通過計算機程序以無規(guī)律 的間隔執(zhí)行每次迭代，其中無規(guī)律的間隔是不固定的，并且依賴于軟件的負載而改變。對于 這些應用，可使用以下修改的算法在每次迭代時計算逆運動學或dq的值dq = Jt(q)Pz(t_l)(9d)z(t) = z(t-l)+h(Az(t-l)-Jt(q)dq)+h dxd (9e)因此(等式9d)和(等式9e)表示根據(jù)本發(fā)明的另一實施例的K的格式和圖4中 的反饋環(huán)。在本發(fā)明的該實施例中，除了之前實施例的組件之外12)存在諸如可存儲h、具有維度(1)的存儲塊的參數(shù)存儲部件。13)存在諸如可存儲參數(shù)bdq、具有維度(η)的存儲塊的參數(shù)存儲部件。14)存在諸如可存儲dqO、具有維度(η)的存儲塊的參數(shù)存儲部件。15)存在諸如可存儲dq full、具有維度(η)的存儲塊的參數(shù)存儲部件。15)存在諸如可存儲V、具有維度(η)的存儲塊的參數(shù)存儲部件。等式8用于根據(jù)ζ (t)計算z(t-l)。h是需要對于應用而調整的常數(shù)；選擇h使得 該常數(shù)為正，并且小于或等于如技術人員所知的以下系統(tǒng)矩陣(A-J(q) Jt (q)P)所描述的設備(或系統(tǒng))的最小時間常數(shù)的50%。h依賴于A、P和當前雅可比值J(q)的選擇。也 可固定h的值，例如h = 2e-2,然后選擇A和P。在每次迭代可計算等式9d和9e，直到dq 非常小或端點(或終端效果器)達到如當前幀所需要的希望位置。等式9d和9e是對于 圖4中的反饋逆運動學定律的第一階積分求解。還可使用更高階的積分。在等式9d中，dq 可乘以增益(每個自由度的差)以調節(jié)增益中的差異，即，dq = VJt(q)PZ(t-l)，其中V = [vl,v2,... , vn]定義每個自由度η的增益。接下來給出⑵和(A)的導數(shù)。用于(等式9a_c)中㈧和⑵的導數(shù)的算法(等式7a)和(等式9b)中的A是具有維度(m)乘以(m)的負定對角矩陣。(A) 總共具有(m)個元素。
(11) (al)、(a2)、(a3)........(am)為定義下限頻率和(dx)和(dxd)之間的誤差
的正實標量。依賴于系統(tǒng)的動態(tài)表現(xiàn)、機器人或圖形結構或控制力矩陀螺操控定律，(al) 到(am)必須被適當?shù)卣{節(jié)。如果全部自由度(m)的表現(xiàn)被相等加權，則(al) = (a2)
=......=(am) = (a)可被設置為同一值(a)。對于具有快速萬向節(jié)慣性的CMG控制系
統(tǒng)，(a)可被設置為例如al = a2 =...... = am = a = exp (0. 05dt)(12a)(dt)是如上定義的采樣周期，而(exp)代表指數(shù)運算符。對于圖形結構，可以使用以下示例值al = a2 =...... = am = a = exp (5dt)(12b)(等式7b)和(等式9c)中的⑵是如技術人員所知的正定對稱矩陣。其可為滿 矩陣或對角矩陣。為了找到(P)，首先計算被表示為(Pa)的矩陣，如技術人員所知，該矩陣是 Riccati 等式的角軍(Doyle, J. , Glover, K. , Khargonekar, P.,禾口 Francis, B. , "State-space solutions to standard H2and H-infinity control problems，，IEEETransactions on Automatic Control, Vol.34，1989，pp.831-847)。Mt*Pa+Pa*M_Pa*(B2*(V)~-l*B2t-s~_2*B l*Blt)*Pa+Ct*C = O (13)對于(Pa)的導數(shù)，定義了 6個矩陣，除了正的非零標量(s)以外，是(Bi)、(B2)、 (M)、(V)和(C)。(Bi)是具有維度(m)乘以(m)的單位對角矩陣。(Bit)是(Bi)的轉置。(B2)是具有維度(m)乘以(η)的矩陣，并且等于(_1)乘以在一些非零配置(q)= (qO)時計算的雅可比(J(Q))0B2 = -J (qO)(14)(qO)具有維度(η)并且可通過對于變量(q)的非零值的任意組合而構成。(B2t) 是(B2)的轉置。重要的是，注意(J(q0))必須具有非零奇異值。
(C)具有維度(n+m+m)乘以(m)。(C)是如下正整數(shù)式(partition)
(15)根據(jù)等于0的(η)乘以(m)個元素構成第一組。具有維度(m)乘以(m)的第二組等于單位矩陣(I)乘以正標量(w)，所述單位矩 陣⑴具有(m)乘以(m)個元素，所述正標量(w)定義圖4中的循環(huán)的截止頻率，例如w = 4rad/s。(w)需要依賴于應用而被適當?shù)卣{節(jié)。根據(jù)等于0的(m)乘以(m)個元素構成第三組。(等式13)中的(M)是具有維度(m)乘以(m)的負定對角矩陣。M總共具有(m) 個元素。
(16)(ml)、(m2)、(m3)........(mm)為定義(dx)和(dxd)之間的誤差的較低頻率的
衰減的正實標量。依賴于系統(tǒng)的動態(tài)表現(xiàn)、機器人或圖形結構或控制力矩陀螺操控定律， (ml)到(mm)必須被適當?shù)卣{節(jié)。如果全部自由度(m)的表現(xiàn)被相等加權，則(ml) = (m2)
=......=(mm)可被設置為同一值。對于具有快速萬向節(jié)慣性的CMG控制系統(tǒng)，可被設
置為例如ml = m2 =...... = mm = 0. 05 (17a)對于圖形結構，示例值是ml = m2 =...... = mm = 5(17b)(Mt)是(M)的轉置。(等式13)中的(V)是具有維度(η)乘以(η)的正定對角矩陣。(V)總共具有(η) 個元素。
(11)(vl)、(v2)、(v3)........(vm)為對于特定系統(tǒng)可實現(xiàn)的每個自由度(q)定義最
大速率的平方(可能以rad/s或者m/s)的正實標量。依賴于系統(tǒng)的動態(tài)表現(xiàn)、機器人或圖 形結構或控制力矩陀螺操控定律，(vl)到(vn)必須被適當?shù)卣{節(jié)。如果通過具有類似特
征的機制構造全部自由度(n)，則(vl) = (v2) =...... = (vn)可被設置為同一值。對于
具有快速萬向節(jié)慣性的CMG控制系統(tǒng)，可被設置為例如vl = v2 =...... =vn=l. 8*1. 8 (19a)對于圖形結構，示例值為vl = v2 =...... =vn= 1*1 (19b)等式13中的(s)是標量。以下算法用于確定(s)。
步驟-1 首先將(S)設置為較大數(shù)，例如s = 100。步驟-2 對于(s)的該值，通過求解(等式13)確定(Pa)。步驟-3 如果解存在，并且(Pa)為正定矩陣，則減小(s)，例如(s) = (s)/2，并且從步驟_2繼續(xù)算法。步驟-4 如果對于(s)的新值(等式13)未產(chǎn)生正定解，則使用最后的正定解(Pa) 作為(等式13)中Riccati等式的解。一旦根據(jù)(等式13)計算了(Pa)，則通過將(Pa)與(等式19a)或者(等式19b) 中定義的一個自由度的最大速率相乘來計算(等式7b)和(等式9c)中的(P)，即P=(vl)*Pa(20)在包括計算機動畫的許多應用中，可將等式9d和等式9e中的P和A可以被選擇 為常數(shù)。這顯著減少了計算需求。為了計算P和A，以下算法可被用作上述算法的變形步驟-1 選擇A，典型地A為采用從0. 0001到-1000的值的負變量。對于95個自 由度的骨架的典型值為A = -20。步驟-2 取得在當前關節(jié)組合雅可比J(q)并計算JJnorm = sqrt (trace (J (q) Jt (q)))其中trace (J (q) Jt (q))是矩陣的跡(trace)或者沿著(J(q)Jt(q))的主對角線 的元素的和，而sqrt是平方根。步驟-3 選擇P使得P<= (pi/h-A)/JjnormA、h和P的適應根據(jù)步驟_3明顯的是，A和h是相互關連的。例如在典型的算 法中，一旦運動學結構被定義(或者在計算機動畫中的骨架幀被定義)，則步驟-1:A固定為例如A = -20步驟_l:h固定為例如h = 0. 02步驟_3:根據(jù)如下計算P:P <= (pi/h-A)/Jjnorm步驟-4 定義對于終端效果器的虛擬(dummy)軌跡并且利用以上狀態(tài)執(zhí)行。通過 監(jiān)視需要以由計算等式9d和等式9e的希望值處理的迭代數(shù)量，調節(jié)并增加P，以減少迭代 的數(shù)目。如果響應不穩(wěn)定(dp單調增長)，則減少h。步驟-5 —旦達到穩(wěn)定的響應，則調節(jié)A以改進迭代的數(shù)量。典型地增加A以減 少迭代的數(shù)量。僅對于給定的骨架執(zhí)行一次以上適應性算法。反饋逆運動學的實際實現(xiàn)方式(對于固定的采樣間隔)在(等式9a_9c)中定義的FIK算法作為濾波器工作，并且只需要乘法和累加指 令。圖5中示出算法的濾波器形式。以下描述反饋逆運動學算法的實際實現(xiàn)方式。步驟-0 (初始化)將(z(k-l))設置為依從維度(m)的零矢量
將(dq(t-l))設置為依從維度(η)的零矢量步驟-1 使用(q)的當前值計算雅可比(J(q))，通過來自傳感器(相機、電動機光 學編碼器、電動機轉速計或其他)的一些測量或者數(shù)學模型來提供(q)的當前值。雅可比 依賴于對于其求解逆運動學問題的系統(tǒng)的結構。典型地，根據(jù)標量和諸如sin (q)和C0S(q) 的依賴于(q)的三角函數(shù)來構造雅可比。雅可比具有維度(m)乘以(η)。計算雅可比包括 利用其數(shù)字形式代替(q)、求解(J(q))的每個元素并生成依從維度(m)乘以(η)的數(shù)字矩 陣。步驟-2 使用乘法部件、加法部件，并使用用于存儲(dx)的部件將結果存儲到 (dx)，計算dx = J(q)dq(t-1)這給出了具有維度(m)的矢量(dx)。步驟-3 使用減法部件，通過從(dxd)中減去來自步驟-2的結果來計算(等式9a) 中的誤差，即，e = dxd-dx使用存儲部件來將結果存儲到(e)。步驟-4 使用存儲部件⑵和存儲部件(z(t-l))來計算⑵乘以(z(t-l))；將結 果存儲在(tmp)中。tmp = Pz (t_l)該操作使用存儲部件、乘法部件和加法部件。使用用于存儲(tmp)的部件，來存儲 (tmp)，所述(tmp)是具有維度(m)的矢量。步驟-5 將雅可比的轉置(Jt(q))乘以如在步驟_4中計算的(tmp)來導出(等 式9c)中的(dq)dq = Jt (q) tmp該操作使用乘法部件、加法部件和用于存儲(dq)的部件。步驟_6 使用(z(t_l))、(A)和在步驟_3中計算的(e)，根據(jù)(等式9b)計算z(t)，z(t) = Az(t-l)+e該操作使用加法部件、乘法部件和用于存儲(z(t))的部件步驟-7:設置ζ (t-1) = ζ (t)dq(t-l) = dq該操作使用用于存儲(z(t-l))和(dq(t-l))的部件。這些值將被用于在(dt)秒 后將發(fā)生的IK算法的下一迭代。步驟-8 使用來自步驟_5的(dq)作為IK問題的解。在(dt)數(shù)量的秒后將發(fā)生 的下一迭代從步驟-1繼續(xù)。圖6中示出了使用實現(xiàn)對于一般逆運動學問題的以上算法C語音指令的計算機軟件程序。運算的總數(shù)等于2*m*m+2*m-l+n*(2m-l) (21)如果(m = 6)，則需要(83+ll*n)次運算來計算雅可比的逆并求解IK問題。如果(m = 12)并且(η = 25)則需要(886)次運算來計算雅可比的逆并求解IK問題。反饋逆運動學的實際實現(xiàn)方式(對于可變的采樣間隔)考慮使用可變采樣間隔的應用，可使用以下算法。步驟-0和步驟-1如上。步驟-3 使用雅可比的轉置(Jt (q))、P和ζ (t_l)，計算公式9d dq = Jt(q)Pz(t_l)步驟-4 :使用雅可比&)、六、2({-1)、11和叔(1，計算等式如z(t) = z(t-l)+h(Az(t-l)_J(q)dq)+h dxd實踐中，ζ (t)是必須被用于步驟_3中的接下來的迭代的ζ (t-Ι)的值。在本次迭代中，使用來自步驟_3的dp作為對于逆運動學問題的解。重復以上步 驟，直到dq減少到閾值以下的小值或/和端點或終端效果器到達目標?？偣蚕聛?，使用每次迭代2*m*n+4*m次乘法和2*m*n_n+2*m次加法，來計算對于逆 運動學問題的解。顯然地，計算的復雜度與m和η成線性。如 Y. Nakamura 禾口 H. Hanafusa, “ Inverse kinematic solution with singularityrobustness for robot manipulator controlJournal of Dynamic systems,Measurements and Control, Vol.108, Sep. 1986 已經(jīng)記載的，對于上述運算,將需 要最少6000個指令、包括用于將消耗額外的乘法和累加指令的除法指令，來對于相同的維 度(m = 12)和(η = 25)計算IK問題。因此，所提出的算法給出了通過以至少因子7對計 算負擔的減少。對于基于傳統(tǒng)IK方法的計算負擔的一些估算值也可在A. A. Maciejewski和 J. Μ. Reagin,"Parallel Algorithm andArchitecturefor the control of kinematically redundant manipulators，，,Roboticsand Automation,IEEE Transactions on,Volume 10, Issue 4，Aug 1994Page(s) :405_414中找到。再次，通過在本發(fā)明中提出的算法可實現(xiàn)與 該論文中呈現(xiàn)的數(shù)目相比減少最少因子10的計算負擔。僅為了示范目的包括(等式21)中列出的運算總數(shù)，并且假設雅可比(J(q))具有 非零元素的全滿結構。這可以通過使用更有效的實現(xiàn)方式而被進一步優(yōu)化，在所述更有效 的實現(xiàn)方式中，在該算法中適當?shù)亟鉀Q(J(q))的零元素。也可利用對(J(q0))的適當調節(jié)使得(P)對角。通過利用2個或多個處理塊，本實現(xiàn)方式還可通過采用并行、多處理方案改進。在 本發(fā)明中提出的算法適于并行結構上的實現(xiàn)方式，而不需要額外的開銷。這可使用若干方 法來完成。例如考慮圖6，在包括兩個處理塊的結構上，(e(t))和(z (t))的計算可與(tmp)和 (dp)的計算并行運行。例如考慮(J(q)dq(t-l))的計算，具有維度(m)乘以(η)的(J(q))可以分割為 其中，每行具有(η)個元素。各行可被并行乘以(dq(t-l))。因此，如果系統(tǒng)具有 (m)個可用處理器，則每個處理器可并行將(J(q))中的每行乘以(dq(t-l))，這導致比在單 個處理器上運行環(huán)快(m)次。同樣的結論對于以上算法中的步驟_4和步驟-5中的計算成立。通過對于乘法的 結果適當構造存儲器指針，這可以零增量開銷而實現(xiàn)。對于定義關節(jié)約束、關節(jié)限制、軌跡和環(huán)境約束以及用于動作重新定標 (retargeting)的空動作為了引入關節(jié)約束、關節(jié)限制、環(huán)境約束以及用于動作重新定標，額外的同類項被 添加到等式2中的主要解 dq = i J (q) dxd+ [I_i J (q) J (q) ] dqO通過以等式4中的反饋形式代替ij(q)，可通過以上提到的反饋形式來計算新的 同類項[I_iJ(q)J(q)]dqO?？紤]到計算機動畫和非固定采樣間隔，完整解可被修改為bdq = Jt(q)Pbz(t_l)(9f)bz (t) = bz (t-1) +h (Abz (t_l)-J (q) bdq) +hj (q) dqO (9 j)則最終的關節(jié)速度為dq_full = dq-bdq+dqO其中根據(jù)等式9D計算dq，根據(jù)等式9f計算bdq并且根據(jù)給定的約束來計算dqO， 例如為了限制關節(jié)動作，例如在動作重新定標中，對于給定的關節(jié)數(shù)目(k)dqO (k) = gain* (bq (k) -q (k))其中bq(k)是對于關節(jié)的希望值(其可為常數(shù)或時變函數(shù))，q(k)是關節(jié)變量(k) 的當前值，而gain是要選擇的正常數(shù)。參考圖7，本發(fā)明的一個實施例包括機器人，該機器人包括終端效果器714和3個 關節(jié)711、712、713。因此，該機器人對應于圖1中的機器人。每個關節(jié)具有3個自由度，并 且包括3個致動器，每個控制1個自由度的動作?？刂葡到y(tǒng)包括計算機720，該計算機720 包括被布置為提供所需的全部處理部件的處理器722、以及被布置為包括所需的全部存儲 部件的存儲器724。輸入(在此情況下是使用者輸入730的形式)被布置為生成到計算機 720的、指示終端效果器714的希望動作的輸入。傳感器740被布置為測量關節(jié)711、712、 713的位置和移動，并向指示其的計算機720提供信號。計算機被布置為使用上述方法來控 制與關節(jié)711、712、713相關的致動器。參考圖8，本發(fā)明的另一實施例包括包含處理器822和存儲器824的PC820。計算 機被連接到顯示器826和用戶輸入830。存儲器824具有存儲在其中的指令，以使得用戶能 夠在計算機上玩游戲，其中圖形圖像832在顯示器834上，以響應于來自用戶輸入830的輸入受控移動。計算機被布置為使用以上描述的一個方法、響 應于來自用戶輸 入830的輸入 信號控制圖像的移動，所述圖像可包括與圖2的圖對應的圖。
權利要求
一種用于控制系統(tǒng)的實時方法，所述系統(tǒng)包括多個控制部件，每個具有至少一個可變參數(shù)(dq)；和受控元件，具有由所述控制部件控制的軌跡，其中所述軌跡通過可變矩陣與所述可變參數(shù)相關聯(lián)，所述方法包括定義將所述可變參數(shù)(dq)與所述軌跡(dx)相關聯(lián)的控制轉移矩陣(K)，并且使用反饋環(huán)，在反饋環(huán)中，計算依賴于作為希望軌跡(dxd)和當前軌跡(dx)之間的差異的誤差(e)的反饋項。
2.如以上任意權利要求所述的方法，其中，所述矩陣是雅可比矩陣。
3.如權利要求1或2所述的方法，其中，通過多次循環(huán)重復計算所述反饋項，使得所述 當前軌跡到達所述希望軌跡。
4.如以上任意權利要求所述的方法，其中，所述矩陣K的輸出(dq)具有被指定為(η) 的維度，所述希望軌跡dxd具有維度(m)，并且(m)小于或等于(η)。
5.如權利要求4所述的方法，包括選擇具有維度(m)乘以(η)的所述控制轉移矩陣 (K)，以及依賴于所述系統(tǒng)的屬性確定(K)形式和數(shù)值。
6.如以上任意權利要求所述的方法，其中，對于給定的(dxd)生成(dq)的數(shù)值算法是 濾波器的形式。
7.如權利要求6所述的方法，其中，所述算法僅需要乘法和累加類型的指令。
8.如以上任意權利要求所述的方法，被布置為在單個處理器或在并行平臺上執(zhí)行所述方法。
9.如以上任意權利要求所述的方法，其中，(K)被布置為即使(J(q))變?yōu)橹忍潟r也給 出(dq)的解。
10.如以上任意權利要求所述的方法，其中，在奇異狀態(tài)下，所述誤差(e)在奇異方向 上增長，并且(K)的全滿結構生成(dq)的非零解，該非零解產(chǎn)生軌跡從奇異狀態(tài)遠離的動作。
11.如以上任意權利要求所述的方法，其使用下述形式的算法 e = dxd-J(q)dq(t_l)z(t) = Az (t-l)+e dq = Jt(q)Pz (t-1) 其中， J為矩陣， Jt為J的轉置，A為負定對角矩陣或者負常數(shù)，以及 P為正定全滿矩陣或正對角矩陣或正常數(shù)。
12.如權利要求1-9中任意一個所述的方法，其使用下述形式的算法 dq = Jt(q)Pz (t-1)ζ (t) =ζ (t-1) +h (Az (t-1)-Jt (q) dq) +dxd 其中， J為矩陣， Jt為J的轉置，A為負定對角矩陣或者負常數(shù)， P為正定全滿矩陣或正對角矩陣或正常數(shù)，以及h為正常數(shù)。
13.如以上任意權利要求所述的方法，其中，所述系統(tǒng)是被布置為顯示可移動對象的圖 像的顯示器，所述控制部件包括所述可移動對象的關節(jié)，并且所述受控對象包括所述可移 動對象的元素。
14.如權利要求1-12中任意一個所述的方法，其中，所述系統(tǒng)是包括機器人和控制系 統(tǒng)的機器人系統(tǒng)，所述受控元件是機器人的元件，并且所述控制部件包括機器人的關節(jié)。
15.如權利要求1-12中任意一個所述的方法，其中，所述控制部件包括控制力矩陀螺 系統(tǒng)的陀螺。
16.一種用于可移動系統(tǒng)的控制系統(tǒng)，所述控制系統(tǒng)被布置為根據(jù)以上任意權利要求 的方法而操作。
17.一種機器人系統(tǒng)，包括機器人和根據(jù)權利要求16的用于控制所述機器人的移動的 控制系統(tǒng)。
18.—種控制力矩陀螺系統(tǒng)，包括多個陀螺和根據(jù)權利要求16的控制系統(tǒng)。
19.一種顯示系統(tǒng)，包括處理部件，被布置為執(zhí)行權利要求1至11中任意一個所述方 法從而生成圖像數(shù)據(jù)、以及顯示部件，被布置為顯示圖像。
全文摘要
一種用于控制系統(tǒng)的實時方法，所述系統(tǒng)包括多個控制部件，每個具有至少一個可變參數(shù)(dq)；和受控元件，具有由所述控制部件控制的軌跡，其中所述軌跡通過可變矩陣與所述可變參數(shù)相關聯(lián)，所述方法包括定義將所述可變參數(shù)dq與所述軌跡dx相關聯(lián)的控制轉移矩陣(K)，并且使用反饋環(huán)，在反饋環(huán)中，計算依賴于作為可能具有被指定為任意維度(n)的希望軌跡(dxd)和當前軌跡(dx)之間的差異的誤差(e)的反饋項。
文檔編號G05D1/08GK101842755SQ200880113784
公開日2010年9月22日 申請日期2008年8月28日 優(yōu)先權日2007年8月28日
發(fā)明者亞歷山大·N·皮徹夫 申請人:薩里大學