CN112833876B - Multi-robot cooperative positioning method integrating odometer and UWB - Google Patents

Abstract

The invention discloses a multi-robot cooperative positioning method integrating odometer and UWB, comprising the following steps: S1: using the odometer and a UWB data acquisition module to separately collect the pose information of the robot and the distance information between multiple groups of UWB nodes; S2: Based on the collected distance information between multiple groups of UWB nodes, the multi-robot cooperative positioning is realized through a nonlinear optimization algorithm; S3: Based on the multi-robot positioning information after nonlinear optimization, the information provided by the robot odometer is fused to construct a pose graph , and optimize it to achieve precise positioning of multi-robot collaboration. UWB collects distance information between robots, and realizes cooperative positioning between multiple robots through nonlinear optimization. The multi-robot cooperative positioning accuracy is higher. The problem of poor positioning accuracy of multi-robot cooperative positioning is solved.

Description

A multi-robot cooperative localization method integrating odometer and UWB

technical field

The invention belongs to the technical field of multi-mobile robot cooperative positioning, and in particular relates to a multi-robot cooperative positioning method integrating odometer and UWB.

Background technique

In recent years, mobile robotics has played an important role in many fields such as industry, medical and service, and it has also been well used in harmful and dangerous situations such as defense and space exploration.

In the research field of mobile robots, localization has always been a hot research topic, it provides real-time precise position for the robot, and these are the prerequisites for the robot to perform path planning and path tracking, so it occupies a very important position in the research of mobile robots. important location.

UWB positioning technology has great advantages in real-time performance and bandwidth, and has stronger anti-interference performance, low cost, low power consumption, and fast data transmission. UWB can measure distance in complex and changeable environment, and improve the accuracy and efficiency of distance measurement. However, the distance measured by UWB is also affected by environmental factors and robot angles. By measuring the distance between multiple robots and averaging the average value of multiple forward and reverse measurements, the impact on the offset can be reduced. Thereby, the distance measurement error is reduced, and a more accurate distance between robots is collected. Since UWB can only provide distance information and cannot complete the pose estimation of mobile robots, cooperative positioning between multiple robots can be achieved through nonlinear optimization.

The odometer provides precise pose changes of the robot in a short time. Although the odometer has accumulated errors for a long time, the odometer can provide precise positioning for the robot in a short time, and the distance information between multiple robots provided by UWB can be used. Correct these errors and improve the positioning accuracy of the robot. Through the pose information provided by multiple robot odometers, combined with the positioning information between multiple robots after nonlinear optimization, and further optimized by the graph optimization algorithm, higher-precision positioning can be obtained. Therefore, the present invention chooses to fuse odometer data and UWB data to realize cooperative precise positioning among multiple mobile robots.

SUMMARY OF THE INVENTION

The purpose of the present invention is to solve the problems of poor cooperative positioning accuracy and low precision of existing multi-robots in complex indoor environments, and proposes a multi-robot cooperative positioning method integrating odometer and UWB.

The technical scheme of the present invention is: a multi-robot cooperative positioning method integrating odometer and UWB comprises the following steps:

S1: Collect the pose information of the robot and the distance information between multiple groups of UWB nodes through the odometer and the UWB data acquisition module respectively;

S2: Based on the collected distance information between multiple groups of UWB nodes, the cooperative positioning of the robot is realized through a nonlinear optimization algorithm;

S3: Based on the multi-robot positioning information after nonlinear optimization, the pose information provided by the robot odometer is fused, the pose graph is constructed, and the optimization is carried out to realize the precise positioning of the multi-robot cooperation.

The beneficial effects of the present invention are as follows: the multi-robot cooperative positioning method of the present invention utilizes odometer and UWB data acquisition module for collection, and is based on a nonlinear optimized multi-robot cooperative localization algorithm and a pose graph optimization algorithm fused with odometry information. Using the odometer and UWB device in the environment as the perception unit, the collection of UWB data and odometer data is realized. UWB collects distance information between robots, and realizes cooperative positioning between multiple robots through nonlinear optimization. The multi-robot cooperative positioning accuracy is higher. The problem of poor positioning accuracy of multi-robot cooperative positioning is solved.

Further, in step S1, the method for collecting the pose information of the robot by using the odometer is as follows: building an encoder on the robot for collection, and obtaining the odometer data;

The method of collecting the distance information between multiple groups of UWB nodes through the UWB data collection module is as follows: carrying UWB tags on different positions of each robot, and collecting the distance information between multiple groups of UWB nodes.

The beneficial effects of the above-mentioned further solutions are: in the present invention, UWB has great advantages in real-time performance and bandwidth, etc., and has stronger anti-interference performance. By carrying UWB tags at different positions on the robot, the UWB node of each robot will calculate the distance between UWB nodes of other robots, so that each robot can obtain multiple sets of distance data relative to other robots.

Further, step S2 includes the following sub-steps:

S21: According to the distance information between multiple groups of UWB nodes, calculate the residual function of the distance measurement value and the calculated value between robot i and robot j;

S22: According to the residual function of the measured value and the calculated value of the distance between robot i and robot j, use the Levenberg-Marquardt method to iterate to obtain the optimal pose of the robot to realize multi-robot cooperative positioning.

The beneficial effects of the above-mentioned further scheme are: in the present invention, although UWB has the advantages of strong anti-interference performance and high ranging accuracy, only distance information can be obtained through UWB, and the pose estimation of the mobile robot cannot be completed, and the odometer can be used. Accurate pose information is measured in a short time, and the combination of the two can achieve precise positioning. This patent adopts a nonlinear optimization algorithm to obtain the relative pose estimation between robots on the basis of UWB distance measurement data.

Further, in step S21, the expression of the residual function of the measured value and the calculated value of the distance between robot i and robot j is:

表示机器人i和机器人j之间残差函数值最小的机器人位姿，argmin(·)表示能够使误差值最小的x的值，f(x)表示残差函数，x＝(x,y,θ)表示机器人的位姿，x表示机器人i相对于机器人j的横坐标，y表示机器人i相对于机器人j的纵坐标，θ表示机器人i相对于机器人j的方向角度，K表示机器人i上的UWB节点的数量，L表示机器人j上的UWB节点的数量，

表示在t时刻机器人i上UWB节点k和机器人j上UWB节点l之间的实际测量值，d(·)表示给定机器人i与机器人j相对位姿的情况下计算机器人i上UWB节点k和机器人j上UWB节点l之间的距离运算，

表示机器人i上的UWB节点k的相对位置，

表示机器人j上的UWB节点l的相对位置，k∈[1,…,K]，l∈[1，…,L]，i∈[1,…,N],j∈[1，…,N]，N代表机器人的数量。in,

represents the robot pose with the smallest residual function value between robot i and robot j, argmin( ) represents the value of x that can minimize the error value, f(x) represents the residual function, x=(x, y, θ ) represents the pose of the robot, x represents the abscissa of the robot i relative to the robot j, y represents the ordinate of the robot i relative to the robot j, θ represents the direction angle of the robot i relative to the robot j, and K represents the UWB on the robot i the number of nodes, L represents the number of UWB nodes on robot j,

represents the actual measurement value between UWB node k on robot i and UWB node l on robot j at time t, d( ) represents the calculation of UWB node k and UWB node k on robot i given the relative pose of robot i and robot j The distance calculation between UWB nodes l on robot j,

represents the relative position of UWB node k on robot i,

Represents the relative position of UWB node l on robot j, k∈[1,…,K], l∈[1,…,L], i∈[1,…,N], j∈[1,…,N ], N represents the number of robots.

The beneficial effect of the above-mentioned further scheme is: in the present invention, the posture of the mobile robot is optimized by using multiple sets of UWB distance information, and the relative posture estimation between the robot i and the robot j can find the best through the minimization of the above equation. It is realized by posture configuration, where f(x) is equivalent to a residual function, which refers to the difference between the actual measured value and the calculated value, and x=(x, y, θ) with the smallest difference is the most good posture.

Further, step S22 includes the following sub-steps:

S221: According to the residual function, establish an approximate index evaluation model, and dynamically adjust the size of the trust region μ;

S222: Limit the step size Δx to the region with the trust radius μ, and calculate Δx;

S223: Perform k iterations in the region with the trust radius μ to obtain the optimal pose of the robot and realize multi-robot cooperative positioning.

