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

Abstract

The present application relates to the technical field of multi-sensor data fusion, and discloses a method and device for determining a pose, a computer device, a medium and a program product. The method comprises: obtaining first pose information and second pose information at a first time; wherein the first pose information is pose information measured by an odometer, and the second pose information is pose information measured by an inertial measurement unit; determining first target pose information at a target time according to the pose information at the first time and the target time; and determining second target pose information at the target time according to the second pose information; determining a first relative error according to the first target pose information, the first pose information and first target observation pose information; determining a second relative error according to the second target pose information, the second pose information and second target observation pose information; and determining a target pose according to the first relative error, the second relative error and a preset confidence level.

Description

Technical Field

[0001] This invention relates to the field of multi-sensor data fusion technology, specifically to methods, apparatus, computer equipment, storage media, and program products for determining pose. Background Technology

[0002] With the increasing aging of the global population and the growing demand from people with mobility impairments, intelligent driving wheelchairs, as a high-tech assistive device, are gradually becoming a key area in the future of assistive technology. Intelligent driving wheelchairs not only meet the basic mobility needs of people with disabilities but also incorporate advanced technologies from modern precision mechanics, intelligent numerical control, and engineering mechanics. However, to achieve widespread application of intelligent driving wheelchairs, the problem of high-precision positioning and navigation in complex environments must be solved.

[0003] Currently, navigation technology for intelligent driving wheelchairs primarily relies on a single sensor, such as a wheeled odometer or an inertial measurement unit (IMU). The specific implementation is as follows:

[0004] Wheel odometry estimates the distance and direction a robot travels by measuring the rotation of its wheels. However, after prolonged operation, the cumulative error of wheel odometry gradually increases due to wheel wear and mechanical errors.

[0005] IMUs estimate a robot's posture and motion by measuring acceleration and angular velocity. However, IMU data is susceptible to noise and drift, leading to a gradual increase in positioning errors after prolonged operation.

[0006] Therefore, accurately locating the position and posture of intelligent driving wheelchairs has become 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 pose.

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

[0009] The pose determination method provided in this embodiment, by incorporating pose information from both the IMU and the wheel odometry, can compensate for the problem of gradually increasing cumulative errors caused by wheel wear and mechanical errors, thereby providing accurate information about the robot's posture and motion. In other words, by fusing data from the odometry and IMU, continuous and high-precision estimation of the robot's pose can be achieved, effectively reducing the cumulative errors after long-term operation.

[0010] Meanwhile, although odometry data has cumulative errors, it is relatively accurate in estimating the robot's distance traveled in a short period of time. Therefore, by fusing the data from both sources, the short-term accuracy of odometry can be used to correct the long-term drift problem of the IMU, thereby improving the overall positioning accuracy.

[0011] Furthermore, the importance of comprehensively utilizing multi-sensor data is highlighted. By comparing the relative errors between the first target pose information, the second target pose information, and their respective observed target pose information, sensor anomalies can be detected, which not only improves positioning accuracy but also enhances the robustness and reliability of the system.

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

[0013] The pose determination method provided in this embodiment can more accurately calculate the robot's pose at different times by integrating information such as the left wheel speed, right wheel speed, distance between the left and right wheels, first rotation angle, and first displacement, thereby reducing positioning errors caused by inaccurate single parameters.

[0014] Furthermore, the robot's pose information can be updated in real time based on the difference between the target time and the first time (i.e., the incremental time). This allows the system to dynamically adapt to the robot's motion state and promptly reflect changes in the robot's position.

[0015] Furthermore, by calculating angular velocity and linear velocity, it can handle various motion modes of robots, such as linear motion, curvilinear motion, and rotation in place, thus enabling it to cope with a wider range of motion scenarios.

[0016] In one possible implementation, the target time includes multiple angular velocity change moments, and the second pose information includes angular velocity, a second rotation angle, and a second displacement. Determining the second target pose information at the target time based on the second pose information includes: 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; and determining the second target pose information at the target time based on the rotation increment matrix, the second rotation angle, and the second displacement.

[0017] The pose determination method provided in this embodiment can accurately record and analyze multiple angular velocity changes and their corresponding angular velocities, thereby precisely capturing the dynamic changes of the robot during motion. Furthermore, the rotation increment matrix is ​​derived from the rotation matrix, a step that accurately calculates the rotation increment of the robot during angular velocity changes. 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, determining the first relative error based on the first target pose information, the first first pose information, and the first target observation pose information includes: acquiring the first observation noise and the first observation pose information of the previous time step at the first time step; determining the second observation pose information of the first target time step based on the first target pose information, the first observation pose information, and the first observation noise; and 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 first pose information.

[0019] The pose determination method provided in this embodiment addresses the unavoidable observation noise in practical applications, which can originate from various factors such as sensor accuracy and environmental interference. By introducing first observation noise, this method can more realistically reflect the uncertainties in the actual observation process, thereby improving the accuracy of error assessment. Utilizing the first observation pose information from the previous moment, a more complete observation sequence can be constructed, thus more accurately assessing the difference between the current observation pose and the target pose, thereby obtaining a more reliable first relative error.

[0020] In one possible implementation, determining the second relative error based on the second target pose information, the second pose information, and the second target observation pose information includes: acquiring the second observation noise and the third observation pose information of the previous time step of the first time step; determining the fourth observation pose information of the first time step based on the second observation noise; integrating the angular velocity to obtain the pose increment; and determining the second relative error based on the second target pose information, the pose increment, the fourth observation pose information, and the third observation pose information.

[0021] The pose determination method provided in this embodiment not only considers the difference between the second target pose information and the second pose information, but also introduces second observation noise and third observation pose information, thereby enabling a more comprehensive assessment of errors. By comprehensively considering these factors, the uncertainty in the actual motion process can be reflected more accurately, improving the precision of error assessment.

[0022] In one possible implementation, the preset confidence level includes a first confidence level and a second confidence level; wherein, determining the target pose 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, 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 pose.

[0023] The pose determination method provided in this embodiment weights the two types of errors using a first confidence level and a second confidence level. This makes the error assessment more comprehensive and can more accurately reflect the uncertainties in the actual motion process. Simultaneously, by minimizing the weighted sum of squares, the impact of individual errors on the overall pose estimation is eliminated or reduced, thus determining the target pose with the minimum total error and improving the accuracy of target pose determination.

[0024] Secondly, the present invention provides an apparatus for determining pose, the apparatus comprising: an acquisition module for acquiring first pose information and second pose information at a first moment; wherein the first pose information is pose information measured by an odometer and the second pose information is pose information measured by an inertial measurement unit; a first determination module for determining first target pose information at a target moment based on the pose information at the first moment and a target moment; and determining second target pose information at the target moment based on the second pose information; a second determination module for determining a first relative error based on the first target pose information, the first pose information, and the first target observed pose information; a third determination module for determining a second relative error based on the second target pose information, the second pose information, and the second target observed pose information; and a fourth determination module for determining a target pose based on the first relative error, the second relative error, and a preset confidence level.

[0025] Thirdly, 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 computer instructions to perform the method for determining pose described in the first aspect or any corresponding embodiment thereof.

[0026] Fourthly, the present invention provides a computer-readable storage medium storing computer instructions for causing a computer to perform the method for determining an orientation described in the first aspect or any corresponding embodiment thereof.

[0027] Fifthly, the present invention provides a computer program product, including computer instructions for causing a computer to perform the method for determining pose described in the first aspect or any corresponding embodiment thereof. Attached Figure Description

[0028] To more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the specific embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0029] Figure 1 This is a flowchart illustrating a method for determining pose according to an embodiment of the present invention;

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

[0031] Figure 3 This is a schematic diagram of the hardware structure of a computer device according to an embodiment of the present invention. Detailed Implementation

[0032] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0033] Based on relevant technologies, it is known that with the increasing aging of the global society and the growing demand from people with mobility impairments, intelligent driving wheelchairs, as a high-tech assistive device, are gradually becoming a key area in future assistive devices. Intelligent driving wheelchairs not only meet the basic mobility needs of people with mobility impairments but also incorporate advanced technologies from modern precision mechanics, intelligent numerical control, and engineering mechanics. However, to achieve widespread application of intelligent driving wheelchairs, the problem of high-precision positioning and navigation in complex environments must be solved.

[0034] Currently, navigation technology for intelligent driving wheelchairs primarily relies on a single sensor, such as a wheeled odometer or an inertial measurement unit (IMU). The specific implementation is as follows:

[0035] Wheel odometry estimates the distance and direction a robot travels by measuring the rotation of its wheels. However, after prolonged operation, the cumulative error of wheel odometry gradually increases due to wheel wear and mechanical errors.

[0036] IMUs estimate a robot's posture and motion by measuring acceleration and angular velocity. However, IMU data is susceptible to noise and drift, leading to a gradual increase in positioning errors after prolonged operation.

[0037] Based on this, the present invention provides a method for determining robot pose. By introducing pose information from an IMU and a wheeled odometer, it can compensate for the problem of gradually increasing cumulative errors caused by wheel wear and mechanical errors, thereby providing accurate information about the robot's posture and motion. In other words, by fusing data from the odometer and IMU, continuous and high-precision estimation of the robot's pose can be achieved, effectively reducing the cumulative errors after long-term operation.

[0038] Meanwhile, although odometry data has cumulative errors, it is relatively accurate in estimating the robot's distance traveled in a short period of time. Therefore, by fusing the data from both sources, the short-term accuracy of odometry can be used to correct the long-term drift problem of the IMU, thereby improving the overall positioning accuracy.

