Positioning mapping method and system based on motion feature constraint of foot-leg robot

Abstract

The invention discloses a positioning mapping method and system based on motion feature constraint of a foot-leg robot, and belongs to the technical field of foot-leg robots. According to the method, incremental positioning estimation is carried out in the positioning mapping process according to motion characteristics of the foot and leg type robot in combination with motion constraints of feet and legs in the environment and environment observation constraints; high-frequency positioning output is realized according to front and rear ends of filtering derivation and global pose factor optimization, and a lightweight foot and leg motion constraint model is combined to optimize and update motion pose positioning and generate a point cloud map. According to the method provided by the invention, light-weight positioning characterization and mapping based on motion and environment characteristics can be realized for the foot-leg robot in a complex environment, and the problems of serious local positioning drifting and degradation distortion of the foot-leg robot in the complex environment due to the motion characteristics of the foot-leg robot are solved; and the robustness of the positioning system of the foot-leg robot and the mapping accuracy are enhanced.

Description

Technical Field

[0001] The present invention relates to the field of mobile robot positioning and mapping technology, and more specifically, to a positioning and mapping method and system based on motion feature constraints of a legged robot. Background Art

[0002] Positioning and mapping are important parts of mobile robot motion control and an important prerequisite for the robot to achieve autonomous path planning and task execution. Existing positioning and mapping methods only rely on a single observation sensor for incremental positioning estimation. Although these methods can achieve real-time positioning and mapping of complex scenes, the special leg-based movement mode of legged mobile robots causes high-frequency jitter characteristics during movement. In complex environments, traditional positioning methods are prone to accumulated positioning drift, resulting in degradation, which directly affects the system positioning quality and mapping efficiency.

[0003] Chinese Patent Publication No. CN113390411A discloses a navigation and positioning method for a legged robot based on a variable configuration sensing device. This method increases the rotational freedom of the relevant mechanical structure to change the sensor's field of view to enhance the positioning ability of the legged robot. However, the particularity and size of its variable configuration machinery cannot adapt to most legged robots. Chinese Patent Publication No. CN106056643A discloses a point cloud-based indoor dynamic scene SLAM method and system. This method obtains the optimal pose estimation through LM iterative optimization convergence, but its map is a two-dimensional plane map, which cannot be applied to the three-dimensional state space state estimation of legged robots. Chinese Patent Publication No. CN106056643A discloses a point cloud-based indoor dynamic scene SLAM method and system. This method obtains the optimal pose estimation through LM iterative optimization convergence, but its map is a two-dimensional plane map, which cannot be applied to the three-dimensional state space state estimation of legged robots. Patent Publication No. CN114987642A discloses a mobile robot positioning method based on sensor fusion. This method can fuse multiple sensor data to obtain high-precision real-time positioning, but it is more dependent on the robot's repositioning performance and cannot guarantee the real-time robustness requirements of legged robots in complex environments; Chinese Patent Publication No. CN117451032A discloses a low-computing power and loosely coupled LiDAR and IMU SLAM method and system. This method can be used for real-time positioning estimation of robots, but its data fusion adopts loosely coupled processing and cannot be used for the high-precision positioning requirements of legged robots under high-frequency foot motion conditions in complex environments.

[0004] In summary, although the current mobile robot positioning and mapping is relatively mature, there are few positioning and mapping methods suitable for legged mobile robots. The high-frequency leg-type motion of legged mobile robots in complex environments has problems such as insufficient local positioning robustness, large positioning and mapping differences, and serious positioning distortion degradation. Therefore, it is necessary to provide an improved technical solution for the above-mentioned shortcomings of the existing technology, and at the same time, it must have the ability to run in real time on the host of the legged robot. Summary of the invention

[0005] Purpose of the invention: The purpose of the present application is to provide a positioning and mapping method and system based on the motion characteristic constraints of legged robots, which can realize high-precision real-time positioning and mapping of legged robots in complex environments, solve the problems of local positioning degradation and distortion, insufficient positioning robustness and accumulation of positioning drift deviation caused by leg and foot movements of legged robots in complex environments, and provide an effective solution to the technical problems of accurate positioning and accurate mapping of legged robots over long periods of time and over long distances.

[0006] Technical solution:

[0007] The first aspect of the present application provides a positioning and mapping method and system based on motion feature constraints of a legged robot, comprising the following steps:

[0008] S1: Acquire various sensor data of the legged robot, including encoder data of motors of various joints of the legged robot, original point cloud data of the laser radar, and IMU inertial unit data, and perform timestamp alignment and data preprocessing on various sensor data;

[0009] S2: Perform time integration based on the IMU inertial unit data to calculate the robot's inertial prior pose estimate;

[0010] S3: Obtain the robot's kinematic state estimate based on the robot's joint encoder data, inertial prior pose state, and robot foot kinematics;

[0011] S4: According to the inertial prior posture and the laser radar raw data, the motion distortion of the laser radar acquisition point is compensated point by point, and a residual model of the laser feature point is constructed;

[0012] S5: According to the robot's kinematic state and feature point residual model, a motion constraint model and an iterative optimization model are constructed, and the state optimization is updated based on the convergence threshold or the number of iterations, and the robot's real-time optimal posture state update is output;

[0013] S6: According to the real-time optimal posture state estimation of the robot, the key frames are screened and the posture graph optimization model is constructed. The state error accumulated by the integration of various sensor data is optimized on the back end, and the optimized state data is corrected and updated in S5; the key frame map is converted to the initial coordinate system, and the map is accumulated and published; S1 to S6 are executed in a loop to update the robot's position and posture until the positioning task is completed.

[0014] In step S1, the timestamp alignment includes: taking the timestamp of the laser radar sensor data as a reference, aligning the timestamps of the inertial unit IMU, the joint motor encoder and the laser radar to complete time synchronization, and the synchronized data calculation method is:

[0015]

[0016] In the formula, Data1 and Data2 are the data before and after the alignment object on the timeline, Data new It is the synchronization data with the laser radar data frame acquisition time. T1 and T2 are the timestamps of the aligned objects on the timeline. new It is the timestamp after alignment; calibrate the internal parameters of IMU and the external parameters of IMU and lidar, the external parameters include rotation parameters and displacement parameters; the internal parameters of IMU include: gyroscope data in IMU data, mean value of acceleration data and zero bias.

[0017] The inertial priori posture estimation of the robot is obtained by extrapolating the IMU inertial unit data through data time integration within the data time period after synchronization; the IMU inertial unit data is obtained by zero drift correction in step S1.

[0018] In step S3, the robot kinematic state estimation includes:

[0019] Based on the mechanical structure of the footed robot and the angle information of the joint motor encoder, the kinematic equation J of the robot foot position is obtained. k (θ), where θ is the angle of the joint motor encoder; based on the robot kinematics Jacobian matrix J, the robot foot velocity equation is obtained: in is the angle differential of the joint motor encoder with respect to time, i.e., the joint motor angular velocity;

[0020] According to the robot's inertial priori posture and the foot end position equation and foot end velocity equation described in step S2, the constraint items and update items on the robot's spatial posture are obtained, and the constraint items and update items are only performed when the foot end is in contact with the ground;

[0021] The spatial constraint equation is as follows:

[0022]

[0023] Wherein, Δx is the robot inertial prior posture sampling calculation result described in step S2, and Δt is the sampling time;

[0024] The updating is performed by the least square method, the constraint equation is the above-mentioned spatial constraint equation, and the robot is estimated based on the kinematic state X.

[0025] The motion equation is the normalized kinematic equation of the foot end points of the leg structure, which contains the vector results of the n leg support points of the leg robot relative to the center of mass of the body in a single support action cycle. The kinematic equation J k (θ) is as follows:

[0026]

[0027] Among them, j k (θ n ) is the vector result of the nth leg of the legged robot relative to the center of mass of the body during its single support action cycle, θ n It is represented as the angle vector of the nth leg joint degree of freedom. The calculation frequency of this equation is consistent with the leg motion cycle of the wheel-legged robot.

[0028] In the single support phase, a linear fit is performed based on the power exerted by the joint on the foot and leg, and the force exerted by the foot end on the ground is calculated and filtered. The calculation method is as follows:

[0029]

[0030] Where F is the force exerted by the foot on the ground, τ i is the torque of the i-th joint motor of a single leg calculated by the robot; threshold filtering is performed according to the force applied by the foot end to the ground, and if it exceeds the threshold setting interval, it is determined that the current leg is in the support phase state. The threshold value range is m is the mass of the robot body, k is the number of robot legs, and g is the acceleration due to gravity.

[0031] In step S4, the laser feature point residual model includes:

[0032] The feature points are obtained by using motion distortion compensation data through curvature calculation;

[0033] The motion distortion compensation data is obtained by performing point-by-point motion back propagation on the laser radar raw data through inertial prior posture state estimation;

