WO2024093203A1 - 位姿调整方法、位姿调整装置、电子设备及可读存储介质 - Google Patents

位姿调整方法、位姿调整装置、电子设备及可读存储介质 Download PDF

Info

Publication number
WO2024093203A1
WO2024093203A1 PCT/CN2023/095353 CN2023095353W WO2024093203A1 WO 2024093203 A1 WO2024093203 A1 WO 2024093203A1 CN 2023095353 W CN2023095353 W CN 2023095353W WO 2024093203 A1 WO2024093203 A1 WO 2024093203A1
Authority
WO
WIPO (PCT)
Prior art keywords
coordinates
mobile device
noise
current moment
moment
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.)
Ceased
Application number
PCT/CN2023/095353
Other languages
English (en)
French (fr)
Inventor
吴迪
赵文泉
张琪昌
罗旭彬
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
FJ Dynamics Co Ltd
Original Assignee
FJ Dynamics Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by FJ Dynamics Co Ltd filed Critical FJ Dynamics Co Ltd
Priority to EP23866636.6A priority Critical patent/EP4394322A4/en
Priority to US18/694,511 priority patent/US12530036B2/en
Publication of WO2024093203A1 publication Critical patent/WO2024093203A1/zh
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/20—Control system inputs
    • G05D1/24—Arrangements for determining position or orientation
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/20—Control system inputs
    • G05D1/24—Arrangements for determining position or orientation
    • G05D1/245—Arrangements for determining position or orientation using dead reckoning
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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/20—Instruments for performing navigational calculations
    • 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/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/20—Control system inputs
    • G05D1/24—Arrangements for determining position or orientation
    • G05D1/247—Arrangements for determining position or orientation using signals provided by artificial sources external to the vehicle, e.g. navigation beacons
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/40—Control within particular dimensions
    • G05D1/43—Control of position or course in two dimensions [2D]
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2109/00—Types of controlled vehicles
    • G05D2109/10—Land vehicles
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2111/00—Details of signals used for control of position, course, altitude or attitude of land, water, air or space vehicles
    • G05D2111/30—Radio signals
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2111/00—Details of signals used for control of position, course, altitude or attitude of land, water, air or space vehicles
    • G05D2111/50—Internal signals, i.e. from sensors located in the vehicle, e.g. from compasses or angular sensors
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2111/00—Details of signals used for control of position, course, altitude or attitude of land, water, air or space vehicles
    • G05D2111/50—Internal signals, i.e. from sensors located in the vehicle, e.g. from compasses or angular sensors
    • G05D2111/52—Internal signals, i.e. from sensors located in the vehicle, e.g. from compasses or angular sensors generated by inertial navigation means, e.g. gyroscopes or accelerometers
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2111/00—Details of signals used for control of position, course, altitude or attitude of land, water, air or space vehicles
    • G05D2111/50—Internal signals, i.e. from sensors located in the vehicle, e.g. from compasses or angular sensors
    • G05D2111/54—Internal signals, i.e. from sensors located in the vehicle, e.g. from compasses or angular sensors for measuring the travel distances, e.g. by counting the revolutions of wheels
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2111/00—Details of signals used for control of position, course, altitude or attitude of land, water, air or space vehicles
    • G05D2111/60—Combination of two or more signals
    • G05D2111/67—Sensor fusion
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Definitions

  • the present application relates to the field of positioning technology, and in particular to a posture adjustment method, a posture adjustment device, an electronic device, and a computer-readable storage medium.
  • Robot real-time positioning technology is the key to the robot's autonomous navigation, which enables the robot to be used in various automation scenarios, such as agricultural automation and logistics automation.
  • various automation scenarios such as agricultural automation and logistics automation.
  • environmental factors in various usage scenarios such as weather, obstacles on the road, and ground bumps, there will be errors in real-time positioning. This will greatly affect the accuracy of the robot's positioning.
  • the first aspect of the present application provides a posture adjustment method, which is applied to a mobile device.
  • the posture adjustment method includes: obtaining the coordinates and noise parameters of the mobile device at the previous moment; the coordinates and noise parameters of the previous moment are predicted by a Kalman filter model; the previous moment is the previous moment adjacent to the current moment.
  • a number of measurement coordinates of the mobile device at the current moment are obtained through a number of measurement modules; based on the number of measurement coordinates, the coordinates of the previous moment and the noise parameters of the previous moment, the coordinates and noise parameters of the mobile device at the current moment are predicted by a Kalman filter model; according to the coordinates at the current moment and the preset trajectory of the mobile device, the posture of the mobile device at the current moment is adjusted.
  • the present application combines the coordinates and noise parameters of the previous moment obtained based on the Kalman filter model with a number of measurement coordinates at the current moment to predict the coordinates of the mobile device at the current moment through the Kalman filter model, so as to reduce external environmental interference and sensor interference to a large extent, thereby obtaining more accurate coordinates of the mobile device at the current moment.
  • the Kalman filter model includes a coordinate update model and a noise update model. Based on a number of measured coordinates, the coordinates at the previous moment, and the noise parameters at the previous moment, the Kalman filter model is used to predict the coordinates of the mobile device at the current moment, including: based on a number of measured coordinates, the coordinates at the previous moment, and the noise parameters at the previous moment, to respectively output corresponding calculation parameters according to the coordinate update model and the noise update model; based on a number of measured coordinates, the coordinates at the previous moment, the noise parameters at the previous moment, and the calculation parameters output by the noise update model, the coordinates at the current moment are obtained according to the coordinate update model; based on a number of measured coordinates, the coordinates at the previous moment, the noise parameters at the previous moment, and the calculation parameters output by the coordinate update model, the noise parameters at the current moment are obtained according to the noise update model.
  • the present application introduces a coordinate update model and a noise update model into the Kalman filter model, and enables the noise parameters to be dynamically involved in the real-time calculation of the coordinates at the current moment.
  • the accuracy of the coordinates at the current moment predicted based on the Kalman filter model is further improved;
  • the posture adjustment method further includes:
  • the noise parameters include observation noise parameters and state noise parameters, the observation noise parameters characterize the noise caused by the external working environment of the mobile device, and the state noise parameters characterize the noise caused by the error of the measurement module.
  • the present application introduces observation noise parameters and state noise parameters to achieve real-time dynamic prediction of observation noise parameters and state noise parameters through a Kalman filter model, and then the predicted observation noise parameters and state noise parameters at the current moment are involved in the prediction of the coordinates at the current moment, thereby solving the problem of the traditional Kalman filter divergence caused by the influence of external uncertain factors on state estimation in the multi-sensor fusion positioning method, and at the same time, it can also solve the problem that each sensor cannot eliminate the accumulated error in time and thus cannot achieve continuous seamless navigation.
  • the state noise parameters include the state noise mean and the state noise variance
  • the observation noise parameters include the observation noise mean and the observation noise variance
  • the plurality of measurement modules include a UWB module, an IMU module, and an odometer module
  • the plurality of measurement coordinates include a first measurement coordinate, a second measurement coordinate, and a third measurement coordinate.
  • the posture adjustment method further includes: obtaining the first measurement coordinate of the mobile device at the current moment through the UWB module; obtaining the second measurement coordinate of the mobile device at the current moment through the IMU module; and obtaining the third measurement coordinate of the mobile device at the current moment through the odometer module.
  • the present application obtains the plurality of measurement coordinates of the mobile device at the current moment through the UWB module, the IMU module, and the odometer module to solve the problem of poor robustness caused by a single sensor in the posture adjustment method.
  • obtaining the second measurement coordinates of the mobile device at the current moment through the IMU module includes: obtaining the posture data of the mobile device through the IMU module; filtering the posture data based on a sliding average filtering algorithm; processing the filtered posture data based on a dead reckoning algorithm to obtain the second measurement coordinates.
  • the present application first filters the posture data obtained by the IMU module through a sliding average filtering algorithm to reduce interference; and then processes the posture data through a dead reckoning algorithm to obtain a second measurement coordinate with higher accuracy.
  • the first measurement coordinate package of the mobile device at the current moment is obtained by the UWB module.
  • the invention discloses a method for transmitting a first measurement coordinate to a mobile device through a UWB module, wherein the first measurement coordinate is determined based on the distance information returned by the first measurement coordinate.
  • the invention discloses a method for transmitting a first measurement coordinate to a mobile device through a UWB module, wherein the first measurement coordinate is determined based on the distance information returned by the first measurement coordinate.
  • the invention discloses a method for transmitting a first measurement coordinate to a mobile device through a UWB module, wherein the first measurement coordinate is determined based on the distance information returned by the first measurement coordinate.
  • the invention discloses a method for transmitting a first measurement coordinate to a mobile device through a UWB module, wherein the first measurement coordinate is determined based on the distance information returned by the first measurement coordinate.
  • the second aspect of the present application provides a posture adjustment device, which is applied to a mobile device.
  • the posture adjustment device includes: a first acquisition module, which acquires the coordinates and noise parameters of the mobile device at the previous moment, the coordinates and noise parameters of the previous moment are predicted by a Kalman filter model, and the previous moment is the previous moment adjacent to the current moment; a second acquisition module, which acquires a plurality of measurement coordinates of the mobile device at the current moment through a plurality of measurement modules; a third acquisition module, which predicts the coordinates of the mobile device at the current moment through a Kalman filter model based on the plurality of measurement coordinates, the coordinates of the previous moment and the noise parameters of the previous moment; an adjustment module, which is used to adjust the posture of the mobile device at the current moment according to the coordinates at the current moment and the preset trajectory of the mobile device.
  • a third aspect of the present application provides a computer-readable storage medium, in which at least one instruction is stored, and the at least one instruction is executed by a processor in an electronic device to implement the above-mentioned posture adjustment method.
  • a fourth aspect of the present application provides an electronic device, comprising a memory storing at least one instruction; and a processor for executing the instructions stored in the memory to implement any of the posture adjustment methods described above.
  • the posture adjustment method provided in the present application predicts the coordinates of the mobile device at the current moment through a Kalman filter model based on the coordinates at the previous moment, the noise parameters at the previous moment, and a number of measured coordinates at the current moment. In this way, not only can the problem of Kalman filter divergence caused by environmental interference and sensor interference during the positioning process of the mobile device be reduced, but also the accumulated errors can be eliminated in time by continuously obtaining more accurate coordinates of the mobile device, and the deviation between the actual trajectory of the mobile device and the preset trajectory can be calibrated more effectively to achieve seamless navigation.
  • FIG1 is a flow chart of a posture adjustment method provided in an embodiment of the present application.
  • FIG. 2 is a functional module diagram of an electronic device provided in an embodiment of the application.
  • FIG. 3 is a schematic diagram of the structure of an electronic device provided in an embodiment of the present application.
  • Robot real-time positioning technology is the key to the robot's autonomous navigation, which enables the robot to be used in various automation scenarios, such as agricultural automation scenarios, logistics automation scenarios, etc.
  • various automation scenarios such as agricultural automation scenarios, logistics automation scenarios, etc.
  • environmental factors in various usage scenarios such as weather, obstacles on the road, ground bumps, etc.
  • real-time positioning errors may occur, which will greatly affect the accuracy of robot positioning.
  • an embodiment of the present application provides a posture adjustment method to achieve more accurate robot positioning.
  • Figure 1 is a flow chart of the posture adjustment method provided by the first embodiment of the present application. It should be understood that although the various steps in the flow chart of Figure 1 are displayed in sequence as indicated by the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless clearly stated in this application, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some of the steps in the figure may include multiple sub-steps or multiple stages, and these sub-steps or stages are not necessarily executed at the same time, but can be executed at different times, and their execution order is not necessarily sequential, but can be executed in turn or alternately with other steps or at least a part of the sub-steps or stages of other steps. Therefore, according to different needs, the order of the steps in the flowchart can be changed, and some steps can be omitted.
  • the posture adjustment method can be applied to a mobile device (not shown) and executed by a processor on the mobile device. It can be understood that the mobile device can be a pusher robot, a lawn mower or any other movable electronic device (see FIG. 3 ).
  • the posture adjustment method includes the following steps.
  • Step S1 Acquire the coordinates and noise parameters of the mobile device at the previous moment, and the coordinates and noise parameters at the previous moment are predicted by the Kalman filter model, wherein the previous moment is the previous moment adjacent to the current moment.
  • the processor of the mobile device when the processor of the mobile device receives the positioning instruction, the mobile device starts to move, and the processor first obtains the initial coordinates and initial noise parameters of the mobile device at the initial time before the movement starts.
  • the initial coordinates are used to describe the coordinates of the mobile device at the initial time.
  • the initial noise parameters are parameters obtained by initializing the mobile device based on the noise model at the initial time.
  • the noise model can be a Gaussian noise model, a random noise model, or a salt and pepper noise model.
  • the coordinates and noise parameters of the mobile device at the next moment after the initial moment can be predicted through the Kalman filter model based on the initial coordinates and the initial noise parameters.
  • the processor can also obtain the coordinates and noise parameters of the mobile device at the previous moment predicted by the Kalman filter model based on a preset period.
  • the previous moment refers to the previous moment adjacent to the current moment.
  • the mobile device obtains the coordinates and noise parameters of the mobile device at the previous moment predicted by the Kalman filter model every 1 second. Specifically, for example, if the time corresponding to the current moment is 12:00, the time corresponding to the previous moment is 11:59.
  • the coordinates and noise parameters of the previous moment acquired by the processor are the initial coordinates and initial noise parameters.
  • Step S2 obtaining a plurality of measurement coordinates of the mobile device at the current moment through a plurality of measurement modules respectively.
  • the mobile device moves in a preset direction, and the processor also obtains several measurement coordinates of the mobile device at the current moment through several measurement modules. It can be understood that in the embodiment of the present application, the current moment does not include the initial moment.
  • the measurement module may be a sensor related to positioning.
  • the measurement module includes but is not limited to an ultra-wideband (UWB) positioning module, an inertial measurement unit (IMU), an odometer module, or a positioning module based on a global positioning system (GPS). This application does not limit the specific type of the measurement module.
  • UWB ultra-wideband
  • IMU inertial measurement unit
  • GPS global positioning system
  • the measurement module is used to obtain the posture data of the mobile device. It can be understood that the posture data includes but is not limited to position information, linear velocity information, linear acceleration information, angular velocity information, and deflection angle information.
  • the measured coordinates may be the coordinates of the mobile device obtained by calculating the position data obtained by the measurement module. It can be understood that in the embodiment of the present application, the measured coordinates of the mobile device and the coordinates predicted by the mobile device based on the Kalman filter model are coordinates in the same coordinate system.
  • the coordinate system may be a coordinate system established with at least the initial position as the origin.
  • the several measurement modules in step S2 refer to at least one measurement module, and correspondingly, the several measurement coordinates refer to at least one measurement coordinate.
  • Step S3 Based on a number of measured coordinates, coordinates at a previous moment and noise parameters at a previous moment, the coordinates of the mobile device at the current moment are predicted by a Kalman filter model.
  • step S2 in order to obtain more accurate coordinates of the mobile device, in step S3, in addition to the measured coordinates of the mobile device at the current moment, the coordinates of the mobile device at the previous moment and the noise parameters at the previous moment are also introduced to participate in the calculation, and the coordinates of the mobile device at the current moment are predicted based on the Kalman filter model to improve the accuracy of the positioning of the mobile device.
  • the Kalman filter model includes a coordinate update model and a noise update model.
  • the coordinate update model is used to generate the coordinates of the mobile device at the next moment.
  • the noise update model is used to generate the noise parameters of the mobile device at the next moment.
  • the coordinate update model and the noise update model affect each other to generate the coordinates of the mobile device at the current moment and the noise parameters at the current moment.
  • step S3 further includes the following sub-steps:
  • Step S31 Based on a number of measured coordinates, coordinates at a previous moment, and noise parameters at a previous moment, the coordinate update model and the noise update model are used to output corresponding calculation parameters.
  • step S31 the processor outputs a first calculation parameter according to the coordinate update model based on the plurality of measured coordinates, the coordinates at the last moment, and the noise parameters at the last moment.
  • the processor also outputs a second calculation parameter according to the noise update model based on the plurality of measured coordinates, the coordinates at the last moment, and the noise parameters at the last moment.
  • Step S32 based on a number of measured coordinates, coordinates at a previous moment, noise parameters at a previous moment and calculation parameters outputted by the noise update model, the coordinates at the current moment are obtained according to the coordinate update model.
  • step S32 the processor obtains the coordinates of the mobile device at the current moment based on a number of measured coordinates, the coordinates at the previous moment, the noise parameter at the previous moment, and the second calculation parameter.
  • Step S33 based on a number of measured coordinates, coordinates at a previous moment, noise parameters at a previous moment and calculation parameters outputted by the coordinate update model, the noise parameters at the current moment are obtained according to the noise update model.
  • step S33 the processor obtains the noise parameter of the mobile device at the current moment based on the plurality of measured coordinates, the coordinates at the previous moment, the noise parameter at the previous moment and the first calculation parameter and according to the noise update model.
  • the coordinates obtained through the above steps S31-S33 are not only obtained based on the coordinates of the previous moment, the noise parameters of the previous moment and several measured coordinates, but are actually also obtained based on the noise parameters of the current moment, so that the accuracy of the coordinates of the mobile device at the current moment can be further improved.
  • the noise parameters of the mobile device at the current moment are not only obtained based on the coordinates of the previous moment, the noise parameters of the previous moment and several measured coordinates, but the noise parameters of the mobile device at the current moment are also obtained based on the coordinates of the mobile device at the current moment, that is, in the embodiment of the present application, the noise parameters of the mobile device at the current moment are updated and iterated in real time together with the coordinates of the current moment.
  • steps S31-S33 by introducing the coordinate update model and the noise update model into the Kalman filter model, and making the noise parameters dynamically participate in the prediction operation of the coordinates at the current moment in real time, the accuracy of the coordinates at the current moment predicted based on the Kalman filter model can be further improved.
  • step S31-step S32 the following content takes the coordinate update model as an extended Kalman filter model as an example, and illustrates the specific calculation process of the above steps S31-S32 in combination with the noise update model.
  • the noise parameters include observation noise parameters and state noise parameters.
  • the observation noise parameters characterize the noise caused by the external working environment of the mobile device.
  • the state noise parameters characterize the noise caused by the error of the measurement module.
  • the observation noise parameter is obtained based on Gaussian noise.
  • the state noise parameter is obtained based on another Gaussian noise.
  • the two Gaussian noises are independent of each other.
  • the observation noise parameter and the state noise parameter can also be obtained based on other noise models, such as random noise or salt and pepper noise.
  • the specific selected noise model can be determined according to the environment in which the mobile device is located, and the present application does not limit this. It can be understood that in other embodiments, the observation noise parameter and the state noise parameter can also be obtained based on different types of noise models, and the present application does not limit this.
  • the state noise parameters include the state noise mean and the state noise variance.
  • the observation noise parameters include the observation noise mean and the observation noise variance.
  • Xk ⁇ k-1 is the coordinate with error of the mobile device at the current moment that is not updated according to the variance of the state noise of the mobile device at the current moment;
  • A is the state transfer matrix of the Kalman filter model;
  • Xk-1 is the coordinate of the mobile device at the previous moment;
  • B is the control gain matrix;
  • Uk-1 is the control input variable related to the control of the mobile device at the previous moment;
  • qk-1 is the mean value of the state noise of the mobile device at the previous moment.
  • P k ⁇ k-1 is the covariance with error of the mobile device at the current moment that is not updated according to the observation matrix; P k-1 is the covariance of the noise parameter of the mobile device at the previous moment; Q k-1 is the variance of the state noise of the mobile device at the previous moment.
  • Kk is the Kalman gain
  • H is the observation matrix
  • Rk is the variance of the observation noise of the mobile device at the current moment.
  • e k is the error between the coordinate X k at the current moment and the coordinate X k ⁇ k-1 with error at the current moment
  • Z k is the observation data of the mobile device, which can be composed of posture data obtained by several measurement modules
  • r k-1 is the mean of the observation noise of the mobile device at the previous moment.
  • Xk is the coordinate of the mobile device at the current moment
  • Qk is the variance of the state noise of the mobile device at the current moment.
  • Pk is the covariance of the noise parameters of the mobile device at the current moment; I is the unit matrix.
  • the value range of b is [0.95, 0.99].
  • P k-1 is the covariance of the mobile device at the previous moment.
  • r k is the mean of the observation noise of the mobile device at the current moment.
  • R k-1 is the variance of the observation noise of the mobile device at the previous moment.
  • the processor obtains the first calculation parameters based on the acquired measurement coordinates, the coordinates at the previous moment, the noise parameters at the previous moment, and the formulas (1), (2), and (4) in the coordinate update model.
  • the first calculation parameters include the coordinates X k of the mobile device at the current moment, the coordinates X k ⁇ k-1 with errors at the current moment, the Kalman gain K k , the error e k , the covariance P k at the current moment, and the covariance P k ⁇ k-1 with errors that the mobile device has not updated according to the observation matrix at the current moment.
  • the coordinate Xk at the current moment is a parameter related to the mean qk of the state noise at the current moment;
  • the Kalman gain Kk , the error ek and the covariance Pk at the current moment are parameters related to the variance Qk of the state noise at the current moment;
  • the coordinate Xk ⁇ k-1 with error at the current moment is a parameter related to the mean rk of the observation noise at the current moment;
  • the error ek and the covariance Pk ⁇ k-1 with error are parameters related to the variance Rk of the observation noise at the current moment.
  • the processor also obtains the second calculation parameters based on the acquired measurement coordinates, the coordinates at the previous moment, the noise parameters at the previous moment, and the formulas (8) and (10) in the noise update model.
  • the second calculation parameters include the mean Q k of the observation noise of the mobile device at the current moment, and the variance R k of the observation noise of the mobile device at the current moment.
  • the second calculation parameters are all related to the Kalman gain K k and the variance R k of the observation noise of the mobile device at the current moment. Parameters related to the covariance P k of the noise parameters at the current moment.
  • step S32 the processor calculates the coordinates X k of the mobile device at the current moment based on a number of measured coordinates, the coordinates at the previous moment, the noise parameter at the previous moment and the second calculation parameter and according to the coordinate update model.
  • the processor can calculate the coordinates X k of the mobile device at the current moment according to the second calculation parameters, such as the mean Q k of the observation noise of the mobile device at the current moment, the variance R k of the observation noise of the mobile device at the current moment, and the equation group consisting of formulas (3), (5) and ( 6 ) in the coordinate update model.
  • step S33 the processor calculates the noise parameter of the mobile device at the current moment based on a number of measured coordinates, coordinates at the previous moment, noise parameters at the previous moment and the first calculation parameter and according to the noise update model.
  • the processor can calculate the noise parameters of the mobile device at the current moment (including the mean q k of the state noise at the current moment, the variance Q k of the state noise at the current moment, the mean r k of the observation noise at the current moment, and the variance R k of the observation noise at the current moment) according to the first calculation parameters, such as the coordinate X k of the mobile device at the current moment, the coordinate X k ⁇ k-1 with error, the Kalman gain K k , the error e k, the covariance P k at the current moment, and the covariance P k ⁇ k-1 with error, and the equation group composed of formulas (7), (8), ( 9) and ( 10 ) in the noise update model.
  • the processor can iteratively update the coordinates and noise parameters of each current moment through the Kalman filter model according to the coordinates and noise parameters of the previous moment and a number of measured coordinates.
  • observation noise parameters and state noise parameters can be realized through the Kalman filter model, so that the predicted observation noise parameters and state noise parameters at the current moment are involved in the prediction of the coordinates at the current moment, so as to solve the problem of traditional Kalman filtering divergence caused by the influence of external uncertain factors on state estimation in the multi-sensor fusion positioning method, and at the same time solve the problem that each sensor cannot eliminate the accumulated error in time and thus cannot achieve continuous seamless navigation.
  • the initial noise parameters include the initial observation noise mean, the initial observation noise variance, the initial state noise mean and the initial state noise variance.
  • the initial noise parameters can be obtained by randomly initializing the two Gaussian noise models mentioned above at the initial moment.
  • the coordinate update model can also be replaced with other models, such as any one of the Kalman filter models such as the optimal estimation model of the Kalman filter, the adaptive Kalman filter model, and the unscented Kalman filter, and the present application does not limit this.
  • Step S4 adjusting the position and posture of the mobile device at the current moment according to the coordinates at the current moment and the preset trajectory of the mobile device.
  • step S4 after the processor obtains the coordinates of the mobile device at the current moment according to steps S1 to S3, when the coordinates of the mobile device at the current moment deviate from the preset trajectory of the mobile device, the position and posture of the mobile device at the current moment are adjusted.
  • the preset trajectory includes a plurality of preset coordinates stored in a memory in the mobile device.
  • the processor calculates the position deviation between the coordinates at the current moment and the plurality of preset coordinates, and selects the coordinates in the plurality of preset coordinates that are consistent with the position.
  • the preset coordinates corresponding to the minimum deviation value are set as the target coordinates of the mobile device at the current moment, and then the mobile device is controlled to move to the target coordinates.
  • the posture adjustment method provided by the present application further includes:
  • Step S5 Update the coordinates and noise parameters at the current moment to the coordinates and noise parameters at the previous moment, and execute the above steps S1-S5 cyclically until receiving an instruction to stop positioning.
  • step S5 by updating the coordinates and noise parameters of the current moment to the coordinates and noise parameters of the previous moment, the mobile device can continue to obtain the coordinates and noise parameters of the current moment through the Kalman filter model based on the coordinates and noise parameters of the previous moment and several measured coordinates when the next moment arrives.
  • the several measurement modules in step S2 include a UWB module, an IMU module, and an odometer module.
  • the UWB module, the IMU module, and the odometer module are all electrically connected to the processor of the mobile device.
  • the processor obtains multiple sets of posture data of the mobile device through the several measurement modules respectively, so as to reduce the problem of poor robustness of the mobile device caused by a single sensor and improve the positioning accuracy.
  • step S2 includes the following sub-steps.
  • Step S21 obtaining the first measurement coordinates of the mobile device at the current moment through the UWB module.
  • a plurality of base stations are arranged around the route of the mobile device.
  • the processor periodically requests ranging information from the base stations through a UWB module installed on the mobile device.
  • the processor further calculates the first measurement coordinates of the mobile device in real time based on the ranging information returned by the plurality of base stations.
  • the processor can calculate the distance between the mobile device and each base station based on a two-way ranging method, a one-way ranging method or other distance calculation methods.
  • the two-way ranging method can be any one of a time of flight (TOF) ranging method, a time of arrival (TOA) ranging method, a time difference of arrival (TDOA) method or an angle of arrival (AOA) method.
  • the one-way ranging method can be a one-way ranging method based on the transmission time and arrival time of the ranging signal output by the base station, etc.
  • the processor communicates with several base stations through the UWB module to request ranging information.
  • the UWB module disconnects communication with the base station whose distance exceeds a preset threshold.
  • the processor determines the first measurement coordinates of the mobile device based on the ranging information returned by the several base stations that maintain communication with the UWB module.
  • the processor may perform calculations based on the three-point positioning principle and the multi-variable positioning algorithm according to the obtained ranging information to obtain the first measurement coordinates of the mobile device.
  • the action range of the mobile device can be expanded while obtaining the first measurement coordinates through the UWB module.
  • the preset number may be 3 or more, and this application does not limit the preset number. system.
  • Step S22 Acquire the second measurement coordinates of the mobile device at the current moment through the IMU module.
  • the IMU module includes three single-axis accelerometers and three single-axis gyroscopes.
  • the accelerometer is used to detect the acceleration signal of the mobile device on each axis in the coordinate system.
  • the gyroscope is used to detect the angular velocity and acceleration of the mobile device in the coordinate system, and use this to calculate the posture of the mobile device.
  • step S22 includes the following sub-steps:
  • Step S221 Acquire the first position data of the mobile device through the IMU module.
  • the first position data includes position data such as velocity data and acceleration data.
  • Step S222 Filter the first pose data based on a sliding average filtering algorithm.
  • Step S223 Processing the filtered first pose data based on the dead reckoning algorithm to obtain a second measurement coordinate.
  • the dead reckoning algorithm includes the following formula.
  • vt is the current speed of the mobile device
  • vt -1 is the speed of the mobile device at the last moment
  • St is the current distance of the mobile device from the starting point
  • St-1 is the distance of the mobile device from the starting point at the last moment
  • a is the linear acceleration obtained by the mobile device through the IMU module.
  • step S22 can also be implemented by other methods, and is not limited to the above steps S221-S223.
  • the mobile device is also provided with a GPS positioning module.
  • the processor can obtain the intermediate coordinates obtained before the GPS signal is disconnected through the GPS positioning module.
  • the IMU module further calculates the relative coordinates of the mobile device relative to the intermediate coordinates according to the method of the above steps S221-S223.
  • the processor can further obtain the second measurement coordinates of the mobile device at the current moment based on the intermediate coordinates and the relative coordinates.
  • Step S23 obtaining the third measurement coordinates of the mobile device at the current moment through the odometer module.
  • step S23 the processor obtains the second posture data such as the moving distance, angular velocity and linear velocity of the mobile device through the odometer module, and then derives the moving distance data in the second posture data to obtain the speed of the mobile device, so as to further calculate the third measurement coordinates of the mobile device.
  • the second posture data such as the moving distance, angular velocity and linear velocity of the mobile device
  • the moving distance data in the second posture data to obtain the speed of the mobile device, so as to further calculate the third measurement coordinates of the mobile device.
  • the first measurement coordinate, the second measurement coordinate and the third measurement coordinate are coordinates in the same coordinate system.
  • the first measurement coordinate, the second measurement coordinate and the third measurement coordinate are coordinate data in the same world coordinate system.
  • the posture adjustment method provided by the present application combined with the noise update equation, performs real-time dynamic estimation of the observation noise and state noise of the mobile device, and then uses the estimated observation noise and state noise in the Kalman filter algorithm to calculate the fusion coordinates, which solves the problem of the traditional Kalman filter divergence caused by the influence of external uncertain factors on the state estimation in the multi-sensor fusion positioning method, and can also solve the problem that each sensor cannot eliminate the accumulated error in time.
  • the problem of being unable to achieve continuous and seamless navigation has further improved the accuracy and precision of real-time positioning of mobile devices, which is beneficial to improving the operating efficiency and user experience of mobile devices.
  • the second embodiment of the present application further provides a posture adjustment device 100 , which can be applied to a mobile device.
  • the posture adjustment device 100 includes a first acquisition module 10 , a second acquisition module 20 , a third acquisition module 30 and an adjustment module 40 .
  • the first acquisition module 10 is used to acquire the fusion coordinates and noise parameters of the mobile device at the previous moment.
  • the coordinates and noise parameters at the previous moment are predicted by the Kalman filter model.
  • the previous moment is the previous moment adjacent to the current moment.
  • the second acquisition module 20 is used to acquire a plurality of corresponding measurement coordinates of the mobile device at the current moment through a plurality of measurement modules.
  • the third acquisition module 30 predicts the coordinates of the mobile device at the current moment through a Kalman filter model based on a number of measured coordinates of the mobile device, the coordinates at the previous moment, and the noise parameters at the previous moment.
  • the adjustment module 40 is used to adjust the position and posture of the mobile device at the current moment according to the coordinates at the current moment and the preset trajectory of the mobile device.
  • the various modules in the above-mentioned posture adjustment device 100 can be implemented in whole or in part by software, hardware and a combination thereof.
  • the above-mentioned modules can be embedded in or independent of the processor in hardware form, or can be stored in the memory in software form, so that the processor can call and execute the operations corresponding to the above modules.
  • the first acquisition module 10, the second acquisition module 20, the third acquisition module 30 and the adjustment module 40 are used to execute the various steps in the embodiment corresponding to Figure 1. Please refer to the relevant description of the previous embodiment for details, which will not be repeated here. Please refer to Figure 3.
  • the third embodiment of the present application also provides an electronic device 200.
  • the electronic device 200 includes a processor 201 and a memory 202.
  • the memory 202 stores a computer program 203.
  • the computer program 203 includes at least one instruction.
  • the processor 201 is used to execute at least one instruction stored in the memory 202 to implement steps S1 to S5 in the above-mentioned posture adjustment method embodiment.
  • the computer program 203 may be divided into one or more units, one or more units are stored in the memory 202, and are executed by the processor 201 to complete the present application.
  • One or more units may be a series of computer program instruction segments that can complete specific functions, and the instruction segments are used to describe the execution process of the computer program 203 in the electronic device.
  • the computer program 203 may be divided into a first acquisition unit, a second acquisition unit, a third acquisition unit, and an adjustment unit, and the specific functions of each unit are as follows:
  • the first acquisition unit is used to acquire the fusion coordinates and noise parameters of the mobile device at the previous moment.
  • the coordinates and noise parameters at the previous moment are predicted by the Kalman filter model.
  • the previous moment is the previous moment adjacent to the current moment.
  • the second acquisition unit is used to acquire a plurality of corresponding measurement coordinates of the mobile device at the current moment through a plurality of measurement modules.
  • the third acquisition unit predicts the coordinates of the mobile device at the current moment through a Kalman filter model based on a number of measured coordinates of the mobile device, the coordinates at the previous moment, and the noise parameters at the previous moment.
  • the adjustment unit is used to adjust the position and posture of the mobile device at the current moment according to the coordinates at the current moment and the preset trajectory of the mobile device.
  • the electronic device 200 may include but is not limited to a processor 201 and a memory 202. Those skilled in the art can understand that FIG3 is only an example of the electronic device 200 and does not constitute a limitation on the electronic device 200.
  • the electronic device 200 may include more or fewer components than shown in the figure, or may combine certain components, or different components.
  • the electronic device 200 may also include an input and output device, a network access device, a bus, a motion mechanism, etc.
  • the processor 201 may be a central processing unit (CPU), or It may be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc.
  • the general-purpose processor may be a microprocessor or processor 201 or any conventional processor, etc.
  • the processor 201 is the control center of the electronic device, and uses various interfaces and lines to connect various parts of the entire electronic device.
  • the memory 202 can be used to store computer programs and/or modules/units.
  • the processor 201 implements various functions of the electronic device by running or executing the computer programs and/or modules/units stored in the memory 202, and calling the data stored in the memory 202.
  • the memory 202 may mainly include a program storage area and a data storage area.
  • the program storage area may store an operating system, an application required for at least one function (such as a sound playback function, an image playback function, etc.), etc.
  • the data storage area may store data created according to the use of the electronic device (such as video data, audio data, phone book, etc.), etc.
  • the memory 202 may include a high-speed random access memory, and may also include a non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), at least one disk storage device, a flash memory device, or other volatile solid-state storage devices.
  • a non-volatile memory such as a hard disk, a memory, a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), at least one disk storage device, a flash memory device, or other volatile solid-state storage devices.
  • a non-volatile memory such as a hard disk, a memory, a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash
  • Another embodiment of the present application further provides a computer-readable storage medium, which stores a computer program including at least one instruction, and the at least one instruction is executed by a processor in an electronic device to implement the above posture adjustment method.
  • the computer program may be divided into one or more modules/units, one or more modules/units are stored in the memory 202, and executed by the processor 201 to complete the present application.
  • One or more modules/units may be a series of computer program instruction segments capable of completing specific functions, and the instruction segments are used to describe the execution process of the computer program in the electronic device.
  • the present application implements all or part of the processes in the above-mentioned embodiment method, and the computer program that can be completed by instructing the relevant hardware through a computer program can be stored in a computer-readable storage medium.
  • the computer program includes computer program code, and the computer program code can be in the form of source code, executable file or some intermediate form.
  • the computer-readable medium may include: any entity or device that can carry computer program code, recording medium, USB flash drive, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), electrical signal and software distribution medium, etc.
  • the content contained in the computer-readable medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction.
  • the computer-readable medium does not include electrical carrier signals and telecommunication signals.
  • each functional module in each embodiment of the present application can be integrated into the same processing module, or each module can exist physically separately, or two or more modules can be integrated into the same module.
  • the above integrated modules can be implemented in the form of hardware or in the form of hardware plus software functional modules.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Navigation (AREA)

Abstract

本申请提供一种位姿调整方法、位姿调整装置、电子设备及可读存储介质。该方法包括：获取移动装置在上一时刻的坐标及噪声参数，上一时刻的坐标及噪声参数通过卡尔曼滤波模型预测得到，上一时刻为与当前时刻相邻的上一时刻；通过若干测量模块获取移动装置在当前时刻的若干测量坐标；基于若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，通过卡尔曼滤波模型预测移动装置在当前时刻的坐标；根据当前时刻的坐标与移动装置的预设轨迹，调整移动装置在当前时刻的位姿。本申请提供的位姿调整方法可有效提高定位精度。

Description

位姿调整方法、位姿调整装置、电子设备及可读存储介质
本申请要求于2022年11月02日提交中国专利局、申请号为202211364479.8、名称为“位姿调整方法、位姿调整装置、电子设备及可读存储介质”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本申请涉及定位技术领域，具体涉及一种位姿调整方法、位姿调整装置、电子设备及计算机可读存储介质。
背景技术
机器人实时定位技术是机器人实现自主导航的关键，其使得机器人应用于各种自动化场景，例如农业自动化场景、物流自动化场景。然而，在机器人执行作业的过程中，由于各种使用场景中的环境因素，比如天气影响，路上的障碍物，地面颠簸等，导致实时定位有误差。如此，将极大影响机器人定位的精度。
发明内容
鉴于此，有必要提供一种位姿调整方法、电子装置、移动装置及计算机可读存储介质，以提定位精度。
本申请第一方面提供一种位姿调整方法，应用于移动装置。该位姿调整方法包括：获取移动装置在上一时刻的坐标及噪声参数；上一时刻的坐标及噪声参数通过卡尔曼滤波模型预测得到；上一时刻为与当前时刻相邻的上一时刻。通过若干测量模块获取移动装置在当前时刻的若干测量坐标；基于若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，通过卡尔曼滤波模型预测移动装置在当前时刻的坐标及噪声参数；根据当前时刻的坐标与移动装置的预设轨迹，调整移动装置在当前时刻的位姿。本申请通过将基于卡尔曼滤波模型得到的上一时刻的坐标及噪声参数，结合当前时刻的若干测量坐标，以通过卡尔曼滤波模型预测移动装置的当前时刻的坐标，以较大程度地降低外界环境干扰、传感器干扰，从而获取较准确的移动装置的当前时刻的坐标。
根据本申请的一个实施例，卡尔曼滤波模型包括坐标更新模型及噪声更新模型，基于若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，通过卡尔曼滤波模型预测移动装置在当前时刻的坐标包括：基于若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，以分别根据坐标更新模型及噪声更新模型输出对应的计算参数；基于若干测量坐标、上一时刻的坐标、上一时刻的噪声参数及基于噪声更新模型输出的计算参数，根据坐标更新模型得到当前时刻的坐标；基于若干测量坐标、上一时刻的坐标、上一时刻的噪声参数及基于坐标更新模型输出的计算参数，根据噪声更新模型得到当前时刻的噪声参数。本申请通过在卡尔曼滤波模型中引入坐标更新模型及噪声更新模型，并使噪声参数实时动态地参与进对当前时刻的坐 标的预测运算中，以进一步提高基于卡尔曼滤波模型预测得到的当前时刻的坐标的准确度；
在执行根据当前时刻的坐标与移动装置的预设轨迹，调整移动装置在当前时刻的位姿之后，位姿调整方法还包括：
将当前时刻的坐标和噪声参数更新为上一时刻的坐标及噪声参数。
根据本申请的一个实施例，噪声参数包括观测噪声参数及状态噪声参数，观测噪声参数表征移动装置的外界工作环境引起的噪声，状态噪声参数表征测量模块的误差引起的噪声。本申请通过引入观测噪声参数及状态噪声参数，以通过卡尔曼滤波模型实现对观测噪声参数及状态噪声参数的实时动态预测，进而将预测得到的当前时刻的观测噪声参数及状态噪声参数参与对当前时刻的坐标的预测，从而解决多传感器融合定位方法中状态估计受外界不确定因素的影响而导致传统卡尔曼滤波发散的问题，同时还可以解决各传感器无法及时消除累积误差从而无法实现连续无缝导航的问题。
根据本申请的一个实施例，状态噪声参数包括状态噪声均值及状态噪声方差，观测噪声参数包括观测噪声均值及观测噪声方差；噪声更新模型包括如下方程：
qk＝(1-dk)qk-1+dk(Xk-AXk-1)；

