本發(fā)明涉及計算機技術領域，尤其涉及一種多自由度機器人的逆運動學求解系統(tǒng)。

背景技術：

隨著傳感器、人工智能、物聯(lián)網(wǎng)以及大數(shù)據(jù)技術的發(fā)展，各種高級的機器人被用于工業(yè)生產(chǎn)，或者代替人類作一些重復性高或危險的工作。靈活性、可操作性是度量機器人能力的重要標準，多關節(jié)、高自由度使機器人的功能更強大，可以完成更多的任務。逆向運動學在求解高自由度機器人的控制參數(shù)中扮演著重要的角色，是機器人領域眾多學者的研究方向。然而，由于逆向運動學求解困難、計算速度慢等因素，嚴重限制了機器人處理問題的實時能力，此外，逆向運動學的計算消耗大量的能量，這對于電池電量有限的移動式機器人而言，續(xù)航是一個巨大的挑戰(zhàn)。

在現(xiàn)有技術中，機器人的逆運動學求解通常采用cpu和gpu來計算，然而對于高自由度機器人，cpu計算非常耗時，不能滿足機器人控制的實時性要求；同時gpu功耗大，且設備便攜度低，不能滿足移動式機器人的功耗要求。

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

本發(fā)明的目的在于克服上述現(xiàn)有技術的缺陷，提供一種并行度高，功耗低，高自由度的機器人逆向運動學加速器。

根據(jù)本發(fā)明的一個方面，提供了一種機器人的逆運動學求解系統(tǒng)，用于基于輸入的機器人的目標位姿值和自由度獲得與所述目標位姿值對應的關節(jié)角度值，該系統(tǒng)包括：

參數(shù)初始化模塊，用于獲得參數(shù)初始值，所述參數(shù)初始值包括關節(jié)角度初始值、位姿初始值以及所述位姿初始值相對于所述目標位姿值的誤差初始值；

逆運動學調(diào)度器，用于基于所述目標位姿值執(zhí)行判斷，以決定是將當前的關節(jié)角度值選擇作為與所述目標位姿值對應的關節(jié)角度值還是將當前的關節(jié)角度值發(fā)送給雅克比計算單元，其中，所述當前的關節(jié)角度值是來自于參數(shù)初始化模塊的關節(jié)角度初始值或來自于參數(shù)選擇器的關節(jié)角度值；

雅克比計算單元，用于基于接收的所述關節(jié)角度值獲得雅克比矩陣及雅克比轉(zhuǎn)置矩陣；

位姿更新單元，用于基于所述雅克比矩陣和雅克比轉(zhuǎn)置矩陣獲得多個新的關節(jié)角度值；

參數(shù)選擇器，用于從所述多個新的關節(jié)角度值中選擇一個并發(fā)送給逆運動學調(diào)度器。

在本發(fā)明的系統(tǒng)中，所述參數(shù)初始化模塊包括：用于生成關節(jié)角度初始值的隨機信號發(fā)生器；用于獲得位姿初始值的三角平方計算器和矩陣乘法器；用于獲得誤差初始值的加法器和三角平方計算器。

在本發(fā)明的系統(tǒng)中，所述逆運動學調(diào)度器通過將當前的關節(jié)角度值所對應的誤差與預定閾值進行比較來決定是將當前的關節(jié)角度值選擇作為與所述目標位姿值對應的關節(jié)角度值還是將當前的關節(jié)角度值發(fā)送給雅克比計算單元，其中，當前的關節(jié)角度值所對應的誤差是指當前的關節(jié)角度值所對應的位姿值與所述目標位姿值之間的誤差。

在本發(fā)明的系統(tǒng)中，所述雅克比計算單元包括用于獲得雅克比矩陣及雅克比轉(zhuǎn)置矩陣的三角平方計算器和矩陣乘法器。

在本發(fā)明的系統(tǒng)中，所述位姿更新單元包括：參數(shù)計算器，用于計算前向參數(shù)；變量更新模塊，用于計算關節(jié)角度變量的增量；寄存器，用于保存關節(jié)角度變量的增量；位姿計算模塊，用于根據(jù)關節(jié)角度變量的增量計算新的位姿值；誤差計算器，用于計算新的位姿值和所述目標位姿值的之間的誤差；控制器，用于控制變量更新模塊、寄存器、位姿計算模塊、誤差計算器的執(zhí)行。

在本發(fā)明的系統(tǒng)中，所述參數(shù)計算器采用矩陣乘法器來計算所述前向參數(shù)。

在本發(fā)明的系統(tǒng)中，所述矩陣乘法器是4*4矩陣乘法器。

在本發(fā)明的系統(tǒng)中，所述位姿計算模塊采用加法器和三角平方計算器來計算新的位姿。

在本發(fā)明的系統(tǒng)中，所述參數(shù)選擇器包括：用于比較多個關節(jié)角度值的比較器；用于選擇一個關節(jié)角度值的數(shù)據(jù)選擇器。

在本發(fā)明的系統(tǒng)中，所述位姿更新單元是至少兩個，用于執(zhí)行并行計算。

本發(fā)明以多關節(jié)、高自由度機器人為研究對象，解決機器人逆運動學的求解問題，以為機器人提供控制其運動的參數(shù)。

相對于現(xiàn)有技術，本發(fā)明的優(yōu)點是：以專用的硬件系統(tǒng)求解控制機器人運動的參數(shù)，降低了計算功耗；通過高度并行的系統(tǒng)結(jié)構，可有效地加速逆運動學求解，從而保證對機器人實時控制；可以計算任意自由度的逆運動學問題，不局限與單一自由度的機器人控制問題，通用性更強，適用范圍更廣。

附圖說明

以下附圖僅對本發(fā)明作示意性的說明和解釋，并不用于限定本發(fā)明的范圍，其中：

圖1示出了多自由度機器人的示意圖。

圖2示出了根據(jù)本發(fā)明一個實施例的基于雅克比轉(zhuǎn)置法的逆運動學求解的流程圖。

圖3示出了根據(jù)本發(fā)明一個實施例的多自由度的機器人的逆運動學求解系統(tǒng)的框圖。

圖4示出了根據(jù)本發(fā)明一個實施例的位姿更新單元的框圖。

圖5示出了根據(jù)本發(fā)明一個實施例的三角平方計算器和4*4矩陣乘法器的示意圖。

圖6示出了根據(jù)本發(fā)明一個實施例的雅克比矩陣計算示例。

具體實施方式

為了使本發(fā)明的目的、技術方案、設計方法及優(yōu)點更加清楚明了，以下結(jié)合附圖通過具體實施例對本發(fā)明進一步詳細說明。應當理解，此處所描述的具體實施例僅用以解釋本發(fā)明，并不用于限定本發(fā)明。

本發(fā)明的系統(tǒng)可應用于多自由度機器人的機械臂控制，如圖1所示的機器人的機械臂有8個關節(jié)(即8個自由度)，通過控制各個關節(jié)的轉(zhuǎn)動能夠控制機器人運動。針對機械臂的逆運動學求解問題，是指從機器人的抓手的目標位姿pl(xo,yo,zo)出發(fā)，求解出機器人與目標位姿對應的各關節(jié)的角度。這種關聯(lián)關系通常在對機械臂進行運動學分析的基礎上，建立機器人的抓手目標位姿和各關節(jié)的角度變化之間的關系方程，通過采用雅克比矩陣進行逆運動學求解。對于關系方程的建立和雅克比矩陣求解的詳細過程屬于現(xiàn)有技術，在此不再贅述。本文將重點描述采用asic硬件實現(xiàn)機器人逆運動學求解的系統(tǒng)。

本發(fā)明的目的在于當給定機器人的目標位姿pl(xo,yo,zo)時，根據(jù)本發(fā)明提供的系統(tǒng)獲得對應的各個關節(jié)的關節(jié)角度θ(θ1,θ2,...θ8)(以8自由度為例)，通過將獲得的關節(jié)角度輸入給機器人，可靈活的控制機器人的運動，如圖1中的控制系統(tǒng)即可采用本發(fā)明的系統(tǒng)。

為了便于理解本發(fā)明的硬件實現(xiàn)，圖2示出了本發(fā)明中基于雅可比轉(zhuǎn)置法的逆運動學求解過程。

步驟一、輸入目標位姿pl(xo,yo,zo)，其中，x0,y0,z0表示在三個方向的坐標值；

步驟二、隨機生成一組關節(jié)角度變量初始值θinit，并且令關節(jié)角度變量θ＝θinit，計算初始化位姿pinit，并且令位姿變量p＝pinit，計算初始化誤差δeinit(即初始化位姿和目標位姿之間的誤差)，并且令誤差變量error＝δeinit；

步驟三、將誤差變量error與預定閾值(threshold)比較，以判斷是否滿足精度要求，即error＜threshold；如果滿足，則輸出關節(jié)角度變量θ，否則，繼續(xù)執(zhí)行以下步驟；

步驟四、根據(jù)關節(jié)角度變量θ計算雅克比矩陣j及其雅克比轉(zhuǎn)置矩陣j^t；

步驟五、根據(jù)j、j^t、error并行計算出前向參數(shù)α1,α2,...,αk，并求解出對應的關節(jié)增量δθ；

步驟六、根據(jù)δθ、θ計算出新的位姿p1,p2,...,pk以及與目標位姿pl的誤差e1,e2,...,ek；

