主权项 |
1、一种适用于全光纤数字测斜仪的捷联惯性组合测量控制装置,包括:用于存储惯性组合测量任务控制程序的计算机;用于输入角速度信息的光纤陀螺;用于输入比力信息的加速度计;用于输入地磁分量信息的磁通门;用于输入零速修正信息的下井探管;用于输入缆长信息的计数器,计数器安装在光缆绞盘上,光缆的一端连接在光缆绞盘上,另一端固定在下井探管上;其特征在于还包括:捷联惯性测量、磁测量、光缆运行速度测量、卡尔曼滤波器和信息比较A单元、信息比较B单元、信息比较C单元,所述捷联惯性测量首先接收A)所述光纤陀螺输出的下井探管机体坐标系ObXbYbZb下的角速度信息;和 B)所述加速度计输出的下井探管机体坐标系ObXbYbZb下的比力信息;然后对所述角速度信息和所述比力信息采用航迹推算处理后,输出C)下井探管的速度信息Vx、Vy、Vz;和 D)下井探管姿态信息的方位角ψ、横滚角φ、倾斜角θ;最后将E)所述速度信息Vx、Vy、Vz输出给所述信息比较B单元和所述信息比较C单元;将F)所述姿态信息输出给信息比较A单元;所述磁通门测量首先接收A)磁通门组件输出的下井探管机体坐标系ObXbYbZb 下的地磁分量信息;和 B)加速度计输出的下井探管机体坐标系ObXbYbZb下的比力信息;然后对所述地磁分量信息和所述比力信息采用磁测量计算,并将计算出的相对于磁北的方位角转换为相对于真北的方位角ψc后,输出所述下井探管姿态信息的方位角ψc、横滚角φc、倾斜角θc;最后将C)所述姿态信息输出给信息比较A单元;所述光缆运行速度测量是采用所述计数器测量得到的光缆长度在单位时间内光缆长度增量得到下井探管运行时在其机体坐标系ObXbYbZb下的速度信息;所述卡尔曼滤波器接收A)信息比较A单元输出的姿态差值<math> <mrow> <msub> <mi>Z</mi> <mi>KA</mi> </msub> <mo>=</mo> <mfenced open='[' close=']' separators=''> <mi></mi> <mtable> <mtr> <mtd> <mi>ψ</mi> <mo>-</mo> <msub> <mi>ψ</mi> <mi>c</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mi>θ</mi> <mo>-</mo> <msub> <mi>θ</mi> <mi>c</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mi>φ</mi> <mo>-</mo> <msub> <mi>φ</mi> <mi>c</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow> </math> 和B)信息比较B单元输出的速度差值<math> <mrow> <msub> <mi>Z</mi> <mi>KB</mi> </msub> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>V</mi> <mi>x</mi> </msub> <mo>-</mo> <msub> <mi>V</mi> <mi>xc</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>V</mi> <mi>y</mi> </msub> <mo>-</mo> <msub> <mi>V</mi> <mi>yc</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>V</mi> <mi>z</mi> </msub> <mo>-</mo> <msub> <mi>V</mi> <mi>zc</mi> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </math> 和C)信息比较C单元输出的速度差值<math> <mrow> <msub> <mi>Z</mi> <mi>KC</mi> </msub> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>V</mi> <mi>x</mi> </msub> <mo>-</mo> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mi>y</mi> </msub> <mo>-</mo> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mi>V</mi> <mi>z</mi> </msub> <mo>-</mo> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow> </math> 利用离散型卡尔曼滤波器对接收信息进行数据融合实现对状态变量X=[δrx,δry,δrz,δrx,δry,δrz,ηx,ηy,ηz,δfx,δfy,δfz,δωx,δωy,δωz]的进行最优估计,根据估计的状态变量对所述捷联惯性测量进行在线误差补偿,输出补偿后的下井探管姿态信息<math> <mrow> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>ψ</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>θ</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>φ</mi> <mn>0</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mi>ψ</mi> <mo>-</mo> <msub> <mi>η</mi> <mi>x</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mi>θ</mi> <mo>-</mo> <msub> <mi>η</mi> <mi>y</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mi>φ</mi> <mo>-</mo> <msub> <mi>η</mi> <mi>z</mi> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </math> 给所述计算机显示界面;式中,δrx表示当地地理坐标系OtXtYtZt下的位置误差X轴分量,δry表示当地地理坐标系OtXtYtZt下的位置误差Y轴分量,δrz表示当地地理坐标系OtXtYtZt下的位置误差Z轴分量,δvx表示当地地理坐标系OtXtYtZt下的速度误差X轴分量,δvy表示当地地理坐标系OtXtYtZt下的速度误差Y轴分量,δvz表示当地地理坐标系OtXtYtZt下的速度误差Z轴分量,ηx表示方位角ψ误差,ηy表示倾斜角θ误差,ηz表示横滚角φ误差,ηzδfx表示X轴加速度计误差,δfy表示Y轴加速度计误差,δfz表示Z轴加速度计误差,δωx表示X轴光纤陀螺误差,δωy表示Y轴光纤陀螺误差,δωz表示Z轴光纤陀螺误差;所述信息比较A单元,用于完成对所述捷联惯性测量输出的下井探管姿态信息的方位角ψ、横滚角φ、倾斜角θ与所述磁测量输出的下井探管姿态信息的方位角ψc、横滚角φc、倾斜角θc相减输出姿态差值<math> <mrow> <msub> <mi>Z</mi> <mi>KA</mi> </msub> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mi>ψ</mi> <mo>-</mo> <msub> <mi>ψ</mi> <mi>c</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mi>θ</mi> <mo>-</mo> <msub> <mi>θ</mi> <mi>c</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mi>φ</mi> <mo>-</mo> <msub> <mi>φ</mi> <mi>c</mi> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </math> 给卡尔曼滤波器;所述信息比较B单元,用于完成对所述捷联惯性测量输出的下井探管的速度信息Vx、Vy、Vz与所述光缆运动速度测量输出的速度信息Vxc、Vyc、Vzc相减输出速度差值<math> <mrow> <msub> <mi>Z</mi> <mi>KB</mi> </msub> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>V</mi> <mi>x</mi> </msub> <mo>-</mo> <msub> <mi>V</mi> <mi>xc</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>V</mi> <mi>y</mi> </msub> <mo>-</mo> <msub> <mi>V</mi> <mi>yc</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>V</mi> <mi>z</mi> </msub> <mo>-</mo> <msub> <mi>V</mi> <mi>zc</mi> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </math> 给卡尔曼滤波器;所述信息比较C单元,用于完成对所述捷联惯性测量输出的下井探管的速度信息Vx、Vy、Vz与所述下井探管静止量输出的零速度信息相减输出速度差值<math> <mrow> <msub> <mi>Z</mi> <mi>KC</mi> </msub> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>V</mi> <mi>x</mi> </msub> <mo>-</mo> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mi>V</mi> <mi>y</mi> </msub> <mo>-</mo> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mi>V</mi> <mi>z</mi> </msub> <mo>-</mo> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </math> 给卡尔曼滤波器。 |