rk＝(1-dk)rk-1+dk(Zk-HXk∨k-1)；
其中，qk为移动装置在当前时刻的状态噪声的均值；dk＝(1-b)/(1-bk+1)，其中b为遗忘因子，其范围为0<b<1；qk-1为移动装置在上一时刻的状态噪声的均值；Xk为移动装置在当前时刻的坐标；A为状态转移矩阵；Xk-1为上一时刻的坐标；Qk为当前时刻的状态噪声的方差；Qk-1为上一时刻的状态噪声的方差；Kk为卡尔曼增益；ek为当前时刻的坐标Xk与当前时刻的具有误差的坐标Xk∨k-1之间的误差；Pk为当前时刻的协方差；Pk-1为上一时刻的协方差；rk为当前时刻的观测噪声的均值；rk-1为上一时刻的观测噪声的均值；Zk为移动装置的观测数据；H为观测矩阵；Xk∨k-1为移动装置在当前时刻未根据当前时刻的状态噪声的方差进行更新的、具有误差的坐标；Rk为当前时刻的观测噪声的方差；Rk-1为上一时刻的观测噪声的方差；Pk∨k-1为移动装置在当前时刻未根据观测矩阵进行更新的、具有误差的协方差。
根据本申请的一个实施例，若干测量模块包括UWB模块、IMU模块及里程计模块，若干测量坐标包括第一测量坐标、第二测量坐标及第三测量坐标。如此，位姿调整方法还包括：通过UWB模块获取移动装置在当前时刻的第一测量坐标；通过IMU模块获取移动装置在当前时刻的第二测量坐标；通过里程计模块获取移动装置在当前时刻的第三测量坐标。本申请通过UWB模块、IMU模块及里程计模块获取移动装置在当前时刻的若干测量坐标，以解决位姿调整方法中由于单一传感器所带来的鲁棒性差的问题。
根据本申请的一个实施例，通过IMU模块获取移动装置在当前时刻的第二测量坐标包括：通过IMU模块获取移动装置的位姿数据；基于滑动平均滤波算法对位姿数据进行滤波；基于航迹推算算法对滤波后的位姿数据进行处理，以得到第二测量坐标。本申请先通过滑动平均滤波算法对IMU模块获取的位姿数据滤波，以降低干扰；再通过航迹推算算法对位姿数据进行处理，以获得准确度较高的第二测量坐标。
根据本申请的一个实施例，通过UWB模块获取移动装置在当前时刻的第一测量坐标包 括：通过UWB模块与若干基站通信，以请求测距信息，且当与UWB模块通信的基站的数量大于等于预设数量时，UWB模块与距离超过预设阈值的基站断开通信；根据若干基站返回的测距信息确定第一测量坐标。本申请通过UWB模块与距离超过预设阈值的基站断开通信，从而扩大移动装置的移动范围。
本申请第二方面提供一种位姿调整装置，应用于移动装置，位姿调整装置包括：第一获取模块，获取移动装置在上一时刻的坐标及噪声参数，上一时刻的坐标及噪声参数通过卡尔曼滤波模型预测得到，上一时刻为与当前时刻相邻的上一时刻；第二获取模块，通过若干测量模块获取移动装置在当前时刻的若干测量坐标；第三获取模块，基于若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，通过卡尔曼滤波模型预测移动装置在当前时刻的坐标；调整模块，用于根据当前时刻的坐标与移动装置的预设轨迹，调整移动装置在当前时刻的位姿。
本申请第三方面提供一种计算机可读存储介质，计算机可读存储介质中存储有至少一个指令，至少一个指令被电子设备中的处理器执行以实现上述位姿调整方法。
本申请第四方面提供一种电子设备，包括存储器，存储至少一个指令；及处理器，用以执行存储器中存储的指令以实现上述任一项所述的位姿调整方法。
本申请提供的位姿调整方法，基于上一时刻的坐标、上一时刻的噪声参数、当前时刻的若干测量坐标，通过卡尔曼滤波模型预测移动装置在当前时刻的坐标，如此，不仅可降低移动装置定位过程中受环境干扰及传感器干扰而导致的卡尔曼滤波发散的问题，还可通过持续获得移动装置的较为准确的坐标，及时消除累积误差，更加有效地校准移动装置的实际轨迹与预设轨迹的偏差，实现无缝导航。
附图说明
图1是本申请一实施例提供的位姿调整方法的流程图。
图2是申请一实施例提供的电子装置的功能模块图。
图3是本申请一实施例提供的电子设备的结构示意图。
如下具体实施方式将结合上述附图进一步说明本申请。
主要元件符号说明
位姿调整装置                          100
第一获取模块                          10
第二获取模块                          20
第三获取模块                          30
调整模块                              40
电子设备                              200
处理器                                201
存储器                                202
计算机程序                            203
如下具体实施方式将结合上述附图进一步说明本申请。
具体实施方式
为了能够更清楚地理解本申请的上述目的、特征和优点，下面结合附图和具体实施例对本申请进行详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。
在下面的描述中阐述了很多具体细节以便于充分理解本申请，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本申请。
机器人实时定位技术是机器人实现自主导航的关键，其使得机器人应用于各种自动化场景，例如农业自动化场景、物流自动化场景等。然而，在机器人执行作业的过程中，由于各种使用场景中的环境因素，比如天气影响，路上的障碍物，地面颠簸等，导致实时定位有误差。如此，将极大影响机器人定位的精度。
基于此，本申请实施例提供一种位姿调整方法，以实现更精确的机器人定位。
请参阅图1，图1是本申请第一实施例提供的位姿调整方法的流程图。应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本申请中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，图中的至少部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。所以，根据不同的需求，该流程图中步骤的顺序可以改变，某些步骤可以省略。
其中，位姿调整方法可应用于移动装置(图未示)，并由移动装置上的处理器执行。可以理解，移动装置可以是推料机器人、割草机或其他任何可移动的电子设备(可参见图3)。
在一些实施例中，位姿调整方法包括如下步骤。
步骤S1：获取移动装置在上一时刻的坐标及噪声参数，且上一时刻的坐标及噪声参数通过卡尔曼滤波模型预测得到。其中，上一时刻为与当前时刻相邻的上一时刻。
在本实施例中，当移动装置的处理器接收到定位指令后，移动装置开始移动，且处理器首先获取移动装置在开始移动前的初始时刻的初始坐标及初始噪声参数。其中，初始坐标用于描述移动装置在初始时刻所在的坐标。初始噪声参数为移动装置在初始时刻基于噪声模型初始化得到的参数。例如，在一些实施例中，噪声模型可以是高斯噪声模型、随机噪声模型或椒盐噪声模型等。
处理器获取到初始坐标及初始噪声参数后，可以基于该初始坐标及初始噪声参数，通过卡尔曼滤波模型对移动装置在初始时刻过后的下一时刻的坐标及噪声参数进行预测。
如此，在开始对移动装置进行定位后，处理器还可以基于预设周期，获取通过卡尔曼滤波模型预测出的移动装置在上一时刻的坐标及噪声参数。
其中，上一时刻指与当前时刻相邻的上一时刻。例如，当预设周期为1秒时，则移动装置每隔1秒获取一次通过卡尔曼滤波模型预测出的移动装置在上一时刻的坐标及噪声参数。具体地，例如当前时刻对应的时间为12:00，则上一时刻对应的时间为11:59。
需要说明的是，若当前时刻为初始时刻的下一时刻，则处理器所获取到的上一时刻的坐标及噪声参数，为初始坐标及初始噪声参数。
步骤S2：分别通过若干测量模块获取移动装置在当前时刻的若干测量坐标。
在开始定位后，移动装置朝预设方向运动，处理器还分别通过若干测量模块获取移动装置在当前时刻的若干测量坐标。可以理解，在本申请实施例中，当前时刻不包括初始时刻。
其中，测量模块可以是与定位相关的传感器。例如，在一些实施例中，测量模块包括但不限于超宽带(Ultra Wide Band，UWB)定位模块、惯性测量模块(Inertial Measurement Unit，IMU)、里程计模块或基于全球定位系统(Global Positioning System，GPS)的定位模块等。本申请并不对测量模块的具体类型进行限制。
测量模块用于获取移动装置的位姿数据。可以理解，位姿数据包括但不限于位置信息、线速度信息、线加速度信息、角速度信息及偏角信息等。
测量坐标可以是根据测量模块获取的位姿数据进行计算，以获取到的移动装置的坐标。可以理解，在本申请实施例中，移动装置的测量坐标，与移动装置基于卡尔曼滤波模型预测得到的坐标为同一坐标系下的坐标。例如，在一些实施例中，坐标系可以是至少以初始时刻所在位置为原点所建立的坐标系。
可以理解，步骤S2中的若干测量模块指至少一个测量模块，对应地，若干测量坐标指至少一测量坐标。
步骤S3：基于若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，通过卡尔曼滤波模型预测移动装置在当前时刻的坐标。
可以理解，移动装置开始移动后，由于外界干扰或测量模块本身的误差，步骤S2中获取到的若干测量坐标，可能与移动装置的实际坐标有偏差。因此，为了得到移动装置更准确的坐标，在步骤S3中，除了移动装置在当前时刻的测量坐标，还引入移动装置上一时刻的坐标及上一时刻的噪声参数参与计算，并基于卡尔曼滤波模型预测移动装置在当前时刻的坐标，以提高移动装置定位的准确度。
进一步地，在一些实施例中，卡尔曼滤波模型包括坐标更新模型及噪声更新模型。其中，坐标更新模型用于生成移动装置的下一时刻的坐标。噪声更新模型用于生成移动装置的下一时刻的噪声参数。且坐标更新模型与噪声更新模型互相影响，以生成移动装置在当前时刻的坐标及当前时刻的噪声参数。
例如，在一些实施例中，步骤S3进一步包括如下子步骤：
步骤S31：基于若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，以分别根据坐标更新模型及噪声更新模型输出对应的计算参数。
在本申请实施例中，在步骤S31中，处理器基于若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，以根据坐标更新模型输出第一计算参数。处理器还基于若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，以根据噪声更新模型输出第二计算参数。
步骤S32：基于若干测量坐标、上一时刻的坐标、上一时刻的噪声参数及基于噪声更新模型输出的计算参数，根据坐标更新模型得到当前时刻的坐标。
在步骤S32中，处理器基于若干测量坐标、上一时刻的坐标、上一时刻的噪声参数及第二计算参数，得到所述移动装置当前时刻的坐标。
步骤S33：基于若干测量坐标、上一时刻的坐标、上一时刻的噪声参数及基于坐标更新模型输出的计算参数，根据噪声更新模型得到当前时刻的噪声参数。
在步骤S33中，处理器基于若干测量坐标、上一时刻的坐标、上一时刻的噪声参数及第一计算参数，并根据噪声更新模型得到移动装置当前时刻的噪声参数。
可以理解，通过上述步骤S31-S33获得的坐标，不仅是基于上一时刻的坐标、上一时刻的噪声参数及若干测量坐标得到的，实际上还基于当前时刻的噪声参数得到的，如此，可进一步提升移动装置当前时刻的坐标的准确度。类似地，移动装置当前时刻的噪声参数也不仅仅基于上一时刻的坐标、上一时刻的噪声参数及若干测量坐标得到的，移动装置当前时刻的噪声参数还基于移动装置当前时刻的坐标得到的，也就是说，在本申请实施例中，移动装置当前时刻的噪声参数是与当前时刻的坐标一起实时更新迭代的。
如此，在步骤S31-S33中，通过在卡尔曼滤波模型中引入坐标更新模型及噪声更新模型，并使噪声参数实时动态地参与进对当前时刻的坐标的预测运算中，以进一步提高基于卡尔曼滤波模型预测得到的当前时刻的坐标的准确度。
为了更好地说明步骤S31-步骤S32，如下内容以坐标更新模型为扩展卡尔曼滤波模型为例，并结合噪声更新模型说明上述步骤S31-S32的具体计算过程。
具体地，在一些实施例中，噪声参数包括观测噪声参数及状态噪声参数。其中，观测噪声参数表征移动装置的外界工作环境引起的噪声。状态噪声参数表征测量模块的误差引起的噪声。在本申请实施例中，观测噪声参数基于高斯噪声得到。状态噪声参数基于另一高斯噪声得到。且两高斯噪声互相独立。可以理解，在其他实施例中，观测噪声参数及状态噪声参数亦可以基于其他噪声模型得到，例如随机噪声或椒盐噪声等。具体选取的噪声模型可根据移动装置所处的环境进行决定，本申请并不对此进行限制。可以理解，在其他实施例中，观测噪声参数及状态噪声参数还可以分别基于不同类型的噪声模型得到，本申请亦不对此进行限制。
进一步地，状态噪声参数包括状态噪声均值及状态噪声方差。观测噪声参数包括观测噪声均值及观测噪声方差。
其中，坐标更新模型包括如下方程：
Xk∨k-1＝AXk-1+BUk-1+qk-1   (1)；
Pk∨k-1＝APk-1AT+Qk-1     (2)；
Kk＝Pk∨k-1HT[HPk∨k-1HT+Rk]-1  (3)；
ek＝Zk-HXk∨k-1-rk-1     (4)；
Xk＝Xk∨k-1+Qkek     (5)；
Pk＝[I-KkH]Pk∨k-1     (6)。
噪声更新模型包括如下方程：
qk＝(1-dk)qk-1+dk(Xk-AXk-1)      (7)；

