Robot positioning method and electronic equipment

Abstract

The invention relates to the technical field of space positioning, and provides a robot positioning method and electronic equipment. The method comprises the steps that pose theoretical parameters of the robot are acquired; acquiring a rotation angle measurement parameter of the robot; fusing the pose theoretical parameters and the rotation angle measurement parameters to obtain fused pose parameters of the robot; acquiring position dynamic parameters of the robot; and fusing the fusion pose parameter and the position dynamic parameter to obtain a fusion dynamic pose of the robot. On the basis, theoretical accuracy and measurement real-time performance are fully utilized, errors of pure rotation angle measurement or pure theoretical calculation are reduced, accurate fusion pose parameters are obtained, meanwhile, the fusion pose parameters and the position dynamic parameters are combined, the position pure measurement error is reduced, the dynamic position of the robot in the actual environment is considered, and the real-time performance of the robot is improved. Through fusion of multi-dimensional information, errors and uncertainty possibly brought by a single information source can be reduced, and the robustness and accuracy of positioning are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of spatial positioning, and in particular, to a robot positioning method and an electronic device. Background Art

[0002] In a complex workshop environment, the positioning accuracy of an AGV (Automated Guided Vehicle) is not high enough. In an indoor home environment where there are more and more household appliances, children's toys and furniture, indoor mobile devices such as a sweeping robot need high-precision mobile positioning technology support to complete the environmental cleaning work. In an office environment with a large number of documents and storage boxes and diverse space layouts, the high-precision mobile positioning of a sweeping robot becomes more and more difficult.

[0003] However, the existing robot spatial positioning methods have the technical problem that their accuracy needs to be improved. Summary of the Invention

[0004] The purpose of the present invention is to provide a robot positioning method and an electronic device, aiming to solve the technical problem that the accuracy of the existing robot spatial positioning methods needs to be improved.

[0005] In a first aspect, the present application provides a robot positioning method, and the method includes the following steps:

[0006] S100: Obtain the theoretical pose parameters of the robot;

[0007] S200: Obtain the measured corner parameters of the robot;

[0008] S300: Fuse the theoretical pose parameters and the measured corner parameters to obtain the fused pose parameters of the robot;

[0009] S400: Obtain the position dynamic parameters of the robot;

[0010] S500: Fuse the fused pose parameters and the position dynamic parameters to obtain the fused dynamic pose of the robot.

[0011] In one embodiment, step S100 includes the following steps:

[0012] S110: Obtain the fused pose parameters of the previous moment;

[0013] S120: Obtain the position dynamic parameters of the previous moment;

[0014] S130: Obtain the speed dynamic parameters of the current moment;

[0015] S140: Based on the pose change model of Lie group theory, obtain the theoretical pose parameters of the current moment.

[0016] In one embodiment, step S140 includes the following steps:

[0017] S141: Complete the autonomous navigation of the robot based on the path planner;

[0018] S142: Obtain the position representation parameters of the sensor for representing the position of the robot;

[0019] S143: Establish the Lie algebra matrix model of the robot;

[0020] S144: Establish the Lie group matrix of the pose change at each position through exponential mapping;

[0021] S145: Derive the theoretical pose parameters at this moment based on the Lie group matrix.

[0022] In one embodiment, step S200 includes the following steps:

[0023] S210: Obtain the first pose matrix of the robot using the lidar positioning system;

[0024] S220: Obtain the second pose matrix of the robot using the ultra-wideband technology positioning system;

[0025] S230: Use the Kalman filter to fuse the first pose matrix and the second pose matrix to obtain the rotation angle measurement parameters.

[0026] In one embodiment, step S400 includes the following steps:

[0027] S410: Obtain the first measured position parameter of the first position;

[0028] S420: Obtain the second measured position parameter of the first position;

[0029] S430: Calculate the fusion position dynamic parameter according to the first measured position parameter and the second measured position parameter.

[0030] In one embodiment, in any dimension, the variance of the first measured position parameter ≤ 4 mm.

[0031] In one embodiment, in any dimension, the variance of the second measured position parameter ≤ 16 mm.

[0032] In one embodiment, the first measured position parameter and the second measured position parameter are uncorrelated, and their covariance is 0.

[0033] In one embodiment, step S410 includes the following steps:

[0034] S411: Obtain the rotation angle parameter of the first position using the first sensor;

[0035] S412: Obtain the distance parameter of the first position using the second sensor;

[0036] S413: Fuse the rotation angle parameter and the distance parameter to obtain the first measured position parameter.

[0037] In one embodiment, the first sensor is fixedly installed at the first position at a preset angle. The first sensor is a sensor board integrated with a geomagnetometer and an accelerometer. Step S411 includes the following steps:

[0038] S4111: Collect the magnetic field intensity data of the first position through the geomagnetometer;

