CN112833876B - Multi-robot cooperative positioning method integrating odometer and UWB - Google Patents
Multi-robot cooperative positioning method integrating odometer and UWB Download PDFInfo
- Publication number
- CN112833876B CN112833876B CN202011625879.0A CN202011625879A CN112833876B CN 112833876 B CN112833876 B CN 112833876B CN 202011625879 A CN202011625879 A CN 202011625879A CN 112833876 B CN112833876 B CN 112833876B
- Authority
- CN
- China
- Prior art keywords
- robot
- uwb
- pose
- denotes
- information
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
Abstract
The invention discloses a multi-robot cooperative positioning method fusing a speedometer and a UWB, which comprises the following steps: s1: acquiring pose information of the robot and distance information between a plurality of groups of UWB nodes by using a odometer and a UWB data acquisition module respectively; s2: based on the acquired distance information among the multiple groups of UWB nodes, the multi-robot cooperative positioning is realized through a nonlinear optimization algorithm; s3: based on the nonlinear optimized multi-robot positioning information, the information provided by the robot odometer is fused, a pose graph is constructed, optimization is carried out, and accurate positioning of multi-robot cooperation is achieved. UWB collects distance information among robots, cooperative positioning among multiple robots is achieved through nonlinear optimization, the odometer provides approximate pose change of the robots, and pose information after nonlinear optimization is fused through a graph optimization algorithm, so that the precision of the cooperative positioning of multiple robots is higher. The problem of the multi-robot cooperation positioning accuracy is poor is solved.
Description
Technical Field
The invention belongs to the technical field of multi-mobile-robot cooperative positioning, and particularly relates to a multi-robot cooperative positioning method integrating a speedometer and a UWB.
Background
In recent years, mobile robot technology plays an important role in many fields such as industry, medical treatment and service, and is also well applied to harmful and dangerous occasions such as national defense and space exploration fields.
In the research field of mobile robots, positioning is always a popular research topic, which provides real-time accurate positions for robots, and these are the prerequisites for robots to perform path planning and path tracking, so that it occupies a very important position in the research of mobile robots.
The ultra-wideband positioning technology has great advantages in the aspects of real-time performance, bandwidth and the like, and has stronger anti-interference performance, low cost, low power consumption and high data transmission speed. UWB can carry out the measurement of distance under complicated changeable environment, improves the precision and the efficiency of range finding. However, the distance measured by UWB is also influenced by the environment factors, the angle of the robot and other conditions, and the influence on the offset can be reduced by mutual distance measurement among a plurality of robots and averaging the average values of forward and reverse multiple measurements, so that the distance measurement error is reduced, and the more accurate distance between the robots is acquired. Because UWB can only provide distance information, can not finish the position appearance estimation to mobile robot, can realize the cooperation location between a plurality of robots through nonlinear optimization.
The odometer provides accurate pose change of the robot in a short time, although accumulated errors exist in the odometer for a long time, the odometer can provide accurate positioning for the robot in a short time, and distance information between multiple robots provided by UWB can correct the errors, so that the positioning accuracy of the robot is improved. And by the pose information provided by the plurality of robot odometers and the positioning information between the plurality of robots after nonlinear optimization, higher-precision positioning can be obtained through further optimization of a graph optimization algorithm. Therefore, the method selectively fuses the odometer data and the UWB data to realize the cooperative accurate positioning among a plurality of mobile robots.
Disclosure of Invention
The invention aims to solve the problems of poor precision and low precision of the cooperative positioning of the existing multi-robot in a complex indoor environment, and provides a multi-robot cooperative positioning method integrating an odometer and a UWB.
The technical scheme of the invention is as follows: a multi-robot cooperative positioning method fusing a odometer and a UWB comprises the following steps:
s1: the pose information of the robot and the distance information between a plurality of groups of UWB nodes are respectively collected through a speedometer and a UWB data collection module;
s2: based on the acquired distance information among the multiple groups of UWB nodes, the robot cooperative positioning is realized through a nonlinear optimization algorithm;
s3: based on the nonlinear optimized multi-robot positioning information, the pose information provided by the robot odometer is fused, a pose graph is constructed and optimized, and the accurate positioning of multi-robot cooperation is realized.
The invention has the beneficial effects that: the multi-robot cooperative positioning method utilizes the odometer and the UWB data acquisition module to acquire, and is based on a nonlinear optimization multi-robot cooperative positioning algorithm and a pose graph optimization algorithm fusing odometer information. The odometer and the UWB equipment in the environment are used as sensing units to realize the collection of the UWB data and the odometer data. UWB collects distance information among robots, cooperative positioning among multiple robots is achieved through nonlinear optimization, the odometer provides approximate pose change of the robots, and pose information after nonlinear optimization is fused through a graph optimization algorithm, so that the precision of the cooperative positioning of multiple robots is higher. The problem of the multi-robot cooperation positioning accuracy is poor is solved.
Further, in step S1, the method for acquiring pose information of the robot by the odometer includes: the encoder is built on the robot to be collected, and odometer data are obtained;
the method for acquiring the distance information among a plurality of groups of UWB nodes through the UWB data acquisition module comprises the following steps: and carrying UWB labels at different positions of each robot, and acquiring distance information among multiple groups of UWB nodes.
The beneficial effects of the further scheme are as follows: in the invention, UWB has great advantages in real-time performance and bandwidth, and the like, and has stronger anti-interference performance. By carrying UWB tags at different positions on the robot, the UWB node of each robot can calculate the distance between the UWB node of each robot and other UWB nodes of the robot, so that each robot can obtain multiple groups of distance data relative to other robots.
Further, step S2 includes the following sub-steps:
s21: calculating a residual function of a distance measurement value and a calculated value between the robot i and the robot j according to the distance information between the plurality of groups of UWB nodes;
s22: and (4) according to a residual function of a distance measurement value and a calculated value between the robot i and the robot j, iteration is carried out by using a Levenberg-Marquardt method to obtain the optimal pose of the robot, so that the multi-robot cooperative positioning is realized.
The beneficial effects of the further scheme are as follows: in the invention, although UWB has the advantages of strong anti-interference performance and high ranging precision, UWB can only acquire distance information and cannot finish pose estimation of the mobile robot, and the odometer can measure accurate pose information in a short time, and the combination of the two can realize accurate positioning. The method adopts a nonlinear optimization algorithm to obtain the estimation of the relative pose between the robots on the basis of UWB distance measurement data.
Further, in step S21, the expression of the residual function between the distance measurement value and the calculated value between robot i and robot j is:
wherein the content of the first and second substances,
denotes a robot pose at which a residual function value between the robot i and the robot j is minimum, argmin (·) denotes a value of x that can minimize an error value, f (x) denotes a residual function, x ═ x denotes a pose of the robot, x denotes an abscissa of the robot i with respect to the robot j, y denotes an ordinate of the robot i with respect to the robot j, θ denotes a direction angle of the robot i with respect to the robot j, K denotes the number of UWB nodes on the robot i, L denotes the number of UWB nodes on the robot j,
represents the actual measurement value between the UWB node k on the robot i and the UWB node l on the robot j at the time t, d (-) represents the distance calculation between the UWB node k on the robot i and the UWB node l on the robot j under the condition that the relative pose of the robot i and the robot j is given,
indicating the relative position of UWB node k on robot i,
indicating the relative position of UWB node l on robot j, k e 1, …,K]，l∈[1，…,L]，i∈[1,…,N],j∈[1，…,N]and N represents the number of robots.
The beneficial effects of the further scheme are as follows: in the invention, the posture of the mobile robot is optimized by using a plurality of groups of UWB distance information, and the estimation of the relative posture between the robot i and the robot j can be realized by finding the optimal posture configuration through the minimization of the equation, wherein f (x) is equivalent to a residual function and refers to the difference between an actual measured value and a calculated value, and x with the minimum difference is (x, y, theta) which is the optimal posture.
Further, step S22 includes the following sub-steps:
s221: establishing an approximate index evaluation model according to the residual function, and dynamically adjusting the size of the confidence region mu;
s222: limiting the step length delta x in a region with the confidence radius mu, and calculating delta x;
s223: and performing k iterations in the region with the confidence radius of mu to obtain the optimal pose of the robot, thereby realizing the multi-robot cooperative positioning.
The beneficial effects of the further scheme are as follows: in the invention, the traditional method needs to traverse the whole (x, y, theta) value range to obtain the optimal pose estimation, and the calculation efficiency is too low. The method adopts a Levenberg-Marquardt method to improve the calculation efficiency, and the optimization idea of the method is as follows: in each iteration, the optimized range is determined, and then the optimal point in the range is determined. I.e. the step Δ x is equal to (Δ x, Δ y, Δ θ)TLimiting the region with the confidence radius mu, then finding the optimal step length in the region, and iterating for k times if rho<0, indicating a trend of the fitting error toward rising rather than falling (opposite to the optimization target direction), which should be let xk+1＝xkThe iterative calculation can be continued by setting μ to 0.5 μ empirically. If ρ>0 indicates the same direction as the optimization target, when let xk+1＝xk+ΔxkWhen Δ xkAnd stopping iteration when the time is long enough, wherein the obtained pose is the optimal pose.
Further, in step S221, the method of performing k iterations in the region with the confidence radius μ includes: dynamically adjusting the region with the confidence radius of mu by establishing an approximate index evaluation model, wherein the expression of the approximate index evaluation model is as follows:
where Δ 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 an approximation indicator;
the method for dynamically adjusting the region with the confidence radius mu according to the approximation index rho comprises the following steps: if 0< rho is less than or equal to 0.25, the confidence radius mu is reduced by half, if rho is greater than 0.75, the confidence radius mu is enlarged by half, and if 0.25< rho is less than or equal to 0.75, the confidence radius mu is not adjusted;
k iterations in the region with confidence radius mu, if rho<0, then let xk+1＝xkAnd reducing the confidence radius mu by half and continuing the iteration, if rho is larger than 0, making xk+1＝xk+ΔxkWhen Δ xkWhen the pose is smaller than the set threshold value, obtaining the initial optimal pose x of the robotk+1Wherein, Δ xkRepresents the step size, x, after k iterationskRepresenting the pose of the last iteration.
The beneficial effects of the further scheme are as follows: in the invention, in the updating and iterating process, the range of the confidence region mu is determined according to the difference between the approximate model and the actual function, and if the difference is small, the range of the confidence region is enlarged; if the difference is large, the range is narrowed. In order to judge whether the difference between the approximate model and the actual function is good or not, the fitting difference of the approximate index judging model is set, and the radius of the confidence domain is dynamically adjusted. In the expression, the numerator is a value at which the objective function actually changes, and the denominator is a value at which the approximation model changes. When the region is adjusted, rho is too small, which indicates that the value of the approximate model change is larger than the value of the actual change, namely the approximation effect is poor, and the range of the confidence domain needs to be narrowed; if rho is too large, the approximate change value is smaller than the actual change value, and mu needs to be expanded; and p is close to 1, the approximation effect is considered to be better, and the value of mu does not need to be changed.
Further, in step S222, the calculation formula of the step Δ x is:
(H(x)+λI)Δx＝-J(x)Tf(x)
wherein H (x) represents an approximation of the sea plug matrix, H (x) J (x)TJ (x), I denotes the identity matrix, λ denotes the coefficient factor, j (x) denotes f (x) the first derivative with respect to x.
The beneficial effects of the further scheme are as follows: in the invention, a constrained optimization problem is converted into an unconstrained optimization problem through a Lagrange multiplier in the Levenberg-Marquardt algorithm, and a linear equation for calculating increment can be obtained by expanding an unconstrained optimization equation Taylor. Wherein J (x) is the first derivative of f (x) with respect to x, which is a Jacobian matrix of L.K rows and 3 columns; λ is a coefficient factor for positively defining the matrix (H + λ I), and the initial value of λ is J (x)TJ (x) maximum value of diagonal element.
Further, in step S3, the method of constructing the pose graph includes: using the pose of the robot as a vertex, and using the constraint relation between poses at different moments as edges;
based on the pose graph, calculating the pose under the condition of minimum constraint by a least square method, wherein the calculation formula is as follows:
wherein, T represents the time length,
odometer data representing the robot i at time t,
odometry data representing robot i at time t +1,
odometry data representing the robot i at the time t to t +1,
an information matrix representing the observations of robot i at times t to t +1,
indicating the odometry constraint of robot i at time t,
odometry data representing robot j at time t,
representing the robot pose with the minimum residual function value between the robot i and the robot j,
representing the state constraint between robot i and robot j,
is an information matrix of observations between robot i and robot j at time t,
odometry data representing robot j at time t +1,
representing odometry data of the robot j from the moment t to the moment t +1,
is the odometry constraint for robot j at time t,
information matrix representing observed values between robots j at times t to t +1, (i, j) ∈ [1, …, N]And N represents the number of robots.
The beneficial effects of the further scheme are as follows: in the 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 carried out to realize multi-robot cooperation accurate positioning. Although the odometer has accumulated errors for a long time, the odometer can provide accurate pose change of the robot in a short time. And the position and attitude information provided by the odometer is combined with the position and attitude information after nonlinear optimization to obtain higher-precision positioning. The pose of the robot is used as a vertex, the constraint relation between the estimated poses at different moments is used as an edge, the pose transformation constraint of the robot at adjacent moments in the odometer is included, and the pose under the condition of minimizing the constraint is obtained through a least square method based on the position constraint of UWB.
Drawings
FIG. 1 is a flow chart of a multi-robot cooperative positioning method;
FIG. 2 is a block diagram of multi-robot cooperative positioning;
FIG. 3 is a UWB-based cooperative positioning diagram of multiple robots of the present invention;
FIG. 4 is a diagram of multi-robot odometer-based collaborative positioning of the present invention;
FIG. 5 is a diagram of the multi-robot cooperative localization of fusing odometry data with UWB data in accordance with the present invention.
Detailed Description
The embodiments of the present invention will be further described with reference to the accompanying drawings.
Before describing specific embodiments of the present invention, in order to make the solution of the present invention more clear and complete, the definitions of the abbreviations and key terms appearing in the present invention will be explained first:
UWB: a wireless carrier communication technique using a frequency bandwidth of 1GHz or more.
As shown in fig. 1, the invention provides a multi-robot cooperative positioning method fusing a odometer and a UWB, comprising the following steps:
s1: the pose information of the robot and the distance information between a plurality of groups of UWB nodes are respectively collected through a speedometer and a UWB data collection module;
s2: based on the acquired distance information among the multiple groups of UWB nodes, the robot cooperative positioning is realized through a nonlinear optimization algorithm;
s3: based on the nonlinear optimized multi-robot positioning information, the pose information provided by the robot odometer is fused, a pose graph is constructed and optimized, and the accurate positioning of multi-robot cooperation is realized.
In the embodiment of the present invention, as shown in fig. 1, in step S1, the method for acquiring pose information of the robot by the odometer includes: the encoder is built on the robot to be collected, and odometer data are obtained;
the method for acquiring the distance information among a plurality of groups of UWB nodes through the UWB data acquisition module comprises the following steps: and carrying UWB labels at different positions of each robot, and acquiring distance information among multiple groups of UWB nodes.
In the invention, UWB has great advantages in real-time performance and bandwidth, and the like, and has stronger anti-interference performance. By carrying UWB tags at different positions on the robot, the UWB node of each robot can calculate the distance between the UWB node of each robot and other UWB nodes of the robot, so that each robot can obtain multiple groups of distance data relative to other robots.
In the embodiment of the present invention, as shown in fig. 1, step S2 includes the following sub-steps:
s21: calculating a residual function of a distance measurement value and a calculated value between the robot i and the robot j according to the distance information between the plurality of groups of UWB nodes;
s22: and (4) according to a residual function of a distance measurement value and a calculated value between the robot i and the robot j, iteration is carried out by using a Levenberg-Marquardt method to obtain the optimal pose of the robot, so that the multi-robot cooperative positioning is realized.
In the invention, although UWB has the advantages of strong anti-interference performance and high ranging precision, UWB can only acquire distance information and cannot finish pose estimation of the mobile robot, and the odometer can measure accurate pose information in a short time, and the combination of the two can realize accurate positioning. The method adopts a nonlinear optimization algorithm to obtain the estimation of the relative pose between the 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 between the distance measurement value and the calculated value between robot i and robot j is:
wherein the content of the first and second substances,
denotes a robot pose at which a residual function value between the robot i and the robot j is minimum, argmin (·) denotes a value of x that can minimize an error value, f (x) denotes a residual function, x ═ x denotes a pose of the robot, x denotes an abscissa of the robot i with respect to the robot j, y denotes an ordinate of the robot i with respect to the robot j, θ denotes a direction angle of the robot i with respect to the robot j, K denotes the number of UWB nodes on the robot i, L denotes the number of UWB nodes on the robot j,
represents the actual measurement value between the UWB node k on the robot i and the UWB node l on the robot j at the time t, d (-) represents the distance calculation between the UWB node k on the robot i and the UWB node l on the robot j under the condition that the relative pose of the robot i and the robot j is given,
indicating 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]And N represents the number of robots.
In the invention, the posture of the mobile robot is optimized by using a plurality of groups of UWB distance information, and the estimation of the relative posture between the robot i and the robot j can be realized by finding the optimal posture configuration through the minimization of the equation, wherein f (x) is equivalent to a residual function and refers to the difference between an actual measured value and a calculated value, and x with the minimum difference is (x, y, theta) which is the optimal posture.
In the embodiment of the present invention, as shown in fig. 1, step S22 includes the following sub-steps:
s221: establishing an approximate index evaluation model according to the residual function, and dynamically adjusting the size of the confidence region mu;
s222: limiting the step length delta x in a region with the confidence radius mu, and calculating delta x;
s223: and performing k iterations in the region with the confidence radius of mu to obtain the optimal pose of the robot, thereby realizing the multi-robot cooperative positioning.
In the invention, the traditional method needs to traverse the whole (x, y, theta) value range to obtain the optimal pose estimation, and the calculation efficiency is too low. The method adopts a Levenberg-Marquardt method to improve the calculation efficiency, and the optimization idea of the method is as follows: in each iteration, the optimized range is determined, and then the optimal point in the range is determined. I.e. the step Δ x is equal to (Δ x, Δ y, Δ θ)TLimiting the region with the confidence radius mu, then finding the optimal step length in the region, and iterating for k times if rho<0, indicating a trend of the fitting error toward rising rather than falling (opposite to the optimization target direction), which should be let xk+1＝xkThe iterative calculation can be continued by setting μ to 0.5 μ empirically. If ρ>0 indicates the same direction as the optimization target, when let xk+1＝xk+ΔxkWhen Δ xkAnd stopping iteration when the time is long enough, wherein the obtained pose is the optimal pose.
In the embodiment of the present invention, as shown in fig. 1, in step S221, the method for performing k iterations in the region with the confidence radius μ includes: dynamically adjusting the region with the confidence radius of mu by establishing an approximate index evaluation model, wherein the expression of the approximate index evaluation model is as follows:
where Δ 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 an approximation indicator;
the method for dynamically adjusting the region with the confidence radius mu according to the approximation index rho comprises the following steps: if 0< rho is less than or equal to 0.25, the confidence radius mu is reduced by half, if rho is greater than 0.75, the confidence radius mu is enlarged by half, and if 0.25< rho is less than or equal to 0.75, the confidence radius mu is not adjusted;
k iterations in the region with confidence radius mu, if rho<0, then let xk+1＝xkAnd reducing the confidence radius mu by half and continuing the iteration, if rho is larger than 0, making xk+1＝xk+ΔxkWhen Δ xkWhen the pose is smaller than the set threshold value, obtaining the initial optimal pose x of the robotk+1Wherein, Δ xkRepresents the step size, x, after k iterationskRepresenting the pose of the last iteration.
In the invention, in the updating and iterating process, the range of the confidence region mu is determined according to the difference between the approximate model and the actual function, and if the difference is small, the range of the confidence region is enlarged; if the difference is large, the range is narrowed. In order to judge whether the difference between the approximate model and the actual function is good or not, the fitting difference of the approximate index judging model is set, and the radius of the confidence domain is dynamically adjusted. In the expression, the numerator is a value at which the objective function actually changes, and the denominator is a value at which the approximation model changes. When the region is adjusted, rho is too small, which indicates that the value of the approximate model change is larger than the value of the actual change, namely the approximation effect is poor, and the range of the confidence domain needs to be narrowed; if rho is too large, the approximate change value is smaller than the actual change value, and mu needs to be expanded; and p is close to 1, the approximation effect is considered to be better, and the value of mu does not need to be changed.
In the embodiment of the present invention, as shown in fig. 1, in step S222, the calculation formula of the step Δ x is:
(H(x)+λI)Δx＝-J(x)Tf(x)
wherein H (x) represents an approximation of the sea plug matrix, H (x) J (x)TJ (x), I denotes the identity matrix, λ denotes the coefficient factor, j (x) denotes f (x) the first derivative with respect to x.
In the present invention, the Levenberg-Marquardt algorithm is implemented by a LagrangeThe multiplier converts a constrained optimization problem into an unconstrained optimization problem, and a linear equation for calculating increment can be obtained by expanding an unconstrained optimization equation Taylor. Wherein J (x) is the first derivative of f (x) with respect to x, which is a Jacobian matrix of L.K rows and 3 columns; λ is a coefficient factor for positively defining the matrix (H + λ I), and the initial value of λ is J (x)TJ (x) maximum value of diagonal element.
In the embodiment of the present invention, as shown in fig. 1, in step S3, the method for constructing the pose graph includes: using the pose of the robot as a vertex, and using the constraint relation between poses at different moments as edges;
based on the pose graph, calculating the pose under the condition of minimum constraint by a least square method, wherein the calculation formula is as follows:
wherein, T represents the time length,
odometer data representing the robot i at time t,
odometer data, Δ x, representing robot i at time t +1i tOdometry data representing the robot i at the time t to t +1,
an information matrix representing the observations of robot i at times t to t +1,
indicating the odometry constraint of robot i at time t,
odometry data representing robot j at time t,
representing the robot pose with the minimum residual function value between the robot i and the robot j,
representing the state constraint between robot i and robot j,
is an information matrix of observations between robot i and robot j at time t,
odometry data representing robot j at time t +1,
representing odometry data of the robot j from the moment t to the moment t +1,
is the odometry constraint for robot j at time t,
information matrix representing observed values between robots j at times t to t +1, (i, j) ∈ [1, …, N]And N represents the number of robots.
In the 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 carried out to realize multi-robot cooperation accurate positioning. Although the odometer has accumulated errors for a long time, the odometer can provide accurate pose change of the robot in a short time. And the position and attitude information provided by the odometer is combined with the position and attitude information after nonlinear optimization to obtain higher-precision positioning. The pose of the robot is used as a vertex, the constraint relation between the estimated poses at different moments is used as an edge, the pose transformation constraint of the robot at adjacent moments in the odometer is included, and the pose under the condition of minimizing the constraint is obtained through a least square method based on the position constraint of UWB.
In the embodiment of the invention, as shown in fig. 2, distance information is measured among a plurality of robots through UWB, odometer information of the robots is fused, position information is updated in real time in the moving process of the robots, and accurate positioning of cooperation of the robots is realized.
In the embodiment of the present invention, as shown in fig. 3, the solid line represents the real track of the robot 1, the dotted line represents the real track of the robot 2, and the short line represents the positioning track of the robot 2 based on UWB. It can be seen from the figure that in UWB-based positioning, the difference from the real trajectory of the robot is large, and the positioning accuracy is poor.
In the embodiment of the present invention, as shown in fig. 4, the solid line represents the real trajectory of the robot 1, the short line represents the odometer-based trajectory of the robot 1, the dotted line represents the real trajectory of the robot 2, and the small short line represents the odometer-based trajectory of the robot 2. As can be seen from the figure, the odometer has high accuracy in a short time, but there is a cumulative error with the movement of the robot, resulting in poor robot positioning accuracy.
In the embodiment of the present invention, as shown in fig. 5, a solid line in the figure represents a real track of the robot 1, a dotted line represents a track of the robot 1 fusing odometer information on the basis of UWB, a short line represents a real track of the robot 2, and a small short line represents a track of the robot 2 fusing odometer information on the basis of UWB.
The working principle and the process of the invention are as follows: the odometer and the UWB equipment in the environment are used as sensing units to realize the collection of the UWB data and the odometer data. UWB collects distance information among robots, cooperative positioning among multiple robots is achieved through nonlinear optimization, the odometer provides approximate pose change of the robots, and more accurate pose can be provided by fusing positioning information among multiple robots.
The invention has the beneficial effects that: the multi-robot cooperative positioning method utilizes the odometer and the UWB data acquisition module to acquire, and is based on a nonlinear optimization multi-robot cooperative positioning algorithm and a pose graph optimization algorithm fusing odometer information. The odometer and the UWB equipment in the environment are used as sensing units to realize the collection of the UWB data and the odometer data. UWB collects distance information among robots, cooperative positioning among multiple robots is achieved through nonlinear optimization, the odometer provides approximate pose change of the robots, and pose information after nonlinear optimization is fused through a graph optimization algorithm, so that the precision of the cooperative positioning of multiple robots is higher. The problem of the multi-robot cooperation positioning accuracy is poor is solved.
It will be appreciated by those of ordinary skill in the art that the embodiments described herein are intended to assist the reader in understanding the principles of the invention and are to be construed as being without limitation to such specifically recited embodiments and examples. Those skilled in the art can make various other specific changes and combinations based on the teachings of the present invention without departing from the spirit of the invention, and these changes and combinations are within the scope of the invention.
Claims (2)
1. A multi-robot cooperative positioning method fusing a speedometer and a UWB is characterized by comprising the following steps:
s1: the pose information of the robot and the distance information between a plurality of groups of UWB nodes are respectively collected through a speedometer and a UWB data collection module;
s2: based on the acquired distance information among the multiple groups of UWB nodes, the robot cooperative positioning is realized through a nonlinear optimization algorithm;
the step S2 includes the following sub-steps:
s21: calculating a residual function of a distance measurement value and a calculated value between the robot i and the robot j according to the distance information between the plurality of groups of UWB nodes;
s22: according to a residual function of a distance measurement value and a calculated value between the robot i and the robot j, iteration is carried out by using a Levenberg-Marquardt method to obtain the optimal pose of the robot, and multi-robot cooperative positioning is realized;
in step S21, the expression of the residual function between the distance measurement value and the calculated value between robot i and robot j is:
wherein the content of the first and second substances,
denotes a robot pose at which a residual function value between the robot i and the robot j is minimum, argmin (·) denotes a value of x that can minimize an error value, f (x) denotes a residual function, x ═ x denotes a pose of the robot, x denotes an abscissa of the robot i with respect to the robot j, y denotes an ordinate of the robot i with respect to the robot j, θ denotes a direction angle of the robot i with respect to the robot j, K denotes the number of UWB nodes on the robot i, L denotes the number of UWB nodes on the robot j,
represents the actual measurement value between the UWB node k on the robot i and the UWB node l on the robot j at the time t, d (-) represents the distance calculation between the UWB node k on the robot i and the UWB node l on the robot j under the condition that the relative pose of the robot i and the robot j is given,
indicating 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: establishing an approximate index evaluation model according to the residual function, and dynamically adjusting the size of the confidence region mu;
s222: limiting the step length delta x in a region with the confidence radius mu, and calculating delta x;
s223: performing k iterations in a region with a confidence radius of mu to obtain the optimal pose of the robot, and realizing the multi-robot cooperative positioning;
in step S221, the method of performing k iterations in the region with the confidence radius μ includes: dynamically adjusting the region with the confidence radius of mu by establishing an approximate index evaluation model, wherein the expression of the approximate index evaluation model is as follows:
where Δ 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 an approximation indicator;
the method for dynamically adjusting the region with the confidence radius mu according to the approximation index rho comprises the following steps: if 0< rho is less than or equal to 0.25, the confidence radius mu is reduced by half, if rho is greater than 0.75, the confidence radius mu is enlarged by half, and if 0.25< rho is less than or equal to 0.75, the confidence radius mu is not adjusted;
k iterations in the region with confidence radius mu, if rho<0, then let xk+1＝xkAnd reducing the confidence radius mu by half and continuing the iteration, if rho is larger than 0, making xk+1＝xk+ΔxkWhen Δ xkWhen the pose is smaller than the set threshold value, obtaining the initial optimal pose x of the robotk+1Wherein, Δ xkRepresents the step size, x, after k iterationskRepresenting the pose of the last iteration;
in step S222, the calculation formula of the step Δ x is:
(H(x)+λI)Δx＝-J(x)Tf(x)
wherein H (x) represents an approximation of the sea plug matrix, H (x) J (x)TJ (x), I denotes an identity matrix, λ denotes a coefficient factor, j (x) denotes f (x) a first derivative with respect to x;
s3: based on the nonlinear optimized multi-robot positioning information, integrating pose information provided by the robot odometer, constructing a pose graph, and optimizing to realize accurate positioning of multi-robot cooperation;
in step S3, the method for constructing the pose graph includes: using the pose of the robot as a vertex, and using the constraint relation between poses at different moments as edges;
based on the pose graph, calculating the pose under the condition of minimum constraint by a least square method, wherein the calculation formula is as follows:
wherein, T represents the time length,
odometer data representing the robot i at time t,
odometry data representing robot i at time t +1,
odometry data representing the robot i at the time t to t +1,
an information matrix representing the observations of robot i at times t to t +1,
indicating the odometry constraint of robot i at time t,
odometry data representing robot j at time t,
representing the robot pose with the minimum residual function value between the robot i and the robot j,
representing the state constraint between robot i and robot j,
is an information matrix of observations between robot i and robot j at time t,
odometry data representing robot j at time t +1,
representing odometry data of the robot j from the moment t to the moment t +1,
is the odometry constraint for robot j at time t,
information matrix representing observed values between robots j at times t to t +1, (i, j) ∈ [1, …, N]And N represents the number of robots.
2. The multi-robot cooperative positioning method integrating the odometer and the UWB according to claim 1, wherein in step S1, the method for acquiring pose information of the robot through the odometer includes: the encoder is built on the robot to be collected, and odometer data are obtained;
the method for acquiring the distance information among a plurality of groups of UWB nodes through the UWB data acquisition module comprises the following steps: and carrying UWB labels at different positions of each robot, and acquiring distance information among multiple groups of UWB nodes.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011625879.0A CN112833876B (en) | 2020-12-30 | 2020-12-30 | Multi-robot cooperative positioning method integrating odometer and UWB |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011625879.0A CN112833876B (en) | 2020-12-30 | 2020-12-30 | Multi-robot cooperative positioning method integrating odometer and UWB |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112833876A CN112833876A (en) | 2021-05-25 |
| CN112833876B true CN112833876B (en) | 2022-02-11 |
Family
ID=75924539
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011625879.0A Active CN112833876B (en) | 2020-12-30 | 2020-12-30 | Multi-robot cooperative positioning method integrating odometer and UWB |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112833876B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114623823B (en) * | 2022-05-16 | 2022-09-13 | 青岛慧拓智能机器有限公司 | UWB (ultra wide band) multi-mode positioning system, method and device integrating odometer |
| CN116753963B (en) * | 2023-08-16 | 2023-12-01 | 湖南大学 | Indoor mobile robot positioning method based on trust domain algorithm |
Citations (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101835265A (en) * | 2010-04-16 | 2010-09-15 | 大连海事大学 | Resource optimal distribution method of non-regenerative cooperative multiband ultra-wideband system |
| WO2014081945A2 (en) * | 2012-11-21 | 2014-05-30 | The Trustees Of Columbia University In The City Of New York | Systems, methods, and devices for image reconstruction using combined pde-constrained and simplified spherical harmonics algorithm |
| CN106052684A (en) * | 2016-06-16 | 2016-10-26 | 济南大学 | Mobile robot IMU/UWB/code disc loose combination navigation system and method adopting multi-mode description |
| CN106352879A (en) * | 2016-09-28 | 2017-01-25 | 深圳市普渡科技有限公司 | Pose estimation method with UWB positioning and coder fusion on basis of graph optimization |
| CN107148081A (en) * | 2017-06-02 | 2017-09-08 | 重庆邮电大学 | Mono-station location method based on nonlinear restriction least square |
| WO2018170224A2 (en) * | 2017-03-15 | 2018-09-20 | The Regents Of The University Of Michigan | Latent oscillator frequency estimation for ranging measurements |
| CN108981720A (en) * | 2018-10-25 | 2018-12-11 | 哈尔滨工业大学 | The indoor navigation system of UWB and inertial navigation fusion |
| CN109141412A (en) * | 2018-08-06 | 2019-01-04 | 济南大学 | Towards the UFIR filtering algorithm and system for having shortage of data INS/UWB combination pedestrian navigation |
| CN109946730A (en) * | 2019-03-06 | 2019-06-28 | 东南大学 | Ultra-wideband-based high-reliability fusion positioning method for vehicles under cooperation of vehicle and road |
| CN109993113A (en) * | 2019-03-29 | 2019-07-09 | 东北大学 | A kind of position and orientation estimation method based on the fusion of RGB-D and IMU information |
| CN110244715A (en) * | 2019-05-23 | 2019-09-17 | 西安理工大学 | A kind of multiple mobile robot's high-precision cooperative tracking method based on super-broadband tech |
| CN110375730A (en) * | 2019-06-12 | 2019-10-25 | 深圳大学 | The indoor positioning navigation system merged based on IMU and UWB |
| CN110645974A (en) * | 2019-09-26 | 2020-01-03 | 西南科技大学 | Mobile robot indoor map construction method fusing multiple sensors |
| CN111381586A (en) * | 2018-12-11 | 2020-07-07 | 深圳市优必选科技有限公司 | Robot and movement control method and device thereof |
| CN111417069A (en) * | 2020-03-27 | 2020-07-14 | 安徽理工大学 | Positioning device and method for coal mine underground transportation robot with wheel type odometer and UWB integrated |
| CN111678513A (en) * | 2020-06-18 | 2020-09-18 | 山东建筑大学 | Ultra-wideband/inertial navigation tight coupling indoor positioning device and system |
| CN111854733A (en) * | 2020-07-28 | 2020-10-30 | 广东博智林机器人有限公司 | Multi-source fusion positioning method and system |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8639426B2 (en) * | 2010-07-15 | 2014-01-28 | George C Dedes | GPS/IMU/video/radar absolute/relative positioning communication/computation sensor platform for automotive safety applications |
| US9606217B2 (en) * | 2012-05-01 | 2017-03-28 | 5D Robotics, Inc. | Collaborative spatial positioning |
| CN108664024B (en) * | 2018-05-08 | 2020-09-25 | 清华大学 | Motion planning and cooperative positioning method and device for unmanned vehicle network formation |
| CN110657803B (en) * | 2018-06-28 | 2021-10-29 | 深圳市优必选科技有限公司 | Robot positioning method, device and storage device |
| CN113906414A (en) * | 2019-03-05 | 2022-01-07 | 辉达公司 | Distributed processing for generating pose maps for high definition maps for navigating autonomous vehicles |
| CN111337018B (en) * | 2020-05-21 | 2020-09-01 | 上海高仙自动化科技发展有限公司 | Positioning method and device, intelligent robot and computer readable storage medium |
-
2020
- 2020-12-30 CN CN202011625879.0A patent/CN112833876B/en active Active
Patent Citations (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101835265A (en) * | 2010-04-16 | 2010-09-15 | 大连海事大学 | Resource optimal distribution method of non-regenerative cooperative multiband ultra-wideband system |
| WO2014081945A2 (en) * | 2012-11-21 | 2014-05-30 | The Trustees Of Columbia University In The City Of New York | Systems, methods, and devices for image reconstruction using combined pde-constrained and simplified spherical harmonics algorithm |
| CN106052684A (en) * | 2016-06-16 | 2016-10-26 | 济南大学 | Mobile robot IMU/UWB/code disc loose combination navigation system and method adopting multi-mode description |
| CN106352879A (en) * | 2016-09-28 | 2017-01-25 | 深圳市普渡科技有限公司 | Pose estimation method with UWB positioning and coder fusion on basis of graph optimization |
| WO2018170224A2 (en) * | 2017-03-15 | 2018-09-20 | The Regents Of The University Of Michigan | Latent oscillator frequency estimation for ranging measurements |
| CN107148081A (en) * | 2017-06-02 | 2017-09-08 | 重庆邮电大学 | Mono-station location method based on nonlinear restriction least square |
| CN109141412A (en) * | 2018-08-06 | 2019-01-04 | 济南大学 | Towards the UFIR filtering algorithm and system for having shortage of data INS/UWB combination pedestrian navigation |
| CN108981720A (en) * | 2018-10-25 | 2018-12-11 | 哈尔滨工业大学 | The indoor navigation system of UWB and inertial navigation fusion |
| CN111381586A (en) * | 2018-12-11 | 2020-07-07 | 深圳市优必选科技有限公司 | Robot and movement control method and device thereof |
| CN109946730A (en) * | 2019-03-06 | 2019-06-28 | 东南大学 | Ultra-wideband-based high-reliability fusion positioning method for vehicles under cooperation of vehicle and road |
| CN109993113A (en) * | 2019-03-29 | 2019-07-09 | 东北大学 | A kind of position and orientation estimation method based on the fusion of RGB-D and IMU information |
| CN110244715A (en) * | 2019-05-23 | 2019-09-17 | 西安理工大学 | A kind of multiple mobile robot's high-precision cooperative tracking method based on super-broadband tech |
| CN110375730A (en) * | 2019-06-12 | 2019-10-25 | 深圳大学 | The indoor positioning navigation system merged based on IMU and UWB |
| CN110645974A (en) * | 2019-09-26 | 2020-01-03 | 西南科技大学 | Mobile robot indoor map construction method fusing multiple sensors |
| CN111417069A (en) * | 2020-03-27 | 2020-07-14 | 安徽理工大学 | Positioning device and method for coal mine underground transportation robot with wheel type odometer and UWB integrated |
| CN111678513A (en) * | 2020-06-18 | 2020-09-18 | 山东建筑大学 | Ultra-wideband/inertial navigation tight coupling indoor positioning device and system |
| CN111854733A (en) * | 2020-07-28 | 2020-10-30 | 广东博智林机器人有限公司 | Multi-source fusion positioning method and system |
Non-Patent Citations (5)
| Title |
|---|
| Localization with Particle Filter Based on Odometer and UWB;Pınar Oğuz Ekim;《2020 International Congress on Human-Computer Interaction, Optimization and Robotic Applications (HORA)》;20200730;全文 * |
| Research of through-wall detection and location technique for multihuman targets using ultra wideband radar;Zhang YangLyu 等;《Chinese Medical Equipment Journal》;20160815;第37卷(第8期);10-13 * |
| 基于ＭＥＭＳ／ＵＷＢ组合的室内定位方法;郁嘉宇等;《导航定位与授时》;20181130;第5卷(第6期);73-81 * |
| 基于UWB信号室内定位系统性能优化研究;李伟杰;《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》;20150215(第02期);I136-476 * |
| 辐射环境下遥操作机器人防撞系统的设计;石正坤等;《中国核学会2011年学术年会》;20111231;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112833876A (en) | 2021-05-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112833876B (en) | Multi-robot cooperative positioning method integrating odometer and UWB | |
| CN112230243B (en) | Indoor map construction method for mobile robot | |
| CN110244715B (en) | Multi-mobile-robot high-precision cooperative tracking method based on ultra wide band technology | |
| CN112533149B (en) | Moving target positioning algorithm based on UWB mobile node | |
| CN110514225A (en) | The calibrating external parameters and precise positioning method of Multi-sensor Fusion under a kind of mine | |
| CN105263113A (en) | Wi-Fi location fingerprint map building method and system based on crowd-sourcing | |
| CN108414972B (en) | Mobile robot RFID positioning method based on phase characteristics | |
| CN109597864A (en) | Instant positioning and map constructing method and the system of ellipsoid boundary Kalman filtering | |
| CN101466070A (en) | Wireless indoor orientation method based on automatic learning of wireless signal acceptance strength distribution | |
| CN110346758A (en) | Ultra wide band positioning and alarming method for power applied to substation inspection personnel | |
| CN104469942A (en) | Indoor positioning method based on hidden Markov model | |
| CN104507159A (en) | A method for hybrid indoor positioning based on WiFi (Wireless Fidelity) received signal strength | |
| CN105773622A (en) | Industrial robot absolute accuracy calibrating method based on IEKF | |
| CN112367614B (en) | LSTM-based Wi-Fi and geomagnetic field fusion indoor positioning algorithm | |
| CN106932752A (en) | A kind of real-time indoor orientation method based on RF phse monitoring | |
| Zhang et al. | Review on UWB-based and multi-sensor fusion positioning algorithms in indoor environment | |
| CN110213813B (en) | Intelligent management method for inertial sensor in indoor positioning technology | |
| Feng et al. | Novel LiDAR-assisted UWB positioning compensation for indoor robot localization | |
| Yang et al. | Cooperative indoor localization system based uwb and random forest algorithm in complicated underground nlos scenario | |
| CN116772860A (en) | Novel indoor positioning system based on integration of wireless positioning technology and visual artificial intelligence | |
| Guo et al. | Research on indoor wireless positioning precision optimization based on UWB | |
| Kong et al. | Hybrid indoor positioning method of BLE and monocular VINS based smartphone | |
| CN109375160A (en) | A kind of angle error estimation method in Pure orientation passive location | |
| CN116419147A (en) | Indoor UWB positioning resolving method based on distance difference under non-line-of-sight environment | |
| CN114302359A (en) | High-precision indoor positioning method based on WiFi-PDR fusion |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |