方法可應(yīng)用于處理裝置，參照?qǐng)D8,該方法可以包括：
[0095] 步驟S300、獲取關(guān)節(jié)級(jí)聯(lián)系統(tǒng)重心沿X軸方向上的加速度心和重心沿y軸方向 上的加速度^:，:并確定關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的零力矩點(diǎn)（xzmp，y zmp，〇);
[0096] 可設(shè)關(guān)節(jié)級(jí)聯(lián)系統(tǒng)重心的表達(dá)式為（xg，yg，z g)，假定關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的接觸面（地 面）是平坦的，則關(guān)節(jié)級(jí)聯(lián)系統(tǒng)重心到地面的高度zg是常量，因此可在計(jì)算關(guān)節(jié)級(jí)聯(lián)系統(tǒng) 重心時(shí)不考慮zg;
[0097] 關(guān)節(jié)級(jí)聯(lián)系統(tǒng)重心沿X軸方向上的加速度&，和重心沿y軸方向上的加速度·^可 以通過運(yùn)動(dòng)姿態(tài)傳感器獲取；
[0098] 關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的零力矩點(diǎn)（ZMP)是關(guān)節(jié)級(jí)聯(lián)系統(tǒng)動(dòng)態(tài)穩(wěn)定運(yùn)動(dòng)的重要指標(biāo)，ZMP 落在腳掌的范圍里面，則關(guān)節(jié)級(jí)聯(lián)系統(tǒng)（如機(jī)器人）可以穩(wěn)定的行走；如圖9所示，腳在接 觸地面時(shí)會(huì)有反作用力（N)也會(huì)產(chǎn)生力矩（M)，而反作用力、慣性力的凈力矩的和為零的點(diǎn) 則稱為零點(diǎn)力矩（如圖9的P點(diǎn)所示）。
[0099] 步驟S310、根據(jù)公式
計(jì)算xg，根據(jù)公式 計(jì)算 yg，其中，關(guān)節(jié)級(jí)聯(lián)系統(tǒng)重心的表達(dá)式為（xg，yg，zg)，z gS常量。
[0100] 需要說明的是，關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的ZMP的計(jì)算公式為：
[0102] 通過ZMP的計(jì)算公式可改寫出公式：進(jìn) 而通過改寫的公式可計(jì)算出xg和y g。
[0103] 針對(duì)檢測關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的位移的情況，基于圖7所示關(guān)節(jié)級(jí)聯(lián)系統(tǒng)，圖10示出了 本發(fā)明實(shí)施例提供的關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的運(yùn)動(dòng)參數(shù)檢測方法的又一流程圖，該方法可應(yīng)用于處 理裝置，參照?qǐng)D10,該方法可以包括：
[0104] 步驟S400、檢測各接觸點(diǎn)的總旋轉(zhuǎn)角度速率，總旋轉(zhuǎn)角度速率=sqrt (X旋轉(zhuǎn)角度 速率*X旋轉(zhuǎn)角度速率+Y旋轉(zhuǎn)角度速率*Y旋轉(zhuǎn)角度速率+Z旋轉(zhuǎn)角度速率*Z旋轉(zhuǎn)角度速 率）；
[0105] 接觸點(diǎn)為關(guān)節(jié)級(jí)聯(lián)系統(tǒng)與地面接觸的點(diǎn)，如人體的腳。
[0106] 步驟S410、根據(jù)接觸點(diǎn)的總旋轉(zhuǎn)角度速率確定落地點(diǎn)，其中，總旋轉(zhuǎn)角度速率從大 于閾值到小于閾值變化時(shí)，接觸點(diǎn)的運(yùn)動(dòng)正處在落地點(diǎn)，總旋轉(zhuǎn)角度速率從小于閾值到大 于閾值變化時(shí)，接觸點(diǎn)的運(yùn)動(dòng)則正處在離地點(diǎn)；
[0107] 以接觸點(diǎn)為腳為例，落地點(diǎn)可以為人體邁步時(shí)的落腳點(diǎn)，離地點(diǎn)可以為人體邁步 時(shí)的起腳點(diǎn)；
[0108] 圖11示出了人體步行的過程示意圖，參照?qǐng)D11，人在步行過程當(dāng)中總是有一個(gè)腳 與地接觸，以此為支點(diǎn)，向前移動(dòng)另一只腳的移動(dòng)過程當(dāng)中靠的是兩條腿各個(gè)關(guān)節(jié)的轉(zhuǎn)動(dòng) 產(chǎn)生肢端的位移來推動(dòng)人體前行；而在此過程當(dāng)中與地接觸的點(diǎn)在步行的過程當(dāng)中相對(duì)地 面靜止，其相對(duì)地面的位置沒有變化，于是可以以接觸點(diǎn)為標(biāo)準(zhǔn)，通過連桿旋轉(zhuǎn)向量的原理 推算人在邁步過程當(dāng)中產(chǎn)生的相對(duì)于地面的位移；
[0109] 運(yùn)動(dòng)方式解析過程中，需要知道系統(tǒng)在全局坐標(biāo)系中移動(dòng)的位移，本發(fā)明實(shí)施例 進(jìn)行運(yùn)動(dòng)追蹤采用運(yùn)動(dòng)姿態(tài)傳感器測量各個(gè)連桿的姿態(tài)數(shù)據(jù)，即通過安裝在連桿部位的運(yùn) 動(dòng)姿態(tài)傳感器結(jié)合預(yù)測步態(tài)運(yùn)動(dòng)規(guī)律利用上述提到的向量旋轉(zhuǎn)原理確定關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的 運(yùn)動(dòng)軌跡；
[0110] 由以上推算原理可以看出，為了推算人體步行的位移，正確判斷落地點(diǎn)十分重要； 本發(fā)明實(shí)施例采用的是"腳部角速度閾值法"進(jìn)行落腳點(diǎn)判斷；主要是連續(xù)測量計(jì)算人體 腳端運(yùn)動(dòng)的總的轉(zhuǎn)動(dòng)角速度，不斷計(jì)算判斷所述腳部運(yùn)動(dòng)的角速度值并與預(yù)設(shè)閥值進(jìn)行比 較；
[0111] 總旋轉(zhuǎn)角度速率=sqrt (X旋轉(zhuǎn)角度速率*X旋轉(zhuǎn)角度速率+Y旋轉(zhuǎn)角度速率*旋 轉(zhuǎn)角度速率+Z旋轉(zhuǎn)角度速率*旋轉(zhuǎn)角度速率）；
[0112] 具體的，判斷每一時(shí)刻腳部的狀態(tài)；當(dāng)總的角速度值（總旋轉(zhuǎn)角度速率）從大于閾 值到小于閾值變化時(shí)，即腳的運(yùn)動(dòng)正處在落腳點(diǎn)（對(duì)應(yīng)接觸點(diǎn)的運(yùn)動(dòng)正處在落地點(diǎn)）；當(dāng)角 速度值從小于閾值到大于閾值變化時(shí)，腳的運(yùn)動(dòng)則正處在起腳點(diǎn)（對(duì)應(yīng)接觸點(diǎn)的運(yùn)動(dòng)則正 處在離地點(diǎn)）；尤其需要關(guān)注腳的落腳點(diǎn)，即腳的狀態(tài)從"晃動(dòng)"到"靜止"轉(zhuǎn)換的位置，如圖 12所示。
[0113] 可選的，通過壓力分布傳感器可以獲知支撐點(diǎn)的壓力值方向與趨勢，可以作為關(guān) 節(jié)級(jí)聯(lián)系統(tǒng)的運(yùn)動(dòng)姿態(tài)的有益補(bǔ)充；
[0114] 如圖13所示，通過判斷支撐點(diǎn)各個(gè)部分的壓力分布，可以得出如圖13所示的二維 壓力分布圖形，通過判斷壓力點(diǎn)的運(yùn)動(dòng)走向，即：從足跟到足尖部分的壓力走向，可以輕易 確定關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的運(yùn)動(dòng)趨勢。
[0115] 步驟S420、根據(jù)連桿之間的層次關(guān)系，通過落地點(diǎn)的連桿位置推算，落地點(diǎn)與另一 接觸點(diǎn)之間關(guān)聯(lián)的各連桿的位置；
[0116] 步驟S430、根據(jù)落地點(diǎn)與另一接觸點(diǎn)之間關(guān)聯(lián)的各連桿的位置，計(jì)算接觸點(diǎn)當(dāng)前 落地所對(duì)應(yīng)的位移。
[0117] 步驟S420主要以已知一段連桿一端的位置，由旋轉(zhuǎn)矩陣推出另一端位置的方法 為基本原理（即圖4所示計(jì)算連桿的運(yùn)動(dòng)位置的方式）；如圖14所示，以一個(gè)關(guān)節(jié)聯(lián)系的 下肢兩條腿四段連桿A，B，C，D為例說明關(guān)節(jié)級(jí)聯(lián)系統(tǒng)位移軌跡的推算過程，通過安裝在腳 上連桿的運(yùn)動(dòng)姿態(tài)傳感器可判斷P5正處在落地的位置，在落地時(shí)刻進(jìn)行位移推算過程，計(jì) 算人行走的位移：
[0118] 具體的，判斷連桿A的下端為接觸點(diǎn)靜止之后，接觸點(diǎn)坐標(biāo)P1在兩段連桿轉(zhuǎn)動(dòng)的 過程當(dāng)中不會(huì)發(fā)生變化，確定其為坐標(biāo)零點(diǎn)；把連桿看成豎直向上的向量，向量的長度為連 桿的長度，通過連桿A的姿態(tài)構(gòu)成旋轉(zhuǎn)矩陣，推算連桿A上端的坐標(biāo)位置P2 ;由于連桿A和 連桿B通過關(guān)節(jié)互相連接，于是連桿A的上端和連桿B的下端具有相同的3維坐標(biāo)P2對(duì)于 連桿B ;
[0119] 用相同的方法，把連桿B看作是起點(diǎn)在P2,豎直向上的向量，并與通過連桿B的姿 態(tài)構(gòu)成旋轉(zhuǎn)矩陣相乘，即使用旋轉(zhuǎn)矩陣進(jìn)行旋轉(zhuǎn)，此時(shí)連桿B的上端三維坐標(biāo)位置為P3 ;以 此類推，分別通過A，B，C，D連桿的固連關(guān)系以及C和D的連桿姿態(tài)數(shù)據(jù)推算出P3, P4, P5 點(diǎn)的坐標(biāo)；
[0120] 于是獲得了落地點(diǎn)pi以及已知點(diǎn)p5之間位置的相互關(guān)系，也就獲得了人兩腳之 間的位移關(guān)系，這樣就可以通過上一步的位置P5反推得到新的落步點(diǎn)pi的位置，就可以得 到了步行一步的位移。
[0121] 值得注意的是，圖10所示方法在步行過程中位移的連續(xù)推算過程為：
[0122] 人的步行過程是以左腳右腳分別行走一步作為一個(gè)基本循環(huán)：即左腳向前邁出 落地，右腳向前邁出落地作為一個(gè)循環(huán)交替前行，如圖15所示；在步行的一個(gè)循環(huán)的過程 當(dāng)中，選取左腳落地時(shí)刻，右腳準(zhǔn)備抬起的時(shí)刻開始計(jì)算，由于上一次右腳落地的位置pl 是已知的，假設(shè)P5點(diǎn)為坐標(biāo)原點(diǎn)開始沿著連桿層次關(guān)系尋找每段連桿的"父連桿"以及各 個(gè)連桿的姿態(tài)按照上面提到向量旋轉(zhuǎn)的方法算出胯部P3的位置；然后再通過尋找"子連 桿"的順序推得Pl點(diǎn)的位置，此時(shí)Pl點(diǎn)的位置為相對(duì)于與P5時(shí)的位置，而不是在全局坐標(biāo) 系中的全局位置；
[0123] 即P5相對(duì)P1的位移=_P1相對(duì)P5的位移；由于上一步的位置P1已知，于是根據(jù) 此相對(duì)關(guān)系就可以計(jì)算左腳新邁出一步的位移：Deltas = P1+(P5相對(duì)P1)-(P1相對(duì)P5);
[0124] 相同的原理，在右腳落地時(shí)反推出P9和P5之間的位置關(guān)系；這樣就可以由上一步 已知的P5的位置推出P9的位置.這樣在雙腳交替進(jìn)行步行的過程當(dāng)中就可以推出位移。
[0125] 本發(fā)明實(shí)施例提供的關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的運(yùn)動(dòng)參數(shù)檢測方法，可實(shí)現(xiàn)關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的 連桿運(yùn)動(dòng)位置、重心、位移等運(yùn)動(dòng)參數(shù)的檢測，且無對(duì)于使用要求的局限，可被普遍應(yīng)用。
[0126] 下面對(duì)本發(fā)明實(shí)施例提供的關(guān)節(jié)級(jí)聯(lián)系統(tǒng)進(jìn)行介紹，下文描述的關(guān)節(jié)級(jí)聯(lián)系統(tǒng)可 與上文描述的關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的運(yùn)動(dòng)參數(shù)檢測方法相互對(duì)應(yīng)參照。
[0127] 本發(fā)明實(shí)施例提供的關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的結(jié)構(gòu)可如圖3所示，包括：多個(gè)連桿1，設(shè)置 于各連桿上的運(yùn)動(dòng)姿態(tài)傳感器2,連接各關(guān)聯(lián)連桿的電機(jī)3,和處理裝置4 ;處理裝置4與運(yùn) 動(dòng)姿態(tài)傳感器2通過有線或無線方式連接，以實(shí)現(xiàn)數(shù)據(jù)通信；
[0128] 其中，處理裝置4主要實(shí)現(xiàn)關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的運(yùn)動(dòng)參數(shù)檢測，處理裝置4可用于，獲 取運(yùn)動(dòng)姿態(tài)傳感器所感應(yīng)的運(yùn)動(dòng)姿態(tài)數(shù)據(jù)；根據(jù)所述運(yùn)動(dòng)姿態(tài)數(shù)據(jù)分析關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的運(yùn) 動(dòng)參數(shù)。
[0129] 可選的，運(yùn)動(dòng)參數(shù)可以包括：關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的連桿的運(yùn)動(dòng)位置；處理裝置4可先對(duì) 運(yùn)動(dòng)姿態(tài)傳感器進(jìn)行初始姿態(tài)校準(zhǔn)，使得連桿處于初始姿態(tài)的初始連桿向量與運(yùn)動(dòng)姿態(tài)傳 感器的測量坐標(biāo)系對(duì)齊；
[0130] 具體的，在進(jìn)行初始姿態(tài)校準(zhǔn)時(shí)，可將連桿的向量V(輸出）_向量V(補(bǔ)償），得到 連桿處于初始姿態(tài)的初始連桿向量；
[0131] 在檢測關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的連桿的運(yùn)動(dòng)位置的方面，處理裝置4具體 可用于，獲取連桿運(yùn)動(dòng)時(shí)，連桿運(yùn)動(dòng)的連桿姿態(tài)歐拉角，所述連桿姿態(tài)歐拉 角包括：連桿運(yùn)動(dòng)的俯仰角Θ，連桿運(yùn)動(dòng)的橫滾角φ，和連桿運(yùn)動(dòng)的航向 角Φ ;根據(jù)所述連桿姿態(tài)歐拉角生成連桿運(yùn)動(dòng)所對(duì)應(yīng)的三維旋轉(zhuǎn)矩陣R，R為
將連桿處于初始姿 態(tài)的初始連桿向量乘以R，得到連桿運(yùn)動(dòng)所對(duì)應(yīng)的連桿向量，所述連桿向量表示連桿的運(yùn)動(dòng) 位置。
[0132] 可選的，本發(fā)明實(shí)施例提供的關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的結(jié)構(gòu)還可如圖7所示，還包括：設(shè)置 于關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的支撐點(diǎn)下方的壓力分布傳感器5 ;
[0133] 在圖7所示關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的結(jié)構(gòu)上，處理裝置可實(shí)現(xiàn)關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的重心檢測； 對(duì)應(yīng)的，運(yùn)動(dòng)參數(shù)可以包括：關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的重心；
[0134] 在檢測關(guān)節(jié)級(jí)聯(lián)系統(tǒng)的重心方面，處理裝置具體可用于，獲取關(guān)節(jié)級(jí)聯(lián)系統(tǒng)重心 、沿X軸方向±的加速度4，重心沿y軸方向上的加速度~確定關(guān)