CN114137590B - 一种基于正交轴系调平装置北向基准修正的目标定位方法 - Google Patents

Abstract

本发明属于目标定位技术领域，具体涉及一种基于正交轴系调平装置北向基准修正的目标定位方法，该方法针对采用了自动调平装置的目标定位***，利用向量的空间旋转变换，基于自动调平装置轴系结构约束，按照调平面与水平面平行的边界条件建立一系列数学计算方法，能够在不测量自动调平装置轴系旋转角度的情况下从自动调平装置下部北向基准准确解算出调平台调平后的上部北向基准，通过这一北向基准修正过程，该方法解决了此类目标定位***传统的目标定位方法不做北向基准修正，直接引用自动调平装置下部北向基准所产生的倾斜工况下目标定位误差增大的问题，在各种倾斜条件下均可获得一致的精度。

Description

一种基于正交轴系调平装置北向基准修正的目标定位方法

技术领域

本发明属于目标定位技术领域，具体涉及一种基于正交轴系调平装置北向基准修正的目标定位方法。

背景技术

(一)技术需求

基于地面观测的目标定位***需要最终获得目标的地理坐标。这就需要解决以下两个技术问题，第一个问题是测量观测点的坐标，第二个问题是测量目标点与观测点的相对位置关系。观测点坐标测量通过卫星定位或惯性导航等手段可以实时获取。对于第二个问题，需要通过光学观瞄测量观测点到目标点构成的空间矢量的方向，即瞄线方向，同时通过测距操作获取目标距离。为实现瞄线方向的测量，需将用于光学观瞄的光学成像传感器安装在两轴转台上构成观瞄装置，理论上，将惯导直接与观瞄装置固连即可实时测量瞄线方向，但在工程应用中，地面观测的目标定位***通常采用车载平台，惯导需要与车体固连保证车辆的惯性导航功能，同时，对目标的搜索过程涉及两轴转台频繁的旋转运动，这将导致惯导测姿数据产生较大的漂移，无法保证测角精度。因此，如图1所示，典型的地面观测的目标定位***均将惯导与***承载车辆固连。另一方面，由于野外环境下，观测点很可能会存在坡度，***承载车辆只能倾斜停放，对于某些目标定位测量装置，其工作条件要求方位回转轴必须与水平面垂直。这就需要增加调平机构，调平机构通过两个正交轴系的旋转补偿使上部调平面，即目标定位测量装置的安装面能够在车辆倾斜时也始终保持水平。调平机构的结构安装方式如图1所示，调平机构基座的安装面法线与惯导z轴平行，调平机构基座可以与惯导固连，或通过方位回转机构连接。

由于惯导与车体固连，调平面方位角没有与其固连的测姿传感器，因此需要将惯导获得的北向基准传递到调平面上，这样，在调平面上的观瞄装置就可以获得目标相对于观测点的地理方向，从而实现目标定位。

(二)现有的技术方案

针对要求方位回转轴必须垂直于水平面的目标定位测量***，典型的解决方案如下：

1)安装惯性导航装置

用于获取北向基准的惯性导航装置与车体固连，安装方向为：惯性导航装置的x轴指向车体正前方，z轴指向车顶正上方。定义惯性导航装置坐标系为x₂y₂z₂；

2)安装调平机构

在车辆顶部安装面安装调平机构，车辆顶部安装面与惯性导航装置的xy平面平行，即调平机构底座平面与惯性导航装置的xy平面平行；如图2所示，调平机构的上部平面通过两个回转轴与调平机构底座平面铰联，通过上部平面的倾斜传感器可以实时测量上部平面偏离水平的程度，通过负反馈控制两个回转轴上的旋转，使上部平面保持水平；

3)校准两轴转台瞄线方向

在两轴转台的俯仰机构上安装一个或多个光学成像传感器、激光测距机。两轴转台瞄线方向以激光测距机的激光发射方向为基准，通过调整各成像传感器图像上叠加的十字分划位置，使十字交点与激光照射点重合。经过上述瞄线方向校准过程，在后续的观瞄操作中，就能够通过操作两轴转台旋转，使十字分划交点压住目标，即可保证瞄线指向目标；

4)安装两轴转台

在调平机构的上部平面上安装两轴转台。两轴转台方位轴垂直于安装基座，俯仰轴垂直于方位轴，定义方位、俯仰零位为调平机构的上部平面与调平机构底座平面平行时，使瞄线指向车体正前方的两轴转台回转位置；

5)建立观测坐标系x₀y₀z₀

基于以上结构安装关系，能够保证两轴转台方位回转轴始终垂直于水平面。以两轴转台架设位置为原点，转台零位的瞄线方向为x轴，转台方位轴方向为z轴，建立观测坐标系x₀y₀z₀；

6)获取目标在观测坐标系下的直角坐标(x₀,y₀,z₀)

当瞄线指向目标时，基于平台方位、俯仰轴上安装的角度传感器获取两轴转台当前位置相对于其初始零位的方位、俯仰旋转角度θ，结合测距操作获取目标距离d，即为目标在观测坐标系下的极坐标数据，由/>可计算出观测坐标系下的直角坐标(x₀,y₀,z₀)；

7)获取观测坐标系相对于北西天坐标系x₁y₁z₁的旋转变换关系

惯性导航装置输出的姿态角(a,p,r)表示了惯导坐标系相对于北西天坐标系的旋转变换关系，所述北西天坐标系是以观测点为原点，真北方向为x轴，天空方向为z轴；根据图2所示的结构关系，观测坐标系相对于惯导坐标系首先调整了方位角，再依次绕调平机构的两个正交轴回转调整。

在调平状态下，观测坐标系相对于北西天坐标系仅存在方位旋转。而在调平机构的上部平面与调平机构底座平面平行时，该方位旋转量即惯性导航装置输出的姿态角的方位分量a。

现有的目标定位方法认为调平机构的调平动作，即依次绕调平机构的两个正交轴的回转调整，不会改变调平面的方位角，因此，直接采用惯导输出的方位角a加上两轴转台方位回转角作为观测坐标系相对于北西天坐标系的方位旋转量。

8)获取北西天坐标系相对与地心坐标系x₃y₃z₃的旋转、平移关系

通过卫星定位或惯性导航测量手段，可以获得两轴转台架设位置的包括经纬度、高程的坐标数据(L,B,h)，经纬度、高程数据可以对应转换为地心坐标系下的直角坐标量(X,Y,Z)。所述地心坐标系是以地理椭球中心为原点，原点到北极点方向为z轴，原点到赤道0经度点方向为x轴的平面直角坐标系；经纬度确定了北西天坐标系相对与地心坐标系的旋转变换关系；由经纬度转换出的直角坐标量(X,Y,Z)确定了北西天坐标系相对与地心坐标系的平移关系。

9)通过坐标变换将目标在观测坐标系下坐标转换到目标在地心坐标系下的坐标

根据前述步骤获得的观测坐标系相对于北西天坐标系的旋转变换关系、北西天坐标系相对与地心坐标系的旋转、平移关系，对目标在观测坐标系下的极坐标数据依次进行坐标变换，得到目标在地心坐标系下的坐标。

10)将目标在地心坐标系下的坐标转换为经纬度表示。

(三)现有技术方案的缺陷

在前述方案的第7)步中，假设了“调平动作不会改变调平面的方位角”。该假设实际上仅在俯仰、横滚调整量均为小角度时，才能在一定误差范围内成立。经过试验验证，采用当前方案的定位***，在车体停放在12度坡面上时，相对于水平停放，目标定向误差增加了0.6度左右，在坡面坡度继续增加时，误差继续增大。由于大坡度下可能产生车体倾覆，通常对于车载目标定位***可工作的最大坡度进行了限定，最大限定值从5度到21度不等，大部分***最大坡度要求是大于等于12度的。这就使得当前方案仅能应用于小角度倾斜应用，应用范围大大受限。

发明内容

(一)要解决的技术问题

本发明要解决的技术问题是：针对现有的要求方位回转轴必须垂直与水平面的车载目标定位测量***在较大的载车倾斜角度下目标定位误差大的问题，本发明要解决的技术问题是如何实现一种能够在各种倾斜工况下均能达到与载车倾斜角度为零，即载车停放在水平面上展开侦察的工况一致的目标定位精度的目标定位方法。

(二)技术方案

为解决上述技术问题，本发明提供一种基于正交轴系调平装置北向基准修正的目标定位方法，所述目标定位方法包括具体步骤如下：

步骤1：操作两轴转台对目标测距，获取目标在观测坐标系x₀y₀z₀下的坐标；

操作两轴转台旋转，使叠加在传感器上的十字分划交点压住目标，触发激光测距，获取目标距离d；纪录激光测距触发时刻的两轴转台的方位角度俯仰角度θ₀；按照下式计算出目标在观测坐标系下的坐标：

步骤2：获取北向基准；

纪录激光测距触发时刻的惯导姿态角，包括方位角a，俯仰角p，横滚角r；

步骤3：将北向基准传递到观测坐标系；

即基于惯导姿态角(a，p，r)、调平基座相对惯导的方位回转角求解观测坐标系相对于北西天坐标系的方位旋转量a₀；

这一步骤的方法包括：环节1：选择初始矢量；环节2：计算初始矢量绕指定矢量旋转指定角度后的矢量方向；环节3：在二维搜索空间上的基于多次细分搜索进行数值逼近运算；

步骤4：获取观测点经纬度、高程；

通过卫星定位或惯性导航测量手段，获得观测点经纬度(L₀,B₀)、高程h₀；

步骤5：将目标在观测坐标系下的坐标转换到北西天坐标系x₁y₁z₁；

按照以下公式确定观测坐标系到北西天坐标系的变换关系；

[x₁,y₁,z₁,1]^T＝A^-1[x₀,y₀,z₀,1]^T………………………………2)

式中，

步骤6：将目标在北西天坐标系x₁y₁z₁下的坐标转换到地心坐标系x₃y₃z₃；

建立过渡坐标系x_by_bz_b，其原点与北西天坐标系一致，x轴方向为天空方向，即北西天坐标系的z轴方向，y轴方向为东向，即北西天坐标系的y轴负方向，z轴方向为北向，即北西天坐标系的x轴方向；

按照以下公式将观测点经纬度、高程数据转换为地心坐标系下的直角坐标量(X,Y,Z)；

式中，R＝6378137.0,e＝0.0066943800229

按照以下公式确定北西天坐标系相对与地心坐标系下的变换关系；

[x₃,y₃,z₃,1]^T＝A^-1[x_b,y_b,z_b,1]^T

即[x₃,y₃,z₃,1]^T＝A^-1[z₁,-y₁,x₁,1]^T.............................4)

式中，

步骤7：将目标的地心坐标(x₃,y₃,z₃)转换为经纬度(L,B,h)；

按照下式计算：

其中：B的计算采用递归算法求解；

至此，得到了目标的地理坐标(L,B,h)。