[0039] Furthermore, the importance of comprehensively utilizing multi-sensor data is highlighted. By comparing the relative errors between the first target pose information, the second target pose information, and their respective observed target pose information, sensor anomalies can be detected, which not only improves positioning accuracy but also enhances the robustness and reliability of the system.

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

[0041] This embodiment provides a method for determining pose, which can be used in computer devices such as computers and servers. Figure 1 This is a flowchart illustrating a method for determining pose according to an embodiment of the present invention, as shown below. Figure 1 As shown, the process includes the following steps:

[0042] Step S101: Obtain the first pose information and the second pose information at the first moment; wherein, the first pose information is the pose information measured by the odometer and the second pose information is the pose information measured by the inertial measurement unit.

[0043] An odometer is a sensor used to measure the distance and direction of movement of a device. An inertial measurement unit (IMU) is a device that integrates sensors such as accelerometers and gyroscopes to measure the inertial data of the device. The device being measured can be a wheelchair or other equipment; no specific limitation is made here.

[0044] The first pose information can characterize the device's position and attitude relative to the initial position at a given moment. This first moment can be any point in time, such as 1 second or 2 seconds, without specific limitations. Specifically, the first pose information is obtained by measuring the device using an odometer.

[0045] The second pose information can characterize the position and attitude of the device relative to the initial position at a first moment. Specifically, the second pose information can be obtained by measuring the device using 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 position obtained by integrating the acceleration and angular velocity data measured by the IMU is (10.1, 19.9) and the attitude is (0.01, 0.02, -0.01).

[0047] As an example, when determining the first pose information via odometry, in SE(3), the robot's pose in space can be a 4x4 homogeneous transformation matrix (T(t)) that includes rotation and displacement. Specifically, it can be represented by the following equation:

[0048] Where R(t)∈SO(3) is a 3x3 rotation matrix used to represent the robot's posture (orientation) in three-dimensional space, and p(t)∈R3 is used to characterize the robot's position at time t.

[0049] As an example, when determining the second pose information via an IMU, the angle of the IMU can be: ξ = [υ x υ y υ z ω x ω y ω z ] T ; where ω x (t) represents the speed at which the robot rotates around the x-axis, ω y (t) represents the speed at which the robot rotates around the y-axis, ω z(t) represents the rate of rotation of the robot around the z-axis. In this embodiment, only the rate of rotation of the robot around the z-axis needs to be processed. Therefore, the change in rotation angle can be obtained through time integration, specifically using the following formula:

[0050] Where i represents a sub-time point within the first time point, meaning the first time point can be divided into multiple sub-time points. N represents the total number of sub-time points obtained by dividing the first time point, Δt. i It represents the difference between adjacent sub-time points.

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

[0052] Where ω^ is the rotation matrix, ω z The speed of rotation about the z-axis.

[0053] For small step sizes, the rotation increment matrix is ​​approximately ΔR≈exp(ω^Δt)=I+ω^Δt; where I is the identity matrix and Δt is the difference between adjacent time steps. The IMU's attitude can be updated using the rotation increment matrix.

[0054] Step S102: Based on the pose information at the first moment and the target moment, determine the first target pose information at the target moment; and based on the second pose information, determine the second target pose information at the target moment.

[0055] The first target pose information can characterize the target's position and orientation (i.e., pose) at any given moment during the odometry measurement process. In pose, position refers to coordinates (x, y, z) in three-dimensional space, while orientation is represented by rotation matrices or Euler angles (such as pitch, yaw, and roll). Similarly, the second target pose information can be the target's position and orientation at any given moment during the IMU measurement process.

[0056] The target time can represent a future time at which pose estimation needs to be performed. Based on the pose information of the first time and the target time, the first target pose information of the target time is determined using a certain prediction or interpolation algorithm. Similarly, the second target pose information of the target time is determined based on the second pose information. No specific limitations are 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. Linear or nonlinear interpolation of the pose information can also be used. The time interval between the target time and the first time is 1 second. Using the Kalman filtering algorithm, the first target pose information at the target time is predicted to be (11, 21) based on the pose information at the first time, with the pose remaining unchanged. Similarly, the second target pose information at the target time is predicted to be (10.2, 20.0) based on the IMU pose information, with a slight change in pose.

[0058] Step S103: Determine the first relative error based on the first target pose information, the first first pose information, and the first target observation pose information.

[0059] The first target observation pose information can characterize the pose difference between adjacent time steps, etc., without specific limitations here. Specifically, the positional difference between the first target pose information and the first position information can be obtained through simple vector subtraction, while the directional 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 position information can be combined with the first target observation pose information to determine the first relative error.

[0060] Step S104: Determine the second relative error based on the second target pose information, the second pose information, and the second target observation pose information.

