[發明專利]一種裝備三維模型機構運動通用控制方法

有效

• 申請號: 201610452387.3
• 申請日: 2016-06-22
• 公開（公告）號: CN106647796B
• 公開（公告）日: 2019-06-14
• 發明（設計）人: 魏茂洲;葛超;陳國利;李博;米中賀;魏五洲;王豐雙
• 申請（專利權）人: 中國人民解放軍63863部隊
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 長春科宇專利代理有限責任公司 22001
• 代理人: 馬守忠

1.一種裝備三維模型運動通用控制方法，其特征在于，步驟和條件如下：

【1】．硬件：包括計算機，計算機要具備如下技術條件：a.具備網絡通信接口，能接收驅動數據；b.顯示設備包括顯示器或投影儀；c.輸入設備包括鍵盤和鼠標；計算機中存儲和運行有管理和控制程序；

【2】．裝備三維模型：裝備三維模型由多個獨立的部件組成，根據裝備的結構和工作原理，部件采用樹狀結構組織，第1級部件為根部件，根部件上可以連接多個部件作為第2級部件，第2級的每一個部件也可以連接多個部件作為第3級部件，依次類推構成樹狀組織結構；

裝備三維模型運動是指裝備三維模型中一個部件或多個部件的運動組合，部件運動方式有旋轉、平移、分離三種基本類型；

【3】．坐標系：

[1]特定三維空間坐標系：

部件的運動用特定三維空間坐標系的平移參數和旋轉參數表示，假設部件i所在特定三維空間坐標系用(OXYZ) m表示，部件i在該坐標系中的位置姿態可以用參數Pm(Xi，Yi，Zi，Φi，Θi，Ψi)表示，其中(Xi，Yi，Zi)表示部件i在三個坐標軸方向的平移分量，(Φi，Θi，Ψi)表示部件i在三個坐標軸方向上的旋轉分量，根據參數Pm可以計算部件在特定三維空間坐標系(OXYZ) m的空間變換矩陣M，計算公式如下：

[2]絕對坐標系與相對坐標系：

在控制程序中，部件的位置姿態用固定在大地上某一點的三維空間坐標系(OXYZ) a簡稱絕對坐標系表示，絕對坐標系中的位置姿態數用Pa(Xi，Yi，Zi，Φi，Θi，Ψi)表示，部件的運動用參數Pa對應的空間變換矩陣M_a控制，為了便于計算和描述，部件i用與固定在部件i所連接的上一級部件n上的三維空間坐標系(OXYZ) ri（簡稱相對坐標系）表示，部件i在相對坐標系中的位置姿態參數為Pri(Xi，Yi，Zi，Φi，Θi，Ψi)；

從相對坐標系 (OXYZ) ri到絕對坐標系(OXYZ) a的轉換方法如下：假設根部件1在絕對坐標系(OXYZ) a中變換矩陣為M₁，第2級部件在部件1固連三維空間坐標系中的空間變換矩陣為M₂，部件n的空間變換矩陣為M_n，部件i的空間變換矩陣為Mi，則部件i在絕對坐標系中的變換矩陣為：，使用空間變換矩陣Mai即可實現部件i在絕對坐標系(OXYZ) a中的運動控制；

【4】．運動控制參數配置： 控制程序的運動控制參數配置由下列步驟順序完成：

步驟100：開始，開始配置流程；

步驟105：根據裝備的工作原理，列出所有部件在運動過程中需要執行的動作，分析每一個動作在相對坐標系(OXYZ) ri中的運動初始位置姿態參數Pri(Xi0，Yi0，Zi0，Φi0，Θi0，Ψi0)，運動終止位置姿態參數Pri(Xin，Yin，Zin，Φin，Θin，Ψin)和動作執行時間Ti；

步驟110：將步驟105中確定的部件運動參數作為輸入提交給控制程序；

步驟115：根據控制步長t計算并保存每一部件的位置姿態偏移量

ΔXi=(Xin-Xi0)t/Ti，ΔYi=(Yin-Yi0)t/Ti，ΔZi=(Zin-Zi0)t/Ti，

ΔΦi=(Φin-Φi0)t/Ti，ΔΘi=(Θin-Θi0)t/Ti，ΔΨi=(Ψin-Ψi0)t/Ti；

步驟120：為每一部件指定控制指令，并輸入運動控制程序，控制指令是外部的驅動數據，用戶鼠標或鍵盤輸入；

步驟125：結束，運動控制參數配置流程結束；

【5】．運動控制過程：控制程序的運動控制過程由下列步驟順序完成：

步驟200：開始；

步驟205：在計算機中設置定時器步長t；

步驟210：啟動定時器；

步驟215：讀取第1個部件狀態；

步驟220：判斷部件的控制指令是否已經輸入，否，進行步驟235，是，進行步驟225；

步驟225：讀取部件當前運動參數Pri(Xij，Yij，Zij，Φij，Θij，Ψij)，計算部件的新運動姿態參數Pri2(Xij+ΔXi，Yij+ΔYi，Zij+ΔZi，Φij+ΔΦi，Θij+ΘXi，Ψij+ΔΨi)；

步驟230：根據計算的部件的新運動參數Pri2，計算絕對坐標系下的空間變換矩陣Mai，根據Mai控制部件更新到新的控制位置姿態；

步驟235：判斷當前處理的部件是否為最后一個部件，否，進行步驟240，是，進行步驟245；

步驟240：讀取下一個部件狀態，跳轉到步驟220，循環完成所有部件的處理；

步驟245：判斷運動執行時間是否小于部件運動時間Ti，如果小于Ti，定時器繼續工作，跳轉到步驟250等待下一個定時周期，否則，停止定時器，進行步驟255；

步驟250：判斷定時步長t是否完成，是，跳轉到步驟215，開始下一個定時周期，否，跳轉到本步驟，循環判斷，直到定時步長完成；

步驟255：保存所有部件的運動參數；

步驟260：結束，運動控制過程結束。