A quadruped robot positioning method, system, storage medium and robot

Abstract

The present invention relates to the field of robot positioning technology, and discloses a method, system, storage medium and robot for positioning a quadruped robot, including: judging whether the inertial measurement unit is in a zero-speed state based on inertial measurement unit data; for each leg, converting the inertial measurement unit data into posture information through posture solution based on the inertial measurement unit data and the judgment result of the zero-speed state; obtaining a priori state estimation through Kalman filtering based on the inertial measurement unit data and posture information of all legs; obtaining a posteriori state estimation through zero-speed correction and delayed Kalman filtering based on the inertial measurement unit data, posture information and the judgment result of the zero-speed state; for the posteriori state estimation, obtaining the position of the quadruped robot through inertial measurement unit distance constraints and single-step stride constraints. Effectively reduce the error accumulation problem of the robot in motion, thereby realizing accurate estimation of the robot position.

Description

Technical Field

[0001] The present invention relates to the field of robot positioning technology, and in particular to a quadruped robot positioning method, system, storage medium and robot. Background Art

[0002] The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

[0003] Whether it is navigation or task execution, the reliability and effectiveness of quadruped robots are inseparable from accurate positioning. Therefore, accurate positioning is the basis for quadruped robots to achieve various tasks. However, the accurate positioning of quadruped robots has always been a challenging problem. Traditional positioning methods rely on devices such as GPS (Global Positioning System) or lidar. However, these devices may be limited in certain environments, such as indoors, inside buildings or rugged terrain. The development of quadruped robots in rescue, exploration, military and other aspects is often accompanied by complex terrain and harsh environment, which has greatly restricted their development. The inertial navigation system reflects its unique value among various positioning methods with its advantages of being free from environmental restrictions and low cost. Compared with traditional positioning methods, the inertial navigation system is not restricted by the environment and can achieve accurate positioning indoors, underground, mountainous areas or other areas with poor GPS signals without relying on the support of external devices, thereby reducing the positioning cost and system complexity.

[0004] However, in the existing technology, due to factors such as IMU (inertial measurement unit) integration drift and attitude solution errors, the accuracy of long-term positioning based solely on IMU is often difficult to meet application requirements. Inertial navigation solution algorithms usually need to be integrated with other sensors (such as global positioning systems, magnetometers, etc.) to correct errors and improve positioning accuracy and stability. However, this method requires expensive hardware equipment and external signal support, which limits the application scope and popularity of quadruped robots. Summary of the invention

[0005] In order to solve the above-mentioned problems, the present invention provides a quadruped robot positioning method, system, storage medium and robot, which utilize inertial measurement unit data of at least two legs and adopt zero-speed correction technology assisted by IMU distance constraints and single-step stride constraints to position the legged robot, effectively reducing the error accumulation problem of the robot during movement, thereby achieving accurate estimation of the robot's position and improving the accuracy and reliability of position estimation.

[0006] In order to achieve the above object, the present invention adopts the following technical solution:

[0007] A first aspect of the present invention provides a quadruped robot positioning method, comprising:

[0008] acquiring inertial measurement unit data on at least two legs of the quadruped robot;

[0009] Based on the inertial measurement unit data, determine whether the inertial measurement unit is in a zero speed state;

[0010] For each leg, based on the inertial measurement unit data and the judgment result of the zero-speed state, the inertial measurement unit data is converted into attitude information through attitude solution;

[0011] Based on the inertial measurement unit data and attitude information of all legs, the prior state estimate is obtained through Kalman filtering;

[0012] Based on the inertial measurement unit data, attitude information and the judgment result of the zero-speed state, for the prior state estimation, a posteriori state estimation is obtained through zero-speed correction and delayed Kalman filtering;

[0013] For the posterior state estimation, the position of the quadruped robot is obtained through the inertial measurement unit distance constraint and the single-step stride constraint.

[0014] Furthermore, the step of posture solving includes:

[0015] Rotate the gravitational acceleration from the geographic coordinate system to the carrier coordinate system;

[0016] The gravitational acceleration in the carrier coordinate system is cross-multiplied with the acceleration in the inertial measurement unit data to obtain the error correction value;

[0017] Based on the error correction amount, according to proportional integral adjustment, when the inertial measurement unit is in a zero speed state, the angular velocity in the inertial measurement unit data is corrected for error;

[0018] Based on the corrected angular velocity, the attitude information is calculated.

