[發明專利]一種基于磁場的六自由度位移測量方法

有效

• 申請號: 201510959014.0
• 申請日: 2015-12-18
• 公開（公告）號: CN105403140B
• 公開（公告）日: 2018-10-30
• 發明（設計）人: 朱煜;胡金春;尹文生;季釔澎;張鳴;楊開明;胡楚雄;成榮
• 申請（專利權）人: 清華大學
• 主分類號: G01B7/02
• 分類號: G01B7/02
• 代理公司: 北京鴻元知識產權代理有限公司 11327
• 代理人: 邸更巖

1.一種基于磁場的六自由度位移測量方法，其特征在于所述方法包括如下步驟：

1)在相互運動的兩個物體上分別固定永磁體和n個磁傳感器，其中n大于等于所測自由度的個數；

2)在永磁體上建立定坐標系OXYZ，在定坐標系OXYZ下建立永磁體的磁感應強度與空間位置坐標的數學關系：B(x,y,z)＝B_x(x,y,z)i+B_y(x,y,z)j+B_z(x,y,z)k，其中B_x,B_y,B_z分別為B在X軸、Y軸和Z軸方向上的分量，i,j,k分別為X軸、Y軸和Z軸的單位矢量；所述永磁體的形狀為規則的幾何圖形；

3)在磁傳感器所在的運動物體上建立動坐標系O'X'Y'Z'，n個磁傳感器的安裝位置，即在動坐標系O'X'Y'Z'下的坐標為(x_i0,y_i0,z_i0)，其中i＝1,2,...,n；

4)設初始時刻動坐標系O'X'Y'Z'與定坐標系OXYZ相重合，若某時刻運動物體在定坐標系OXYZ下產生六自由度位移X＝(x,y,z,θ_x,θ_y,θ_z)^T，其中x,y,z為O'在定坐標系OXYZ下分別沿X軸、Y軸和Z軸的平動位移，θ_x,θ_y,θ_z為O'分別繞X軸、Y軸和Z軸的旋轉角，則該時刻這n個磁傳感器在定坐標系OXYZ的坐標(x_i,y_i,z_i)為(x_i,y_i,z_i)^T＝R(x_i0,y_i0,z_i0)^T+(x,y,z)^T，

式中R為旋轉矩陣，此時磁傳感器中的檢測量為B_i＝B_x(x_i,y_i,z_i)i·a'+B_y(x_i,y_i,z_i)j·a'+B_z(x_i,y_i,z_i)k·a'，其中a'為所選磁傳感器敏感方向的單位矢量；

5)運動物體以一定速度在磁場空間中連續運動，磁傳感器中的檢測信號為B_n，利用步驟4)中得到的磁傳感器的檢測量與空間位置坐標的關系，n個磁傳感器就有n個位置方程，利用牛頓迭代法進行位置方程的解算：

假設在初始時刻t＝t₀時，位置坐標X₀＝(x₀,y₀,z₀,θ_x0,θ_y0,θ_z0)；通過n個傳感器測得的磁感應強度信號為B_k，而通過計算得到的磁傳感器檢測量為B_i，算出測量偏差△B＝B_k-B_i，設置判定收斂的條件ε和迭代次數k，計算判定不等式△B<ε；

a.若不等式成立，則該時刻的位移即為X₀＝(x₀,y₀,z₀,θ_x0,θ_y0,θ_z0)；

b.若不等式不成立，則計算該時刻的雅克比矩陣和位移偏差：△x＝[J^T(X₀)J(X₀)]^-1J^T(X₀)△B，更新該時刻的位移X₀＝X₀+△x，將更新過后的位移帶入判定不等式計算直至不等式成立或達到迭代次數為止，否則重復這一迭代過程；

6)在下一時刻t＝t₁時，將位置坐標初始值X₁更新為上一時刻t＝t₀的最終位移迭代值X₀，計算判定不等式直到不等式成立或達到迭代次數為止；得到六自由度位移X＝(x,y,z,θ_x,θ_y,θ_z)^T，實現位移測量。