本發(fā)明涉及一種冗余自由度機構位置更新方法。
背景技術：
天線伺服系統(tǒng)為研究對象，將其抽象為一個四自由度的運動平臺。提出了一種基于關節(jié)空間運動恒定速比假設的冗余自由度機械臂關節(jié)更新方法，將研究重點放在冗余自由度機械臂姿態(tài)隨動系統(tǒng)的關節(jié)角位置求解上，獲得了基于局部速度最優(yōu)的一步位置更新數(shù)值解。能夠快速計算出專利中所涉及的四個自由度的姿態(tài)伺服的運動學位置逆解。對于冗余自由度的運動機構而言，常用的優(yōu)化方法是基于關節(jié)空間的速度，可以得到關節(jié)局部的速度最優(yōu)解，但是對于位置層面的關注較少。本專利給出了一種具有廣泛適用性的冗余自由度機構的運動學逆解位置解法。
技術實現(xiàn)要素：
本發(fā)明的目的在于提供一種冗余自由度機構位置更新方法。
本發(fā)明的目的是這樣實現(xiàn)的：
(1)對載體現(xiàn)有的姿態(tài)進行測量，獲得載體此時的航向角ψ、俯仰角φ和橫搖角γ；
(2)根據(jù)衛(wèi)星的參數(shù)，和第(1)步的測量結果，利用下式：
計算出來在當前時刻衛(wèi)星在載體坐標系下的描述矩陣Ts；式(1)中的航向角ψ、俯仰角φ和橫搖角γ為第(1)步的測量值，Ee代表衛(wèi)星參數(shù)的俯仰角，Ae代表衛(wèi)星參數(shù)的方位角，σe代表衛(wèi)星參數(shù)的極化角，C代表cos，S代表sin，例如：CEe代表cosEe，SEe代表sinEe；此時的天線系統(tǒng)的四個關節(jié)角為θ1，θ2，θ3，θ4；關節(jié)角度與描述矩陣Ts之間的函數(shù)關系為：Ts＝F(θ1,θ2,θ3,θ4)；
(3)當前時刻的天線框架的姿態(tài)矩陣為Ta，利用第(2)步中的衛(wèi)星矩陣Ts與Ta求解出兩個坐標系之間的轉動微元，
其中δx為繞x軸的轉動微元，δy為繞y軸的轉動微元，δz為繞z軸的轉動微元；為矩陣Ta的逆矩陣；將其中的速度微元提取出來組成向量[δx δy δz]T；
(4)對當前的天線系統(tǒng)結構計算雅克比矩陣值，記為J；
(5)求取天線系統(tǒng)的雅克比矩陣J的Moore-Penrose逆(簡稱M-P逆)，將M-P逆記為J+；
(6)求取四個關節(jié)角的運動微元：
Θ＝[dθ1 dθ2 dθ3 dθ4]T＝J+[δx δy δz]T
其中，Θ為關節(jié)運動微元向量，dθ1為第一個關節(jié)角的運動微元，dθ2為第二個關節(jié)角的運動微元，dθ3為第三個關節(jié)角的運動微元，dθ4為第四個關節(jié)角的運動微元；
(7)計算各關節(jié)角之前運動微元的比例值，其中k2為dθ2與dθ1之間的比，其中k3為dθ3與dθ1之間的比，其中k4為dθ4與dθ1之間的比；
(8)進行位置更新時每個關節(jié)的運動量記為：Δθ1，Δθ2，Δθ3，Δθ4，利用再(7)中求得的k2，k3和k4以及Δθ1表示Δθ2，Δθ3，Δθ4有：
Δθ2＝k2Δθ1
Δθ3＝k3Δθ1
Δθ4＝k4Δθ1
(9)建立位置更新后的關節(jié)角度方程為：
Ta＝F(θ1+Δθ1,θ2+k2Δθ1,θ3+k3Δθ1,θ4+k4Δθ1)
其中規(guī)則F是建立在關節(jié)角度和末端姿態(tài)之間的約束方程；這個約束關系的是通過建立冗余自由度運動機構的D-H模型得到的；這個模型給定了機構四個運動角度與末端姿態(tài)描述矩陣之間的對應關系；
(10)利用牛頓迭代方法，求解出第(9)步中的方程，得到Δθ1，再利用式(4)求得Δθ2，Δθ3，Δθ4；
(11)得到關節(jié)的更新位置為θ1+Δθ1，θ2+Δθ2，θ3+Δθ3，θ4+Δθ4，完成位置更新。
本發(fā)明的有益效果在于：
本專利的方法具有直接計算得到冗余自由度機構運動學位置逆解的有點，逆解的精度高，適用于對末端姿態(tài)有較高精度的應用場合。同時本專利的方法適合具有跟蹤結構的應用，在算法進行位置更新的過程中，重點是求解目標位置狀態(tài)下所對應的各關節(jié)位置值，而不用去費力規(guī)劃兩個狀態(tài)之間的位置狀態(tài)值，大大化簡了運算步驟，依靠給定的一個目標位置就可以方便地進行冗余自由度機構的位置更新。
附圖說明
圖1為本發(fā)明流程圖。
具體實施方式
下面結合附圖對本發(fā)明做進一步描述。
本專利發(fā)明算法的解法的步驟：
本專利提出了一種基于最優(yōu)速比和關節(jié)約束方程的關節(jié)空間關節(jié)角度值數(shù)值解法，稱為恒定速比(RCC)位置更新方法。方法旨在解決現(xiàn)有冗余自由度機構運動速度規(guī)劃方法無法約束末端姿態(tài)的問題。方法的過程是這樣：
(1)對載體現(xiàn)有的姿態(tài)進行測量，獲得載體此時的航向角ψ、俯仰角φ和橫搖角γ；
(2)根據(jù)衛(wèi)星的參數(shù)，和第(1)步的測量結果，利用下式：
計算出來在當前時刻衛(wèi)星在載體坐標系下的描述矩陣Ts；式(1)中的航向角ψ、俯仰角φ和橫搖角γ為第(1)步的測量值，Ee代表衛(wèi)星參數(shù)的俯仰角，Ae代表衛(wèi)星參數(shù)的方位角，σe代表衛(wèi)星參數(shù)的極化角，C代表cos，S代表sin，例如：CEe代表cosEe，SEe代表sinEe。此時的天線系統(tǒng)的四個關節(jié)角為θ1，θ2，θ3，θ4；關節(jié)角度與描述矩陣Ts之間的函數(shù)關系為：Ts＝F(θ1,θ2,θ3,θ4)。
(3)當前時刻的天線框架的姿態(tài)矩陣為Ta，利用第(2)步中的衛(wèi)星矩陣Ts與Ta求解出兩個坐標系之間的轉動微元，
其中δx為繞x軸的轉動微元，δy為繞y軸的轉動微元，δz為繞z軸的轉動微元。為矩陣Ta的逆矩陣。將其中的速度微元提取出來組成向量[δx δy δz]T。
(4)對當前的天線系統(tǒng)結構計算雅克比矩陣值，記為J。
(5)求取天線系統(tǒng)的雅克比矩陣J的Moore-Penrose逆(簡稱M-P逆)，將M-P逆記為J+。
(6)求取四個關節(jié)角的運動微元：
Θ＝[dθ1 dθ2 dθ3 dθ4]T＝J+[δx δy δz]T (3)
其中，Θ為關節(jié)運動微元向量，dθ1為第一個關節(jié)角的運動微元，dθ2為第二個關節(jié)角的運動微元，dθ3為第三個關節(jié)角的運動微元，dθ4為第四個關節(jié)角的運動微元。
(7)計算各關節(jié)角之前運動微元的比例值，其中k2為dθ2與dθ1之間的比，其中k3為dθ3與dθ1之間的比，其中k4為dθ4與dθ1之間的比。
(8)進行位置更新時每個關節(jié)的運動量記為：Δθ1，Δθ2，Δθ3，Δθ4，利用再(7)中求得的k2，k3和k4以及Δθ1表示Δθ2，Δθ3，Δθ4有：
(9)建立位置更新后的關節(jié)角度方程為：
Ta＝F(θ1+Δθ1,θ2+k2Δθ1,θ3+k3Δθ1,θ4+k4Δθ1)
其中規(guī)則F是建立在關節(jié)角度和末端姿態(tài)之間的約束方程。這個約束關系的是通過建立冗余自由度運動機構的D-H模型得到的。這個模型給定了機構四個運動角度與末端姿態(tài)描述矩陣之間的對應關系。
(10)利用牛頓迭代方法，求解出第(9)步中的方程，得到Δθ1，再利用式(4)求得Δθ2，Δθ3，Δθ4。
(11)得到關節(jié)的更新位置為θ1+Δθ1，θ2+Δθ2，θ3+Δθ3，θ4+Δθ4，完成位置更新。
可以看出，以上分析的RCC方法，不僅適用于本專利中涉及的結構，同時也可以推廣至使用轉動關節(jié)描述的冗余自由度機械臂中，具有廣泛的適用性。本專利的方法具有直接計算得到冗余自由度機構運動學位置逆解的有點，逆解的精度高，適用于對末端姿態(tài)有較高精度的應用場合。同時本專利的方法適合具有跟蹤結構的應用，在算法進行位置更新的過程中，重點是求解目標位置狀態(tài)下所對應的各關節(jié)位置值，而不用去費力規(guī)劃兩個狀態(tài)之間的位置狀態(tài)值，大大化簡了運算步驟，依靠給定的一個目標位置就可以方便地進行冗余自由度機構的位置更新。