Autonomous positioning method based on human body spatial displacement model and inertial navigation system

Abstract

The present invention provides an autonomous positioning method based on a human body spatial displacement model and an inertial navigation system, comprising obtaining a person's specific force and angular velocity through a self-contained sensor and performing inertial navigation solution to obtain a three-dimensional attitude angle, a three-dimensional velocity, and a three-dimensional position at each moment in a sampling window; inputting the person's specific force and angular velocity into the human body spatial displacement model to obtain a three-dimensional displacement of human body movement within the sampling window; inputting the three-dimensional displacement into a human body spatial displacement calculation credibility model to generate a three-dimensional displacement credibility; constructing an adaptive random cloning error state extended Kalman filter through the three-dimensional misalignment angle, three-dimensional velocity error, three-dimensional position error, three-dimensional displacement, and three-dimensional displacement credibility at each moment in the sampling window, and optimally estimating the three-dimensional attitude angle, three-dimensional velocity, and three-dimensional position at the current moment to obtain final positioning information. The present invention reduces the long-term drift error of the inertial navigation system and improves the accuracy and reliability of the positioning result.

Description

Technical Field

[0001] The present invention relates to the field of positioning technology, and in particular to an autonomous positioning method based on a human body spatial displacement model and an inertial navigation system. Background Art

[0002] With the rapid development of smart cities and smart factories, applications based on positioning and navigation are attracting more and more attention from people and related companies, especially in complex urban and factory environments, such as airport halls, exhibition halls, warehouses, libraries, urban canyons, underground parking lots, mines, etc., where it is often necessary to determine the location information of mobile terminals or their holders.

[0003] Since satellite signals are rejected or attenuated in these environments, satellite navigation positioning methods cannot be implemented. Existing autonomous positioning methods predict the position of the human body by measuring human motion information, which has the characteristics of strong autonomy and high accuracy. However, due to the measurement deviation of inertial devices in existing autonomous positioning methods, the errors of the inertial navigation system continue to accumulate, and external observation means are required for continuous correction, and the accuracy and reliability of autonomous positioning are relatively weak. Summary of the invention

[0004] In view of this, the present invention provides an autonomous positioning method based on a human body spatial displacement model and an inertial navigation system to at least solve the above-mentioned problem.

[0005] The present invention provides an autonomous positioning method based on a human body space displacement model and an inertial navigation system, comprising: step 1, obtaining a person's specific force and angular velocity through a self-contained sensor and performing inertial navigation solution to obtain a three-dimensional attitude angle, a three-dimensional velocity, and a three-dimensional position at each moment in a sampling window; step 2, inputting the person's specific force and angular velocity into the human body space displacement model to obtain a three-dimensional displacement of the human body movement in the sampling window; step 3, inputting the three-dimensional displacement into a human body space displacement calculation credibility model to generate a three-dimensional displacement credibility; step 4, constructing an adaptive random cloning error state extended Kalman filter through the three-dimensional misalignment angle, three-dimensional velocity error, three-dimensional position error, three-dimensional displacement and three-dimensional displacement credibility at each moment in the sampling window, and performing optimal estimation of the three-dimensional attitude angle, three-dimensional velocity and three-dimensional position at the current moment through the adaptive random cloning error state extended Kalman filter to obtain final positioning information.

[0006] In one implementation, the self-contained sensor includes an accelerometer, a gyroscope, a magnetometer, and a barometer.

[0007] In another implementation, the human body spatial displacement model is obtained by training a deep neural network, and the training loss function is defined as follows:

[0008]

[0009] in represents the true value of the three-dimensional displacement obtained by the visual inertial odometer, d represents the three-dimensional displacement output by the human body spatial displacement model, It means to perform a sum operation on N data, N is the sampling window length, k is the data sequence number, and ||·|| means the binary norm operation.

[0010] In another implementation, the human body spatial displacement calculation credibility model is obtained by training a deep neural network, and the training loss function is defined as follows:

[0011]

[0012] in, It represents the function of the three-dimensional displacement credibility σ output by the human body spatial displacement calculation credibility model. det represents the matrix determinant operation. It is assumed that the displacement credibility of the three axes X, Y, and Z are independent of each other. It can be simplified to a diagonal matrix of dimension 3×3, namely:

[0013]

[0014] in, They respectively represent the displacement credibility in the X, Y, and Z directions output by the human body spatial displacement calculation credibility model, and diag() represents a diagonal matrix.

[0015] In another implementation, the full error state vector of the adaptive stochastic clonal error state extended Kalman filter is defined as:

[0016] δX=(δξ1,…,δξ N ,δs)

[0017] Among them, δξ k (k=1, ..., N) represents the error state vector cloned in the sampling window of length N, δs represents the error state vector at the current moment;

[0018] The error state vector of the adaptive random clone error state extended Kalman filter is defined as:

[0019] δξ k =(δθ k ,δP k ), δs=(δθ,δv,δP)

[0020] Among them, δθ, δv, and δP represent the misalignment angle, velocity error, and position error, respectively;

[0021] The full state vector of the adaptive random clone error state extended Kalman filter is defined as:

[0022] X=(ξ1,…,ξ N , s)

[0023] Among them, ξ k (k=1, ..., N) represents the cloned state vector in the sampling window of length N, and s represents the state vector at the current moment;

[0024] The state vector of the adaptive stochastic clonal error state extended Kalman filter is defined as:

[0025] ξ k =(θ k , P k ), s = (θ, v, P)

[0026] Among them, θ, v, and P represent attitude angle, velocity, and position, respectively.

[0027] In another implementation, the error state propagation model used by the adaptive stochastic clonal error state extended Kalman filter is defined as:

[0028]

[0029] in, represents the skew-symmetric matrix of the specific force in the navigation coordinate system, I represents the 3×3 unit matrix, 0 represents the 3×3 zero matrix, η gk , η ak denote the gyroscope and accelerometer measurement noise respectively, and k denotes a time point.

[0030] In another implementation, the observation model used in the adaptive random clonal error state extended Kalman filter to describe the three-dimensional displacement relationship between the human body spatial displacement model and the inertial navigation system is defined as:

[0031]

[0032] in, represents the rotation matrix on the yaw axis, which is used to align the human body spatial displacement model with the displacement output by the inertial navigation system. P represents the position calculated by the inertial navigation system. represents the three-dimensional displacement output by the human body spatial displacement model during the time period between i and j, η dij Represents the observation noise output of the network model of human spatial displacement.

[0033] In another implementation, the observation noise output by the human body spatial displacement network model follows a normal distribution in The covariance matrix representing the credibility of the three-dimensional displacement is used to update the Kalman filter gain K and the error state covariance matrix to achieve dynamic tight coupling between the human body spatial displacement model and the inertial navigation system.

[0034] In another implementation, the optimal estimation of the full state vector X of the adaptive random clonal error state extended Kalman filter is calculated as follows:

[0035]

[0036] Wherein, K represents the Kalman filter gain of the filter, represents direct and operation;

[0037] The current position P in the full state vector X after optimal estimation is extracted for final positioning.

[0038] Compared with the prior art, the present invention has the following beneficial effects:

[0039] (1) The inertial navigation system can provide short-term high-precision three-dimensional continuous-time positioning information, but there are long-term drift errors. The human body spatial displacement model is insensitive to complex human body movements. The present invention can construct distance constraints for the inertial navigation system and utilize the human body spatial displacement model and the inertial navigation system to dynamically tightly couple, thereby improving the accuracy and robustness of autonomous positioning of personnel under complex movements, especially solving the problem of autonomous positioning of personnel while riding in a vehicle.

[0040] (2) The present invention introduces a dynamic trade-off mechanism to dynamically adjust the fusion weight of the spatial displacement model data and the inertial navigation data according to the credibility of the displacement measurement, thereby increasing the reliability of the positioning result.

[0041] (3) The present invention is wearable and more convenient to operate, and is suitable for autonomous positioning of personnel under complex movements. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 Schematic diagram of the installation of a self-contained sensor according to an embodiment of the present invention.

[0043] Figure 2 The figure is a specific flow chart of the autonomous positioning method based on the human body spatial displacement model and the inertial navigation system according to an embodiment of the present invention.

[0044] Figure 3 For Figure 2 The corresponding overall flow chart of the autonomous positioning method based on the human body spatial displacement model and the inertial navigation system according to the embodiment of the present invention. DETAILED DESCRIPTION

[0045] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0046] See also Figure 1 The self-contained sensor 1 of the embodiment of the present invention is installed at the toes to measure the specific force and angular velocity of the human body. The self-contained sensor 1 includes an accelerometer, a gyroscope, a magnetometer, and a barometer.

[0047] See also Figure 3 The overall flow chart of the autonomous positioning method based on the human body spatial displacement model and the inertial navigation system according to the embodiment of the present invention comprises the following steps:

[0048] Step 1, obtain the specific force and angular velocity of the person through a self-contained sensor and perform inertial navigation calculation to obtain the three-dimensional attitude angle, three-dimensional velocity, and three-dimensional position at each moment in the sampling window;

[0049] It should be understood that a total of 9-dimensional original state vectors, including the three-dimensional attitude angle, three-dimensional velocity, and three-dimensional position at each moment in the sampling window, are obtained.

[0050] It should also be understood that the self-contained sensor slides the window to acquire personnel data.

[0051] Step 2, inputting the specific force and angular velocity of the person into the human body spatial displacement model to obtain the three-dimensional displacement of the human body movement within the sampling window;

[0052] Step 3, inputting the three-dimensional displacement into the human body spatial displacement calculation credibility model to generate the three-dimensional displacement credibility;

[0053] Step 4: construct an adaptive random cloning error state extended Kalman filter through the 3D misalignment angle, 3D velocity error, 3D position error, 3D displacement and 3D displacement credibility at each moment in the sampling window, and use the adaptive random cloning error state extended Kalman filter to optimally estimate the 3D attitude angle, 3D velocity and 3D position at the current moment to obtain the final positioning information.

[0054] It should be understood that the three-dimensional position vector is used for final positioning.

[0055] The present invention realizes dynamic tight coupling between the human body spatial displacement model and the inertial navigation system through the three-dimensional credibility generated by the human body spatial displacement calculation credibility model, thereby improving the accuracy and reliability of autonomous positioning of personnel under complex movements.

[0056] See also Figure 2The autonomous positioning method based on the human body spatial displacement model and the inertial navigation system of the embodiment of the present invention comprises the following specific processes:

[0057] Step 21, using a self-contained sensor sliding window to collect the human body specific force and angular velocity for a time length of N;

[0058] Step 22, using the strapdown inertial navigation algorithm to calculate the three-dimensional attitude angle θ, three-dimensional velocity v, and three-dimensional position P in the window at each moment in step 21, a total of 9-dimensional original state vectors, to construct the full state vector X=(ξ1, ..., ξ N , s), where ξ 1≤k≤N =(θ k , P k ) represents the cloned state vector in the sampling window of length N, k represents the time point, and s = (θ, v, P) represents the state vector at the current moment;

[0059] Step 23, inputting the specific force and angular velocity collected by the sliding window in step 21 into the human body spatial displacement model to calculate the three-dimensional displacement of the human body movement within the window time period

[0060] Step 24, inputting the three-dimensional displacement calculated in step 23 into the human body spatial displacement calculation credibility model to calculate the three-dimensional displacement credibility of the three-dimensional displacement And calculate the covariance matrix of the three-dimensional displacement credibility;

[0061] The calculation formula of the covariance matrix is ​​as follows:

[0062]

[0063] in, They respectively represent the displacement credibility in the sampling window time in the three directions of XYZ output by the credibility model; diag() represents a diagonal matrix.

