CN115950419A - Combined navigation method, device and system for subminiature unmanned aerial vehicle - Google Patents

Combined navigation method, device and system for subminiature unmanned aerial vehicle Download PDF

Info

Publication number
CN115950419A
CN115950419A CN202211607090.1A CN202211607090A CN115950419A CN 115950419 A CN115950419 A CN 115950419A CN 202211607090 A CN202211607090 A CN 202211607090A CN 115950419 A CN115950419 A CN 115950419A
Authority
CN
China
Prior art keywords
information
navigation
unmanned aerial
aerial vehicle
coordinate system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211607090.1A
Other languages
Chinese (zh)
Inventor
刘宁
袁超杰
戚文昊
董一平
刘福朝
苏中
赵旭
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Information Science and Technology University
Original Assignee
Beijing Information Science and Technology University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Information Science and Technology University filed Critical Beijing Information Science and Technology University
Priority to CN202211607090.1A priority Critical patent/CN115950419A/en
Publication of CN115950419A publication Critical patent/CN115950419A/en
Pending legal-status Critical Current

Links

Images

Landscapes

  • Navigation (AREA)

Abstract

The application discloses a combined navigation method, a device and a system for a subminiature unmanned aerial vehicle. Wherein, the method comprises the following steps: acquiring acceleration information and angular velocity information of the unmanned aerial vehicle through an inertial measurement unit, and performing attitude calculation based on the acceleration information and the angular velocity information to obtain pose information of the unmanned aerial vehicle; receiving satellite differential correction information sent by a ground station at a preset frequency, and correcting the pose information by using the satellite differential correction information; navigating the unmanned aerial vehicle based on the corrected pose information. The method and the device solve the technical problems of discrete inertia and inaccurate satellite navigation in the related technology.

Description

超小型无人机用组合导航方法、装置及系统Combined navigation method, device and system for ultra-small unmanned aerial vehicle

技术领域Technical Field

本申请涉及导航领域,具体而言,涉及一种超小型无人机用组合导航方法、装置及系统。The present application relates to the field of navigation, and more specifically, to a combined navigation method, device and system for ultra-small unmanned aerial vehicles.

背景技术Background Art

现代卫星导航系统两次定位之间的时间间隔长,每次定位还需要10分钟以上的跟踪,不能连续提供无人机位置信息,因此,会常与惯性导航系统组合。The time interval between two positionings of modern satellite navigation systems is long, and each positioning requires more than 10 minutes of tracking. It cannot provide drone location information continuously, so it is often combined with an inertial navigation system.

采用卫星全球定位系统能使无人机在任何地区实时得到位置和速度信息。但无人机在作剧烈机动动作时或当卫星全球定位系统信噪比低时,导航精度将大为降低。而在惯性导航设备和卫星导航设备精度确定的情况下,分立式结构很难提高惯性/卫星组合导航的定位精度。The use of satellite global positioning system can enable drones to obtain real-time position and speed information in any area. However, when drones make violent maneuvers or when the signal-to-noise ratio of the satellite global positioning system is low, the navigation accuracy will be greatly reduced. When the accuracy of inertial navigation equipment and satellite navigation equipment is determined, it is difficult to improve the positioning accuracy of inertial/satellite combined navigation with a discrete structure.

针对上述的问题,目前尚未提出有效的解决方案。To address the above-mentioned problems, no effective solution has been proposed yet.

发明内容Summary of the invention

本申请实施例提供了一种超小型无人机用组合导航方法、装置及系统,以至少解决分立式的惯性、卫星导航不精确的技术问题。The embodiments of the present application provide a combined navigation method, device and system for an ultra-small unmanned aerial vehicle, so as to at least solve the technical problem of inaccurate discrete inertial and satellite navigation.

根据本申请实施例的一个方面,提供了一种超小型无人机用组合导航方法,包括:通过捷联惯性测量单元获取所述无人机的加速度信息和角速度信息,并基于所述加速度和角速度信息进行姿态解算,得到所述无人机的位姿信息;接收地面站以预设的频率发送的卫星差分修正信息,并利用所述卫星差分修正信息,来修正所述位姿信息;基于修正后的所述位姿信息,来对所述无人机进行导航。According to one aspect of an embodiment of the present application, a combined navigation method for an ultra-small unmanned aerial vehicle is provided, comprising: acquiring acceleration information and angular velocity information of the unmanned aerial vehicle through a strapdown inertial measurement unit, and performing attitude calculation based on the acceleration and angular velocity information to obtain the position and posture information of the unmanned aerial vehicle; receiving satellite differential correction information sent by a ground station at a preset frequency, and using the satellite differential correction information to correct the position and posture information; and navigating the unmanned aerial vehicle based on the corrected position and posture information.

根据本申请实施例的另一方面,还提供了一种超小型无人机用组合导航装置,包括:捷联解算模块,被配置为通过捷联惯性测量单元获取所述无人机的加速度信息和角速度信息,并基于所述加速度和角速度信息进行姿态解算,得到所述无人机的位姿信息;修正模块,被配置为接收地面站以预设的频率发送的卫星差分修正信息,利用所述卫星差分修正信息,来修正所述位姿信息;导航模块,被配置为基于修正后的所述位姿信息,来对所述无人机进行导航。According to another aspect of an embodiment of the present application, a combined navigation device for an ultra-small unmanned aerial vehicle is also provided, including: a strapdown solution module, configured to obtain acceleration information and angular velocity information of the unmanned aerial vehicle through a strapdown inertial measurement unit, and perform attitude solution based on the acceleration and angular velocity information to obtain the posture information of the unmanned aerial vehicle; a correction module, configured to receive satellite differential correction information sent by a ground station at a preset frequency, and use the satellite differential correction information to correct the posture information; a navigation module, configured to navigate the unmanned aerial vehicle based on the corrected posture information.

根据本申请实施例的又一方面,还提供了一种超小型无人机用组合导航系统,包括:捷联惯性测量单元,被配置为获取所述无人机的加速度信息和角速度信息;卫星差分定位系统,被配置为以预设的频率发送的卫星差分修正信息;所述捷联惯性测量单元包括导航计算机,所述导航计算机为如上所述的超小型无人机用组合导航装置。According to another aspect of the embodiment of the present application, a combined navigation system for an ultra-small unmanned aerial vehicle is also provided, including: a strapdown inertial measurement unit, configured to obtain acceleration information and angular velocity information of the unmanned aerial vehicle; a satellite differential positioning system, configured to send satellite differential correction information at a preset frequency; the strapdown inertial measurement unit includes a navigation computer, and the navigation computer is the combined navigation device for the ultra-small unmanned aerial vehicle as described above.

在本申请实施例中,接收地面站以预设的频率发送的卫星差分修正信息,并利用所述卫星差分修正信息,来修正所述位姿信息;基于修正后的所述位姿信息,来对所述无人机进行导航,从而解决了分立式的惯性、卫星导航不精确的技术问题。In an embodiment of the present application, satellite differential correction information sent by a ground station at a preset frequency is received, and the satellite differential correction information is used to correct the posture information; based on the corrected posture information, the UAV is navigated, thereby solving the technical problem of inaccurate discrete inertial and satellite navigation.

附图说明BRIEF DESCRIPTION OF THE DRAWINGS

此处所说明的附图用来提供对本申请的进一步理解,构成本申请的一部分,本申请的示意性实施例及其说明用于解释本申请,并不构成对本申请的不当限定。在附图中:The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

图1是根据本申请实施例的一种超小型无人机用组合导航方法的流程图;FIG1 is a flow chart of a combined navigation method for an ultra-small UAV according to an embodiment of the present application;

图2A是根据本申请实施例的另一种超小型无人机用组合导航方法的流程图;FIG2A is a flow chart of another combined navigation method for ultra-small UAV according to an embodiment of the present application;

图2B是根据本申请实施例的地理坐标系的示意图;FIG2B is a schematic diagram of a geographic coordinate system according to an embodiment of the present application;

图2C是根据本申请实施例的松耦合结构的示意图;FIG2C is a schematic diagram of a loosely coupled structure according to an embodiment of the present application;

图3是根据本申请实施例的卡尔曼滤波误差修正方法的流程图;FIG3 is a flow chart of a Kalman filter error correction method according to an embodiment of the present application;

图4是根据本申请实施例的捷联惯性测量单元系统组成示意图;4 is a schematic diagram of the composition of a strapdown inertial measurement unit system according to an embodiment of the present application;

图5是根据本申请实施例的惯性测量组件的硬件设计电路图;5 is a hardware design circuit diagram of an inertial measurement assembly according to an embodiment of the present application;

图6是根据本申请实施例的导航计算机的硬件设计电路图;6 is a hardware design circuit diagram of a navigation computer according to an embodiment of the present application;

图7是根据本申请实施例的二次电源模块的硬件设计电路图;7 is a hardware design circuit diagram of a secondary power supply module according to an embodiment of the present application;

图8是根据本申请实施例的卫星接收机的硬件设计电路图。FIG8 is a hardware design circuit diagram of a satellite receiver according to an embodiment of the present application.

具体实施方式DETAILED DESCRIPTION

为了使本技术领域的人员更好地理解本申请方案,下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本申请一部分的实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都应当属于本申请保护的范围。In order to enable those skilled in the art to better understand the solution of the present application, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present application.

需要说明的是,本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象,而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换,以便这里描述的本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外,术语“包括”和“具有”以及他们的任何变形,意图在于覆盖不排他的包含,例如,包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元,而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。It should be noted that the terms "first", "second", etc. in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments of the present application described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device comprising a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

根据本申请实施例,提供了一种超小型无人机用组合导航方法,如图1所示,该方法包括:According to an embodiment of the present application, a combined navigation method for an ultra-small UAV is provided, as shown in FIG1 , the method comprising:

步骤S102通过捷联惯性测量单元获取所述无人机的加速度信息和角速度信息,并基于所述加速度和角速度信息进行姿态解算,得到所述无人机的位姿信息。Step S102 obtains acceleration information and angular velocity information of the UAV through a strapdown inertial measurement unit, and performs attitude calculation based on the acceleration and angular velocity information to obtain the position and posture information of the UAV.

首先,通过捷联惯性测量单元获取所述无人机的加速度信息和角速度信息之后,基于所述角速度信息,利用四元数法进行姿态解算,得到所述载体的姿态角。具体地,利用四元数来描述所述位姿信息,并基于所述描述确定弹体坐标系到地理坐标系的转换关系式与所述四元数之间的关系;利用角速度增量,基于所确定的所述转换关系式与所述四元数之间的关系,来求解描述所述位姿信息的所述四元数;基于所求解出的所述四元数,来进行姿态解算,得到所述载体的姿态角。First, after obtaining the acceleration information and angular velocity information of the UAV through the strapdown inertial measurement unit, the attitude is solved using the quaternion method based on the angular velocity information to obtain the attitude angle of the carrier. Specifically, the posture information is described using quaternions, and the relationship between the conversion relationship from the missile body coordinate system to the geographic coordinate system and the quaternion is determined based on the description; the quaternion describing the posture information is solved based on the determined relationship between the conversion relationship and the quaternion using the angular velocity increment; and the attitude is solved based on the solved quaternion to obtain the attitude angle of the carrier.

接着,基于所述加速度信息,进行捷联解算,得到所述载体的速度信息和位置信息。具体地,基于地速在惯性坐标系下的变化率、所述加速度信息中的比力矢量、以及地球的自转角速度,确定所述地速在导航坐标系的变化率;基于所述地速在导航坐标系的变化率,确定导航方程,并基于所述导航方程,确定在所述导航坐标系中所述载体的速度沿真北、东向和当地垂线方向的分量。例如,基于所述载体的经度和纬度的变化率,确定地理坐标系相对于地球固连坐标系的转动角速率;基于所述转动角速率,确定所述载体的当地重力矢量;基于由于重力异常造成的当地重力矢量方向相对于当地垂线方向的角度偏差、当前维度、当前经度和当前距地球表面的高度、以及所述当地重力矢量,确定在所述导航坐标系中所述载体的速度沿真北、东向和当地垂线方向的分量。Then, based on the acceleration information, a strapdown solution is performed to obtain the speed information and position information of the carrier. Specifically, based on the rate of change of the ground speed in the inertial coordinate system, the specific force vector in the acceleration information, and the angular velocity of the earth's rotation, the rate of change of the ground speed in the navigation coordinate system is determined; based on the rate of change of the ground speed in the navigation coordinate system, a navigation equation is determined, and based on the navigation equation, the components of the speed of the carrier in the true north, east and local vertical directions in the navigation coordinate system are determined. For example, based on the rate of change of the longitude and latitude of the carrier, the angular rate of rotation of the geographic coordinate system relative to the earth's fixed coordinate system is determined; based on the angular rate of rotation, the local gravity vector of the carrier is determined; based on the angular deviation of the local gravity vector direction relative to the local vertical direction caused by gravity anomaly, the current latitude, the current longitude and the current height from the earth's surface, and the local gravity vector, the components of the speed of the carrier in the navigation coordinate system along the true north, east and local vertical directions are determined.

步骤S104,接收地面站以预设的频率发送的卫星差分修正信息,并利用所述卫星差分修正信息,来修正所述位姿信息。Step S104, receiving satellite differential correction information sent by the ground station at a preset frequency, and using the satellite differential correction information to correct the posture information.

首先,基于所述卫星差分修正信息,利用卡尔曼滤波器来估计位置、速度和姿态的误差。具体地,将来自导航卫星系统的导航解的位置、速度和姿态作为量测信息输入到所述卡尔曼滤波器,作为初始估计值;所述卡尔曼滤波器在预测阶段,对所述初始估计值进行线性化处理,并基于线性化处理后的所述初始估计值来确定误差协方差;基于所述误差协方差来确定卡尔曼增益,并基于所述卡尔曼增益重新确定所述误差协方差;基于重新确定的所述误差协方差,来估计捷联解算的所述位置、速度和姿态的误差。First, based on the satellite differential correction information, the Kalman filter is used to estimate the errors of position, velocity and attitude. Specifically, the position, velocity and attitude of the navigation solution from the navigation satellite system are input into the Kalman filter as measurement information as initial estimation values; in the prediction stage, the Kalman filter linearizes the initial estimation values and determines the error covariance based on the initial estimation values after linearization; determines the Kalman gain based on the error covariance, and redetermines the error covariance based on the Kalman gain; estimates the errors of the position, velocity and attitude of the strapdown solution based on the redetermined error covariance.

然后,基于所估计出的误差,来修正所述位姿信息。Then, the pose information is corrected based on the estimated error.

步骤S106,基于修正后的所述位姿信息,来对所述无人机进行导航。Step S106: Navigate the UAV based on the corrected position information.

在本申请实施例中,接收地面站以预设的频率发送的卫星差分修正信息,并利用所述卫星差分修正信息,来修正所述位姿信息;基于修正后的所述位姿信息,来对所述无人机进行导航,从而解决了分立式的惯性、卫星导航不精确的技术问题In the embodiment of the present application, satellite differential correction information sent by a ground station at a preset frequency is received, and the satellite differential correction information is used to correct the posture information; based on the corrected posture information, the drone is navigated, thereby solving the technical problem of inaccurate discrete inertial and satellite navigation.

实施例2Example 2

根据本申请实施例,还提供了另一种超小型无人机用组合导航方法,如图2A所示,该方法包括:According to an embodiment of the present application, another combined navigation method for an ultra-small UAV is provided, as shown in FIG2A , the method comprising:

步骤S202,坐标系设计。Step S202: coordinate system design.

导航坐标系采用北-天-东坐标系。坐标原点取在飞行器质心,oxn轴指向北极,oyn轴垂直于当地水平面指向天,ozn轴指向东与oxn轴、oyn轴呈右手定则,如图2B所示。The navigation coordinate system adopts the north-sky-east coordinate system. The origin of the coordinate is taken at the center of mass of the aircraft, the ox n axis points to the North Pole, the oy n axis is perpendicular to the local horizontal plane and points to the sky, and the oz n axis points to the east in the right-hand rule with the ox n axis and the oy n axis, as shown in Figure 2B.