[0019] Further, the zero speed correction is expressed as: ;in, Indicates time The posterior state estimate of ; Indicates time A priori state estimate of ; Represents the prior state covariance matrix at time k; Indicates time The posterior state covariance matrix of ; Indicates time The Kalman gain of is the covariance matrix of the observation noise; represents the state observation matrix; represents zero-speed pseudo-measurement; Indicates the judgment result of zero speed state.

[0020] Furthermore, the delayed Kalman filtering step includes:

[0021] Assume that at time The posterior state estimate is updated on , then calculate the time The correction value of the prior state estimate of : ,in, Indicates time The prior state estimate of Indicates time The Kalman gain, represents zero-speed pseudo-measurement, represents the state observation matrix;

[0022] Update the past through recursive relations The forecast amount at each moment: , ;in, , is the number of moments between the two latest zero speeds, It's time The prior state estimate of Indicates time The posterior state estimate of Represents the attenuation coefficient.

[0023] Furthermore, after preprocessing the inertial measurement unit data using a Butterworth filter, it is determined whether the inertial measurement unit is in a zero speed state.

[0024] Furthermore, the single-step stride constraint is: within the range of a single stride, the difference in the positions of the quadruped robot at any two moments is limited by an upper threshold.

[0025] Furthermore, the inertial measurement unit distance constraint is that at the same moment, the position difference between the two inertial measurement units is limited by an upper threshold.

[0026] A second aspect of the present invention provides a quadruped robot positioning system, comprising:

[0027] A data acquisition module, configured to: acquire inertial measurement unit data on at least two legs of the quadruped robot;

[0028] A zero speed determination module is configured to: determine whether the inertial measurement unit is in a zero speed state based on the inertial measurement unit data;

[0029] A posture calculation module is configured to: for each leg, based on the inertial measurement unit data and the determination result of the zero-speed state, convert the inertial measurement unit data into posture information through posture calculation;

[0030] A Kalman filter module is configured to: obtain a priori state estimates based on inertial measurement unit data and posture information of all legs through Kalman filtering;

[0031] A zero-speed correction and time-delay smoothing module is configured to: based on the inertial measurement unit data, attitude information and the judgment result of the zero-speed state, obtain a posteriori state estimation through zero-speed correction and delayed Kalman filtering for the prior state estimation;

[0032] A constraint module is configured to: for a posteriori state estimation, obtain the position of the quadruped robot through inertial measurement unit distance constraints and single-step stride constraints. The third aspect of the present invention provides a computer-readable storage medium on which a computer program is stored, which is executed by a processor. When the program is executed by the processor, the steps in a quadruped robot positioning method as described above are implemented.

[0033] A fourth aspect of the present invention provides a robot that adopts the steps in a quadruped robot positioning method as described above.

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

[0035] The present invention provides a positioning method for a quadruped robot, which utilizes inertial measurement unit data of at least two legs and adopts zero-speed correction technology assisted by IMU distance constraints and single-step stride constraints to position the legged robot, effectively reducing the error accumulation problem of the robot during movement, thereby achieving accurate estimation of the robot's position, thereby improving the accuracy and reliability of position estimation.

[0036] The present invention provides a quadruped robot positioning method, which utilizes IMU distance constraints and single-step stride constraints, and by utilizing data from at least two IMU sensors, achieves accurate estimation of the robot's position, thereby effectively reducing costs.

[0037] The present invention provides a quadruped robot positioning method, which combines time-delay smoothing technology and Kalman filtering algorithm to deal with the problem of tortuous oscillation in positioning results. By smoothing the robot position solution results, the error accumulation problem in the robot movement process can be effectively controlled, the positioning accuracy is further improved, and the accurate estimation of the robot position can be achieved. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] The accompanying drawings, which constitute a part of the specification of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention, but do not constitute limitations of the present invention.

[0039] Figure 1 This is a flow chart of a quadruped robot positioning method according to Embodiment 1 of the present invention;

[0040] Figure 2 This is a schematic diagram of the installation position of the dual IMU sensor of Example 1 of the present invention. DETAILED DESCRIPTION

[0041] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0042] It should be noted that the following detailed descriptions are exemplary and are intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0043] In the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other. The present invention is further described below with reference to the accompanying drawings and embodiments.

[0044] Embodiment 1

[0045] The purpose of this first embodiment is to provide a positioning method for a quadruped robot.

[0046] In order to improve the autonomous positioning accuracy of quadruped robots and reduce the positioning cost, a low-cost, efficient and accurate quadruped robot positioning method was developed based on the inertial measurement unit to address the shortcomings of existing research. The method can accurately estimate the position and posture of the quadruped robot without relying on external equipment.