rk＝(1-dk)rk-1+dk(Zk-HXk∨k-1)        (9)；
其中，在公式(1)中，Xk∨k-1为移动装置在当前时刻未根据移动装置在当前时刻的状态噪声的方差进行更新的具有误差的坐标；A为卡尔曼滤波模型的状态转移矩阵；Xk-1为移动装置上一时刻的坐标；B为控制增益矩阵；Uk-1为移动装置在上一时刻的与控制相关的控制输入变量；qk-1为移动装置在上一时刻的状态噪声的均值。
在公式(2)中，Pk∨k-1为移动装置在当前时刻未根据观测矩阵进行更新的具有误差的协方差；Pk-1为移动装置在上一时刻的噪声参数的协方差；Qk-1为移动装置在上一时刻的状态噪声的方差。
在公式(3)中，Kk为卡尔曼增益；H为观测矩阵；Rk为移动装置在当前时刻的观测噪声的方差。
在公式(4)中，ek为当前时刻的坐标Xk与当前时刻的具有误差的坐标Xk∨k-1之间的误差；Zk为移动装置的观测数据，可由若干测量模块获取的位姿数据组成；rk-1为移动装置在上一时刻的观测噪声的均值。
在公式(5)中，Xk为移动装置在当前时刻的坐标；Qk为移动装置在当前时刻的状态噪声的方差。
在公式(6)中，Pk为移动装置在当前时刻的噪声参数的协方差；I为单位矩阵。
在公式(7)中，qk为移动装置在当前时刻的状态噪声的均值；dk＝(1-b)/(1-bk+1)，其中b为遗忘因子，其范围为0<b<1。在本申请实施例中，b的取值范围为[0.95，0.99]。
在公式(8)中，Pk-1为移动装置在上一时刻的协方差。
在公式(9)中，rk为移动装置在当前时刻的观测噪声的均值。
在公式(10)中，Rk-1为移动装置在上一时刻的观测噪声的方差。
具体的，在步骤S31中，处理器基于获取到的若干测量坐标、上一时刻的坐标、上一时刻的噪声参数及坐标更新模型中的公式(1)、(2)、(4)得到第一计算参数。第一计算参数包括移动装置在当前时刻的坐标Xk、当前时刻具有误差的坐标Xk∨k-1、卡尔曼增益Kk、误差ek、当前时刻的协方差Pk，以及移动装置在当前时刻未根据观测矩阵进行更新的、具有误差的协方差Pk∨k-1。
在本申请实施例中，根据噪声更新模型，当前时刻的坐标Xk为与当前时刻的状态噪声的均值qk有关的参数；卡尔曼增益Kk、误差ek及当前时刻的协方差Pk为与当前时刻的状态噪声的方差Qk有关的参数；当前时刻具有误差的坐标Xk∨k-1为与当前时刻的观测噪声的均值rk有关的参数；误差ek及具有误差的协方差Pk∨k-1为与当前时刻的观测噪声的方差Rk有关的参数。
在步骤S31中，处理器还基于获取到的若干测量坐标、上一时刻的坐标、上一时刻的噪声参数，以及噪声更新模型中的公式(8)和公式(10)得到第二计算参数。第二计算参数包括移动装置在当前时刻的观测噪声的均值Qk，以及移动装置在当前时刻的观测噪声的方差Rk。在本申请实施例中，第二计算参数均为与卡尔曼增益Kk及与移动装置在当前时刻的在 当前时刻的噪声参数的协方差Pk有关的参数。
具体的，在步骤S32中，处理器基于若干测量坐标、上一时刻的坐标、上一时刻的噪声参数及第二计算参数，并根据坐标更新模型计算得到移动装置在当前时刻的坐标Xk。
可以理解，根据坐标更新模型，基于若干测量坐标、上一时刻的坐标、上一时刻的噪声参数，可直接得到移动装置在当前时刻具有误差的坐标Xk∨k-1、移动装置在当前时刻的具有误差的协方差Pk∨k-1及误差ek。进一步地，处理器再根据第二计算参数，例如移动装置在当前时刻的观测噪声的均值Qk、移动装置在当前时刻的观测噪声的方差Rk，及坐标更新模型中的公式(3)、(5)及(6)组成的方程组，则可计算得到移动装置在当前时刻的坐标Xk。
具体的，在步骤S33中，处理器基于若干测量坐标、上一时刻的坐标、上一时刻的噪声参数及第一计算参数，并根据噪声更新模型计算得到移动装置在当前时刻的噪声参数。
可以理解，根据噪声参数更新模型，基于若干测量坐标、上一时刻的坐标、上一时刻的噪声参数，可直接得到移动装置在当前时刻的观测噪声的均值。进一步地，处理器再根据第一计算参数，例如移动装置在当前时刻的坐标Xk、具有误差的坐标Xk∨k-1、卡尔曼增益Kk、误差ek、当前时刻的协方差Pk及具有误差的协方差Pk∨k-1，以及噪声更新模型中的公式(7)、(8)、(9)和(10)组成的方程组，则可计算得到移动装置在当前时刻的噪声参数(包括当前时刻的状态噪声的均值qk、当前时刻的状态噪声的方差Qk、当前时刻的观测噪声的均值rk及当前时刻的观测噪声的方差Rk)。
如此，基于上述步骤S31-步骤S33，在每一当前时刻到来时，处理器可根据上一时刻的坐标及噪声参数、及若干测量坐标，通过卡尔曼滤波模型对每一当前时刻的坐标及噪声参数进行迭代更新。
可以理解，通过引入观测噪声参数及状态噪声参数，以通过卡尔曼滤波模型实现对观测噪声参数及状态噪声参数的实时动态预测，从而将预测得到的当前时刻的观测噪声参数及状态噪声参数参与进对当前时刻的坐标的预测，以解决多传感器融合定位方法中状态估计受外界不确定因素的影响而导致传统卡尔曼滤波发散的问题，同时还可以解决各传感器无法及时消除累积误差从而无法实现连续无缝导航的问题。
可以理解，在本申请实施例中，在初始时刻，初始噪声参数包括初始观测噪声均值、初始观测噪声方差、初始状态噪声均值及初始状态噪声方差。且初始噪声参数可由上述提及的两高斯噪声模型分别在初始时刻经由随机初始化得到。
可以理解，在其他实施例中，坐标更新模型亦可更换为其他模型，例如卡尔曼滤波的最优估计模型、自适应卡尔曼滤波模型、无迹卡尔曼滤波等卡尔曼滤波模型中的任意一种，本申请并不对此进行限制。
步骤S4：根据当前时刻的坐标与移动装置的预设轨迹，调整移动装置在当前时刻的位姿。
在步骤S4中，当处理器根据步骤S1-步骤S3获取到移动装置在当前时刻的坐标后，当移动装置的当前时刻的坐标偏离移动装置的预设轨迹时，调整移动装置在当前时刻的位姿。
例如，在一些实施例中，预设轨迹包括存储在移动装置中的存储器中的若干预设坐标。处理器通过计算当前时刻的坐标与若干预设坐标的位置偏差，并选择若干预设坐标中与位 置偏差最小值的对应的预设坐标作为移动装置在当前时刻的目标坐标，进而控制移动装置移动至目标坐标。
可以理解，本申请并不对步骤S4中判断移动装置的当前时刻的坐标与移动装置的预设轨迹是否存在偏差的具体方法进行限制，本领域技术人员可以根据需要做出其他调整。
在一些实施例中，本申请提供的位姿调整方法还包括：
步骤S5：将当前时刻的坐标及噪声参数更新为上一时刻的坐标及噪声参数，并循环执行上述步骤S1-S5，直至接收到停止定位的指令。
可以理解，在步骤S5中，通过将当前时刻的坐标及噪声参数更新为上一时刻的坐标及噪声参数，使得移动装置在下一时刻到来时，可继续基于上一时刻的坐标及噪声参数，及若干测量坐标，通过卡尔曼滤波模型得到当前时刻的坐标及噪声参数。
可以理解，通过上述步骤S1-S5的循环，不仅可降低移动装置定位过程中受环境干扰及传感器干扰而导致的卡尔曼滤波发散的问题，还可通过持续获得移动装置的较为准确的坐标，及时消除累积误差，更加有效地校准移动装置的实际轨迹与预设轨迹的偏差，实现无缝导航。
可以理解，在一些实施例中，步骤S2中的若干测量模块包括UWB模块、IMU模块及里程计模块。且UWB模块、IMU模块及里程计模块均电连接至移动装置的处理器。如此，处理器分别通过若干测量模块获得移动装置的多组位姿数据，以降低移动装置因单一传感器而导致的鲁棒性差的问题，并提升定位精确度。
在本申请实施例中，步骤S2包括如下子步骤。
步骤S21：通过UWB模块获取移动装置在当前时刻的第一测量坐标。
在一些实施例中，移动装置行经路线的周围还设置有若干基站。处理器通过安装于移动装置上的UWB模块定时向基站请求测距信息。处理器进一步基于若干基站传回的测距信息，实时计算得到移动装置的第一测量坐标。
可以理解，处理器可基于双向测距法、单向测距法或其他距离计算方法计算得到移动装置与每一基站之间的距离。在一些实施例中，双向测距法可以是飞行时间(Time of flight，TOF)测距法、到达时间(Time of arrival，TOA)测距法、到达时间差(Time difference of arrival，TDOA)法或到达角度(Angle of arrival，AOA)法中的任意一种。单向测距法可以是基于基站输出的测距信号的发射时间与到达时间的单向测距法等。
具体的，在本申请实施例中，处理器通过UWB模块与若干基站通信，以请求测距信息。当与UWB模块通信的基站的数量大于或等于预设数量时，UWB模块与距离超过预设阈值的基站断开通信。然后，处理器根据与UWB模块保持通信的若干基站返回的测距信息，确定移动装置的第一测量坐标。
进一步地，在一些实施例中，处理器获取测距信息后，可基于三点定位原理及多变定位算法，根据获取到的测距信息进行计算，以得到移动装置的第一测量坐标。
可以理解，在本申请实施例中，通过UWB模块与多个基站之间的切换，可在通过UWB模块获取第一测量坐标的同时，扩大移动装置的行动范围。
在一些实施例中，预设数量可以是3个或3个以上，本申请并不对该预设数量进行限 制。
步骤S22：通过IMU模块获取移动装置在当前时刻的第二测量坐标。
可以理解，IMU模块包括三个单轴的加速度计和三个单轴的陀螺仪。其中，加速度计用于检测移动装置在坐标系下每一轴上的加速度信号。陀螺仪用于检测移动装置在坐标系中的角速度和加速度，并以此解算出移动装置的姿态。
在一些实施例中，步骤S22包括如下子步骤：
步骤S221：通过IMU模块获取移动装置的第一位姿数据。
可以理解，第一位姿数据包括速度数据及加速度数据等位姿数据。
步骤S222：基于滑动平均滤波算法对第一位姿数据进行滤波。
如此，以降低第一位姿数据的噪声。
步骤S223：基于航迹推算算法对滤波后的第一位姿数据进行处理，以得到第二测量坐标。
在一些实施例中，航迹推算算法包括如下公式。

