Robot positioning method and device, electronic equipment and storage medium

Abstract

The invention relates to a robot positioning method and device, electronic equipment and a storage medium, and the method comprises the steps: obtaining part data from a target part in a robot, the target part comprising an inertial measurement unit and / or a joint motor; predicting a speed sequence diagram of the robot based on the component data, wherein the speed sequence diagram represents the speed of the robot at each timestamp; and determining the motion track of the robot according to the speed sequence diagram. According to the embodiment of the invention, the robot can be accurately positioned by using the information acquired by the inertial measurement unit and / or the joint motor of the robot body.

Description

Technical Field

[0001] The present disclosure relates to the field of robots, and in particular to a robot positioning method and device, electronic equipment, and storage medium. Background Art

[0002] Positioning is a fundamental function for mobile robots. Its basic operating principle is to use information collected by the robot's sensors, including its own state and surrounding environment, to estimate the robot's posture and state within the environment. Accurate positioning information enables the robot to perform higher-level planning and decision-making tasks. For robots operating over long periods of time, the positioning system must be accurate, real-time, stable, and robust.

[0003] In existing technologies, vision-based visual inertial odometry (VIO) methods are widely used to solve the problem of robot positioning in unknown environments. Current robot positioning relies heavily on data provided by external sensors (e.g., cameras, lidar, etc.). However, when robots encounter degraded environments where vision and lidar are unavailable (e.g., foggy and dusty weather, underground tunnels, etc.), their own sensors become an important source of localization. However, the data returned by these sensors can contain a significant amount of nonlinear noise, and the noise can even change with the robot's motion state. Modeling and eliminating noise for each type of robot, each type of sensor, and even at each moment is time-consuming and laborious. Therefore, how to leverage the robot's own information to achieve precise localization has become a pressing issue. Summary of the Invention

[0004] In view of this, the present disclosure proposes a robot positioning method and device, electronic equipment and storage medium, which can predict the speed of the robot at each time stamp and determine the motion trajectory of the robot through data collected by the inertial measurement unit and / or joint motor of the robot body, thereby realizing accurate positioning of the robot using the body information.

[0005] According to one aspect of the present disclosure, a robot positioning method is provided, the method comprising: acquiring component data from a target component in a robot, the target component comprising an inertial measurement unit and / or a joint motor; predicting a velocity sequence graph of the robot based on the component data, the velocity sequence graph representing the velocity of the robot at each time stamp; and determining a motion trajectory of the robot according to the velocity sequence graph.

[0006] In a possible implementation, the velocity sequence diagram of the robot predicted based on the component data includes: inputting the component data into a prediction model for calculation to obtain the displacement of the robot; determining the velocity of the robot at each time stamp according to the displacement of the robot and the sampling frequency of the target component, where the time stamp is determined according to the sampling frequency; and forming the velocity sequence diagram of the robot according to the velocity of the robot at each time stamp.

[0007] In a possible implementation, the component data includes the first acceleration and the first angular velocity of the robot's movement collected by the inertial measurement unit, and the prediction model includes a first prediction model. Among them, the step of inputting the component data into the prediction model for calculation to obtain the displacement of the robot includes: using the attitude quaternion to transform the first acceleration and the first angular velocity in the inertial measurement unit coordinate system into the world coordinate system to obtain the second acceleration and the second angular velocity, where the attitude quaternion represents the rotation angle of the inertial measurement unit at the initial moment and the current moment; and inputting the second acceleration and the second angular velocity into the first prediction model for calculation to obtain the displacement of the robot.

[0008] In a possible implementation, the step of inputting the second acceleration and the second angular velocity into the first prediction model for calculation to obtain the displacement of the robot includes: eliminating the bias of the second acceleration and the second angular velocity according to the initial acceleration and the initial angular velocity collected by the inertial measurement unit at the initial moment to obtain the preprocessed second acceleration and the second angular velocity; and inputting the preprocessed second acceleration and the second angular velocity into the first prediction model to obtain the displacement of the robot.

[0009] In a possible implementation, the component data includes the first torque and the first angle returned by the joint motor. The prediction model further includes a second prediction model. The joint motor represents the motor device that drives the movement structure of the robot, and the joint motor is placed at the joint of the movement structure. The first torque represents the output torque of the joint motor, and the first angle represents the angle of rotation of the joint motor. Among them, the step of inputting the component data into the prediction model for calculation to obtain the displacement of the robot includes: inputting the first torque and the first angle into the second prediction model for calculation according to the model structure of the robot to obtain the displacement of the robot.

[0010] In a possible implementation, the component data includes the first acceleration and the first angular velocity collected by the inertial measurement unit, the first torque and the first angle returned by the joint motor, and the prediction model includes a first prediction model and a second prediction model. Among them, inputting the component data into the prediction model for calculation to obtain the displacement of the robot includes: inputting the first acceleration and the first angular velocity into the first prediction model for calculation to obtain a first displacement; inputting the first torque and the first angle into the second prediction model for calculation according to the model structure of the robot to obtain a second displacement; determining the first displacement and the first uncertainty of the first prediction model as the mean and variance of a Gaussian distribution respectively to obtain a first function conforming to the Gaussian distribution, and determining the second displacement and the second uncertainty of the second prediction model as the mean and variance of a Gaussian distribution respectively to obtain a second function conforming to the Gaussian distribution; fusing the first function and the second function to obtain a third function conforming to the Gaussian distribution, and determining the displacement of the robot according to the third function.

[0011] In a possible implementation, determining the motion trajectory of the robot according to the velocity sequence diagram includes: determining the motion trajectory of the robot according to the positioning frequency of the robot, the initial position, and the velocity sequence diagram.

[0012] In a possible implementation, determining the velocity of the robot at each time stamp according to the displacement of the robot and the sampling frequency of the target component includes: determining a corresponding plurality of sampling times based on the sampling frequency of the robot, and determining a plurality of velocities to be processed of the robot within each sampling time according to the displacement of the robot; taking the mean of the plurality of velocities to be processed at each sampling time as the velocity corresponding to the time stamp of the sampling time.

[0013] In a possible implementation, the method further includes a first training step for the first prediction model, and the first training step includes: obtaining a first training set, where the first training set includes: the third acceleration and the third angular velocity of the robot motion collected by the inertial measurement unit, and the corresponding actual displacement of the robot; inputting the third acceleration and the third angular velocity into the first prediction model to be trained for calculation to obtain a first predicted displacement; calculating a first loss of the first prediction model to be trained according to the first predicted displacement and the corresponding actual displacement by using a mean square error function, and updating the parameters of the first prediction model to be trained according to the first loss until a converged first prediction model is obtained; inputting the third acceleration and the third angular velocity into the converged first prediction model for calculation to obtain a second predicted displacement; calculating a second loss of the converged first prediction model according to the second predicted displacement and the corresponding actual displacement by using a maximum Gaussian likelihood function, and updating the parameters of the converged first prediction model according to the second loss until the converged first prediction model converges again, and completing the training of the first prediction model to obtain the first prediction model.

[0014] In a possible implementation, the method further includes a second training step for the second prediction model, and the second training step includes: obtaining a second training set, where the second training set includes: the second torque and the second angle returned by the joint motor, and the corresponding actual displacement of the robot; inputting the second torque and the second angle into the second prediction model to be trained for calculation to obtain a third predicted displacement; calculating a third loss of the second prediction model to be trained according to the third predicted displacement and the corresponding actual displacement by using a mean square error function, and updating the parameters of the second prediction model to be trained according to the third loss until a converged second prediction model is obtained; inputting the second torque and the second angle into the converged second prediction model for calculation to obtain a fourth predicted displacement, calculating a fourth loss of the converged second prediction model according to the fourth predicted displacement and the corresponding actual displacement by using a maximum Gaussian likelihood function, and updating the parameters of the converged second prediction model according to the fourth loss until the converged second prediction model converges again, and completing the training of the second prediction model to obtain the second prediction model.

[0015] According to one aspect of the present disclosure, a robot positioning device is provided. The device includes: an acquisition module configured to acquire component data from a target component in the robot, the target component including an inertial measurement unit and / or a joint motor; a prediction module configured to predict a speed sequence diagram of the robot based on the component data, the speed sequence diagram representing the speed of the robot at each time stamp; and a trajectory determination module configured to determine a motion trajectory of the robot according to the speed sequence diagram.

[0016] In a possible implementation manner, the prediction module is further configured to: input the component data into a prediction model for calculation to obtain a displacement amount of the robot; determine the speed of the robot at each time stamp according to the displacement amount of the robot and the sampling frequency of the target component, the time stamp being determined according to the sampling frequency; and form the speed sequence diagram of the robot according to the speed of the robot at each time stamp.

[0017] In a possible implementation manner, the component data includes a first acceleration and a first angular velocity of the robot movement collected by the inertial measurement unit, and the prediction model includes a first prediction model. Wherein, the inputting the component data into the prediction model for calculation to obtain the displacement amount of the robot includes: converting the first acceleration and the first angular velocity in the inertial measurement unit coordinate system to the world coordinate system by using an attitude quaternion to obtain a second acceleration and a second angular velocity, the attitude quaternion representing the rotation angle of the inertial measurement unit at the initial moment and the current moment; and inputting the second acceleration and the second angular velocity into the first prediction model for calculation to obtain the displacement amount of the robot.

[0018] In a possible implementation manner, the inputting the second acceleration and the second angular velocity into the first prediction model for calculation to obtain the displacement amount of the robot includes: eliminating the bias of the second acceleration and the second angular velocity according to the initial acceleration and the initial angular velocity collected by the inertial measurement unit at the initial moment to obtain a preprocessed second acceleration and a second angular velocity; and inputting the preprocessed second acceleration and the second angular velocity into the first prediction model to obtain the displacement amount of the robot.

[0019] In a possible implementation, the component data includes the first torque and the first angle returned by the joint motor. The prediction model further includes a second prediction model. The joint motor represents the motor device that drives the motion structure of the robot. The joint motor is placed at the joint of the motion structure. The first torque represents the output torque of the joint motor, and the first angle represents the angle by which the joint motor rotates. Wherein, inputting the component data into the prediction model for calculation to obtain the displacement of the robot includes: inputting the first torque and the first angle into the second prediction model for calculation according to the model structure of the robot to obtain the displacement of the robot.

[0020] In a possible implementation, the component data includes the first acceleration and the first angular velocity collected by the inertial measurement unit, the first torque and the first angle returned by the joint motor. The prediction model includes a first prediction model and a second prediction model. Wherein, inputting the component data into the prediction model for calculation to obtain the displacement of the robot includes: inputting the first acceleration and the first angular velocity into the first prediction model for calculation to obtain a first displacement; inputting the first torque and the first angle into the second prediction model for calculation according to the model structure of the robot to obtain a second displacement; respectively determining the first displacement and the first uncertainty of the first prediction model as the mean and variance of a Gaussian distribution to obtain a first function conforming to the Gaussian distribution, and respectively determining the second displacement and the second uncertainty of the second prediction model as the mean and variance of a Gaussian distribution to obtain a second function conforming to the Gaussian distribution; fusing the first function and the second function to obtain a third function conforming to the Gaussian distribution, and determining the displacement of the robot according to the third function.

[0021] In a possible implementation, the trajectory determination module is further configured to: determine the motion trajectory of the robot according to the positioning frequency of the robot, the initial position, and the velocity sequence diagram.

[0022] In a possible implementation, determining the velocity of the robot at each time stamp according to the displacement of the robot and the sampling frequency of the target component includes: determining a corresponding plurality of sampling times based on the sampling frequency of the robot, and determining a plurality of to-be-processed velocities of the robot within each sampling time according to the displacement of the robot; taking the mean of the plurality of to-be-processed velocities at each sampling time as the velocity corresponding to the time stamp of the sampling time.

[0023] In a possible implementation, the device further includes a first training module for a first prediction model, and the first training module is configured to: obtain a first training set, where the first training set includes: a third acceleration and a third angular velocity of the robot's movement collected by an inertial measurement unit, and the corresponding actual displacement of the robot; input the third acceleration and the third angular velocity into the first prediction model to be trained for calculation to obtain a first predicted displacement; calculate a first loss of the first prediction model to be trained according to the first predicted displacement and the corresponding actual displacement by using a mean square error function, and update the parameters of the first prediction model to be trained according to the first loss until a converged first prediction model is obtained; input the third acceleration and the third angular velocity into the converged first prediction model for calculation to obtain a second predicted displacement; calculate a second loss of the converged first prediction model according to the second predicted displacement and the corresponding actual displacement by using a maximum Gaussian likelihood function, and update the parameters of the converged first prediction model according to the second loss until the converged first prediction model converges again, and complete the training of the first prediction model to obtain the first prediction model.

[0024] In a possible implementation, the device further includes a second training module for a second prediction model, and the second training module is configured to: obtain a second training set, where the second training set includes: a second torque and a second angle returned by a joint motor, and the corresponding actual displacement of the robot; input the second torque and the second angle into the second prediction model to be trained for calculation to obtain a third predicted displacement; calculate a third loss of the second prediction model to be trained according to the third predicted displacement and the corresponding actual displacement by using a mean square error function, and update the parameters of the second prediction model to be trained according to the third loss until a converged second prediction model is obtained; input the second torque and the second angle into the converged second prediction model for calculation to obtain a fourth predicted displacement, calculate a fourth loss of the converged second prediction model according to the fourth predicted displacement and the corresponding actual displacement by using a maximum Gaussian likelihood function, and update the parameters of the converged second prediction model according to the fourth loss until the converged second prediction model converges again, and complete the training of the second prediction model to obtain the second prediction model.

[0025] According to an aspect of the present disclosure, there is provided an electronic device, including: a processor; a memory for storing instructions executable by the processor; wherein, the processor is configured to call the instructions stored in the memory to execute the above method.

[0026] According to one aspect of the present disclosure, there is provided a computer-readable storage medium having computer program instructions stored thereon, and when the computer program instructions are executed by a processor, the above method is implemented.

[0027] In the embodiments of the present disclosure, the displacement amount of the robot is predicted by obtaining the body information from the inertial measurement unit and / or joint motors of the robot body, and the speed of the robot at each time stamp is determined to obtain a speed sequence diagram of the robot, and the motion trajectory of the robot is determined by using the speed sequence diagram. According to the motion trajectory, the positioning of the robot can be accurately and real-time completed.

[0028] It should be understood that the above general description and the following detailed description are only exemplary and explanatory, and do not limit the present disclosure. According to the following detailed description of the exemplary embodiments with reference to the accompanying drawings, other features and aspects of the present disclosure will become clear. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] The accompanying drawings herein are incorporated into the specification and constitute a part of the specification. These drawings illustrate embodiments consistent with the present disclosure and are used together with the specification to explain the technical solutions of the present disclosure.

[0030] Figure 1 A flowchart showing a method for robot positioning according to an embodiment of the present disclosure.

[0031] Figure 2 A schematic diagram showing an inertial measurement unit odometer according to an embodiment of the present disclosure.

[0032] Figures 3a - 3c A schematic diagram showing a comparison between the predicted value and the true value of the first prediction model according to an embodiment of the present disclosure.

[0033] Figures 4a - 4c A schematic diagram showing a comparison between the predicted value and the true value of the second prediction model according to an embodiment of the present disclosure.

[0034] Figures 5a - 5b A schematic diagram showing a motion trajectory predicted by an inertial measurement unit odometer according to an embodiment of the present disclosure.

[0035] Figures 6a - 6b A schematic diagram showing a motion trajectory predicted by a joint motor odometer according to an embodiment of the present disclosure.

[0036] Figure 7 A schematic diagram showing the structure of a robot positioning device according to an embodiment of the present disclosure.

[0037] Figure 8 A block diagram showing an electronic device 1900 according to an embodiment of the present disclosure. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0038] Various exemplary embodiments, features, and aspects of the present disclosure will be described in detail below with reference to the accompanying drawings. Like reference numerals in the drawings denote functionally identical or similar elements. Although various aspects of the embodiments are shown in the drawings, the drawings are not necessarily drawn to scale unless otherwise specified.

[0039] As used herein, the term "exemplary" means "serving as an example, embodiment, or illustration". Any embodiment described herein as "exemplary" is not necessarily to be construed as superior or better than other embodiments.

[0040] As used herein, the term "and / or" merely describes an association relationship between associated objects and indicates that three relationships may exist. For example, A and / or B may represent: A exists alone, A and B exist simultaneously, and B exists alone. In addition, the term "at least one" as used herein means any one of a plurality or any combination of at least two of a plurality. For example, including at least one of A, B, and C may represent including any one or more elements selected from the set consisting of A, B, and C.

[0041] In addition, to better illustrate the present disclosure, numerous specific details are given in the following detailed description. Those skilled in the art should understand that the present disclosure can be implemented without some specific details. In some instances, methods, means, elements, and circuits well known to those skilled in the art are not described in detail so as to highlight the gist of the present disclosure.

[0042] Currently, the methods for robot positioning mainly include: satellite signal-based positioning, Ultra-Wide Band (UWB) positioning, lidar, vision cameras, etc. Among them, the satellite signal-based positioning method can only be used in outdoor environments. The UWB-based method requires pre-installing base stations in the environment and can only be used within the coverage of the base stations. For the positioning method based on laser sensors, the sensor measurement frequency is low, and it can only measure the structural information of objects and cannot measure the texture and color information of objects. When using vision sensors, operation failures often occur when the environmental visibility is low, the camera moves quickly, or the environment contains dynamic targets. Therefore, how to achieve robot positioning in the case of degradation where external sensors are unavailable is a problem that needs to be solved.

[0043] To solve the above technical problems, an embodiment of the present disclosure proposes a robot positioning method, which can predict the speed of the robot at each timestamp and determine the movement trajectory of the robot through the data collected by the inertial measurement unit and / or joint motors of the robot body, thereby achieving precise positioning of the robot using the body information. Figure 1 A flowchart showing the robot positioning method according to an embodiment of the present disclosure is shown. As Figure 1 shown, the robot positioning method may include:

[0044] Step S11: Obtain component data from target components in the robot, where the target components include an inertial measurement unit and / or joint motors.

[0045] Step S12: Predict a speed sequence diagram of the robot based on the component data, where the speed sequence diagram represents the speed of the robot at each timestamp.

[0046] Step S13: Determine the motion trajectory of the robot according to the speed sequence diagram.

[0047] In step S11, it is possible to obtain component data from target components in the robot.

[0048] Among them, the target components may include an inertial measurement unit and / or joint motors.

[0049] An inertial measurement unit (IMU) can be used to detect and measure acceleration, tilt, shock, vibration, rotation, and multi-degree-of-freedom motion. It is usually composed of three single-axis accelerometers and three single-axis gyroscopes. The accelerometers can measure the acceleration signals of the three axes of the robot, and the gyroscopes can measure the angular velocity signals of the robot. That is, by using the inertial measurement unit (target component), the acceleration signal and angular velocity signal (component data) of the robot can be obtained. In one example, the inertial measurement unit can also automatically perform signal processing based on the collected acceleration signals and angular velocity signals, and calculate the attitude change of the robot. That is, the inertial measurement unit can obtain the attitude quaternion (component data) of the robot.

[0050] Joint motors are used to provide power for the motion structure of the robot. In one example, for a legged robot, the leg structure is the motion structure of the robot. The arrangement position of the leg joint motors can be selected differently according to requirements. For example, for the leg structure of a robot composed of a series of linkages, the leg motion can be driven by placing shutdown motors at each joint. In this way, the leg model of the robot is simple and easy to control. Or the joint motors can be placed on the shoulder of the robot. In this way, the leg structure without joint motors can reduce the mass of the legs. As a target component, the joint motors can collect data related to the motor itself, such as motor torque, motor rotation angle, motor angular velocity, motor angular acceleration, and motor temperature, as component data by the system. In one example, the joint motors can read their own data in real time and transmit it to the acquisition unit of the robot. For example, motor A1 can complete data acquisition by itself. The joint motors can also collect data of the joint motors by setting additional sensors. In the embodiments of the present disclosure, the type of the robot, the motion structure, the model of the joint motors, and the placement position of the joint motors are not limited, and can be set according to actual needs.

[0051] Among them, the velocity sequence diagram represents the velocity of the robot at each time stamp. The velocity sequence of the robot is recorded in the velocity sequence diagram. The velocity of the robot at each time stamp can be predicted through the component data of the robot, and the velocity sequence diagram can be obtained by processing the velocities at each time stamp. The following will detail how to obtain the velocity sequence diagram.

[0052] In a possible implementation manner, step S12 may include:

[0053] Step S21, inputting the component data into a prediction model for calculation to obtain the displacement of the robot;

[0054] Step S22, determining the velocity of the robot at each time stamp according to the displacement of the robot and the sampling frequency of the target component, where the time stamp is determined according to the sampling frequency;

[0055] Step S23, forming the velocity sequence diagram of the robot according to the velocities of the robot at each time stamp.

[0056] In step S21, the target component may be an inertial measurement unit and / or a joint motor. Different target components can obtain different types of component data. In the embodiments of the present disclosure, different prediction models can be selected according to different types of component data. By inputting the component data into the prediction model, the displacement of the robot in different time periods can be predicted. Among them, according to the different types of component data, the methods for calculating the displacement of the robot include the following Method 1 - Method 3. The following will respectively give a schematic description of Method 1 - Method 3.

[0057] Method 1: In a possible implementation manner, the component data includes first component data, and the first component data is the first acceleration and the first angular velocity of the robot's movement collected by the inertial measurement unit. The prediction model includes a first prediction model. Among them, the inputting the component data into the prediction model for calculation to obtain the displacement of the robot may include: using the attitude quaternion to convert the first acceleration and the first angular velocity in the inertial measurement unit coordinate system to the world coordinate system to obtain a second acceleration and a second angular velocity, where the attitude quaternion represents the rotation angle of the inertial measurement unit at the initial moment and the current moment; inputting the second acceleration and the second angular velocity into the first prediction model for calculation to obtain the displacement of the robot.

[0058] In this implementation manner, the inertial measurement unit can collect the first acceleration and the first angular velocity during the robot's movement and input the collected component data into the first prediction model in real time, and the displacement of the robot is predicted through the first prediction model.

[0059] In an embodiment of the present disclosure, after the inertial measurement unit collects the first component data, it can pass the collected first component data to a processing unit that can predict the displacement based on the first component data through the Robot Operating System (ROS), wherein the ROS system is an operating system for the robot that can transfer data between separate processing nodes (such as a node for collecting component data and a node for predicting displacement). Those skilled in the art can set up the ROS system according to actual needs, and the present disclosure does not impose any restrictions on this.

[0060] During the transmission of the first component data through the ROS system, a window time needs to be set in advance. The inertial measurement unit takes the first component data within the fixed window time and inputs it into the first prediction model each time. The window time can be set as needed. In an example, the window time is set to 1 second, that is, the inertial measurement unit inputs the first component data within 1s into the first prediction model each time.

[0061] The number of first component data collected within the window time is determined by the sampling frequency, that is, the inertial measurement unit can collect N first component data (acceleration data) in one window time (1s). Angular velocity data N is determined by the sampling frequency. In one example, the sampling frequency of the inertial measurement unit may be 250 Hz. In this case, the inertial measurement unit may collect 250 pieces of first component data within a window time. The first component data collected within each window time is input into the first prediction model to obtain the displacement of the robot at that moment.

[0062] Since the inertial measurement unit is installed on the robot, its installation relationship is fixed. It can be considered that the first acceleration and first angular velocity measured by the inertial measurement unit are measured in the body coordinate system (inertial measurement unit coordinate system). In one example, in order to reduce the calculation process in the subsequent displacement prediction process, the first acceleration and first angular velocity can be converted from the inertial measurement unit coordinate system to the world coordinate system before inputting the first prediction model. Among them, the inertial measurement unit coordinate system can be based on the IMU geometric center as the origin, the X-axis direction is defined as the roll axis direction, the Y-axis direction is defined as the pitch axis direction, and the Z-axis direction is defined as the heading axis direction; the world coordinate system can be the world initial coordinate system, that is, the X and Y axes are located in the local horizontal plane, the positive direction of the Z axis points to the sky, and the initial heading angle is 0.

[0063] In the embodiments of the present disclosure, the first acceleration and the first angular velocity can be transformed into the world coordinate system through the attitude quaternion. The attitude quaternion can represent the rotation angle of the inertial measurement unit at the initial moment and the current moment. The initial moment can be the power-on moment of the robot or any arbitrarily selected measurement moment. The transformation between two coordinate systems can be regarded as the transformation of the angular position relationship between the two coordinate systems, that is, it can be regarded as the fixed-point rotation of a coordinate system. Therefore, the transformation of the coordinate system can be completed through the attitude quaternion. As mentioned above, according to different ways of obtaining the attitude quaternion of the inertial measurement unit, the methods are also different. For example, for the Unitree A1 robot, the inertial measurement unit installed on it can automatically output the corresponding attitude quaternion when collecting the first acceleration and the first angular velocity. For an inertial measurement unit that cannot directly output the attitude quaternion, the corresponding attitude quaternion can be obtained through the first acceleration and the first angular velocity. The attitude of the robot will not change due to its linear velocity, that is, only the angular velocity can change the attitude of the robot. Integrating the first angular velocity can obtain the angles of the axes of the robot. Through the angles of the axes changes, the attitude quaternion of the robot at the current moment can be determined.

[0064] Considering that the inertial measurement unit collects the first acceleration and the first angular velocity during the movement of the robot, affected by factors such as the temperature, zero bias, and vibration of the inertial measurement unit, the measured data is prone to errors. In order to eliminate the errors, after obtaining the second acceleration and the second angular velocity, they can be processed to reduce the data errors.

[0065] In a possible implementation manner, the step of inputting the second acceleration and the second angular velocity into the first prediction model for calculation to obtain the displacement of the robot may include: eliminating the bias of the second acceleration and the second angular velocity according to the initial acceleration and the initial angular velocity collected by the inertial measurement unit at the initial moment to obtain the preprocessed second acceleration and second angular velocity; inputting the preprocessed second acceleration and second angular velocity into the first prediction model to obtain the displacement of the robot.

[0066] As described above, the inertial measurement unit is affected by zero bias during measurement. When the robot is stationary and not subject to external forces, the data collected by the sensor should be 0. However, in reality, a data can be collected at this moment. Then this data can be regarded as the bias at rest. By collecting the acceleration and angular velocity of the robot when it is stationary and not subject to external forces through the inertial measurement unit, it can be collected multiple times. After obtaining multiple stationary data, the average value can be taken as the initial acceleration and initial angular velocity for subsequent bias elimination operations each time. That is, after obtaining the second acceleration and the second angular velocity, the bias can be eliminated through the initial acceleration and initial angular velocity to obtain the preprocessed second acceleration and second angular velocity. For the actual noise fluctuations other than zero bias, appropriate zero-mean random noise can be added to the training set during the training of the first prediction model to enhance the data, which will be introduced in detail in the training part of the first prediction model.

[0067] In the embodiment of the present disclosure, the inertial measurement unit samples the acceleration and angular velocity at a sampling frequency of 250 Hz, and uses a fixed time window (such as 1 s) to obtain the first acceleration The first angular velocity and inputs them into the processing unit of the robot through the ROS system. The first acceleration and first angular velocity in the inertial measurement coordinate system are converted to the world coordinate system by using the attitude quaternion corresponding to the first acceleration and first angular velocity to obtain the second acceleration and the second angular velocity The bias of the second acceleration and the second angular velocity is eliminated to obtain the preprocessed second acceleration and second angular velocity, and the preprocessed second acceleration and second angular velocity are input into the first prediction model to predict the displacement of the robot.

[0068] Method 2: In a possible implementation manner, the component data includes second component data, and the second component data is the first torque and the first angle returned by the joint motor. The prediction model further includes a second prediction model. The joint motor represents a motor device that drives the motion structure of the robot. The joint motor is placed at the joint of the motion structure. The first torque represents the output torque of the joint motor, and the first angle represents the angle of rotation of the joint motor. Among them, inputting the component data into the prediction model for calculation to obtain the displacement of the robot may include: inputting the first torque and the first angle into the second prediction model for calculation according to the model structure of the robot to obtain the displacement of the robot.

[0069] The joint motor can collect the first torque and the first angle of the joint motor when the robot is moving, and input the collected second component data into the second prediction model in real time, and predict the displacement of the robot through the second prediction model.

[0070] In an embodiment of the present disclosure, after the joint motor collects the second component data, the collected second component data can be transmitted to a processing unit that can predict the displacement based on the second component data through ROS. Similarly, when transmitting the first torque and the first angle through the ROS system, a window time and a sampling frequency need to be set. In one example, the window time can be 1 s and the sampling frequency can be 250 Hz. That is, the joint motor inputs the second component data within 1 s into the second prediction model each time.

[0071] According to the model of the robot motion structure, the velocity of the motion structure in the carrier coordinate system can be calculated through forward kinematic solution based on the first torque and the first angle of the shutdown motor. That is, the velocity vector of the robot's motion structure can be obtained from the first torque and the first angle. The dimension of this velocity vector is determined by the structure of different robots. For example, for a quadruped robot, its velocity vector should be 4×3, which represents the velocity vectors of the four legs in three axes.

[0072] The carrier coordinate system represents a coordinate system fixed to the robot's body structure. As the robot moves, the carrier coordinate system moves accordingly. That is, the velocity vector obtained based on the first torque and the first angle is the relative velocity in the carrier coordinate system. However, the displacement of the robot within the window time cannot be determined from the relative velocity. At this time, an initial velocity is introduced, and the average velocity of the previous window time can be used as the initial velocity. In this way, the displacement of the robot can be determined based on the relative velocity of the motion structure and the initial velocity.

[0073] In an embodiment of the present disclosure, the joint motor collects the first torque and the first angle and inputs them into the second prediction model. The relative velocity of the robot's motion structure can be obtained based on the first torque and the first angle. The displacement of the robot in the current window time can be predicted based on the relative velocity of the robot and the initial velocity fed back.

[0074] Method three: In a possible implementation method, the component data includes first component data and second component data, the first component data is the first acceleration and first angular velocity collected by the inertial measurement unit, and the second component data is the first torque and first angle returned by the joint motor. The prediction model includes a first prediction model and a second prediction model, wherein inputting the component data into the prediction model for calculation to obtain the displacement of the robot may include: inputting the first acceleration and the first angular velocity into the first prediction model for calculation to obtain the first displacement; inputting the first torque and the first angle into the second prediction model for calculation according to the model structure of the robot to obtain the second displacement; determining the first displacement and the first uncertainty of the first prediction model as the mean and variance of the Gaussian distribution respectively to obtain a first function that conforms to the Gaussian distribution, and determining the second displacement and the second uncertainty of the second prediction model as the mean and variance of the Gaussian distribution respectively to obtain a second function that conforms to the Gaussian distribution; fusing the first function and the second function to obtain a third function that conforms to the Gaussian distribution, and determining the displacement of the robot according to the third function.

[0075] In the disclosed embodiment, the displacement of the robot can be predicted by the data collected by the inertial measurement unit and the joint motor. Similarly, after the inertial measurement unit and the joint motor collect the component data, the collected component data can be transmitted through ROS to a processing unit that can predict the displacement based on the component data. When transmitting the component data through the ROS system, a window time and sampling frequency need to be set. In an example, the window time can be 1s and the sampling frequency can be 250Hz.

[0076] Inputting the first acceleration and the first angular velocity into the first prediction model can obtain the first displacement, and inputting the first torque and the first angle into the second prediction model can obtain the second displacement. Obviously, the sources of the component data are different, and in actual use, each target component has errors, but the distribution of their errors is different. In addition, for the prediction model, the prediction results given may be different from the actual values. If the prediction model can give a more reasonable uncertainty for the wrong prediction, the credibility of the prediction results can be judged. The most intuitive way to measure uncertainty is variance. For a set of data (p i represents the predicted displacement at different sampling moments), variance σ 2 The calculation formula is:

[0077]

[0078] in, represents the actual displacement of the robot, and N represents the total amount of data in this group of data, which is equivalent to the number of component data collected within a window time.

[0079] The uncertainties of the first prediction model and the second prediction model can be obtained through Formula 1. Denote the first uncertainty as and the second uncertainty as . The first displacement can be denoted as μ1, and the second displacement can be denoted as μ2. Determine the first displacement μ1 and the first uncertainty of the first prediction model as the mean and variance of the Gaussian distribution respectively to obtain the first function that conforms to the Gaussian distribution. Determine the second displacement μ2 and the second uncertainty of the second prediction model as the mean and variance of the Gaussian distribution respectively to obtain the second function that conforms to the Gaussian distribution. The multiplication of two Gaussian distributions is still a Gaussian distribution. Therefore, by fusing the first function and the second function, the third function can be obtained, whose mean is and whose variance is It can be seen that after multiplying the first function and the second function, the variance of the third function is significantly reduced. As mentioned before, the smaller the variance means the smaller the uncertainty of the prediction model. Therefore, take the mean μ3 of the third function as the displacement of the robot. In this way, the influence of each other's errors is eliminated by fusing two prediction models.

[0080] Among them, in Method 3, when calculating the second displacement by inputting the first torque and the first angle into the second prediction model according to the model structure of the robot, the component data obtained by the inertial measurement unit can be used to assist in obtaining the second displacement.

[0081] In the embodiments of the present disclosure, when the component data includes the first component data and the second component data, after obtaining the component data collected by the target component, the first torque and the first angle can be input into the second prediction model according to the model structure of the robot. At this time, the first acceleration collected by the inertial measurement unit is stored in the processing unit at the same time. In order to make the prediction of the component data returned by the joint motor more accurate, the first acceleration can be used to assist the prediction of the second prediction model.

[0082] In the embodiments of the present disclosure, a velocity pre-integration module with a loop structure is introduced to integrate the acceleration information at the current window time:

[0083]

[0084] Among them, represents the velocity of the robot at time t in the world coordinate system, represents the initial velocity, Represents the acceleration of the robot at time t in the world coordinate system. The initial velocity can be the average velocity of the previous time window.

[0085] As described above, the overall velocity of the robot can be predicted using the first acceleration, that is, the velocity at time t+1 can be predicted using the acceleration at time t and the predicted velocity at time t in the previous moment. In this way, the overall velocity of the robot within this window time can be predicted using 250 acceleration data samples obtained within a window time. When inputting the first torque and the first angle into the second prediction model according to the model structure of the robot, a more accurate second displacement can be obtained using the pre-estimation of the robot's velocity within this window time.

[0086] In the embodiment of the present disclosure, component data (the first acceleration, the first angular velocity, the first torque, the first angle) can be obtained through an inertial measurement unit and / or joint motors. The two target components can be used separately or simultaneously. When used separately, the first acceleration and the first angular velocity collected by the inertial measurement unit can be input into the first prediction model to predict the displacement of the robot; the first torque and the first angle collected by the joint motors can be input into the second prediction model to predict the displacement of the robot. When used simultaneously, the first acceleration and the first angular velocity collected by the inertial measurement unit are input into the first prediction model to obtain the first displacement, and the first torque and the first angle collected by the joint motors are input into the second prediction model to obtain the second displacement. The two displacements are fused to obtain the displacement of the robot.

[0087] For the above step S22, it may include: determining a corresponding plurality of sampling times based on the sampling frequency of the robot, and determining a plurality of to-be-processed velocities of the robot at each of the sampling times according to the displacement of the robot; taking the mean of the plurality of to-be-processed velocities at each sampling time as the velocity corresponding to the time stamp of the sampling time.

[0088] In the embodiment of the present disclosure, the sampling time difference can be determined according to the sampling frequency. For example, when the sampling frequency is 250 Hz, the sampling time is 0.004 s. In step S21, the robot can predict the displacement of the robot according to the prediction model. Therefore, according to the displacement of the robot at each sampling time, the average velocity of the robot at each sampling time can be predicted. Regarding this average velocity as the velocity of the robot within the sampling time, a time stamp is set at this sampling moment, and the above-obtained average velocity is the velocity corresponding to this time stamp.

[0089] For the above-mentioned step S23, in a possible implementation, when the robot performs positioning, a positioning frequency needs to be set, and the positioning moment is determined through the positioning frequency. At each positioning moment, the robot can take the component data collected within a time period for positioning. The time period can be set as needed. In one example, the component data within a window time can be taken for positioning, that is, the component data within 1 s can be taken at each positioning moment to complete the positioning.

[0090] In one example, the positioning frequency can be 50 Hz, that is, the time difference between every two positioning moments is 0.02 s. From the above description, it can be seen that at each positioning moment, the component data collected within a time period needs to be taken to complete the positioning. Assuming that the component data within a window time (1 s) is used for positioning, then the time difference between the starts of every two window times is 0.02 s. It can be predicted that there will be a 0.98 s overlapping area for two adjacent window times. Therefore, in order to make the robot positioning more accurate, the speed of each timestamp can be updated in real time.

[0091] In the embodiment of the present disclosure, after the positioning starts, at the first positioning moment, the average speed of the robot within each sampling time can be predicted according to the displacement within the first window time. A timestamp is set at each sampling moment, and thus the speed corresponding to the timestamp within the first window time can be obtained. At the second positioning moment, the average speed of the robot within each sampling time within the second window time can be predicted according to the displacement within the second window time. There is an overlapping area between the second window time and the first window time. Therefore, for the sampling time for which the speed already exists, the two speeds are fused to obtain the speed corresponding to the timestamp. For the sampling time for which there is no speed to be processed, a timestamp can be set at this moment. By traversing all timestamps, each timestamp can be updated and created to obtain the speed sequence diagram of the robot.

[0092] For the above-mentioned step S13, in a possible implementation, determining the motion trajectory of the robot according to the speed sequence diagram may include: determining the motion trajectory of the robot according to the positioning frequency, initial position of the robot, and the speed sequence diagram.

[0093] The odometer of the robot is a model that can represent the position and state of the robot in the world coordinate system at each moment. After obtaining the speed sequence diagram of the robot, the motion trajectory of the robot can be planned. In a possible implementation, the zero-order hold method can be used to obtain a continuous motion trajectory from the discrete speed, that is, at each positioning moment, the speed in the speed sequence diagram is multiplied by the sampling time to obtain the motion trajectory at this positioning moment. The positioning of the robot can be completed through the motion trajectory.

[0094] In a possible implementation, a speed can be given to each positioning moment. By fusing the speeds at each timestamp within a positioning moment in the speed sequence diagram, the speed at each positioning moment can be obtained. Similarly, using the zero-order hold method, the discrete speeds can be integrated to obtain a continuous motion trajectory.

[0095] Figure 2 The schematic diagram of the inertial measurement unit odometer according to an embodiment of the present disclosure is shown. As Figure 2 shown, taking the first way of using the inertial measurement unit alone as an example, t0 represents the first positioning moment, and the acceleration and the angular velocity respectively represent the acceleration and angular velocity data collected by the inertial measurement unit within the first window time at the first positioning moment. represents the speed at the center point of the window time starting from t0. This speed can be obtained by fusing the speeds within the window time starting from t0 in the speed sequence diagram, or the average speed can be obtained through the displacement amount of a window time and the window time, and it is regarded as the speed at the window center moment. Δt is the time difference between two positioning moments. represents the center point of the window time starting from t0, which is used as the initial position. It can be seen that the motion trajectory of the robot can be predicted through the speed and time at the current moment.

[0096] In the embodiment of the present disclosure, by inputting the component data collected by the accumulated target component (inertial measurement unit / joint motor) in real time into the trained prediction model, the displacement amount of the robot body during the prediction period is predicted. The speed points are updated through the speed sequence diagram to estimate the overall speed of the robot. Finally, the motion trajectory of the robot is restored through the speed sequence diagram, so as to achieve precise positioning of the robot using the body information.

[0097] The training method of the above-mentioned prediction model will be described below.

[0098] In a possible implementation, the method includes a first training step for a first prediction model, and the first training step may include: obtaining a first training set, where the first training set includes: the third acceleration and the third angular velocity of the robot's movement collected by an inertial measurement unit, and the corresponding actual displacement of the robot; inputting the third acceleration and the third angular velocity into the first prediction model to be trained for calculation to obtain a first predicted displacement; calculating a first loss of the first prediction model to be trained according to the first predicted displacement and the corresponding actual displacement by using a mean square error function, and updating the parameters of the first prediction model to be trained according to the first loss until a converged first prediction model is obtained; inputting the third acceleration and the third angular velocity into the converged first prediction model for calculation to obtain a second predicted displacement; calculating a second loss of the converged first prediction model according to the second predicted displacement and the corresponding actual displacement by using a maximum Gaussian likelihood function, and updating the parameters of the converged first prediction model according to the second loss until the converged first prediction model converges again, and completing the training of the first prediction model to obtain the first prediction model.

[0099] In the embodiments of the present disclosure, the training of the first prediction model is divided into two stages. In the first stage, a mean square error function is used

[0100]

[0101] where p i represents the displacement predicted by the first prediction model, and represents the actual displacement of the robot.

[0102] In the embodiments of the present disclosure, an initial parameter is set for the first prediction model. The inertial measurement unit collects the third acceleration and the third angular velocity of the robot, and uses them as a training set to input into the first prediction model to obtain a first predicted displacement. Substituting the first predicted displacement and the corresponding actual displacement into Equation 3 can obtain the first loss of the first prediction model. According to the first loss, the parameters of the first prediction model are continuously updated. In the first stage of this model training, the mean square error function accelerates the convergence of the training model, and a converged first prediction model is obtained.

[0103] In the second stage, a maximum Gaussian likelihood function is used to continue the training for the converged first prediction model:

[0104]

[0105] where represents the sum of the actual displacements of the robot during the entire sampling time.

[0106] In the embodiment of the present disclosure, the third acceleration and the third angular velocity are input into the converged first prediction model for calculation to obtain a second predicted displacement; the second predicted displacement and the corresponding actual displacement are brought into Formula 4 to calculate the second loss, and the parameters of the first prediction model are updated according to the second loss. The distance between the predicted displacement and the actual displacement is further reduced through the maximum Gaussian likelihood function, and the trained first prediction model is obtained.

[0107] The third acceleration and the third angular velocity collected by the inertial measurement unit will have noise fluctuations. Therefore, before inputting the third acceleration and the third angular velocity into the first prediction model, zero-mean random noise, i.e., Gaussian white noise, can be added to them. In this way, the data is enhanced, and the problem that the prediction model is sensitive to noise is avoided.

[0108] In a possible implementation manner, the method further includes a second training step for the second prediction model. The second training step may include: obtaining a second training set, where the second training set includes: the second torque and the second angle returned by the joint motor, and the corresponding actual displacement of the robot; inputting the second torque and the second angle into the second prediction model to be trained for calculation to obtain a third predicted displacement; calculating the third loss of the second prediction model to be trained according to the third predicted displacement and the corresponding actual displacement by using the mean square error function, and updating the parameters of the second prediction model to be trained according to the third loss until a converged second prediction model is obtained; inputting the second torque and the second angle into the converged second prediction model for calculation to obtain a fourth predicted displacement, calculating the fourth loss of the converged second prediction model according to the fourth predicted displacement and the corresponding actual displacement by using the maximum Gaussian likelihood function, and updating the parameters of the converged second prediction model according to the fourth loss until the converged second prediction model converges again, and completing the training of the second prediction model to obtain the second prediction model.

[0109] In the embodiment of the present disclosure, an initial parameter is set for the second prediction model. The joint motor collects the second torque and the second angle of the robot, and uses them as a training set to input into the second prediction model to obtain a third predicted displacement. Bringing the third predicted displacement and the corresponding actual displacement into Formula 3 can obtain the third loss of the second prediction model. The parameters of the second prediction model are continuously updated according to the third loss. In the first stage of this model training, the mean square error function speeds up the convergence of the training model, and a converged second prediction model is obtained.

[0110] In the embodiments of the present disclosure, the second torque and the second angle are input into the converged second prediction model for calculation to obtain a fourth predicted displacement; the fourth predicted displacement and the corresponding actual displacement are brought into Formula 4 to calculate the fourth loss, and the parameters of the second prediction model are updated according to the fourth loss. The distance between the predicted displacement and the actual displacement is further reduced through the maximum Gaussian likelihood function, and the trained second prediction model is obtained.

[0111] In a possible implementation, the Unitree A1 quadruped robot is used to verify the above method. The sampling frequencies of the inertial measurement unit and the joint motors are set to 250 Hz, and the positioning frequency is set to 50 Hz. The inertial measurement unit and the joint motors are respectively used to predict the motion trajectory of the robot. Figures 3a - 3c A schematic diagram showing the comparison between the predicted value and the true value of the first prediction model according to the embodiments of the present disclosure is shown. 20,000 component data collected by the inertial measurement unit are input into the first prediction model to obtain the predicted value. As Figure 3a shown, the horizontal axis represents the data sequence collected by the inertial measurement unit, the vertical axis represents the change in the x-axis in the world coordinate system, and the blue curve represents the x-axis predicted value curve obtained by inputting the first component data collected by the inertial measurement unit into the first prediction model, and the yellow curve represents the actual change in the x-axis of the robot in the world coordinate system. It can be seen that the predicted value of the first prediction model in the x-axis direction is basically consistent with the actual change of the robot, which proves that the predicted value of the first prediction model in the x-axis direction is accurate. As Figure 3b shown, the horizontal axis represents the data sequence collected by the inertial measurement unit, the vertical axis represents the change in the y-axis in the world coordinate system, and the blue curve represents the y-axis predicted value curve obtained by inputting the first component data collected by the inertial measurement unit into the first prediction model, and the yellow curve represents the actual change in the y-axis of the robot in the world coordinate system. It can be seen that the predicted value of the first prediction model in the y-axis direction is basically consistent with the actual change of the robot, which proves that the predicted value of the first prediction model in the y-axis direction is accurate. As Figure 3c shown, the horizontal axis represents the data sequence collected by the inertial measurement unit, the vertical axis represents the change in the z-axis in the world coordinate system, and the blue curve represents the z-axis predicted value curve obtained by inputting the first component data collected by the inertial measurement unit into the first prediction model, and the yellow curve represents the actual change in the z-axis of the robot in the world coordinate system. It can be seen that the predicted value of the first prediction model in the z-axis direction is basically stable around 0. Because a quadruped robot is used, in fact, the z-axis is the yaw direction, and the quadruped robot will gradually deviate from the original track over time, and there is a yaw swing during the movement. It can be seen that the first prediction model has good anti-noise ability in the z-axis direction.

[0112] Figures 4a - 4cA schematic diagram showing the comparison between the predicted values and the true values of the second prediction model according to an embodiment of the present disclosure. 8000 component data are collected by the joint motor and input into the second prediction model to obtain the predicted values. As Figure 4a shown, the horizontal axis represents the data sequence collected by the joint motor, and the vertical axis represents the change in the x-axis in the world coordinate system. The blue curve represents the x-axis predicted value curve obtained by inputting the second component data collected by the joint motor into the second prediction model, and the yellow curve represents the actual change in the x-axis of the robot in the world coordinate system. It can be seen that the predicted values of the second prediction model in the x-axis direction are basically consistent with the actual changes of the robot, proving that the predicted values of the second prediction model in the x-axis direction are accurate. As Figure 4b shown, the horizontal axis represents the data sequence collected by the joint motor, and the vertical axis represents the change in the y-axis in the world coordinate system. The blue curve represents the y-axis predicted value curve obtained by inputting the second component data collected by the joint motor into the second prediction model, and the yellow curve represents the actual change in the y-axis of the robot in the world coordinate system. It can be seen that the predicted values of the second prediction model in the y-axis direction are basically consistent with the actual changes of the robot, proving that the predicted values of the second prediction model in the y-axis direction are accurate. As Figure 4c shown, the horizontal axis represents the data sequence collected by the joint motor, and the vertical axis represents the change in the z-axis in the world coordinate system. The blue curve represents the z-axis predicted value curve obtained by inputting the second component data collected by the joint motor into the second prediction model, and the yellow curve represents the actual change in the z-axis of the robot in the world coordinate system. Since a quadruped robot is used, in fact, the z-axis is the yaw direction, and the quadruped robot will gradually deviate from the original track over time, and there is a yaw swing during the movement. It can be seen that because the second prediction model uses the predicted values obtained from the second component data collected by the shutdown motor, that is, the prediction is made through the mechanical structure, the predicted values in the z-axis are consistent with the yaw swing.

[0113] When evaluating the overall motion trajectory of the robot, the absolute trajectory error (ATE) between the predicted trajectory and the actual trajectory is compared to verify the performance of the prediction method. The root mean square error of the sampled absolute position error is used to show the accuracy of the predicted trajectory. Define the set of predicted coordinates as The set of actual coordinates of the robot is Calculate the root mean square error of its absolute position according to Equation 5:

[0114]

[0115] The trajectory predicted by the prediction model can be verified through the evaluation function. Figures 5a - 5bA schematic diagram showing the motion trajectory predicted by the inertial measurement unit odometer according to an embodiment of the present disclosure. The inertial measurement unit collects 100,000 component data and inputs it into the first prediction model to obtain a predicted value, and the predicted trajectory is obtained through the inertial measurement unit odometer. As Figure 5a shown, the abscissa is the actual trajectory change of the world coordinate system on the x-axis, the ordinate is the actual trajectory change of the world coordinate system on the y-axis, the blue curve is the motion trajectory predicted by the inertial measurement unit odometer, and the yellow curve represents the actual trajectory of the robot. It can be seen that the two curves are similar. To accurately describe the error between the two trajectories, the root mean square error is used to calculate the distance between the two trajectories. As Figure 5b shown, the abscissa is the number of data collected by the inertial measurement unit, and the ordinate is the absolute trajectory error between the predicted trajectory and the actual trajectory. It can be seen that the root mean square error is 2.204, which proves that the prediction of the inertial measurement unit odometer is credible.

[0116] Figures 6a - 6b A schematic diagram showing the motion trajectory predicted by the joint motor odometer according to an embodiment of the present disclosure. The joint motor collects 40,000 component data and inputs it into the second prediction model to obtain a predicted value, and the predicted trajectory is obtained through the joint motor odometer. As Figure 6a shown, the abscissa is the actual trajectory change of the world coordinate system on the x-axis, the ordinate is the actual trajectory change of the world coordinate system on the y-axis, the blue curve is the motion trajectory predicted by the joint motor odometer, and the yellow curve represents the actual trajectory of the robot. It can be seen that the two curves are similar. To accurately describe the error between the two trajectories, the root mean square error is used to calculate the distance between the two trajectories. As Figure 6b shown, the abscissa is the number of data collected by the joint motor, and the ordinate is the absolute trajectory error between the predicted trajectory and the actual trajectory. It can be seen that the root mean square error is 1.508, which proves that the prediction of the joint motor odometer is credible.

[0117] In an embodiment of the present disclosure, a robot positioning method is proposed, which respectively realizes the prediction of the robot's trajectory through the inertial measurement unit and / or the joint motor. By real-time inputting the component data collected by the accumulated target components (inertial measurement unit / joint motor) into the trained prediction model, the displacement of the robot body during the prediction period is predicted, and the speed points are updated through the speed sequence diagram to realize the estimation of the overall speed of the robot. Finally, the motion trajectory of the robot is restored through the speed sequence diagram, overcoming the problem that the robot cannot be accurately positioned in a degraded environment, expanding the application scenarios of robot positioning, improving its robustness and practicability, and realizing the accurate positioning of the robot using the body information.

[0118] It can be understood that, for the above-mentioned method embodiments mentioned in the present disclosure, without violating the principle logic, they can be combined with each other to form combined embodiments. Due to space limitations, the present disclosure will not elaborate further. Those skilled in the art can understand that in the above-mentioned methods of the specific implementation manner, the specific execution order of each step should be determined according to its function and possible internal logic.

[0119] In addition, the present disclosure also provides a robot positioning device, an electronic device, a computer-readable storage medium, and a program, all of which can be used to implement any one of the robot positioning methods provided by the present disclosure. For the corresponding technical solutions and descriptions, refer to the corresponding records in the method section, and details will not be repeated here.

[0120] Figure 7 The structural schematic diagram of the robot positioning device according to an embodiment of the present disclosure is shown. As Figure 7 shown, the device 70 may include:

[0121] An acquisition module 71, configured to acquire component data from a target component in the robot, where the target component includes an inertial measurement unit and / or a joint motor;

[0122] A prediction module 72, configured to predict a speed sequence diagram of the robot based on the component data, where the speed sequence diagram represents the speed of the robot at each time stamp;

[0123] A trajectory determination module 73, configured to determine a motion trajectory of the robot according to the speed sequence diagram.

[0124] In a possible implementation manner, the prediction module is further configured to: input the component data into a prediction model for calculation to obtain a displacement amount of the robot; determine the speed of the robot at each time stamp according to the displacement amount of the robot and the sampling frequency of the target component, where the time stamp is determined according to the sampling frequency; form the speed sequence diagram of the robot according to the speed of the robot at each time stamp.

[0125] In a possible implementation manner, the component data includes a first acceleration and a first angular velocity of the robot's motion collected by the inertial measurement unit, and the prediction model includes a first prediction model. Wherein, inputting the component data into the prediction model for calculation to obtain the displacement amount of the robot includes: using an attitude quaternion to convert the first acceleration and the first angular velocity in the inertial measurement unit coordinate system to the world coordinate system to obtain a second acceleration and a second angular velocity, where the attitude quaternion represents the rotation angle of the inertial measurement unit at the initial moment and the current moment; inputting the second acceleration and the second angular velocity into the first prediction model for calculation to obtain the displacement amount of the robot.

[0126] In a possible implementation, the calculating the displacement of the robot by inputting the second acceleration and the second angular velocity into the first prediction model includes: eliminating the bias of the second acceleration and the second angular velocity according to the initial acceleration and the initial angular velocity collected by the inertial measurement unit at the initial moment to obtain the preprocessed second acceleration and second angular velocity; and inputting the preprocessed second acceleration and second angular velocity into the first prediction model to obtain the displacement of the robot.

[0127] In a possible implementation, the component data includes the first torque and the first angle returned by the joint motor, the prediction model further includes a second prediction model, the joint motor represents a motor device that drives the motion structure of the robot, the joint motor is placed at the joint of the motion structure, the first torque represents the output torque of the joint motor, and the first angle represents the angle of rotation of the joint motor. Wherein, the calculating the displacement of the robot by inputting the component data into the prediction model includes: calculating the displacement of the robot by inputting the first torque and the first angle into the second prediction model according to the model structure of the robot.

[0128] In a possible implementation, the component data includes the first acceleration and the first angular velocity collected by the inertial measurement unit and the first torque and the first angle returned by the joint motor, and the prediction model includes a first prediction model and a second prediction model. Wherein, the calculating the displacement of the robot by inputting the component data into the prediction model includes: inputting the first acceleration and the first angular velocity into the first prediction model for calculation to obtain a first displacement; calculating the second displacement by inputting the first torque and the first angle into the second prediction model according to the model structure of the robot; determining the first displacement and the first uncertainty of the first prediction model as the mean and variance of the Gaussian distribution respectively to obtain a first function conforming to the Gaussian distribution, and determining the second displacement and the second uncertainty of the second prediction model as the mean and variance of the Gaussian distribution respectively to obtain a second function conforming to the Gaussian distribution; fusing the first function and the second function to obtain a third function conforming to the Gaussian distribution, and determining the displacement of the robot according to the third function.

[0129] In a possible implementation, the trajectory determination module is further configured to: determine the motion trajectory of the robot according to the positioning frequency of the robot, the initial position, and the velocity sequence diagram.

[0130] In a possible implementation, determining the speed of the robot at each timestamp according to the displacement amount of the robot and the sampling frequency of the target component includes: determining a corresponding plurality of sampling times based on the sampling frequency of the robot, and determining a plurality of speeds to be processed of the robot within each of the sampling times according to the displacement amount of the robot; taking the mean value of the plurality of speeds to be processed at each sampling time as the speed corresponding to the timestamp of the sampling time where it is located.

[0131] In some embodiments, the functions or modules included in the device provided by the embodiments of the present disclosure can be used to execute the methods described in the above method embodiments, and the specific implementation can refer to the description of the above method embodiments. For the sake of brevity, it will not be repeated here.

[0132] In a possible implementation, the device includes a first training module for a first prediction model, and the first training module is used to: obtain a first training set, where the first training set includes: the third acceleration and the third angular velocity of the robot movement collected by an inertial measurement unit, and the corresponding actual displacement amount of the robot; input the third acceleration and the third angular velocity into the first prediction model to be trained for calculation to obtain a first predicted displacement amount; calculate a first loss of the first prediction model to be trained according to the first predicted displacement amount and the corresponding actual displacement amount by using a mean square error function, and update the parameters of the first prediction model to be trained according to the first loss until a converged first prediction model is obtained; input the third acceleration and the third angular velocity into the converged first prediction model for calculation to obtain a second predicted displacement amount; calculate a second loss of the converged first prediction model according to the second predicted displacement amount and the corresponding actual displacement amount by using a maximum Gaussian likelihood function, and update the parameters of the converged first prediction model according to the second loss until the converged first prediction model converges again, and complete the training of the first prediction model to obtain the first prediction model.

[0133] In a possible implementation, the device further includes a second training module for a second prediction model, and the second training module is configured to: obtain a second training set, where the second training set includes: a second torque and a second angle returned by a joint motor, and an actual displacement of the corresponding robot; input the second torque and the second angle into a second prediction model to be trained for calculation to obtain a third predicted displacement; calculate a third loss of the second prediction model to be trained by using a mean square error function according to the third predicted displacement and the corresponding actual displacement, and update parameters of the second prediction model to be trained according to the third loss until a converged second prediction model is obtained; input the second torque and the second angle into the converged second prediction model for calculation to obtain a fourth predicted displacement, calculate a fourth loss of the converged second prediction model by using a maximum Gaussian likelihood function according to the fourth predicted displacement and the corresponding actual displacement, and update parameters of the converged second prediction model according to the fourth loss until the converged second prediction model converges again, and complete the training of the second prediction model to obtain the second prediction model.

[0134] Embodiments of the present disclosure further provide a computer-readable storage medium, on which computer program instructions are stored, and when the computer program instructions are executed by a processor, the above method is implemented. The computer-readable storage medium may be a volatile or non-volatile computer-readable storage medium.

[0135] Embodiments of the present disclosure further provide an electronic device, including: a processor; a memory for storing instructions executable by the processor; wherein, the processor is configured to call the instructions stored in the memory to execute the above method.

[0136] Embodiments of the present disclosure further provide a computer program product, including computer-readable code, or a non-volatile computer-readable storage medium carrying the computer-readable code, and when the computer-readable code runs in a processor of an electronic device, the processor in the electronic device executes the above method.

[0137] The electronic device may be provided as a server or other forms of devices.

[0138] Figure 8 A block diagram of an electronic device 1900 according to an embodiment of the present disclosure is shown. For example, the electronic device 1900 may be provided as a server or a terminal device. Refer to Figure 8, the electronic device 1900 includes a processing component 1922, which further includes one or more processors, and memory resources represented by a memory 1932 for storing instructions executable by the processing component 1922, such as application programs. The application programs stored in the memory 1932 may include one or more modules each corresponding to a set of instructions. In addition, the processing component 1922 is configured to execute instructions to perform the above method.

[0139] The electronic device 1900 may further include a power component 1926 configured to perform power management of the electronic device 1900, a wired or wireless network interface 1950 configured to connect the electronic device 1900 to a network, and an input / output (I / O) interface 1958. The electronic device 1900 may operate based on an operating system stored in the memory 1932, such as Microsoft Server Operating System (Windows Server TM ), the graphical user interface-based operating system launched by Apple Inc. (Mac OS X TM ), the multi-user and multi-process computer operating system (Unix TM ), the free and open-source Unix-like operating system (Linux TM ), the open-source Unix-like operating system (FreeBSD TM ) or the like.

[0140] In an exemplary embodiment, a non-volatile computer-readable storage medium is also provided, such as the memory 1932 including computer program instructions, and the above computer program instructions can be executed by the processing component 1922 of the electronic device 1900 to complete the above method. The computer program product can be specifically implemented in a manner of hardware, software, or a combination thereof. In an alternative embodiment, the computer program product is specifically embodied as a computer storage medium, and in another alternative embodiment, the computer program product is specifically embodied as a software product, such as a Software Development Kit (SDK), etc.

[0141] The above descriptions of the various embodiments tend to emphasize the differences between the various embodiments, and their similarities or similarities can be referred to each other. For the sake of brevity, they will not be repeated herein.

[0142] Those skilled in the art can understand that in the above method of the specific implementation manner, the writing order of each step does not mean a strict execution order and does not constitute any limitation to the implementation process. The specific execution order of each step should be determined by its function and possible internal logic.

[0143] If the technical solution of this application involves personal information, before the product applying the technical solution of this application processes personal information, it has clearly informed the personal information processing rules and obtained the individual's independent consent. If the technical solution of this application involves sensitive personal information, before the product applying the technical solution of this application processes sensitive personal information, it has obtained the individual's separate consent and at the same time meets the requirements of "express consent". For example, at a personal information collection device such as a camera, a clear and prominent sign is set up to inform that the personal information collection scope has been entered and personal information will be collected. If an individual voluntarily enters the collection scope, it is regarded as consenting to the collection of their personal information; or on the personal information processing device, when the personal information processing rules are informed by obvious signs / information, personal authorization is obtained through pop-up messages or by asking the individual to upload their personal information by themselves; among them, the personal information processing rules may include information such as the personal information processor, the purpose of personal information processing, the processing method, and the types of personal information processed.

[0144] The embodiments of the present disclosure have been described above. The above description is exemplary and not exhaustive, and is not limited to the disclosed embodiments. Many modifications and variations are obvious to those of ordinary skill in the art in the technical field without departing from the scope and spirit of the described embodiments. The choice of terms used herein is intended to best explain the principles of the embodiments, practical applications, or improvements to technologies in the market, or to enable other ordinary skill in the art in the technical field to understand the embodiments disclosed herein.

Claims

1. A robot positioning method, characterized in that, The method includes: Obtaining component data from a target component in the robot, where the target component includes an inertial measurement unit and / or joint motors; Predicting a velocity sequence diagram of the robot based on the component data, where the velocity sequence diagram represents the velocity of the robot at each time stamp; Determining the motion trajectory of the robot according to the velocity sequence diagram.

2. The method according to claim 1, characterized in that The predicting the velocity sequence diagram of the robot based on the component data includes: Inputting the component data into a prediction model for calculation to obtain the displacement of the robot; Determining the velocity of the robot at each time stamp according to the displacement of the robot and the sampling frequency of the target component, where the time stamp is determined according to the sampling frequency; Forming the velocity sequence diagram of the robot according to the velocity of the robot at each time stamp.

3. The method according to claim 2, characterized in that, The component data includes the first acceleration and the first angular velocity of the robot's motion collected by the inertial measurement unit, and the prediction model includes a first prediction model. Wherein, the inputting the component data into the prediction model for calculation to obtain the displacement of the robot includes: Converting the first acceleration and the first angular velocity in the inertial measurement unit coordinate system to the world coordinate system by using an attitude quaternion to obtain a second acceleration and a second angular velocity, where the attitude quaternion represents the rotation angle of the inertial measurement unit at the initial moment and the current moment; Inputting the second acceleration and the second angular velocity into the first prediction model for calculation to obtain the displacement of the robot.

4. The method according to claim 3, characterized in that, The inputting the second acceleration and the second angular velocity into the first prediction model for calculation to obtain the displacement of the robot includes: Performing bias elimination on the second acceleration and the second angular velocity according to the initial acceleration and the initial angular velocity collected by the inertial measurement unit at the initial moment to obtain the preprocessed second acceleration and second angular velocity; Inputting the preprocessed second acceleration and second angular velocity into the first prediction model to obtain the displacement of the robot.

5. The method according to claim 2, characterized in that, The component data includes the first torque and the first angle returned by the joint motor. The prediction model further includes a second prediction model. The joint motor represents a motor device that drives the motion structure of the robot. The joint motor is placed at the joint of the motion structure. The first torque represents the output torque of the joint motor, and the first angle represents the rotation angle of the joint motor. Wherein, the inputting the component data into the prediction model for calculation to obtain the displacement of the robot includes: Inputting the first torque and the first angle into the second prediction model for calculation according to the model structure of the robot to obtain the displacement of the robot.

6. The method according to claim 2, characterized in that The component data includes the first acceleration and the first angular velocity collected by the inertial measurement unit, the first torque and the first angle returned by the joint motor. The prediction model includes a first prediction model and a second prediction model. Wherein, the inputting the component data into the prediction model for calculation to obtain the displacement of the robot includes: Inputting the first acceleration and the first angular velocity into the first prediction model for calculation to obtain a first displacement; Input the first torque and the first angle into a second prediction model for calculation according to the model structure of the robot to obtain a second displacement amount; Determine the first displacement amount and the first uncertainty of the first prediction model as the mean and variance of a Gaussian distribution respectively to obtain a first function conforming to the Gaussian distribution, and determine the second displacement amount and the second uncertainty of the second prediction model as the mean and variance of a Gaussian distribution respectively to obtain a second function conforming to the Gaussian distribution; Fuse the first function and the second function to obtain a third function conforming to the Gaussian distribution, and determine the displacement amount of the robot according to the third function.

7. The method according to claim 1, wherein The determining the motion trajectory of the robot according to the velocity sequence diagram includes: Determine the motion trajectory of the robot according to the positioning frequency of the robot, the initial position and the velocity sequence diagram.

8. The method according to claim 2, characterized in that, The determining the velocity of the robot at each time stamp according to the displacement amount of the robot and the sampling frequency of the target component includes: Determine a corresponding plurality of sampling times based on the sampling frequency of the robot, and determine a plurality of speeds to be processed of the robot within each of the sampling times according to the displacement amount of the robot; Use the mean of the plurality of speeds to be processed at each sampling time as the speed corresponding to the time stamp of the sampling time.

9. The method according to claim 3 or 6, characterized in that The method further includes a first training step for the first prediction model, and the first training step includes: Obtain a first training set, where the first training set includes: the third acceleration and the third angular velocity of the robot motion collected by an inertial measurement unit, and the corresponding actual displacement amount of the robot; Input the third acceleration and the third angular velocity into a first prediction model to be trained for calculation to obtain a first predicted displacement amount; calculate a first loss of the first prediction model to be trained according to the first predicted displacement amount and the corresponding actual displacement amount by using a mean square error function, and update the parameters of the first prediction model to be trained according to the first loss until a converged first prediction model is obtained; Input the third acceleration and the third angular velocity into the converged first prediction model for calculation to obtain a second predicted displacement amount; calculate a second loss of the converged first prediction model according to the second predicted displacement amount and the corresponding actual displacement amount by using a maximum Gaussian likelihood function, and update the parameters of the converged first prediction model according to the second loss until the converged first prediction model converges again, and complete the training of the first prediction model to obtain the first prediction model.

10. The method according to claim 5 or 6, characterized in that, The method further includes a second training step for the second prediction model, and the second training step includes: Obtain a second training set, where the second training set includes: the second torque and the second angle returned by a joint motor, and the corresponding actual displacement amount of the robot; Input the second moment and the second angle into the second prediction model to be trained for calculation to obtain a third predicted displacement; calculate a third loss of the second prediction model to be trained by using a mean square error function based on the third predicted displacement and the corresponding actual displacement, and update the parameters of the second prediction model to be trained according to the third loss until a converged second prediction model is obtained; Input the second moment and the second angle into the converged second prediction model for calculation to obtain a fourth predicted displacement, calculate a fourth loss of the converged second prediction model by using a maximum Gaussian likelihood function based on the fourth predicted displacement and the corresponding actual displacement, and update the parameters of the converged second prediction model according to the fourth loss until the converged second prediction model converges again, and complete the training of the second prediction model to obtain the second prediction model.

11. A robot positioning device, characterized in that, The device includes: An acquisition module, configured to acquire component data from a target component in a robot, where the target component includes an inertial measurement unit and / or a joint motor; A prediction module, configured to predict a velocity sequence diagram of the robot based on the component data, where the velocity sequence diagram represents the velocities of the robot at each time stamp; A trajectory determination module, configured to determine a motion trajectory of the robot according to the velocity sequence diagram.

12. The device according to claim 11, characterized in that, The prediction module is further configured to: Input the component data into a prediction model for calculation to obtain a displacement of the robot; Determine the velocities of the robot at each time stamp according to the displacement of the robot and the sampling frequency of the target component, where the time stamp is determined according to the sampling frequency; Form a velocity sequence diagram of the robot according to the velocities of the robot at each time stamp.

13. The device according to claim 12, wherein, The component data includes a first acceleration and a first angular velocity of the robot's motion collected by the inertial measurement unit, and the prediction model includes a first prediction model, wherein, the inputting the component data into the prediction model for calculation to obtain the displacement of the robot includes: Converting the first acceleration and the first angular velocity in the inertial measurement unit coordinate system to the world coordinate system by using an attitude quaternion to obtain a second acceleration and a second angular velocity, where the attitude quaternion represents the rotation angle of the inertial measurement unit at the initial moment and the current moment; Inputting the second acceleration and the second angular velocity into the first prediction model for calculation to obtain the displacement of the robot.

14. The device according to claim 13, characterized in that, The inputting the second acceleration and the second angular velocity into the first prediction model for calculation to obtain the displacement of the robot includes: Performing bias elimination on the second acceleration and the second angular velocity according to an initial acceleration and an initial angular velocity collected by the inertial measurement unit at the initial moment to obtain a preprocessed second acceleration and a second angular velocity; Inputting the preprocessed second acceleration and second angular velocity into the first prediction model to obtain the displacement of the robot.

15. The device according to claim 12, characterized in that, The component data includes the first torque and the first angle returned by the joint motor. The prediction model further includes a second prediction model. The joint motor represents the motor device that drives the motion structure of the robot. The joint motor is placed at the joint of the motion structure. The first torque represents the output torque of the joint motor, and the first angle represents the angle by which the joint motor rotates. Among them, the step of inputting the component data into the prediction model for calculation to obtain the displacement of the robot includes: Inputting the first torque and the first angle into the second prediction model for calculation according to the model structure of the robot to obtain the displacement of the robot.

16. The device according to claim 12, characterized in that, The component data includes the first acceleration and the first angular velocity collected by the inertial measurement unit, the first torque and the first angle returned by the joint motor. The prediction model includes a first prediction model and a second prediction model. Among them, the step of inputting the component data into the prediction model for calculation to obtain the displacement of the robot includes: Inputting the first acceleration and the first angular velocity into the first prediction model for calculation to obtain a first displacement; Inputting the first torque and the first angle into the second prediction model for calculation according to the model structure of the robot to obtain a second displacement; Determining the first displacement and the first uncertainty of the first prediction model as the mean and variance of a Gaussian distribution respectively to obtain a first function conforming to the Gaussian distribution, and determining the second displacement and the second uncertainty of the second prediction model as the mean and variance of a Gaussian distribution respectively to obtain a second function conforming to the Gaussian distribution; Fusing the first function and the second function to obtain a third function conforming to the Gaussian distribution, and determining the displacement of the robot according to the third function.

17. The device according to claim 11, characterized in that, The trajectory determination module is further configured to: Determine the motion trajectory of the robot according to the positioning frequency of the robot, the initial position, and the velocity sequence diagram.

18. The device according to claim 12, characterized in that, The step of determining the velocity of the robot at each time stamp according to the displacement of the robot and the sampling frequency of the target component includes: Determining a corresponding plurality of sampling times based on the sampling frequency of the robot, and determining a plurality of to-be-processed velocities of the robot within each sampling time according to the displacement of the robot; Taking the mean value of the plurality of to-be-processed velocities at each sampling time as the velocity at the time stamp corresponding to the sampling time.

19. The device according to claim 13 or 16, characterized in that, The device further includes a first training module for the first prediction model. The first training module is configured to: Obtain a first training set, where the first training set includes: the third acceleration and the third angular velocity of the robot's motion collected by the inertial measurement unit, and the corresponding actual displacement of the robot; Inputting the third acceleration and the third angular velocity into the first prediction model to be trained for calculation to obtain a first predicted displacement; calculating a first loss of the first prediction model to be trained according to the first predicted displacement and the corresponding actual displacement by using a mean square error function, and updating the parameters of the first prediction model to be trained according to the first loss until a converged first prediction model is obtained. Input the third acceleration and the third angular velocity into the converged first prediction model for calculation to obtain a second predicted displacement; calculate a second loss of the converged first prediction model by using the maximum Gaussian likelihood function according to the second predicted displacement and the corresponding actual displacement, and update the parameters of the converged first prediction model according to the second loss until the converged first prediction model converges again, and complete the training of the first prediction model to obtain the first prediction model.

20. The device according to claim 15 or 16, characterized in that The device further includes a second training module for the second prediction model, and the second training module is configured to: Obtain a second training set, where the second training set includes: a second torque and a second angle returned by a joint motor, and the corresponding actual displacement of the robot; Input the second torque and the second angle into the second prediction model to be trained for calculation to obtain a third predicted displacement; calculate a third loss of the second prediction model to be trained by using the mean square error function according to the third predicted displacement and the corresponding actual displacement, and update the parameters of the second prediction model to be trained according to the third loss until a converged second prediction model is obtained; Input the second torque and the second angle into the converged second prediction model for calculation to obtain a fourth predicted displacement, calculate a fourth loss of the converged second prediction model by using the maximum Gaussian likelihood function according to the fourth predicted displacement and the corresponding actual displacement, and update the parameters of the converged second prediction model according to the fourth loss until the converged second prediction model converges again, and complete the training of the second prediction model to obtain the second prediction model.

21. An electronic device, characterized in that, Comprising: A processor; A memory for storing instructions executable by the processor; Wherein, the processor is configured to call the instructions stored in the memory to execute the method according to any one of claims 1 to 10.

22. A computer-readable storage medium having computer program instructions stored thereon, characterized in that, When the computer program instructions are executed by the processor, the method according to any one of claims 1 to 10 is implemented.

Images