专利名称：：基于双轴光电测量系统的地面车位置、姿态及航向测量方法
技术领域：
：本发明涉及地面车快速精确定位、定向、测姿方法，特别涉及利用光电测量系统的地面车快速精确定位定向技术，属于导航定位
技术领域：
。
背景技术：
：陆地导航系统(LNS，LandNavigationSystem)已成为世界上发达国家陆军装备现代化的一个重要组成部分。LNS是信息化战场实现体系对抗的基本要素，它不仅可以有效地提高路地战车的作战精度，还可以极大地提高战车的快速反应能力和作战的机动性、自主性。地面车LNS要解决的核心问题是准确快速的定位定向、姿态感知的技术。目前陆地战车LNS大多数采用GPS和惯性导航系统(INS)或航向参考系统(DR)构成的组合导航系统来实现导航定位。该方法存在的缺陷是在GPS受干扰或者战时不能使用GPS的情况下，INS/DR系统由于随时间累计的误差而不能满足陆地战车所需要的定位定向及姿态精度。在现有陆地导航系统定位定向精度不能满足特定任务要求的情况下，利用光电探测系统的载体位置、姿态和航向感知技术可以很好的解决地面车的定位定向及姿态感知问题。现有基于光电探测系统的地面车精确定位定向方法(申请号200710063640.7)是一种基于光电测量系统单轴(航向轴)测量信息的算法，存在以下不足-无法解算得到地面车辆的姿态(横滚、俯仰角)和高度信息。算法建立在平面模型基础上，实际应用中无法保证地面车水平，车体的姿态角会造成光电探测系统航向角测量耦合误差，从而影响最终定位/定向精度。
发明内容本发明要解决的技术问题是针对现有陆地导航系统和基于单轴光学探测系统的地面车快速定位定向算法存在的问题，发明了一种基于双轴光电测量系统的地面车快速位置、姿态、航向测量方法。基于双轴光电测量系统的地面车快速位置、姿态、航向测量方法(后简称发明)，其
发明内容主要包括1、发明提出车辆上装有惯导系统INS和双轴光电测量系统。车辆驶入指定发射区域后，利用双轴光电测量系统光轴依次瞄准三个已知位置靶标中心，利用双轴光电测量系统可以测得此时光轴矢量在载体坐标系的方位角和俯仰角。利用光电测量系统测量的载体坐标系的方位角和俯仰角信息，惯导系统提供的车体初始姿态信息快速完成地面车位置、姿态、航向的测量。2、发明提出原始双轴光电测量系统测量信息的修正算法。利用惯导提供的车体初始姿态信息修正1中测量得到的载体坐标系下方位、俯仰角信息，得到锁定光轴矢量在近似地理坐标系下的方位角和俯仰角。3、发明提出车体姿态角、高度解算方法。发明提出利用2中计算得到的修正后方位角，耙标的位置(含经度、纬度)、车体初始位置/航向计算得到车体的位置误差和航向误差，并利用得到的位置/航向误差修正车体初始位置/航向。发明提出利用2中计算得到的修正后俯仰角、靶标的位置(含经度、纬度、高度)、车体的初始高度、前面得到修正后的车体位置/航向计算得到车体的姿态角误差和高度误差，并利用得到的高度/姿态误差修正车体初始高度/姿态。4、发明提出修正、迭代计算方法，修正迭代计算方法是算法实现高精度解算的关键。修正迭代计算方法为当3计算得到误差修正量未达到指定精度(大于预设的阈值)，由3计算得到的修正后的车体位置、姿态和航向作为初值，重复2、3、4进行迭代解算；当3计算得到的误差修正量达到指定精度(小于等于预设的阈值)，输出修正后的车体位置、姿态、航向作为地面车精确的位置、姿态、航向信息。发明的特点主要包括本发明提出利用双轴光电测量系统。双轴光电测量系统可以同时测量靶标相对车体坐标系的方位角、俯仰角信息。本发明提出了利用车体初始姿态对车体系下的方位角、俯仰角测量信息进行修正，得到近似地理坐标系内的方位角、俯仰角信息。利用近似地理系下的方位角进行误差三角形法解算，可以避免有车体姿态耦合造成车体位置、航向解算误差。本发明提出利用近似地理坐标系下的俯仰角测量信息、航向角信息、位置信息，计算车体高度、姿态角的算法。本发明提出了以惯导位置/姿态/航向作为初始值的迭代计算方法。迭代算法中把载体系下的测量信息转换到近似地理坐标系，利用近似地理系下的角度测量信息进行位置/姿态/航向的解算，可以保证位置/姿态/航向算法始终工作在小角度线形区域，并且通过不断修正最终做到精确的解算，避免了由于姿态算法线形化近似带来的误差，同时也避免了由于姿态误差造成的位置/航向计算误差。图1为发明实际应用情景示意图2为基于平面误差三角形的快速定位定向算法示意图3为发明中双轴光电测量系统光轴矢量方位角、俯仰角测量原理示意图4为发明中地面车位置、姿态、航向快速测量算法流程示意图。具体实施方式-地面车上装有导航系统INS或DR系统和双轴光电测量系统，发射场安放三个位置精确的靶标。INS或DR系统可以提供车辆粗略位置和姿态/航向信息(Am、％—鹏、、鹏、y。一鹏、《_鹏、几_鹏)。发射场三个靶标精确位置已知(々、A、A)。地面车辆驶入指定定位区域(耙标构成的三角形中心区域，如图1所示)，本发明算法实施主要分为初值给定、光学测量、迭代解算三部分*利用双轴光电探测系统对三个靶标进行瞄准测量首先由车体初始位置/航向信息(^—ws、p。鹏、/y鹏、^_鹏)、靶标的位置信息(zl,、p,、A)通过公式(1)计算第一个靶标相对光轴的角度(",/')，用于自动转动光轴，靶标点进入光电探测系统的视野，此时可以采用人工或自动方式对目标进行瞄准(保证目标中心位于视场中心)，双轴光电探测平台此时可以测量出光轴矢量相对车体坐标系的方位/俯仰角(",-)e，如图3所示。"=tan(-^-r=~)—W0鹏(仍1o鹏)一(1)"={[W—鹏)cosO。—鹏)]+—外—鹏)}ie〃￡)其中A为地球半径。重复上过程完成三个靶标的测量，得到一组测量信息(a,，A)c。*利用上步的测量信息解算车体位置、姿态、航向解算车体位置/姿态/航向需要的信息包括>初始位置/姿态/航向鹏、炉。_,船、&_鹏、(^_鹏、《—鹏、r。—腦)>靶标位置信息(4、A、>角度测量信息(a,，A)c经过测量得到上信息后，通过本发明提出的算法可以精确解算出车体的位置/姿态/航向。算法解算过程如附图4所示可分为五步，具体如下第一步计算初值车体的位置/航向/姿态",％,4,^,《,/。)赋值为车载惯导给出的(Vm、％_鹏、V鹏、/ws、《o」ws、6—/船)。第二步:将车体系下的测量信息转化到近似地理坐标系利用光电平台锁定靶标i时得到的测量航向、俯仰角为(",，A)e，车体坐标系的姿态角信息(W。、《、y0)。利用下公式计算近似地理坐标系下的角度测量信息(",,A》-0、「100、〈cos;r00sin/0、cosAcos",00cos《—sin<90010cosAcos;、ool乂(osingcos6>0,0COS",上公式右边全部为己知测量量，通过下公式可以取得0、(100、'cos700sin;K0、一cos/,sinor,cosy000cos《010cos/,cosa,=、o00、0sin<90'cos《。y0cosvc3由下公式计算得到近似地理坐标系下的测量(a,.，A)e:A=sin-'(C3);,=tan-'(CVq);第三步平面解算车体位置、航向由上步计算得到的近似地理坐标系下的测量信息(",，A)e，靶标的位置(4、p,)，车体的初始位置/姿态/航向(;io，外,A)，^，《,;0,通过平面误差三角形法解算地面车的位置(义，W和航向误差A^。。平面误差三角形解法为"基于光学探测系统的地面车定位定向法"(200710063640.7)提出，关于误差三角形解算的详细说明见附录。利用计算得到的(/^,A^)按照下公式更新车体的位置/姿态/航向d^A)，^AJ。)。A0=义P0=Py0,+A^0第四步:立体解算车体高度、姿态将上步更新后的车体坐标系位置、姿态，靶标的位置信息、近似地理系下俯仰角。0,<^0，^,^0,^),/0，；1/，^.，力/,^;)带入公式(5)取1=1，2，3，可得关于A&、△《A^。的线性方程组，据此可计算车体的高度、姿态角误差(AApA6。，A;g。——~AA0-cos^/0A<90+sin《y0A;K0=-加-1(^~~^")十(5)其中《为靶标到车的距离，计算公式为《={-;gcos(p。)]2+("-%)2}1/2&。由计算得到的位置高度、横滚角、俯仰角误差(AA,A《，A7。)修正车体坐标系",外，A)，W《'^)。第五步:将修正后的位置姿态(^,^，4,^,《,0代入第二步中迭代解算，直至修正误差量达到预定精度，得到车体精确的位置、姿态和航向"，外，4,v/。,《，;g。具体实施例子为了验证本发明提出的基于双轴光学探测系统的地面车快速位置、姿态、航向测量方法的可行性，利用北航研制的双轴光学探测系统，在北京十三陵水库的河滩上进行了本发明的验证实验。双轴光学探测系统的角度测量精度为30"，实验中靶标的位置精度为5cm。实验中过程中车体保持位置/姿态/航向不动，通过不同靶标的组合下的结算结果的重复性度量算法精度。实验结果如下表所示-<table>tableseeoriginaldocumentpage7</column></row><table>从结果可以看出，本专利提出的算法(在现有实验条件下)位置误差优于0.5m，航向角/姿态角的解算误差优于30"。实验结果验证了本发明所诉的方法是有效的，并且可以达到很高的解算精度，有广阔的应用前景。最后所应说明的是所有的不脱离本发明基本思路和范围内的修改或局部替换，均应涵盖在本发明的权利要求范围中。附录l基于平面误差三角形的快速定位定向算法地面车停放位置为0，该点地理北向为0N，如下图所示。A、B、C为靶标。地面车在0时，车载设备给的车体纵轴北向偏角误差为A^。光电探测平台在地理系水平面内测量到的靶标方位角为a,。如附图2所示，过A点做平行于北向的直线AD。再过A点作直线^4',该直线与AD间的夹角为^，,，艮P:Za"'=^ml。如果导航设备给出的北向误差角A^-0，那么直线^4'和AO重合。同理，过B、C点分别作直线朋'和CC'，三条直线A4'、和CC'两两相交，形成三角形乂万'C'，这个三角形称为误差三角形，误差三角形的大小与误差角A^的大小有关。设A^的最大值为A^^。分别调整Z"^'及直线S^和CC'对应的角度，使其在h^-Av^x,l,+A^皿]之间对应连续变化，每次变化的角度增量为^^，其中n为角度分划数，可根据系统的定向精度要求取。每取一组角度值，由相应的直线^4'、s万'和cc'形成一个误差三角形，而地面车的真实地理坐标(x,少)在该三角形所确定的区域内。当取某组角度值时，得到的误差三角形的边长之和最小，此时得到的ZDA4'、Z/)5&和^DCC'即可认为是靶标相对于车体的真实方位角，对比初始时刻车载设备给出的北向，就可以得出北向误差，同时得到地面车的平面位置。权利要求1、基于双轴光电测量系统的地面车位置、姿态、航向测量方法，其特征在于步骤如下(1)车辆上装有惯导系统和双轴光电测量系统，车辆驶入指定定位区域，惯导系统为车辆提供位置、姿态、航向初始值；(2)利用双轴光电测量系统分别锁定三个已知位置靶标，在锁定靶标时，测出三个光轴矢量相对车体坐标系的方位角和俯仰角；(3)利用惯导系统提供的车体初始姿态将步骤(2)中测量的车体坐标系下光轴矢量方位、俯仰角信息转化到近似地理坐标系下，得到三个光轴矢量在近似地理坐标系下的方位角和俯仰角；(4)由上述三个靶标的位置和光电测量系统锁定靶标时光轴矢量在近似地理坐标系下的方位角计算惯导系统提供的车体位置和航向误差；由靶标位置、车体位置和航向及光电测量系统锁定靶标时光轴矢量在近似地理坐标系下的俯仰角计算惯导系统提供的车体姿态和高度误差；(5)当步骤(4)所述车体位置、车体姿态、航向误差和高度误差修正量小于等于某一预先设定的阈值时，首先用(4)计算得到的所述车体位置和航向误差以及所述车体姿态和高度误差修正车体位置、姿态和航向，然后输出车体精确的位置、姿态和航向信息；当步骤(4)所述误差修正量大于某一预先设定的阈值时，首先用(4)计算得到的所述车体位置和航向误差以及所述车体姿态和高度误差修正车体位置、姿态和航向，并以修正后的位置、姿态和航向作为初始值，重复步骤(3)(4)。2、根据权利要求l所述的基于双轴光电测量系统的地面车位置、姿态、航向测量方法，其特征在于所述的双轴光电测量系统测量维度为载体的方位、俯仰两个轴向。3、根据权利要求l所述的基于双轴光电测量系统的地面车位置、姿态、航向测量方法，其特征在于为了达到高精度的计算结果，方法中经过步骤(3)，将光学系统测量信息变换到近似地理坐标系，得到锁定靶标时光轴矢量在近似地理坐标系下的方位角、俯仰角，从而抑制了由于车体停放不平造成的算法误差。4、根据权利要求1所述的基于双轴光电测量系统的地面车位置、姿态、航向测量方法，其特征在于为了达到高精度的计算结果，方法中采用了步骤(5)中所述的修正、迭代计算方法。5、根据权利要求l所述的基于双轴光电测量系统的地面车位置、姿态、航向测量方法，其特征在于所述步骤(4)中提出的姿态和高度计算过程，采用了将非线性方程组在真值附近线性化的方法。全文摘要基于双轴光电测量系统的地面车位置、姿态和航向测量方法车辆上装有惯导系统和双轴光电测量系统，车辆驶入指定区域，惯导系统提供车辆位置、姿态和航向初始值；利用双轴光电测量系统锁定三个已知位置靶标，测出此时光轴矢量相对车体坐标系的方位角和俯仰角；利用惯导提供的初始位置、姿态和航向及双轴光电测量系统测量的三组方位角和俯仰角信息，通过特定的算法计算得到车辆的精确位置、姿态和航向。本发明实施简单、快速、精确，并且可以为多个车辆同时服务，是地面车位置、姿态、航向快速精确测量的新途径。文档编号G01C21/28GK101672655SQ20091009315公开日2010年3月17日申请日期2009年9月25日优先权日2009年9月25日发明者冯培德,孟庆季,魁李,钟海波申请人:北京航空航天大学