其中，vt是移动装置的当前速度，vt-1是移动装置的上一时刻的速度，St是移动装置当前距离出发点的距离，St-1是移动装置上一时刻距离出发点的距离，a是移动装置通过IMU模块获取到的线加速度。如此，在步骤S223中，处理器基于上述公式(11)及(12)可计算得到移动装置运动的距离及速度方向，从而计算得到移动装置相对出发点的第二测量坐标。
可以理解，在其他实施例中，步骤S22亦可以通过其他方法实现，并不局限于上述步骤S221-S223。例如，在一些应用场景中，移动装置还设置有GPS定位模块。如此，当移动装置工作于GPS信号较差的地方，处理器可通过GPS定位模块获取GPS信号断开前得到的中间坐标。然后IMU模块进而根据上述步骤S221-S223的方法，计算出移动装置相对于该中间坐标的相对坐标。如此，处理器可进一步基于该中间坐标及相对坐标得到移动装置在当前时刻的第二测量坐标。
步骤S23：通过里程计模块获取移动装置在当前时刻的第三测量坐标。
可以理解，在步骤S23中，处理器通过里程计模块获取移动装置的移动距离、角速度及线速度等第二位姿数据，然后对第二位姿数据中的移动距离数据进行求导处理，以得到移动装置的速度，从而可进一步计算得到移动装置的第三测量坐标。
在本申请实施例中，第一测量坐标、第二测量坐标及第三测量坐标为同一坐标系下的坐标。例如，在一些实施例中，第一测量坐标、第二测量坐标及第三测量坐标均为同一世界坐标系下的坐标数据。
可以理解，在本申请实施例中，通过UWB模块、IMU模块及里程计模块获取移动装置在当前时刻的若干测量坐标，相较于仅通过单一传感器获取移动装置在当前时刻的测量坐标，可提高位姿调整方法的鲁棒性。
可以理解，本申请提供的位姿调整方法，结合噪声更新方程，对移动装置的观测噪声及状态噪声进行实时动态的估计，然后将估计出的观测噪声及状态噪声用于卡尔曼滤波计算融合坐标的算法中，解决了多传感器融合定位方法中状态估计受外界不确定因素的影响而导致传统卡尔曼滤波发散的问题，同时还可以解决各传感器无法及时消除累积误差从而 无法实现连续无缝导航的问题，使得移动装置的实时定位的准确度及精确度进一步提高，有利于提升移动装置的作业效率及用户体验。
请参阅图2，本申请第二实施例还提供一种位姿调整装置100，可应用于移动装置上。位姿调整装置100包括第一获取模块10、第二获取模块20、第三获取模块30及调整模块40。
其中，第一获取模块10用于获取移动装置在上一时刻的融合坐标及噪声参数。且上一时刻的坐标及噪声参数通过卡尔曼滤波模型预测得到。上一时刻为与当前时刻相邻的上一时刻。
第二获取模块20用于通过若干测量模块获取移动装置在当前时刻的对应的若干测量坐标。
第三获取模块30基于移动装置的若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，通过卡尔曼滤波模型预测移动装置在当前时刻的坐标。
调整模块40用于根据当前时刻的坐标与移动装置的预设轨迹，调整移动装置在当前时刻的位姿。
可以理解，上述位姿调整装置100中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于处理器中，也可以以软件形式存储于存储器中，以便于处理器调用执行以上各个模块对应的操作。其中，第一获取模块10、第二获取模块20、第三获取模块30及调整模块40用于执行图1对应的实施例中的各步骤，具体请参阅上一实施例的相关描述，在此不再赘述。请参阅图3，本申请第三实施例还提供一种电子设备200。电子设备200包括处理器201及存储器202。存储器202存储有计算机程序203。计算机程序203包括至少一个指令。处理器201用于执行存储器202中存储的至少一个指令，以实现上述位姿调整方法实施例中的步骤S1至步骤S5。
进一步地，计算机程序203可以被分割成一个或多个单元，一个或者多个单元被存储在存储器202中，并由处理器201执行，以完成本申请。一个或多个单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序203在电子设备中的执行过程。例如，计算机程序203可以被分割成第一获取单元、第二获取单元、第三获取单元及调整单元，各单元具体功能如下：
第一获取单元用于获取移动装置在上一时刻的融合坐标及噪声参数。且上一时刻的坐标及噪声参数通过卡尔曼滤波模型预测得到。上一时刻为与当前时刻相邻的上一时刻。
第二获取单元用于通过若干测量模块获取移动装置在当前时刻的对应的若干测量坐标。
第三获取单元基于移动装置的若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，通过卡尔曼滤波模型预测移动装置在当前时刻的坐标。
调整单元用于根据当前时刻的坐标与移动装置的预设轨迹，调整移动装置在当前时刻的位姿。
可以理解，电子设备200可以包括但不仅限于处理器201、存储器202。本领域技术人员可以理解，图3仅仅是电子设备200的示例，并不构成对电子设备200的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如电子设备200还可以包括输入输出设备、网络接入设备、总线、运动机构等。
可以理解，处理器201可以是中央处理模块(Central Processing Unit，CPU)，还可 以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者处理器201也可以是任何常规的处理器等，处理器201是电子设备的控制中心，利用各种接口和线路连接整个电子设备的各个部分。
存储器202可用于存储计算机程序和/或模块/单元。处理器201通过运行或执行存储在存储器202内的计算机程序和/或模块/单元，以及调用存储在存储器202内的数据，实现电子设备的各种功能。存储器202可主要包括存储程序区和存储数据区。其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等。存储数据区可存储根据电子设备的使用所创建的数据(比如视频数据、音频数据、电话本等)等。此外，存储器202可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
本申请另一实施例还提供一种计算机可读存储介质，其存储有包括至少一个指令的计算机程序，至少一个指令被电子设备中的处理器执行以实现如上的位姿调整方法。
示例性的，计算机程序可以被分割成一个或多个模块/单元，一个或者多个模块/单元被存储在存储器202中，并由处理器201执行，以完成本申请。一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，指令段用于描述计算机程序在电子设备中的执行过程。
本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成的计算机程序可存储于一计算机可读存储介质中，计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，计算机程序包括计算机程序代码，计算机程序代码可以为源代码形式、可执行文件或某些中间形式等。计算机可读介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电信号以及软件分发介质等。需要说明的是，计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
在本申请所提供的几个实施例中，应该理解到，所揭露的电子设备和方法，可以通过其它的方式实现。例如，以上所描述的电子设备实施例仅仅是示意性的，例如，模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。
另外，在本申请各个实施例中的各功能模块可以集成在相同处理模块中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在相同模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用硬件加软件功能模块的形式实现。
对于本领域技术人员而言，显然本申请不限于上述示范性实施例的细节，而且在不背离本申请的精神或基本特征的情况下，能够以其他的具体形式实现本申请。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本申请的范围由所附权 利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本申请内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外，显然“包括”一词不排除其他模块或步骤，单数不排除复数。电子设备权利要求中陈述的多个模块或电子设备也可以由同一个模块或电子设备通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。
最后应说明的是，以上实施例仅用以说明本申请的技术方案而非限制，尽管参照较佳实施例对本申请进行了详细说明，本领域的普通技术人员应当理解，可以对本申请的技术方案进行修改或等同替换，而不脱离本申请技术方案的精神和范围。