载体坐标系坐标原点在飞行器质心,oxb轴沿飞行器纵轴,指向飞行器头部为正,oyb轴在飞行器纵向对称面内,垂直oxb轴指向上为正,ozb轴与oxb轴及oyb轴呈右手定则,与载体固连。The origin of the carrier coordinate system is at the center of mass of the aircraft, the ox b axis is along the longitudinal axis of the aircraft, pointing to the head of the aircraft is positive, the oy b axis is in the longitudinal symmetry plane of the aircraft, perpendicular to the ox b axis and pointing upwards is positive, the oz b axis, the ox b axis and the oy b axis are in the right-hand rule, and are fixed to the carrier.

采用正欧拉可用真航向角ψN、俯仰角

Figure BDA0003997575740000053
滚转角γ来直接描述弹体坐标系到地理坐标系的转换关系式为:Using positive Euler, the true heading angle ψ N and pitch angle
Figure BDA0003997575740000053
The roll angle γ is used to directly describe the transformation relationship from the missile body coordinate system to the geographic coordinate system:

Figure BDA0003997575740000052
Figure BDA0003997575740000052

在使用正欧拉时地理坐标系和载体坐标系的俯仰角和滚转角定义相同,偏航角和真航向角的关系如下式:When using positive Euler, the pitch angle and roll angle of the geographic coordinate system and the carrier coordinate system are defined the same, and the relationship between the yaw angle and the true heading angle is as follows:

Figure BDA0003997575740000061
Figure BDA0003997575740000061

其中,A表示角度偏差。Wherein, A represents the angle deviation.

本实施例中,采用上述方式对坐标系进行转换,能够避免坐标转换时存在的误差,从而使得后续的导航解算更为准确。In this embodiment, the coordinate system is converted in the above manner, which can avoid errors in coordinate conversion, thereby making subsequent navigation solutions more accurate.

步骤S204,姿态解算。Step S204: posture calculation.

本实施采用四元数法,利用四个元素来描述全部位姿信息,四元数q可表示为:This implementation uses the quaternion method, using four elements to describe all posture information. The quaternion q can be expressed as:

q=a+bi+cj+dk (1)q=a+bi+cj+dk (1)

其中,a,b,c,d为四元数的实部,i、j、k为虚数单位。Among them, a, b, c, d are the real parts of the quaternion, and i, j, k are the imaginary units.

设有b系中矢量vb,其在n系中表示为vn,则:Suppose there is a vector v b in the b system, which is represented by v n in the n system, then:

Figure BDA0003997575740000062
Figure BDA0003997575740000062

式中q*=a-bi-cj-dk是四元数q的复共轭。Wherein q * =a-bi-cj-dk is the complex conjugate of the quaternion q.

由上式可得

Figure BDA0003997575740000063
与四元数之间的关系:From the above formula, we can get
Figure BDA0003997575740000063
Relationship with quaternions:

Figure BDA0003997575740000064
Figure BDA0003997575740000064

采用四元数姿态解算,需要求解如下方程:Using quaternion attitude solution, you need to solve the following equation:

Figure BDA0003997575740000065
Figure BDA0003997575740000065

其中p=[0,ωT],

Figure BDA0003997575740000066
表示四元数的微分形式,q表示四元数,角速度ω的分量为ωx、ωy、ωz。式(4)也可用矩阵形式表示:where p = [0, ω T ],
Figure BDA0003997575740000066
represents the differential form of the quaternion, q represents the quaternion, and the components of the angular velocity ω are ω x , ω y , ω z . Equation (4) can also be expressed in matrix form:

Figure BDA0003997575740000071
Figure BDA0003997575740000071

式中In the formula

Figure BDA0003997575740000072
Figure BDA0003997575740000072

由角增量计算解四元数,其解可表示为:The quaternion solution is calculated by the angle increment, and its solution can be expressed as:

Figure BDA0003997575740000073
Figure BDA0003997575740000073

其中,qk表示k时刻的四元数,tk积分计算时刻。Where q k represents the quaternion at time k, and t k represents the integral calculation time.

带入W,令Substitute W and let

Figure BDA0003997575740000074
Figure BDA0003997575740000074

最终可得解为:The final solution is:

Figure BDA0003997575740000075
Figure BDA0003997575740000075

式中,

Figure BDA0003997575740000076
T为系统采样周期,I为4x4的单位矩阵。In the formula,
Figure BDA0003997575740000076
T is the system sampling period, and I is the 4x4 identity matrix.

由此可通过四元数的求解得到姿态角,公式如下:Therefore, the attitude angle can be obtained by solving the quaternion. The formula is as follows:

Figure BDA0003997575740000081
Figure BDA0003997575740000081

θ=arcsin[-C31]=arcsin[2(bd-ac)]θ=arcsin[-C 31 ]=arcsin[2(bd-ac)]

Figure BDA0003997575740000082
Figure BDA0003997575740000082

式中矩阵C即为式(3)。The matrix C in the formula is formula (3).

本实施例中,采用上述方式解算姿态角,可以使得解算出的姿态角更为精确,从而为后续导航提供了精确的基础数据。In this embodiment, the above-mentioned method is used to solve the attitude angle, so that the solved attitude angle can be more accurate, thereby providing accurate basic data for subsequent navigation.

步骤S206,捷联解算。Step S206, strapdown solution.

导航方程可表示为如下形式:The navigation equation can be expressed as follows:

Figure BDA0003997575740000083
Figure BDA0003997575740000083

其中,r表示位置矢量,f表示比力,通过一次积分得到速度,二次积分得到位置。Among them, r represents the position vector, f represents the specific force, the velocity is obtained by the first integration, and the position is obtained by the second integration.

Figure BDA0003997575740000084
Figure BDA0003997575740000084

其中,vn表示导航坐标系速度。在地球上工作在当地地理坐标系中的导航系统,地速表示为

Figure BDA0003997575740000085
它相对于导航坐标系的变化率可通过其在惯性系下的变化率表示:Where v n represents the navigation coordinate system speed. For a navigation system working in the local geographic coordinate system on the earth, the ground speed is expressed as
Figure BDA0003997575740000085
Its rate of change relative to the navigation coordinate system can be expressed by its rate of change in the inertial system:

Figure BDA0003997575740000086
Figure BDA0003997575740000086

其中,

Figure BDA0003997575740000087
ve表地速,ωen表示导航坐标系相对于地球坐标系的转动角速率,ωie表示地球坐标系相对于惯性坐标系的转动角速率,g1=g-ωie×[ωie×r]代入式(13)则有:in,
Figure BDA0003997575740000087
v e represents the ground speed, ω en represents the rotational angular velocity of the navigation coordinate system relative to the earth coordinate system, ω ie represents the rotational angular velocity of the earth coordinate system relative to the inertial coordinate system, g 1 =g-ω ie ×[ω ie ×r] Substituting into formula (13), we have:

Figure BDA0003997575740000091
Figure BDA0003997575740000091

导航方程可表示为如下形式:The navigation equation can be expressed as follows:

Figure BDA0003997575740000092
Figure BDA0003997575740000092

速度沿真北、东向和当地垂线方向分量为:

Figure BDA0003997575740000093
其中,vN,vE,vD分别表示沿真北、东向和当地垂线方向的速度,
Figure BDA0003997575740000094
表示地球坐标系相对于惯性坐标系的转动角速率、
Figure BDA0003997575740000095
表示导航坐标系相对于地球坐标系的转动角速率、
Figure BDA0003997575740000096
表示地速。The components of velocity along true north, east and local vertical are:
Figure BDA0003997575740000093
Where, vN , vE , and vD represent the velocities along the true north, east, and local vertical directions, respectively.
Figure BDA0003997575740000094
represents the angular velocity of the Earth coordinate system relative to the inertial coordinate system,
Figure BDA0003997575740000095
Indicates the angular rate of rotation of the navigation coordinate system relative to the earth coordinate system,
Figure BDA0003997575740000096
Indicates ground speed.

fw是一组3个加速度计测量的比力矢量,分解到当地地理参考坐标系中为:fw is a set of specific force vectors measured by three accelerometers, decomposed into the local georeferenced coordinate system as:

fn=[fN fE fD]T (16)f n = [f N f E f D ] T (16)