[0064] Step 25, calculating the error state propagation model;

[0065] The error state propagation model calculation formula is as follows:

[0066]

[0067] Among them, δθ, δv, and δP represent the misalignment angle, velocity error, and position error, respectively; represents the skew-symmetric matrix of the specific force in the navigation coordinate system; I represents the 3×3 unit matrix; 0 represents the 3×3 zero matrix; η gk , η ak denote the gyroscope and accelerometer measurement noise respectively; k denotes a time point.

[0068] Calculate the observation model that reflects the three-dimensional displacement relationship between the human body spatial displacement model and the inertial navigation system;

[0069] The observation model calculation formula is as follows:

[0070]

[0071] in, represents the rotation matrix on the yaw axis, which is used to align the human body spatial displacement model with the displacement output by the inertial navigation system; P represents the position calculated by the inertial navigation system; represents the three-dimensional displacement output by the human body spatial displacement model within the sampling window period; η dij represents the observation noise output by the human body spatial displacement network model in step 23. The observation noise follows a normal distribution in The covariance matrix of the three-dimensional displacement credibility in step 24 is represented.

[0072] Step 26, calculating the error state propagation covariance matrix and performing enhancement processing on the covariance matrix; the enhancement processing is consistent with the frequency of filter observation update;

[0073] Step 27, using the enhanced error state propagation covariance matrix in step 27 and the covariance matrix of the three-dimensional displacement credibility in step 24 to calculate the Kalman filter gain K and update the error covariance matrix;

[0074] Step 28, optimally estimate the filter full state vector X, the formula is as follows:

[0075]

[0076] Wherein, K represents the Kalman filter gain of the filter; Represents a direct and operation.

[0077] Step 29: extract the current position P in the optimal estimated full state vector X for final positioning.

[0078] Compared with the prior art, the present invention has the following beneficial effects:

[0079] (1) The inertial navigation system can provide short-term high-precision three-dimensional continuous-time positioning information, but there are long-term drift errors. The human body spatial displacement model is insensitive to complex human body movements. The present invention can construct distance constraints for the inertial navigation system and utilize the human body spatial displacement model and the inertial navigation system to dynamically tightly couple, thereby improving the accuracy and robustness of autonomous positioning of personnel under complex movements, especially solving the problem of autonomous positioning of personnel while riding in a vehicle.

[0080] (2) The present invention introduces a dynamic trade-off mechanism to dynamically adjust the fusion weight of the spatial displacement model data and the inertial navigation data according to the credibility of the displacement measurement, thereby increasing the reliability of the positioning result.

[0081] (3) The present invention is wearable and more convenient to operate, and is suitable for autonomous positioning of personnel under complex movements.

[0082] It should be noted that the present invention may be a method, a system, an apparatus and / or a computer program product. The computer program product may include a computer-readable storage medium carrying computer-readable program instructions for causing a processor to implement various aspects of the present invention.

[0083] Computer readable storage medium can be a tangible device that can hold and store instructions used by an instruction execution device. Computer readable storage medium can be, for example, but not limited to, an electrical storage device, a magnetic storage device, an optical storage device, an electromagnetic storage device, a semiconductor storage device, or any suitable combination thereof. More specific examples (non-exhaustive list) of computer readable storage medium include: a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), a static random access memory (SRAM), a portable compact disk read-only memory (CD-ROM), a digital versatile disk (DVD), a memory stick, a floppy disk, a mechanical encoding device, for example, a punch card or a convex structure in a groove on which instructions are stored, and any suitable combination thereof. The computer readable storage medium used here is not interpreted as a transient signal itself, such as a radio wave or other freely propagating electromagnetic wave, an electromagnetic wave propagated by a waveguide or other transmission medium (for example, a light pulse by an optical fiber cable), or an electrical signal transmitted by a wire.