Claims (10)

  1. 一种位姿调整方法，应用于移动装置，所述位姿调整方法包括：
    获取所述移动装置在上一时刻的坐标及噪声参数，所述上一时刻的坐标及噪声参数通过卡尔曼滤波模型预测得到，所述上一时刻为与当前时刻相邻的上一时刻；
    通过若干测量模块获取所述移动装置在所述当前时刻的若干测量坐标；
    基于若干所述测量坐标、所述上一时刻的坐标及所述上一时刻的噪声参数，通过所述卡尔曼滤波模型预测所述移动装置在所述当前时刻的坐标；
    根据所述当前时刻的坐标与所述移动装置的预设轨迹，调整所述移动装置在所述当前时刻的位姿。
  2. 如权利要求1所述的位姿调整方法，其中，所述卡尔曼滤波模型包括坐标更新模型及噪声更新模型，所述基于若干所述测量坐标、所述上一时刻的坐标及所述上一时刻的噪声参数，通过所述卡尔曼滤波模型预测所述移动装置在所述当前时刻的坐标包括：
    基于若干所述测量坐标、所述上一时刻的坐标及所述上一时刻的噪声参数，以分别根据所述坐标更新模型及所述噪声更新模型输出对应的计算参数；
    基于若干所述测量坐标、所述上一时刻的坐标、所述上一时刻的噪声参数及基于所述噪声更新模型输出的计算参数，根据所述坐标更新模型得到所述当前时刻的坐标；
    基于若干所述测量坐标、所述上一时刻的坐标、所述上一时刻的噪声参数及基于所述坐标更新模型输出的计算参数，根据所述噪声更新模型得到所述当前时刻的噪声参数；
    在所述根据所述当前时刻的坐标与所述移动装置的预设轨迹，调整所述移动装置在所述当前时刻的位姿之后，所述位姿调整方法还包括：
    将所述当前时刻的坐标和噪声参数更新为所述上一时刻的坐标及噪声参数。
  3. 如权利要求1所述的位姿调整方法，其中，所述噪声参数包括观测噪声参数及状态噪声参数，所述观测噪声参数表征所述移动装置的外界工作环境引起的噪声，所述状态噪声参数表征所述测量模块的误差引起的噪声。
  4. 如权利要求3所述的位姿调整方法，其中，所述状态噪声参数包括状态噪声均值及状态噪声方差，所述观测噪声参数包括观测噪声均值及观测噪声方差；所述噪声更新模型包括如下方程：
    qk＝(1-dk)qk-1+dk(Xk-AXk-1)；

    rk＝(1-dk)rk-1+dk(Zk-HXk∨k-1)；
    其中，qk为所述移动装置在当前时刻的状态噪声的均值；dk＝(1-b)/(1-bk+1)，其中b为遗忘因子，其范围为0<b<1；qk-1为所述移动装置在上一时刻的状态噪声的均值；Xk为所述移动装置在当前时刻的坐标；A为状态转移矩阵；Xk-1为所述上一时刻的坐标；Qk为所述当前时刻的状态噪声的方差；Qk-1为所述上一时刻的状态噪声的方差；Kk为卡尔曼增益；ek为所述当前时刻的坐标Xk与所述当前时刻的具有误差的坐标Xk∨k-1之间的误差；Pk为所述当前时刻的协方差；Pk-1为所述上一时刻的协方差；rk为所述当前时刻的观测噪声的均值；rk-1为所述上一时刻的观测噪声的均值；Zk为所述移动装置的观测数据；H为观测矩阵； Xk∨k-1为所述移动装置在当前时刻未根据所述当前时刻的状态噪声的方差进行更新的、具有误差的坐标；Rk为所述当前时刻的观测噪声的方差；Rk-1为所述上一时刻的观测噪声的方差；Pk∨k-1为所述移动装置在当前时刻未根据所述观测矩阵进行更新的、具有误差的协方差。
  5. 如权利要求1所述的位姿调整方法，其中，所述若干测量模块包括UWB模块、IMU模块及里程计模块，所述若干测量坐标包括第一测量坐标、第二测量坐标及第三测量坐标，所述位姿调整方法还包括：
    通过所述UWB模块获取所述移动装置在当前时刻的所述第一测量坐标；
    通过所述IMU模块获取所述移动装置在当前时刻的所述第二测量坐标；
    通过所述里程计模块获取所述移动装置在当前时刻的所述第三测量坐标。
  6. 如权利要求5所述的位姿调整方法，其中，所述通过所述IMU模块获取所述移动装置在当前时刻的第二测量坐标包括：
    通过所述IMU模块获取所述移动装置的位姿数据；
    基于滑动平均滤波算法对所述位姿数据进行滤波；
    基于航迹推算算法对滤波后的所述位姿数据进行处理，以得到所述第二测量坐标。
  7. 如权利要求5所述的位姿调整方法，其中，所述通过所述UWB模块获取所述移动装置在当前时刻的所述第一测量坐标包括：
    通过所述UWB模块与若干基站通信，以请求测距信息，且当与所述UWB模块通信的基站的数量大于等于预设数量时，所述UWB模块与距离超过预设阈值的基站断开通信；
    根据若干所述基站返回的测距信息确定所述第一测量坐标。
  8. 一种位姿调整装置，应用于移动装置，所述位姿调整装置包括：
    第一获取模块，获取所述移动装置在上一时刻的坐标及噪声参数，所述上一时刻的坐标及噪声参数通过卡尔曼滤波模型预测得到，所述上一时刻为与当前时刻相邻的上一时刻；
    第二获取模块，通过若干测量模块获取所述移动装置在所述当前时刻的若干测量坐标；
    第三获取模块，基于若干所述测量坐标、所述上一时刻的坐标及所述上一时刻的噪声参数，通过所述卡尔曼滤波模型预测所述移动装置在所述当前时刻的坐标；
    调整模块，用于根据所述当前时刻的坐标与所述移动装置的预设轨迹，调整所述移动装置在所述当前时刻的位姿。
  9. 一种计算机可读存储介质，所述计算机可读存储介质中存储有至少一个指令，所述至少一个指令被电子设备中的处理器执行以实现如权利要求1至7中任意一项所述的位姿调整方法。
  10. 一种电子设备，所述电子设备包括：
    存储器，存储至少一个指令；及
    处理器，用以执行所述存储器中存储的指令以实现如权利要求1至7中任意一项所述的位姿调整方法。