Figure BDA0003997575740000097
是当地地理坐标系中地球的自转角速度:
Figure BDA0003997575740000097
is the angular velocity of the Earth's rotation in the local geographic coordinate system:

Figure BDA0003997575740000098
Figure BDA0003997575740000098

其中,L0表示,

Figure BDA0003997575740000099
表示当地地理坐标系相对于地球固连坐标系的转动角速率,即转移速率,其值可以用经度和纬度的变化率表示如下:Among them, L0 means,
Figure BDA0003997575740000099
It represents the angular rate of rotation of the local geographic coordinate system relative to the earth's fixed coordinate system, that is, the transfer rate. Its value can be expressed by the rate of change of longitude and latitude as follows:

Figure BDA00039975757400000910
Figure BDA00039975757400000910

使

Figure BDA00039975757400000911
其中,L表示纬度,得:make
Figure BDA00039975757400000911
Where L represents latitude, we get:

Figure BDA00039975757400000912
Figure BDA00039975757400000912

式(19)中,R0为地球半径;h为距地球表面高度。In formula (19), R0 is the radius of the earth; h is the height from the earth's surface.

Figure BDA00039975757400000913
是当地重力矢量,它由地球的质量引力(g)和地球转动产生的向心加速度(ωie×ωie×R)组成。因此,可以写成:
Figure BDA00039975757400000913
is the local gravity vector, which is composed of the mass gravitational force of the earth (g) and the centripetal acceleration caused by the rotation of the earth (ω ie ×ω ie ×R). Therefore, it can be written as:

Figure BDA0003997575740000101
Figure BDA0003997575740000101

其中,Ω表示地球自转角速率,导航方程可以表示为如下分量形式:Among them, Ω represents the angular rate of the earth's rotation, and the navigation equation can be expressed in the following component form:

Figure BDA0003997575740000102
Figure BDA0003997575740000102

Figure BDA0003997575740000103
Figure BDA0003997575740000103

Figure BDA0003997575740000104
Figure BDA0003997575740000104

式中:ξ、η为由于重力异常造成的当地重力矢量方向相对于当地垂线方向的第一角度偏差和第二角度偏差。Where: ξ, η are the first angle deviation and second angle deviation of the local gravity vector direction relative to the local vertical direction caused by gravity anomaly.

纬度、经度和距地球表面的高度由下列公式给出:Latitude, longitude, and altitude above the Earth's surface are given by the following formulas:

Figure BDA0003997575740000105
Figure BDA0003997575740000105

Figure BDA0003997575740000106
Figure BDA0003997575740000106

Figure BDA0003997575740000107
Figure BDA0003997575740000107

其中,

Figure BDA0003997575740000108
表示纬度微分形式,
Figure BDA0003997575740000109
表示经度微分形式,
Figure BDA00039975757400001010
表示距地球表面高度微分形式。in,
Figure BDA0003997575740000108
represents the latitude differential form,
Figure BDA0003997575740000109
represents the differential form of longitude,
Figure BDA00039975757400001010
Represents the differential form of the height above the earth's surface.

本实施例中,在解算速度信息时,不仅考虑了纬度、经度和距地球表面的高度,还引入了由于重力异常造成的当地重力矢量方向相对于当地垂线方向的角度偏差,从而使得计算出的速度信息更为准确。In this embodiment, when calculating the speed information, not only the latitude, longitude and altitude from the earth's surface are taken into account, but also the angular deviation of the local gravity vector direction relative to the local vertical direction caused by gravity anomaly is introduced, so that the calculated speed information is more accurate.

步骤S208,组合导航。Step S208, combined navigation.

由微捷联惯性测量单元和移动站卫星接收机(GNSS)构成组合导航,GNSS负责重置惯性导航在一定时间周期内的累计误差。采用松组合结构,被估计的位置、速度和姿态误差用于修正惯性导航的解,具体架构如图2C所示。The integrated navigation is composed of a micro strapdown inertial measurement unit and a mobile station satellite receiver (GNSS). The GNSS is responsible for resetting the accumulated error of the inertial navigation within a certain time period. The loose combination structure is adopted, and the estimated position, velocity and attitude errors are used to correct the inertial navigation solution. The specific architecture is shown in Figure 2C.

本实施例的松组合结构是一个串联系统,来自GNSS的导航解输出位置和速度作为量测信息输入到卡尔曼滤波器中,用卡尔曼滤波器来估计捷联解算的误差。扩展Kalman滤波器(EKF)方法的具体流程如图3所示,包括以下步骤:步骤S302,建立系统模型和量测模型;步骤S304,输入初始估计值;步骤S306,在预测阶段进行线性化处理,估计误差协方差;步骤S308,计算卡尔曼增益,基于卡尔曼增益重新估计误差协方差。The loose combination structure of this embodiment is a series system, and the position and velocity output from the navigation solution of GNSS are input into the Kalman filter as measurement information, and the Kalman filter is used to estimate the error of the strapdown solution. The specific process of the extended Kalman filter (EKF) method is shown in Figure 3, including the following steps: step S302, establishing a system model and a measurement model; step S304, inputting an initial estimate; step S306, performing linearization processing in the prediction stage to estimate the error covariance; step S308, calculating the Kalman gain, and re-estimating the error covariance based on the Kalman gain.

本实施例提供的松耦合集成的主要优点是简单和冗余性,可以用于任何微惯性测量单元(即捷联惯性测量单元)和GNSS设备,特别适合改进算法应用,在松耦合的结构中,存在一个独立的GNSS可用导航解,其在组合导航解之外。在开环微惯性测量单元修正执行时,也存在一个独立微惯性测量单元导航解,这支持基本的并行导航解。The main advantages of the loosely coupled integration provided by this embodiment are simplicity and redundancy, which can be used for any micro-inertial measurement unit (i.e., strapdown inertial measurement unit) and GNSS equipment, and is particularly suitable for improved algorithm applications. In the loosely coupled structure, there is an independent GNSS available navigation solution, which is outside the combined navigation solution. When the open-loop micro-inertial measurement unit correction is executed, there is also an independent micro-inertial measurement unit navigation solution, which supports basic parallel navigation solutions.

在本实施例的微惯性测量单元误差模型中,是基于小失准角的误差模型,通过一个小扰动表示误差因素影响,推导出微惯性测量单元误差的非线性微分方程。根据微惯性测量单元和GNSS的误差状态方程,组合系统的状态变量,由微惯性测量单元和GNSS的误差状态变量共同组合而成,可用式(27)表示。In the error model of the micro-inertial measurement unit in this embodiment, an error model based on a small misalignment angle is used to represent the influence of the error factor by a small disturbance, and a nonlinear differential equation of the error of the micro-inertial measurement unit is derived. According to the error state equations of the micro-inertial measurement unit and the GNSS, the state variables of the combined system are formed by combining the error state variables of the micro-inertial measurement unit and the GNSS, which can be expressed by formula (27).

Figure BDA0003997575740000111
Figure BDA0003997575740000111

其中,

Figure BDA0003997575740000112
表示系统状态变量微分形式,F(t)表示状态转移矩阵,X(t)表示系统状态向量,B(t)表示控制矩阵,u(t)表示控制变量,G(t)表示系统噪声驱动矩阵,w(t)表示系统噪声矩阵。in,
Figure BDA0003997575740000112
represents the differential form of the system state variable, F(t) represents the state transfer matrix, X (t) represents the system state vector, B(t) represents the control matrix, u(t) represents the control variable, G(t) represents the system noise driving matrix, and w(t) represents the system noise matrix.

车载微惯性测量单元的误差包括X轴方向滚转角偏差δα,Y轴方向俯仰角偏差δβ,Z轴方向偏航角偏差δγ三个速度误差δVN,δVE,δVD和三个位置误差δL,δl,δh,δfx δfy δfz分别为X轴、Y轴和Z轴加速度偏差(m/s2),δωx δωy δωz分别为X轴Y轴Z轴陀螺偏差(°/s),用下式(28)表示:The errors of the vehicle-mounted micro-inertial measurement unit include the roll angle deviation δα in the X-axis direction, the pitch angle deviation δ β in the Y-axis direction, and the yaw angle deviation δ γ in the Z-axis direction. The three velocity errors δ VN , δ VE , δ VD and the three position errors δL, δl, δh, δf x δf y δf z are the acceleration deviations of the X-axis, Y-axis and Z-axis (m/s 2 ), and δ ωx δ ωy δ ωz are the gyro deviations of the X-axis, Y-axis and Z-axis (°/s), respectively, which are expressed by the following formula (28):

Figure BDA0003997575740000121
Figure BDA0003997575740000121

由误差模型对其简化处理得到状态转移矩阵F(t)为(29):The state transfer matrix F (t) is obtained by simplifying the error model as (29):

Figure BDA0003997575740000122
Figure BDA0003997575740000122

Figure BDA0003997575740000123
Figure BDA0003997575740000123

其中,FS-9×9表示9×9维惯导系统误差,FDCM-6×6表示6×6维方向余弦矩阵,DCM3×3表示3×3维方向余弦矩阵。Among them, FS-9×9 represents the 9×9 dimensional inertial navigation system error, FDCM-6×6 represents the 6×6 dimensional direction cosine matrix, and DCM 3×3 represents the 3×3 dimensional direction cosine matrix.

姿态角误差通过车载参考系到导航参考系的方向余弦矩阵和陀螺的误差联系用式(31)表示。The attitude angle error is expressed by equation (31) through the direction cosine matrix from the vehicle reference system to the navigation reference system and the gyro error.

Figure BDA0003997575740000131
Figure BDA0003997575740000131

其中,C表示方向余弦矩阵元素。Where C represents the direction cosine matrix element.

F矩阵其中元素如式(32)到(43)表示:The elements of the F matrix are expressed as follows:

Figure BDA0003997575740000132
Figure BDA0003997575740000132

Figure BDA0003997575740000133
Figure BDA0003997575740000133

Figure BDA0003997575740000134
Figure BDA0003997575740000134

Figure BDA0003997575740000135
Figure BDA0003997575740000135

Figure BDA0003997575740000136
Figure BDA0003997575740000136

Figure BDA0003997575740000137
Figure BDA0003997575740000137

Figure BDA0003997575740000138
Figure BDA0003997575740000138

Figure BDA0003997575740000139
Figure BDA0003997575740000139

Figure BDA00039975757400001310
Figure BDA00039975757400001310

Figure BDA00039975757400001311
Figure BDA00039975757400001311

Figure BDA00039975757400001312
Figure BDA00039975757400001312

Figure BDA0003997575740000141
Figure BDA0003997575740000141

其中R是地球半径,VN,VE,VD是微惯性测量单元指示北、东、地向速度,fn,fE,fD是MIMU的北、东和地向比力测量,L为微惯性测量单元输出纬度,Ω是地球自转角速度,高度相对于地球的半径可忽略不计,将高度省略简化处理。Where R is the radius of the Earth, V N , VE , and V D are the north, east, and ground velocities indicated by the micro-inertial measurement unit, f n , f E , and f D are the north, east, and ground specific force measurements of the MIMU, L is the latitude output by the micro-inertial measurement unit, Ω is the angular velocity of the Earth's rotation, and the height is negligible relative to the radius of the Earth, so the height is omitted for simplification.

组合系统的量测模型体现在矩阵H中,将量测模型与滤波状态联系起来。本实施例的观测信息是GNSS系统的速度、位置和姿态,姿态包括偏航角α和俯仰角β,GNSS无法提供滚转角。GNS提供车辆的当前位置信息,以地理参考系为导航参考系,经度LG、纬度lG、高度hG为参数。GNSS提供车辆速度信息,采用笛卡尔速度[VN VE VD]作为GNSS速度量测。The measurement model of the combined system is embodied in the matrix H, which links the measurement model with the filtering state. The observation information of this embodiment is the speed, position and attitude of the GNSS system. The attitude includes the yaw angle α and the pitch angle β. GNSS cannot provide the roll angle. GNS provides the current position information of the vehicle, with the geographic reference system as the navigation reference system, and the longitude LG , latitude lG , and altitude hG as parameters. GNSS provides vehicle speed information, using the Cartesian speed [ VNVED ] as the GNSS speed measurement.

GNSS提供的位置表示为地理下的真值和误差之和,如(44)表示:The position provided by GNSS is expressed as the sum of the true value and the error in the geographic context, as expressed in (44):

Figure BDA0003997575740000142
Figure BDA0003997575740000142

其中,LG表示地理坐标系下纬度,lG表示地理坐标系下经度,hG表示地理坐标系下高度,LN表示导航坐标系下纬度,NN表示卫星接收计北向位置误差,NE表示卫星接收计东向位置误差,RN表示横向曲率半径,hN表示导航坐标系下高度,Nh卫星接收计天向位置误差。δL,δl,δh为GNSS相对于导航参考系的位置误差量测如(45)表示:Wherein, L G represents the latitude in the geographic coordinate system, l G represents the longitude in the geographic coordinate system, h G represents the altitude in the geographic coordinate system, L N represents the latitude in the navigation coordinate system, N N represents the north position error of the satellite receiving meter, N E represents the east position error of the satellite receiving meter, R N represents the lateral curvature radius, h N represents the altitude in the navigation coordinate system, and Nh represents the celestial position error of the satellite receiving meter. δL, δl, δh are the position error measurements of GNSS relative to the navigation reference system, as expressed in (45):

Figure BDA0003997575740000143
Figure BDA0003997575740000143

其中,LI表示惯性坐标系下纬度,lI表示惯性坐标系下经度,lG表示地理坐标系下经度,RM表示子午面曲率半径,RN表示横向曲率半径,PGN表示地理坐标系下北向位置,PGE表示地理坐标系下东向位置,PGD表示地理坐标系下地向位置。微惯性测量单元的速度量测信息为导航系的真值和相应的速度误差之和,GNSS速度量测信息表示为N系下的真值与相应的测速误差之差。MN,ME,MD为GNSS的测速误差项在北东地坐标轴上的分量,速度量测如式(46)所示:Wherein, LI represents the latitude in the inertial coordinate system, IL represents the longitude in the inertial coordinate system, LG represents the longitude in the geographic coordinate system, RM represents the radius of curvature of the meridian plane, RN represents the radius of transverse curvature, PGN represents the north position in the geographic coordinate system, PGE represents the east position in the geographic coordinate system, and PGD represents the ground position in the geographic coordinate system. The velocity measurement information of the micro-inertial measurement unit is the sum of the true value of the navigation system and the corresponding velocity error, and the GNSS velocity measurement information is expressed as the difference between the true value in the N system and the corresponding velocity measurement error. MN , ME , MD are the components of the GNSS velocity measurement error term on the north-east coordinate axis. The velocity measurement is shown in formula (46):

Figure BDA0003997575740000151
Figure BDA0003997575740000151

其中,VIN表示惯性坐标系下北向速度,VGN表示地理坐标系下北向速度,VIE表示惯性坐标系下东向速度,VGE表示地理坐标系下东向速度,VID表示惯性坐标系下地向速度,VGD表示地理坐标系下地向速度,MN表示卫星接收机北向误差速度,ME表示卫星接收机东向误差速度,MD表示卫星接收机地向误差速度,δVN表示北向速度误差分量,δVE表示地向速度误差分量,δVD表示地向速度误差分量,HV表示量测矩阵,X(t)表示系统状态向量,Vv(t)表示量测噪声向量。Wherein, V IN represents the north velocity in the inertial coordinate system, V GN represents the north velocity in the geographic coordinate system, V IE represents the east velocity in the inertial coordinate system, V GE represents the east velocity in the geographic coordinate system, V ID represents the ground velocity in the inertial coordinate system, V GD represents the ground velocity in the geographic coordinate system, MN represents the north error velocity of the satellite receiver, ME represents the east error velocity of the satellite receiver, MD represents the ground error velocity of the satellite receiver, δ VN represents the north velocity error component, δ VE represents the ground velocity error component, δ VD represents the ground velocity error component, HV represents the measurement matrix, X(t) represents the system state vector, and Vv(t) represents the measurement noise vector.

上式中

Figure BDA0003997575740000154
速度的标准差由式(47)表示。In the above formula
Figure BDA0003997575740000154
The standard deviation of the velocity is expressed by equation (47).