[0039] S4112: Collect the acceleration data of the first position through the accelerometer;

[0040] S4113: Determine the yaw angle of the sensor board based on the acceleration data and the magnetic field intensity data;

[0041] S4114: Determine the rotation angle parameter of the first position based on the angle between the yaw angle and the geographic north direction.

[0042] In one embodiment, the robot is an AGV, a sweeping robot, or a multi-axis robot.

[0043] In one embodiment, the fused dynamic pose is represented by an N*M matrix, where both N and M are positive integers, N≥1, 6≥M≥2, N represents the degree-of-freedom node, and M represents the position and attitude parameter corresponding to the degree-of-freedom node.

[0044] In a second aspect, the present application provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps in the robot positioning method as described in any one of the above.

[0045] The beneficial effects of the robot positioning method and the electronic device provided by the present invention are as follows: By combining the fused pose theory parameters and the rotation angle measurement parameters, the errors of pure rotation angle measurement or pure theoretical calculation are reduced, and the theoretical accuracy and measurement real-time performance are fully utilized to obtain accurate fused pose parameters. Further, by combining the fused pose parameters and the position dynamic parameters, both the pure position measurement error is reduced, and the dynamic position of the robot in the actual environment is considered, significantly improving the positioning accuracy. Through the fusion of multi-dimensional information, it helps to reduce the errors and uncertainties that may be brought by a single information source, helps to correct the positioning result in real time, improves the robustness and accuracy of the positioning, and ensures that the robot can achieve precise positioning in various scenarios, solving the technical problem that the existing robot spatial positioning methods have room for improvement in accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following will briefly introduce the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0047] Figure 1 A flowchart of a robot positioning method provided by an embodiment of the present invention;

[0048] Figure 2 A scanning schematic diagram of a lidar;

[0049] Figure 3 A schematic diagram of a UWB layout model and a positioning principle;

[0050] Figure 4 Another flowchart of a robot positioning method provided by an embodiment of the present invention;

[0051] Figure 5 A flowchart of step S140 in an embodiment of the present invention;

[0052] Figure 6 A mapping schematic diagram of Lie groups and Lie algebras;

[0053] Figure 7 For the Lie algebra matrix A s A schematic diagram of the multiplication operation;

[0054] Figure 8 A flowchart of step S200 in an embodiment of the present invention;

[0055] Figure 9 Another flowchart of a robot positioning method provided by an embodiment of the present invention;

[0056] Figure 10 Another flowchart of the robot positioning method provided by the embodiment of the present invention;

[0057] Figure 11 The flowchart of step S411 in the embodiment of the present invention;

[0058] Figure 12 The flowchart of step S412 in the embodiment of the present invention;

[0059] Figure 13 The fused pose parameter C in the embodiment of the present invention t The schematic diagram of the relationship with time;

[0060] Figure 14 The structural diagram of the electronic device provided by the embodiment of the present invention. Detailed implementation manners

[0061] The embodiments of the present invention will be described in detail below. The examples of the embodiments are shown in the drawings, where the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the drawings are exemplary and are intended to explain the present invention, but should not be construed as limiting the present invention.

[0062] Referring to "one embodiment" or "embodiments" throughout the specification means that the specific features, structures, or characteristics described in connection with the embodiments are included in at least one embodiment of the present application. Thus, the phrases "in one embodiment" or "in some embodiments" appearing throughout the specification do not necessarily refer to the same embodiment. In addition, in one or more embodiments, the specific features, structures, or characteristics may be combined in any suitable manner.

[0063] In the description of the present invention, it should be understood that the orientation or positional relationship indicated by the terms "length", "width", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and thus should not be construed as limiting the present invention.

[0064] In addition, the terms "first" and "second" are only used for descriptive purposes and should not be construed as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, the features defined with "first" and "second" may explicitly or implicitly include one or more of such features.

[0065] In the present invention, unless otherwise clearly specified or limited, terms such as "installation", "connection", "linkage", "fixation", etc. shall be understood in a broad sense. For example, it may be a fixed connection, a detachable connection, or an integral one; it may be a mechanical connection or an electrical connection; it may be a direct connection or an indirect connection through an intermediate medium, and it may be the communication inside two components or the interaction relationship between two components. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0066] In a first aspect, in combination with Figure 1 , the present application provides a robot positioning method, which includes the following steps:

[0067] S100: Obtain the theoretical pose parameters A of the robot t .

[0068] S200: Obtain the angular measurement parameters B of the robot t .

[0069] S300: Fuse the theoretical pose parameters A t and the angular measurement parameters B t to obtain the fused pose parameters C of the robot t . Based on this, by combining the fused theoretical pose parameters A t and the angular measurement parameters B t , the error of pure angular measurement or pure theoretical calculation is reduced, the theoretical accuracy and measurement real-time performance are fully utilized, and the accurate fused pose parameters C t are obtained.