[0034] The laser residual model is the distance residual value from the feature point to the feature surface;

[0035] The distance residual value formula is as follows:

[0036]

[0037] Among them, A, B, C, D are the plane equation parameters of the characteristic surface, and d is the calculated distance residual value;

[0038] The characteristic surface is the plane where the laser radar data points are located in the global map.

[0039] In step S5, the model is iteratively optimized, including:

[0040] The iterative optimization algorithm of the iterative optimization model is an ESKF filtering optimization algorithm;

[0041] The observation model equation of the iterative optimization is:

[0042] h=u T ((R-p+t)-q);

[0043] Among them, u T is the normal vector of the feature surface, i.e., ABC in claim 5, p is the data feature point detected by the laser radar, q is the feature surface point closest to p in the feature surface in the global map, and R, t is the iteratively calculated positioning state quantity;

[0044] The observation update amount of the iterative optimization model is the distance residual value d described in claim 5;

[0045] In step S5, the motion constraint model includes:

[0046] The motion constraint model equation is:

[0047]

[0048] t z =λ1H+λ2L;

[0049] Where Δh L and Δh leg is the height increment in the displacement state calculated by the iterative optimization model and the robot kinematic state estimation; σ is the weight gain, which is between 0.8 and 1.2; λ1 and λ2 are the constraint weight ratios, which are between 0.4 and 0.7; H and L are the state quantity and the gain state increment in the height direction, respectively; t z is the height state after constraint.

[0050] In step S6, the key frame pose graph optimization model includes:

[0051] Key frame screening is performed according to the state quantities obtained by the iterative optimization model and the motion constraint model, wherein the key frame screening is performed in time and space, the time Δt between adjacent key frames is between 3 and 10 seconds, and the Euclidean distance ||Δx|| between adjacent key frames is between 5 and 10 meters;

[0052] According to the position encoding of the key frames, a pose-based factor model is constructed, and the pose information of the key frames is added to the global factor graph optimization as a positioning constraint;

[0053] Performing real-time loop closure constraints based on the current key frame pose and the optimized key frame pose, wherein the loop closure constraints include loop closure constraint key frame detection and loop closure constraint registration;

[0054] The loop constraint key frame detection constrains the spatial Euclidean distance and time of all historical key frames with the current key frame, and the spatial Euclidean distance and time that meet the constraints are regarded as the loop constraint key frame of the current frame, the key frame time Δt of the loop constraint is greater than 30s, and the Euclidean distance ||Δx|| of the key frame of the loop constraint is less than 2m;

[0055] The loop constraint registration adopts the ICP registration method of the frame-local keyframe map to obtain the constraint result, the local keyframe is a local map constructed by the n keyframes before and after the loop constraint keyframe corresponding to the current keyframe, and the value of n is 15-20. The constraint result is the loop constraint posture conversion relationship obtained by ICP registration;

[0056] The loop closure constraint includes adding two key frames corresponding to the constraint and the loop closure pose transformation relationship into the global factor graph optimization.

[0057] A second aspect of the present application provides an electronic device, comprising: at least one processor and a memory;

[0058] The memory stores computer-executable instructions;

[0059] The at least one processor executes the computer execution commands stored in the memory, so that the at least one processor performs the steps and methods described in the first aspect and various possible designs of the first aspect.

[0060] The third aspect of the present application provides a computer-readable storage medium, which stores computer-executable instructions. When a processor executes the computer-executable instructions, the steps and methods described in the first aspect and various possible designs of the first aspect are implemented.

[0061] Compared with the prior art, the present invention has the following significant advantages:

[0062] (1) The present invention provides a positioning and mapping method based on motion characteristic constraints of legged robots. In the positioning and mapping process, the motion characteristics of the legged robot are combined with the motion constraints of the legs in the environment and the environmental observation constraints to solve the problem of poor scene positioning robustness and degradation distortion caused by high-frequency leg motion of the legged robot in complex environments.

[0063] (2) Compared with other traditional algorithms, the present invention not only reduces the computing resource load to achieve real-time positioning refresh efficiency, but also optimizes the global positioning posture and reduces the accumulated drift error caused by long-range odometers in large-scale complex environments.

[0064] (3) The present invention adopts key motion feature constraints and key frame back-end optimization to improve the interactive correlation between the robot's own motion and environmental observation in a complex environment, avoid the estimation discontinuity caused by redundant feature matching optimization, and fundamentally ensure the geometric unity of the pose estimation system.