Figure BDA0003997575740000152
Figure BDA0003997575740000152

其中,HDOP表示位置精度因子,

Figure BDA0003997575740000153
表示位置标准差微分形式。Among them, HDOP represents the position precision factor,
Figure BDA0003997575740000153
Represents the differential form of the location standard deviation.

微惯性测量单元姿态量测表示为导航系下真值与相应姿态误差,GNSS姿态量测表示为N系下真值与相应姿态误差,航向角和俯仰角测量方程为(48):The attitude measurement of the micro-inertial measurement unit is expressed as the true value and the corresponding attitude error in the navigation system, and the attitude measurement of the GNSS is expressed as the true value and the corresponding attitude error in the N system. The heading angle and pitch angle measurement equations are (48):

Figure BDA0003997575740000161
Figure BDA0003997575740000161

其中,

Figure BDA00039975757400001612
表示姿态量测方程,
Figure BDA0003997575740000162
表示惯性坐标系下航向角,
Figure BDA0003997575740000163
表示地理坐标系下航向角,
Figure BDA0003997575740000164
表示惯性坐标系下俯仰角,
Figure BDA0003997575740000165
表示地理坐标系下俯仰角,
Figure BDA0003997575740000166
表示姿态观测矩阵,X(t)表示系统状态向量,
Figure BDA0003997575740000167
表示量测噪声矩阵。in,
Figure BDA00039975757400001612
represents the attitude measurement equation,
Figure BDA0003997575740000162
represents the heading angle in the inertial coordinate system,
Figure BDA0003997575740000163
Indicates the heading angle in the geographic coordinate system.
Figure BDA0003997575740000164
represents the pitch angle in the inertial coordinate system,
Figure BDA0003997575740000165
Indicates the elevation angle in the geographic coordinate system.
Figure BDA0003997575740000166
represents the attitude observation matrix, X(t) represents the system state vector,
Figure BDA0003997575740000167
represents the measurement noise matrix.

将位置、速度和姿态量(45)(46)(48)测合并表示如公式49所示:The position, velocity and attitude measurements (45)(46)(48) are combined and expressed as shown in formula 49:

Figure BDA0003997575740000168
Figure BDA0003997575740000168

其中,z表示量测方程,Zp(t)表示位置量测方程,Zv(t)表示速度量测方程,

Figure BDA0003997575740000169
表示姿态量测方程,HV表示速度量测矩阵,HP表示位置量测矩阵,
Figure BDA00039975757400001610
表示姿态量测矩阵,vp表示卫星接收计位置测量误差,vv表示卫星接收计速度测量误差,Xk+1表示k+1时刻系统估计状态,k表示当前导航解算时刻,Vk+1表示k+1时刻量测噪声序列,H表示量测矩阵。
Figure BDA00039975757400001611
为GNSS观测噪声。Wherein, z represents the measurement equation, Z p(t) represents the position measurement equation, and Z v(t) represents the velocity measurement equation.
Figure BDA0003997575740000169
represents the attitude measurement equation, H V represents the velocity measurement matrix, HP represents the position measurement matrix,
Figure BDA00039975757400001610
represents the attitude measurement matrix, v p represents the satellite receiver position measurement error, v v represents the satellite receiver velocity measurement error, X k+1 represents the system estimated state at time k+1, k represents the current navigation solution time, V k+1 represents the measurement noise sequence at time k+1, and H represents the measurement matrix.
Figure BDA00039975757400001611
is the GNSS observation noise.

本实施例,采用速度、位置和姿态误差的方式建立组合系统的量测方程,比直接采用状态值更适合动态系统的变化。In this embodiment, the measurement equation of the combined system is established by using speed, position and posture errors, which is more suitable for changes in the dynamic system than directly using state values.

步骤S210,初始对准。Step S210: initial alignment.

利用卫星基准站测量的基准信息:经度、纬度、高度、姿态角、俯仰角和航向角信息。将相关的基准信息传递给捷联惯性测量单元,捷联惯性测量单元接收到基准信息,利用建立的EKF滤波器,结合自身惯性、卫星数据,完成速度、位置匹配下的传递对准。从而实现无人机端的初始对准。The reference information measured by the satellite reference station: longitude, latitude, altitude, attitude angle, pitch angle and heading angle information. The relevant reference information is passed to the strapdown inertial measurement unit. The strapdown inertial measurement unit receives the reference information and uses the established EKF filter, combined with its own inertia and satellite data, to complete the transfer alignment under speed and position matching. This achieves the initial alignment of the drone end.

本申请解决了现有技术中分立式的惯性、卫星导航系统体积大、成本高的技术问题,具有体积小、成本低的有益效果。The present application solves the technical problems of large size and high cost of discrete inertial and satellite navigation systems in the prior art, and has the beneficial effects of small size and low cost.

需要说明的是,对于前述的各方法实施例,为了简单描述,故将其都表述为一系列的动作组合,但是本领域技术人员应该知悉,本申请并不受所描述的动作顺序的限制,因为依据本申请,某些步骤可以采用其他顺序或者同时进行。其次,本领域技术人员也应该知悉,说明书中所描述的实施例均属于优选实施例,所涉及的动作和模块并不一定是本申请所必须的。It should be noted that, for the above-mentioned method embodiments, for the sake of simplicity, they are all expressed as a series of action combinations, but those skilled in the art should be aware that the present application is not limited by the order of the actions described, because according to the present application, certain steps can be performed in other orders or simultaneously. Secondly, those skilled in the art should also be aware that the embodiments described in the specification are all preferred embodiments, and the actions and modules involved are not necessarily required by the present application.

通过以上的实施方式的描述,本领域的技术人员可以清楚地了解到根据上述实施例的方法可借助软件加必需的通用硬件平台的方式来实现,当然也可以通过硬件,但很多情况下前者是更佳的实施方式。基于这样的理解,本申请的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中,包括若干指令用以使得一台终端设备(可以是手机,计算机,服务器,或者网络设备等)执行本申请各个实施例所述的方法。Through the description of the above implementation methods, those skilled in the art can clearly understand that the method according to the above embodiment can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present application, or the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium (such as ROM/RAM, magnetic disk, optical disk), and includes a number of instructions for a terminal device (which can be a mobile phone, computer, server, or network device, etc.) to execute the methods described in each embodiment of the present application.

实施例3Example 3

根据本申请实施例,提供了一种超小型无人机用组合导航系统,包括捷联惯性测量单元及卫星差分定位系统。According to an embodiment of the present application, a combined navigation system for an ultra-small unmanned aerial vehicle is provided, comprising a strapdown inertial measurement unit and a satellite differential positioning system.

捷联惯性测量单元,如图4所示,包括惯性测量组件42、导航计算机44、二次电源模块40、晶振48以及对外连接器46、移动站卫星接收机49。As shown in FIG. 4 , the strapdown inertial measurement unit includes an inertial measurement assembly 42 , a navigation computer 44 , a secondary power supply module 40 , a crystal oscillator 48 , an external connector 46 , and a mobile station satellite receiver 49 .

1)惯性测量组件1) Inertial Measurement Unit

惯性测量组件42主要进行载体的加速度和角速度信息获取,通过四线制SPI接口,将数据上传给导航计算机44。惯性测量组件42内置3轴陀螺仪和3轴加速度计,其具体指标如表1所示。The inertial measurement unit 42 mainly acquires the acceleration and angular velocity information of the carrier, and uploads the data to the navigation computer 44 through a four-wire SPI interface. The inertial measurement unit 42 has a built-in 3-axis gyroscope and a 3-axis accelerometer, and its specific indicators are shown in Table 1.

Figure BDA0003997575740000181
Figure BDA0003997575740000181

表1Table 1

硬件电路设计图如图5所示。芯片通过四线制SPI接口与导航计算机44相连接,供电电源为3.3V,配有独立的旁路电容进行滤波处理。The hardware circuit design is shown in Figure 5. The chip is connected to the navigation computer 44 via a four-wire SPI interface, the power supply is 3.3V, and is equipped with an independent bypass capacitor for filtering.

2)导航计算机2) Navigation Computer

