Summary of the invention
The main purpose of the application is to provide a kind of localization method and device for Indoor Robot, to solve interiorPositioning is easy the problem of being influenced by environmental change.
To achieve the goals above, according to the one aspect of the application, a kind of positioning for Indoor Robot is providedMethod.
The localization method in Indoor Robot according to the application includes: the odometer information for obtaining robot, is pacified in advanceAt least one UWB is filled for providing ranging information, determines initialized location；According to real-time odometer information and described at least oneRanging information is updated position location in a UWB, which comprises determine initialized location x, y, direction yaw andCovariance matrix P；Angular speed vx, the linear velocity va in odometer information, refresh interval dt are obtained, and according to those informationOriginal state X (x, y, yaw) is updated to position；According to the X combination P obtained in real time, deterministic sampling point is generated, and is updatedThe deterministic sampling point；The estimated value for calculating X and P, obtaining N number of UWB ranging information is d1..dN, and successively more new state.
Further, when calculating the estimated value of X and P, comprising:
Wherein noise W (wx, wa) indicates line error and angle error ratio, and X ' is next position.
Further, according to the X combination P obtained in real time, deterministic sampling point is generated, and updates the deterministic sampling pointInclude:
According to the X combination P obtained in real time, deterministic sampling point: X is generated0..X2L, updating the deterministic sampling point is X‘0..X’2L,
X '=x+vx*dt*cos (yaw) * (1+wx)
Y '=y+vx*dt*sin (yaw) * (1+wx)
Yaw '=yaw+va*dt* (1+wa).
Further, method further include:
Circulation execution angular speed vx, the linear velocity va obtained in odometer information, the refresh interval dt, and according toThose information are updated original state X (x, y, yaw) to position；The X combination P that the basis obtains in real time generates certaintySampled point, and update the deterministic sampling point；The estimated value for calculating X and P, obtaining N number of UWB ranging information is d1..dN, and according toThe execution step of secondary more new state.
Further, for k-th of UWB ranging information, wherein k=1..N, and use UKF state transition equation update shapeState variable.
To achieve the goals above, according to the another aspect of the application, a kind of positioning for Indoor Robot is providedDevice.
The positioning device for Indoor Robot according to the application includes: to obtain the odometer information of robot, in advanceAt least one UWB is installed for providing ranging information, determines initialized location；According to real-time odometer information and it is described at leastRanging information is updated position location in one UWB, which comprises determining module, for determining initialized locationX, y, direction yaw and covariance matrix P；Module is obtained, for obtaining the angular speed vx in odometer information, linear velocity va,Refresh interval dt, and original state X (x, y, yaw) is updated to position according to those information；Deterministic sampling point mouldBlock for generating deterministic sampling point according to the X combination P obtained in real time, and updates the deterministic sampling point；Update module,For calculating the estimated value of X and P, obtaining N number of UWB ranging information is d1..dN, and successively more new state.
Further, the update module, is used for
Wherein noise W (wx, wa) indicates line error and angle error ratio, and X ' is next position.
Further, the deterministic sampling point module, is used for
According to the X combination P obtained in real time, deterministic sampling point: X is generated0..X2L, updating the deterministic sampling point is X‘0..X’2L,
X '=x+vx*dt*cos (yaw) * (1+wx)
Y '=y+vx*dt*sin (yaw) * (1+wx)
Yaw '=yaw+va*dt* (1+wa).
Further, further includes: loop module is used for
Circulation execution angular speed vx, the linear velocity va obtained in odometer information, the refresh interval dt, and according toThose information are updated original state X (x, y, yaw) to position；
The X combination P that the basis obtains in real time generates deterministic sampling point, and updates the deterministic sampling point；
The estimated value for calculating X and P, obtaining N number of UWB ranging information is d1..dN, and the successively execution step of more new state.
To achieve the goals above, according to the another aspect of the application, a kind of robot is provided, it is described for executingLocalization method for Indoor Robot.
In the embodiment of the present application, using determine initialized location x, the mode of y, direction yaw and covariance matrix P,By obtain odometer information in angular speed vx, linear velocity va, refresh interval dt, and according to those information to position intoRow updates original state X (x, y, yaw), has reached according to the X combination P obtained in real time, has generated deterministic sampling point, and update instituteThe purpose of deterministic sampling point is stated, to realize the estimated value for calculating X and P, obtaining N number of UWB ranging information is d1..dN, andThe successively technical effect of more new state, and then solve indoor positioning and be easy the technical issues of being influenced by environmental change.
In addition, one of the application is used for the localization method of Indoor Robot, deployment flexibly, can be according to live feelingsCondition, disposes one or more UWB, and the more precision of number are higher.And it is suitable for spacious occasion, illumination is without influence.
Specific embodiment
In order to make those skilled in the art more fully understand application scheme, below in conjunction in the embodiment of the present applicationAttached drawing, the technical scheme in the embodiment of the application is clearly and completely described, it is clear that described embodiment is onlyThe embodiment of the application a part, instead of all the embodiments.Based on the embodiment in the application, ordinary skill peopleMember's every other embodiment obtained without making creative work, all should belong to the model of the application protectionIt encloses.
It should be noted that the description and claims of this application and term " first " in above-mentioned attached drawing, "Two " etc. be to be used to distinguish similar objects, without being used to describe a particular order or precedence order.It should be understood that using in this wayData be interchangeable under appropriate circumstances, so as to embodiments herein described herein.In addition, term " includes " and " toolHave " and their any deformation, it is intended that cover it is non-exclusive include, for example, containing a series of steps or unitsProcess, method, system, product or equipment those of are not necessarily limited to be clearly listed step or unit, but may include without clearOther step or units listing to Chu or intrinsic for these process, methods, product or equipment.
In this application, term " on ", "lower", "left", "right", "front", "rear", "top", "bottom", "inner", "outside"," in ", "vertical", "horizontal", " transverse direction ", the orientation or positional relationship of the instructions such as " longitudinal direction " be orientation based on the figure orPositional relationship.These terms are not intended to limit indicated dress primarily to better describe the application and embodimentSet, element or component must have particular orientation, or constructed and operated with particular orientation.
Also, above-mentioned part term is other than it can be used to indicate that orientation or positional relationship, it is also possible to for indicating itHis meaning, such as term " on " also are likely used for indicating certain relations of dependence or connection relationship in some cases.For abilityFor the those of ordinary skill of domain, the concrete meaning of these terms in this application can be understood as the case may be.
In addition, term " installation ", " setting ", " being equipped with ", " connection ", " connected ", " socket " shall be understood in a broad sense.For example,It may be a fixed connection, be detachably connected or monolithic construction；It can be mechanical connection, or electrical connection；It can be direct phaseIt even, or indirectly connected through an intermediary, or is two connections internal between device, element or component.For those of ordinary skills, the concrete meaning of above-mentioned term in this application can be understood as the case may be.
It should be noted that in the absence of conflict, the features in the embodiments and the embodiments of the present application can phaseMutually combination.The application is described in detail below with reference to the accompanying drawings and in conjunction with the embodiments.
The locating scheme for being designed to provide a kind of Indoor Robot in the embodiment of the present application, including position and orientation letterBreath.The fusion locating scheme in conjunction with robot odometer and UWB is used by the method in the embodiment of the present application, length can be takenIt mends short, provides low cost, low calculated load, relatively reliable Global localization scheme can also be used as the means that position is given for change.
As shown in Figure 1, when executing the localization method in the embodiment of the present application, it is necessary first to obtain the odometer of robotInformation installs at least one UWB for providing ranging information in advance, determines initialized location；According to real-time odometer informationPosition location is updated with ranging information at least one described UWB.
Specifically, the odometer information (i.e. linear velocity and angular speed) to robot is used in embodiments herein, withAnd the ranging information that a UWB of N (N >=1) installed in advance is provided, initialized location is needed in embodiments herein, then rootPosition location is updated according to real-time odometer information and UWB ranging information.
Method include thes steps that following S100 to step S103:
Step S100 determines initialized location x, y, direction yaw and covariance matrix P；
Step S101 obtains angular speed vx, linear velocity va in odometer information, refresh interval dt, and according to thisA little information are updated original state X (x, y, yaw) to position；
Step S102 generates deterministic sampling point according to the X combination P obtained in real time, and updates the deterministic samplingPoint；
Step S103 calculates the estimated value of X and P, and obtaining N number of UWB ranging information is d1..dN, and successively more new state.
Specifically, it is determined that initialized location x, y, direction yaw and covariance matrix P, obtain state X (x, y, yaw).It include: position (x, y) and towards yaw and covariance matrix P.Further include noise W (wx, wa), for as line error withAngle error ratio.
Obtain angular speed vx, the linear velocity va in odometer information, refresh interval dt；
Original state X (x, y, yaw) is updated to position according to those information；
According to the state X combination covariance matrix P obtained in real time, deterministic sampling point is generated, and updates the certaintySampled point.
It should be noted that can be by setting initialized location (x, y) and direction (yaw), and can rule of thumb setDetermine the initial value of P.
It should be noted that above-mentioned generation deterministic sampling point, and update the deterministic sampling point and refer to, according to existingX, P generate deterministic sampling point be sigma points:X0..X2L.Then updating deterministic sampling point sigma points isX‘0..X’2L。
Estimated value is calculated, and according to the known position UWB uxi, uyi, range error noise_d obtains N number of UWB ranging letterBreath is d1..dN, successively more new state.
It can be seen from the above description that the application realizes following technical effect:
In embodiments herein, by initialized location, then believed according to real-time odometer information and UWB rangingBreath is to update position location.
In the embodiment of the present application, using determine initialized location x, the mode of y, direction yaw and covariance matrix P,By obtain odometer information in angular speed vx, linear velocity va, refresh interval dt, and according to those information to position intoRow updates original state X (x, y, yaw), has reached according to the X combination P obtained in real time, has generated deterministic sampling point, and update instituteThe purpose of deterministic sampling point is stated, to realize the estimated value for calculating X and P, obtaining N number of UWB ranging information is d1..dN, andThe successively technical effect of more new state, and then solve indoor positioning and be easy the technical issues of being influenced by environmental change.It is based onThe locating scheme of above-mentioned Indoor Robot, including position and orientation information.
According to the embodiment of the present application, as preferred in the present embodiment this method comprises:
When calculating the estimated value of X and P, comprising:
Wherein noise W (wx, wa) indicates line error and angle error ratio, and X ' is next position.
According to the embodiment of the present application, as preferred in the present embodiment, according to the X combination P obtained in real time, certainty is generatedSampled point, and update the deterministic sampling point and include:
According to the X combination P obtained in real time, deterministic sampling point: X is generated0..X2L, updating the deterministic sampling point is X‘0..X’2L,
X'=x+vx*dt*cos (yaw) * (1+wx)
Y'=y+vx*dt*sin (yaw) * (1+wx)
Yaw'=yaw+va*dt* (1+wa).
According to the embodiment of the present application, as preferred in the present embodiment, further includes:
Circulation execution angular speed vx, the linear velocity va obtained in odometer information, the refresh interval dt, and according toThose information are updated original state X (x, y, yaw) to position；
The X combination P that the basis obtains in real time generates deterministic sampling point, and updates the deterministic sampling point；
The estimated value for calculating X and P, obtaining N number of UWB ranging information is d1..dN, and the successively execution step of more new state.
Preferably for k-th of UWB ranging information, wherein k=1..N, and use UKF state transition equation more new stateVariable.
It should be noted that UKF state transition equation those skilled in the art can voluntarily be selected, and pass through UKFState transition equation updates the state variable in UWB ranging information.
It should be noted that step shown in the flowchart of the accompanying drawings can be in such as a group of computer-executable instructionsIt is executed in computer system, although also, logical order is shown in flow charts, and it in some cases, can be with notThe sequence being same as herein executes shown or described step.
According to the embodiment of the present application, additionally provide a kind of for implementing the positioning dress for Indoor Robot of the above methodIt sets, obtains the odometer information of robot, at least one UWB is installed for providing ranging information in advance, determines initialized location；Position location is updated according to ranging information in real-time odometer information and at least one described UWB, as shown in Fig. 2,The device comprises determining that module 10, for determining initialized location x, y, direction yaw and covariance matrix P；Obtain module20, for obtaining the angular speed vx in odometer information, linear velocity va, refresh interval dt, and aligned according to those informationIt sets and is updated original state X (x, y, yaw)；Deterministic sampling point module 30, for generating according to the X combination P obtained in real timeDeterministic sampling point, and update the deterministic sampling point；Update module 40 obtains N number of for calculating the estimated value of X and PUWB ranging information is d1..dN, and successively more new state.
In the above-mentioned module of the embodiment of the present application, specifically, it is determined that initialized location x, y, direction yaw and association sidePoor matrix P obtains state X (x, y, yaw).It include: position (x, y) and towards yaw and covariance matrix P.It further include making an uproarSound W (wx, wa), for being used as line error and angle error ratio.
Obtain angular speed vx, the linear velocity va in odometer information, refresh interval dt；
Original state X (x, y, yaw) is updated to position according to those information；
According to the state X combination covariance matrix P obtained in real time, deterministic sampling point is generated, and updates the certaintySampled point.
It should be noted that can be by setting initialized location (x, y) and direction (yaw), and can rule of thumb setDetermine the initial value of P.
It should be noted that above-mentioned generation deterministic sampling point, and update the deterministic sampling point and refer to, according to existingX, P generate deterministic sampling point be sigma points:X0..X2L.Then updating deterministic sampling point sigma points isX‘0..X’2L。
Estimated value is calculated, and according to the known position UWB uxi, uyi, range error noise_d obtains N number of UWB ranging letterBreath is d1..dN, successively more new state.
Specifically, the update module, is used for
Wherein noise W (wx, wa) indicates line error and angle error ratio, and X ' is next position.
Specifically, the deterministic sampling point module, is used for
According to the X combination P obtained in real time, deterministic sampling point: X is generated0..X2L, updating the deterministic sampling point is X‘0..X’2L,
X'=x+vx*dt*cos (yaw) * (1+wx)
Y'=y+vx*dt*sin (yaw) * (1+wx)
Yaw '=yaw+va*dt* (1+wa).
Specifically, module further include: loop module is used for
Circulation execution angular speed vx, the linear velocity va obtained in odometer information, the refresh interval dt, and according toThose information are updated original state X (x, y, yaw) to position；
The X combination P that the basis obtains in real time generates deterministic sampling point, and updates the deterministic sampling point；
The estimated value for calculating X and P, obtaining N number of UWB ranging information is d1..dN, and the successively execution step of more new state.
The realization principle of the application is as follows:
Using the odometer information (i.e. linear velocity and angular speed) to robot in the embodiment of the present application, and installation in advanceThe ranging information that provides of a UWB ranging information of N (>=1 N), need initialized location in the embodiment of the present application, then basisReal-time odometer information and UWB ranging information update position location.Basic step is as follows.
Specifically, by taking UKF is realized as an example:
State is expressed as X (x, y, yaw), respectively position (x, y) and towards yaw, covariance matrix P, noise W (wx,Wa) (line error and angle error ratio).
Initial value (can the rule of thumb set) circulation that initialized location (x, y) and direction (yaw) and P is arranged executesFollowing steps:
Odometer information, angular speed and linear velocity vx, va, refresh interval dt are obtained, updated according to this information (x,Y, yaw).
Sigma ρ oints:X is generated according to existing X, P0..X2L, update sigma points are as follows:
X‘0..X’2L:
X '=x+vx*dt*cos (yaw) * (1+wx)
Y '=y+vx*dt*sin (yaw) * (1+wx)
Yaw '=yaw+va*dt* (1+wa)
Wherein, state variable includes X-coordinate, y-coordinate and angle.
Error wx, wa are the estimation error of odometer linear velocity and angular velocity data.
It is as follows to calculate estimated value:
The known position UWB uxi, uyi, range error noise_d, obtaining N number of UWB ranging information is d1..dN, successively updateState.For k-th of UWB, state variable is updated using UKF state transition equation.
Obviously, those skilled in the art should be understood that each module of above-mentioned the application or each step can be with generalComputing device realize that they can be concentrated on a single computing device, or be distributed in multiple computing devices and formedNetwork on, optionally, they can be realized with the program code that computing device can perform, it is thus possible to which they are storedBe performed by computing device in the storage device, perhaps they are fabricated to each integrated circuit modules or by theyIn multiple modules or step be fabricated to single integrated circuit module to realize.In this way, the application be not limited to it is any specificHardware and software combines.
The foregoing is merely preferred embodiment of the present application, are not intended to limit this application, for the skill of this fieldFor art personnel, various changes and changes are possible in this application.Within the spirit and principles of this application, made any to repairChange, equivalent replacement, improvement etc., should be included within the scope of protection of this application.