步驟七、從e1,e2,...,ek中選擇出最小的誤差以及其對應的關節(jié)角度變量θ，并且令誤差變量error＝該最小的誤差；

步驟八、判斷error是否滿足精度要求，即error＜threshold；如果滿足，輸出關節(jié)角度變量θ，否則返回步驟四，繼續(xù)執(zhí)行。

圖3示出了根據(jù)本發(fā)明一個實施例的多自由度逆運動學求解系統(tǒng)的框圖。該系統(tǒng)包括參數(shù)初始化模塊、逆運動學調(diào)度器、雅克比計算單元、位姿更新單元和參數(shù)選擇器。該系統(tǒng)的輸入是目標位姿pl(xo,yo,zo)和自由度n，其中，自由度n可以是任意值。輸出是與目標位姿對應的各關節(jié)角度值。

參數(shù)初始化模塊：用于隨機生成初始化參數(shù)，例如初始化關節(jié)角度變量θinit、計算初始化位姿pinit和δeinit等。參數(shù)初始化模塊可由隨機信號發(fā)生器實現(xiàn)。

逆運動學調(diào)度器：用于協(xié)調(diào)整個系統(tǒng)中運行任務的調(diào)度和控制，例如，判斷當前誤差和預定的誤差閾值的大小，控制迭代次數(shù)等。逆運動學調(diào)度器可由有限狀態(tài)機實現(xiàn)，包含比較器、數(shù)據(jù)選擇器等。

雅克比計算單元(jcu)：用于計算關節(jié)角度變量對應的雅克比矩陣及其轉(zhuǎn)置矩陣。雅克比計算單元可由三角平方計算器和4*4的矩陣乘法器實現(xiàn)。

位姿更新單元(puu)：用于計算機器人的機械臂的新的位姿、關節(jié)角度變量和以及對應的誤差，可采用多個位姿更新單元并行計算，例如，32個。位姿更新單元包含參數(shù)計算器和正向運動學計算器。

參數(shù)選擇器：用于從多個誤差中選擇最小的一個誤差并返回給逆運動學調(diào)度器。參數(shù)選擇器可由比較器或排序器實現(xiàn)。

此外，該系統(tǒng)還提供輸入輸出接口，各模塊中包含有存儲器或寄存器，以保存中間的計算結(jié)果或保存從其他模塊接收的參數(shù)，模塊之間的參數(shù)通過總線傳輸。

具體而言，本發(fā)明的系統(tǒng)執(zhí)行的操作過程為：從系統(tǒng)外部接收目標位姿pl(xo,yo,zo)以及自由度n(在此實施例中n＝8)；參數(shù)初始化模塊隨機初始化關節(jié)角度，計算初始化位姿；雅克比計算單元計算關節(jié)角度變量對應的雅克比矩陣及其轉(zhuǎn)置矩陣；逆運動學調(diào)度器根據(jù)自由度n控制三角平方或雅克比矩陣計算的迭代次數(shù)；位姿更新單元并行計算出新的位姿、關節(jié)角度變量以及對應的誤差；同時逆運動學調(diào)度器根據(jù)誤差error是否小于閾值threshold來控制位姿更新單元的迭代次數(shù)；參數(shù)選擇器從多個誤差中選擇最小的一個誤差，返回給逆運動學調(diào)度器。在此過程中，當逆運動學調(diào)度器判斷為誤差error小于閾值threshold，則停止迭代并控制輸出關節(jié)角度θ(θ1,θ2,...θ8)。

為進一步理解本發(fā)明，圖4示出了圖3中的位姿更新單元的框圖。該位姿更新單元(puu)包括：參數(shù)計算器，可由乘法器和加法器實現(xiàn)；正向運動學計算器(fku)。該正向運動學計算器包含控制器、變量更新模塊、寄存器、位姿計算模塊、誤差計算器。

位姿更新單元接收來自于圖3中的雅克比計算單元的雅克比矩陣j以及轉(zhuǎn)置j^t、誤差error、當前位姿p和目標位姿pl，內(nèi)部并行生成不同的前向參數(shù)α1,α2,...,αk并并行計算新的位姿；輸出為對應的k組關節(jié)角度變量θ1,θ2,...,θk和誤差e1,e2,...,ek。參數(shù)計算器用于計算正向運動學計算器需要的前向參數(shù)α1,α2,...,αk；正向運動學計算器計算新的關節(jié)角度變量和位姿。其中，正向運動學計算器的變量更新模塊計算關節(jié)角度變量的增量δθ，并保存到寄存器中；位姿計算模塊根據(jù)增量δθ計算新的位姿，傳遞給誤差計算器；誤差計算器計算新的位姿與目標位姿的誤差；控制器用于控制變量更新模塊、寄存器、位姿計算模塊、誤差計算器的執(zhí)行順序。正向運動學計算器采用流水線操作，控制器控制變量更新模塊計算增量δθ后保存到寄存器中，同時控制位姿計算模塊計算新的位姿以及控制變量更新模塊繼續(xù)接收前向參數(shù)αk計算增量，控制器整個流水線起調(diào)度作用。

圖5示出了根據(jù)本發(fā)明的一個實施例的雅克比計算單元的框圖，其包括三角平方計算器和4*4矩陣乘法器。

圖6示出了利用圖5的雅克比計算單元計算雅克比矩陣的一個實例。第一步，根據(jù)關節(jié)角度θ1,θ2,...,θk,...,θn構造n個4*4的矩陣t[n]，使用三角平方計算器計算矩陣元素的三角函數(shù)，得到tr[n]；第二步，4*4矩陣乘法器接收矩陣tr[k]與tr[k-1]，進行矩陣相乘操作，得到trl[n]；第三步，trl[n]減去trl[k]，并通過矩陣乘法器與trl[k]相乘，得到最終的雅克比矩陣j[n]。計算過程并不代表一定同時執(zhí)行n個三角函數(shù)計算或者矩陣乘法，也可以使用流水線技術串行計算雅克比矩陣。

綜上所述，根據(jù)本發(fā)明的多自由度逆運動學求解系統(tǒng)的具體過程為：

步驟一，初始化模塊使用隨機信號發(fā)生器隨機生成一組關節(jié)角度變量初始值θinit，同時使用三角平方計算器以及4*4矩陣乘法器計算初始化位姿pinit，利用加法器和三角平方計算器計算初始化誤差error，將參數(shù)發(fā)給逆運動學調(diào)度器；

步驟二，逆運動學調(diào)度器接收參數(shù)error使用比較器判斷error是否滿足精度要求，即error＜threshold；如果滿足，數(shù)據(jù)選擇器選擇輸出電路以輸出關節(jié)角度變量θ，否則數(shù)據(jù)選擇器選擇計算電路將關節(jié)角度變量θ發(fā)給雅克比計算單元，繼續(xù)執(zhí)行。

步驟三，雅克比計算單元接收關節(jié)角度變量θ，使用三角平方計算器和4*4矩陣乘法器計算雅克比矩陣j及其雅克比轉(zhuǎn)置矩陣j^t，逆運動學調(diào)度器根據(jù)輸入的自由度控制雅克比矩陣計算的迭代次數(shù)，計算完成后，將獲得的雅克比矩陣j及其轉(zhuǎn)置矩陣j^t發(fā)送到各個并行的位姿更新單元puu；

步驟四，位姿更新單元根據(jù)j，j^t，error使用4*4矩陣乘法器計算參數(shù)α1,α2,...,αk并傳遞給內(nèi)部的正向運動學計算器fku；

步驟五，正向運動學計算器中的變量更新模塊接收error、j^t，使用4*4矩陣乘法器計算關節(jié)增量δθ并保存到寄存器中；

步驟六，位姿計算模塊接收寄存器中的δθ，使用加法器和三角平方計算器計算出新的位姿p；

步驟七，誤差計算器使用加法器和三角平方計算器計算新的位姿和目標位姿的誤差e1,e2,...,ek；整個位姿計算單元輸出新的k組關節(jié)角度變量θ1,θ2,...,θk和誤差e1,e2,...,ek；

步驟八，參數(shù)選擇器使用比較器從e1,e2,...,ek中選擇出最小的偏差error以及使用數(shù)據(jù)選擇器選擇對應的關節(jié)角度變量θ，并發(fā)送給逆運動學調(diào)度器；

步驟九，逆運動學調(diào)度器使用比較器判斷error是否滿足精度要求，即error＜threshold；如果滿足，數(shù)據(jù)選擇器選擇輸出電路輸出關節(jié)角度變量θ，否則，數(shù)據(jù)選擇器選擇計算電路將關節(jié)角度變量θ發(fā)給雅克比計算單元，重復步驟三，繼續(xù)執(zhí)行。

本領域的技術人員應理解的是，本文涉及比較器、判斷器、三角平方計算器等可以采用市售的標準器件，4*4矩陣乘法器也可以采用其他維度的矩陣乘法器。

以上已經(jīng)描述了本發(fā)明的各實施例，上述說明是示例性的，并非窮盡性的，并且也不限于所披露的各實施例。在不偏離所說明的各實施例的范圍和精神的情況下，對于本技術領域的普通技術人員來說許多修改和變更都是顯而易見的。本文中所用術語的選擇，旨在最好地解釋各實施例的原理、實際應用或?qū)κ袌鲋械募夹g改進，或者使本技術領域的其它普通技術人員能理解本文披露的各實施例。