The beneficial effects of the above-mentioned further solutions are: in the present invention, the traditional method needs to traverse the entire (x, y, θ) value range to obtain the best pose estimation, and the calculation efficiency is too low. The present patent uses the Levenberg-Marquardt method to improve the computational efficiency, and the optimization idea of the method is: in each iteration, the optimization range is first determined, and then the optimal point within the range is determined. That is, the step size Δx=(Δx, Δy, Δθ) ^T is limited to a region with a trust radius of μ, and then the optimal step size is found in this region. After k iterations, if ρ<0, it means the fitting error To the trend of rising rather than falling (opposite to the optimization target direction), x _k+1 = x _k should be set at this time, and μ=0.5μ can be set according to experience before continuing the iterative calculation. If ρ>0 indicates the same direction as the optimization target, then let x _k+1 =x _k +Δx _k , stop the iteration when Δx _k is small enough, and the pose obtained at this time is the best pose.

Further, in step S221, the method of performing k iterations in the region with the trust radius μ is: dynamically adjust the region with the trust radius μ by establishing an approximate index evaluation model, and the expression of the approximate index evaluation model is:

Among them, Δx represents the step size, J(x) represents the first derivative of f(x) with respect to x, f(x+Δx) represents the increment of f(x), and ρ represents the approximate index;

The method of dynamically adjusting the region of the trust radius μ according to the approximate index ρ is: if 0<ρ≤0.25, then reduce the trust radius μ by half, if ρ>0.75, then increase the trust radius μ by half, if 0.25<ρ≤0.75 , the trust radius μ is not adjusted;

Carry out k iterations in the region with the trust radius μ, if ρ<0, then let x _k+1 = x _k , and reduce the trust radius μ by half to continue the iteration, if ρ > 0, then let x _k+1 = x _k +Δx _k , when Δx _k is less than the set threshold, the initial optimal pose x _k+1 of the robot is obtained, where Δx _k represents the step size after k iterations, and x _k represents the pose of the previous iteration.

The beneficial effect of the above-mentioned further scheme is: in the present invention, in the process of updating and iterating, the range of the trust region μ is determined according to the difference between the approximate model and the actual function, if the difference is small, the range of the trust region is expanded; if the difference is large , narrow this range. In order to judge the quality of the difference between the approximate model and the actual function, an approximate index is set to judge the size of the model fitting difference, and the trust region radius is dynamically adjusted. In the expression, the numerator is the value by which the objective function actually changes, and the denominator is the value by which the approximate model changes. When performing regional adjustment, if ρ is too small, it means that the value of the approximate model change is larger than the actual change value, that is, the approximation effect is poor, and the confidence region needs to be narrowed; if ρ is too large, it means that the approximate change value is smaller than the actual change value. , it is necessary to expand μ; when ρ is close to 1, it is considered that the approximation effect is better, and there is no need to change the value of μ.

Further, in step S222, the calculation formula of step size Δx is:

(H(x)+λI)Δx=-J(x) ^T f(x)

Among them, H(x) represents the approximation of f(x) Hessian matrix, H(x)=J(x) ^T J(x), I represents the identity matrix, λ represents the coefficient factor, and J(x) represents f(x) ) with respect to the first derivative of x.

The beneficial effects of the above-mentioned further scheme are: in the present invention, a constrained optimization problem is transformed into an unconstrained optimization problem through a Lagrange multiplier in the Levenberg-Marquardt algorithm, and the unconstrained optimization equation Taylor Unfolding yields a linear equation that computes the increments. Among them, J(x) is the first derivative of f(x) with respect to x, which is the Jacobian matrix of L·K rows and 3 columns; λ is the coefficient factor, the purpose is to make the matrix (H+λI) positive definite, the initial value of λ The value is the maximum value of the diagonal elements of J(x) ^T J(x).

Further, in step S3, the method for constructing the pose graph is as follows: the pose of the robot is used as a vertex, and the constraint relationship between the poses at different times is used as an edge;

Based on the pose graph, the pose with the minimum constraints is calculated by the least squares method. The calculation formula is:

表示机器人i在t时刻的里程计数据，

表示机器人i在t+1时刻的里程计数据，

表示机器人i在t至t+1时刻的里程计数据，

表示机器人i在t至t+1时刻观察值的信息矩阵，

表示机器人i在t时刻的里程计约束，

表示机器人j在t时刻的里程计数据，

表示机器人i和机器人j之间残差函数值最小的机器人位姿，

表示机器人i和机器人j之间的位态约束，

是机器人i和机器人j之间在t时刻观察值的信息矩阵，

表示机器人j在t+1时刻的里程计数据，

代表机器人j在t至t+1时刻的里程计数据，

是机器人j在t时刻的里程计约束，

表示机器人j之间在t至t+1时刻观察值的信息矩阵，(i,j)∈[1,…,N]，N代表机器人的数量。Among them, T represents the duration,

represents the odometer data of robot i at time t,

represents the odometer data of robot i at time t+1,

represents the odometer data of robot i from time t to t+1,

is the information matrix representing the observations of robot i from time t to t+1,

represents the odometry constraint of robot i at time t,

represents the odometer data of robot j at time t,

represents the robot pose with the smallest residual function value between robot i and robot j,

represents the positional constraints between robot i and robot j,

is the information matrix of observations between robot i and robot j at time t,

represents the odometer data of robot j at time t+1,

represents the odometer data of robot j from t to t+1,

is the odometry constraint of robot j at time t,

is the information matrix representing the observed values between robot j at time t to t+1, (i,j)∈[1,…,N], where N represents the number of robots.

The beneficial effects of the above-mentioned further scheme are: in the present invention, in step S3, based on the pose information after nonlinear optimization, the information provided by the odometer is fused, the pose graph is constructed, and optimized, so as to realize precise multi-robot cooperative positioning. Although the odometer has accumulated errors for a long time, the odometer can provide accurate pose changes of the robot in a short time. Through the pose information provided by the odometer, combined with the pose information after nonlinear optimization, higher-precision positioning can be obtained. Taking the pose of the robot as a vertex, and the constraint relationship between the estimated poses at different times as edges, including the pose transformation constraints of the robot at adjacent moments in the odometer, and the position constraints based on UWB, the constraints are obtained by the least squares method. Minimum-case pose.

Description of drawings

Fig. 1 is the flow chart of the multi-robot cooperative positioning method;

Fig. 2 is the structure diagram of multi-robot cooperative positioning;

Fig. 3 is a multi-robot cooperative positioning diagram based on UWB of the present invention;

4 is a multi-robot based odometer-based collaborative positioning diagram of the present invention;

FIG. 5 is a multi-robot cooperative positioning diagram of the present invention that fuses odometer data and UWB data.

Detailed ways

The embodiments of the present invention will be further described below with reference to the accompanying drawings.

Before describing the specific embodiments of the present invention, in order to make the solution of the present invention clearer and more complete, the definitions of abbreviations and key terms that appear in the present invention are first described:

UWB: A wireless carrier communication technology that uses a frequency bandwidth above 1 GHz.

As shown in Figure 1, the present invention provides a multi-robot cooperative positioning method integrating odometer and UWB, comprising the following steps:

S1: Collect the pose information of the robot and the distance information between multiple groups of UWB nodes through the odometer and the UWB data acquisition module respectively;

S2: Based on the collected distance information between multiple groups of UWB nodes, the cooperative positioning of the robot is realized through a nonlinear optimization algorithm;

S3: Based on the multi-robot positioning information after nonlinear optimization, the pose information provided by the robot odometer is fused, the pose graph is constructed, and the optimization is carried out to realize the precise positioning of the multi-robot cooperation.

In the embodiment of the present invention, as shown in FIG. 1 , in step S1, the method for collecting the pose information of the robot by using the odometer is as follows: build an encoder on the robot for collection, and obtain the odometer data;

The method of collecting the distance information between multiple groups of UWB nodes through the UWB data collection module is as follows: carrying UWB tags on different positions of each robot, and collecting the distance information between multiple groups of UWB nodes.

In the present invention, UWB has great advantages in real-time performance and bandwidth, etc., and has stronger anti-interference performance. By carrying UWB tags at different positions on the robot, the UWB node of each robot will calculate the distance between UWB nodes of other robots, so that each robot can obtain multiple sets of distance data relative to other robots.

In this embodiment of the present invention, as shown in FIG. 1 , step S2 includes the following sub-steps:

S21: According to the distance information between multiple groups of UWB nodes, calculate the residual function of the distance measurement value and the calculated value between robot i and robot j;

S22: According to the residual function of the measured value and the calculated value of the distance between robot i and robot j, use the Levenberg-Marquardt method to iterate to obtain the optimal pose of the robot to realize multi-robot cooperative positioning.

In the present invention, although UWB has the advantages of strong anti-interference performance and high ranging accuracy, UWB can only obtain distance information, and cannot complete the pose estimation of the mobile robot, and the odometer can measure the precise position in a short time. Attitude information, the combination of the two can achieve accurate positioning. This patent adopts a nonlinear optimization algorithm to obtain the relative pose estimation between robots on the basis of UWB distance measurement data.

In the embodiment of the present invention, as shown in FIG. 1, in step S21, the expression of the residual function of the measured value and the calculated value of the distance between robot i and robot j is:

表示机器人i和机器人j之间残差函数值最小的机器人位姿，argmin(·)表示能够使误差值最小的x的值，f(x)表示残差函数，x＝(x,y,θ)表示机器人的位姿，x表示机器人i相对于机器人j的横坐标，y表示机器人i相对于机器人j的纵坐标，θ表示机器人i相对于机器人j的方向角度，K表示机器人i上的UWB节点的数量，L表示机器人j上的UWB节点的数量，

表示在t时刻机器人i上UWB节点k和机器人j上UWB节点l之间的实际测量值，d(·)表示给定机器人i与机器人j相对位姿的情况下计算机器人i上UWB节点k和机器人j上UWB节点l之间的距离运算，

表示机器人i上的UWB节点k的相对位置，

表示机器人j上的UWB节点l的相对位置，k∈[1,…,K]，l∈[1，…,L]，i∈[1,…,N],j∈[1，…,N]，N代表机器人的数量。in,

represents the robot pose with the smallest residual function value between robot i and robot j, argmin( ) represents the value of x that can minimize the error value, f(x) represents the residual function, x=(x, y, θ ) represents the pose of the robot, x represents the abscissa of the robot i relative to the robot j, y represents the ordinate of the robot i relative to the robot j, θ represents the direction angle of the robot i relative to the robot j, and K represents the UWB on the robot i the number of nodes, L represents the number of UWB nodes on robot j,

represents the actual measurement value between UWB node k on robot i and UWB node l on robot j at time t, d( ) represents the calculation of UWB node k and UWB node k on robot i given the relative pose of robot i and robot j The distance calculation between UWB nodes l on robot j,

represents the relative position of UWB node k on robot i,

Represents the relative position of UWB node l on robot j, k∈[1,…,K], l∈[1,…,L], i∈[1,…,N], j∈[1,…,N ], N represents the number of robots.

In the present invention, the pose of the mobile robot is optimized by using multiple sets of UWB distance information, and the relative pose estimation between robot i and robot j can be realized by finding the best pose configuration by minimizing the above equation, where f (x) is equivalent to a residual function, which refers to the difference between the actual measured value and the calculated value, and x=(x, y, θ) with the smallest difference is the optimal pose.

In this embodiment of the present invention, as shown in FIG. 1 , step S22 includes the following sub-steps:

S221: According to the residual function, establish an approximate index evaluation model, and dynamically adjust the size of the trust region μ;

S222: Limit the step size Δx to the region with the trust radius μ, and calculate Δx;

S223: Perform k iterations in the region with the trust radius μ to obtain the optimal pose of the robot and realize multi-robot cooperative positioning.

In the present invention, the traditional method needs to traverse the entire (x, y, θ) value range to obtain the best pose estimation, and the calculation efficiency is too low. The present patent uses the Levenberg-Marquardt method to improve the computational efficiency, and the optimization idea of the method is: in each iteration, the optimization range is first determined, and then the optimal point within the range is determined. That is, the step size Δx=(Δx, Δy, Δθ) ^T is limited to a region with a trust radius of μ, and then the optimal step size is found in this region. After k iterations, if ρ<0, it means the fitting error To the trend of rising instead of falling (opposite to the optimization target), x _k+1 = x _k should be set at this time, and μ=0.5μ can be set according to experience before continuing the iterative calculation. If ρ>0 indicates the same direction as the optimization target, then let x _k+1 =x _k +Δx _k , stop the iteration when Δx _k is small enough, and the pose obtained at this time is the best pose.

In the embodiment of the present invention, as shown in FIG. 1 , in step S221, the method of performing k iterations in the region with the trust radius μ is: dynamically adjust the region with the trust radius μ by establishing an approximate index evaluation model, and the approximate index The expression of the evaluation model is:

Among them, Δx represents the step size, J(x) represents the first derivative of f(x) with respect to x, f(x+Δx) represents the increment of f(x), and ρ represents the approximate index;

The method of dynamically adjusting the region of the trust radius μ according to the approximate index ρ is: if 0<ρ≤0.25, then reduce the trust radius μ by half, if ρ>0.75, then increase the trust radius μ by half, if 0.25<ρ≤0.75 , the trust radius μ is not adjusted;

Carry out k iterations in the region with the trust radius μ, if ρ<0, then let x _k+1 = x _k , and reduce the trust radius μ by half to continue the iteration, if ρ > 0, then let x _k+1 = x _k +Δx _k , when Δx _k is less than the set threshold, the initial optimal pose x _k+1 of the robot is obtained, where Δx _k represents the step size after k iterations, and x _k represents the pose of the previous iteration.

In the present invention, during the update iteration process, the range of the trust region μ is determined according to the difference between the approximate model and the actual function. If the difference is small, the range of the trust region is expanded; if the difference is large, the range is reduced. In order to judge the quality of the difference between the approximate model and the actual function, an approximate index is set to judge the size of the model fitting difference, and the trust region radius is dynamically adjusted. In the expression, the numerator is the value by which the objective function actually changes, and the denominator is the value by which the approximate model changes. When performing regional adjustment, if ρ is too small, it means that the value of the approximate model change is larger than the actual change value, that is, the approximation effect is poor, and the confidence region needs to be narrowed; if ρ is too large, it means that the approximate change value is smaller than the actual change value. , it is necessary to expand μ; when ρ is close to 1, it is considered that the approximation effect is better, and there is no need to change the value of μ.

In the embodiment of the present invention, as shown in FIG. 1 , in step S222, the calculation formula of the step size Δx is:

(H(x)+λI)Δx=-J(x) ^T f(x)

Among them, H(x) represents the approximation of f(x) Hessian matrix, H(x)=J(x) ^T J(x), I represents the identity matrix, λ represents the coefficient factor, and J(x) represents f(x) ) with respect to the first derivative of x.

In the present invention, a constrained optimization problem is transformed into an unconstrained optimization problem through a Lagrangian multiplier in the Levenberg-Marquardt algorithm, and the Taylor expansion of the unconstrained optimization equation can obtain a calculation increment of Linear equation. Among them, J(x) is the first derivative of f(x) with respect to x, which is the Jacobian matrix of L·K rows and 3 columns; λ is the coefficient factor, the purpose is to make the matrix (H+λI) positive definite, the initial value of λ The value is the maximum value of the diagonal elements of J(x) ^T J(x).

In the embodiment of the present invention, as shown in FIG. 1 , in step S3, the method for constructing the pose graph is as follows: the pose of the robot is used as a vertex, and the constraint relationship between the poses at different times is used as an edge;

Based on the pose graph, the pose with the minimum constraints is calculated by the least squares method. The calculation formula is:

表示机器人i在t时刻的里程计数据，

表示机器人i在t+1时刻的里程计数据，Δx_i ^t表示机器人i在t至t+1时刻的里程计数据，

表示机器人i在t至t+1时刻观察值的信息矩阵，

表示机器人i在t时刻的里程计约束，

表示机器人j在t时刻的里程计数据，

表示机器人i和机器人j之间残差函数值最小的机器人位姿，

表示机器人i和机器人j之间的位态约束，

是机器人i和机器人j之间在t时刻观察值的信息矩阵，

表示机器人j在t+1时刻的里程计数据，

代表机器人j在t至t+1时刻的里程计数据，

是机器人j在t时刻的里程计约束，

表示机器人j之间在t至t+1时刻观察值的信息矩阵，(i,j)∈[1,…,N]，N代表机器人的数量。Among them, T represents the duration,

represents the odometer data of robot i at time t,

represents the odometer data of robot i at time t+1, Δx _i ^t represents the odometer data of robot i from time t to t+1,

is the information matrix representing the observations of robot i from time t to t+1,

represents the odometry constraint of robot i at time t,

represents the odometer data of robot j at time t,

represents the robot pose with the smallest residual function value between robot i and robot j,

represents the positional constraints between robot i and robot j,

is the information matrix of observations between robot i and robot j at time t,

represents the odometer data of robot j at time t+1,

represents the odometer data of robot j from t to t+1,

is the odometry constraint of robot j at time t,

is the information matrix representing the observed values between robot j at time t to t+1, (i,j)∈[1,…,N], where N represents the number of robots.

