Method and device for determining pose, computer equipment, medium and program product

Abstract

The invention relates to the technical field of multi-sensor data fusion, and discloses a pose determination method and device, computer equipment, a medium and a program product. The method comprises the following steps: acquiring first pose information and second pose information at a first moment; wherein the first pose information is pose information measured by a speedometer, and the second pose information is pose information measured by an inertial measurement unit; determining first target pose information of the target moment according to the pose information of the first moment and the target moment; determining second target pose information at the target moment according to the second pose information; determining a first relative error according to the first target pose information, the first pose information and the first target observation pose information; determining a second relative error according to the second target pose information, the second pose information and the second target observation pose information; and determining a target pose according to the first relative error, the second relative error and a preset confidence coefficient.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-sensor data fusion, and in particular to a method, device, computer equipment, storage medium and program product for determining posture. Background Art

[0002] With the intensification of the global aging society and the increasing needs of people with mobility impairments, smart driving wheelchairs, as a high-tech assistive device, are gradually becoming a key area of ​​assistive devices in the future. Smart driving wheelchairs not only meet the basic mobility needs of people with limited mobility, but also incorporate advanced technologies in the fields of modern precision machinery, intelligent numerical control, engineering mechanics, etc. However, to achieve the widespread application of smart driving wheelchairs, it is necessary to solve the problem of high-precision positioning and navigation in complex environments.

[0003] Currently, the navigation technology of smart driving wheelchairs mainly relies on a single sensor, such as a wheel odometer or an inertial measurement unit (IMU). The specific approach is as follows:

[0004] Wheel odometry estimates the robot's moving distance and direction by measuring the rotation of the wheels. However, after long-term operation, the accumulated error of the wheel odometry will gradually increase due to wheel wear and mechanical errors.

[0005] IMU estimates the robot's posture and motion by measuring acceleration and angular velocity. However, IMU data is susceptible to noise and drift, resulting in a gradual increase in positioning error after long-term operation.

[0006] Therefore, how to accurately locate the position of the intelligent driving wheelchair becomes a problem that needs to be solved. Summary of the invention

[0007] In view of this, the present invention provides a method, apparatus, computer device, storage medium and program product for determining posture.

[0008] In a first aspect, the present invention provides a method for determining posture, the method comprising: obtaining first posture information and second posture information at a first moment; wherein the first posture information is the posture information measured by an odometer, and the second posture information is the posture information measured by an inertial measurement unit; determining the first target posture information at the target moment according to the posture information at the first moment and the target moment; and determining the second target posture information at the target moment according to the second posture information; determining a first relative error according to the first target posture information, the first posture information and the first target observed posture information; determining a second relative error according to the second target posture information, the second posture information and the second target observed posture information; determining the target posture according to the first relative error, the second relative error and a preset confidence level.

[0009] The method for determining the posture provided in this embodiment can compensate for the problem of gradually increasing cumulative errors due to wheel wear and mechanical errors by introducing the posture information of the IMU and the wheel odometer, thereby providing accurate information about the robot's posture and motion. That is, by fusing the data of the odometer and the IMU, continuous and high-precision estimation of the robot's posture can be achieved, effectively reducing the cumulative error after long-term operation.

[0010] At the same time, although there are cumulative errors in the odometer data, the estimation of the robot's moving distance in a short period of time is relatively accurate. Therefore, by fusing the data of the two, the short-term accuracy of the odometer can be used to correct the long-term drift problem of the IMU, thereby improving the overall positioning accuracy.

[0011] In addition, by comprehensively utilizing the importance of multi-sensor data, the sensor abnormality can be detected by comparing the relative errors of the first target pose information and the second target pose information with their respective target observation pose information, which not only improves the positioning accuracy, but also enhances the robustness and reliability of the system.

[0012] In one possible implementation, the posture information at the first moment includes: left wheel speed, right wheel speed, distance between the left wheel and the right wheel, a first rotation angle and a first displacement; wherein, determining the first target posture information at the target moment based on the posture information at the first moment and the target moment includes: determining the angular velocity and the linear velocity based on the left wheel speed, the right wheel speed, and the distance between the left wheel and the right wheel; determining the incremental moment based on the difference between the target moment and the first moment; determining the first target posture information at the target moment based on the incremental moment, angular velocity, linear velocity, rotation angle and displacement.

[0013] The method for determining the posture provided in this embodiment can more accurately calculate the posture of the robot at different times by integrating information on the left wheel speed, the right wheel speed, the distance between the left wheel and the right wheel, the first rotation angle and the first displacement, thereby reducing the positioning error caused by the inaccuracy of a single parameter.

[0014] Moreover, the robot’s position information can be updated in real time based on the difference between the target moment and the first moment (i.e., the incremental moment). This enables the system to dynamically adapt to the robot’s motion state and reflect the robot’s position changes in a timely manner.

[0015] In addition, by calculating the angular velocity and linear velocity, the robot can handle various motion modes such as linear motion, curved motion, and rotation on the spot, thereby coping with more motion scenarios.

[0016] In one possible implementation, the target moment includes multiple angular velocity change moments, and the second posture information includes angular velocity, a second rotation angle, and a second displacement; wherein, the second target posture information at the target moment is determined based on the second posture information, including: determining the target angular velocity based on the multiple angular velocity change moments and the angular velocity corresponding to each angular velocity change moment; determining the rotation matrix based on the target angular velocity, and determining the rotation increment matrix based on the rotation matrix; determining the second target posture information at the target moment based on the rotation increment matrix, the second rotation angle, and the second displacement.