[0084] The computer-readable program instructions described herein can be downloaded from a computer-readable storage medium to each computing / processing device, or downloaded to an external computer or external storage device via a network, such as the Internet, a local area network, a wide area network, and / or a wireless network. The network can include copper transmission cables, optical fiber transmissions, wireless transmissions, routers, firewalls, switches, gateway computers, and / or edge servers. The network adapter card or network interface in each computing / processing device receives the computer-readable program instructions from the network and forwards the computer-readable program instructions for storage in the computer-readable storage medium in each computing / processing device.

[0085] The computer program instructions for performing the operation of the present invention may be assembly instructions, instruction set architecture (ISA) instructions, machine instructions, machine-dependent instructions, microcode, firmware instructions, state setting data, or source code or object code written in any combination of one or more programming languages, including object-oriented programming languages, such as Smalltalk, C++, Python, etc., and conventional procedural programming languages, such as "C" language or similar programming languages. Computer-readable program instructions may be executed entirely on a user's computer, partially on a user's computer, as an independent software package, partially on a user's computer, partially on a remote computer, or entirely on a remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer via any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (e.g., using an Internet service provider to connect via the Internet). In some embodiments, an electronic circuit, such as a programmable logic circuit, a field programmable gate array (FPGA), or a programmable logic array (PLA), may be personalized by utilizing the state information of the computer-readable program instructions, and the electronic circuit may execute the computer-readable program instructions, thereby realizing various aspects of the present invention.

[0086] Various aspects of the present invention are described herein with reference to the flow charts and / or block diagrams of the methods, devices (systems) and computer program products according to embodiments of the present invention. It should be understood that each box of the flow chart and / or block diagram and the combination of each box in the flow chart and / or block diagram can be implemented by computer-readable program instructions.

[0087] These computer-readable program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, thereby producing a machine, so that when these instructions are executed by the processor of the computer or other programmable data processing device, a device that implements the functions / actions specified in one or more boxes in the flowchart and / or block diagram is generated. These computer-readable program instructions can also be stored in a computer-readable storage medium, and these instructions cause the computer, programmable data processing device, and / or other equipment to work in a specific manner, so that the computer-readable medium storing the instructions includes a manufactured product, which includes instructions for implementing various aspects of the functions / actions specified in one or more boxes in the flowchart and / or block diagram.

[0088] Computer-readable program instructions may also be loaded onto a computer, other programmable data processing apparatus, or other device so that a series of operating steps are performed on the computer, other programmable data processing apparatus, or other device to produce a computer-implemented process, thereby causing the instructions executed on the computer, other programmable data processing apparatus, or other device to implement the functions / actions specified in one or more boxes in the flowchart and / or block diagram.

[0089] The flow charts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the systems, methods and computer program products according to multiple embodiments of the present invention. In this regard, each box in the flow chart or block diagram can represent a part of a module, a program segment or an instruction, and a part of the module, a program segment or an instruction contains one or more executable instructions for realizing the specified logical function. In some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flow chart, and the combination of the boxes in the block diagram and / or flow chart can be implemented by a dedicated hardware-based system that performs the specified function or action, or can be implemented by a combination of dedicated hardware and computer instructions. It is well known to those skilled in the art that it is equivalent to implement it by hardware, implement it by software, and implement it by combining software and hardware.

[0090] Embodiments of the present invention have been described above, and the above description is exemplary, not exhaustive, and is not limited to the disclosed embodiments. Many modifications and variations will be apparent to those of ordinary skill in the art without departing from the scope and spirit of the described embodiments. The selection of terms used herein is intended to best explain the principles of the embodiments, practical applications, or technical improvements in the marketplace, or to enable other persons of ordinary skill in the art to understand the embodiments disclosed herein. The scope of the present invention is defined by the appended claims.

Claims

1. An autonomous positioning method based on a human body spatial displacement model and an inertial navigation system, characterized in that: include: Step 1, obtain the specific force and angular velocity of the person through a self-contained sensor and perform inertial navigation calculation to obtain the three-dimensional attitude angle, three-dimensional velocity, and three-dimensional position at each moment in the sampling window; Step 2, inputting the specific force and angular velocity of the person into the human body spatial displacement model to obtain the three-dimensional displacement of the human body movement within the sampling window; Step 3, inputting the three-dimensional displacement into the human body spatial displacement calculation credibility model to generate the three-dimensional displacement credibility; Step 4, construct an adaptive random clonal error state extended Kalman filter through the 3D misalignment angle, 3D velocity error, 3D position error, 3D displacement and 3D displacement credibility at each moment in the sampling window, and optimally estimate the 3D attitude angle, 3D velocity and 3D position at the current moment through the adaptive random clonal error state extended Kalman filter to obtain the final positioning information; Among them, the full error state vector of the adaptive random clone error state extended Kalman filter is defined as: in, Indicates the length N The error state vector cloned within the sampling window of Represents the error state vector at the current moment; The error state vector of the adaptive random clone error state extended Kalman filter is defined as: ， in, , , They represent misalignment angle, velocity error, and position error respectively; The full state vector of the adaptive random clone error state extended Kalman filter is defined as: in, Indicates the length N The state vector cloned within the sampling window of Represents the current state vector; The state vector of the adaptive stochastic clonal error state extended Kalman filter is defined as: ， in, , , Respectively represent attitude angle, speed, and position; The observation model used in the adaptive random clonal error state extended Kalman filter to describe the three-dimensional displacement relationship between the human body spatial displacement model and the inertial navigation system is defined as: in, Represents the rotation matrix on the yaw axis, which is used to align the human body spatial displacement model with the output displacement of the inertial navigation system. represents the position calculated by the inertial navigation system, Indicated in and The three-dimensional displacement output by the human body spatial displacement model during the time period between represents the observation noise output by the human body spatial displacement model; Extended Kalman filter full state vector for adaptive random clonal error state X The optimal estimate is calculated as follows: in, represents the Kalman filter gain of the filter, represents direct and operation; Extract the full state vector after optimal estimation X Current location in For final positioning.

2. The method according to claim 1, characterized in that Self-contained sensors include accelerometer, gyroscope, magnetometer, barometer.

3. The method according to claim 1, characterized in that The human body spatial displacement model is obtained by training a deep neural network, and the training loss function is defined as follows: in, represents the true value of the three-dimensional displacement obtained by the inertial navigation system, d Represents the three-dimensional displacement output by the human body spatial displacement model, Express N The data are summed up, N represents the sampling window length, Indicates the data sequence number. Represents the bi-norm operation.

4. The method according to claim 3, characterized in that The human body spatial displacement calculation credibility model is obtained by training a deep neural network, and the training loss function is defined as follows: in, Represents the three-dimensional displacement credibility output by the human body spatial displacement calculation credibility model The function of represents the matrix determinant operation, assuming that the displacement credibility of the three axes X, Y, and Z are independent of each other. It can be simplified to the dimension The diagonal matrix is: in, , , They represent the displacement credibility in the three directions of X, Y, and Z output by the human body spatial displacement calculation credibility model. represents a diagonal matrix.

5. The method according to claim 1, characterized in that The error state propagation model used by the adaptive stochastic clonal error state extended Kalman filter is defined as: in, The skew-symmetric matrix representing the specific force in the navigation coordinate system, express The identity matrix of express The zero matrix of , denote the gyroscope and accelerometer measurement noise respectively, k Indicates the data sequence number.

6. The method according to claim 1, characterized in that The observation noise output by the human body spatial displacement model follows a normal distribution ,in The covariance matrix representing the credibility of the three-dimensional displacement is used to update the Kalman filter gain and error state covariance matrix to achieve dynamic tight coupling between the human body spatial displacement model and the inertial navigation system, where , , They represent the displacement credibility in the sampling window time in the three directions of X, Y, and Z output by the human body spatial displacement calculation credibility model. represents a diagonal matrix.

Images