In the present invention, in step S3, based on the pose information after nonlinear optimization, the information provided by the odometer is fused to construct a pose graph, and optimization is performed to realize precise positioning of multi-robot cooperation. Although the odometer has accumulated errors for a long time, the odometer can provide accurate pose changes of the robot in a short time. Through the pose information provided by the odometer, combined with the pose information after nonlinear optimization, higher-precision positioning can be obtained. Taking the pose of the robot as a vertex, and the constraint relationship between the estimated poses at different times as edges, including the pose transformation constraints of the robot at adjacent moments in the odometer, and the position constraints based on UWB, the constraints are obtained by the least squares method. Minimum-case pose.

In the embodiment of the present invention, as shown in FIG. 2 , distance information is measured between multiple robots through UWB, the odometer information of the robots is integrated, and the position information is updated in real time during the robot movement, so as to realize precise positioning of multi-robot cooperation.

In the embodiment of the present invention, as shown in FIG. 3 , the solid line in the figure represents the real trajectory of robot 1, the dotted line represents the real trajectory of robot 2, and the short line represents the positioning trajectory of robot 2 based on UWB. It can be seen from the figure that in the positioning based on UWB, there is a large gap with the real trajectory of the robot, and the positioning accuracy is poor.

In the embodiment of the present invention, as shown in FIG. 4 , the solid line in the figure represents the real trajectory of the robot 1, the short line represents the trajectory of the robot 1 based on the odometer, the dotted line represents the real trajectory of the robot 2, and the small short line represents the mileage-based trajectory of the robot 2 track of the meter. It can be seen from the figure that the odometer has high accuracy in a short period of time, but there will be accumulated errors as the robot moves, resulting in poor positioning accuracy of the robot.

In the embodiment of the present invention, as shown in FIG. 5 , the solid line in the figure represents the real trajectory of robot 1, the dotted line represents the trajectory of robot 1 fused with odometer information on the basis of UWB, the short line represents the real trajectory of robot 2, and the small line represents the real trajectory of robot 2. The short line represents the trajectory of robot 2 fused with odometry information on the basis of UWB. It can be seen from the figure that compared with the first two methods, the multi-robot cooperative positioning accuracy of the fusion of odometry data and UWB data is higher, and it is more accurate than the original trajectory. fit.

The working principle and process of the present invention are as follows: using the odometer and UWB equipment in the environment as the sensing unit to realize the collection of UWB data and odometer data. UWB collects distance information between robots, and realizes cooperative positioning among multiple robots through nonlinear optimization. Odometer provides approximate pose changes of robots, and fusion of positioning information between multiple robots can provide more accurate poses.

The beneficial effects of the present invention are as follows: the multi-robot cooperative positioning method of the present invention utilizes an odometer and a UWB data acquisition module for collection, and is based on a nonlinear optimized multi-robot cooperative positioning algorithm and a pose graph optimization algorithm fused with odometry information. Using the odometer and UWB device in the environment as the perception unit, the collection of UWB data and odometer data is realized. UWB collects distance information between robots, and realizes cooperative positioning between multiple robots through nonlinear optimization. The multi-robot cooperative positioning accuracy is higher. The problem of poor positioning accuracy of multi-robot cooperative positioning is solved.

Those of ordinary skill in the art will appreciate that the embodiments described herein are intended to assist readers in understanding the principles of the present invention, and it should be understood that the scope of protection of the present invention is not limited to such specific statements and embodiments. Those skilled in the art can make various other specific modifications and combinations without departing from the essence of the present invention according to the technical teaching disclosed in the present invention, and these modifications and combinations still fall within the protection scope of the present invention.

Claims (2)

1. a multi-robot cooperative positioning method of fusion odometer and UWB, is characterized in that, comprises the following steps: S1: Collect the pose information of the robot and the distance information between multiple groups of UWB nodes through the odometer and the UWB data acquisition module respectively; S2: Based on the collected distance information between multiple groups of UWB nodes, the cooperative positioning of the robot is realized through a nonlinear optimization algorithm; The step S2 includes the following sub-steps: S21: According to the distance information between multiple groups of UWB nodes, calculate the residual function of the distance measurement value and the calculated value between robot i and robot j; S22: According to the residual function of the measured value and the calculated value of the distance between robot i and robot j, use the Levenberg-Marquardt method to iterate to obtain the optimal pose of the robot to realize multi-robot cooperative positioning; In the step S21, the expression of the residual function of the measured value and the calculated value of the distance between robot i and robot j is:

in,

represents the robot pose with the smallest residual function value between robot i and robot j, argmin( ) represents the value of x that can minimize the error value, f(x) represents the residual function, x=(x, y, θ ) represents the pose of the robot, x represents the abscissa of the robot i relative to the robot j, y represents the ordinate of the robot i relative to the robot j, θ represents the direction angle of the robot i relative to the robot j, and K represents the UWB on the robot i the number of nodes, L represents the number of UWB nodes on robot j,

represents the actual measurement value between UWB node k on robot i and UWB node l on robot j at time t, d( ) represents the calculation of UWB node k and UWB node k on robot i given the relative pose of robot i and robot j The distance calculation between UWB nodes l on robot j,

represents the relative position of UWB node k on robot i,

Represents the relative position of UWB node l on robot j, k∈[1,…,K], l∈[1,…,L], i∈[1,…,N], j∈[1,…,N ], N represents the number of robots; The step S22 includes the following sub-steps: S221: According to the residual function, establish an approximate index evaluation model, and dynamically adjust the size of the trust region μ; S222: Limit the step size Δx to the region with the trust radius μ, and calculate Δx; S223: Perform k iterations in an area with a trust radius μ to obtain the optimal pose of the robot and realize multi-robot cooperative positioning; In the step S221, the method of performing k iterations in the region with the trust radius μ is: dynamically adjust the region with the trust radius μ by establishing an approximate index evaluation model, and the expression of the approximate index evaluation model is:

Among them, Δx represents the step size, J(x) represents the first derivative of f(x) with respect to x, f(x+Δx) represents the increment of f(x), and ρ represents the approximate index; The method of dynamically adjusting the region of the trust radius μ according to the approximate index ρ is: if 0<ρ≤0.25, then reduce the trust radius μ by half, if ρ>0.75, then increase the trust radius μ by half, if 0.25<ρ≤0.75 , the trust radius μ is not adjusted; Carry out k iterations in the region with the trust radius μ, if ρ<0, then let x _k+1 = x _k , and reduce the trust radius μ by half to continue the iteration, if ρ > 0, then let x _k+1 = x _k +Δx _k , when Δx _k is less than the set threshold, the initial optimal pose x _k+1 of the robot is obtained, where Δx _k represents the step size after k iterations, and x _k represents the pose of the previous iteration; In the step S222, the calculation formula of the step size Δx is: (H(x)+λI)Δx=-J(x) ^T f(x) Among them, H(x) represents the approximation of f(x) Hessian matrix, H(x)=J(x) ^T J(x), I represents the identity matrix, λ represents the coefficient factor, and J(x) represents f(x) ) with respect to the first derivative of x; S3: Based on the multi-robot positioning information after nonlinear optimization, fuse the pose information provided by the robot odometer, construct a pose graph, and optimize it to achieve precise positioning of multi-robot collaboration; In the step S3, the method for constructing the pose graph is as follows: the pose of the robot is used as a vertex, and the constraint relationship between the poses at different times is used as an edge; Based on the pose graph, the pose with the minimum constraints is calculated by the least squares method. The calculation formula is:

Among them, T represents the duration,

represents the odometer data of robot i at time t,

represents the odometer data of robot i at time t+1,

represents the odometer data of robot i from time t to t+1,

is the information matrix representing the observations of robot i from time t to t+1,

represents the odometry constraint of robot i at time t,

represents the odometer data of robot j at time t,

represents the robot pose with the smallest residual function value between robot i and robot j,

represents the positional constraints between robot i and robot j,

is the information matrix of observations between robot i and robot j at time t,

represents the odometer data of robot j at time t+1,

represents the odometer data of robot j from t to t+1,

is the odometry constraint of robot j at time t,

is the information matrix representing the observed values between robot j at time t to t+1, (i,j)∈[1,…,N], where N represents the number of robots. 2. The multi-robot cooperative positioning method of fusing odometer and UWB according to claim 1, is characterized in that, in described step S1, the method for collecting the pose information of robot by odometer is: the encoder is built on the robot to collect data on the odometer; The method of collecting the distance information between multiple groups of UWB nodes through the UWB data acquisition module is as follows: carrying UWB tags on different positions of each robot, and collecting the distance information between multiple groups of UWB nodes.

Images