[0061] The second target's observed pose information can characterize the pose difference between adjacent time points, etc., without specific limitations here. Specifically, the second target's pose information and the positional difference between the second pose information can be obtained through simple vector subtraction, while the directional 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 second target's pose information and the error between the second pose information can be combined with the second target's observed pose information to determine the first relative error.

[0062] Step S105: Determine the target pose based on the first relative error, the second relative error, and the 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 levels of the first relative error and the second relative error can be the same or different. The confidence level can be a manually set value or determined through other methods; no specific limitations are made here.

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

[0065] The pose determination method provided in this embodiment, by incorporating pose information from both the IMU and the wheel odometry, can compensate for the problem of gradually increasing cumulative errors caused by wheel wear and mechanical errors, thereby providing accurate information about the robot's posture and motion. In other words, by fusing data from the odometry and IMU, continuous and high-precision estimation of the robot's pose can be achieved, effectively reducing the cumulative errors after long-term operation.

[0066] Meanwhile, although odometry data has cumulative errors, it is relatively accurate in estimating the robot's distance traveled in a short period of time. Therefore, by fusing the data from both sources, the short-term accuracy of odometry can be used to correct the long-term drift problem of the IMU, thereby improving the overall positioning accuracy.

[0067] Furthermore, the importance of comprehensively utilizing multi-sensor data is highlighted. By comparing the relative errors between the first target pose information, the second target pose information, and their respective observed target pose information, sensor anomalies can be detected, which not only improves positioning accuracy but also enhances the robustness and reliability of the system.

[0068] In one possible implementation, the pose information at the first moment includes: left wheel speed, right wheel speed, distance between the left and right wheels, first rotation angle, and first displacement; wherein, in step S102 above, determining the first target pose information at the target moment based on the pose information at the first moment and the target moment includes:

[0069] Step a1: Determine the angular velocity and linear velocity based on the speed of the left wheel, the speed of the right wheel, and the distance between the left and right wheels.

[0070] The speed of the left wheel characterizes the speed of the left wheel during the movement of the device. The speed of the right wheel characterizes the speed of the right wheel during the movement of the device. The distance between the left and right wheels characterizes the straight-line distance between the left and right wheels in the axis of symmetry.

[0071] Specifically, by measuring the real-time speeds 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., wheel spacing), the angular velocity and linear velocity of the robot can be calculated using the differential drive principle. 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 those skilled in the art.

[0072] In one possible implementation, the angular velocity and linear velocity are determined based on the speeds of the left and right wheels and the distance between them, using the following formula:

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

[0074] Step a2: Determine the incremental time based on the difference between the target time and the first time.

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

[0076] Step a3: Determine the first target pose information at the target time based on the incremental time, angular velocity, linear velocity, rotation angle, and displacement.

[0077] The robot's rotation angle and displacement during that time period are calculated using incremental time, angular velocity, and linear velocity.

[0078] As an example, linear velocity and incremental time can be used to calculate the distance a robot moves in a straight line. If the robot moves in a two-dimensional plane, it may be necessary to calculate the displacement in the x and y directions separately. If the robot moves in a three-dimensional plane, it may be necessary to calculate the displacement in the x, y, and z directions separately. Angular velocity and incremental time can be used to calculate the angle of rotation of the robot around a point or axis within an incremental time.

[0079] The robot's position is updated based on the initial position (position at the first moment) and the calculated displacement. If the robot is moving 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 is moving in three-dimensional space, the displacement in the z direction also needs to be considered. The robot's orientation is updated based on the initial orientation (orientation at the first moment) and the calculated rotation angle. The updated position and orientation information are combined to form the first target pose information at the target moment.

[0080] In one possible implementation, the first target pose information at the target moment is determined based on the incremental time, angular velocity, linear velocity, rotation angle, and displacement, using the following formula:

[0081] Where Δt is the increment time (i.e., the difference between adjacent time points), t(t) is the displacement, R(t) is the attitude in three-dimensional space, V is the linear velocity, and W is the angular velocity.

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

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

[0084] ΔT=exp(ξ^Δt); where, ξ=[v x v y v z ω x ω y ω z ] T Let ξ be a 6D twist vector representing the robot's linear and angular velocities in x, y, and z directions in 3D space. ξ^ is its antisymmetric matrix, used to generate pose increments.

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

[0086]

[0087] The pose increment matrix can be calculated using the 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 identity matrix.

[0089] Therefore, we can conclude that:

[0090] Furthermore, for wheeled robots, since the motion occurs in a two-dimensional plane, therefore v y The formula can be simplified by setting the expression = 0. The simplified formula is as follows:

[0091]

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

[0093] In summary, the following calculations can be performed:

[0094]

[0095] The pose determination method provided in this embodiment can more accurately calculate the robot's pose at different times by integrating information such as the left wheel speed, right wheel speed, distance between the left and right wheels, first rotation angle, and first displacement, thereby reducing positioning errors caused by inaccurate single parameters.