导航计算机44主要进行导航算法解算,本实施例中,将导航计算机44的工作主频设置为160MHz,配备浮点计算单元,自身存储程序空间为2MB,内存800KB,能够满足5ms一个周期的导航解算要求。此外,导航计算机44还具有丰富的DMA通道,用于进行片外芯片操作与逻辑运算。具体资源分配如下表2所示。The navigation computer 44 is mainly used for solving the navigation algorithm. In this embodiment, the operating main frequency of the navigation computer 44 is set to 160MHz, equipped with a floating-point calculation unit, 2MB of its own program storage space, and 800KB of memory, which can meet the navigation solution requirement of a cycle of 5ms. In addition, the navigation computer 44 also has abundant DMA channels for off-chip chip operations and logic operations. The specific resource allocation is shown in Table 2 below.

序号Serial number 资源名称Resource Name 关联设备Related equipment 备注Remark 11 USART0USART0 外部通信External Communications 导航数据输出TTL@3.3VNavigation data output TTL@3.3V 22 USART1USART1 外部通信External Communications 导航数据输出RS-422@3.3VNavigation data output RS-422@3.3V 33 SPI0SPI0 外部通信External Communications 四线制SPI,速率25MHzFour-wire SPI, speed 25MHz 44 SPI1SPI1 惯性测量组件Inertial Measurement Unit 四线制SPIFour-wire SPI

表2Table 2

具体硬件设计如图6所示。该芯片的最小系统包括:启动电容、复位电路和时钟源等。The specific hardware design is shown in Figure 6. The minimum system of the chip includes: startup capacitor, reset circuit and clock source, etc.

3)二次电源模块3) Secondary power module

二次电源模块40如图7所示,主要是进行内部供电的调整与匹配。本实施例中,放置了两个独立芯片进行电源输出,一路用于提供给移动站卫星接收机49,一路提供给板上其他数字电路。为提高电源性能,减少设备间的干扰,在电源设计过程中,输入、输出均进行了π型滤波器设计。The secondary power supply module 40 is shown in FIG7 , and is mainly used to adjust and match the internal power supply. In this embodiment, two independent chips are placed for power output, one for providing power to the mobile station satellite receiver 49, and the other for providing power to other digital circuits on the board. In order to improve the power supply performance and reduce interference between devices, a π-type filter design is performed on both the input and output during the power supply design process.

4)晶振4) Crystal oscillator

本实施中晶振48具有温度特性好,可靠性高等特点,其稳定性≤20ppm。In this embodiment, the crystal oscillator 48 has the characteristics of good temperature characteristics and high reliability, and its stability is ≤20ppm.

5)对外连接器5) External connector

本实施例中,对外连接器46的间距为1.0mm的表贴插针,并镀金,这样,在保证稳定性的前提下,减少接触阻抗。In this embodiment, the external connector 46 has surface mount pins with a pitch of 1.0 mm and is gold plated, so that the contact impedance is reduced while ensuring stability.

星差分定位系统包括卫星差分定位系统、移动站卫星导航系统、移动站卫星机、移动站卫星接收天线、移动站卫星接收馈线、基准站卫星导航系统、基准站卫星接收机、基准站卫星接收天线、基准站卫星接收天线、基准站卫星接收馈线、基准站卫星接收馈线、基准站卫星天线连接器、基准站卫星天线连接器、二次电源模块、接口转换模块和数据融合单元。The satellite differential positioning system includes a satellite differential positioning system, a mobile station satellite navigation system, a mobile station satellite machine, a mobile station satellite receiving antenna, a mobile station satellite receiving feeder, a base station satellite navigation system, a base station satellite receiver, a base station satellite receiving antenna, a base station satellite receiving antenna, a base station satellite receiving feeder, a base station satellite receiving feeder, a base station satellite antenna connector, a base station satellite antenna connector, a secondary power supply module, an interface conversion module and a data fusion unit.

1)移动站卫星导航系统1) Mobile station satellite navigation system

移动站卫星导航系统的具体指标如表3所示。The specific indicators of the mobile station satellite navigation system are shown in Table 3.

Figure BDA0003997575740000201
Figure BDA0003997575740000201

表3Table 3

具体硬件设计电路如图8所示。The specific hardware design circuit is shown in Figure 8.

2)基准站卫星导航系统2) Base station satellite navigation system

基准站卫星导航系统同时具备定位和姿态测量功能,配合高精度惯性测量单元,实现高精度基准位姿获取。主要技术指标如下表4所示。The base station satellite navigation system has both positioning and attitude measurement functions, and cooperates with the high-precision inertial measurement unit to achieve high-precision base attitude acquisition. The main technical indicators are shown in Table 4.

Figure BDA0003997575740000202
Figure BDA0003997575740000202

Figure BDA0003997575740000211
Figure BDA0003997575740000211

表4Table 4

卫星接收机用于接收高精度卫星定位数据,实现高精度的航空级定位精度。结合高精度的惯性测量单元和数据融合单元,实现数据有效融合,完成最终的位置、姿态初始测量。The satellite receiver is used to receive high-precision satellite positioning data to achieve high-precision aviation-grade positioning accuracy. Combined with the high-precision inertial measurement unit and data fusion unit, effective data fusion is achieved to complete the final initial measurement of position and attitude.

实施例4Example 4

根据本申请实施例,还提供了一种超小型无人机用组合导航装置,包括:捷联解算模块,被配置为通过惯性测量单元获取所述无人机的加速度信息和角速度信息,并基于所述加速度和角速度信息进行姿态解算,得到所述无人机的位姿信息;修正模块,被配置为接收地面站以预设的频率发送的卫星差分修正信息,利用所述卫星差分修正信息,来修正所述位姿信息;导航模块,被配置为基于修正后的所述位姿信息,来对所述无人机进行导航。According to an embodiment of the present application, a combined navigation device for an ultra-small unmanned aerial vehicle is also provided, including: a strapdown solution module, configured to obtain acceleration information and angular velocity information of the unmanned aerial vehicle through an inertial measurement unit, and perform attitude solution based on the acceleration and angular velocity information to obtain the posture information of the unmanned aerial vehicle; a correction module, configured to receive satellite differential correction information sent by a ground station at a preset frequency, and use the satellite differential correction information to correct the posture information; a navigation module, configured to navigate the unmanned aerial vehicle based on the corrected posture information.

可选地,本实施例中的具体示例可以参考上述实施例1和实施例2中所描述的示例,本实施例在此不再赘述。Optionally, the specific examples in this embodiment may refer to the examples described in the above-mentioned Embodiment 1 and Embodiment 2, and this embodiment will not be described in detail here.

实施例5Example 5

本申请的实施例还提供了一种存储介质。该存储介质被设置为存储用于执行以上实施例1和2中的方法的程序代码。The embodiment of the present application further provides a storage medium, which is configured to store program codes for executing the methods in the above embodiments 1 and 2.