[0017] The method for determining the posture provided in this embodiment can accurately record and analyze multiple angular velocity change moments and their corresponding angular velocities, thereby accurately capturing the dynamic changes of the robot during movement. In addition, the rotation increment matrix is ​​further derived through the rotation matrix. This step can accurately calculate the rotation increment of the robot during the angular velocity change process. By combining multiple parameters such as rotation increment, angular velocity change, second rotation angle and second displacement, effective fusion of multi-source information is achieved.

[0018] In one possible implementation, a first relative error is determined based on first target pose information, first position information and first target observation pose information, including: obtaining first observation noise and the first observation pose information at a previous moment of the first moment; determining second observation pose information of the first target moment based on the first target pose information, the first observation pose information and the first observation noise; determining the first relative error based on the first observation pose information, the second observation pose information, the first target pose information and the first position information.

[0019] In the method for determining the posture provided in this embodiment, observation noise is unavoidable in practical applications, and it may come from various factors such as sensor accuracy and environmental interference. By introducing the first observation noise, the method can more truly reflect the uncertainty in the actual observation process, thereby improving the accuracy of error assessment. Using the first observation posture information at the previous moment of the first moment, a more complete observation sequence can be constructed, thereby more accurately assessing the difference between the observation posture at the current moment and the target posture, thereby obtaining a more reliable first relative error.

[0020] In one possible implementation, the second relative error is determined based on the second target posture information, the second posture information and the second target observation posture information, including: obtaining the second observation noise and the third observation posture information at the previous moment of the first moment; determining the fourth observation posture information at the first moment based on the second observation noise; integrating the angular velocity to obtain the posture increment; determining the second relative error based on the second target posture information, the posture increment, the fourth observation posture information and the third observation posture information.

[0021] The method for determining the posture provided by this embodiment not only considers the difference between the second target posture information and the second posture information, but also introduces the second observation noise and the third observation posture information, so that the error can be evaluated more comprehensively. By comprehensively considering these factors, the uncertainty in the actual motion process can be more accurately reflected, and the accuracy of error evaluation can be improved.

[0022] In one possible implementation, the preset confidence level includes a first confidence level and a second confidence level; wherein, determining the target posture based on the first relative error, the second relative error and the preset confidence level includes: determining a weighted sum of squares based on the first relative error and the first confidence level, the second relative error and the second confidence level; optimizing the first relative error and the second relative error to minimize the weighted sum of squares to obtain the target posture.

[0023] The method for determining the posture provided in this embodiment performs weighted processing on the two errors through the first confidence level and the second confidence level. This makes the error evaluation more comprehensive and can more accurately reflect the uncertainty in the actual motion process. At the same time, by minimizing the weighted sum of squares, the influence of a single error on the overall posture estimation is eliminated or reduced to determine the target posture with the minimum total error, thereby improving the accuracy of the target posture determination.

[0024] In a second aspect, the present invention provides a device for determining posture, which includes: an acquisition module, used to acquire first posture information and second posture information at a first moment; wherein the first posture information is the posture information measured by the odometer, and the second posture information is the posture information measured by the inertial measurement unit; a first determination module, used to determine the first target posture information at the target moment according to the posture information at the first moment and the target moment; and to determine the second target posture information at the target moment according to the second posture information; a second determination module, used to determine a first relative error according to the first target posture information, the first posture information and the first target observed posture information; a third determination module, used to determine a second relative error according to the second target posture information, the second posture information and the second target observed posture information; a fourth determination module, used to determine the target posture according to the first relative error, the second relative error and a preset confidence level.

[0025] In a third aspect, the present invention provides a computer device, comprising: a memory and a processor, the memory and the processor being communicatively connected to each other, the memory storing computer instructions, and the processor executing the method for determining the position posture of the first aspect or any corresponding embodiment thereof by executing the computer instructions.

[0026] In a fourth aspect, the present invention provides a computer-readable storage medium having computer instructions stored thereon, the computer instructions being used to cause a computer to execute the method for determining a position posture according to the first aspect or any corresponding embodiment thereof.

[0027] In a fifth aspect, the present invention provides a computer program product, including computer instructions, which are used to enable a computer to execute the method for determining a position posture according to the first aspect or any corresponding embodiment thereof. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] In order to more clearly illustrate the specific implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0029] Figure 1 is a schematic flow chart of a method for determining a posture according to an embodiment of the present invention;

[0030] Figure 2 is a structural block diagram of a device for determining a posture according to an embodiment of the present invention;

[0031] Figure 3 It is a schematic diagram of the hardware structure of a computer device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0032] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0033] Based on relevant technologies, with the intensification of the global aging society and the increasing needs of people with mobility impairments, smart driving wheelchairs, as a high-tech assistive device, are gradually becoming a key area of ​​assistive devices in the future. Smart driving wheelchairs not only meet the basic mobility needs of people with limited mobility, but also incorporate advanced technologies in the fields of modern precision machinery, intelligent numerical control, engineering mechanics, etc. However, to achieve the widespread application of smart driving wheelchairs, it is necessary to solve the problem of high-precision positioning and navigation in complex environments.

[0034] Currently, the navigation technology of smart driving wheelchairs mainly relies on a single sensor, such as a wheel odometer or an inertial measurement unit (IMU). The specific approach is as follows:

[0035] Wheel odometry estimates the robot's moving distance and direction by measuring the rotation of the wheels. However, after long-term operation, the accumulated error of the wheel odometry will gradually increase due to wheel wear and mechanical errors.

[0036] IMU estimates the robot's posture and motion by measuring acceleration and angular velocity. However, IMU data is susceptible to noise and drift, resulting in a gradual increase in positioning error after long-term operation.

[0037] Based on this, the present invention provides a method for determining the posture, which can compensate for the problem of gradually increasing cumulative errors due to wheel wear and mechanical errors by introducing the posture information of the IMU and the wheel odometer, thereby providing accurate information about the robot's posture and motion. That is, by fusing the data of the odometer and IMU, continuous and high-precision estimation of the robot's posture can be achieved, effectively reducing the cumulative error after long-term operation.

[0038] At the same time, although there are cumulative errors in the odometer data, the estimation of the robot's moving distance in a short period of time is relatively accurate. Therefore, by fusing the data of the two, the short-term accuracy of the odometer can be used to correct the long-term drift problem of the IMU, thereby improving the overall positioning accuracy.

[0039] In addition, by comprehensively utilizing the importance of multi-sensor data, the sensor abnormality can be detected by comparing the relative errors of the first target pose information and the second target pose information with their respective target observation pose information, which not only improves the positioning accuracy, but also enhances the robustness and reliability of the system.

[0040] According to an embodiment of the present invention, an embodiment of a method for determining posture is provided. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0041] In this embodiment, a method for determining a posture is provided, which can be used in computer devices, such as computers, servers, etc. Figure 1 is a flow chart of a method for determining a posture according to an embodiment of the present invention. Figure 1 As shown, the process includes the following steps:

[0042] Step S101, obtaining first posture information and second posture information at a first moment; wherein the first posture information is posture information measured by an odometer, and the second posture information is posture information measured by an inertial measurement unit.

[0043] The odometer can be a sensor used to measure the distance and direction of a device. The inertial measurement unit (IMU) can be a device that integrates sensors such as accelerometers and gyroscopes to measure the inertial data of the device. The measuring device can be a wheelchair or other device, which is not specifically limited here.

[0044] The first position information can represent the position and position of the device relative to the starting position at the first moment. The first moment can be any time point. The first moment can be 1s or 2s, etc., which is not specifically limited here. Specifically, the first position information is obtained by measuring the device by the odometer.

[0045] The second position and posture information may represent the position and posture of the device relative to the starting position at the first moment, etc. Specifically, the second position and posture information may be obtained by measuring the device with an inertial measurement unit.

[0046] As an example, at the first moment, the position measured by the odometer is (10, 20), and the attitude is (0, 0, 0) (indicating no rotation); the acceleration and angular velocity data measured by the IMU are integrated to obtain a position of (10.1, 19.9) and an attitude of (0.01, 0.02, -0.01).

[0047] As an example, when the first pose information is determined by the odometer, in SE(3), the robot's pose in space can be a 4x4 homogeneous transformation matrix (T(t)), which includes rotation and displacement. It can be specifically expressed by the following formula:

[0048] Among them, R(t)∈SO(3) is a 3x3 rotation matrix used to represent the posture (direction) of the robot in three-dimensional space, and p(t)∈R3 is used to represent the position of the robot at time t.

[0049] As an example, when the second posture information is determined by the IMU, the angle of the IMU may be: ξ=[υ x , y , z ,ω x ,ω y ,ω z ] T ; Among them, ω x (t) is the speed at which the robot rotates around the x-axis, ω y (t) is the speed at which the robot rotates around the y-axis, ω z(t) is the rate at which the robot rotates around the z-axis. In this embodiment, only the rate at which the robot rotates around the z-axis may be processed. Therefore, the change in the rotation angle may be obtained by time integration, specifically by using the following formula:

[0050] Where i is a sub-moment in the first moment, that is, the first moment can be divided into multiple sub-moments. N is the total number of sub-moments obtained by dividing the first moment, Δt i is the difference between adjacent sub-times.

[0051] According to the IMU angular velocity, the update of the rotation matrix can be expressed by SE(3) Lie algebra. The rotation part in Lie algebra is expressed as:

[0052] Among them, ω^ is the rotation matrix, ω z is the rotation rate around the z-axis.

[0053] For small step sizes, the rotation increment matrix is ​​approximately ΔR≈exp(ω^Δt)=I+ω^Δt; where I is the unit matrix. Δt is the difference between adjacent moments. By rotating the increment matrix, the IMU data can update the attitude.

[0054] Step S102, determining first target posture information at the target moment according to the posture information at the first moment and the target moment; and determining second target posture information at the target moment according to the second posture information.

[0055] The first target pose information can represent the position and orientation (i.e., pose) of the target at the time of the odometer measurement. In pose, the position refers to the coordinates (x, y, z) in three-dimensional space, while the orientation is represented by the rotation matrix or Euler angles (such as pitch, yaw, and roll). Similarly, the second target pose information can be the position and orientation of the target at the time of the IMU measurement.

[0056] The target moment may represent a future moment at which pose estimation is required. Based on the pose information at the first moment and the target moment, a prediction or interpolation algorithm is used to determine the first target pose information at the target moment, and similarly, based on the second pose information, the second target pose information at the target moment is determined. No specific limitation is made here, and it can be implemented by those skilled in the art.

[0057] As an example, Kalman filtering, particle filtering and other algorithms can be used for prediction, and linear interpolation or nonlinear interpolation of posture information can also be used for prediction. The time interval between the target moment and the first moment is 1 second. Using the Kalman filtering algorithm, the first target posture information at the target moment is predicted to be (11, 21) based on the posture information at the first moment, and the posture remains unchanged; similarly, the second target posture information at the target moment is predicted to be (10.2, 20.0) based on the posture information of the IMU, and the posture changes slightly.

[0058] Step S103, determining a first relative error according to the first target pose information, the first pose information and the first target observation pose information.

[0059] The first target observation pose information can represent the pose difference between adjacent moments, etc., which is not specifically limited here. Specifically, the position difference between the first target pose information and the first pose information can be obtained by simple vector subtraction, while the direction difference may require more complex calculations, such as calculating the difference between two rotation matrices or using Euler angles to represent and calculate the difference. Then, the error between the first target pose information and the first pose information can be combined with the first target observation pose information to determine the first relative error.

[0060] Step S104, determining a second relative error according to the second target posture information, the second posture information and the second target observation posture information.