[0096] Furthermore, the robot's pose information can be updated in real time based on the difference between the target time and the first time (i.e., the incremental time). This allows the system to dynamically adapt to the robot's motion state and promptly reflect changes in the robot's position.

[0097] Furthermore, by calculating angular velocity and linear velocity, it can handle various motion modes of robots, such as linear motion, curvilinear motion, and rotation in place, thus enabling it to cope with a wider range of motion scenarios.

[0098] In one possible implementation, the target time includes multiple angular velocity change moments, and the second pose information includes angular velocity, second rotation angle, and second displacement; wherein, in step S102 above, determining the second target pose information at the target time based on the second pose information includes:

[0099] Step b1: Determine the target angular velocity based on the multiple angular velocity change times and the angular velocity corresponding to each angular velocity change time.

[0100] The moment of angular velocity change can be characterized as the moment of angular velocity change corresponding to each sub-moment within the aforementioned incremental time process. From a series of angular velocity change time points and their corresponding angular velocity values, the angular velocity at the target moment can be inferred or calculated. This may involve data smoothing, interpolation, or predictions based on physical models, etc., which are not specifically limited here.

[0101] In one possible implementation, the target angular velocity can be determined using the following formula.

[0102] Where Δθ is the target angular velocity, t′ is any time between t0 and t, i is the number of times between t0 and t (positive integer), and t i For each positive integer, Δt represents the time interval. i It represents the difference between two adjacent time points.

[0103] Step b2: Determine the rotation matrix based on the target angular velocity, and then determine the rotation increment matrix based on the rotation matrix.

[0104] A rotation matrix can be generated using the target angular velocity, representing the rotation from an initial direction to the target direction. Then, based on this rotation matrix, a rotation increment matrix is ​​calculated, representing the relative rotation from the current direction to the target direction.

[0105] As an example, rotation matrices can be represented and computed using Euler angles, Rodrigues parameters (rotation vectors), or quaternions.

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

[0107] In one possible implementation, the following formula can be used to determine:

[0108] Where T(t) is the pose of the IMU at the first moment of measurement; R′(t) is the rotation matrix of the IMU, and t′(t) is the translation vector of the IMU.

[0109] The pose can be updated using IMU data by using a rotation increment matrix.

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

[0111] Step b3: Determine the second target pose information at the target time based on the rotation increment matrix, the second rotation angle, and the second displacement.

[0112] The rotation increment matrix is ​​combined with a second rotation angle (which may be obtained from another source or a previous step) and a second displacement (representing linear movement along a certain direction) to calculate the complete pose information at the target time.

[0113] As an example, a rotation increment matrix can be applied to the current direction and combined with a 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 then 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 component. Add the second displacement (dx, dy, dz) to the current position to obtain the final displacement component. Combine the rotation and displacement components to form the second target pose information.

[0114] In one possible implementation, the pose information of the second target can be determined using the following formula:

[0115]

[0116] The pose determination method provided in this embodiment can accurately record and analyze multiple angular velocity changes and their corresponding angular velocities, thereby precisely capturing the dynamic changes of the robot during motion. Furthermore, by deriving the rotation increment matrix from the rotation matrix, this step can accurately calculate the rotation increment of the robot during angular velocity changes. 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.

[0117] In one possible implementation, step S103 above includes:

[0118] Step c1: Obtain the first observation noise and the first observation pose information of the previous time step.

[0119] The first observation pose information of the time preceding the first time step can be the position and orientation information of the object obtained through observation at a time step before the first target time step (e.g., the time step immediately preceding the first time step). The first observation noise can represent the uncertainty or error introduced during the observation process. It may be caused by sensor accuracy limitations, environmental interference, or imperfections in the data processing algorithm. Specifically, the accuracy parameters of the first observation noise can be obtained from the sensor's datasheet or estimated through actual testing, etc., without specific limitations here. For example: using a LiDAR sensor to observe a mobile robot. At time t-1 (i.e., the time step preceding the first time step), the robot's observation pose information (position x, y, z and attitude angles roll, pitch, yaw) and the LiDAR's observation noise parameters (such as distance error, angle error, etc.) are obtained.

[0120] Step c2: Determine the second observation pose information of the first target at the first time based on the first target pose information, the first observation pose information, and the first observation noise.

[0121] Using the first target pose information (obtainable through some prediction or estimation method), the first observed pose information, and the first observation noise, the observed pose information at the first target time is calculated or estimated. This typically involves data fusion, filtering, or prediction algorithms, which are not specifically limited here. For example, the pose information at time t (i.e., the first target time) can be predicted based on the observed pose information at time t-1 and the robot's motion commands. Then, the prediction result can be fused with the actual observed pose information at time t (considering the observation noise) to obtain a more accurate second observed pose information.

[0122] In one possible implementation, the second observation pose information at the first target time can be determined using the following formula:

[0123] in, The second observation pose information at the first target time, T(t). -1 T(t+1) represents the difference between the first target pose information and the first target pose information. This refers to the observation noise of the odometer.

[0124] Step c3: Determine the first relative error based on the first observation pose information, the second observation pose information, the first target pose information, and the first pose information.

[0125] The first relative error can 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 measure between different pose information can be calculated.

[0126] In one possible implementation, the first relative error can be determined using the following formula:

[0127] Where, T(t1) -1 T(t2) represents the difference between the first target pose information and the first pose information. The difference between the first observation pose information and the second observation pose information. This is the first relative error.

[0128] The pose determination method provided in this embodiment addresses the unavoidable observation noise in practical applications, which can originate from various factors such as sensor accuracy and environmental interference. By introducing first observation noise, this method can more realistically reflect the uncertainties in the actual observation process, thereby improving the accuracy of error assessment. Utilizing the first observation pose information from the previous moment, a more complete observation sequence can be constructed, thus more accurately assessing the difference between the current observation pose and the target pose, thereby obtaining a more reliable first relative error.

[0129] In one possible implementation, step S104 above includes:

[0130] Step d1: Obtain the second observation noise and the third observation pose information of the previous time step of the first time step.

[0131] The third observation pose information can characterize the position and orientation information of an object (such as a robot or vehicle) observed by sensors (such as cameras, lidar, etc.). The second observation noise can characterize the noise introduced during the observation process due to factors such as sensor errors and environmental interference.

[0132] As an example, the observation noise at the current moment (second moment) and the observation pose information at the previous moment (the moment before the first moment) are obtained from sensors or other data sources.

[0133] Step d2: Determine the fourth observation pose information at the first moment based on the second observation noise.

[0134] Using target pose information, current observation pose information, and observation noise, the observation pose information at the first moment is estimated or corrected through a certain algorithm (such as Kalman filtering, particle filtering, etc.).

[0135] In one possible implementation, the fourth observation pose information at the first moment can be determined using the following formula:

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

[0137] Step d3: Integrate the angular velocity to obtain the pose increment.

[0138] The angular velocity measured by the sensor is integrated to calculate the change in position and orientation of the object over a period of time. Specifically, numerical integration methods (such as trapezoidal integral, Simpson integral, etc.) can be used to determine the pose increment. No specific limitation is made here, and the implementation can be carried out by those skilled in the art.

[0139] Step d4: Determine the second relative error based on the second target pose information, pose increment, fourth observation pose information, and third observation pose information.

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

[0141] In one possible implementation, the second relative error can be determined using the following formula:

[0142] in, The difference between the fourth observation pose information and the third observation pose information. For pose increment, This is the second relative error.

[0143] The pose determination method provided in this embodiment not only considers the difference between the second target pose information and the second pose information, but also introduces second observation noise and third observation pose information, thereby enabling a more comprehensive assessment of errors. By comprehensively considering these factors, the uncertainty in the actual motion process can be reflected more accurately, improving the precision of error assessment.

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

[0145] Step e1: Determine the weighted sum of squares based on the first relative error and the first confidence level, the second relative error and the second confidence level.

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

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

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

[0149] Step e2: Optimize the first relative error and the second relative error to minimize the weighted sum of squares and obtain the target pose.