[0070] S400: Obtain the position dynamic parameters D of the robot t .

[0071] S500: Fuse the fused pose parameters C t and the position dynamic parameters D t to obtain the fused dynamic pose NL of the robot t . Based on this, by combining the fused pose parameters C t and the position dynamic parameters D t , both the pure position measurement error is reduced, and the dynamic position of the robot in the actual environment is considered, significantly improving the positioning accuracy. Through the fusion of multi-dimensional information, it helps to reduce the errors and uncertainties that may be brought by a single information source, helps to correct the positioning result in real time, improves the robustness and accuracy of the positioning, and ensures that the robot can achieve precise positioning in various scenarios.

[0072] It should be noted that existing real-time dynamic pose measurement technologies can directly measure the dynamic pose of a robot, such as Wi-Fi technology, Pedestrian Dead Reckoning (PDR), Ultra WideBand (UWB), machine vision, lidar, and Inertial Measurement Unit (IMU). However, when the inventor conducted robot positioning tests, it was found that the above real-time dynamic pose measurement technologies all had inevitable drawbacks.

[0073] For example, combined with Figure 2 , existing lidar can obtain extremely high angular, distance, and velocity resolutions, has good concealment and strong anti-active interference capabilities, but has a high cost and poor penetration, that is, it cannot measure the robot located in the occluded area. Combined with Figure 3 , existing UWB has a unique pulse communication mechanism. By arranging base stations (TDoA Anchor) at four angles indoors, the four base stations cooperate to achieve the pose of the robot (tag) to be located, and has significant advantages in anti-multipath interference, and can obtain high-cost performance, high stability, and high-precision wireless ranging; however, its cost is high and the coverage range is small, and it cannot measure the pose and is not applicable to the robot pose positioning in a workshop or office building. In addition, in the prior art, the Wi-Fi positioning technology has low accuracy, and the system construction is cumbersome and unstable. PDR has cumulative errors. IMU has the advantages of high accuracy, high speed, and no need for satellite signals, but has obvious cumulative errors.

[0074] In some embodiments, the robot is an AGV, a sweeping robot, or a multi-axis robot. The fused dynamic pose NL t of the robot can be represented by 3D coordinates, Euler angles, quaternions, rotation matrices, homogeneous transformation matrices, or Lie algebras, which is not uniquely limited here.

[0075] Specifically, the AGV and the sweeping robot move and rotate on a plane. For example, their positions are located by two-dimensional coordinates (x, y) on the plane, and their rotation angles are located by the rotation angle θz around the vertical axis. That is, only three parameters (θz, x, y) can represent their fused dynamic pose NL t, of course, it can also be represented by two parameters, and the parameter values of other degrees of freedom are 0. For example, (x, y, 0, 0, 0, θz) or (x, y, 0, θz, 0, 0). The multi-axis robot includes multiple rigid body arms that are sequentially connected by joints. For example, each rigid body arm requires six parameters (θx, θy, θz, x, y, z) to represent its pose, namely three position parameters (x, y, z) and three rotation parameters (θx, θy, θz). x represents the position of the rigid body arm in the X-axis direction of space. y represents the position of the rigid body arm in the Y-axis direction of space. z represents the position of the rigid body arm in the Z-axis direction of space. θx represents the rotation angle around the X-axis. θy represents the rotation angle around the Y-axis. θz represents the rotation angle around the Z-axis.

[0076] In some embodiments, the fused dynamic pose NL t is represented by an N*M matrix, where both N and M are positive integers, N≥1, 6≥M≥2, N represents the degree-of-freedom node, and M represents the position and pose parameters corresponding to the degree-of-freedom node. For AGVs and floor cleaning robots, N is 1 and M is 3. For multi-axis robots, N is the number of joints and M is 6.

[0077] When t = 0, that is, at the initial moment, the fused dynamic pose NL t is the initial value, and no multi-information dimension fusion is required. When t≥1, the fused dynamic pose NL t is fused and calculated using the above method.

[0078] In some embodiments, in combination with Figure 4 , step S100 includes the following steps:

[0079] S110: Obtain the fused pose parameter NL t-1 .

[0080] S120: Obtain the position dynamic parameter D t-1 .

[0081] S130: Obtain the velocity dynamic parameter w' t .

[0082] S140: Based on the pose change model of Lie group theory, obtain the pose theoretical parameter A t .

[0083] Compared with other pose representation methods, for example, the rotation matrix representation supports transitivity and undergoes n different rotations. However, it has more parameters, is difficult to implement matrix interpolation, multiplication operations lead to increased computational complexity and drift accumulation, and the constraints of the unit orthogonal matrix are not convenient for optimization; the Euler angle representation enables intuitive images, but different rotation orders lead to different results, and there is a gimbal lock problem; although the quaternion representation can avoid the gimbal lock problem, the calculation is cumbersome and cannot meet the requirements of real-time dynamic positioning. The Lie group theory adopted in this application can provide a mathematical description of rigid body transformations (including translation and rotation), can accurately represent the pose changes of the robot, break through the theoretical bottleneck of pose representation, has lower computational complexity and higher computational efficiency when dealing with complex motions, meets the requirements of real-time dynamic positioning, and comprehensively considers the fused pose parameter NL at the previous moment t-1 , the position dynamic parameter D at the previous moment t-1 and the velocity dynamic parameter w’ at the current moment t , so as to more accurately provide the pose theory parameter A at the current moment t .

[0084] In one embodiment, in combination with Figure 5 , step S140 includes the following steps:

[0085] S141: Obtain the position representation parameter used by the sensor to represent the position of the robot.

[0086] Optionally, step S141 further includes: completing the autonomous navigation of the robot based on the path planner. For example, completing the autonomous navigation according to the layout path planner based on the DWA algorithm and the global path planner based on the Dijkstra algorithm.

[0087] S142: Establish the Lie algebra matrix model of the robot.

[0088] S143: Refer to Figure 6 , and establish the Lie group matrix of the pose change at each position through exponential mapping.

[0089] S144: Based on the Lie group matrix, deduce the pose theory parameter A at the current moment t .

[0090] Based on this, according to the position representation parameter provided by the sensor, a general Lie algebra matrix model is constructed. When the dynamic position parameter and the measurement parameter are obtained, the pose theory parameter A at the current moment can be directly obtained through exponential mapping t . In this way, through the conversion relationship between the Lie group and the Lie algebra, the pose theory speculation problem is transformed into an unconstrained optimization problem, so as to use the existing optimization algorithms to quickly solve. Among them, the dynamic position parameter can be based on the fused pose parameter NL at the previous moment t-1 , the position dynamic parameter D at the previous momentt-1 and the speed dynamic parameter w’ at this moment t is calculated.

[0091] Specifically, based on Lie group theory, a = (l, m, n) ∈ so(3); a represents the attitude parameter of the robot's rotation axis, and d represents the position parameter of the robot. so(3) represents Lie algebra, and SO(3) represents Lie group. [a x represents the matrix form of Lie algebra. a = (l, m, n) represents the unit vector form of Lie algebra. Since the rotation axis is always numerically upward, a = (0, 0, 1). Assume the initial state of the robot is NL0, and the state after one movement is NL1. NL t is a six-dimensional column vector including at least one, and the six-dimensional column vector contains the attitude parameter and position parameter of the robot.

[0092] In this application, R represents the Lie group rotation matrix for rotating an angle θ around the robot's rotation axis a. This matrix R can be obtained through the Rodriguez formula (see Equation (1)), and the pose theory parameter A t is obtained through Equation (2).

[0093]

[0094] where the Lie algebra matrix I is the identity matrix.

[0095] A t = NA t-1 (2)

[0096] where A = [d × .[[]END]]

[0097] Optionally, in Equation (1), combined with Figure 7 , the multiplication operation of the Lie algebra matrix A s adopts the following steps:

[0098] S1441: Read all elements of the first Lie algebra matrix A s into the first vector memory in sequence along the row direction. Assume then after executing step S1441, the first vector memory represents the vector i = (a11, a12, a13, a21, a22, a23, a31, a32, a33).

[0099] S1442: Read all elements of the second Lie algebra matrix A s into the second vector memory in sequence along the column direction. That is, the second vector memory represents the vector j = (a11, a21, a31, a12, a22, a32, a13, a23, a33).

[0100] S1443: Read the elements stored in the first vector memory segment by segment using the first calculation window g, that is, obtain three vectors g1, g2, and g3 respectively. Read the elements stored in the second vector memory segment by segment using the second calculation window h, that is, obtain three vectors h1, h2, and h3 respectively.

[0101] Among them, the length of the first calculation window g is the length of the row vector of the Lie algebra matrix A s and the length of the second calculation window h is the length of the column vector of the Lie algebra matrix A s In this embodiment, the lengths of both the first calculation window g and the second calculation window h are 3.

[0102] S1444: Output the multiplication result of two Lie algebra matrices A in matrix form according to the vector dot product results of the first calculation window g and the second calculation window h. That is, calculate the vector dot products g1·h1, g1·h2, g1·h3,..., g3·h1, g3·h2, g3·h3 respectively, then s Based on this, perform vector dot products through the first calculation window g and the second calculation window h respectively, and then combine the corresponding multiplication operation results of the two Lie algebra matrices A according to the vector dot products, making full use of the space of the first vector memory and the second vector memory, thereby improving the multiplication operation efficiency of the two Lie algebra matrices A

[0103] s s

[0104] In one of the embodiments, in combination with Figure 8 , step S200 includes the following steps:

[0105] S210: Obtain the first pose matrix of the robot using the lidar positioning system.

[0106] S220: Obtain the second pose matrix of the robot using the ultra-wideband technology positioning system.

[0107] S230: Use the Kalman filter to fuse the first pose matrix and the second pose matrix to obtain the rotation angle measurement parameter B t .

[0108] Based on this, since the lidar positioning system has a limited range for detecting waves to search for and capture targets, and the ultra-wideband technology positioning system is easily affected by non-line-of-sight propagation, using the two in combination can make full use of their respective advantages, make up for each other's deficiencies, and improve the navigation accuracy.

[0109] Among them, the information fusion algorithm not only needs to cope with the diversity and complexity of data, but also needs to improve real-time performance and accuracy. The fusion algorithms can be roughly divided into two categories: random algorithms and artificial intelligence algorithms. Random algorithms include weighted average method, Kalman filter, multi-Bayesian estimation method, etc.; artificial intelligence algorithms include fuzzy logic, neural network, etc. Therefore, when processing multi-sensor information in robot mobile positioning, selecting an efficient and accurate information fusion algorithm is the key. The Kalman filter method is intuitive, has strong fault tolerance and good versatility. Although it belongs to data-level fusion, it is crucial for sensor fusion. In this embodiment, the Kalman filter algorithm is adopted to reduce the measurement error of the dynamic attitude multi-dimensional information fusion system.

[0110] In some embodiments, in combination with Figure 9 , step S400 includes the following steps:

[0111] S410: Obtain the first measured position parameter E1 of the first position t .

[0112] S420: Obtain the second measured position parameter E2 of the first position t .

[0113] S430: According to the first measured position parameter E1 t and the second measured position parameter E2 t , calculate the fusion position dynamic parameter D t . Optionally, the fusion algorithm can be a weighted average method, a Kalman filter, a multi-Bayesian estimation method, fuzzy logic, a neural network, etc., which is not uniquely limited here. Preferably, the Kalman filter is used to fuse the first measured position parameter E1 t and the second measured position parameter E2 t .

[0114] Based on this, by fusing two or more measured position parameters from different sensors, they can complement and verify each other, thereby reducing the errors that may be generated by a single sensor and improving the measurement accuracy of the position dynamic parameter D t .

[0115] In this embodiment, through the first measured position parameter E1 t and the second measured position parameter E2 t , these two measured position parameters are used to calculate the fusion position dynamic parameter D t . The required measured position parameters are few, and the corresponding required measurement hardware is few, which is conducive to reducing the measurement cost. At the same time, the two measured position parameters can overcome the errors of single measurement. It can be understood that the fusion position dynamic parameter D t can be calculated through more than three measured position parameters.

[0116] In one embodiment, in any dimension, the first measured position parameter E1 t has a variance ≤ 4 mm, and the selected first measured position parameter E1 t has high reliability, thereby improving the positioning accuracy. In other words, the data of the first measured position parameter E1 t on the X-axis, Y-axis or Z-axis is continuous and has a small variance, thereby excluding discrete error measurement values.

[0117] In one embodiment, in any dimension, the second measured position parameter E2 t has a variance ≤ 16 mm, and the selected second measured position parameter E2 t has high reliability, thereby improving the positioning accuracy. In other words, the data of the second measured position parameter E2 t on the X-axis, Y-axis or Z-axis is continuous and has a small variance, thereby excluding discrete error measurement values.

[0118] In one embodiment, the first measured position parameter E1 t and the second measured position parameter E2 t are uncorrelated, and their covariance is 0. Two uncorrelated parameters mean that the information they provide is independent, without overlap or redundancy. On the one hand, it simplifies the calculation in the fusion process and may improve the efficiency and accuracy of the algorithm. On the other hand, it reduces error propagation and helps to reduce the errors that may be generated by a single sensor.

[0119] In one embodiment, in combination with Figure 10 , one sensor is used to obtain the first sensing parameter F1 t , and the first sensing parameter F1 t is used to calculate the first measured position parameter E1 t . One sensor is used to obtain the second sensing parameter F2 t , which is used to calculate the second measured position parameter E2 t . Among them, the measurement data structures of different sensors are different and need to be converted into measured position parameters for easy fusion calculation.

[0120] In another embodiment, the first measured position parameter E1 t is measured by at least two sensors. By fusing data from different sensors, the errors that may exist in a single sensor can be eliminated or significantly reduced. Each sensor may be affected by factors such as the environment, manufacturing accuracy, aging, or operating conditions and generate errors. Using multiple sensors and fusing their data can form a kind of redundancy, thereby reducing the impact of these errors on the final measurement result and further improving the measurement accuracy.

[0121] For example, step S410 includes the following steps:

[0122] S411: Obtain the rotation angle parameter of the first position using the first sensor.

[0123] S412: Obtain the distance parameter of the first position using the second sensor.

[0124] S413: Fuse the rotation angle parameter and the distance parameter to obtain the first measured position parameter E1 t 。

[0125] Based on this, referring to spherical coordinates, by obtaining the rotation angle parameter and the distance parameter, the spatial coordinate position of the first position can be calculated, that is, the first measured position parameter E1 t 。

[0126] In a possible example, the first sensor is fixedly installed at the first position at a preset angle, and the first sensor is a sensor board integrated with a geomagnetometer and an accelerometer.

[0127] Combined Figure 11 , step S411 includes the following steps:

[0128] S4111: The geomagnetometer obtains the magnetic field intensity data of the first position.

[0129] S4112: The accelerometer obtains the acceleration data of the first position.

[0130] S4113: Based on the acceleration data and the magnetic field intensity data, obtain the yaw angle of the sensor board.

[0131] S4114: Based on the angle between the yaw angle and the direction of the geographic north pole, obtain the rotation angle parameter of the first position.

[0132] Among them, the dotted line directions of the respective axial lines of the geomagnetometer and the accelerometer both point to the same direction, so as to facilitate the accuracy of data when calculating the yaw angle of the sensor board, facilitate data fusion, and simplify the calculation. The geomagnetometer and the accelerometer can accurately collect the magnetic field intensity data and acceleration data during the movement of the first position, and then a reliable and accurate yaw angle can be obtained, so as to obtain an accurate rotation angle parameter based on the yaw angle.

[0133] In this embodiment, when the first sensor is fixedly installed at the first position at a preset angle, in a possible example, the direction indicated by the axial dotted line of the first sensor coincides with the direction of the geographic north pole. At this time, the angle between the yaw angle and the direction of the geographic north pole is the rotation angle of the first position. In another possible example, the direction indicated by the axial dotted line of the first sensor does not coincide with the direction of the geographic north pole, and there is a first angle between the direction indicated by the axial dotted line of the first sensor and the direction of the geographic north pole, then the sum of the first angle and the yaw angle is the rotation angle of the first position.

[0134] Optionally, based on the acceleration and magnetic field intensity corresponding to the first position at multiple moments respectively, the first sensor determines the sub-yaw angles corresponding to the first position at multiple moments. Step S4113 includes: First, according to the calculation formulas of the pitch angle and roll angle, and combining the acceleration data of the first sensor at different time points, calculate the pitch angle and roll angle of the first sensor at these time points. Second, use the calculated pitch angle and roll angle to convert the magnetic field intensity data of the first sensor at different time points from the sensor coordinate system to the geographic coordinate system, so as to obtain the converted magnetic field intensity value. Third, based on the calculation formula of the yaw angle and the converted magnetic field intensity data, determine the sub-yaw angles of the first sensor at different time points.

[0135] For example, first, the acceleration obtained by the accelerometer at the t-th moment is a = (a x , a y , a z ). Substitute the acceleration a = (a x , a y , a z ) into the pitch angle formula and roll angle formula respectively, and the pitch angle θ corresponding to the first position at the t-th moment is obtained as θ = arctan(a x / (a y 2 + a z 2 ) 1 / 2 ), and the roll angle φ corresponding to the first position at the t-th moment is φ = arctan(a y / a z ). Secondly, the magnetic field intensity obtained by the geomagnetometer is m = (m x , m y , m z ). Convert the sensor coordinate system to the geographic coordinate system, that is, m' = H × m, and the converted magnetic field intensity m' = (m' x , m' y , m' z ) is obtained, where the specific form of the rotation matrix H is Finally, substitute m' = (m' x , m' y , m' z ) into the yaw angle formula, and the sub-yaw angle ψ corresponding to the first position at the t-th moment is obtained as ψ = arctan2(m' y , / m' x ).

[0136] In a possible example, the second sensor is a binocular laser sensor. The binocular laser sensor includes a left laser and a right laser, and the binocular laser sensor is rotatably mounted on the robot.

[0137] Combined with Figure 12 , step S412 includes the following steps:

[0138] S4121: Obtain the left distance from the left laser to the first position.

[0139] S4122: Obtain the right distance from the right laser to the first position.

[0140] S4123: When the left distance and the right distance are equal, calculate the distance parameter according to the interpupillary distance b of the binocular laser sensor and the right distance. When the left distance and the right distance are equal, the binocular laser sensor is facing the first position directly, and the calculated distance parameter is more accurate at this time.

[0141] S4124: When the left distance and the right distance are not equal, rotate the binocular laser sensor until the left distance and the right distance are equal, and then calculate the distance parameter according to the interpupillary distance b of the binocular laser sensor and the right distance.

[0142] Based on this, assuming that both the left distance and the right distance are equal to L, use the symmetry of the isosceles triangle to reduce the amount of calculation, and the distance parameter In dynamic measurement, due to various interference factors (such as noise, vibration, etc.), the left distance and the right distance may fluctuate. However, since the left distance and the right distance are equal, the fluctuation errors received by the two lasers cancel each other out, and the binocular laser sensor is still facing the target point directly, thus ensuring the stability and reliability of the calculation result.

[0143] Combined with Figure 10 , a specific calculation process for solving the fused position dynamic parameter D t and the second measured position parameter E2 t is introduced in detail. t of a specific calculation process.

[0144] Assume t = 3, E13 = [0, 0, 0, 30, 35, 0], E23 = [0, 0, 0, 32, 37, 0]. For the X coordinate, assume the fused position dynamic parameter D3 = E13 + K(E13 - E23). Take the variance on both sides D(D3) = D(E13 + K(E13 - E23)), so D(D3) = (1 - K) 2 D(E13) + K 2 D(E23). Take the derivative of the coefficient K to get the point where the derivative function is zero, and obtain K = 2. Substitute it back into the original formula to get D(D t ) = 3.2mm, D(D t ) < D(E1 t ) < D(E1 t ), and the same is true for the Y coordinate, thus obtaining D t = E1 t + 0.2(E1t -E2 t ),D3 = E13 + 0.2(E13 - E23) = [0,0,0,30.4,35.4,0]. The dynamic parameters D of the fusion position at other times t The results can be seen in Table 1.

[0145]

[0146] Table 1 Dynamic parameters D of the fusion position t A specific calculation result

[0147] Combined with Figure 10 and Figure 13 , a specific calculation process of the fusion pose parameter Ct and the position dynamic parameter Dt is introduced in detail to obtain the fusion dynamic pose NLt of the robot.

[0148] First, at times t0 to t5, the dynamic parameters D of the fusion position are obtained through actual measurement fusion t (the results are from Table 1) and the rotation angle measurement parameter B t , the dynamic parameters D of the fusion position t are D0 = [0,0,0,0,0,0] mm, D1 = [0,0,0,10.4,13.4,0] mm, D2 = [0,0,0,20.6,26.4,0] mm, D3 = [0,0,0,30.4,35.4,0] mm, D4 = [0,0,0,40.6,39,0] mm, D5 = [0,0,0,51,58.4,0] mm; the rotation angle measurement parameter B t are B0 = [cos(0.01π), sin(0.01π),0,0,0,0], B1 = [cos(0.11π), sin(0.11π),0,0,0,0], B2 = [cos(0.13π), sin(0.13π),0,0,0,0], B3 = [cos(0.22π),sin(0.22π),0,0,0,0], B4 = [cos(0.34π),sin(0.34π),0,0,0,0], B5 = [cos(0.25π),sin(0.25π),0,0,0,0].

[0149] Second, according to a = (l,m,n) representing the attitude vector, the theoretical pose parameters A corresponding to times t0 to t5 are calculated t, since the pose theory parameter A0 = the rotation angle measurement parameter B0 = [0.9995, 0.0314, 0, 0, 0, 0], the fused pose parameter C0 = [0.9995, 0.0314, 0, 0, 0, 0] is obtained after fusion. Combining with D0 = [0, 0, 0, 0, 0, 0] mm, the fused dynamic pose NL0 = [0.9995, 0.0314, 0, 0, 0, 0] is further obtained, and the pose dynamic speed parameter ω'0 = 0.2560 rad / s.

[0150] When t = 1, the rotation angle measurement parameter B1 = [0.945, 0.335, 0, 0, 0, 0], the angular velocity ω1 = 0.3723 rad / s, and the average pose dynamic speed parameter ω'1 from t = 0 to t = 1 is ω'1=(0.2560 + 0.3723) / 2 = 0.314159 rad / s, that is, the angle increment is θ1 = ω t 'Δt = 0.314159 rad. In this case, since the rotation axis is always numerically upward, so a = (001), R represents the rotation Lie group matrix that rotates by θ angle around the axis a, R1 = e θ1As = I + sinθ1A s +(1 - cosθ1)A s A s ∈SO(3), The pose theory parameter A1 = R1*A0 = (0.9409, 0.3387, 0, 0, 0, 0), the fused pose parameter C1 = f(A1, B1) = (0.94418, 0.33574, 0, 0, 0, 0), the fused position dynamic parameter D1 = (0, 0, 0, 10.4, 13.4, 0), and the fused dynamic pose NL0 = [0.94418, 0.33574, 0, 10.4, 13.4, 0] is further obtained.

[0151] When t = 1, the rotation angle measurement parameter B1 = [0.9176, 0.3971, 0, 0, 0, 0], the angular velocity ω2 = -0.2467 rad / s, the average pose dynamic speed parameter ω'2 from t = 1 to t = 2 is 0.06235 rad / s, and the angle increment is θ2 = ω'2Δt = 0.06235 rad, The pose theory parameter A2 = R2*A1 = (0.9214, 0.3939, 0, 0, 0, 0), the fused pose parameter C2 = f(A2, B2) = (0.91836, 0.39646, 0, 0, 0, 0), the fused position dynamic parameter D1 = (0, 0, 0, 20.6, 26.4, 0), and the fused dynamic pose NL0 = [0.9214, 0.3939, 0, 20.6, 26.4, 0] is further obtained.

[0152] Thirdly, similarly, the fusion dynamic pose NL at other moments can be calculated. t .

[0153] In a second aspect, in combination with Figure 14 , the present application provides an electronic device 20, including a memory 22, a processor 21, and a computer program 23 stored in the memory 22 and operable on the processor 21. When the processor 21 executes the computer program 23, each step in the robot positioning method as described in any one of the above is implemented.

[0154] Among them, the electronic device 20 can be a vehicle control device, a mobile phone, a tablet computer, a wearable device, an augmented reality (AR) / virtual reality (VR) device, a laptop computer, an ultra-mobile personal computer (UMPC), a netbook, a personal digital assistant (PDA), etc., and is not uniquely limited herein.

[0155] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, and improvements made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A robot positioning method, characterized in that: The method comprises the following steps: S100: Obtain theoretical posture parameters of the robot; S200: Obtaining the rotation angle measurement parameters of the robot; S300: fusing the theoretical posture parameters and the rotation angle measurement parameters to obtain fused posture parameters of the robot; S400: Obtain the position dynamic parameters of the robot; S500: Fusing the fused posture parameter and the position dynamic parameter to obtain a fused dynamic posture of the robot.

2. The robot positioning method according to claim 1, characterized in that: Step S100 includes the following steps: S110: Obtain the fusion posture parameters of the previous moment; S120: Obtaining the position dynamic parameters at the last moment; S130: Obtaining speed dynamic parameters at this moment; S140: Based on the posture change model of Lie group theory, the posture theoretical parameters at this moment are obtained.

3. The robot positioning method according to claim 2, characterized in that: Step S140 includes the following steps: S141: Obtaining position representation parameters of the sensor used to represent the position of the robot; S142: Establish the Lie algebra matrix model of the robot; S143: Establishing a Lie group matrix of posture change at each position through exponential mapping; S144: Based on the Lie group matrix, derive the theoretical parameters of the posture at this moment.

4. The robot positioning method according to claim 1, characterized in that: Step S200 includes the following steps: S210: Obtaining the first position matrix of the robot using a laser radar positioning system; S220: using the ultra-wideband technology positioning system to obtain the second posture matrix of the robot; S230: Using Kalman filtering to fuse the first posture matrix and the second posture matrix to obtain the rotation angle measurement parameter.

5. The robot positioning method according to claim 1, characterized in that: Step S400 includes the following steps: S410: Acquire a first measured position parameter of a first position; S420: Acquire a second measured position parameter of the first position; S430: Calculate and fuse the position dynamic parameter according to the first measured position parameter and the second measured position parameter.

6. The robot positioning method according to claim 5, characterized in that: The first measured position parameter and the second measured position parameter are unrelated, and their covariance is zero.

7. The robot positioning method according to claim 5, characterized in that: Step S410 includes the following steps: S411: Acquire a rotation angle parameter of a first position using a first sensor; S412: using a second sensor to obtain a distance parameter of the first position; S413: Fusing the rotation angle parameter and the distance parameter to obtain the first measured position parameter.

8. The robot positioning method according to claim 7, characterized in that: The first sensor is fixedly installed at the first position at a preset angle, and the first sensor is a sensor board integrated with a magnetometer and an accelerometer. Step S411 includes the following steps: S4111: Collecting magnetic field intensity data of the first position by the geomagnetic meter; S4112: Collecting acceleration data of the first position by using the accelerometer; S4113: Determine the yaw angle of the sensor board based on the acceleration data and the magnetic field strength data; S4114: Determine a rotation angle parameter of the first position based on an angle between the yaw angle and the direction of the geographic North Pole.

9. The robot positioning method according to any one of claims 1 to 8, characterized in that: The robot is an AGV, a sweeping robot or a multi-axis robot; And / or, the fused dynamic posture is represented by an N*M matrix, wherein N and M are both positive integers, N≥1, 6≥M≥2, N represents a degree of freedom node, and M represents a position posture parameter corresponding to the degree of freedom node.

10. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, each step of the robot positioning method according to any one of claims 1 to 9 is implemented.

Images