[0065] Further features and advantages of the present invention will become apparent from the following detailed description of exemplary embodiments of the present invention with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] Figure 1 A system block diagram of a positioning and mapping method based on motion feature constraints of a legged robot provided in an embodiment of the present invention;

[0067] Figure 2 A flowchart of a positioning and mapping method based on motion feature constraints of a legged robot provided in an embodiment of the present invention;

[0068] Figure 3 A data processing flow chart of a positioning and mapping method based on motion feature constraints of a legged robot provided in an embodiment of the present invention;

[0069] Figure 4 A schematic block diagram of the structure of a computer device provided in an embodiment of the present invention. Specific implementation plan

[0070] The present invention is further described below in conjunction with the accompanying drawings and specific embodiments to fully understand the purpose, scheme and effect of the present invention. It should be noted that the embodiments and features in the embodiments of this application can be combined with each other without conflict.

[0071] Figure 1 A system block diagram of a positioning and mapping method based on motion feature constraints of a legged robot provided in an embodiment of the present invention.

[0072] Figure 2 The present invention provides a flowchart of a positioning and mapping method based on motion feature constraints of a legged robot. Figure 2 As shown, the method includes:

[0073] S1: Acquire various sensor data of the legged robot, including encoder data of motors of various joints of the legged robot, original point cloud data of the laser radar, and IMU inertial unit data, and perform timestamp alignment and data preprocessing on various sensor data;

[0074] Specifically, the sensors carried by the robot are diverse and multi-source, and the data collection frequency of each type of sensor is inconsistent. The sensor data received by the system needs to be aligned in time and the relevant data needs to be distorted and time compensated. The compensated aligned data makes the state estimation obtained by multi-sensor data fusion more reliable.

[0075] S2: Perform time integration based on the IMU inertial unit data to calculate the robot's inertial prior pose estimate;

[0076] Specifically, by integrating the angular velocity and angular acceleration collected by the IMU inertial unit over time, the state posture integral within a certain time length is obtained, and based on the state posture integral as the prior state, weighted filtering of other observation data is aligned to perform posture optimization.

[0077] S3: Obtain the robot's kinematic state estimate based on the robot's joint encoder data, inertial prior pose state, and robot foot kinematics;

[0078] Specifically, the prior state posture quantity in step S2 is used as input, combined with the posture quantity obtained by the positive kinematics of the quadruped robot, and the posture quantity of the positive kinematics is used as the observation gain to perform weighted positioning optimization of the posture.

[0079] S4: According to the inertial prior posture and the laser radar raw data, the motion distortion of the laser radar acquisition point is compensated point by point, and a residual model of the laser feature point is constructed;

[0080] Specifically, the prior state posture in step S2 is used as input, combined with the lidar data for distortion compensation, and the compensated frame-frame and frame-map residual observations are used as weighted gains to complete the posture optimization.

[0081] S5: According to the robot's kinematic state and feature point residual model, a motion constraint model and an iterative optimization model are constructed, and the state optimization is updated based on the convergence threshold or the number of iterations, and the robot's real-time optimal posture state is output.

[0082] S6: According to the real-time optimal posture state estimation of the robot, the key frames are screened and the posture graph optimization model is constructed. The state error accumulated by the integration of various sensor data is optimized on the back end, and the optimized state data is corrected and updated in S5; the key frame map is converted to the initial coordinate system, and the map is accumulated and published; S1 to S6 are executed in a loop to update the robot's position and posture until the positioning task is completed.

[0083] Figure 3 A data processing flow chart of a positioning and mapping method based on motion feature constraints of a legged robot provided in an embodiment of the present invention.

[0084] After the program starts executing, it reads the data of various sensors mounted on the robot in turn, and performs time alignment and data calibration. The time alignment program mainly includes: using the timestamp of the laser radar sensor data as the reference, aligning the timestamp of the inertial unit IMU, joint motor encoder and laser radar to complete time synchronization. The synchronized data calculation method is as follows:

[0085]

[0086] In the formula, Data1 and Data2 are the data before and after the alignment object on the timeline, Data new It is the synchronization data with the laser radar data frame acquisition time. T1 and T2 are the timestamps of the aligned objects on the timeline. new It is the timestamp after alignment; calibrate the internal parameters of IMU and the external parameters of IMU and lidar. The external parameters include rotation parameters and displacement parameters. The internal parameters of IMU include: gyroscope data in IMU data, mean value of acceleration data and zero bias.