[0047] A quadruped robot positioning method provided in this embodiment has carried out research on three parts: a Kalman filter pose estimation algorithm based on zero-speed information, a Kalman filter algorithm based on distance constraints, and a time-delay smoothing Kalman filter. It aims to improve the accuracy of low-cost quadruped robot positioning in response to the shortcomings of current quadruped robot positioning methods.

[0048] This embodiment provides a quadruped robot positioning method, comprising the following steps:

[0049] Step 1: Dual IMU sensor peripheral configuration.

[0050] like Figure 2As shown in the figure, the structure of the quadruped robot includes a body (main body) and four limbs. There are four legs connected to the main body, which are located at the front left (FL), front right (FR), rear left (HL), and rear right (HR) positions. Two IMU1s are installed on the front left leg and rear right leg of the quadruped robot, respectively, at the end of the calf, close to the ground.

[0051] Step 2: Construct the Kalman fusion algorithm framework.

[0052] like Figure 1 As shown, the Kalman fusion algorithm framework includes IMU data collection (including rear right leg IMU data collection and front left leg IMU data collection), zero speed detection, INS (inertial navigation system) algorithm, ZUPT (zero speed update), delayed Kalman filter, stride constraint and dual IMU distance constraint.

[0053] The dual IMU system collects data through the IMUs installed on the robot's front left leg (FL) and rear right leg (HR), and uses INS (inertial navigation system) mechanism processing to infer the posture and position; with the assistance of the zero-speed detector, the velocity information of each IMU is processed by ZUPT (zero-speed update) and smoothed by a delayed Kalman filter; then, the IMU data is optimized through stride constraints and dual-IMU distance constraints, and finally the trajectory of the robot's movement is output.

[0054] Through the synergistic effect of the above parts, the IMU data can be effectively used to achieve accurate estimation and tracking of the robot's position, providing reliable support for the stability and accuracy of navigation.

[0055] The main goal of Kalman filtering is to estimate the state of a system by fusing the dynamic model of the system (quadruped robot) with actual observation data. In Kalman filtering, the system state is represented by a state vector, which contains all the relevant variables of the system. The filter predicts the next moment of the state by using the system's dynamic equations and revising this prediction based on observed information.

[0056] By fusing inertial sensor data with information from other sensors, Kalman filtering can improve the positioning system's accurate estimation of the platform's position, velocity, and attitude, reduce the impact of error accumulation and positioning interruptions, and thus enable the positioning system to remain stable and reliable in complex environments and dynamic motion conditions.

[0057] Step 3: For the front left leg or the rear right leg, the IMU sensor collects the accelerometer output value (acceleration) and the gyroscope measurement value (angular velocity).

[0058] Step 4: Zero speed detection, get zero speed information (judgment result of whether IMU is in zero speed state) ).

[0059] For the front left leg or the rear right leg, the IMU data (angular velocity and acceleration) collected in step 3 are preprocessed and used for zero-speed judgment, and then the zero-speed judgment algorithm is used to identify whether the IMU is in a zero-speed state.

[0060] First, due to the presence of noise in the high-frequency data collected by the IMU, it is difficult to directly determine whether the robot dog (quadruped robot) is currently in a zero-speed state. A Butterwörf bandpass filter is designed to solve this problem. The design principle of the Butterwörf filter is based on the optimization of frequency response to effectively filter out or retain signals within a specific frequency range.

[0061] Then, according to the preprocessed data and based on a certain threshold, it is determined whether the robot dog is in a zero speed state at the current moment.

[0062] Specifically, after preprocessing the IMU data with a Butterfly filter, the zero-speed detector determines whether the amplitude of the angular velocity in the IMU data is lower than the zero-speed judgment threshold. If so, it is determined that the quadruped robot is in a zero-speed state.

[0063] Step 5: For the front left leg or the rear right leg, use the accelerometer output value to correct the gyroscope measurement value. Based on the corrected angular velocity, the attitude information is obtained through the attitude solution in the INS algorithm to provide an estimate of the current attitude of the robot. The specific steps include:

[0064] Step (501), rotate the gravity acceleration from the geographic coordinate system to the carrier coordinate system: .

[0065] The carrier coordinate system is expressed as The navigation coordinate system (geographic coordinate system) is expressed as the {n} system. The navigation coordinate system uses the East-North-Sky (ENU) coordinate system. Indicates from Tie to The rotation matrix of the system, =( , , ) indicates relative to The gravitational acceleration of the system, , and They represent the gravitational acceleration in The weight of the axis, Represents the gravitational acceleration in the navigation coordinate system.

[0066] Step (502), assuming that the actual acceleration measured by the IMU accelerometer is , cross-product the gravity acceleration in the carrier coordinate system with the actual IMU accelerometer output to get the error correction: .

[0067] in, represents the error compensation of angular velocity (i.e., the error correction amount); is the acceleration measured by the accelerometer, Respectively represent the acceleration in The weight of the axis.

[0068] Step (503), based on the error correction amount According to the PI (proportional integral) adjustment, the actual IMU gyroscope measurement value (angular velocity) is corrected for error, and the PI parameters are adjusted according to the system state. When the system is in a non-zero speed state, and Set to zero to ensure the accuracy and stability of the pose estimation system: .in, Represents the angular velocity measured by the IMU gyroscope; Represents the IMU gyroscope measurement value after PI correction; is the proportional gain; is the integral gain.

[0069] Step (504), based on the corrected angular velocity , perform attitude calculation and get k +1 moment IMU attitude information, the steps are as follows:

[0070] (1) Calculate the derivative of the quaternion using the corrected angular velocity, ,in, is the quaternion of the IMU attitude from the n system to the b system at time k (i.e., k IMU attitude information at all times), represents the angular velocity at time k The skew-symmetric matrix is represents the derivative of the attitude quaternion at time k;

[0071] (2) Integrate the derivative of the attitude quaternion at time k to obtain the attitude quaternion at time k+1, which represents the attitude information of the IMU at time k+1: ,in, is the time step.

[0072] Step 6: Kalman filter prediction process. For the front left leg and the rear right leg, the Kalman filter uses IMU data and posture information to predict the robot's state at time k. Make an estimate and get the prior state estimate at time k .

[0073] The core of the Kalman filter is two main steps: prediction step and update step. In the prediction step, the dynamic model of the system is used to predict the current state and calculate the predicted state covariance. In the update step, the predicted state is corrected using the observed data to obtain the final state estimate and covariance. In this way, the Kalman filter can effectively process information from different sources and provide an optimal estimate of the system state.

[0074] In the framework of the dual IMU system, two sub-IMU systems are connected to the front left leg and rear right leg of the legged robot respectively, and the Kalman filter is applied to information fusion.

[0075] According to the kinematic formula .in, It’s speed; It is the location; Indicates from Tie to The rotation matrix of the system; It is the acceleration data measured by IMU in the carrier coordinate system; Represents the gravitational acceleration relative to the navigation coordinate system; It represents the derivative of velocity, i.e. the rate of change of velocity; represents the derivative of position, that is, velocity.

[0076] Using the above kinematic formulas, the state model can be written.

[0077] The state vector of the dual IMU system at time k is defined as follows: .in, and Respectively represent the speed and position of the left IMU; and Respectively represent the speed and position of the right IMU; subscript represents the IMU attached to the left front leg, Represents the IMU attached to the right hind leg.

[0078] Step 601: Use the attitude information of the IMU at time k Convert the acceleration in the carrier coordinate system collected in step 3 to the navigation coordinate system to obtain the acceleration in the navigation coordinate system at time k .

[0079] Step 602, the prediction process of Kalman filtering is as follows. The prediction process predicts the state at the next moment through the state transfer equation and updates the state covariance matrix: .in, Indicated in The prior state estimate at time t; Represents the posterior state estimate at time k-1; represents the posterior state covariance matrix at time k-1; Represents the prior state covariance matrix at time k; Indicates time The control input is the acceleration measured by the IMU in the navigation coordinate system; represents the state transfer matrix; represents the control input matrix; Represents the covariance matrix of the state noise.

[0080] in, , ,in, represents an n-row and m-column zero matrix, represents the n-row and m-column identity matrix; , The corresponding matrix represents the left front leg; , The corresponding matrix represents the right hind leg; , .

[0081] Step 7: Zero speed update (zero speed correction) and delayed Kalman filtering.

[0082] This embodiment proposes a Kalman filter algorithm design assisted by ZUPT (Zero Velocity Update, zero speed correction).

[0083] The inertial navigation system is a navigation technology based on inertial measurement elements. Its principle is to measure acceleration and angular velocity through IMU, so as to infer the posture, velocity and displacement information of the robot moving in space. IMU measures at a high sampling rate and is usually composed of a gyroscope and an accelerometer. The core idea of ​​posture estimation is to infer the robot's posture and position changes by integrating the IMU measurement data. Specifically, the angular velocity measured by the gyroscope is integrated to obtain the posture change, and the acceleration measured by the accelerometer is double-integrated to obtain the displacement after subtracting the gravity acceleration. Using this method to solve the posture and position, accurate results can be obtained in a short time, but it will cause serious integral drift on a longer time scale. First, the acceleration measurement value needs to be double-integrated, which will cause the measurement error to be amplified twice. Secondly, the acceleration measurement value needs to subtract the gravity acceleration, and any error in the posture solution will cause the gravity acceleration to leak into other components. In order to solve the drift problem, this embodiment uses the Kalman filter fusion algorithm to weightedly fuse different observation information, thereby improving the accuracy of posture and position solution. In this process, zero-speed information is introduced as important auxiliary observation information, which can effectively limit the speed error and further reduce the position error.

[0084] As a commonly used state estimation method, Kalman filtering can effectively combine measurements with prior knowledge, thereby improving the accuracy and stability of pose estimation.

[0085] Zero velocity update uses zero velocity information to limit velocity solution errors, and thus position solution errors. When the IMU is assumed to be stationary at a certain moment, the state of the IMU at this moment is called the zero velocity state. By detecting and analyzing the amplitude of the angular velocity, it can be determined whether the IMU is in a zero velocity state. Once the zero velocity state is detected, the ZUPT algorithm will set the velocity to zero to ensure that the IMU velocity estimate is consistent with the actual situation. Through Kalman filter fusion and position solution algorithms, the velocity information can be converted into a more accurate estimate of the IMU position. Therefore, even in long-term motion, the accumulated error caused by double integration can be effectively corrected, thereby improving the accuracy and stability of IMU positioning.

[0086] Step 701: If If is 1, the following calculation is performed to obtain the posterior state estimate at time k: .

[0087] in, Indicates time The posterior state estimate of ; Indicates time The posterior state covariance matrix of ; Indicates time The Kalman gain of is the covariance matrix of the observation noise; represents the state observation matrix; represents zero-speed pseudo-measurement; Indicates the judgment result of the zero speed state obtained by the zero speed detector; Indicates the zero speed judgment threshold, ; , .

[0088] The above formula intuitively reflects the relationship between the prediction error and the measurement error in the state optimal estimation process. Specifically, when the Kalman gain When it is close to 0, it means that the system trusts the model's prediction value more. When it is close to 1, it means that the system trusts the actual observed measurement value more.

[0089] Step 702: Based on the Kalman filter at time The posterior state estimate obtained on , before update The forecast amount at a moment.

[0090] In this embodiment, It is the number of moments between the two most recent zero speeds.

[0091] In this embodiment, a time-delay smoothing Kalman filter is used: .

[0092] Here, it is assumed that the Kalman filter is The posterior state estimate is updated on and the posterior covariance matrix , use the time Update the past with information The predicted value at each moment, the Kalman gain is known to be , use it to calculate the time Corrected value of the prior prediction of : .

[0093] Expect to apply the correction value to the previous moment , assuming that an attenuation coefficient is applied to the correction value , so that it decreases as the time decreases, and the past is updated through the following recursive relationship The forecast amount at each moment: , .in, , It's time The prior predictor of (the prior state estimate), Indicates at time Time to time The smoothed estimate of the state (i.e., at the moment In this way, the compensation calculated by the Kalman gain can be used to update the previous state according to the weight of the time series. The prediction quantity at each moment can improve the prediction results at the past moments to a certain extent, making the overall state estimation more accurate.

[0094] Step 8: Based on the quadruped robot states corresponding to all legs (the posterior state estimate at time k), the position of the quadruped robot is obtained through dual IMU distance constraints and single-step stride constraints.

[0095] The dual IMUs attached to the legged robot have structural constraints themselves, such as position constraints and distance constraints. And the motion of the quadruped robot is also subject to distance constraints. The distance constraint limits the allowed range of motion of the quadruped robot for each step, ensuring that it stays within the specified range. The distance constraint helps achieve accurate positioning trajectories in complex real-world environments.

[0096] Step 801: Single-step stride constraint. For each IMU, based on the posterior state estimate at the latest moment (time k) and the previous zero-speed moment, the difference (stride) between the positions at the two moments is calculated. If the difference is greater than the first threshold, the single-step stride constraint algorithm is used to obtain and , to update the position part of the posterior state estimate at time k and its posterior covariance matrix. For the convenience of representation, the posterior state estimate at time k and its posterior covariance matrix are expressed as and : ; .