PCT/CN2023/095353 2022-11-02 2023-05-19 位姿调整方法、位姿调整装置、电子设备及可读存储介质 Ceased WO2024093203A1 (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
EP23866636.6A EP4394322A4 (en) 2022-11-02 2023-05-19 POSE ADJUSTMENT METHOD, POSE ADJUSTMENT APPARATUS, ELECTRONIC DEVICE, AND READABLE RECORDING MEDIUM
US18/694,511 US12530036B2 (en) 2022-11-02 2023-05-19 Pose adjustment method, pose adjustment device, electronic equipment and readable storage medium

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202211364479.8 2022-11-02
CN202211364479.8A CN116659487B (zh) 2022-11-02 2022-11-02 位姿调整方法、位姿调整装置、电子设备及可读存储介质

Publications (1)

Publication Number Publication Date
WO2024093203A1 true WO2024093203A1 (zh) 2024-05-10

Family

ID=87715922

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2023/095353 Ceased WO2024093203A1 (zh) 2022-11-02 2023-05-19 位姿调整方法、位姿调整装置、电子设备及可读存储介质

Country Status (4)

Country Link
US (1) US12530036B2 (zh)
EP (1) EP4394322A4 (zh)
CN (1) CN116659487B (zh)
WO (1) WO2024093203A1 (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN120560280B (zh) * 2025-07-31 2025-12-12 诺文科风机(北京)有限公司 基于uwb定位的瓦斯巡检机器人协同控制方法及系统
CN120578874B (zh) * 2025-07-31 2025-11-18 河北师范大学 基于uwb与imu融合的外骨骼控制滤波方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105043385A (zh) * 2015-06-05 2015-11-11 北京信息科技大学 一种行人自主导航定位的自适应卡尔曼滤波方法
US20160097788A1 (en) * 2014-10-07 2016-04-07 Snappafras Corp. Pedestrian direction of motion determination system and method
CN109959381A (zh) * 2017-12-22 2019-07-02 深圳市优必选科技有限公司 一种定位方法、装置、机器人及计算机可读存储介质
CN114166218A (zh) * 2021-11-30 2022-03-11 上海智能网联汽车技术中心有限公司 基于多定位融合的室内定位导航系统及方法
CN114623823A (zh) * 2022-05-16 2022-06-14 青岛慧拓智能机器有限公司 一种融合里程计的uwb多模态定位系统、方法及装置

Family Cites Families (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6772100B2 (en) * 2001-12-17 2004-08-03 Lockheed Martin Corporation Multipath mitigation filter
US7145478B2 (en) * 2002-12-17 2006-12-05 Evolution Robotics, Inc. Systems and methods for controlling a density of visual landmarks in a visual simultaneous localization and mapping system
KR101234797B1 (ko) * 2006-04-04 2013-02-20 삼성전자주식회사 로봇 및 산출된 공분산을 이용한 로봇의 위치 추정 방법
US9482536B2 (en) * 2012-05-31 2016-11-01 International Electronic Machines Corp. Pose estimation
EP3074832A4 (en) * 2013-11-27 2017-08-30 The Trustees Of The University Of Pennsylvania Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (mav)
US9767576B2 (en) * 2014-08-22 2017-09-19 Applied Research Associates, Inc. Techniques for accurate pose estimation in outdoor environments
CN105928514A (zh) * 2016-04-14 2016-09-07 广州智能装备研究院有限公司 基于图像与惯性技术的agv复合导引系统
CN108896049A (zh) * 2018-06-01 2018-11-27 重庆锐纳达自动化技术有限公司 一种机器人室内运动定位方法
US10727949B2 (en) * 2018-10-12 2020-07-28 Hughes Network Systems, LLC. Systems and methods for high-altitude radio/optical hybrid platform
CN111380514A (zh) * 2018-12-29 2020-07-07 深圳市优必选科技有限公司 机器人位姿估计方法、装置、终端及计算机存储介质
CN110686671B (zh) * 2019-09-29 2021-11-09 同济大学 基于多传感器信息融合的室内3d实时定位方法及装置
CN111985300B (zh) * 2020-06-29 2023-11-03 魔门塔(苏州)科技有限公司 自动驾驶动态目标定位方法、装置、电子设备及存储介质
CN112013836B (zh) * 2020-08-14 2022-02-08 北京航空航天大学 一种基于改进自适应卡尔曼滤波的航姿参考系统算法
CN112671373B (zh) * 2020-12-21 2024-04-26 北京科技大学 一种基于误差控制的卡尔曼滤波自适应滤波算法
CN113280808A (zh) * 2021-05-25 2021-08-20 上海大学 一种移动机器人定位精度提升方法及系统
CN113375667B (zh) * 2021-07-15 2022-02-22 北京百度网讯科技有限公司 导航方法、装置、设备和存储介质
CN114413909A (zh) * 2022-01-25 2022-04-29 台州学院 一种室内移动机器人定位方法及系统
CN114710744B (zh) * 2022-04-12 2023-03-21 电子科技大学 一种深度融合WiFi测距与PDR推算的室内定位方法
CN114964262A (zh) * 2022-05-24 2022-08-30 深圳大学 一种基于多源融合的机器人定位方法
CN115205339B (zh) * 2022-08-11 2025-10-31 湖南工程学院 基于改进yolox的移动机器人目标跟随方法
CN117191049A (zh) * 2023-08-10 2023-12-08 北京航空航天大学 一种航空导航网络机载定位方法及系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160097788A1 (en) * 2014-10-07 2016-04-07 Snappafras Corp. Pedestrian direction of motion determination system and method
CN105043385A (zh) * 2015-06-05 2015-11-11 北京信息科技大学 一种行人自主导航定位的自适应卡尔曼滤波方法
CN109959381A (zh) * 2017-12-22 2019-07-02 深圳市优必选科技有限公司 一种定位方法、装置、机器人及计算机可读存储介质
CN114166218A (zh) * 2021-11-30 2022-03-11 上海智能网联汽车技术中心有限公司 基于多定位融合的室内定位导航系统及方法
CN114623823A (zh) * 2022-05-16 2022-06-14 青岛慧拓智能机器有限公司 一种融合里程计的uwb多模态定位系统、方法及装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP4394322A4 *

Also Published As

Publication number Publication date
CN116659487A (zh) 2023-08-29
EP4394322A1 (en) 2024-07-03
US12530036B2 (en) 2026-01-20
EP4394322A4 (en) 2025-10-22
CN116659487B (zh) 2024-07-12
US20250130573A1 (en) 2025-04-24

Similar Documents

Publication Publication Date Title
US10852139B2 (en) Positioning method, positioning device, and robot
CN109883423B (zh) 基于卡尔曼滤波的定位方法、系统、设备及存储介质
AU2018252237B2 (en) Co-localisation
CN113108791A (zh) 一种导航定位方法及导航定位设备
US20240118083A1 (en) Localisation of mobile device using image and non-image sensor data in server processing
US20210183100A1 (en) Data processing method and apparatus
WO2018214978A1 (zh) 定位装置及方法以及自动行走设备
WO2023142353A1 (zh) 一种位姿预测方法及装置
WO2024093203A1 (zh) 位姿调整方法、位姿调整装置、电子设备及可读存储介质
WO2018068771A1 (zh) 目标跟踪方法、系统、电子设备和计算机存储介质
WO2015023634A2 (en) Visual-based inertial navigation
CN113066127B (zh) 一种在线标定设备参数的视觉惯性里程计方法和系统
CN112985391A (zh) 一种基于惯性和双目视觉的多无人机协同导航方法和装置
CN112556696B (zh) 一种对象定位方法、装置、计算机设备以及存储介质
CN114001736A (zh) 定位方法、定位装置、存储介质与电子设备
CN109945864A (zh) 室内行车定位融合方法、装置、存储介质及终端设备
CN112729301A (zh) 一种基于多源数据融合的室内定位方法
CN117288178B (zh) 一种地图构建方法、装置、计算机设备及存储介质
TW202316888A (zh) 基於眾包校正的測距型定位系統及方法
CN114895241B (zh) 一种基于数据和模型联合驱动的弹性融合定位方法和装置
CN113534226A (zh) 一种基于智能手机场景识别的室内外无缝定位算法
CN116295357A (zh) 地图更新方法、装置、自移动设备及计算机可读存储介质
CN113110448B (zh) 基于最小二乘的目标检测方法、系统、机器人和存储介质
CN114698094B (zh) 一种数据处理方法及装置
CN116481538A (zh) 一种轨迹优化方法、装置、设备和存储介质

Legal Events

Date Code Title Description
WWE Wipo information: entry into national phase

Ref document number: 18694511

Country of ref document: US

ENP Entry into the national phase

Ref document number: 2023866636

Country of ref document: EP

Effective date: 20240327

WWP Wipo information: published in national office

Ref document number: 18694511

Country of ref document: US

NENP Non-entry into the national phase

Ref country code: DE

WWG Wipo information: grant in national office

Ref document number: 18694511

Country of ref document: US