[0150] Optimization algorithms (such as gradient descent, Newton's method, and genetic algorithms) can be used to minimize the weighted sum of squares, thereby finding the optimal target pose. For example, gradient descent can be used to optimize the weighted sum of squares. An initial target pose guess can be set, then the gradient (i.e., the derivative of the weighted sum of squares with respect to the target pose) can be calculated, and the target pose can be updated according to the gradient direction. By iterating this process continuously until the weighted sum of squares converges to its minimum, the optimal target pose can be obtained.

[0151] This embodiment also provides a device for determining pose, which is used to implement the above embodiments and preferred embodiments; details already described will not be repeated. As used below, the term "module" can refer to a combination of software and / or hardware that implements a predetermined function. Although the device described in the following embodiments is preferably implemented in software, hardware implementation, or a combination of software and hardware, is also possible and contemplated.

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

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

[0154] In one possible implementation, the target time includes multiple angular velocity change moments, and the second pose information includes angular velocity, a second rotation angle, and a second displacement. The first determining module 202 includes: a fourth determining unit, used to determine the target angular velocity based on the multiple angular velocity change moments and the angular velocity corresponding to each angular velocity change moment; a fifth determining unit, used to determine a rotation matrix based on the target angular velocity and a rotation increment matrix based on the rotation matrix; and a sixth determining unit, used to determine the second target pose information at the target time based on the rotation increment matrix, the second rotation angle, and the second displacement.

[0155] In one possible implementation, the second determining module 203 includes: a first acquiring unit, configured to acquire first observation noise and first observation pose information of the previous time step of the first time step; a seventh determining unit, configured to determine second observation pose information of the first target time step based on the first target pose information, the first observation pose information and the first observation noise; and an eighth determining unit, configured to determine a first relative error based on the first observation pose information, the second observation pose information, the first target pose information and the first first pose information.

[0156] In one possible implementation, the third determining module 204 includes: a second acquisition unit for acquiring second observation noise and third observation pose information of the previous time step of the first time step; a ninth determining unit for determining fourth observation pose information of the first time step based on the second observation noise; an integration unit for integrating the angular velocity to obtain the pose increment; and a tenth determining unit for determining a second relative error based on the second target pose information, the pose increment, the fourth observation pose information, and the third observation pose information.

[0157] In one possible implementation, the preset confidence level includes a first confidence level and a second confidence level; wherein, the fourth determining module 205 includes: an eleventh determining 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; and 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 pose.

[0158] Further functional descriptions of the above modules and units are the same as those in the corresponding embodiments described above, and will not be repeated here.

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

[0160] This invention also provides a computer device having the above-described features. Figure 2 The device shown is for determining the pose.

[0161] Please see Figure 3 , Figure 3 This is a schematic diagram of the structure of a computer device provided in an optional embodiment of the present invention, such as... Figure 3As shown, the computer device includes one or more processors 10, memory 20, and interfaces for connecting the components, including high-speed interfaces and low-speed interfaces. The components communicate with each other via different buses and can be mounted on a common motherboard or otherwise installed as needed. The processors can process instructions executed within the computer device, including instructions stored in or on memory to display graphical information of a GUI on external input / output devices (such as display devices coupled to the interfaces). In some alternative implementations, multiple processors and / or multiple buses can be used with multiple memories and multiple memory modules, if desired. Similarly, multiple computer devices can be connected, each providing some of the necessary operations (e.g., as a server array, a group of blade servers, or a multiprocessor system). Figure 3 Take a processor 10 as an example.

[0162] Processor 10 may be a central processing unit, a network processor, or a combination thereof. Processor 10 may further include a hardware chip. The hardware chip may be an application-specific integrated circuit (ASIC), a programmable logic device (PLD), or a combination thereof. The programmable logic device may be a complex programmable logic device (CAMP), a field-programmable gate array (FPGA), a general-purpose array logic (GDA), or any combination thereof.

[0163] The memory 20 stores instructions executable by at least one processor 10 to cause at least one processor 10 to perform the method shown in the above embodiments.

[0164] The memory 20 may include a program storage area and a data storage area. The program storage area may store the operating system and applications required for at least one function; the data storage area may store data created based on the use of the computer device. Furthermore, the memory 20 may include high-speed random access memory and may also include non-transitory memory, such as at least one disk storage device, flash memory device, or other non-transitory solid-state storage device. In some alternative embodiments, the memory 20 may optionally include memory remotely located relative to the processor 10, and these remote memories may be connected to the computer device via a network. Examples of such networks include, but are not limited to, the Internet, intranets, local area networks, mobile communication networks, and combinations thereof.

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

[0166] The computer device also includes a communication interface 30 for communicating with other devices or communication networks.

[0167] This invention also provides a computer-readable storage medium. The methods described above according to embodiments of the invention can be implemented in hardware or firmware, or implemented as computer code that can be recorded on a storage medium, or implemented as computer code downloaded via a network and originally stored on a remote storage medium or a non-transitory machine-readable storage medium and then stored on a local storage medium. Thus, the methods described herein can be processed by software stored on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. The storage medium can be a magnetic disk, optical disk, read-only memory, random access memory, flash memory, hard disk, or solid-state drive, etc.; further, the storage medium can also include combinations of the above types of memory. It is understood that computers, processors, microprocessor controllers, or programmable hardware include storage components capable of storing or receiving software or computer code, which, when accessed and executed by the computer, processor, or hardware, implements the methods shown in the above embodiments.

[0168] A portion of this invention can be applied as a computer program product, such as computer program instructions, which, when executed by a computer, can invoke or provide the methods and / or technical solutions according to the invention through the operation of the computer. Those skilled in the art will understand that the forms in which computer program instructions exist in a computer-readable medium include, but are not limited to, source files, executable files, installation package files, etc. Correspondingly, the ways in which computer program instructions are executed by a computer include, but are not limited to: the computer directly executing the instructions, or the computer compiling the instructions and then executing the corresponding compiled program, or the computer reading and executing the instructions, or the computer reading and installing the instructions and then executing the corresponding installed program. Here, the computer-readable medium can be any available computer-readable storage medium or communication medium accessible to a computer.

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

Claims

1. A method of determining a pose, characterized by, The method comprises: obtaining first pose information and second pose information at a first time; wherein the first pose information is pose information measured by an odometer, and the second pose information is pose information measured by an inertial measurement unit; determining first target pose information at a target time according to the pose information at the first time and the target time, and determining second target pose information at the target time according to the second pose information; determining a first relative error according to the first target pose information, the first pose information and first target observed pose information; determining a second relative error according to the second target pose information, the second pose information and second target observed pose information; determining a target pose according to the first relative error, the second relative error and a preset confidence level; the pose information at the first time comprises left wheel speed, right wheel speed, distance between the left wheel and the right wheel, first rotation angle and first displacement; wherein the determination of the first target pose information at the target time according to the pose information at the first time and the target time comprises: determining angular velocity and linear velocity according to the left wheel speed, the right wheel speed and the distance between the left wheel and the right wheel; determining an incremental time according to the difference between the target time and the first time; determining the first target pose information at the target time according to the incremental time, the angular velocity, the linear velocity, the rotation angle and the displacement; the determination of the first target pose information at the target time according to the incremental time, the angular velocity, the linear velocity, the rotation angle and the displacement comprises: where Δt is the incremental time, i.e. the difference between adjacent times, t(t) is the displacement, R(t) is the pose in three-dimensional space, V is the linear velocity, and W is the angular velocity.

2. The method of determining a pose according to claim 1, wherein, the target time comprises a plurality of angular velocity change times, and the second pose information comprises angular velocity, second rotation angle and second displacement; wherein the determination of the second target pose information at the target time according to the second pose information comprises: determining target angular velocity according to the plurality of angular velocity change times and the angular velocity corresponding to each angular velocity change time; determining a rotation matrix according to the target angular velocity, and determining a rotation incremental matrix according to the rotation matrix; determining the second target pose information at the target time according to the rotation incremental matrix, the second rotation angle and the second displacement.

3. The method of determining a pose of claim 1, wherein, the determination of the first relative error according to the first target pose information, the first pose information and first target observed pose information comprises: obtaining first observation noise and first observed pose information at a previous time of the first time; determining second observed pose information at the target time according to the first target pose information, the first observed pose information and the first observation noise; determining the first relative error according to the first observed pose information, the second observed pose information, the first target pose information and the first pose information.

4. The method of determining a pose of claim 2, wherein, the determination of the second relative error according to the second target pose information, the second pose information and second target observed pose information comprises: obtaining second observation noise and third observed pose information at a previous time of the first time; determining fourth observed pose information at the first time according to the second observation noise; integrating the angular velocity to obtain a pose increment; Determine a second relative error according to the second target pose information, the pose increment, the fourth observed pose information, and the third observed pose information.

5. The method of determining a pose of claim 1, wherein, The preset confidence degree includes a first confidence degree and a second confidence degree; and determining the target pose according to the first relative error, the second relative error, and the preset confidence degree includes: Determine a weighted sum of squares according to the first relative error and the first confidence degree, and the second relative error and the second confidence degree. Optimize the first relative error and the second relative error to minimize the weighted sum of squares to obtain the target pose.

6. An apparatus for determining a pose, the apparatus comprising: The device includes: An acquisition module is configured to acquire first pose information and second pose information at a first time; the first pose information is measured by a wheel odometer, and the second pose information is measured by an inertial measurement unit; A first determination module is configured to determine first target pose information at a target time according to the pose information at the first time and the target time, and determine second target pose information at the target time according to the second pose information; A second determination module is configured to determine a first relative error according to the first target pose information, the first pose information, and first target observed pose information; A third determination module is configured to determine a second relative error according to the second target pose information, the second pose information, and second target observed pose information; A fourth determination module is configured to determine a target pose according to the first relative error, the second relative error, and a preset confidence degree; The pose information at the first time includes left wheel speed, right wheel speed, distance between the left wheel and the right wheel, first rotation angle, and first displacement; the first determination module includes a first determination unit configured to determine angular velocity and 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 configured to determine an incremental time according to a difference between the target time and the first time; and a third determination unit configured to determine the first target pose information at the target time according to the incremental time, the angular velocity, the linear velocity, the rotation angle, and the displacement; the third determination unit is further configured to determine the first target pose information at the target time according to the incremental time, the angular velocity, the linear velocity, the rotation angle, and the displacement, including: where Δt is the incremental time, i.e. the difference between adjacent times, t(t) is the displacement, R(t) is the pose in three-dimensional space, V is the linear velocity, and W is the angular velocity.

7. A computer device, comprising: The device includes: A memory and a processor are communicatively connected, the memory stores computer instructions, and the processor executes the computer instructions to perform the method for determining a pose according to any one of claims 1 to 5.

8. A computer-readable storage medium, characterized in that, The computer readable storage medium stores computer instructions for causing a computer to perform the method for determining a pose according to any one of claims 1 to 5.

9. A computer program product, characterised in that, The computer readable storage medium stores computer instructions for causing a computer to perform the method for determining a pose according to any one of claims 1 to 5.

Images