[0087] The aligned IMU data is time-integrated to obtain the prior pose state estimation. The aligned IMU data is angular acceleration data and acceleration data. Some IMU sensors are 9-axis inertial IMU sensors, and the prior pose state estimation can be weighted based on its spatial pose data.

[0088] The prior pose state obtained from the aligned IMU data is calculated as follows:

[0089] p(t+Δt)=p(t)+vΔt+0.5*R(ab a )*Δt 2 +0.5*g*Δt 2 ;

[0090] R(t+Δt)=R(t)Exp((ω-b g )Δt);

[0091] v(t+Δt)=v(t)+(R(ab a )Δt+gΔt);

[0092] b g (t+Δt)=b g (t)+η bg Δt;

[0093] b a (t+Δt)=b a (t)+η ba Δt;

[0094] Among them, p, v, and R are the position, velocity, and rotation posture of the robot in the positioning state; a and ω are the acceleration and angular acceleration of the IMU data respectively; b a , b g , η ba , η bg are the deviation of acceleration and angular acceleration and the data acquisition noise, respectively.

[0095] The motion state is estimated based on the prior posture state obtained from the IMU data and the kinematic state of the legged robot, including:

[0096] Based on the mechanical structure of the leg-like robot and the angle information of the joint motor encoder, the kinematic equation J of the robot foot position is obtained. k (θ), where θ is the angle of the joint motor encoder; based on the robot kinematics Jacobian matrix J, the robot foot velocity equation is obtained: in is the angle differential of the joint motor encoder with respect to time, i.e., the joint motor angular velocity;

[0097] According to the robot's prior posture state and the foot end position equation and foot end velocity equation, the constraint items and update items on the robot's spatial posture are obtained, and the constraint items and update items are only performed when the foot end is in contact with the ground;

[0098] The spatial constraint equation is as follows:

[0099]

[0100] Among them, Δx is the calculation result of the robot's prior posture state mentioned above, and Δt is the sampling time;

[0101] The updating is performed by the least square method, the constraint equation is the above-mentioned spatial constraint equation, and the robot is estimated based on the kinematic state X.

[0102] The motion equation is the normalized kinematic equation of the foot end points of the leg structure, which contains the vector results of the n leg support points of the leg robot relative to the center of mass of the body in a single support action cycle. The kinematic equation J k (θ) is as follows:

[0103]

[0104] Among them, j k (θ n ) is the vector result of the nth leg of the legged robot relative to the center of mass of the body during its single support action cycle, θ n It is represented as the angle vector of the nth leg joint degree of freedom. The calculation frequency of this equation is consistent with the leg motion cycle of the wheel-legged robot.

[0105] In the single support phase, a linear fit is performed based on the power exerted by the joint on the foot and leg, and the force exerted by the foot end on the ground is calculated and filtered. The calculation method is as follows:

[0106]

[0107] Where F is the force exerted by the foot on the ground, τ i is the torque of the i-th joint motor of a single leg calculated by the robot; threshold filtering is performed according to the force applied by the foot end to the ground, and if it exceeds the threshold setting interval, it is determined that the current leg is in the support phase state. The threshold value range is m is the mass of the robot body, k is the number of robot legs, and g is the acceleration due to gravity.

[0108] The prior pose state obtained from the IMU data is combined with the LiDAR point cloud observation to estimate the motion state, including:

[0109] Point cloud motion distortion processing. The point cloud motion distortion compensation data is obtained by performing point-by-point motion backpropagation on the lidar raw data through inertial prior pose state estimation.

[0110] Point cloud feature processing: Feature points are obtained by using motion distortion compensation data and curvature calculation.

[0111] Calculate the residual of feature points. The residual is the residual value of the distance from the feature point to the feature surface. The calculation formula is as follows:

[0112]

[0113] Among them, A, B, C, D are the plane equation parameters of the characteristic surface, and d is the calculated distance residual value;

[0114] The above-mentioned characteristic surface is the plane where the lidar data points are located in the global map.

[0115] According to the kinematic state estimation of the legged robot, the pose fusion state constraint calculation is constructed, which includes the kinematic constraint model calculation and the pose weight gain fusion calculation;

[0116] The calculation equation of the kinematic constraint model is as follows:

[0117]

[0118] The pose weight gain fusion calculation equation is as follows:

[0119] t z =λ1H+λ2L;

[0120] Where Δh L and Δhleg is the height increment in the displacement state calculated by the iterative optimization model and the robot kinematic state estimation; σ is the weight gain, which is between 0.8 and 1.2; λ1 and λ2 are the constraint weight ratios, which are between 0.4 and 0.7; H and L are the state quantity and the gain state increment in the height direction, respectively; t z is the height state after constraint.

[0121] The motion feature constraint state calculation continuously receives the optimal result calculated by the back-end pose factor graph, constrains the optimal pose state, and finally feeds the constrained pose back to the iterator for optimization iteration.

[0122] The constrained pose state is optimized and updated through the total residual iterator. The total residual iterator includes: iterative optimization model and total residual calculation model.

[0123] The iterative optimization algorithm of the iterative optimization model is the ESKF filter optimization algorithm or the KF filter variant algorithm model. The total residual calculation model is the lidar observation residual registration model, and its observation residual registration model equation is:

[0124] h=u T ((R·p+t)-q);

[0125] Among them, u T is the normal vector of the feature surface, i.e., ABC as described in claim 5, p is the data feature point detected by the laser radar, q is the feature surface point closest to p in the feature surface in the global map, and R, t is the iteratively calculated positioning state quantity.

[0126] The optimization update is continuously performed through the total residual iterator until the iterator reaches the iteration number threshold or the iterator converges, and the iteration is completed; otherwise, the iterative update continues.

[0127] The pose-based factor graph optimization backend mainly uses the robot pose state updated by the iterator to perform smooth optimization fitting of the backend trajectory, selects key frames according to the set pose threshold and time threshold, and then constructs the backend loop optimization based on the pose of the key frames and the registration of the frame-map. Specifically, it includes:

[0128] (1) Key frame processing: Obtain the spatial pose of the current data frame after iterator optimization. If the pose distance and time interval between the current data frame and the previous key frame are greater than the set threshold, set the current data frame as a key frame and encode the pose factor. Otherwise, continue to wait for the next data frame to determine whether it is a key frame. The first frame of data processed and received by the system is automatically set as a key frame. The time threshold Δt for determining the key frame is between 3 and 10 seconds, and the Euclidean distance ||Δx|| for determining the pose of the key frame is between 5 and 10 meters. The pose of the current key frame is encoded into the pose factor graph.

[0129] (2) Loop detection: Based on the key frame pose encoding, check in turn whether the Euclidean distance between the state pose of the latest key frame and the state pose of the historical key frame in space is less than the threshold, and whether its time is greater than the set threshold; if the threshold condition is met, the two key frames are set as a loop constraint relationship, and the latest key frame only maintains one loop constraint relationship; the key frame time threshold Δt of the loop constraint is greater than 30s, and the Euclidean distance threshold ||Δx|| of the spatial pose of the key frame of the loop constraint is less than 2m; after determining the loop constraint, the constraint condition is obtained by the ICP registration method of the frame-local key frame map. The local key frame is a local map constructed by the n key frames before and after the loop constraint historical key frame corresponding to the current latest key frame, and n is 15-20. The constraint condition is the loop constraint pose transformation relationship obtained by ICP registration; and its loop constraint condition is added to the pose factor graph.

[0130] (3) Pose graph optimization: The pose factor graph is optimized and updated using a factor graph optimization algorithm. Each time a new keyframe pose factor is added to the factor graph, the entire pose factor graph is optimized and updated in a timely manner to obtain the optimal pose estimation result. The factor graph optimization algorithm can be used to update and optimize iSAM or G20. The output of the pose graph optimization is input into the pose fusion module for constraint integration.

[0131] Exemplarily, the above method and apparatus may be implemented in the form of a computer program, which may be run on a positioning and mapping device system. Figure 4 As shown, the positioning and mapping device system includes a system processing bus, a sensor component connected to the bus, a readable storage medium, a processor, an internal memory, a data storage device and a network interface.

[0132] The sensor components include laser radar, inertial sensor IMU unit, and joint motor encoder.

[0133] The readable storage medium can store an operating system, a positioning program and a mapping program. The positioning program and the mapping program include program instructions to enable the positioning and mapping device system to execute a multi-sensor-based legged robot mobile positioning and mapping method as provided in the above embodiment.

[0134] The processor is used to provide computing and control capabilities, implement the instruction set execution function as described in the aforementioned embodiment, and support the operation of the entire system.

[0135] The internal memory provides an operating environment for the positioning program and the mapping program in the readable storage medium, and enables the processor to execute the steps of the instruction set system of the aforementioned embodiment when the positioning program and the mapping program are executed by the processor.

[0136] The data storage is a storage medium for storing the results of the execution of the multi-sensor-based legged robot mobile positioning and mapping method provided in the above embodiment. The data storage may include a high-speed RAM memory, or may include a non-volatile storage NVM, such as at least one disk storage.

[0137] The network interface is used for network communication of the positioning and mapping equipment system, such as data reception, task allocation and sending, etc.

[0138] The above embodiments are only used for the technical solutions of the present application, rather than limiting the same. Those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some or all of the technical features therein may be replaced by equivalents, and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. A positioning and mapping method and system based on motion feature constraints of legged robots, characterized in that: Includes steps: S1: Acquire various sensor data of the leg-like robot, including encoder data of motors of various joints of the leg-like robot, original point cloud data of laser radar, and IMU inertial unit data, and perform timestamp alignment and data preprocessing on various sensor data; S2: Perform time integration based on the IMU inertial unit data to calculate the robot's inertial prior pose estimate; S3: Calculate the contact state of the leg and foot and update the robot's motion state estimate based on the encoder data of each joint of the leg robot, the inertial prior posture state, and the robot's leg and foot kinematics; S4: According to the inertial prior posture and the laser radar raw data, the motion distortion of the laser radar acquisition point is compensated point by point, and a residual model of the laser feature point is constructed; S5: According to the robot's foot and leg kinematic state and feature point residual model, a motion constraint model and an iterative optimization model are constructed based on the threshold of the foot end force, and the state optimization is updated based on the convergence threshold or the number of iterations, and the robot's real-time optimal posture state is output; S6: According to the real-time optimal posture state estimation of the robot, the key frames are selected and the posture graph optimization model is constructed. The state error accumulated by the sensor data is optimized at the back end, and the optimized state data is corrected and updated in S5. The key frame map is converted to the initial coordinate system to accumulate and publish the map; S1 to S6 are executed in a loop to update the robot's position and posture until the positioning task is completed.

2. According to claim 1, a positioning and mapping method and system based on motion feature constraints of legged robots, characterized in that: The timestamp alignment described in step S1 includes: taking the timestamp of the laser radar sensor data as a reference, aligning the timestamps of the inertial unit IMU, the joint motor encoder and the laser radar to complete time synchronization, and the synchronized data calculation method is as follows: In the formula, Data1 and Data2 are the data before and after the alignment object on the timeline, Data new It is the synchronization data with the laser radar data frame acquisition time. T1 and T2 are the timestamps of the aligned objects on the timeline. new It is the timestamp after alignment; calibrate the internal parameters of IMU and the external parameters of IMU and lidar, the external parameters include rotation parameters and displacement parameters; the internal parameters of IMU include: gyroscope data, acceleration data mean and zero bias in IMU data; the inertial prior pose estimation of the robot is obtained by calculating the IMU inertial unit data through data time integration within the data time period after synchronization; the IMU inertial unit data is obtained by zero drift correction in step S1.

3. According to claim 1, a positioning and mapping method and system based on motion feature constraints of legged robots, characterized in that: The robot kinematic state estimation in step S3 includes: Based on the mechanical structure of the leg-type robot and the angle information of the joint motor encoder, the kinematic equation J of the foot-end position of the leg-type robot is obtained. k (θ), where θ is the angle of the joint motor encoder; based on the robot kinematics Jacobian matrix J, the robot foot velocity equation is obtained: in is the angle differential of the joint motor encoder with respect to time, i.e., the joint motor angular velocity; According to the robot's inertial priori posture and the foot end position equation and foot end velocity equation described in step S2, the constraint items and update items on the robot's spatial posture are obtained, and the constraint items and update items are only performed when the foot end is in contact with the ground; The spatial constraint equation is as follows: Wherein, Δx is the robot inertial prior posture sampling calculation result described in step S2, and Δt is the sampling time; The updating is performed by the least square method, the constraint equation is the above-mentioned spatial constraint equation, and the robot is estimated based on the kinematic state X.

4. The kinematic equation J of the foot position of the legged robot according to claim 3 k (θ), characterized in that The motion equation is the normalized kinematic equation of the foot end points of the leg structure, which contains the vector results of the n leg support points of the leg robot relative to the center of mass of the body in a single support action cycle. The kinematic equation J k (θ) is as follows: Among them, j k (θ n ) is the vector result of the nth leg of the legged robot relative to the center of mass of the body during its single support action cycle, θ n It is represented as the angle vector of the nth leg joint degree of freedom. The calculation frequency of this equation is consistent with the leg motion cycle of the wheel-legged robot.

5. The single support action cycle according to claim 4, characterized in that: In a single stance phase, a linear fit is performed based on the power exerted by the joint on the foot and leg, and the force exerted by the foot end on the ground is calculated and filtered. The calculation method is as follows: Where F is the force exerted by the foot on the ground, τ i is the torque of the i-th joint motor of a single leg calculated by the robot; threshold filtering is performed according to the force applied by the foot end to the ground, and if it exceeds the threshold setting interval, it is determined that the current leg is in the support phase state. The threshold value range is m is the mass of the robot body, k is the number of robot legs, g is the gravitational acceleration, and J is the Jacobian matrix of the robot's current calculated leg; the force calculation parameters applied by the foot end to the ground are the angle and torque value of the joint motor, and the data sampling period and frequency are consistent with the joint data output.

6. According to claim 1, a positioning and mapping method and system based on motion feature constraints of legged robots, characterized in that: The laser feature point residual model in step S4 includes: The feature points are obtained by using motion distortion compensation data through curvature calculation; The motion distortion compensation data is obtained by performing point-by-point motion back propagation on the laser radar raw data through inertial prior posture state estimation; The laser residual model is the distance residual value from the feature point to the feature surface; The distance residual value formula is as follows: Among them, A, B, C, D are the plane equation parameters of the characteristic surface, and d is the calculated distance residual value; The characteristic surface is the plane where the laser radar data points are located in the global map. The iterative optimization model comprises: The iterative optimization algorithm of the iterative optimization model is an ESKF filtering optimization algorithm; The observation model equation of the iterative optimization is: h=u T ((R·p+t)-q); Among them, u T is the normal vector of the feature surface, i.e., ABC in claim 5, p is the data feature point detected by the laser radar, q is the feature surface point closest to p in the feature surface in the global map, and R, t is the iteratively calculated positioning state quantity; The observed update amount of the iterative optimization model is the distance residual value d described in claim 5.

7. According to claim 1, a positioning and mapping method and system based on motion feature constraints of legged robots, characterized in that: The motion constraint model in step S5 includes: The motion constraint model equation is: t z =λ1H+λ2L; Where Δh L and Δh leg is the height increment in the displacement state calculated by the iterative optimization model and the robot kinematic state estimation; σ is the weight gain, which is between 0.8 and 1.2; λ1 and λ2 are the constraint weight ratios, which are between 0.4 and 0.7; H and L are the state quantity and the gain state increment in the height direction, respectively; t z is the height state after constraint.

8. According to claim 1, a positioning and mapping method and system based on motion feature constraints of legged robots, characterized in that: The key frame pose graph optimization model in step S6 includes: Key frame screening is performed according to the state quantities obtained by the iterative optimization model and the motion constraint model, wherein the key frame screening is performed in time and space, the time Δt between adjacent key frames is between 3 and 10 seconds, and the Euclidean distance ||Δx|| between adjacent key frames is between 5 and 10 meters; According to the position encoding of the key frames, a pose-based factor model is constructed, and the pose information of the key frames is added to the global factor graph optimization as a positioning constraint; Performing real-time loop closure constraints based on the current key frame pose and the optimized key frame pose, wherein the loop closure constraints include loop closure constraint key frame detection and loop closure constraint registration; The loop constraint key frame detection constrains the spatial Euclidean distance and time of all historical key frames with the current key frame, and the spatial Euclidean distance and time that meet the constraints are regarded as the loop constraint key frame of the current frame, the key frame time Δt of the loop constraint is greater than 30s, and the Euclidean distance ||Δx|| of the key frame of the loop constraint is less than 2m; The loop constraint registration adopts the ICP registration method of the frame-local keyframe map to obtain the constraint result, the local keyframe is a local map constructed by the n keyframes before and after the loop constraint keyframe corresponding to the current keyframe, and the value of n is 15-20. The constraint result is the loop constraint posture conversion relationship obtained by ICP registration; The loop closure constraint includes adding two key frames corresponding to the constraint and the loop closure pose conversion relationship into the global factor graph optimization.

9. Electronic equipment, including: at least one processor and memory; The memory stores computer-executable instructions; The at least one processor executes the computer execution instructions stored in the memory, so that the at least one processor performs the method as described in claims 1-8.

10. A computer-readable storage medium storing computer-executable instructions, wherein when a processor executes the computer-executable instructions, the method as claimed in claims 1 to 8 is implemented.

Images