可选地,在本实施例中,上述存储介质可以包括但不限于:U盘、只读存储器(ROM,Read-Only Memory)、随机存取存储器(RAM,Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。Optionally, in this embodiment, the above-mentioned storage medium may include but is not limited to: a USB flash drive, a read-only memory (ROM), a random access memory (RAM), a mobile hard disk, a magnetic disk or an optical disk, and other media that can store program codes.

上述本申请实施例序号仅仅为了描述,不代表实施例的优劣。The serial numbers of the above-mentioned embodiments of the present application are for description only and do not represent the advantages or disadvantages of the embodiments.

上述实施例中的集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在上述计算机可读取的存储介质中。基于这样的理解,本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来,该计算机软件产品存储在存储介质中,包括若干指令用以使得一台或多台计算机设备(可为个人计算机、服务器或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。If the integrated units in the above embodiments are implemented in the form of software functional units and sold or used as independent products, they can be stored in the above computer-readable storage medium. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, which is stored in a storage medium and includes several instructions for enabling one or more computer devices (which may be personal computers, servers, or network devices, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present application.

在本申请的上述实施例中,对各个实施例的描述都各有侧重,某个实施例中没有详述的部分,可以参见其他实施例的相关描述。In the above embodiments of the present application, the description of each embodiment has its own emphasis. For parts that are not described in detail in a certain embodiment, please refer to the relevant descriptions of other embodiments.

在本申请所提供的几个实施例中,应该理解到,所揭露的客户端,可通过其它的方式实现。其中,以上所描述的装置实施例仅仅是示意性的,例如所述单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。另一点,所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口,单元或模块的间接耦合或通信连接,可以是电性或其它的形式。In the several embodiments provided in the present application, it should be understood that the disclosed client can be implemented in other ways. Among them, the device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of units or modules, which can be electrical or other forms.

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

另外,在本申请各个实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

以上所述仅是本申请的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本申请原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本申请的保护范围。The above is only a preferred implementation of the present application. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present application. These improvements and modifications should also be regarded as the scope of protection of the present application.

Claims (10)

1. An integrated navigation method for a subminiature unmanned aerial vehicle, comprising:
acquiring acceleration information and angular velocity information of the unmanned aerial vehicle through an inertial measurement unit, and performing attitude calculation based on the acceleration information and the angular velocity information to obtain pose information of the unmanned aerial vehicle;
receiving satellite differential correction information sent by a ground station at a preset frequency, and correcting the pose information by using the satellite differential correction information;
navigating the drone based on the revised pose information.
2. The method of claim 1, wherein performing an attitude solution based on the acceleration information and angular velocity information comprises:
carrying out attitude calculation by utilizing a quaternion method based on the angular velocity information to obtain an attitude angle of the unmanned aerial vehicle;
and performing strapdown calculation based on the acceleration information to obtain the speed information and the position information of the unmanned aerial vehicle.
3. The method of claim 2, wherein performing attitude calculation by using a quaternion method based on the angular velocity information to obtain an attitude angle of the drone comprises:
describing the pose information by using quaternions, and determining a relation between a conversion relation from a projectile coordinate system to a geographic coordinate system and the quaternions based on the description;
solving the quaternion describing the pose information based on the determined relationship between the conversion relation and the quaternion using an angular velocity increment;
and carrying out attitude calculation based on the solved quaternion to obtain an attitude angle of the unmanned aerial vehicle.
4. The method according to claim 2, wherein performing strapdown solution based on the acceleration information to obtain the velocity information of the drone comprises:
determining the change rate of the ground speed in a navigation coordinate system based on the change rate of the ground speed in an inertial coordinate system, the specific force vector in the acceleration information and the rotation angular velocity of the earth;
and determining a navigation equation based on the change rate of the ground speed in a navigation coordinate system, and determining components of the speed of the unmanned aerial vehicle along the true north direction, the east direction and the local vertical direction in the navigation coordinate system based on the navigation equation.
5. The method of claim 4, wherein determining the components of the velocity of the drone along true north, east, and local vertical directions in the navigational coordinate system based on the navigational equations comprises:
determining the rotation angular rate of a geographic coordinate system relative to an earth fixed connection coordinate system based on the change rate of the longitude and the latitude of the unmanned aerial vehicle;
determining a local gravity vector of the drone based on the angular rate of rotation;
determining components of the velocity of the drone in the navigation coordinate system along true north, east and local vertical directions based on an angular deviation of a local gravity vector direction relative to a local vertical direction due to a gravity anomaly, a current latitude, a current longitude and a current altitude from the earth's surface, and the local gravity vector.
6. The method according to claim 1, wherein correcting the pose information using the satellite differential correction information comprises:
estimating errors of position, velocity and attitude with a kalman filter based on the satellite difference correction information;
correcting the pose information based on the estimated error.
7. The method of claim 1, wherein estimating errors for position, velocity, and attitude using a kalman filter comprises:
inputting the position, the speed and the attitude of a navigation solution from a navigation satellite system into the Kalman filter as measurement information to serve as an initial estimation value;
in a prediction stage, the Kalman filter carries out linearization processing on the initial estimation value and determines an error covariance based on the initial estimation value after linearization processing;
determining a Kalman gain based on the error covariance, and re-determining the error covariance based on the Kalman gain;
estimating errors of the position, velocity, and attitude of the strapdown solution based on the re-determined error covariance.
8. An integrated navigation device for a subminiature unmanned aerial vehicle, comprising:
the strapdown resolving module is configured to acquire acceleration information and angular velocity information of the unmanned aerial vehicle through an inertial measurement unit, and perform attitude resolving based on the acceleration information and the angular velocity information to obtain pose information of the unmanned aerial vehicle;
the correction module is configured to receive satellite differential correction information sent by a ground station at a preset frequency, and correct the pose information by using the satellite differential correction information;
a navigation module configured to navigate the drone based on the pose information after the correction.
9. An integrated navigation system for a subminiature unmanned aerial vehicle, comprising:
a strapdown inertial measurement unit configured to acquire acceleration information and angular velocity information of the drone;
a satellite differential positioning system configured to transmit satellite differential correction information at a preset frequency;
wherein the strapdown inertial measurement unit includes a navigation computer, the navigation computer being the integrated navigation device for a subminiature unmanned aerial vehicle according to claim 8.
10. A computer-readable storage medium, on which a program is stored, which, when executed, causes a computer to carry out the method according to any one of claims 1 to 7.
CN202211607090.1A 2022-12-14 2022-12-14 Combined navigation method, device and system for subminiature unmanned aerial vehicle Pending CN115950419A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211607090.1A CN115950419A (en) 2022-12-14 2022-12-14 Combined navigation method, device and system for subminiature unmanned aerial vehicle

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211607090.1A CN115950419A (en) 2022-12-14 2022-12-14 Combined navigation method, device and system for subminiature unmanned aerial vehicle

Publications (1)

Publication Number Publication Date
CN115950419A true CN115950419A (en) 2023-04-11

Family

ID=87286959

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211607090.1A Pending CN115950419A (en) 2022-12-14 2022-12-14 Combined navigation method, device and system for subminiature unmanned aerial vehicle

Country Status (1)

Country Link
CN (1) CN115950419A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116699665A (en) * 2023-08-08 2023-09-05 山东科技大学 An unmanned ship positioning system and method suitable for offshore photovoltaic power plant environment

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116699665A (en) * 2023-08-08 2023-09-05 山东科技大学 An unmanned ship positioning system and method suitable for offshore photovoltaic power plant environment

Similar Documents

Publication Publication Date Title
CN108180925B (en) Odometer-assisted vehicle-mounted dynamic alignment method
CN108151737B (en) Unmanned aerial vehicle swarm cooperative navigation method under condition of dynamic mutual observation relationship
CN111982106A (en) Navigation method, navigation device, storage medium and electronic device
CN112611394B (en) Aircraft attitude alignment method and system under emission coordinate system
CN113253325B (en) Inertial satellite sequential tight combination lie group filtering method
CN113050143B (en) A Tightly Coupled Navigation Method in Transmitting Inertial Coordinate System
CN102901514A (en) Collaborative initial alignment method based on multiple-inertia-unit informational constraint
CN107063262A (en) A kind of complementary filter method resolved for UAV Attitude
CN108426575A (en) With the improved inertial navigation polar region transverse direction air navigation aid of ellipsoid model of globe
CN116222551A (en) An underwater navigation method and device integrating multiple data
Sheta et al. Improved localization for Android smartphones based on integration of raw GNSS measurements and IMU sensors
CN115950419A (en) Combined navigation method, device and system for subminiature unmanned aerial vehicle
CN113009816A (en) Method and device for determining time synchronization error, storage medium and electronic device
CN114993305B (en) A combined navigation method for guided projectiles based on launching coordinate system
CN111207734B (en) EKF-based unmanned aerial vehicle integrated navigation method
CN106643726B (en) Unified inertial navigation resolving method
CN109084756B (en) A Gravity-View Motion Parameter Identification and Accelerometer Zero Bias Separation Method
CN118189946A (en) Integrated navigation method of tightly coupled inertial navigation radar based on radar coordinate system
CN114993296B (en) High dynamic integrated navigation method for guided projectile
CN115980811A (en) Task scheduling method and system for integrated navigation device
CN113703019B (en) Fault processing method of navigation system, electronic equipment and storage medium
CN110375740A (en) Automobile navigation method, device, equipment and storage medium
CN113155156A (en) Method and device for determining running information, storage medium and electronic device
CN116699509A (en) Method and device for determining the heading of agricultural machinery, and agricultural machinery and vehicle
Avrutov Gyro north and latitude finder

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination