A pedestrian autonomous positioning method based on RISC-V

Abstract

This invention discloses a RISC-V-based method for autonomous pedestrian localization, comprising the following steps: acquiring three-dimensional pedestrian motion data from accelerometer and gyroscope sensors; calculating attitude angles and transforming these angles from the carrier coordinate system to the navigation coordinate system using a coordinate transformation matrix; performing inertial navigation fundamental calculations based on the pedestrian motion data, attitude angles, and coordinate transformation matrix to determine the pedestrian's current attitude, velocity, and position; performing zero-velocity detection based on a neural network to determine if the pedestrian's current state is at a zero-velocity point; if so, proceeding to the next step; otherwise, returning to the first step; updating the pedestrian's current attitude, position, and velocity using a Kalman filter (EKF); and completing autonomous pedestrian localization based on the updated attitude, position, and velocity. This invention reduces noise errors, computational load, and processor execution time and resources, and exhibits good portability.

Description

Technical Field

[0001] This invention relates to the field of vector processor technology, specifically including a pedestrian autonomous localization method based on RISC-V. Background Technology

[0002] RISC-V is an open-source instruction set architecture based on the principles of reduced instruction set computing (RISC). RISC-V employs a modular design, including the basic RV32I, as well as extended instruction sets such as RV32M, RV32F, and RV32V. RV32V is a vector extension instruction set of RISC-V. Unlike traditional SIMD instructions, vector instructions support variable-length vector registers and separate the vector length from the maximum number of operands that can be performed per clock cycle. Based on the RV32V vector extension instruction set, the performance of many algorithms can be improved.

[0003] Positioning and navigation algorithms are widely used in various fields in today's society. Among them, inertial navigation systems based on inertial sensors can achieve autonomous positioning and navigation without relying on external information. Traditional inertial navigation systems have high requirements for sensors and therefore have not been used for pedestrian positioning. However, the emerging micromechanical inertial measurement units (IMMUs) have significant advantages in terms of size, weight, and price, and are widely used in pedestrian inertial navigation systems. A basic IMU typically consists of a three-axis accelerometer and a three-axis gyroscope. However, current measurement methods are susceptible to noise interference. To address this issue, many positioning and navigation algorithms use Kalman filtering to estimate the state of the dynamic system to reduce noise errors. However, because Kalman filtering algorithms involve a large amount of computation, these algorithms require significant resources and time to execute on traditional processor architectures, which limits their application in resource-constrained and performance-limited devices. Summary of the Invention

[0004] To address the aforementioned shortcomings of existing technologies, this invention provides a RISC-V-based pedestrian autonomous localization method that solves the problems of existing technologies being susceptible to noise interference, having poor portability and modifiability, high resource and time costs, and being limited in application to resource-constrained and performance-limited devices.

[0005] To achieve the above-mentioned objectives, the technical solution adopted by this invention is: a pedestrian autonomous localization method based on RISC-V, comprising the following steps:

[0006] S1. Acquire three-dimensional data of pedestrian motion from accelerometer and gyroscope sensors;

[0007] S2. Calculate the attitude angles based on the three-dimensional data of pedestrian movement, and transform the attitude angles from the carrier coordinate system to the navigation coordinate system using the coordinate transformation matrix.

[0008] S3. Based on the pedestrian's three-dimensional motion data, attitude angles, and coordinate transformation matrix, perform basic inertial navigation calculations to determine the pedestrian's current attitude, velocity, and position.

[0009] S4. Perform zero-velocity detection based on neural networks using the pedestrian's current posture, speed, and position. Determine whether the pedestrian's current state is a zero-velocity point. If yes, proceed to step S5; otherwise, proceed to step S1.

[0010] S5. Use the Kalman filter (EKF) to calculate and update the pedestrian's current attitude, position, and velocity; obtain the updated attitude, updated position, and updated velocity.

[0011] S6. Complete pedestrian autonomous localization based on the updated posture, updated position, and updated velocity.

[0012] Furthermore, the specific implementation of step S2 is as follows:

[0013] S2-1. Calculate the attitude angles based on the three-dimensional data of pedestrian movement; the attitude angles include roll angle γ, pitch angle θ, and heading angle ψ.

[0014] S2-2. Transform the roll angle γ, pitch angle θ, and heading angle ψ from the carrier coordinate system to the navigation coordinate system using a 3*3 coordinate transformation matrix, and store the values ​​of the coordinate transformation matrix into a 3*3 floating-point array C.

[0015] Furthermore, the specific implementation of step S3 is as follows:

[0016] S3-1. Calculate the average value of the three-dimensional pedestrian motion data collected by the gyroscope sensor to obtain ω. x (t), ω y (t), ω z (t), and the average value ω x (t), ω y (t), ω z (t) are stored sequentially into the floating-point array variable gyr[3]; where t is time; ω x (t) represents the average value of the first dimension of data; ω y (t) represents the average value of the second dimension data; ω z (t) represents the average value of the third dimension of data;

[0017] S3-2, According to ω x (t), ω y (t), ω z (t) Calculate the skew-symmetric matrix Ω t ;

[0018] S3-3, According to the formula:

[0019]

[0020] Obtain the updated pose matrix As the current posture of the pedestrian; among them C is a 3x3 floating-point array; I is the identity matrix; Δt is the sampling time interval; Let be the skew-symmetric matrix of the gyroscope at time t;

[0021] S3-4, Based on the updated pose matrix Get the pedestrian's current speed and position.

[0022] Furthermore, the specific implementation of steps S3-4 is as follows:

[0023] According to the formula:

[0024]

[0025] v(t)=v(t-Δt)+[a n (t-Δt)+a n (t)*Δt / 2]

[0026] p(t)=p(t-Δt)+[v(t-Δt)+v(t)*Δt / 2]

[0027] The inertial navigation acceleration a varies with time. n (t), velocity v(t) and position p(t); where a b (t) is a matrix composed of the average values ​​of the three-dimensional pedestrian motion data collected by the accelerometer; [·] T The matrix is ​​the transpose; v(t-Δt) is the velocity at the previous moment; p(t-Δt) is the position at the previous moment; a n (t-Δt) represents the acceleration at the previous moment.

[0028] Furthermore, the specific implementation of step S5 is as follows:

[0029] S5-1. According to the formula:

[0030] P k|k-1 =F k P k-1 (F k ) T +Q

[0031] Obtain the covariance matrix P of the prediction vector at time k. k|k-1 Where Q represents the covariance matrix of the noise vector; F k Represents the state transition matrix. I 3×3 It is a 3x3 identity matrix. A skew-symmetric cross matrix composed of three-axis acceleration measurements in the navigation coordinate system. This represents the acceleration at time k along the z-axis in the navigation coordinate system. Let represent the acceleration at time k along the y-axis in the navigation coordinate system. This represents the acceleration at time k along the x-axis in the navigation coordinate system; (·) T Represents the transpose of a matrix;

[0032] S5-2, According to the formula:

[0033]

[0034] Obtain the error vector Among them, H k The observation matrix; Let be the transpose of the observation matrix; R represents the covariance matrix of a multivariate independent normal distribution; (·) -1 Represents the inverse of a matrix;

[0035] S5-3. Update the pedestrian's pose, position, and velocity based on the error vector; obtain the updated pose, updated position, and updated velocity.

[0036] The beneficial effects of this invention are as follows: This invention uses the Kalman filtering method, which can reduce noise error; it can reduce the number of instructions executed during operation, thereby reducing the amount of computation, reducing the time and resources required for processor execution, increasing the feasibility of this invention in resource-constrained and performance-limited devices, and overcoming the disadvantages of traditional instructions having fixed data widths, requiring programs to be constructed according to specific data widths, resulting in poor portability. Attached Figure Description

[0037] Figure 1 This is a flowchart of the present invention;

[0038] Figure 2 This is a system block diagram of the present invention. Detailed Implementation

[0039] The specific embodiments of the present invention are described below to enable those skilled in the art to understand the present invention. However, it should be understood that the present invention is not limited to the scope of the specific embodiments. For those skilled in the art, various changes are obvious as long as they are within the spirit and scope of the present invention as defined and determined by the appended claims. All inventions utilizing the concept of the present invention are protected.

[0040] like Figure 1 As shown, a pedestrian autonomous localization method based on RISC-V includes the following steps:

[0041] S1. Acquire three-dimensional data of pedestrian motion from accelerometer and gyroscope sensors;

[0042] S2. Calculate the attitude angles based on the three-dimensional data of pedestrian movement, and transform the attitude angles from the carrier coordinate system to the navigation coordinate system using the coordinate transformation matrix.

[0043] S3. Based on the pedestrian's three-dimensional motion data, attitude angles, and coordinate transformation matrix, perform basic inertial navigation calculations to determine the pedestrian's current attitude, velocity, and position.

[0044] S4. Perform zero-velocity detection based on neural networks using the pedestrian's current posture, speed, and position. Determine whether the pedestrian's current state is a zero-velocity point. If yes, proceed to step S5; otherwise, proceed to step S1.

[0045] S5. Use the Kalman filter (EKF) to calculate and update the pedestrian's current attitude, position, and velocity; obtain the updated attitude, updated position, and updated velocity.

[0046] S6. Complete pedestrian autonomous localization based on the updated posture, updated position, and updated velocity.

[0047] The specific implementation method of step S2 is as follows:

[0048] S2-1. Calculate the attitude angles based on the three-dimensional data of pedestrian movement; the attitude angles include roll angle γ, pitch angle θ, and heading angle ψ.

[0049] S2-2. Transform the roll angle γ, pitch angle θ, and heading angle ψ from the carrier coordinate system to the navigation coordinate system using a 3*3 coordinate transformation matrix, and store the values ​​of the coordinate transformation matrix into a 3*3 floating-point array C.

[0050] The specific implementation method of step S3 is as follows:

[0051] S3-1. Calculate the average value of the three-dimensional pedestrian motion data collected by the gyroscope sensor to obtain ω. x (t), ω y (t), ω z (t), and the average value ω x (t), ω y (t), ω z (t) are stored sequentially into the floating-point array variable gyr[3]; where t is time; ω x (t) represents the average value of the first dimension of data; ω y (t) represents the average value of the second dimension data; ω z (t) represents the average value of the third dimension of data;

[0052] S3-2, According to ω x (t), ωy (t), ω z (t) Calculate the skew-symmetric matrix Ω t ;

[0053] S3-3, According to the formula:

[0054]

[0055] Obtain the updated pose matrix As the current posture of the pedestrian; among them C is a 3x3 floating-point array; I is the identity matrix; Δt is the sampling time interval; Let be the skew-symmetric matrix of the gyroscope at time t;

[0056] S3-4, Based on the updated pose matrix Get the pedestrian's current speed and position.

[0057] The specific implementation method of step S3-4 is as follows:

[0058] According to the formula:

[0059]

[0060] v(t)=v(t-Δt)+[a n (t-Δt)+a n (t)*Δt / 2]

[0061] p(t)=p(t-Δt)+[v(t-Δt)+v(t)*Δt / 2]

[0062] The inertial navigation acceleration a varies with time. n (t), velocity v(t) and position p(t); where a b (t) is a matrix composed of the average values ​​of the three-dimensional pedestrian motion data collected by the accelerometer; [·] T The matrix is ​​the transpose; v(t-Δt) is the velocity at the previous moment; p(t-Δt) is the position at the previous moment; a n (t-Δt) represents the acceleration at the previous moment.

[0063] The specific implementation method of step S5 is as follows:

[0064] S5-1. According to the formula:

[0065] P k|k-1 =F k P k-1 (F k ) T +Q

[0066] Obtain the covariance matrix Pk of the prediction vector at time k.k-1 Where Q represents the covariance matrix of the noise vector; F k Represents the state transition matrix. I 3×3 It is a 3x3 identity matrix. A skew-symmetric cross matrix composed of three-axis acceleration measurements in the navigation coordinate system. This represents the acceleration at time k along the z-axis in the navigation coordinate system. Let represent the acceleration at time k along the y-axis in the navigation coordinate system. This represents the acceleration at time k along the x-axis in the navigation coordinate system; (·) T Represents the transpose of a matrix;

[0067] S5-2, According to the formula:

[0068]

[0069] Obtain the error vector Among them, H k The observation matrix; Let be the transpose of the observation matrix; R represents the covariance matrix of a multivariate independent normal distribution; (·) -1 Represents the inverse of a matrix;

[0070] S5-3. Update the pedestrian's pose, position, and velocity based on the error vector; obtain the updated pose, updated position, and updated velocity.

[0071] like Figure 2 As shown in the diagram, the system block diagram mainly includes a basic calculation module, a zero-velocity update module, and an autonomous positioning module. The basic calculation module is used to obtain the pedestrian's attitude angle, velocity, and position information from data collected from the gyroscope and accelerometer. The zero-velocity update module is used to distinguish between zero-velocity points and non-zero-velocity points. The zero-velocity update module, combined with the autonomous positioning module, can effectively correct the current attitude angle, velocity, and position to suppress trajectory divergence caused by errors.

[0072] In one embodiment of this invention, in existing RISC-based matrix addition, subtraction, and multiplication methods, RISC-based computation typically uses the most basic SISD (Single Instruction Single Data) approach. Taking matrix addition as an example, in SISD, the computer loads an element from one matrix and immediately adds it to an element from another matrix, storing the result in the address corresponding to the result matrix. This process of adding elements one by one and repeating it multiple times is required to complete the matrix addition calculation, and each addition involves three instructions: loading, adding, and storing, which is very time-consuming. In our improved matrix addition method, we can directly read matrix elements of effective vector length (vl) from the address and add these matrix elements simultaneously in parallel. Thus, processing multiple elements still only requires three instructions: loading, adding, and storing, which effectively reduces computation time and the number of instructions. The principles of matrix subtraction and matrix multiplication are basically the same as those of matrix addition.

[0073] Inertial measurement units (including gyroscopes and accelerometers) were used to collect data from 131,601 points around the teaching building, and a pedestrian localization algorithm was used to calculate the subject's trajectory based on this data. In Linux, compiling the original algorithm using the GCC toolchain took 20.317361 seconds and executed 2,031,736,5811 instructions. Compiling the improved algorithm took only 1.670458 seconds and executed 1,670,463,626 instructions, a reduction of 91.7%, effectively reducing computation time and the number of instructions.

[0074] The matrix addition, matrix subtraction, and matrix multiplication used in this invention rely on the RISC-V vector extension instruction set and vector computation.

[0075] Matrix addition, call the header file<riscv_vector.h> The inline assembly function `vsetvl_e32m1` in the code obtains the effective vector length (vl) based on a given matrix, SEW, and LMUL; it defines pointer variables `*ptr_a`, `*ptr_b`, and `*ptr_c` pointing to the matrices to be added (a and b) and the resulting matrix (c), respectively; and it calls the header file...<riscv_vector.h> The inline assembly function `vle32_v_f32ml` in the header file is used to load matrices `a` and `b` of length `vl` from the memory pointed to by the starting address of the pointer variable into `vec_a` and `vec_b` of type `vfloat32m1_t`; it calls the header file...<riscv_vector.h> The inline assembly function `vfadd_vv_f32m1` in the header file is used to add vectors `vec_a` and `vec_b` and store the result in vector `vec_c`; it calls the header file...<riscv_vector.h> The inline assembly function vse32_v_f32m1 in the file is used to store the result of the addition into the memory pointed to by the starting address of the pointer variable ptr_c;

[0076] Matrix subtraction, call the header file<riscv_vector.h> The inline assembly function `vsetvl_e32m1` in the code is used to obtain the effective vector length (vl) based on a given matrix, SEW, and LMUL; pointer variables `*ptr_a`, `*ptr_b`, and `*ptr_c` are defined to point to the matrices to be operated on, `a` and `b`, and the result matrix, `c`, respectively; the header file is called.<riscv_vector.h> The inline assembly function `vle32_v_f32ml` in the header file is used to load matrices `a` and `b` of length `vl` from the memory pointed to by the starting address of the pointer variable into `vec_a` and `vec_b` of type `vfloat32m1_t`; it calls the header file...<riscv_vector.h> The inline assembly function `vfsub_vv_f32m1` in the header file is used to subtract vectors `vec_a` and `vec_b` and store the result in vector `vec_c`; it calls the header file...<riscv_vector.h> The inline assembly function vse32_v_f32m1 in the file is used to store the result of the subtraction into the memory pointed to by the starting address of the pointer variable ptr_c;

[0077] Matrix multiplication, call the header file<riscv_vector.h> The inline assembly function `vsetvl_e32m1` in the code is used to obtain the effective vector length (vl) based on a given matrix, SEW, and LMUL. Pointer variables `*b_n_ptr` and `*c_n_ptr` are defined, pointing to the matrix `b` to be operated on and the result matrix `c`, respectively. Pointer variables `*a_k_ptr` and `*b_k_ptr` are defined, pointing to the matrix `a` to be operated on and the pointer variable `*b_n_ptr`, respectively. A `vfloat32m1_t` matrix `acc` is defined to temporarily store the result of element-wise multiplication in each iteration. The header file is called.<riscv_vector.h> The inline assembly function `vfmv_v_f_f32m1` in the header file is used to initialize the matrix `acc`;<riscv_vector.h> The inline assembly function `vle32_v_f32ml` in the header file is used to load the matrix `b_k_ptr` of length `vl` from the memory pointed to by the pointer variable `start` into the `vfloat32m1_t` type variable `b_n_data`.<riscv_vector.h> The inline assembly function `vfmacc_vf_f32m1` in the header file is used for iteration. It multiplies the elements corresponding to length `vl` in matrices `a` and `b`, adds them to matrix `acc`, and stores the result in matrix `acc`. This process is repeated until all elements in the matrix have been calculated. The header file is then called.<riscv_vector.h> The inline assembly function vse32_v_f32m1 in the code stores the result of the multiplication into the memory pointed to by the starting address of the pointer variable c_n_ptr.