其中，所述观测坐标系x₀y₀z₀的建立为：以两轴转台架设位置为原点，转台零位的瞄线方向为x轴，转台方位轴方向为z轴，建立观测坐标系x₀y₀z₀。

其中，所述步骤3中，所述选择初始矢量，即在调平基座未旋转，且底座平面与调平面平行时的自动调平装置初始状态下，在自动调平装置的底座平面、调平面上分别选择确定的沿结构边线的单位矢量其中/>是需要传递的北向基准矢量，保证这两条选定的结构边线平行，即/>

再选择绕底座平面法线旋转90度的矢量，令该矢量为/>显然，在自动调平装置初始状态下，/>绕调平面法线旋转90度形成的矢量/>对应的结构边线与/>对应的结构边线平行，即/>这样，对于自动调平装置的任意位置，/>都是/>经过特定旋转变换T的结果，该旋转变换是依次按照第1旋转轴/>旋转θ角度，再绕第2旋转轴/>旋转γ角度的两次旋转变换的顺序组合；同理，对于自动调平装置的任意位置，/>都是/>经过旋转变换T的结果；为了统一基准，统一在北－西－天坐标系下进行计算，此处选择右手系坐标系，故xyz轴分别对应北西天方向；

惯导输出的方位角a、俯仰角p、横滚角r是指在北－西－天坐标系下按照zyx顺规旋转的三个角度量，即首先绕z轴旋转，再绕y轴旋转，再绕x轴旋转，方位角即北向角；由于惯导与自动调平装置底座固连，则在北－西－天坐标系中的值可通过由(a，p，r)确定的旋转变换求解出来；而/>则需再进行一次绕/>轴旋转θ的旋转变换求解出来；这样，旋转变换T就由(a，p，r，θ，γ)五个参量确定；

由于θ，γ角度未知，但旋转变换T满足调平条件：的z分量为0；

这样，通过在二维空间基于多次细分搜索方式，对θ，γ的各种取值进行数值逼近运算，获得满足调平条件的θ，γ的数值解，从而求出旋转变换T，这样，就能够计算出在该工况下的从而得到a₀，即完成了北向基准传递。

其中，所述步骤3中，所述环节1的选择初始矢量，具体为：

在惯导坐标系x₂y₂z₂下,定义向量即沿惯导坐标系x轴正方向的单位矢量，定义向量/>即自动调平装置底座平面纵向结构边线方向的单位矢量，定义向量/>即自动调平装置调平面纵向结构边线方向的单位矢量；

在调平基座未旋转，且底座平面与调平面平行时， 为绕惯导坐标系z轴旋转/>得到；/>为/>经调平过程的依次绕第1、第2旋转轴的两次旋转后得到；

在惯导坐标系x₂y₂z₂下,定义向量为(1,0,0)；

在惯导坐标系的xy平面上选择与垂直的矢量/>(0，1，0)，对应的，向量/>即自动调平装置底座平面横向结构边线方向的单位矢量，向量/>即自动调平装置调平面横向结构边线方向的单位矢量；

同样的，在调平基座未旋转，且底座平面与调平面平行时， 为/>绕惯导坐标系z轴旋转/>得到；/>为/>经调平过程的依次绕第1、第2旋转轴的两次旋转后得到；

因此，从惯导坐标系获得的北向的基准传递过程包括1次方位旋转，以及调平过程引起的依次绕自动调平装置的两个正交轴的两次旋转。在调平基座未旋转，且底座平面与调平面平行时，上述方位旋转轴矢量为(0,0,1)，调平过程第1旋转轴矢量为(0,1,0)，第2旋转轴矢量为(1,0,0)；

在任意工况下，自动调平装置调平基座绕方位旋转轴进行了的方位回转，自动调平装置执行调平操作的过程是一个依次绕第1、第2旋转轴依次旋转θ_t、γ_t的旋转变换，调平面上的/> 分别就是/>经过上述旋转变换后的矢量。

其中，所述步骤3中，所述环节2的计算初始矢量绕指定矢量旋转指定角度后的矢量方向，具体为：

矢量在惯导坐标系下的值可由下式解得：

[x_2A",y_2A",z_2A"]^T＝A₁ ^-1[1,0,0]^T…………………………6)

旋转变换矩阵A₁由如下的旋转变换公式计算出，其中θ、γ分别指方位、俯仰、横滚角，这里/>θ＝θ_t、γ＝γ_t；方位、横滚旋转正方向遵循右手准则，俯仰正方向遵循左手准则；

同理，矢量在惯导坐标系下的值可由下式解得：

[x_2B",y_2B",z_2B"]^T＝A₁ ^-1[0,1,0]^T…………………………8)

矢量 在北西天坐标系下的值可由下式解得：

A₂由公式7)计算出，这里θ＝p、γ＝t；

这样，对于某一任意给出的一组θ_t、γ_t，均可以计算出/> 根据调平约束/>在北西天坐标系下的z分量为0。则可以通过逐次细分搜索确定θ_t、γ_t的值，从而解出调平状态下的/>这样基准/>的北向数据就传递到了/>上，即在底座倾斜的情况下，完成了北向基准的修正。

其中，所述步骤3中，所述环节3的在二维搜索空间上的基于多次细分搜索进行数值逼近运算，具体如下：

首先令γ_t＝0，计算到/>的旋转过程，对θ_t逐次细分搜索；首次搜索是将搜索域(-w,w)划分为2n等份，在2n+1个划分点上计算，寻找使/>的z分量绝对值最小的点p；首轮搜索完成后，定义初始搜索步长：

这里，k为略小于1的数，用于使搜索边界产生一定程度叠加，避免错误收敛；

以首轮搜索出的点p为中心，对p+gap、p、p－gap点计算，寻找使的z分量绝对值最小的点更新p，调整搜索步长：

这里，k为略小于2的数；重复该步骤，不断减小gap，直到达到指定精度；θ_t确定后，则第二旋转轴矢量得以确定；

然后，用同样方法，计算到/>的旋转过程，对γ_t逐次细分搜索，完成求解过程。

其中，所述环节3中，w是调平装置调平范围。

其中，所述步骤3中，是需要传递的北向基准矢量。

其中，所述步骤3中，运算过程中，选择右手系坐标系，故xyz轴分别对应北西天方向。

其中，所述步骤3中，按照zyx顺规旋转，表示：首先绕z轴旋转，再绕y轴旋转，再绕x轴旋转。

(三)有益效果

与现有技术相比较，本发明技术方案所提供的针对正交轴系调平装置的北向基准修正方法，具有以下优点：

(1)相较传统方法，该方法不再依赖小角度近似条件，对于任何倾斜工况，均基于测量数据严格按照矢量旋转模型计算了调平面基准，而不是采用方位角简单叠加的处理方法，理论上，可适应0到90度的坡面停车工况，采用该方法可以在调平机构全范围内保证目标定位精度

(2)该方法的数学模型仅要求自动调平装置的第1，第2旋转轴正交，不对自动调平装置安装方向进行任何约束，能够适应各种结构安装形式下的北向基准传递；

(3)该方法的核心计算过程均为矩阵乘法操作，便于实际计算过程的算法优化或实现基于硬件加速的计算；

(4)本方法的主要计算量在第3步，即基准传递过程，仅在自动调平装置底座发生移动的情况下才需要重新计算，实际计算耗费资源极小。

附图说明

图1是本发明涉及的一种典型的目标定位装置。

图2是目标定位装置、自动调平装置的空间拓扑结构示意。

图3是本发明的一个优选实例装置的结构装配关系。

图4是针对本发明优选实例涉及的自动调平装置北向基准传递问题的矢量变换关系图。

具体实施方式

为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

为解决上述技术问题，本发明提供一种基于正交轴系调平装置北向基准修正的目标定位方法，该方法摈弃了现有技术方案中“在调平机构进行俯仰、横滚调整时，调平机构上部调平面的方位角不发生变化。”的基本假设，对应地在操作步骤中，不采用现有技术方案中“直接取惯性导航装置输出的方位角a作为观测坐标系相对于北西天坐标系的方位旋转量。”的处理方法，而是将观测坐标系相对于北西天坐标系的方位旋转量视作与惯性导航装置输出的方位角a，车体倾斜量均相关的函数。基于调平机构轴系空间关系确定调平过程的运动约束，以调平机构上部平面处于水平态作为边界条件，基于空间矢量旋转原理建立数学模型，通过数值计算实现在任意倾角下，将惯性导航装置输出的方位角a修正后作为观测坐标系相对于北西天坐标系的方位旋转量。

所述目标定位方法包括具体步骤如下：

步骤1：操作两轴转台对目标测距，获取目标在观测坐标系x₀y₀z₀下的坐标；

操作两轴转台旋转，使叠加在传感器上的十字分划交点压住目标，触发激光测距，获取目标距离d；纪录激光测距触发时刻的两轴转台的方位角度俯仰角度θ₀；按照下式计算出目标在观测坐标系下的坐标：

步骤2：获取北向基准；

纪录激光测距触发时刻的惯导姿态角，包括方位角a，俯仰角p，横滚角r；

步骤3：将北向基准传递到观测坐标系；

即基于惯导姿态角(a，p，r)、调平基座相对惯导的方位回转角求解观测坐标系相对于北西天坐标系的方位旋转量a₀；

这一步骤的方法包括：环节1：选择初始矢量；环节2：计算初始矢量绕指定矢量旋转指定角度后的矢量方向；环节3：在二维搜索空间上的基于多次细分搜索进行数值逼近运算；

步骤4：获取观测点经纬度、高程；

通过卫星定位或惯性导航测量手段，获得观测点经纬度(L₀,B₀)、高程h₀；

步骤5：将目标在观测坐标系下的坐标转换到北西天坐标系x₁y₁z₁；

按照以下公式确定观测坐标系到北西天坐标系的变换关系；

[x₁,y₁,z₁,1]^T＝A^-1[x₀,y₀,z₀,1]^T………………………………2)

式中，

步骤6：将目标在北西天坐标系x₁y₁z₁下的坐标转换到地心坐标系x₃y₃z₃；

建立过渡坐标系x_by_bz_b，其原点与北西天坐标系一致，x轴方向为天空方向，即北西天坐标系的z轴方向，y轴方向为东向，即北西天坐标系的y轴负方向，z轴方向为北向，即北西天坐标系的x轴方向；

按照以下公式将观测点经纬度、高程数据转换为地心坐标系下的直角坐标量(X,Y,Z)；

式中，R＝6378137.0,e＝0.0066943800229

按照以下公式确定北西天坐标系相对与地心坐标系下的变换关系；

[x₃,y₃,z₃,1]^T＝A^-1[x_b,y_b,z_b,1]^T

即[x₃,y₃,z₃,1]^T＝A^-1[z₁,-y₁,x₁,1]^T.............................4)

式中，

步骤7：将目标的地心坐标(x₃,y₃,z₃)转换为经纬度(L,B,h)；

按照下式计算：