[0061] The second target observation posture information can represent the posture difference between adjacent moments, etc., which is not specifically limited here. Specifically, the position difference of the second target posture information and the second posture information can be obtained by simple vector subtraction, while the direction difference may require more complex calculations, such as calculating the difference between two rotation matrices or using Euler angles to represent and calculate the difference. Then, the error between the second target posture information and the second posture information can be combined with the second target observation posture information to determine the first relative error.

[0062] Step S105, determining the target posture according to the first relative error, the second relative error and a preset confidence level.

[0063] The preset confidence level can characterize the confidence levels of the first relative error and the second relative error. The confidence level of the first relative error and the confidence level of the second relative error can be the same or different. The confidence level can be a manually set value or can be determined by other methods, which is not specifically limited here.

[0064] As an example, the preset confidence level may be a covariance matrix.

[0065] The method for determining the posture provided in this embodiment can compensate for the problem of gradually increasing cumulative errors due to wheel wear and mechanical errors by introducing the posture information of the IMU and the wheel odometer, thereby providing accurate information about the robot's posture and motion. That is, by fusing the data of the odometer and the IMU, continuous and high-precision estimation of the robot's posture can be achieved, effectively reducing the cumulative error after long-term operation.

[0066] At the same time, although there are cumulative errors in the odometer data, the estimation of the robot's moving distance in a short period of time is relatively accurate. Therefore, by fusing the data of the two, the short-term accuracy of the odometer can be used to correct the long-term drift problem of the IMU, thereby improving the overall positioning accuracy.

[0067] In addition, by comprehensively utilizing the importance of multi-sensor data, the sensor abnormality can be detected by comparing the relative errors of the first target pose information and the second target pose information with their respective target observation pose information, which not only improves the positioning accuracy, but also enhances the robustness and reliability of the system.

[0068] In a possible implementation, the posture information at the first moment includes: a left wheel speed, a right wheel speed, a distance between the left wheel and the right wheel, a first rotation angle, and a first displacement; wherein, in the above step S102, determining the first target posture information at the target moment according to the posture information at the first moment and the target moment includes:

[0069] Step a1, determining the angular velocity and the linear velocity according to the left wheel speed, the right wheel speed, and the distance between the left wheel and the right wheel.

[0070] The left wheel speed can represent the speed of the left wheel of the device during movement. The right wheel speed can represent the speed of the right wheel of the device during movement. The distance between the left wheel and the right wheel can represent the straight-line distance in the symmetric direction of the left wheel and the right wheel.

[0071] Specifically, by measuring the real-time speed of the left and right wheels (usually expressed in rotational speed, in revolutions per second or circles per second), and the distance between the two wheels (i.e., the wheel spacing), the differential drive principle can be used to calculate the angular velocity and linear velocity of the robot. Other methods can also be used to determine the angular velocity and linear velocity, etc., which are not specifically limited here and can be implemented by technical personnel in this field.

[0072] In a possible implementation, the angular velocity and the linear velocity are determined according to the left wheel speed, the right wheel speed, and the distance between the left wheel and the right wheel, and can be determined using the following formula:

[0073] Among them, L is the left wheel speed, υ Ris the right wheel speed, L is the distance between the left and right wheels, V is the linear velocity, and W is the angular velocity.

[0074] Step a2, determining the incremental time according to the difference between the target time and the first time.

[0075] The incremental time may represent the time required to pass from the first time to the target time. For example, if the first time is 1 second and the target time is 5 seconds, the corresponding incremental time may be 4 seconds.

[0076] Step a3, determining the first target posture information at the target time according to the incremental time, angular velocity, linear velocity, rotation angle and displacement.

[0077] The incremental time, angular velocity and linear velocity are used to calculate the rotation angle and displacement of the robot during the time period.

[0078] As an example, you can use linear velocity and incremental time to calculate the distance the robot moves in a straight line. If the robot moves on a two-dimensional plane, you may need to calculate the displacement in the x and y directions respectively. If the robot moves on a three-dimensional plane, you may need to calculate the displacement in the x, y, and z directions respectively. You can use angular velocity and incremental time to calculate the angle of rotation of the robot around a point or axis within the incremental time.

[0079] Update the robot's position based on the initial position (the position at the first moment) and the calculated displacement. If the robot moves in a two-dimensional plane, the new position can be obtained by adding the displacement in the x and y directions to the initial position. If the robot moves in three-dimensional space, the displacement in the z direction also needs to be considered. Update the robot's orientation based on the initial orientation (the orientation at the first moment) and the calculated rotation angle. Combine the updated position and orientation information to form the first target pose information at the target moment.

[0080] In a possible implementation, the first target posture information at the target time is determined according to the incremental time, angular velocity, linear velocity, rotation angle and displacement, and the following formula can be used:

[0081] Among them, Δt is the incremental time (that is, the difference between adjacent moments), t(t) is the displacement, R(t) is the posture in three-dimensional space, V is the linear velocity, and W is the angular velocity.

[0082] In a possible implementation, the formula for determining the first target pose information at the target moment can be derived through the following process:

[0083] In SE(3), the change of posture can be represented by an incremental transformation matrix ΔT∈SE(3), which describes the change of the robot's posture at time Δt. The posture increment matrix can be written as:

[0084] ΔT=exp(ξ^Δt); where, ξ=[v x , v y , v z ,ω x ,ω y ,ω z ] T is a 6D twist vector, which is used to characterize the linear velocity and angular velocity of the robot in x, y, and z in three-dimensional space. ξ^ is its antisymmetric matrix, which is used to generate the position increment.

[0085] In summary, the expressions of ξ and ξ^ can be:

[0086]

[0087] The pose increment matrix can be calculated by exponential mapping ΔT = exp(ξ^Δt). For small step sizes, it can be approximated by the following formula:

[0088] ΔT≈I+ξ^Δt; where I is the unit matrix.

[0089] Therefore, we can get:

[0090] And, for a wheeled robot, since the motion is in a two-dimensional plane, v y =0, the formula can be simplified to:

[0091]

[0092] Therefore, the relationship between the posture information of the robot at time t and the posture information at time t+Δt can be: T(t+Δt)=T(t)ΔT.

[0093] In summary, we can calculate:

[0094] The method for determining the posture provided in this embodiment can more accurately calculate the posture of the robot at different times by integrating information on the left wheel speed, the right wheel speed, the distance between the left wheel and the right wheel, the first rotation angle and the first displacement, thereby reducing the positioning error caused by the inaccuracy of a single parameter.

[0095] Moreover, the robot’s position information can be updated in real time based on the difference between the target moment and the first moment (i.e., the incremental moment). This enables the system to dynamically adapt to the robot’s motion state and reflect the robot’s position changes in a timely manner.

[0096] In addition, by calculating the angular velocity and linear velocity, the robot can handle various motion modes such as linear motion, curved motion, and rotation on the spot, thereby coping with more motion scenarios.

[0097] In a possible implementation, the target moment includes multiple angular velocity change moments, and the second posture information includes angular velocity, second rotation angle, and second displacement; wherein, in the above step S102, determining the second target posture information at the target moment according to the second posture information includes:

[0098] Step b1, determining a target angular velocity according to a plurality of angular velocity change moments and an angular velocity corresponding to each angular velocity change moment.

[0099] The angular velocity change moment can represent the moment of angular velocity change corresponding to each sub-moment in the process of the above-mentioned incremental moment. From a series of time points of angular velocity change and their corresponding angular velocity values, the angular velocity at the target moment is inferred or calculated. This may involve data smoothing, interpolation, or prediction based on a physical model, etc., which is not specifically limited here.

[0100] In a possible implementation, the target angular velocity may be determined by the following formula.

[0101] Where Δθ is the target angular velocity, t′ is any time between t0 and t, i is the number of times between t0 and t (a positive integer), and t i For each positive integer corresponding to the moment, Δt i is the difference between two adjacent moments.

[0102] Step b2, determining a rotation matrix according to the target angular velocity, and determining a rotation increment matrix according to the rotation matrix.

[0103] The target angular velocity can be used to generate a rotation matrix that represents the rotation from a certain initial orientation to the target orientation. Then, based on this rotation matrix, a rotation delta matrix is ​​calculated that represents the relative rotation from the current orientation to the target orientation.

[0104] As an example, the rotation matrix may be represented and calculated using Euler angles, Rodriguez parameters (rotation vectors), or quaternions.

[0105] As an example, the incremental matrix is ​​obtained by multiplying the inverse of the current rotation matrix by the target rotation matrix.

[0106] In a possible implementation, the following formula can be used to determine:

[0107] Among them, T(t) is the pose at the first moment measured by the IMU; R′(t) is the rotation matrix of the IMU, and t′(t) is the translation vector of the IMU.

[0108] Update the attitude using the IMU data, which can be updated by rotating the delta matrix.

[0109] Wherein, T′(t+Δt) is the second target pose information of the IMU at the target time. ΔR(t) is the rotation increment matrix at time t.

[0110] Step b3, determining the second target posture information at the target moment according to the rotation increment matrix, the second rotation angle and the second displacement.

[0111] The rotation increment matrix is ​​combined with a second rotation angle (possibly obtained from another source or a previous step) and a second displacement (representing a linear movement in some direction) to compute the complete pose information at the target time.

[0112] As an example, the rotation increment matrix can be applied to the current direction and combined with the second displacement to obtain the target pose. For example: the second rotation angle is θ (around the x-axis) and the second displacement is (dx, dy, dz). The target pose can be calculated as follows: Apply the rotation increment matrix ΔR to the current direction to obtain a new direction. Combine the new direction with the second rotation angle θ (which may need to be converted to a rotation matrix first) to obtain the final rotation part. Add the second displacement (dx, dy, dz) to the current position to obtain the final displacement part. Combine the rotation and displacement parts to form the second target pose information.

[0113] In a possible implementation, the second target position information may be determined using the following formula:

[0114]

[0115] The method for determining the posture provided in this embodiment can accurately record and analyze multiple angular velocity change moments and their corresponding angular velocities, thereby accurately capturing the dynamic changes of the robot during movement. In addition, the rotation increment matrix is ​​further derived through the rotation matrix. This step can accurately calculate the rotation increment of the robot during the angular velocity change process. By combining multiple parameters such as rotation increment, angular velocity change, second rotation angle and second displacement, effective fusion of multi-source information is achieved.

[0116] In a possible implementation, the above step S103 includes:

[0117] Step c1, obtaining the first observation noise and the first observation posture information at the previous moment of the first moment.

[0118] The first observed pose information at the previous moment of the first moment may be the position and pose information of the object obtained by observation at a certain moment before the first target moment (for example, the previous moment immediately before the first moment). The first observation noise may represent the uncertainty or error introduced in the observation process. It may be caused by sensor accuracy limitations, environmental interference, or imperfections in the data processing algorithm. Specifically, the first observation noise can obtain its accuracy parameters from the data sheet of the sensor, or be estimated through actual testing, etc., which is not specifically limited here. For example: a laser radar sensor is used to observe a mobile robot. At time t-1 (i.e., the previous moment of the first moment), the observed pose information (position x, y, z and attitude angles roll, pitch, yaw) of the robot and the observation noise parameters of the laser radar (such as distance error, angle error, etc.) are obtained.

[0119] Step c2, determining the second observation posture information of the first target at a moment according to the first target posture information, the first observation posture information and the first observation noise.

[0120] The first target pose information (which can be obtained by some prediction or estimation method), the first observation pose information and the first observation noise are used to calculate or estimate the observation pose information at the first target moment. This usually involves data fusion, filtering or prediction algorithms, etc., which are not specifically limited here. For example: the pose information at time t (i.e., the first target moment) can be predicted based on the observation pose information at time t-1 and the motion instructions of the robot. Then, the prediction result can be fused with the actual observation pose information at time t (taking into account the observation noise) to obtain more accurate second observation pose information.

[0121] In a possible implementation, the second observation pose information at the first target moment can be determined using the following formula:

[0122] in, is the second observation pose information of the first target at the moment, T(t) -1 T(t+1) is the difference between the first target pose information and the first pose information. is the observation noise of the odometer.

[0123] Step c3, determining a first relative error based on the first observation posture information, the second observation posture information, the first target posture information, and the first posture information.

[0124] The first relative error may be determined by the difference between the first observed pose information and the second observed pose information, and the difference between the first target pose information and the first pose information. Specifically, the Euclidean distance, angle difference or other suitable metrics between different pose information may be calculated.

[0125] In a possible implementation, the first relative error may be determined using the following formula:

[0126] Among them, T(t1) -1 T(t2) is the difference between the first target pose information and the first pose information. is the difference between the first observation pose information and the second observation pose information, is the first relative error.

[0127] In the method for determining the posture provided in this embodiment, observation noise is unavoidable in practical applications, and it may come from various factors such as sensor accuracy and environmental interference. By introducing the first observation noise, the method can more truly reflect the uncertainty in the actual observation process, thereby improving the accuracy of error assessment. Using the first observation posture information at the previous moment of the first moment, a more complete observation sequence can be constructed, thereby more accurately assessing the difference between the observation posture at the current moment and the target posture, thereby obtaining a more reliable first relative error.

[0128] In a possible implementation, the above step S104 includes:

[0129] Step d1, obtaining the second observation noise and the third observation posture information at the previous moment of the first moment.

[0130] The third observation pose information can represent the position and pose information of objects (such as robots, vehicles, etc.) observed by sensors (such as cameras, lidar, etc.). The second observation noise can represent the noise introduced by factors such as sensor errors and environmental interference during the observation process.

[0131] As an example, the observation noise at the current moment (the second moment) and the observation pose information at the previous moment (the previous moment of the first moment) are obtained from a sensor or other data source.

[0132] Step d2, determining the fourth observation posture information at the first moment based on the second observation noise.

[0133] The target pose information, the current observation pose information and the observation noise are used to estimate or correct the observation pose information at the first moment through some algorithm (such as Kalman filtering, particle filtering, etc.).

[0134] In a possible implementation, the fourth observation posture information at the first moment may be determined using the following formula:

[0135] z IMU (t) = h IMU (X(t))+n IMU (t); where z IMU (t) is the fourth observation posture information, n IMU (t) is the second observation noise, h IMU (X(t)) is the relationship model between the state and the IMU data, which is used to convert the second target posture information into a vector pattern.

[0136] Step d3, integrate the angular velocity to obtain the posture increment.

[0137] The angular velocity measured by the sensor is integrated to calculate the position and attitude change of the object over a period of time. Specifically, a numerical integration method (such as trapezoidal integration, Simpson integration, etc.) can be used to determine the position and attitude increment, which is not specifically limited here and can be implemented by those skilled in the art.

[0138] Step d4, determining the second relative error based on the second target posture information, the posture increment, the fourth observation posture information, and the third observation posture information.

[0139] The target relative error at the first moment is calculated using the pose increment, the corrected observation pose information and the observation pose information at the previous moment.

[0140] In a possible implementation, the second relative error may be determined using the following formula:

[0141] in, is the difference between the fourth observation pose information and the third observation pose information, is the posture increment, is the second relative error.

[0142] The method for determining the posture provided by this embodiment not only considers the difference between the second target posture information and the second posture information, but also introduces the second observation noise and the third observation posture information, so that the error can be evaluated more comprehensively. By comprehensively considering these factors, the uncertainty in the actual motion process can be more accurately reflected, and the accuracy of error evaluation can be improved.

[0143] In a possible implementation, the preset confidence level includes a first confidence level and a second confidence level; wherein the above step S105 includes:

[0144] Step e1, determining a weighted sum of squares according to the first relative error and the first confidence level, the second relative error and the second confidence level.

[0145] The first confidence level may represent the degree of trust or reliability of the first relative error. The second confidence level may represent the degree of trust or reliability of the second relative error. Specifically, the two relative errors may be weighted according to their respective confidence levels, and then their sum of squares may be calculated. This weighted sum of squares will serve as the objective function for subsequent optimization.

[0146] In one possible implementation, the weighted sum of squares may be determined using the following formula:

[0147] in, is the weighted sum of squares, is the first confidence level, is the second confidence level, is the transpose of the first relative error, is the transpose of the second relative error.

[0148] Step e2, optimizing the first relative error and the second relative error to minimize the weighted sum of squares and obtain the target posture.

[0149] You can use optimization algorithms (such as gradient descent, Newton's method, genetic algorithm, etc.) to minimize the weighted sum of squares to find the optimal target pose. For example, use gradient descent to optimize the weighted sum of squares. You can first set an initial target pose guess, then calculate the gradient (i.e., the derivative of the weighted sum of squares with respect to the target pose), and update the target pose in the direction of the gradient. By continuously iterating this process until the weighted sum of squares converges to the minimum value, you can get the optimal target pose.

[0150] In this embodiment, a device for determining posture is also provided, which is used to implement the above-mentioned embodiments and preferred implementation modes, and the descriptions that have been made will not be repeated. As used below, the term "module" can implement a combination of software and / or hardware for a predetermined function. Although the devices described in the following embodiments are preferably implemented in software, the implementation of hardware, or a combination of software and hardware, is also possible and conceivable.

[0151] This embodiment provides a device for determining a position and posture, such as Figure 2As shown, it includes: an acquisition module 201, used to acquire the first posture information and the second posture information at the first moment; wherein the first posture information is the posture information obtained by the odometer measurement, and the second posture information is the posture information obtained by the inertial measurement unit measurement; a first determination module 202, used to determine the first target posture information at the target moment according to the posture information at the first moment and the target moment; and determine the second target posture information at the target moment according to the second posture information; a second determination module 203, used to determine the first relative error according to the first target posture information, the first posture information and the first target observation posture information; a third determination module 204, used to determine the second relative error according to the second target posture information, the second posture information and the second target observation posture information; a fourth determination module 205, used to determine the target posture according to the first relative error, the second relative error and a preset confidence level.

[0152] In one possible implementation, the posture information at the first moment includes: left wheel speed, right wheel speed, the distance between the left wheel and the right wheel, a first rotation angle and a first displacement; wherein the first determination module 202 includes: a first determination unit, used to determine the angular velocity and the linear velocity according to the left wheel speed, the right wheel speed, and the distance between the left wheel and the right wheel; a second determination unit, used to determine the incremental moment according to the difference between the target moment and the first moment; and a third determination unit, used to determine the first target posture information at the target moment according to the incremental moment, the angular velocity, the linear velocity, the rotation angle and the displacement.

[0153] In one possible implementation, the target moment includes multiple angular velocity change moments, and the second posture information includes angular velocity, a second rotation angle, and a second displacement; wherein the first determination module 202 includes: a fourth determination unit, used to determine the target angular velocity according to the multiple angular velocity change moments and the angular velocity corresponding to each angular velocity change moment; a fifth determination unit, used to determine the rotation matrix according to the target angular velocity, and determine the rotation increment matrix according to the rotation matrix; a sixth determination unit, used to determine the second target posture information at the target moment according to the rotation increment matrix, the second rotation angle, and the second displacement.

[0154] In one possible implementation, the second determination module 203 includes: a first acquisition unit, used to acquire the first observation noise and the first observation posture information at the previous moment of the first moment; a seventh determination unit, used to determine the second observation posture information of the first target moment based on the first target posture information, the first observation posture information and the first observation noise; an eighth determination unit, used to determine the first relative error based on the first observation posture information, the second observation posture information, the first target posture information and the first posture information.

[0155] In one possible implementation, the third determination module 204 includes: a second acquisition unit, used to acquire the second observation noise and the third observation posture information at the previous moment before the first moment; a ninth determination unit, used to determine the fourth observation posture information at the first moment based on the second observation noise; an integration unit, used to integrate the angular velocity to obtain a posture increment; and a tenth determination unit, used to determine the second relative error based on the second target posture information, the posture increment, the fourth observation posture information, and the third observation posture information.

[0156] In one possible implementation, the preset confidence level includes a first confidence level and a second confidence level; wherein the fourth determination module 205 includes: an eleventh determination unit, used to determine a weighted sum of squares based on the first relative error and the first confidence level, the second relative error and the second confidence level; an optimization unit, used to optimize the first relative error and the second relative error to minimize the weighted sum of squares and obtain the target posture.

[0157] The further functional description of each of the above modules and units is the same as that of the above corresponding embodiments and will not be repeated here.

[0158] In this embodiment, the device for determining the positioning posture is presented in the form of a functional unit, where the functional unit refers to an ASIC (Application Specific Integrated Circuit) circuit, a processor and memory that executes one or more software or fixed programs, and / or other devices that can provide the above functions.

[0159] The embodiment of the present invention also provides a computer device having the above Figure 2 The device for determining posture is shown.

[0160] See also Figure 3 , Figure 3 is a schematic diagram of the structure of a computer device provided by an optional embodiment of the present invention, such as Figure 3As shown, the computer device includes: one or more processors 10, a memory 20, and interfaces for connecting various components, including high-speed interfaces and low-speed interfaces. Various components are connected to each other using different buses for communication, and can be installed on a common mainboard or installed in other ways as needed. The processor can process the instructions executed in the computer device, including instructions stored in or on the memory to display the graphical information of the GUI on an external input / output device (such as, a display device coupled to the interface). In some optional embodiments, if necessary, multiple processors and / or multiple buses can be used together with multiple memories and multiple memories. Similarly, multiple computer devices can be connected, and each device provides some necessary operations (for example, as a server array, a group of blade servers, or a multi-processor system). Figure 3 A processor 10 is taken as an example.

[0161] The processor 10 may be a central processing unit, a network processor or a combination thereof. The processor 10 may further include a hardware chip. The hardware chip may be a dedicated integrated circuit, a programmable logic device or a combination thereof. The programmable logic device may be a complex programmable logic device, a field programmable gate array, a general purpose array logic or any combination thereof.

[0162] The memory 20 stores instructions executable by at least one processor 10, so that at least one processor 10 executes the method shown in the above embodiment.

[0163] The memory 20 may include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application required for at least one function; the data storage area may store data created according to the use of the computer device, etc. In addition, the memory 20 may include a high-speed random access memory, and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some optional embodiments, the memory 20 may optionally include a memory remotely arranged relative to the processor 10, and these remote memories may be connected to the computer device via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0164] The memory 20 may include a volatile memory, such as a random access memory; the memory may also include a non-volatile memory, such as a flash memory, a hard disk or a solid state drive; the memory 20 may also include a combination of the above types of memory.

[0165] The computer device further comprises a communication interface 30 for the computer device to communicate with other devices or a communication network.

[0166] The embodiment of the present invention also provides a computer-readable storage medium. The method according to the embodiment of the present invention can be implemented in hardware, firmware, or can be implemented as a computer code that can be recorded in a storage medium, or can be implemented as a computer code that is originally stored in a remote storage medium or a non-temporary machine-readable storage medium and will be stored in a local storage medium through a network download, so that the method described herein can be stored in such software processing on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. Among them, the storage medium can be a magnetic disk, an optical disk, a read-only storage memory, a random access memory, a flash memory, a hard disk or a solid-state hard disk, etc.; further, the storage medium can also include a combination of the above types of memories. It can be understood that a computer, a processor, a microprocessor controller, or programmable hardware includes a storage component that can store or receive software or computer code. When the software or computer code is accessed and executed by a computer, a processor, or hardware, the method shown in the above embodiment is implemented.

[0167] A part of the present invention may be applied as a computer program product, such as a computer program instruction, which, when executed by a computer, can call or provide the method and / or technical solution according to the present invention through the operation of the computer. Those skilled in the art should understand that the existence of the computer program instruction in a computer-readable medium includes, but is not limited to, a source file, an executable file, an installation package file, etc., and accordingly, the way in which the computer program instruction is executed by the computer includes, but is not limited to: the computer directly executes the instruction, or the computer compiles the instruction and then executes the corresponding compiled program, or the computer reads and executes the instruction, or the computer reads and installs the instruction and then executes the corresponding installed program. Here, the computer-readable medium may be any available computer-readable storage medium or communication medium accessible to the computer.

[0168] Although the embodiments of the present invention have been described in conjunction with the accompanying drawings, those skilled in the art may make various modifications and variations without departing from the spirit and scope of the present invention, and such modifications and variations are all within the scope defined by the appended claims.

Claims

1. A method for determining a posture, characterized in that: The method comprises: Obtaining first posture information and second posture information at a first moment; wherein the first posture information is posture information measured by an odometer, and the second posture information is posture information measured by an inertial measurement unit; Determine first target position information at the target moment according to the position information at the first moment and the target moment; and determine second target position information at the target moment according to the second position information; Determining a first relative error according to the first target posture information, the first posture information and the first target observation posture information; Determine a second relative error according to the second target pose information, the second pose information, and the second target observation pose information; The target posture is determined according to the first relative error, the second relative error and a preset confidence level.

2. The method for determining a posture according to claim 1, characterized in that: The posture information at the first moment includes: a left wheel speed, a right wheel speed, a distance between the left wheel and the right wheel, a first rotation angle, and a first displacement; wherein, determining the first target posture information at the target moment according to the posture information at the first moment and the target moment includes: Determine the angular velocity and linear velocity based on the left wheel speed, the right wheel speed, and the distance between the left wheel and the right wheel; Determine the incremental time according to the difference between the target time and the first time; The first target position information at the target moment is determined according to the incremental time, the angular velocity, the linear velocity, the rotation angle and the displacement.

3. The method for determining position and posture according to claim 1, characterized in that: The target moment includes multiple angular velocity change moments, and the second posture information includes angular velocity, second rotation angle, and second displacement; wherein, according to the second posture information, determining the second target posture information at the target moment includes: Determine a target angular velocity according to a plurality of angular velocity change moments and an angular velocity corresponding to each angular velocity change moment; Determine a rotation matrix according to the target angular velocity, and determine a rotation increment matrix according to the rotation matrix; The second target position information at the target moment is determined according to the rotation increment matrix, the second rotation angle and the second displacement.

4. The method for determining a posture according to claim 1, characterized in that: The determining a first relative error according to the first target posture information, the first posture information and the first target observation posture information includes: Obtaining the first observation noise and the first observation pose information at the previous moment of the first moment; Determine second observation posture information of the first target at a time point according to the first target posture information, the first observation posture information and the first observation noise; A first relative error is determined according to the first observation posture information, the second observation posture information, the first target posture information, and the first posture information.

5. The method for determining position and posture according to claim 3, characterized in that: Determining a second relative error according to the second target pose information, the second pose information, and the second target observation pose information includes: Obtaining the second observation noise and the third observation posture information at the previous moment of the first moment; Determining fourth observation posture information at the first moment according to the second observation noise; Integrating the angular velocity to obtain a posture increment; A second relative error is determined according to the second target posture information, the posture increment, the fourth observation posture information, and the third observation posture information.

6. The method for determining position and posture according to claim 1, characterized in that: The preset confidence level includes a first confidence level and a second confidence level; wherein, determining the target posture according to the first relative error, the second relative error and the preset confidence level includes: Determining a weighted sum of squares according to the first relative error and the first confidence level, the second relative error and the second confidence level; The first relative error and the second relative error are optimized to minimize the weighted sum of squares to obtain the target posture.

7. A device for determining a position and posture, characterized in that: The device comprises: An acquisition module, used to acquire first posture information and second posture information at a first moment; wherein the first posture information is posture information measured by an odometer, and the second posture information is posture information measured by an inertial measurement unit; A first determination module is used to determine first target posture information at a target moment according to the posture information at the first moment and the target moment; and to determine second target posture information at the target moment according to the second posture information; A second determination module, used to determine a first relative error according to the first target posture information, the first posture information and the first target observation posture information; A third determination module is used to determine a second relative error according to the second target posture information, the second posture information, and the second target observation posture information; The fourth determination module is used to determine the target posture according to the first relative error, the second relative error and a preset confidence level.

8. A computer device, characterized in that: include: A memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the method for determining posture according to any one of claims 1 to 6 by executing the computer instructions.

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a computer to execute the method for determining a posture according to any one of claims 1 to 6.

10. A computer program product, characterized in that The method comprises computer instructions for causing a computer to execute the method for determining a posture according to any one of claims 1 to 6.

Images