[0078] The cross-compilation simulation method of this invention is as follows: install LLVM 14 and RISCV-GNU-toolchain on the Linux system; install Spike instruction set simulator and pk proxy kernel; create a makefile; compile and run the target program according to the makefile; wherein, the compilation and running of the target program according to the makefile uses the LLVM Clang compiler in conjunction with the GCC toolchain to compile the program; and simulate the target program in Spike.

[0079] This invention uses Kalman filtering, which can reduce noise errors; it can also reduce the number of instructions executed during operation, thereby reducing the amount of computation, reducing the time and resources required for processor execution, increasing the feasibility of using this invention in resource-constrained and performance-limited devices, and has good portability.

Claims

1. A pedestrian autonomous localization method based on RISC-V, characterized in that, Includes the following steps: S1. Acquire three-dimensional data of pedestrian motion from accelerometer and gyroscope sensors; S2. Calculate the attitude angles based on the three-dimensional data of pedestrian movement, and transform the attitude angles from the carrier coordinate system to the navigation coordinate system using the coordinate transformation matrix. S3. Based on the pedestrian's three-dimensional motion data, attitude angles, and coordinate transformation matrix, perform basic inertial navigation calculations to determine the pedestrian's current attitude, velocity, and position. S4. Perform zero-velocity detection based on neural networks using the pedestrian's current posture, speed, and position. Determine whether the pedestrian's current state is a zero-velocity point. If so, proceed to step S5. Otherwise, proceed to step S1; S5. Use the Kalman filter (EKF) to calculate and update the pedestrian's current attitude, position, and velocity; obtain the updated attitude, updated position, and updated velocity. S6. Complete pedestrian autonomous localization based on the updated posture, updated position, and updated velocity; The specific implementation method of step S2 is as follows: S2-1. Calculate the attitude angles based on the pedestrian's three-dimensional motion data; the attitude angles include the roll angle. Pitch angle and heading angle ; S2-2, Adjust the roll angle Pitch angle and heading angle The 3x3 coordinate transformation matrix is ​​used to transform the vehicle coordinate system to the navigation coordinate system, and the value of the coordinate transformation matrix is ​​stored in a 3x3 floating-point array C. The specific implementation method of step S3 is as follows: S3-1, averaging the pedestrian motion three-dimensional data collected by the gyroscope sensor to obtain... and average They are stored sequentially into the floating-point type array variable gyr[3]; where, t For time; The average value of the first dimension of data; The average value of the second dimension of data; The average value of the third dimension data; S3-2, according to Calculate the skew-symmetric matrix ; S3-3, According to the formula: Obtain the updated pose matrix This represents the pedestrian's current posture; among them, The array is a 3x3 floating-point array C; It is the identity matrix; The sampling time interval; Gyroscope t The skew-symmetric matrix at time step; S3-4, Based on the updated pose matrix Get the pedestrian's current speed and position.

2. The pedestrian autonomous localization method based on RISC-V according to claim 1, characterized in that, The specific implementation method of step S3-4 is as follows: According to the formula: Obtain the time-varying acceleration of inertial navigation ,speed and location ;in, A matrix composed of the average values ​​of three-dimensional pedestrian motion data collected by an accelerometer; This is the transpose of the matrix; The velocity at the previous moment; The position at the previous moment; This represents the acceleration at the previous moment.

3. The pedestrian autonomous localization method based on RISC-V according to claim 2, characterized in that, The specific implementation method of step S5 is as follows: S5-1. According to the formula: get k Covariance matrix of the prediction vector at time step ;in, Q The covariance matrix representing the noise vector; Represents the state transition matrix. , It is a 3x3 identity matrix. A skew-symmetric cross matrix composed of three-axis acceleration measurements in the navigation coordinate system. Represents the z-axis direction in the navigation coordinate system k acceleration at any moment Indicates the y-axis direction in the navigation coordinate system k acceleration at any moment Represents the x-axis direction in the navigation coordinate system k Acceleration at any given moment; (·) T Represents the transpose of a matrix; S5-2, According to the formula: Obtain the error vector ;in, The observation matrix; This is the transpose of the observation matrix; R The covariance matrix represents a multivariate independent normal distribution; (·) -1 Represents the inverse of a matrix; S5-3. Update the pedestrian's pose, position, and velocity based on the error vector; obtain the updated pose, updated position, and updated velocity.

Images