其中：B的计算采用递归算法求解；

至此，得到了目标的地理坐标(L,B,h)。

其中，所述观测坐标系x₀y₀z₀的建立为：以两轴转台架设位置为原点，转台零位的瞄线方向为x轴，转台方位轴方向为z轴，建立观测坐标系x₀y₀z₀。

其中，所述步骤3中，所述选择初始矢量，即在调平基座未旋转，且底座平面与调平面平行时的自动调平装置初始状态下，在自动调平装置的底座平面、调平面上分别选择确定的沿结构边线的单位矢量其中/>是需要传递的北向基准矢量，保证这两条选定的结构边线平行，即/>

再选择绕底座平面法线旋转90度的矢量，令该矢量为/>显然，在自动调平装置初始状态下，/>绕调平面法线旋转90度形成的矢量/>对应的结构边线与/>对应的结构边线平行，即/>这样，对于自动调平装置的任意位置，/>都是/>经过特定旋转变换T的结果，该旋转变换是依次按照第1旋转轴/>旋转θ角度，再绕第2旋转轴/>旋转γ角度的两次旋转变换的顺序组合；同理，对于自动调平装置的任意位置，/>都是/>经过旋转变换T的结果；为了统一基准，统一在北－西－天坐标系下进行计算，此处选择右手系坐标系，故xyz轴分别对应北西天方向；

惯导输出的方位角a、俯仰角p、横滚角r是指在北－西－天坐标系下按照zyx顺规旋转的三个角度量，即首先绕z轴旋转，再绕y轴旋转，再绕x轴旋转，方位角即北向角；由于惯导与自动调平装置底座固连，则在北－西－天坐标系中的值可通过由(a，p，r)确定的旋转变换求解出来；而/>则需再进行一次绕/>轴旋转θ的旋转变换求解出来；这样，旋转变换T就由(a，p，r，θ，γ)五个参量确定；

由于θ，γ角度未知，但旋转变换T满足调平条件：的z分量为0；

这样，通过在二维空间基于多次细分搜索方式，对θ，γ的各种取值进行数值逼近运算，获得满足调平条件的θ，γ的数值解，从而求出旋转变换T，这样，就能够计算出在该工况下的从而得到a₀，即完成了北向基准传递。

其中，所述步骤3中，所述环节1的选择初始矢量，具体为：

如图4所示，在惯导坐标系x₂y₂z₂下,定义向量即沿惯导坐标系x轴正方向的单位矢量，定义向量/>即自动调平装置底座平面纵向结构边线方向的单位矢量，定义向量即自动调平装置调平面纵向结构边线方向的单位矢量；

在调平基座未旋转，且底座平面与调平面平行时， 为绕惯导坐标系z轴旋转/>得到；/>为/>经调平过程的依次绕第1、第2旋转轴的两次旋转后得到；

在惯导坐标系x₂y₂z₂下,定义向量为(1,0,0)；

在惯导坐标系的xy平面上选择与垂直的矢量/>(0,1,0)，对应的，向量/>即自动调平装置底座平面横向结构边线方向的单位矢量，向量/>即自动调平装置调平面横向结构边线方向的单位矢量；

同样的，在调平基座未旋转，且底座平面与调平面平行时， 为/>绕惯导坐标系z轴旋转/>得到；/>为/>经调平过程的依次绕第1、第2旋转轴的两次旋转后得到；

因此，从惯导坐标系获得的北向的基准传递过程包括1次方位旋转，以及调平过程引起的依次绕自动调平装置的两个正交轴的两次旋转。在调平基座未旋转，且底座平面与调平面平行时，上述方位旋转轴矢量为(0,0,1)，调平过程第1旋转轴矢量为(0,1,0)，第2旋转轴矢量为(1,0,0)；

在任意工况下，自动调平装置调平基座绕方位旋转轴进行了的方位回转，自动调平装置执行调平操作的过程是一个依次绕第1、第2旋转轴依次旋转θ_t、γ_t的旋转变换，调平面上的/> 分别就是/>经过上述旋转变换后的矢量。

其中，所述步骤3中，所述环节2的计算初始矢量绕指定矢量旋转指定角度后的矢量方向，具体为：

矢量在惯导坐标系下的值可由下式解得：

[x_2A",y_2A",z_2A"]^T＝A₁ ^-1[1,0,0]^T…………………………6)

旋转变换矩阵A₁由如下的旋转变换公式计算出，其中θ、γ分别指方位、俯仰、横滚角，这里/>θ＝θ_t、γ＝γ_t；方位、横滚旋转正方向遵循右手准则，俯仰正方向遵循左手准则；

同理，矢量在惯导坐标系下的值可由下式解得：

[x_2B",y_2B",z_2B"]^T＝A₁ ^-1[0,1,0]^T…………………………8)

矢量 在北西天坐标系下的值可由下式解得：

A₂由公式7)计算出，这里θ＝p、γ＝t；

这样，对于某一任意给出的一组θ_t、γ_t，均可以计算出/> 根据调平约束/>在北西天坐标系下的z分量为0。则可以通过逐次细分搜索确定θ_t、γ_t的值，从而解出调平状态下的/>这样基准/>的北向数据就传递到了/>上，即在底座倾斜的情况下，完成了北向基准的修正。

其中，所述步骤3中，所述环节3的在二维搜索空间上的基于多次细分搜索进行数值逼近运算，具体如下：

首先令γ_t＝0，计算到/>的旋转过程，对θ_t逐次细分搜索；首次搜索是将搜索域(-w,w)划分为2n等份，在2n+1个划分点上计算，寻找使/>的z分量绝对值最小的点p；首轮搜索完成后，定义初始搜索步长：

这里，k为略小于1的数，用于使搜索边界产生一定程度叠加，避免错误收敛；

以首轮搜索出的点p为中心，对p+gap、p、p－gap点计算，寻找使的z分量绝对值最小的点更新p，调整搜索步长：

这里，k为略小于2的数；重复该步骤，不断减小gap，直到达到指定精度；θ_t确定后，则第二旋转轴矢量得以确定；

然后，用同样方法，计算到/>的旋转过程，对γ_t逐次细分搜索，完成求解过程。

其中，所述环节3中，w是调平装置调平范围。

其中，所述步骤3中，是需要传递的北向基准矢量。

其中，所述步骤3中，运算过程中，选择右手系坐标系，故xyz轴分别对应北西天方向。

其中，所述步骤3中，按照zyx顺规旋转，表示：首先绕z轴旋转，再绕y轴旋转，再绕x轴旋转。

实施例1

本发明优选实例中，目标定位装置的两轴转台安装在自动调平装置调平面上。两轴转台采用了挑担式结构，方位机构绕垂直轴z旋转，两侧为共轴联动的俯仰机构，可绕俯仰轴y在正负30度内旋转，俯仰轴是在方位机构上的装置，因此是随方位机构运动的。这样对于安装在该两轴转台俯仰机构上的光学传感器来说，实际是处于一个按zy顺规旋转的两轴旋转机构上的，其光学传感器瞄线方向可以由两轴转台方位、俯仰均处于零位时的初始矢量经由方位角、俯仰角确定的旋转变换解出。

自动调平装置底座平面与惯导坐标系的xy平面平行，底座平面的前向，即底座平面纵向结构边线方向，与惯导坐标系x轴方向成角度。

1)根据本发明提出的步骤1，可得到目标在观测坐标系下的坐标(x₀,y₀,z₀)。

2)根据本发明提出的步骤2，可得到惯导姿态角(a,p,r)。

3)在本实施例下，本发明提出的步骤3具体实现如下：

(1)初始矢量选择

如图4所示，在惯导坐标系x₂y₂z₂下,向量即沿惯导坐标系x轴正方向的单位矢量，向量/>即自动调平装置底座平面纵向结构边线方向的单位矢量，向量/>即自动调平装置调平面纵向结构边线方向的单位矢量。

在调平基座未旋转，且底座平面与调平面平行时， 为绕惯导坐标系z轴旋转/>得到；/>为/>经调平过程的依次绕第1、第2旋转轴的两次旋转后得到。

在惯导坐标系x₂y₂z₂下,向量为(1,0,0)；

在惯导坐标系的xy平面上选择与垂直的矢量/>(0,1,0)，对应的，向量/>即自动调平装置底座平面横向结构边线方向的单位矢量，向量/>即自动调平装置调平面横向结构边线方向的单位矢量。

同样的，在调平基座未旋转，且底座平面与调平面平行时， 为/>绕惯导坐标系z轴旋转/>得到；/>为/>经调平过程的依次绕第1、第2旋转轴的两次旋转后得到。

因此，从惯导坐标系获得的北向的基准传递过程包括1次方位旋转，以及调平过程引起的依次绕自动调平装置的两个正交轴的两次旋转。在调平基座未旋转，且底座平面与调平面平行时，上述方位旋转轴矢量为(0,0,1)，调平过程第1旋转轴矢量为(0,1,0)，第2旋转轴矢量为(1,0,0)。在任意工况下，自动调平装置调平基座绕方位旋转轴进行了的方位回转，自动调平装置执行调平操作的过程是一个依次绕第1、第2旋转轴依次旋转θ_t、γ_t的旋转变换，调平面上的/> 分别就是/>经过上述旋转变换后的矢量。

(2)计算初始矢量绕指定矢量旋转指定角度后的矢量方向

矢量在惯导坐标系下的值可由下式解得：

[x_2A",y_2A",z_2A"]^T＝A₁ ^-1[1,0,0]^T…………………………6)

旋转变换矩阵A₁由如下的旋转变换公式计算出，其中θ、γ分别指方位、俯仰、横滚角，这里/>θ＝θ_t、γ＝γ_t。方位、横滚旋转正方向遵循右手准则，俯仰正方向遵循左手准则。

同理，矢量在惯导坐标系下的值可由下式解得：

[x_2B",y_2B",z_2B"]^T＝A₁ ^-1[0,1,0]^T…………………………8)

矢量 在北西天坐标系下的值可由下式解得：

A₂由公式7)计算出，这里θ＝p、γ＝t。

这样，对于某一任意给出的一组θ_t、γ_t，均可以计算出/> 根据调平约束/>在北西天坐标系下的z分量为0。则可以通过逐次细分搜索确定θ_t、γ_t的值，从而解出调平状态下的/>这样基准/>的北向数据就传递到了/>上，即在底座倾斜的情况下，完成了北向基准的修正。

(3)在二维搜索空间上的基于多次细分搜索的数值计算逼近算法

首先令γ_t＝0，计算到/>的旋转过程，对θ_t逐次细分搜索。首次搜索是将搜索域(-w,w)(这里w是调平装置调平范围，对于本优选实施例，w为21度)划分为2n等份，在2n+1个划分点上计算，寻找使/>的z分量绝对值最小的点p。首轮搜索完成后，定义初始搜索步长/>这里，k为略小于1的数，用于使搜索边界产生一定程度叠加，避免错误收敛。

以首轮搜索出的点p为中心，对p+gap、p、p－gap点计算，寻找使的z分量绝对值最小的点更新p，调整搜索步长/>这里，k为略小于2的数。重复该步骤，不断减小gap，直到达到指定精度。θ_t确定后，则第二旋转轴矢量得以确定。

然后，用同样方法，计算到/>的旋转过程，对γ_t逐次细分搜索，完成求解过程。

在本实施例下，按照本发明提出的步骤4～7即可计算出目标的地理坐标。

综上，本发明属于目标定位技术领域，具体涉及一种基于正交轴系调平装置北向基准修正的目标定位方法，该方法针对采用了自动调平装置的目标定位***，利用向量的空间旋转变换，基于自动调平装置轴系结构约束，按照调平面与水平面平行的边界条件建立一系列数学计算方法，能够在不测量自动调平装置轴系旋转角度的情况下从自动调平装置下部北向基准准确解算出调平台调平后的上部北向基准，通过这一北向基准修正过程，该方法解决了此类目标定位***传统的目标定位方法不做北向基准修正，直接引用自动调平装置下部北向基准所产生的倾斜工况下目标定位误差增大的问题，在各种倾斜条件下均可获得一致的精度。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (10)

1.一种基于正交轴系调平装置北向基准修正的目标定位方法，其特征在于，所述目标定位方法包括具体步骤如下：

步骤1：操作两轴转台对目标测距，获取目标在观测坐标系x0y0z₀下的坐标；

操作两轴转台旋转，使叠加在传感器上的十字分划交点压住目标，触发激光测距，获取目标距离d；纪录激光测距触发时刻的两轴转台的方位角度俯仰角度θ₀；按照下式计算出目标在观测坐标系下的坐标：

步骤2：获取北向基准；

纪录激光测距触发时刻的惯导姿态角，包括方位角a，俯仰角p，横滚角r；

步骤3：将北向基准传递到观测坐标系；

即基于惯导姿态角(a，p，r)、调平基座相对惯导的方位回转角求解观测坐标系相对于北西天坐标系的方位旋转量a₀；

这一步骤的方法包括：环节1：选择初始矢量；环节2：计算初始矢量绕指定矢量旋转指定角度后的矢量方向；环节3：在二维搜索空间上的基于多次细分搜索进行数值逼近运算；

步骤4：获取观测点经纬度、高程；

通过卫星定位或惯性导航测量手段，获得观测点经纬度(L₀,B₀)、高程h₀；

步骤5：将目标在观测坐标系下的坐标转换到北西天坐标系x₁y₁z₁；

按照以下公式确定观测坐标系到北西天坐标系的变换关系；

[x₁,y₁,z₁,1]^T＝A^-1[x₀,y₀,z₀,1]^T………………………………2)

式中，旋转变换矩阵

步骤6：将目标在北西天坐标系x₁y₁z₁下的坐标转换到地心坐标系x₃y₃z₃；

建立过渡坐标系x_by_bz_b，其原点与北西天坐标系一致，x轴方向为天空方向，即北西天坐标系的z轴方向，y轴方向为东向，即北西天坐标系的y轴负方向，z轴方向为北向，即北西天坐标系的x轴方向；

按照以下公式将观测点经纬度、高程数据转换为地心坐标系下的直角坐标量(X,Y,Z)；

式中，

按照以下公式确定北西天坐标系相对与地心坐标系下的变换关系；

[x₃,y₃,z₃,1]^T＝A^-1[x_b,y_b,z_b,1]^T

即[x₃,y₃,z₃,1]^T＝A^-1[z₁,-y₁,x₁,1]^T.............................4)式中，

步骤7：将目标的地心坐标(x₃,y₃,z₃)转换为经纬度(L,B,h)；

按照下式计算：

其中：B的计算采用递归算法求解；

至此，得到了目标的地理坐标(L,B,h)。

2.如权利要求1所述基于正交轴系调平装置北向基准修正的目标定位方法，其特征在于，所述观测坐标系x₀y₀z₀的建立为：以两轴转台架设位置为原点，转台零位的瞄线方向为x轴，转台方位轴方向为z轴，建立观测坐标系x₀y₀z₀。

3.如权利要求1所述基于正交轴系调平装置北向基准修正的目标定位方法，其特征在于，所述步骤3中，所述选择初始矢量，即在调平基座未旋转，且底座平面与调平面平行时的自动调平装置初始状态下，在自动调平装置的底座平面、调平面上分别选择确定的沿结构边线的单位矢量其中/>是需要传递的北向基准矢量，保证这两条选定的结构边线平行，即/>

再选择绕底座平面法线旋转90度的矢量，令该矢量为/>显然，在自动调平装置初始状态下，/>绕调平面法线旋转90度形成的矢量/>对应的结构边线与/>对应的结构边线平行，即/>这样，对于自动调平装置的任意位置，/>都是/>经过特定旋转变换T的结果，该旋转变换是依次按照第1旋转轴/>旋转θ角度，再绕第2旋转轴/>旋转γ角度的两次旋转变换的顺序组合；同理，对于自动调平装置的任意位置，/>都是/>经过旋转变换T的结果；为了统一基准，统一在北－西－天坐标系下进行计算，此处选择右手系坐标系，故xyz轴分别对应北西天方向；

惯导输出的方位角a、俯仰角p、横滚角r是指在北－西－天坐标系下按照zyx顺规旋转的三个角度量，即首先绕z轴旋转，再绕y轴旋转，再绕x轴旋转，方位角即北向角；由于惯导与自动调平装置底座固连，则在北－西－天坐标系中的值可通过由(a，p，r)确定的旋转变换求解出来；而/>则需再进行一次绕/>轴旋转θ的旋转变换求解出来；这样，旋转变换T就由(a，p，r，θ，γ)五个参量确定；

由于θ，γ角度未知，但旋转变换T满足调平条件：的z分量为0；

这样，通过在二维空间基于多次细分搜索方式，对θ，γ的各种取值进行数值逼近运算，获得满足调平条件的θ，γ的数值解，从而求出旋转变换T，这样，就能够计算出在该工况下的从而得到a₀，即完成了北向基准传递。

4.如权利要求1所述基于正交轴系调平装置北向基准修正的目标定位方法，其特征在于，所述步骤3中，所述环节1的选择初始矢量，具体为：

在惯导坐标系x₂y₂z₂下,定义向量即沿惯导坐标系x轴正方向的单位矢量，定义向量即自动调平装置底座平面纵向结构边线方向的单位矢量，定义向量/>即自动调平装置调平面纵向结构边线方向的单位矢量；

在调平基座未旋转，且底座平面与调平面平行时， 为/>绕惯导坐标系z轴旋转/>得到；/>为/>经调平过程的依次绕第1、第2旋转轴的两次旋转后得到；

在惯导坐标系x₂y₂z₂下,定义向量为(1,0,0)；

在惯导坐标系的xy平面上选择与垂直的矢量/>对应的，向量/>即自动调平装置底座平面横向结构边线方向的单位矢量，向量/>即自动调平装置调平面横向结构边线方向的单位矢量；

同样的，在调平基座未旋转，且底座平面与调平面平行时，为/>绕惯导坐标系z轴旋转/>得到；/>为/>经调平过程的依次绕第1、第2旋转轴的两次旋转后得到；

因此，从惯导坐标系获得的北向的基准传递过程包括1次方位旋转，以及调平过程引起的依次绕自动调平装置的两个正交轴的两次旋转；在调平基座未旋转，且底座平面与调平面平行时，上述方位旋转轴矢量为(0,0,1)，调平过程第1旋转轴矢量为(0,1,0)，第2旋转轴矢量为(1,0,0)；

在任意工况下，自动调平装置调平基座绕方位旋转轴进行了的方位回转，自动调平装置执行调平操作的过程是一个依次绕第1、第2旋转轴依次旋转θ_t、γ_t的旋转变换，调平面上的/>分别就是/>经过上述旋转变换后的矢量。

5.如权利要求4所述基于正交轴系调平装置北向基准修正的目标定位方法，其特征在于，所述步骤3中，所述环节2的计算初始矢量绕指定矢量旋转指定角度后的矢量方向，具体为：

矢量在惯导坐标系下的值可由下式解得：

[x_2A",y_2A",z_2A"]^T＝A₁ ^-1[1,0,0]^T…………………………6)

旋转变换矩阵A₁由如下的旋转变换公式计算出，其中θ、γ分别指方位、俯仰、横滚角，这里/>θ＝θ_t、γ＝γ_t；方位、横滚旋转正方向遵循右手准则，俯仰正方向遵循左手准则；

同理，矢量在惯导坐标系下的值可由下式解得：

[x_2B",y_2B",z_2B"]^T＝A₁ ^-1[0,1,0]^T…………………………8)

矢量在北西天坐标系下的值可由下式解得：

A₂由公式7)计算出，这里θ＝p、γ＝t；

这样，对于某一任意给出的一组θ_t、γ_t，均可以计算出/> 根据调平约束在北西天坐标系下的z分量为0；则可以通过逐次细分搜索确定θ_t、γ_t的值，从而解出调平状态下的/>这样基准/>的北向数据就传递到了/>上，即在底座倾斜的情况下，完成了北向基准的修正。

6.如权利要求5所述基于正交轴系调平装置北向基准修正的目标定位方法，其特征在于，所述步骤3中，所述环节3的在二维搜索空间上的基于多次细分搜索进行数值逼近运算，具体如下：

首先令γ_t＝0，计算到/>的旋转过程，对θ_t逐次细分搜索；首次搜索是将搜索域(-w,w)划分为2n等份，在2n+1个划分点上计算，寻找使/>的z分量绝对值最小的点p；首轮搜索完成后，定义初始搜索步长：

这里，k为略小于1的数，用于使搜索边界产生一定程度叠加，避免错误收敛；

以首轮搜索出的点p为中心，对p+gap、p、p－gap点计算，寻找使的z分量绝对值最小的点更新p，调整搜索步长：

这里，k为略小于2的数；重复该步骤，不断减小gap，直到达到指定精度；θ_t确定后，则第二旋转轴矢量得以确定；

然后，用同样方法，计算到/>的旋转过程，对γ_t逐次细分搜索，完成求解过程。

7.如权利要求6所述基于正交轴系调平装置北向基准修正的目标定位方法，其特征在于，所述环节3中，w是调平装置调平范围。

8.如权利要求1所述基于正交轴系调平装置北向基准修正的目标定位方法，其特征在于，所述步骤3中，是需要传递的北向基准矢量。

9.如权利要求1所述基于正交轴系调平装置北向基准修正的目标定位方法，其特征在于，所述步骤3中，运算过程中，选择右手系坐标系，故xyz轴分别对应北西天方向。

10.如权利要求1所述基于正交轴系调平装置北向基准修正的目标定位方法，其特征在于，所述步骤3中，按照zyx顺规旋转，表示：首先绕z轴旋转，再绕y轴旋转，再绕x轴旋转。