[0097] in, Represents the posterior state estimate after being updated by the single-step stride constraint algorithm at time k; is the solution to the least squares problem obtained in the single-step stride-constrained algorithm; Represents the posterior covariance matrix after updating by the single-step stride constraint algorithm; is the Jacobian matrix solved in the single-step stride-constrained algorithm.

[0098] Step 802: Dual IMU distance constraint. For time k, based on the posterior state estimates of the two IMUs, the distance between the positions solved by the two IMUs is calculated. If the distance is greater than the second threshold, the dual IMU distance constraint algorithm is used to obtain and , to update the position part of the posterior state estimate at time k and its posterior covariance matrix. For the convenience of representation, the posterior state estimate at time k and its posterior covariance matrix are expressed as and : ; .

[0099] in, represents the posterior state estimate after being updated by the dual IMU distance constraint algorithm at time k; is the solution to the least squares problem obtained in the dual IMU distance constraint algorithm; Represents the posterior covariance matrix after being updated by the dual IMU distance constraint algorithm; is solved in the dual IMU distance constraint algorithm The Jacobian matrix of

[0100] The single-step stride constraint is: within the range of a single stride, the position difference between the estimates obtained at two consecutive moments is subject to the constraint expressed as The upper threshold limit.

[0101] The dual IMU distance constraint is: at the same time, the position difference between the estimates obtained by the two IMUs is expressed as The upper threshold limit.

[0102] The Kalman fusion algorithm is used to realize the maximum distance constraint (single-step stride constraint and dual IMU distance constraint), and the single-step stride constraint algorithm and the dual IMU distance constraint algorithm are obtained.

[0103] In this embodiment, taking the dual IMU distance constraint as an example, the dual IMU distance constraint algorithm implemented by using the Kalman fusion algorithm is introduced.

[0104] Define the matrix: , Means defined as.

[0105] By projecting the constraint problem into the subsystem to convert it into a least squares problem so that constraints can be imposed.

[0106] .in, represents the solution to the least squares problem; represents an estimate of the state, x Indicates the optimal state that we hope to find.

[0107] In order to solve the constrained least squares problem, the constraints are transformed into Karush-Kuhn-Tucker conditions, and the Lagrange multiplier method is applied to find the optimal solution: .in, is the Lagrange multiplier, , .

[0108] The original constrained optimization problem controlled by the inequality has been transformed into an unconstrained optimization problem with two parameters. By calculating the gradient of the objective function and setting it to zero, a feasible solution can be finally obtained: , using the Gauss-Newton iteration method to calculate the solution .

[0109] Introduction , represents the solution to the least squares problem, The Jacobian matrix of : , is the 12-dimensional identity matrix, , , Is to solve The solution obtained is: yes The covariance matrix of the projected navigation state The covariance matrix of can be approximated as follows: .

[0110] A quadruped robot positioning method provided in this embodiment can capture more robot motion process information, such as zero speed information and distance constraint information, by installing IMU (Inertial Measurement Unit) sensors on the robot legs, thereby providing a richer data source for positioning.

[0111] A quadruped robot positioning method provided in this embodiment utilizes the spatial position characteristics of dual IMUs, and can more comprehensively capture the robot's motion information, thereby improving the accuracy and robustness of the positioning algorithm.

[0112] A quadruped robot positioning method provided in this embodiment utilizes an inertial measurement unit installed on the legs and adopts zero-speed correction technology assisted by dual IMU distance constraints and single-step stride constraints to position the legged robot, effectively reducing the error accumulation problem of the robot during movement, thereby achieving accurate estimation of the robot's position, thereby improving the accuracy and reliability of position estimation.

[0113] A quadruped robot positioning method provided in this embodiment can more accurately capture the robot's motion state and improve the robustness and accuracy of the positioning algorithm by constraining the robot's motion stride.

[0114] A quadruped robot positioning method provided in this embodiment uses Kalman filtering to fuse IMU data with other information. Through the optimization processing of Kalman filtering, the position and posture of the robot can be estimated more accurately, thereby improving the stability and reliability of positioning.

[0115] A quadruped robot positioning method provided in this embodiment utilizes dual IMU distance constraints and single-step stride constraints. By utilizing data from two IMU sensors, accurate estimation of the robot's position is achieved, thereby effectively reducing costs.

[0116] A quadruped robot positioning method provided in this embodiment combines time-delay smoothing technology and Kalman filtering algorithm to deal with the problem of tortuous oscillation in the positioning results. By smoothing the robot position solution results, the error accumulation problem in the robot movement process can be effectively controlled, the positioning accuracy can be further improved, and the accurate estimation of the robot position can be achieved.

[0117] Embodiment 2

[0118] The purpose of this second embodiment is to provide a quadruped robot positioning system, including:

[0119] A data acquisition module, configured to: acquire inertial measurement unit data on at least two legs of the quadruped robot;

[0120] A zero speed determination module is configured to: determine whether the inertial measurement unit is in a zero speed state based on the inertial measurement unit data;

[0121] A posture calculation module is configured to: for each leg, based on the inertial measurement unit data and the determination result of the zero-speed state, convert the inertial measurement unit data into posture information through posture calculation;

[0122] A Kalman filter module is configured to: obtain a priori state estimates based on inertial measurement unit data and posture information of all legs through Kalman filtering;

[0123] A zero-speed correction and delay smoothing module is configured to: based on the inertial measurement unit data, attitude information and the judgment result of the zero-speed state, obtain a posteriori state estimation through zero-speed correction and delayed Kalman filtering for the prior state estimation;

[0124] The constraint module is configured to obtain the position of the quadruped robot through the inertial measurement unit distance constraint and the single-step stride constraint for the posterior state estimation.

[0125] It should be noted here that each module in this embodiment corresponds to each step in Example 1 one by one, and the specific implementation process is the same, which will not be repeated here.

[0126] Embodiment 3

[0127] This embodiment provides a computer-readable storage medium on which a computer program is stored. The program is executed by a processor. When the program is executed by the processor, the steps in the positioning method of a quadruped robot as described in the above-mentioned embodiment 1 are implemented.

[0128] Embodiment 4

[0129] This embodiment provides a robot, which adopts the steps of a quadruped robot positioning method as described in the first embodiment above.

[0130] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

[0131] Although the above describes the specific implementation mode of the present invention in conjunction with the accompanying drawings, it is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art on the basis of the technical solution of the present invention without creative work are still within the scope of protection of the present invention.

Claims

1. A quadruped robot positioning method, characterized in that: include: acquiring inertial measurement unit data on at least two legs of the quadruped robot; Based on the inertial measurement unit data, determine whether the inertial measurement unit is in a zero speed state; For each leg, based on the inertial measurement unit data and the judgment result of the zero-speed state, the inertial measurement unit data is converted into attitude information through attitude solution; The steps of attitude solution include: rotating the gravity acceleration from the geographic coordinate system to the carrier coordinate system; cross-multiplying the gravity acceleration in the carrier coordinate system with the acceleration in the inertial measurement unit data to obtain an error correction amount; based on the error correction amount, according to proportional integral adjustment, when the inertial measurement unit is in a zero-speed state, performing error correction on the angular velocity in the inertial measurement unit data; based on the corrected angular velocity, calculating the attitude information; Based on the inertial measurement unit data and attitude information of all legs, the prior state estimate is obtained through Kalman filtering; Based on the inertial measurement unit data, attitude information and the judgment result of the zero-speed state, for the prior state estimation, the posterior state estimation is obtained through zero-speed correction and delayed Kalman filtering; the steps of delayed Kalman filtering include: assuming that at time The posterior state estimate is updated on , then calculate the time The correction value of the prior state estimate of : ,in, Indicates time The prior state estimate of Indicates time The Kalman gain, represents zero-speed pseudo-measurement, Represents the state observation matrix; updates the past through recursive relations The forecast amount at each moment: , ;in, , is the number of moments between the two latest zero speeds, It's time The prior state estimate of Indicates time The posterior state estimate of represents the attenuation coefficient; For the posterior state estimation, the position of the quadruped robot is obtained through the dual IMU distance constraint and the single-step stride constraint; the single-step stride constraint is: for each IMU, based on the posterior state estimation value at time k and the previous zero-speed time, the difference between the positions at the two moments is calculated. If the difference is greater than the first threshold, the single-step stride constraint algorithm is used to obtain and , to update the posterior state estimate at time k and the position part in its posterior covariance matrix; the single-step stride constraint and the dual IMU distance constraint are implemented using the Kalman fusion algorithm, including: by projecting the constraint problem to the subsystem, converting it into a least squares problem, in order to solve the constrained least squares problem, converting the constraint into a Carlo-Kuhn-Tucker condition, and applying the Lagrange multiplier method to solve the optimal solution, by calculating the gradient of the objective function and setting it to zero, and using the Gauss-Newton iteration method to calculate the Lagrange multiplier , introduce ,Will The Jacobian matrix of : ;in, is the solution to the least squares problem, yes The covariance matrix of represents an estimate of the state, is a 12-dimensional identity matrix, the matrix , It is defined as, Is to solve The solution obtained, is the Lagrange multiplier, , , the upper threshold is expressed as , x It indicates the optimal state that we hope to find. .

2. A quadruped robot positioning method as claimed in claim 1, characterized in that: The zero speed correction is expressed as: ;in, Indicates time The posterior state estimate of ; Indicates time A priori state estimate of ; Represents the prior state covariance matrix at time k; Indicates time The posterior state covariance matrix of ; Indicates time The Kalman gain of is the covariance matrix of the observation noise; represents the state observation matrix; represents zero-speed pseudo-measurement; Indicates the judgment result of zero speed state.

3. A quadruped robot positioning method as claimed in claim 1, characterized in that: After preprocessing the inertial measurement unit data using a Butterworth filter, it is determined whether the inertial measurement unit is in a zero-speed state.

4. A quadruped robot positioning method as claimed in claim 1, characterized in that: The dual IMU distance constraint is: for time k, based on the posterior state estimates of the two IMUs, the distance between the positions solved by the two IMUs is calculated. If the distance is greater than the second threshold, the dual IMU distance constraint algorithm is used to obtain and , to update the posterior state estimate at time k and the position part of its posterior covariance matrix.

5. A quadruped robot positioning system, characterized in that: include: A data acquisition module, configured to: acquire inertial measurement unit data on at least two legs of the quadruped robot; A zero speed determination module is configured to: determine whether the inertial measurement unit is in a zero speed state based on the inertial measurement unit data; A posture calculation module is configured to: for each leg, based on the inertial measurement unit data and the determination result of the zero-speed state, convert the inertial measurement unit data into posture information through posture calculation; The steps of attitude solution include: rotating the gravity acceleration from the geographic coordinate system to the carrier coordinate system; cross-multiplying the gravity acceleration in the carrier coordinate system with the acceleration in the inertial measurement unit data to obtain an error correction amount; based on the error correction amount, according to proportional integral adjustment, when the inertial measurement unit is in a zero-speed state, performing error correction on the angular velocity in the inertial measurement unit data; based on the corrected angular velocity, calculating the attitude information; A Kalman filter module is configured to: obtain a priori state estimates based on inertial measurement unit data and posture information of all legs through Kalman filtering; The zero-speed correction and delay smoothing module is configured to: based on the inertial measurement unit data, attitude information and the judgment result of the zero-speed state, obtain the a posteriori state estimation through zero-speed correction and delayed Kalman filtering for the prior state estimation; the delayed Kalman filtering step includes: assuming that at time The posterior state estimate is updated on , then calculate the time The correction value of the prior state estimate of : ,in, Indicates time The prior state estimate of Indicates time The Kalman gain, represents zero-speed pseudo-measurement, Represents the state observation matrix; updates the past through recursive relations The forecast amount at each moment: , ;in, , is the number of moments between the two latest zero speeds, It's time The prior state estimate of Indicates time The posterior state estimate of represents the attenuation coefficient; The constraint module is configured as follows: for the posterior state estimation, the position of the quadruped robot is obtained through the dual IMU distance constraint and the single-step stride constraint; the single-step stride constraint is: for each IMU, based on the posterior state estimation value at the k moment and the previous zero-speed moment, the difference between the positions at the two moments is calculated. If the difference is greater than the first threshold, the single-step stride constraint algorithm is used to obtain and , is the solution to the least squares problem; the single-step stride constraint and the dual IMU distance constraint are implemented using the Kalman fusion algorithm, including: projecting the constraint problem to the subsystem and converting it into a least squares problem, in order to solve the constrained least squares problem, converting the constraint into the Carlo-Kuhn-Tucker condition, and applying the Lagrange multiplier method to solve the optimal solution, calculating the gradient of the objective function and setting it to zero, and calculating the Lagrange multiplier using the Gauss-Newton iteration method , introduce ,Will The Jacobian matrix of : ;in, yes The covariance matrix of represents an estimate of the state, is a 12-dimensional identity matrix, the matrix , It is defined as, Is to solve The solution obtained, is the Lagrange multiplier, , , the upper threshold is expressed as , x It indicates the optimal state that we hope to find. .

6. A computer-readable storage medium having a computer program stored thereon, the program being executed by a processor, characterized in that: When the program is executed by a processor, the steps in a quadruped robot positioning method as described in any one of claims 1 to 4 are implemented.

7. A robot, characterized in that: Adopt the steps in a quadruped robot positioning method as described in any one of claims 1-4.

Images