WO2022261814A1 - Method and system for simultaneously performing fdd and slam under mobile robot fault - Google Patents

Method and system for simultaneously performing fdd and slam under mobile robot fault Download PDF

Info

Publication number
WO2022261814A1
WO2022261814A1 PCT/CN2021/100035 CN2021100035W WO2022261814A1 WO 2022261814 A1 WO2022261814 A1 WO 2022261814A1 CN 2021100035 W CN2021100035 W CN 2021100035W WO 2022261814 A1 WO2022261814 A1 WO 2022261814A1
Authority
WO
WIPO (PCT)
Prior art keywords
time
robot
linear velocity
sample
state mode
Prior art date
Application number
PCT/CN2021/100035
Other languages
French (fr)
Chinese (zh)
Inventor
段琢华
Original Assignee
电子科技大学中山学院
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 电子科技大学中山学院 filed Critical 电子科技大学中山学院
Priority to PCT/CN2021/100035 priority Critical patent/WO2022261814A1/en
Publication of WO2022261814A1 publication Critical patent/WO2022261814A1/en
Priority to ZA2023/07096A priority patent/ZA202307096B/en

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • 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

Definitions

  • the invention relates to the field of robot fault diagnosis and mapping, in particular to a method and system for simultaneous FDD and SLAM under the fault of a mobile robot.
  • FDD Fault Detection and Diagnosis
  • Detection is to determine whether the system is faulty
  • diagnosis is to determine the faulty component and the type of fault, that is, to determine the behavior mode of each component.
  • Simutaneous Localization and Mapping is one of the key issues for autonomous navigation of mobile robots. When the left wheel encoder and/or the right wheel encoder of a mobile robot fails, how to perform concurrent localization and mapping is critical. However, at present, there is no solution to simultaneously solve the problems of mobile robot fault diagnosis and concurrent localization and mapping.
  • the purpose of the present invention is to provide a method and system for simultaneous FDD and SLAM under the failure of a mobile robot.
  • the present invention provides the following scheme:
  • a method for simultaneous FDD and SLAM under mobile robot faults including:
  • attitude samples of the robot at time t are determined by the empirical probability of each state mode of the robot at time t, and the empirical probability of each state mode of the robot at time t is determined by the robot t - State mode estimation and state transition probability determination at time t; attitude sample distribution in any state mode at time t follows the normal distribution of robot attitude in the corresponding state mode, and the mean value in the normal distribution is determined by the corresponding state mode
  • the estimated values of the left and right wheel linear velocities are determined by the linear velocities measured by the encoders of the left and right wheels at time t or by the linear velocities measured by the encoders of the left and right wheels and the driving speed at time t.
  • the normal distribution The standard deviation in is determined by the empirical standard deviation of the corresponding state mode;
  • the state mode includes normal mode, left wheel encoder failure mode, right wheel encoder failure mode and left and right wheel encoder failure mode simultaneously;
  • the environment point cloud data of robot at t moment and the environment map estimate of robot at t-1 moment determine the weight of each attitude sample;
  • described environment point cloud data is actual measurement data;
  • each attitude sample belongs at t moment and the weight of each attitude sample at t moment, calculate the estimated probability of occurrence of each state pattern at t moment, and select the state pattern with the largest estimated probability as the state pattern estimate at t moment;
  • the target attitude sample includes the attitude sample corresponding to the state mode estimation at time t;
  • the environment map estimation at time t is determined.
  • determining the weight of each attitude sample according to the attitude sample of the robot at time t, the environment point cloud data of the robot at time t, and the environment map estimation of the robot at time t-1 specifically includes:
  • the calculation of the estimated probability of occurrence of each state mode at time t according to the state mode to which each attitude sample belongs at time t and the weight of each attitude sample at time t specifically includes:
  • S k represents the k-th state mode
  • s t represents the state mode at time t
  • the i-th attitude sample at time t When belonging to the state mode S k , is 1, otherwise is 0,
  • N indicates the number of attitude samples at time t.
  • the normal distribution of the robot attitude includes the normal distribution of the linear velocity and the normal distribution of the yaw rate
  • the mean value in the normal distribution of the linear velocity is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode
  • the The standard deviation in the normal distribution of the linear velocity is determined by the empirical standard deviation of the corresponding state mode
  • the mean value in the normal distribution of the yaw rate is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode
  • the yaw rate is positive
  • the standard deviation in the state distribution is determined from the empirical standard deviation of the corresponding state pattern.
  • sampling linear velocity samples and sampling yaw rate samples include:
  • W represents the axis length connecting the left and right wheels of the robot, Indicates the estimated linear velocity of the left wheel of the robot at time t, Represents the estimated value of the linear velocity of the right wheel of the robot at time t; wherein, in the normal mode, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the linear velocity of the right wheel of the robot at time t is estimated is the linear velocity measured by the encoder of the right wheel; in the failure mode of the left wheel encoder, the estimated value of the linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel, and the estimated value of the linear velocity of the right wheel of the robot at time t is the right
  • the encoder of the wheel measures the linear velocity; in the failure mode of the right wheel encoder, the estimated linear velocity of the
  • the present invention also provides a system for simultaneous FDD and SLAM under the failure of a mobile robot, including:
  • the attitude sample acquisition module is used to acquire the attitude samples of the robot at time t; wherein, the distribution probability of the attitude samples of the robot at each state mode at time t is determined by the empirical probability of each state mode of the robot at time t, and each state mode occurs at time t of the robot.
  • the empirical probability of the mode is determined according to the state mode estimation and state transition probability of the robot at time t-1; the attitude sample distribution in any state mode at time t follows the normal distribution of robot attitude in the corresponding state mode, and the The mean value is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode.
  • the estimated value of the linear velocity of the left and right wheels is determined by the linear velocity measured by the encoder of the left and right wheels at time t or by the linear velocity measured by the encoder of the left and right wheels at time t and the driving speed It is determined that the standard deviation in the normal distribution is determined by the empirical standard deviation of the corresponding state mode; the state mode includes a normal mode, a failure mode of the left wheel encoder, a failure mode of the right wheel encoder, and a simultaneous failure mode of the left and right wheel encoders;
  • the sample weight determination module is used to determine the weight of each attitude sample according to the attitude sample of the robot at time t, the environment point cloud data of the robot at time t, and the environment map estimation of the robot at time t-1; wherein, the environment point cloud data is actual measurement data;
  • the state mode estimation determination module is used to calculate the estimated probability of each state mode at time t according to the state mode to which each attitude sample belongs at time t and the weight of each attitude sample at time t, and select the state mode with the largest estimated probability as the state mode at time t state mode estimation;
  • the pose estimation determination module is used to use the pose sample with the largest posterior probability in the target pose sample as the pose estimate of the robot at time t, and the target pose sample includes a pose sample corresponding to the state mode estimation at time t;
  • the environment map estimation determination module is used to determine the environment map estimation at time t according to the environment point cloud data of the robot at time t and the pose estimation of the robot at time t.
  • the sample weight determination module specifically includes:
  • the registration point pair determination unit is used to use the attitude sample as the initial pose estimation, and use the iterative closest point method to determine the registration point pair between the environmental point cloud data at time t and the environmental map estimation at time t-1 during the registration process;
  • a distance calculation unit configured to calculate the distance between two points in each of the registration point pairs
  • a weight determining unit configured to determine a ratio of the registration point pairs whose distance is smaller than a set threshold among the registration point pairs, and use the ratio as the weight of the posture sample at time t.
  • the state mode estimation determination module specifically includes:
  • the state mode estimation probability calculation unit is used for Calculate the estimated probability of occurrence of each state mode at time t Among them, S k represents the k-th state mode, s t represents the state mode at time t, when the i-th attitude sample at time t When belonging to the state mode S k , is 1, otherwise is 0, Indicates the normalized weight of the i-th attitude sample at time t, and N indicates the number of attitude samples at time t.
  • the system also includes:
  • the line speed sample sampling module is used to follow the empirical probability of the occurrence of each state mode at time t and the line speed normal distribution under each state mode of the robot, and sample the line speed sample;
  • the yaw rate sample sampling module is used to sample the yaw rate samples according to the empirical probability of occurrence of each state mode at time t and the normal distribution of the yaw rate under each state mode of the robot;
  • Attitude sample determination module used to determine the attitude sample of the robot at time t according to the linear velocity sample and the yaw rate sample;
  • the normal distribution of the robot attitude includes the normal distribution of the linear velocity and the normal distribution of the yaw rate
  • the mean value in the normal distribution of the linear velocity is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode
  • the The standard deviation in the normal distribution of the linear velocity is determined by the empirical standard deviation of the corresponding state mode
  • the mean value in the normal distribution of the yaw rate is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode
  • the yaw rate is positive
  • the standard deviation in the state distribution is determined from the empirical standard deviation of the corresponding state pattern.
  • the system also includes:
  • Linear velocity normal distribution determination module for:
  • the mean in the normal distribution of the line speed at time t in Indicates the estimated linear velocity of the left wheel of the robot at time t, Represents the estimated value of the linear velocity of the right wheel of the robot at time t; wherein, in the normal mode, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the linear velocity of the right wheel of the robot at time t is estimated is the linear velocity measured by the encoder of the right wheel; in the failure mode of the left wheel encoder, the estimated value of the linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel, and the estimated value of the linear velocity of the right wheel of the robot at time t is the right The encoder of the wheel measures the linear velocity; in the failure mode of the right wheel encoder, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the estimated linear velocity of the right wheel of the robot at time t is the right
  • the yaw rate normal distribution determination module is used for:
  • W represents the axis length connecting the left and right wheels of the robot.
  • the embodiment of the present invention first estimates the state mode at this moment, then selects an accurate pose estimation in the state mode, and finally, according to the pose estimation, The estimation of the environmental map at the previous moment and the environmental point cloud data at the current moment are carried out by using the map matching method to establish the map at this moment. Simultaneous fault diagnosis and concurrent positioning and mapping are realized in the state mode of the mobile robot.
  • FIG. 1 is a flow chart of a method for simultaneous FDD and SLAM under a mobile robot failure provided in Embodiment 1 of the present invention
  • Fig. 2 is a schematic diagram of the system structure of simultaneous FDD and SLAM under the failure of the mobile robot provided by Embodiment 2 of the present invention.
  • this embodiment provides a method for simultaneous FDD and SLAM under a mobile robot failure.
  • the mobile robot targeted by this method is a two-wheel differential robot, with encoders installed on the left and right wheels, and a two-dimensional laser radar installed on the mobile robot body. , the method includes the following steps:
  • Step 101 Obtain a pose sample of the robot at time t.
  • the distribution probability of the attitude samples of the robot on each state mode at time t is determined by the empirical probability of each state mode of the robot at time t, and the empirical probability of each state mode of the robot at time t is estimated according to the state mode of the robot at time t-1 and State transition probabilities are determined.
  • the state mode is shown in Table 1, and the state transition probability can be shown in Table 2.
  • the empirical probability of occurrence of each state mode at time t is obtained, and the attitude samples are selected according to the empirical probability.
  • the state mode at time t-1 is estimated to be S 1
  • the empirical probabilities of each state mode S 1 , S 2 , S 3 , and S 4 at time t are 0.5, 0.2, 0.2,0.1.
  • the attitude samples belonging to state mode S1 account for 0.5
  • the attitude samples belonging to state mode S2 account for 0.2
  • the attitude samples belonging to state mode S3 account for 0.2
  • the attitude samples belonging to state mode S4 account for 0.1.
  • the attitude sample distribution in any state mode at time t follows the normal distribution of the robot attitude in the corresponding state mode, and the mean value in the normal distribution is determined by the estimated values of the left and right wheel linear velocities in the corresponding state mode, and the left and right wheel lines
  • the speed estimate is determined by the linear velocity measured by the encoders of the wheels around the time t or by the linear velocity measured by the encoders of the wheels around the time t and the driving speed, and the standard deviation in the normal distribution is determined by the empirical standard deviation of the corresponding state mode .
  • the empirical standard deviation of state mode S 1 is ⁇ 1
  • the empirical standard deviation of state mode S 2 is ⁇ 2
  • the empirical standard deviation of state mode S 3 is ⁇ 3
  • the empirical standard deviation of state mode S 4 is ⁇ 4 .
  • the 50 attitude samples belonging to the state mode S 1 follow the normal distribution ( ⁇ 1 , ⁇ 1 ), the 20 attitude samples belonging to the state mode S 2 follow the normal distribution ( ⁇ 2 , ⁇ 2 ), and belong to the state mode S
  • the 20 attitude samples of 3 follow normal distribution ( ⁇ 3 , ⁇ 3 ), and the 10 attitude samples belonging to state pattern S 4 follow normal distribution ( ⁇ 4 , ⁇ 4 ).
  • ⁇ 1 , ⁇ 2 , ⁇ 3 , ⁇ 4 are determined according to the estimated values of the left and right wheel linear velocities in the state mode, for example, ⁇ 1 is determined by the estimated values of the left and right wheel linear velocities in the state mode S 1 with Determined, ⁇ 2 is determined by the estimated value of the linear velocity of the left and right wheels in the state mode S 2 with Sure.
  • the method of determining the estimated linear velocity of the left and right wheels in different state modes is as follows:
  • the estimated linear velocity of the robot’s left wheel at time t is the linear velocity measured by the encoder of the left wheel, and the estimated linear velocity of the robot’s right wheel at time t is the linear velocity measured by the encoder of the right wheel ;
  • the estimated linear velocity of the robot’s left wheel at time t is the driving speed of the left wheel, and the estimated linear velocity of the robot’s right wheel at time t is the linear velocity measured by the encoder of the right wheel;
  • the linear velocity of the robot’s left wheel at time t is The estimated value is the linear velocity measured by the encoder of the left wheel, and the estimated linear velocity of the robot’s right wheel at time t is the driving speed of the right wheel ;
  • the estimated linear velocity of the robot’s left wheel at time t is the driving speed of the left wheel
  • the estimated linear velocity of the right wheel of the robot at time t is the driving speed of the right wheel.
  • Step 102 Determine the weight of each pose sample according to the pose sample of the robot at time t, the environment point cloud data of the robot at time t, and the environment map estimation at time t-1.
  • the environmental point cloud data in this embodiment is obtained by actually scanning and measuring the surrounding environment of the robot by laser radar.
  • step 102 specifically includes:
  • Step 103 Calculate the estimated probability of occurrence of each state mode at time t according to the state mode to which each attitude sample belongs and the weight of each attitude sample at time t, and select the state mode with the highest estimated probability as the state mode estimation at time t.
  • step 103 includes:
  • S k represents the k-th state mode
  • s t represents the state mode at time t
  • the i-th attitude sample at time t When belonging to the state mode S k , is 1, otherwise is 0,
  • N indicates the number of attitude samples at time t.
  • Step 104 Use the pose sample with the highest posterior probability among the target pose samples as the pose estimation of the robot at time t, the target pose sample including the pose sample corresponding to the state mode estimation at time t.
  • Step 105 According to the environment point cloud data of the robot at time t and the pose estimation of the robot at time t, determine the environment map estimation at time t.
  • the normal distribution of robot attitude includes linear velocity normal distribution and yaw rate normal distribution. In one example, before step 101, it also includes:
  • the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel
  • the estimated linear velocity of the right wheel of the robot at time t is the linear velocity measured by the encoder of the right wheel
  • the estimated linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel
  • the estimated linear velocity of the right wheel of the robot at time t is the encoder measuring linear velocity of the right wheel
  • the estimated line speed of the left wheel of the robot at time t is the encoder measurement line speed of the left wheel
  • the estimated line speed of the right wheel of the robot at time t is the driving speed of the right wheel
  • the estimated linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel
  • the estimated linear velocity of the right wheel of the robot at time t is the driving speed of the right wheel.
  • the mean value in the normal distribution of the linear velocity is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode
  • the standard deviation in the normal distribution of the linear velocity is determined by the empirical standard deviation of the corresponding state mode
  • the yaw rate is determined by the empirical standard deviation in the corresponding state mode.
  • v t and Respectively represent the robot linear velocity (unit: m/s) and yaw rate (unit: radian/s) at time t, and ⁇ represents the sampling interval (unit: s).
  • W represents the axis length connecting the left and right wheels of the mobile robot, unit: m
  • r represents the radius of the left and right wheels of the mobile robot, unit: m
  • M t respectively represent the readings of the left wheel encoder, right wheel encoder and two-dimensional laser radar at time t
  • ⁇ t is the environment map at time t, represented by a two-dimensional laser point cloud
  • s t is the state mode at time t, and the value range is shown in Table 1
  • the output is the state mode at time t pose at time t map at time t
  • Each sensor has two modes: normal and fault.
  • a mobile robot with two encoders has four state modes, including one normal mode and three a failure mode.
  • the number of particles is set to N, and the particle set is initialized
  • the state mode at time t is
  • the method uses the particle filter as the framework to realize FDD and SLAM of the mobile robot at the same time.
  • the results of FDD are used to provide kinematic models under different fault conditions, and more accurate pose estimation is performed on the kinematic model adapted to the fault.
  • a perceptual model for fault diagnosis and location is realized by map matching method.
  • this embodiment provides a system for simultaneous FDD and SLAM under a mobile robot failure, the system includes:
  • the attitude sample acquisition module 201 is used to acquire the attitude samples of the robot at time t; wherein, the distribution probability of the attitude samples of the robot at each state mode at time t is determined by the empirical probability of each state mode of the robot at time t, and each state mode occurs at time t of the robot.
  • the empirical probability of the state mode is determined according to the state mode estimation and state transition probability of the robot at time t-1; the attitude sample distribution in any state mode at time t follows the normal distribution of the robot attitude in the corresponding state mode, and in the normal distribution
  • the mean value of is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode.
  • the estimated value of the linear velocity of the left and right wheels is determined by the linear velocity measured by the encoder of the left and right wheels at time t or by the linear velocity measured by the encoder of the left and right wheels at time t and the driving
  • the speed is determined, and the standard deviation in the normal distribution is determined by the empirical standard deviation of the corresponding state mode;
  • the state mode includes normal mode, left wheel encoder failure mode, right wheel encoder failure mode and left and right wheel encoder failure mode simultaneously ;
  • the sample weight determination module 202 is used to determine the weight of each pose sample according to the pose sample of the robot at time t, the robot environment point cloud data at time t, and the environment map estimation at time t-1, and the robot lidar is used to measure the environment map ;
  • the state mode estimation determination module 203 is used to calculate the estimated probability of occurrence of each state mode at time t according to the state mode to which each attitude sample belongs at time t and the weight of each attitude sample at time t, and select the state mode with the largest estimated probability as time time t
  • the pose estimation determination module 204 is used to use the pose sample with the largest posterior probability in the target pose sample as the pose estimate of the robot at time t, the target pose sample including the pose sample corresponding to the state mode estimation at time t;
  • the environment map estimation determination module 205 is configured to determine the environment map estimation at time t according to the robot environment point cloud data at time t and the pose estimation of the robot at time t.
  • the sample weight determination module 202 specifically includes:
  • the registration point pair determination unit is used to use the attitude sample as the initial pose estimation, and use the iterative closest point method to determine the registration point pair between the environmental point cloud data at time t and the environmental map estimation at time t-1 during the registration process;
  • a distance calculation unit configured to calculate the distance between two points in each of the registration point pairs
  • a weight determining unit configured to determine a ratio of the registration point pairs whose distance is smaller than a set threshold among the registration point pairs, and use the ratio as the weight of the posture sample at time t.
  • the state mode estimation determination module 203 specifically includes:
  • the state mode estimation probability calculation unit is used for Calculate the estimated probability of occurrence of each state mode at time t Among them, S k represents the k-th state mode, s t represents the state mode at time t, when the i-th attitude sample at time t When belonging to the state mode S k , is 1, otherwise is 0, Indicates the normalized weight of the i-th attitude sample at time t, and N indicates the number of attitude samples at time t.
  • system provided by this embodiment further includes: a linear velocity normal distribution determination module, a yaw rate normal distribution determination module, a linear velocity sample sampling module, a yaw rate sample sampling module, and an attitude sample determination module. in:
  • Line speed normal distribution determination module for:
  • the yaw rate normal distribution determination module is used for:
  • W represents the axis length connecting the left and right wheels of the robot
  • the line speed sample sampling module is used to follow the empirical probability of the occurrence of each state mode at time t and the line speed normal distribution under each state mode of the robot, and sample the line speed sample;
  • the yaw rate sample sampling module is used to sample the yaw rate samples according to the empirical probability of occurrence of each state mode at time t and the normal distribution of the yaw rate under each state mode of the robot;
  • An attitude sample determining module configured to determine an attitude sample of the robot at time t according to the linear velocity sample and the yaw rate sample.
  • the normal distribution of the robot attitude includes the normal distribution of the linear velocity and the normal distribution of the yaw rate
  • the mean value in the normal distribution of the linear velocity is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode
  • the The standard deviation in the normal distribution of the linear velocity is determined by the empirical standard deviation of the corresponding state mode
  • the mean value in the normal distribution of the yaw rate is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode
  • the yaw rate is positive
  • the standard deviation in the state distribution is determined from the empirical standard deviation of the corresponding state pattern.
  • W represents the axis length connecting the left and right wheels of the robot, Indicates the estimated linear velocity of the left wheel of the robot at time t, Represents the estimated value of the linear velocity of the right wheel of the robot at time t; wherein, in the normal mode, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the linear velocity of the right wheel of the robot at time t is estimated is the linear velocity measured by the encoder of the right wheel; in the failure mode of the left wheel encoder, the estimated value of the linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel, and the estimated value of the linear velocity of the right wheel of the robot at time t is the right
  • the encoder of the wheel measures the linear velocity; in the failure mode of the right wheel encoder, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the estimated linear velocity of the right wheel of the robot at time

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

A method and system for simultaneously performing FDD and SLAM under a mobile robot fault. The method comprises: acquiring posture samples of a robot at a moment t (S101); determining the weight of each posture sample according to the posture samples of the robot at the moment t, environmental point cloud data of the robot at the moment t, and an environmental map estimation at a moment t−1 (S102); according to a state mode to which each posture sample belongs at the moment t and the weight of each posture sample at the moment t, calculating an estimation probability of each state mode occurring at the moment t, selecting a state mode with the maximum estimation probability, and taking same as a state mode estimation at the moment t (S103); taking a posture sample with the maximum posterior probability from among target posture samples as a posture estimation of the robot at the moment t, wherein the target posture samples comprise a posture sample corresponding to the state mode estimation at the moment t (S104); and according to the environmental point cloud data and the posture estimation of the robot at the moment t, determining an environmental map estimation at the moment t (S105). By means of the method, fault diagnosis and simultaneous localization and mapping can be simultaneously performed under a mobile robot fault.

Description

移动机器人故障下同时FDD和SLAM的方法及系统Method and system for simultaneous FDD and SLAM under mobile robot failure 技术领域technical field
本发明涉及机器人故障诊断与建图领域，特别是涉及一种移动机器人故障下同时FDD和SLAM的方法及系统。The invention relates to the field of robot fault diagnosis and mapping, in particular to a method and system for simultaneous FDD and SLAM under the fault of a mobile robot.
背景技术Background technique
故障检测与诊断(Fault Detection and Diagnosis,FDD)包含检测与诊断两个过程，检测是确定系统是否发生故障，诊断则是确定发生故障的部件以及故障类型，即确定各部件的行为模式。并发定位与建图(Simutaneous Localization and Mapping，SLAM)是移动机器人自主导航的关键问题之一。当移动机器人的左轮编码器和/或右轮编码器发生故障时，如何进行并发定位与建图十分关键，但目前，尚未出现同时解决移动机器人故障诊断和并发定位与建图问题的方案。Fault Detection and Diagnosis (FDD) includes two processes of detection and diagnosis. Detection is to determine whether the system is faulty, and diagnosis is to determine the faulty component and the type of fault, that is, to determine the behavior mode of each component. Simutaneous Localization and Mapping (SLAM) is one of the key issues for autonomous navigation of mobile robots. When the left wheel encoder and/or the right wheel encoder of a mobile robot fails, how to perform concurrent localization and mapping is critical. However, at present, there is no solution to simultaneously solve the problems of mobile robot fault diagnosis and concurrent localization and mapping.
发明内容Contents of the invention
本发明的目的是提供一种移动机器人故障下同时FDD和SLAM的方法及系统。The purpose of the present invention is to provide a method and system for simultaneous FDD and SLAM under the failure of a mobile robot.
为实现上述目的，本发明提供了如下方案：To achieve the above object, the present invention provides the following scheme:
一种移动机器人故障下同时FDD和SLAM的方法，包括：A method for simultaneous FDD and SLAM under mobile robot faults, including:
获取t时刻机器人的姿态样本；其中，t时刻机器人的姿态样本在各状态模式上的分布概率由t时刻机器人发生各状态模式的经验概率确定，机器人t时刻发生各状态模式的经验概率根据机器人t-1时刻的状态模式估计以及状态转移概率确定；t时刻任一状态模式下的姿态样本分布遵循对应状态模式下机器人姿态的正态分布，所述正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述左右车轮线速度估计值由t时刻左右车轮的编码器测量线速度确定或由t时刻左右车轮的编码器测量线速度以及驱动速度确定，所述正态分布中的标准差由对应状态模式的经验标准差确定；所述状态模式包括正常模式、左轮编码器故障模式、右轮编码器故障模式以及左右轮编码器同时故障模式；Obtain the attitude samples of the robot at time t; among them, the distribution probability of the attitude samples of the robot on each state mode at time t is determined by the empirical probability of each state mode of the robot at time t, and the empirical probability of each state mode of the robot at time t is determined by the robot t - State mode estimation and state transition probability determination at time t; attitude sample distribution in any state mode at time t follows the normal distribution of robot attitude in the corresponding state mode, and the mean value in the normal distribution is determined by the corresponding state mode The estimated values of the left and right wheel linear velocities are determined by the linear velocities measured by the encoders of the left and right wheels at time t or by the linear velocities measured by the encoders of the left and right wheels and the driving speed at time t. The normal distribution The standard deviation in is determined by the empirical standard deviation of the corresponding state mode; The state mode includes normal mode, left wheel encoder failure mode, right wheel encoder failure mode and left and right wheel encoder failure mode simultaneously;
根据t时刻机器人的姿态样本、t时刻机器人的环境点云数据以及t-1时刻机器人的环境地图估计，确定各姿态样本的权重；其中，所述环境点云数据为实际测量数据；According to the posture sample of robot at t moment, the environment point cloud data of robot at t moment and the environment map estimate of robot at t-1 moment, determine the weight of each attitude sample; Wherein, described environment point cloud data is actual measurement data;
根据t时刻各姿态样本所属的状态模式以及t时刻各姿态样本的权重，计算t时刻发生各状态模式的估计概率，并选取估计概率最大的状态模式作为t时刻的状态模式估计；According to the state pattern to which each attitude sample belongs at t moment and the weight of each attitude sample at t moment, calculate the estimated probability of occurrence of each state pattern at t moment, and select the state pattern with the largest estimated probability as the state pattern estimate at t moment;
将目标姿态样本中后验概率最大的姿态样本作为t时刻机器人的位姿估计，所述目标姿态样本包括所述t时刻的状态模式估计对应的姿态样本；Using the attitude sample with the largest posterior probability in the target attitude sample as the pose estimation of the robot at time t, the target attitude sample includes the attitude sample corresponding to the state mode estimation at time t;
根据t时刻机器人的环境点云数据以及t时刻机器人的位姿估计，确定t时刻的环境地图估计。According to the environment point cloud data of the robot at time t and the pose estimation of the robot at time t, the environment map estimation at time t is determined.
可选的，所述根据t时刻机器人的姿态样本、t时刻机器人的环境点云数据以及t-1时刻机器人的环境地图估计，确定各姿态样本的权重，具体包括：Optionally, determining the weight of each attitude sample according to the attitude sample of the robot at time t, the environment point cloud data of the robot at time t, and the environment map estimation of the robot at time t-1, specifically includes:
对t时刻任一所述姿态样本：For any attitude sample at time t:
以所述姿态样本为初始位姿估计，利用迭代最近点方法确定t时刻环境点云数据与t-1时刻的环境地图估计在配准过程中的配准点对；Using the attitude sample as the initial pose estimation, using the iterative closest point method to determine the registration point pairs between the environmental point cloud data at time t and the environmental map estimation at time t-1 during the registration process;
计算各所述配准点对中两点的距离；Calculating the distance between two points in each of the registration points;
确定所述配准点对中所述距离小于设定阈值的配准点对的比例，并将所述比例作为t时刻所述姿态样本的权重。Determining the ratio of the registration point pairs whose distance is smaller than the set threshold among the registration point pairs, and using the ratio as the weight of the attitude sample at time t.
可选的，所述根据t时刻各姿态样本所属的状态模式以及t时刻各姿态样本的权重，计算t时刻发生各状态模式的估计概率，具体包括：Optionally, the calculation of the estimated probability of occurrence of each state mode at time t according to the state mode to which each attitude sample belongs at time t and the weight of each attitude sample at time t specifically includes:
根据
Figure PCTCN2021100035-appb-000001
计算t时刻发生各状态模式的估计概率
Figure PCTCN2021100035-appb-000002
其中，S k表示第k状态模式，s t表示t时刻的状态模式，当t时刻的第i姿态样本
Figure PCTCN2021100035-appb-000003
属于状态模式S k时，
Figure PCTCN2021100035-appb-000004
为1，否则
Figure PCTCN2021100035-appb-000005
为0，
Figure PCTCN2021100035-appb-000006
表示t时刻的第i姿态样本归一化后的权重，N表示t时刻姿态样本的数量。 according to
Figure PCTCN2021100035-appb-000001
Calculate the estimated probability of occurrence of each state mode at time t
Figure PCTCN2021100035-appb-000002
Among them, S k represents the k-th state mode, s t represents the state mode at time t, when the i-th attitude sample at time t
Figure PCTCN2021100035-appb-000003
When belonging to the state mode S k ,
Figure PCTCN2021100035-appb-000004
is 1, otherwise
Figure PCTCN2021100035-appb-000005
is 0,
Figure PCTCN2021100035-appb-000006
Indicates the normalized weight of the i-th attitude sample at time t, and N indicates the number of attitude samples at time t.
可选的，在所述获取t时刻机器人的姿态样本之前，还包括：Optionally, before acquiring the attitude sample of the robot at time t, it also includes:
遵循t时刻各状态模式发生的经验概率以及机器人各状态模式下的线速度正态分布，采样线速度样本；Follow the empirical probability of occurrence of each state mode at time t and the normal distribution of the line speed in each state mode of the robot, and sample line speed samples;
遵循t时刻各状态模式发生的经验概率以及机器人各状态模式下的偏航率正态分布，采样偏航率样本；Follow the empirical probability of occurrence of each state mode at time t and the normal distribution of yaw rate in each state mode of the robot, and sample the yaw rate samples;
根据所述线速度样本和所述偏航率样本确定t时刻机器人的姿态样本；Determine the attitude sample of the robot at time t according to the linear velocity sample and the yaw rate sample;
其中，所述机器人姿态的正态分布包括线速度正态分布和偏航率正态分布，所述线速度正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述线速度正态分布中的标准差由对应状态模式的经验标准差确定，所述偏航率正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述偏航率正态分布中的标准差由对应状态模式的经验标准差确定。Wherein, the normal distribution of the robot attitude includes the normal distribution of the linear velocity and the normal distribution of the yaw rate, and the mean value in the normal distribution of the linear velocity is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode, and the The standard deviation in the normal distribution of the linear velocity is determined by the empirical standard deviation of the corresponding state mode, the mean value in the normal distribution of the yaw rate is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode, and the yaw rate is positive The standard deviation in the state distribution is determined from the empirical standard deviation of the corresponding state pattern.
可选的，在采样线速度样本和采样偏航率样本之前，还包括：Optionally, before sampling linear velocity samples and sampling yaw rate samples, include:
获取机器人的左车轮线速度和右车轮线速度；Obtain the linear velocity of the left wheel and the right wheel of the robot;
根据
Figure PCTCN2021100035-appb-000007
计算t时刻线速度正态分布中的均值
Figure PCTCN2021100035-appb-000008
和t时刻偏航率正态分布中的均值
Figure PCTCN2021100035-appb-000009
其中，W表示连接机器人左右车轮的轴长，
Figure PCTCN2021100035-appb-000010
表示t时刻机器人的左车轮线速度估计值，
Figure PCTCN2021100035-appb-000011
表示t时刻机器人的右车轮线速度估计值；其中，在所述正常模式下，t时刻机器人的左车轮线速度估计值为左车轮的编码器测量线速度，t时刻机器人的右车轮线速度估计值为右车轮的编码器测量线速度；在所述左轮编码器故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的驱动速度，t时刻机器人的右车轮线速度估计值为右车轮的编码器测量线速度；在右轮编码器故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的编码器测量线速度，t时刻机器人的右车轮线速度估计值为右车轮的驱动速度；在所述左右轮编码器同时故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的 驱动速度，t时刻机器人的右车轮线速度估计值为右车轮的驱动速度。 according to
Figure PCTCN2021100035-appb-000007
Compute the mean in the normal distribution of the line speed at time t
Figure PCTCN2021100035-appb-000008
and the mean value in the normal distribution of the yaw rate at time t
Figure PCTCN2021100035-appb-000009
Among them, W represents the axis length connecting the left and right wheels of the robot,
Figure PCTCN2021100035-appb-000010
Indicates the estimated linear velocity of the left wheel of the robot at time t,
Figure PCTCN2021100035-appb-000011
Represents the estimated value of the linear velocity of the right wheel of the robot at time t; wherein, in the normal mode, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the linear velocity of the right wheel of the robot at time t is estimated is the linear velocity measured by the encoder of the right wheel; in the failure mode of the left wheel encoder, the estimated value of the linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel, and the estimated value of the linear velocity of the right wheel of the robot at time t is the right The encoder of the wheel measures the linear velocity; in the failure mode of the right wheel encoder, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the estimated linear velocity of the right wheel of the robot at time t is the right wheel In the simultaneous failure mode of the left and right wheel encoders, the estimated linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel, and the estimated linear velocity of the right wheel of the robot at time t is the driving speed of the right wheel.
本发明还提供了一种移动机器人故障下同时FDD和SLAM的系统，包括：The present invention also provides a system for simultaneous FDD and SLAM under the failure of a mobile robot, including:
姿态样本获取模块，用于获取t时刻机器人的姿态样本；其中，t时刻机器人的姿态样本在各状态模式上的分布概率由t时刻机器人发生各状态模式的经验概率确定，机器人t时刻发生各状态模式的经验概率根据机器人t-1时刻的状态模式估计以及状态转移概率确定；t时刻任一状态模式下的姿态样本分布遵循对应状态模式下机器人姿态的正态分布，所述正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述左右车轮线速度估计值由t时刻左右车轮的编码器测量线速度确定或由t时刻左右车轮的编码器测量线速度以及驱动速度确定，所述正态分布中的标准差由对应状态模式的经验标准差确定；所述状态模式包括正常模式、左轮编码器故障模式、右轮编码器故障模式以及左右轮编码器同时故障模式；The attitude sample acquisition module is used to acquire the attitude samples of the robot at time t; wherein, the distribution probability of the attitude samples of the robot at each state mode at time t is determined by the empirical probability of each state mode of the robot at time t, and each state mode occurs at time t of the robot. The empirical probability of the mode is determined according to the state mode estimation and state transition probability of the robot at time t-1; the attitude sample distribution in any state mode at time t follows the normal distribution of robot attitude in the corresponding state mode, and the The mean value is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode. The estimated value of the linear velocity of the left and right wheels is determined by the linear velocity measured by the encoder of the left and right wheels at time t or by the linear velocity measured by the encoder of the left and right wheels at time t and the driving speed It is determined that the standard deviation in the normal distribution is determined by the empirical standard deviation of the corresponding state mode; the state mode includes a normal mode, a failure mode of the left wheel encoder, a failure mode of the right wheel encoder, and a simultaneous failure mode of the left and right wheel encoders;
样本权重确定模块，用于根据t时刻机器人的姿态样本、t时刻机器人的环境点云数据以及t-1时刻机器人的环境地图估计，确定各姿态样本的权重；其中，所述环境点云数据为实际测量数据；The sample weight determination module is used to determine the weight of each attitude sample according to the attitude sample of the robot at time t, the environment point cloud data of the robot at time t, and the environment map estimation of the robot at time t-1; wherein, the environment point cloud data is actual measurement data;
状态模式估计确定模块，用于根据t时刻各姿态样本所属的状态模式以及t时刻各姿态样本的权重，计算t时刻发生各状态模式的估计概率，并选取估计概率最大的状态模式作为t时刻的状态模式估计；The state mode estimation determination module is used to calculate the estimated probability of each state mode at time t according to the state mode to which each attitude sample belongs at time t and the weight of each attitude sample at time t, and select the state mode with the largest estimated probability as the state mode at time t state mode estimation;
位姿估计确定模块，用于将目标姿态样本中后验概率最大的姿态样本作为t时刻机器人的位姿估计，所述目标姿态样本包括所述t时刻的状态模式估计对应的姿态样本；The pose estimation determination module is used to use the pose sample with the largest posterior probability in the target pose sample as the pose estimate of the robot at time t, and the target pose sample includes a pose sample corresponding to the state mode estimation at time t;
环境地图估计确定模块，用于根据t时刻机器人的环境点云数据以及t时刻机器人的位姿估计，确定t时刻的环境地图估计。The environment map estimation determination module is used to determine the environment map estimation at time t according to the environment point cloud data of the robot at time t and the pose estimation of the robot at time t.
可选的，所述样本权重确定模块，具体包括：Optionally, the sample weight determination module specifically includes:
配准点对确定单元，用于以所述姿态样本为初始位姿估计，利用迭代最近点方法确定t时刻环境点云数据与t-1时刻的环境地图估计在配准过程中的配准点对；The registration point pair determination unit is used to use the attitude sample as the initial pose estimation, and use the iterative closest point method to determine the registration point pair between the environmental point cloud data at time t and the environmental map estimation at time t-1 during the registration process;
距离计算单元，用于计算各所述配准点对中两点的距离；a distance calculation unit, configured to calculate the distance between two points in each of the registration point pairs;
权重确定单元，用于确定所述配准点对中所述距离小于设定阈值的配准点对的比例，并将所述比例作为t时刻所述姿态样本的权重。A weight determining unit, configured to determine a ratio of the registration point pairs whose distance is smaller than a set threshold among the registration point pairs, and use the ratio as the weight of the posture sample at time t.
可选的，所述状态模式估计确定模块，具体包括：Optionally, the state mode estimation determination module specifically includes:
状态模式估计概率计算单元，用于根据
Figure PCTCN2021100035-appb-000012
计算t时刻发生各状态模式的估计概率
Figure PCTCN2021100035-appb-000013
其中，S k表示第k状态模式，s t表示t时刻的状态模式，当t时刻的第i姿态样本
Figure PCTCN2021100035-appb-000014
属于状态模式S k时，
Figure PCTCN2021100035-appb-000015
为1，否则
Figure PCTCN2021100035-appb-000016
为0，
Figure PCTCN2021100035-appb-000017
表示t时刻的第i姿态样本归一化后的权重，N表示t时刻姿态样本的数量。 The state mode estimation probability calculation unit is used for
Figure PCTCN2021100035-appb-000012
Calculate the estimated probability of occurrence of each state mode at time t
Figure PCTCN2021100035-appb-000013
Among them, S k represents the k-th state mode, s t represents the state mode at time t, when the i-th attitude sample at time t
Figure PCTCN2021100035-appb-000014
When belonging to the state mode S k ,
Figure PCTCN2021100035-appb-000015
is 1, otherwise
Figure PCTCN2021100035-appb-000016
is 0,
Figure PCTCN2021100035-appb-000017
Indicates the normalized weight of the i-th attitude sample at time t, and N indicates the number of attitude samples at time t.
可选的，所述系统还包括：Optionally, the system also includes:
线速度样本采样模块，用于遵循t时刻各状态模式发生的经验概率以及机器人各状态模式下的线速度正态分布，采样线速度样本；The line speed sample sampling module is used to follow the empirical probability of the occurrence of each state mode at time t and the line speed normal distribution under each state mode of the robot, and sample the line speed sample;
偏航率样本采样模块，用于遵循t时刻各状态模式发生的经验概率以及机器人各状态模式下的偏航率正态分布，采样偏航率样本；The yaw rate sample sampling module is used to sample the yaw rate samples according to the empirical probability of occurrence of each state mode at time t and the normal distribution of the yaw rate under each state mode of the robot;
姿态样本确定模块，用于根据所述线速度样本和所述偏航率样本确定t时刻机器人的姿态样本；Attitude sample determination module, used to determine the attitude sample of the robot at time t according to the linear velocity sample and the yaw rate sample;
其中，所述机器人姿态的正态分布包括线速度正态分布和偏航率正态分布，所述线速度正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述线速度正态分布中的标准差由对应状态模式的经验标准差确定，所述偏航率正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述偏航率正态分布中的标准差由对应状态模式的经验标准差确定。Wherein, the normal distribution of the robot attitude includes the normal distribution of the linear velocity and the normal distribution of the yaw rate, and the mean value in the normal distribution of the linear velocity is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode, and the The standard deviation in the normal distribution of the linear velocity is determined by the empirical standard deviation of the corresponding state mode, the mean value in the normal distribution of the yaw rate is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode, and the yaw rate is positive The standard deviation in the state distribution is determined from the empirical standard deviation of the corresponding state pattern.
可选的，所述系统还包括：Optionally, the system also includes:
线速度正态分布确定模块，用于：Linear velocity normal distribution determination module for:
根据
Figure PCTCN2021100035-appb-000018
计算t时刻线速度正态分布中的均值
Figure PCTCN2021100035-appb-000019
其中，
Figure PCTCN2021100035-appb-000020
表示t时刻机器人的左车轮线速度估计值，
Figure PCTCN2021100035-appb-000021
表示t时刻机器人的右车轮线 速度估计值；其中，在所述正常模式下，t时刻机器人的左车轮线速度估计值为左车轮的编码器测量线速度，t时刻机器人的右车轮线速度估计值为右车轮的编码器测量线速度；在所述左轮编码器故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的驱动速度，t时刻机器人的右车轮线速度估计值为右车轮的编码器测量线速度；在右轮编码器故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的编码器测量线速度，t时刻机器人的右车轮线速度估计值为右车轮的驱动速度；在所述左右轮编码器同时故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的驱动速度，t时刻机器人的右车轮线速度估计值为右车轮的驱动速度； according to
Figure PCTCN2021100035-appb-000018
Compute the mean in the normal distribution of the line speed at time t
Figure PCTCN2021100035-appb-000019
in,
Figure PCTCN2021100035-appb-000020
Indicates the estimated linear velocity of the left wheel of the robot at time t,
Figure PCTCN2021100035-appb-000021
Represents the estimated value of the linear velocity of the right wheel of the robot at time t; wherein, in the normal mode, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the linear velocity of the right wheel of the robot at time t is estimated is the linear velocity measured by the encoder of the right wheel; in the failure mode of the left wheel encoder, the estimated value of the linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel, and the estimated value of the linear velocity of the right wheel of the robot at time t is the right The encoder of the wheel measures the linear velocity; in the failure mode of the right wheel encoder, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the estimated linear velocity of the right wheel of the robot at time t is the right wheel drive speed; in the simultaneous failure mode of the left and right wheel encoders, the estimated linear velocity of the left wheel of the robot at time t is the drive speed of the left wheel, and the estimated linear velocity of the right wheel of the robot at time t is the drive speed of the right wheel;
偏航率正态分布确定模块，用于：The yaw rate normal distribution determination module is used for:
根据
Figure PCTCN2021100035-appb-000022
计算t时刻偏航率正态分布中的均值
Figure PCTCN2021100035-appb-000023
其中，W表示连接机器人左右车轮的轴长。 according to
Figure PCTCN2021100035-appb-000022
Compute the mean in the normal distribution of the yaw rate at time t
Figure PCTCN2021100035-appb-000023
Among them, W represents the axis length connecting the left and right wheels of the robot.
根据本发明提供的具体实施例，公开了以下技术效果：本发明实施例首先对本时刻的状态模式进行了估计，然后在该状态模式下选取精确的位姿估计，最后，根据该位姿估计、上一时刻的环境地图估计以及本时刻的环境点云数据，利用地图匹配方法进行本时刻的地图建立。实现了移动机器人状态模式下同时进行故障诊断和并发定位与建图。According to the specific embodiment provided by the present invention, the following technical effects are disclosed: the embodiment of the present invention first estimates the state mode at this moment, then selects an accurate pose estimation in the state mode, and finally, according to the pose estimation, The estimation of the environmental map at the previous moment and the environmental point cloud data at the current moment are carried out by using the map matching method to establish the map at this moment. Simultaneous fault diagnosis and concurrent positioning and mapping are realized in the state mode of the mobile robot.
说明书附图Instructions attached
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the accompanying drawings required in the embodiments. Obviously, the accompanying drawings in the following description are only some of the present invention. Embodiments, for those of ordinary skill in the art, other drawings can also be obtained based on these drawings without any creative effort.
图1为本发明实施例1提供的移动机器人故障下同时FDD和SLAM的方法流程图；FIG. 1 is a flow chart of a method for simultaneous FDD and SLAM under a mobile robot failure provided in Embodiment 1 of the present invention;
图2为本发明实施例2提供的移动机器人故障下同时FDD和SLAM 的系统结构示意图。Fig. 2 is a schematic diagram of the system structure of simultaneous FDD and SLAM under the failure of the mobile robot provided by Embodiment 2 of the present invention.
具体实施方式detailed description
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some, not all, embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without making creative efforts belong to the protection scope of the present invention.
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。In order to make the above objects, features and advantages of the present invention more comprehensible, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.
实施例1Example 1
参见图1，本实施例提供了一种移动机器人故障下同时FDD和SLAM的方法，该方法针对的移动机器人为双轮差速机器人，左右车轮安装编码器，移动机器人本体上安装二维激光雷达，该方法包括以下步骤：Referring to Figure 1, this embodiment provides a method for simultaneous FDD and SLAM under a mobile robot failure. The mobile robot targeted by this method is a two-wheel differential robot, with encoders installed on the left and right wheels, and a two-dimensional laser radar installed on the mobile robot body. , the method includes the following steps:
步骤101：获取t时刻机器人的姿态样本。Step 101: Obtain a pose sample of the robot at time t.
其中，t时刻机器人的姿态样本在各状态模式上的分布概率由t时刻机器人发生各状态模式的经验概率确定，机器人t时刻发生各状态模式的经验概率根据机器人t-1时刻的状态模式估计以及状态转移概率确定。状态模式如表1所示，状态转移概率可如表2所示。Among them, the distribution probability of the attitude samples of the robot on each state mode at time t is determined by the empirical probability of each state mode of the robot at time t, and the empirical probability of each state mode of the robot at time t is estimated according to the state mode of the robot at time t-1 and State transition probabilities are determined. The state mode is shown in Table 1, and the state transition probability can be shown in Table 2.
表1 状态模式Table 1 State Mode
状态模式state mode 故障元件Faulty component
S 1 S 1 正常normal
S 2 S 2 左轮编码器故障Left wheel encoder failure
S 3 S 3 右轮编码器故障Right wheel encoder failure
S 4 S 4 左轮、右轮编码器同时出现故障The left and right wheel encoders fail at the same time
表2 p(s t＝S k|s t-1＝S j)k,j∈{1,2,3,4} Table 2 p(s t =S k |s t-1 =S j )k,j∈{1,2,3,4}
Figure PCTCN2021100035-appb-000024
Figure PCTCN2021100035-appb-000024
根据t-1时刻的状态模式估计以及状态转移概率得到t时刻各状态模式发生的经验概率，姿态样本的选取依据该经验概率进行。比如，t-1时刻的状态模式估计为S 1时，根据表2中的状态转移概率可知，t时刻各状态模式S 1、S 2、S 3、S 4的经验概率分别为0.5,0.2,0.2,0.1。那么t时刻采样的姿态样本中属于状态模式S 1的姿态样本占0.5，属于状态模式S 2的姿态样本占0.2，属于状态模式S 3的姿态样本占0.2，属于状态模式S 4的姿态样本占0.1。 According to the state mode estimation and state transition probability at time t-1, the empirical probability of occurrence of each state mode at time t is obtained, and the attitude samples are selected according to the empirical probability. For example, when the state mode at time t-1 is estimated to be S 1 , according to the state transition probabilities in Table 2, the empirical probabilities of each state mode S 1 , S 2 , S 3 , and S 4 at time t are 0.5, 0.2, 0.2,0.1. Then among the attitude samples sampled at time t , the attitude samples belonging to state mode S1 account for 0.5 , the attitude samples belonging to state mode S2 account for 0.2, the attitude samples belonging to state mode S3 account for 0.2, and the attitude samples belonging to state mode S4 account for 0.1.
t时刻任一状态模式下的姿态样本分布遵循对应状态模式下机器人姿态的正态分布，所述正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述左右车轮线速度估计值由t时刻左右车轮的编码器测量线速度确定或由t时刻左右车轮的编码器测量线速度以及驱动速度确定，所述正态分布中的标准差由对应状态模式的经验标准差确定。The attitude sample distribution in any state mode at time t follows the normal distribution of the robot attitude in the corresponding state mode, and the mean value in the normal distribution is determined by the estimated values of the left and right wheel linear velocities in the corresponding state mode, and the left and right wheel lines The speed estimate is determined by the linear velocity measured by the encoders of the wheels around the time t or by the linear velocity measured by the encoders of the wheels around the time t and the driving speed, and the standard deviation in the normal distribution is determined by the empirical standard deviation of the corresponding state mode .
比如，状态模式S 1的经验标准差为σ 1，状态模式S 2的经验标准差为σ 2，状态模式S 3的经验标准差为σ 3，状态模式S 4的经验标准差为σ 4。那么，如果t时刻有100个姿态样本：属于状态模式S 1的姿态样本有50个，属于状态模式S 2的姿态样本占20个，属于状态模式S 3的姿态样本占20个，属于状态模式S 4的姿态样本占10个。其中，属于状态模式S 1的50个姿态样本遵循正态分布(μ 1，σ 1)，属于状态模式S 2的20个姿态样本遵循正态分布(μ 2，σ 2)，属于状态模式S 3的20个姿态样本遵循正态分布 (μ 3，σ 3)，属于状态模式S 4的10个姿态样本遵循正态分布(μ 4，σ 4)。 For example, the empirical standard deviation of state mode S 1 is σ 1 , the empirical standard deviation of state mode S 2 is σ 2 , the empirical standard deviation of state mode S 3 is σ 3 , and the empirical standard deviation of state mode S 4 is σ 4 . Then, if there are 100 attitude samples at time t: there are 50 attitude samples belonging to state mode S1, 20 attitude samples belonging to state mode S2, and 20 attitude samples belonging to state mode S3, which belong to state mode The pose samples of S 4 account for 10. Among them, the 50 attitude samples belonging to the state mode S 1 follow the normal distribution (μ 1 , σ 1 ), the 20 attitude samples belonging to the state mode S 2 follow the normal distribution (μ 2 , σ 2 ), and belong to the state mode S The 20 attitude samples of 3 follow normal distribution (μ 3 , σ 3 ), and the 10 attitude samples belonging to state pattern S 4 follow normal distribution (μ 4 , σ 4 ).
上述μ 1、μ 2、μ 3、μ 4对应状态模式下的左右车轮线速度估计值确定，比如，μ 1由状态模式S 1下的左右车轮线速度估计值
Figure PCTCN2021100035-appb-000025
和
Figure PCTCN2021100035-appb-000026
确定，μ 2由状态模式S 2下的左右车轮线速度估计值
Figure PCTCN2021100035-appb-000027
和
Figure PCTCN2021100035-appb-000028
确定。不同状态模式下左右车轮线速度估计值的确定方法如下： The above μ 1 , μ 2 , μ 3 , μ 4 are determined according to the estimated values of the left and right wheel linear velocities in the state mode, for example, μ 1 is determined by the estimated values of the left and right wheel linear velocities in the state mode S 1
Figure PCTCN2021100035-appb-000025
with
Figure PCTCN2021100035-appb-000026
Determined, μ 2 is determined by the estimated value of the linear velocity of the left and right wheels in the state mode S 2
Figure PCTCN2021100035-appb-000027
with
Figure PCTCN2021100035-appb-000028
Sure. The method of determining the estimated linear velocity of the left and right wheels in different state modes is as follows:
在S 1下，t时刻机器人的左车轮线速度估计值为左车轮的编码器测量线速度，t时刻机器人的右车轮线速度估计值为右车轮的编码器测量线速度；在S 2下，t时刻机器人的左车轮线速度估计值为左车轮的驱动速度，t时刻机器人的右车轮线速度估计值为右车轮的编码器测量线速度；在S 3下，t时刻机器人的左车轮线速度估计值为左车轮的编码器测量线速度，t时刻机器人的右车轮线速度估计值为右车轮的驱动速度；在S 4下，t时刻机器人的左车轮线速度估计值为左车轮的驱动速度，t时刻机器人的右车轮线速度估计值为右车轮的驱动速度。 Under S1, the estimated linear velocity of the robot’s left wheel at time t is the linear velocity measured by the encoder of the left wheel, and the estimated linear velocity of the robot’s right wheel at time t is the linear velocity measured by the encoder of the right wheel ; under S2, The estimated linear velocity of the robot’s left wheel at time t is the driving speed of the left wheel, and the estimated linear velocity of the robot’s right wheel at time t is the linear velocity measured by the encoder of the right wheel; under S3, the linear velocity of the robot’s left wheel at time t is The estimated value is the linear velocity measured by the encoder of the left wheel, and the estimated linear velocity of the robot’s right wheel at time t is the driving speed of the right wheel ; under S4, the estimated linear velocity of the robot’s left wheel at time t is the driving speed of the left wheel , the estimated linear velocity of the right wheel of the robot at time t is the driving speed of the right wheel.
步骤102：根据t时刻机器人的姿态样本、t时刻机器人的环境点云数据以及t-1时刻的环境地图估计，确定各姿态样本的权重。本实施例中的环境点云数据采用激光雷达对机器人的周围环境实际扫描测量得到。Step 102: Determine the weight of each pose sample according to the pose sample of the robot at time t, the environment point cloud data of the robot at time t, and the environment map estimation at time t-1. The environmental point cloud data in this embodiment is obtained by actually scanning and measuring the surrounding environment of the robot by laser radar.
在一个示例中，步骤102具体包括：In an example, step 102 specifically includes:
对t时刻任一所述姿态样本：For any attitude sample at time t:
以所述姿态样本为初始位姿估计，利用迭代最近点方法确定t时刻的环境点云数据与t-1时刻的环境地图估计在配准过程中的配准点对；Using the attitude sample as the initial pose estimation, using the iterative closest point method to determine the registration point pairs between the environment point cloud data at time t and the environment map estimation at time t-1 in the registration process;
计算各所述配准点对中两点的距离；Calculating the distance between two points in each of the registration points;
确定所述配准点对中所述距离小于设定阈值的配准点对的比例，并将所述比例作为t时刻所述姿态样本的权重。Determining the ratio of the registration point pairs whose distance is smaller than the set threshold among the registration point pairs, and using the ratio as the weight of the attitude sample at time t.
步骤103：根据t时刻各姿态样本所属的状态模式以及t时刻各姿态样本的权重，计算t时刻发生各状态模式的估计概率，并选取估计概率最大的状态模式作为t时刻的状态模式估计。Step 103: Calculate the estimated probability of occurrence of each state mode at time t according to the state mode to which each attitude sample belongs and the weight of each attitude sample at time t, and select the state mode with the highest estimated probability as the state mode estimation at time t.
在一个示例中，步骤103包括：In one example, step 103 includes:
根据
Figure PCTCN2021100035-appb-000029
计算t时刻发生各状态模式的估计概率
Figure PCTCN2021100035-appb-000030
其中，S k表示第k状态模式，s t表示t时刻的状态模式，当t时刻的第i姿态样本
Figure PCTCN2021100035-appb-000031
属于状态模式S k时，
Figure PCTCN2021100035-appb-000032
为1，否则
Figure PCTCN2021100035-appb-000033
为0，
Figure PCTCN2021100035-appb-000034
表示t时刻的第i姿态样本归一化后的权重，N表示t时刻姿态样本的数量。 according to
Figure PCTCN2021100035-appb-000029
Calculate the estimated probability of occurrence of each state mode at time t
Figure PCTCN2021100035-appb-000030
Among them, S k represents the k-th state mode, s t represents the state mode at time t, when the i-th attitude sample at time t
Figure PCTCN2021100035-appb-000031
When belonging to the state mode S k ,
Figure PCTCN2021100035-appb-000032
is 1, otherwise
Figure PCTCN2021100035-appb-000033
is 0,
Figure PCTCN2021100035-appb-000034
Indicates the normalized weight of the i-th attitude sample at time t, and N indicates the number of attitude samples at time t.
步骤104：将目标姿态样本中后验概率最大的姿态样本作为t时刻机器人的位姿估计，所述目标姿态样本包括所述t时刻的状态模式估计对应的姿态样本。Step 104: Use the pose sample with the highest posterior probability among the target pose samples as the pose estimation of the robot at time t, the target pose sample including the pose sample corresponding to the state mode estimation at time t.
步骤105：根据t时刻机器人的环境点云数据以及t时刻机器人的位姿估计，确定t时刻的环境地图估计。Step 105: According to the environment point cloud data of the robot at time t and the pose estimation of the robot at time t, determine the environment map estimation at time t.
机器人姿态的正态分布包括线速度正态分布和偏航率正态分布，在一个示例中，在步骤101之前，还包括：The normal distribution of robot attitude includes linear velocity normal distribution and yaw rate normal distribution. In one example, before step 101, it also includes:
(1)获取t时刻机器人的左车轮线速度和右车轮线速度。(1) Obtain the linear velocity of the left wheel and the right wheel of the robot at time t.
(2)根据
Figure PCTCN2021100035-appb-000035
计算t时刻线速度正态分布中的均值
Figure PCTCN2021100035-appb-000036
和t时刻偏航率正态分布中的均值
Figure PCTCN2021100035-appb-000037
其中，
Figure PCTCN2021100035-appb-000038
表示t时刻机器人的左车轮线速度估计值，
Figure PCTCN2021100035-appb-000039
表示t时刻机器人的右车轮线速度估计值，W表示连接机器人左右车轮的轴长。 (2) According to
Figure PCTCN2021100035-appb-000035
Compute the mean in the normal distribution of the line speed at time t
Figure PCTCN2021100035-appb-000036
and the mean value in the normal distribution of the yaw rate at time t
Figure PCTCN2021100035-appb-000037
in,
Figure PCTCN2021100035-appb-000038
Indicates the estimated linear velocity of the left wheel of the robot at time t,
Figure PCTCN2021100035-appb-000039
Indicates the estimated value of the linear velocity of the right wheel of the robot at time t, and W indicates the length of the axis connecting the left and right wheels of the robot.
其中，在所述正常模式下，t时刻机器人的左车轮线速度估计值为左车轮的编码器测量线速度，t时刻机器人的右车轮线速度估计值为右车轮的编码器测量线速度；在所述左轮编码器故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的驱动速度，t时刻机器人的右车轮线速度估计值为右车轮的编码器测量线速度；在右轮编码器故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的编码器测量线速度，t时刻机器人的右车轮线速度估计值为右车轮的驱动速度；在所述左右轮编码器同时故 障模式下，t时刻机器人的左车轮线速度估计值为左车轮的驱动速度，t时刻机器人的右车轮线速度估计值为右车轮的驱动速度。Wherein, in the normal mode, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the estimated linear velocity of the right wheel of the robot at time t is the linear velocity measured by the encoder of the right wheel; Under the failure mode of the left wheel encoder, the estimated linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel, and the estimated linear velocity of the right wheel of the robot at time t is the encoder measuring linear velocity of the right wheel; In the device failure mode, the estimated line speed of the left wheel of the robot at time t is the encoder measurement line speed of the left wheel, and the estimated line speed of the right wheel of the robot at time t is the driving speed of the right wheel; In the fault mode, the estimated linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel, and the estimated linear velocity of the right wheel of the robot at time t is the driving speed of the right wheel.
(3)遵循t时刻各状态模式发生的经验概率以及机器人各状态模式下的线速度正态分布，采样线速度样本。(3) Follow the empirical probability of occurrence of each state mode at time t and the normal distribution of the line speed in each state mode of the robot, and sample line speed samples.
(4)遵循t时刻各状态模式发生的经验概率以及机器人各状态模式(4) Follow the empirical probability of occurrence of each state mode at time t and each state mode of the robot
下的偏航率正态分布，采样偏航率样本。Under the normal distribution of yaw rate, sample yaw rate samples.
其中，线速度正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述线速度正态分布中的标准差由对应状态模式的经验标准差确定，所述偏航率正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述偏航率正态分布中的标准差由对应状态模式的经验标准差确定。Wherein, the mean value in the normal distribution of the linear velocity is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode, the standard deviation in the normal distribution of the linear velocity is determined by the empirical standard deviation of the corresponding state mode, and the yaw rate The mean value in the normal distribution is determined by the estimated values of the left and right wheel linear speeds in the corresponding state mode, and the standard deviation in the normal distribution of the yaw rate is determined by the empirical standard deviation in the corresponding state mode.
比如，需要采样100个线速度样本，t时刻状态模式S 1发生的经验概率为0.5，那么，在采样时，需要采集50个属于状态模式S 1的线速度样本，而且，这50个线速度样本要符合正态分布(μ 1，σ 1)，其中，μ 1采用上述方式求取，σ 1为状态模式S 1对应的线速度经验标准差，该线速度经验标准差为根据经验设定。 For example, it is necessary to sample 100 linear velocity samples, and the empirical probability of state mode S 1 occurring at time t is 0.5, then, during sampling, it is necessary to collect 50 linear velocity samples belonging to state mode S 1 , and these 50 linear velocity samples The sample must conform to the normal distribution (μ 1 , σ 1 ), where μ 1 is obtained by the above method, σ 1 is the empirical standard deviation of the linear velocity corresponding to the state mode S 1 , and the empirical standard deviation of the linear velocity is set according to experience .
(5)根据所述线速度样本和所述偏航率样本确定t时刻机器人的姿态样本。(5) Determine the attitude sample of the robot at time t according to the linear velocity sample and the yaw rate sample.
下面对上述内容所涉及到的特征进行具体详细的介绍The features involved in the above content are described in detail below.
设x t＝[x t,y t,θ t]表示t时刻移动机器人的位姿(2维位置与航向角) Let x t =[x t ,y t ,θ t ] represent the pose of the mobile robot at time t (2-dimensional position and heading angle)
v t和
Figure PCTCN2021100035-appb-000040
分别表示t时刻机器人线速度(单位：m/s)和偏航率(单位：弧度/s)，τ表示采样间隔(单位：s). v t and
Figure PCTCN2021100035-appb-000040
Respectively represent the robot linear velocity (unit: m/s) and yaw rate (unit: radian/s) at time t, and τ represents the sampling interval (unit: s).
Figure PCTCN2021100035-appb-000041
为t时刻左侧线速度，
Figure PCTCN2021100035-appb-000042
为t时刻右侧线速度，单位：m/s
Figure PCTCN2021100035-appb-000041
is the left linear velocity at time t,
Figure PCTCN2021100035-appb-000042
is the linear velocity on the right side at time t, unit: m/s
Figure PCTCN2021100035-appb-000043
为t时刻左轮驱动速度，
Figure PCTCN2021100035-appb-000044
为t时刻右轮驱动速度，单位：m/s
Figure PCTCN2021100035-appb-000043
is the driving speed of the left wheel at time t,
Figure PCTCN2021100035-appb-000044
is the driving speed of the right wheel at time t, unit: m/s
W表示连接移动机器人左右轮的轴长，单位：mW represents the axis length connecting the left and right wheels of the mobile robot, unit: m
r表示移动机器人左右车轮的半径，单位：mr represents the radius of the left and right wheels of the mobile robot, unit: m
令
Figure PCTCN2021100035-appb-000045
M t分别表示t时刻左轮编码器、右轮轮编码器仪、二维激光雷达的读数 make
Figure PCTCN2021100035-appb-000045
M t respectively represent the readings of the left wheel encoder, right wheel encoder and two-dimensional laser radar at time t
Θ t为t时刻的环境地图，采用二维激光点云表示 Θ t is the environment map at time t, represented by a two-dimensional laser point cloud
Figure PCTCN2021100035-appb-000046
为t时刻的第i个样本表示的环境地图
Figure PCTCN2021100035-appb-000046
is the environment map represented by the i-th sample at time t
s t为t时刻的状态模式，取值范围如表1 s t is the state mode at time t, and the value range is shown in Table 1
Figure PCTCN2021100035-appb-000047
为t时刻的线速度标准差
Figure PCTCN2021100035-appb-000047
is the standard deviation of the linear velocity at time t
Figure PCTCN2021100035-appb-000048
为t时刻的偏航率标准差
Figure PCTCN2021100035-appb-000048
is the standard deviation of the yaw rate at time t
Figure PCTCN2021100035-appb-000049
为状态模式S 1情况下的线速度标准差
Figure PCTCN2021100035-appb-000049
is the standard deviation of linear velocity in state mode S 1
Figure PCTCN2021100035-appb-000050
为状态模式S 1情况下的偏航率标准差
Figure PCTCN2021100035-appb-000050
is the standard deviation of yaw rate in state mode S 1
Figure PCTCN2021100035-appb-000051
为状态模式S 2情况下的线速度标准差
Figure PCTCN2021100035-appb-000051
is the standard deviation of linear velocity in state mode S 2
Figure PCTCN2021100035-appb-000052
为状态模式S 2情况下的偏航率标准差
Figure PCTCN2021100035-appb-000052
is the standard deviation of yaw rate in state mode S 2
Figure PCTCN2021100035-appb-000053
为状态模式S 3情况下的线速度标准差
Figure PCTCN2021100035-appb-000053
is the standard deviation of linear velocity in state mode S 3
Figure PCTCN2021100035-appb-000054
为状态模式S 3情况下的偏航率标准差
Figure PCTCN2021100035-appb-000054
is the standard deviation of yaw rate in state mode S 3
Figure PCTCN2021100035-appb-000055
为状态模式S 4情况下的线速度标准差
Figure PCTCN2021100035-appb-000055
is the standard deviation of linear velocity in state mode S 4
Figure PCTCN2021100035-appb-000056
为状态模式S 4情况下的偏航率标准差
Figure PCTCN2021100035-appb-000056
is the standard deviation of yaw rate in state mode S 4
本发明的输入为从1时刻到T时刻的激光雷达扫描M 1..t＝M 1，...,M t，左轮编码器测量
Figure PCTCN2021100035-appb-000057
右轮编码器测量
Figure PCTCN2021100035-appb-000058
左轮驱动速度
Figure PCTCN2021100035-appb-000059
右轮驱动速度
Figure PCTCN2021100035-appb-000060
输出为t时刻状态模式
Figure PCTCN2021100035-appb-000061
t时刻位姿
Figure PCTCN2021100035-appb-000062
t时刻地图
Figure PCTCN2021100035-appb-000063
The input of the present invention is the lidar scanning M 1..t = M 1 ,..., M t from time 1 to time T, and the measurement of the left wheel encoder
Figure PCTCN2021100035-appb-000057
Right wheel encoder measurement
Figure PCTCN2021100035-appb-000058
left wheel drive speed
Figure PCTCN2021100035-appb-000059
right wheel drive speed
Figure PCTCN2021100035-appb-000060
The output is the state mode at time t
Figure PCTCN2021100035-appb-000061
pose at time t
Figure PCTCN2021100035-appb-000062
map at time t
Figure PCTCN2021100035-appb-000063
(1)建立故障空间(1) Establish a fault space
本案考查移动机器人左右轮编码器故障的诊断，每个传感器存在正常和故障两种模式，考虑编码器故障，包含2个编码器的移动机器人移动具 有4种状态模式，包含一个正常模式，和三个故障模式。This case examines the fault diagnosis of the left and right wheel encoders of a mobile robot. Each sensor has two modes: normal and fault. Considering encoder faults, a mobile robot with two encoders has four state modes, including one normal mode and three a failure mode.
(2)初始化(2) Initialization
粒子数目设置为N，初始化粒子集
Figure PCTCN2021100035-appb-000064
Figure PCTCN2021100035-appb-000065
Figure PCTCN2021100035-appb-000066
The number of particles is set to N, and the particle set is initialized
Figure PCTCN2021100035-appb-000064
Figure PCTCN2021100035-appb-000065
Figure PCTCN2021100035-appb-000066
(3)根据表2所示的离散状态转移概率抽取离散样本
Figure PCTCN2021100035-appb-000067
Figure PCTCN2021100035-appb-000068
即
Figure PCTCN2021100035-appb-000069
为S k的概率为
Figure PCTCN2021100035-appb-000070
(3) Draw discrete samples according to the discrete state transition probabilities shown in Table 2
Figure PCTCN2021100035-appb-000067
Figure PCTCN2021100035-appb-000068
which is
Figure PCTCN2021100035-appb-000069
The probability of being S k is
Figure PCTCN2021100035-appb-000070
(4)按照以下方式得到连续状态
Figure PCTCN2021100035-appb-000071
(4) Get the continuous state in the following way
Figure PCTCN2021100035-appb-000071
如果
Figure PCTCN2021100035-appb-000072
则
Figure PCTCN2021100035-appb-000073
if
Figure PCTCN2021100035-appb-000072
but
Figure PCTCN2021100035-appb-000073
如果
Figure PCTCN2021100035-appb-000074
则
Figure PCTCN2021100035-appb-000075
if
Figure PCTCN2021100035-appb-000074
but
Figure PCTCN2021100035-appb-000075
如果
Figure PCTCN2021100035-appb-000076
则
Figure PCTCN2021100035-appb-000077
if
Figure PCTCN2021100035-appb-000076
but
Figure PCTCN2021100035-appb-000077
如果
Figure PCTCN2021100035-appb-000078
则
Figure PCTCN2021100035-appb-000079
if
Figure PCTCN2021100035-appb-000078
but
Figure PCTCN2021100035-appb-000079
Figure PCTCN2021100035-appb-000080
Figure PCTCN2021100035-appb-000080
从正态分布
Figure PCTCN2021100035-appb-000081
采样线速度样本
Figure PCTCN2021100035-appb-000082
from normal distribution
Figure PCTCN2021100035-appb-000081
Sample Line Velocity Sample
Figure PCTCN2021100035-appb-000082
从正态分布
Figure PCTCN2021100035-appb-000083
采样偏航率样本
Figure PCTCN2021100035-appb-000084
from normal distribution
Figure PCTCN2021100035-appb-000083
sample yaw rate samples
Figure PCTCN2021100035-appb-000084
Figure PCTCN2021100035-appb-000085
Figure PCTCN2021100035-appb-000085
Figure PCTCN2021100035-appb-000086
Figure PCTCN2021100035-appb-000086
(5)对于每一个连续状态
Figure PCTCN2021100035-appb-000087
和地图
Figure PCTCN2021100035-appb-000088
按以下方式计算 权重
Figure PCTCN2021100035-appb-000089
(5) For each continuous state
Figure PCTCN2021100035-appb-000087
and map
Figure PCTCN2021100035-appb-000088
Weights are calculated as follows
Figure PCTCN2021100035-appb-000089
以
Figure PCTCN2021100035-appb-000090
为初始位姿估计，利用迭代最近点方法(ICP)对M t和
Figure PCTCN2021100035-appb-000091
进行配准，设获得的位姿估计为
Figure PCTCN2021100035-appb-000092
设
Figure PCTCN2021100035-appb-000093
为M t经过位姿变换
Figure PCTCN2021100035-appb-000094
后的点云，
Figure PCTCN2021100035-appb-000095
为
Figure PCTCN2021100035-appb-000096
中每一点到
Figure PCTCN2021100035-appb-000097
的最近点的距离。Δ t中小于阈值Υ的点的数目为β，
Figure PCTCN2021100035-appb-000098
by
Figure PCTCN2021100035-appb-000090
For initial pose estimation, use iterative closest point method (ICP) to M t and
Figure PCTCN2021100035-appb-000091
For registration, let the obtained pose estimation be
Figure PCTCN2021100035-appb-000092
Assume
Figure PCTCN2021100035-appb-000093
After pose transformation for M t
Figure PCTCN2021100035-appb-000094
After the point cloud,
Figure PCTCN2021100035-appb-000095
for
Figure PCTCN2021100035-appb-000096
every point in
Figure PCTCN2021100035-appb-000097
The distance to the nearest point of . The number of points in Δt smaller than the threshold Y is β,
Figure PCTCN2021100035-appb-000098
位姿样本修正：
Figure PCTCN2021100035-appb-000099
Pose sample correction:
Figure PCTCN2021100035-appb-000099
(6)权重规格化
Figure PCTCN2021100035-appb-000100
(6) Weight normalization
Figure PCTCN2021100035-appb-000100
(7)故障诊断(7) Fault diagnosis
计算边缘概率分布
Figure PCTCN2021100035-appb-000101
Compute marginal probability distributions
Figure PCTCN2021100035-appb-000101
令
Figure PCTCN2021100035-appb-000102
make
Figure PCTCN2021100035-appb-000102
For k＝2 to 4 doFor k＝2 to 4 do
if
Figure PCTCN2021100035-appb-000103
if
Figure PCTCN2021100035-appb-000103
End forEnd for
t时刻状态模式为
Figure PCTCN2021100035-appb-000104
The state mode at time t is
Figure PCTCN2021100035-appb-000104
(8)地图样本更新：(8) Map sample update:
对于i＝1,...,N，将M t经过位姿变换
Figure PCTCN2021100035-appb-000105
得到
Figure PCTCN2021100035-appb-000106
For i=1,...,N, M t undergoes pose transformation
Figure PCTCN2021100035-appb-000105
get
Figure PCTCN2021100035-appb-000106
(9)在与状态模式
Figure PCTCN2021100035-appb-000107
相同的粒子中选择最大后验概率的粒子进行定位与建图：
Figure PCTCN2021100035-appb-000108
按最大后验估计定位与建图
Figure PCTCN2021100035-appb-000109
则t时刻位姿估计为
Figure PCTCN2021100035-appb-000110
环境地图估计为
Figure PCTCN2021100035-appb-000111
(9) In and status mode
Figure PCTCN2021100035-appb-000107
Select the particle with the largest posterior probability among the same particles for positioning and mapping:
Figure PCTCN2021100035-appb-000108
Localization and Mapping by Maximum Posteriori Estimation
Figure PCTCN2021100035-appb-000109
Then the pose at time t is estimated as
Figure PCTCN2021100035-appb-000110
The environmental map is estimated to be
Figure PCTCN2021100035-appb-000111
(10)t＝t+1(10)t=t+1
(11)如果t<＝T，转步骤(3)，否则结束。(11) If t<=T, go to step (3), otherwise end.
方法以粒子滤波器作为框架，同时实现移动机器人FDD和SLAM，FDD的结果用于提供不同故障情况下的运动学模型，在与故障相适应的运动学模型上进行更为精确的位姿估计，利用地图匹配方法实现对故障诊断和定位的感知模型。The method uses the particle filter as the framework to realize FDD and SLAM of the mobile robot at the same time. The results of FDD are used to provide kinematic models under different fault conditions, and more accurate pose estimation is performed on the kinematic model adapted to the fault. A perceptual model for fault diagnosis and location is realized by map matching method.
实施例2Example 2
参见图2，本实施例提供了一种移动机器人故障下同时FDD和SLAM的系统，该系统包括：Referring to FIG. 2, this embodiment provides a system for simultaneous FDD and SLAM under a mobile robot failure, the system includes:
姿态样本获取模块201，用于获取t时刻机器人的姿态样本；其中，t时刻机器人的姿态样本在各状态模式上的分布概率由t时刻机器人发生各状态模式的经验概率确定，机器人t时刻发生各状态模式的经验概率根据机器人t-1时刻的状态模式估计以及状态转移概率确定；t时刻任一状态模式下的姿态样本分布遵循对应状态模式下机器人姿态的正态分布，所述正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述左右车轮线速度估计值由t时刻左右车轮的编码器测量线速度确定或由t时刻左右车轮的编码器测量线速度以及驱动速度确定，所述正态分布中的标准差由对应状态模式的经验标准差确定；所述状态模式包括正常模式、左轮编码器故障模式、右轮编码器故障模式以及左右轮编码器同时故障模式；The attitude sample acquisition module 201 is used to acquire the attitude samples of the robot at time t; wherein, the distribution probability of the attitude samples of the robot at each state mode at time t is determined by the empirical probability of each state mode of the robot at time t, and each state mode occurs at time t of the robot. The empirical probability of the state mode is determined according to the state mode estimation and state transition probability of the robot at time t-1; the attitude sample distribution in any state mode at time t follows the normal distribution of the robot attitude in the corresponding state mode, and in the normal distribution The mean value of is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode. The estimated value of the linear velocity of the left and right wheels is determined by the linear velocity measured by the encoder of the left and right wheels at time t or by the linear velocity measured by the encoder of the left and right wheels at time t and the driving The speed is determined, and the standard deviation in the normal distribution is determined by the empirical standard deviation of the corresponding state mode; the state mode includes normal mode, left wheel encoder failure mode, right wheel encoder failure mode and left and right wheel encoder failure mode simultaneously ;
样本权重确定模块202，用于根据t时刻机器人的姿态样本、t时刻 机器人环境点云数据以及t-1时刻的环境地图估计，确定各姿态样本的权重，所述机器人激光雷达用于测量环境地图；The sample weight determination module 202 is used to determine the weight of each pose sample according to the pose sample of the robot at time t, the robot environment point cloud data at time t, and the environment map estimation at time t-1, and the robot lidar is used to measure the environment map ;
状态模式估计确定模块203，用于根据t时刻各姿态样本所属的状态模式以及t时刻各姿态样本的权重，计算t时刻发生各状态模式的估计概率，并选取估计概率最大的状态模式作为t时刻的状态模式估计；The state mode estimation determination module 203 is used to calculate the estimated probability of occurrence of each state mode at time t according to the state mode to which each attitude sample belongs at time t and the weight of each attitude sample at time t, and select the state mode with the largest estimated probability as time time t The state model estimation of ;
位姿估计确定模块204，用于将目标姿态样本中后验概率最大的姿态样本作为t时刻机器人的位姿估计，所述目标姿态样本包括所述t时刻的状态模式估计对应的姿态样本；The pose estimation determination module 204 is used to use the pose sample with the largest posterior probability in the target pose sample as the pose estimate of the robot at time t, the target pose sample including the pose sample corresponding to the state mode estimation at time t;
环境地图估计确定模块205，用于根据t时刻机器人环境点云数据以及t时刻机器人的位姿估计，确定t时刻的环境地图估计。The environment map estimation determination module 205 is configured to determine the environment map estimation at time t according to the robot environment point cloud data at time t and the pose estimation of the robot at time t.
在一个示例中，所述样本权重确定模块202，具体包括：In an example, the sample weight determination module 202 specifically includes:
配准点对确定单元，用于以所述姿态样本为初始位姿估计，利用迭代最近点方法确定t时刻环境点云数据与t-1时刻的环境地图估计在配准过程中的配准点对；The registration point pair determination unit is used to use the attitude sample as the initial pose estimation, and use the iterative closest point method to determine the registration point pair between the environmental point cloud data at time t and the environmental map estimation at time t-1 during the registration process;
距离计算单元，用于计算各所述配准点对中两点的距离；a distance calculation unit, configured to calculate the distance between two points in each of the registration point pairs;
权重确定单元，用于确定所述配准点对中所述距离小于设定阈值的配准点对的比例，并将所述比例作为t时刻所述姿态样本的权重。A weight determining unit, configured to determine a ratio of the registration point pairs whose distance is smaller than a set threshold among the registration point pairs, and use the ratio as the weight of the posture sample at time t.
所述状态模式估计确定模块203，具体包括：The state mode estimation determination module 203 specifically includes:
状态模式估计概率计算单元，用于根据
Figure PCTCN2021100035-appb-000112
计算t时刻发生各状态模式的估计概率
Figure PCTCN2021100035-appb-000113
其中，S k表示第k状态模式，s t表示t时刻的状态模式，当t时刻的第i姿态样本
Figure PCTCN2021100035-appb-000114
属于状态模式S k时，
Figure PCTCN2021100035-appb-000115
为1，否则
Figure PCTCN2021100035-appb-000116
为0，
Figure PCTCN2021100035-appb-000117
表示t时刻的第i姿态样本归一化后的权重，N表示t时刻姿态样本的数量。 The state mode estimation probability calculation unit is used for
Figure PCTCN2021100035-appb-000112
Calculate the estimated probability of occurrence of each state mode at time t
Figure PCTCN2021100035-appb-000113
Among them, S k represents the k-th state mode, s t represents the state mode at time t, when the i-th attitude sample at time t
Figure PCTCN2021100035-appb-000114
When belonging to the state mode S k ,
Figure PCTCN2021100035-appb-000115
is 1, otherwise
Figure PCTCN2021100035-appb-000116
is 0,
Figure PCTCN2021100035-appb-000117
Indicates the normalized weight of the i-th attitude sample at time t, and N indicates the number of attitude samples at time t.
在一个示例中，本实施例提供的系统还包括：线速度正态分布确定模块、偏航率正态分布确定模块、线速度样本采样模块、偏航率样本采样模块以及姿态样本确定模块。其中：In an example, the system provided by this embodiment further includes: a linear velocity normal distribution determination module, a yaw rate normal distribution determination module, a linear velocity sample sampling module, a yaw rate sample sampling module, and an attitude sample determination module. in:
线速度正态分布确定模块，用于：Line speed normal distribution determination module for:
根据
Figure PCTCN2021100035-appb-000118
计算t时刻线速度正态分布中的均值
Figure PCTCN2021100035-appb-000119
其中，
Figure PCTCN2021100035-appb-000120
表示t时刻机器人的左车轮线速度，
Figure PCTCN2021100035-appb-000121
表示t时刻机器人的右车轮线速度； according to
Figure PCTCN2021100035-appb-000118
Compute the mean in the normal distribution of the line speed at time t
Figure PCTCN2021100035-appb-000119
in,
Figure PCTCN2021100035-appb-000120
Indicates the linear velocity of the left wheel of the robot at time t,
Figure PCTCN2021100035-appb-000121
Indicates the linear velocity of the right wheel of the robot at time t;
偏航率正态分布确定模块，用于：The yaw rate normal distribution determination module is used for:
根据
Figure PCTCN2021100035-appb-000122
计算t时刻偏航率正态分布中的均值
Figure PCTCN2021100035-appb-000123
其中，W表示连接机器人左右车轮的轴长； according to
Figure PCTCN2021100035-appb-000122
Compute the mean in the normal distribution of the yaw rate at time t
Figure PCTCN2021100035-appb-000123
Among them, W represents the axis length connecting the left and right wheels of the robot;
线速度样本采样模块，用于遵循t时刻各状态模式发生的经验概率以及机器人各状态模式下的线速度正态分布，采样线速度样本；The line speed sample sampling module is used to follow the empirical probability of the occurrence of each state mode at time t and the line speed normal distribution under each state mode of the robot, and sample the line speed sample;
偏航率样本采样模块，用于遵循t时刻各状态模式发生的经验概率以及机器人各状态模式下的偏航率正态分布，采样偏航率样本；The yaw rate sample sampling module is used to sample the yaw rate samples according to the empirical probability of occurrence of each state mode at time t and the normal distribution of the yaw rate under each state mode of the robot;
姿态样本确定模块，用于根据所述线速度样本和所述偏航率样本确定t时刻机器人的姿态样本。An attitude sample determining module, configured to determine an attitude sample of the robot at time t according to the linear velocity sample and the yaw rate sample.
其中，所述机器人姿态的正态分布包括线速度正态分布和偏航率正态分布，所述线速度正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述线速度正态分布中的标准差由对应状态模式的经验标准差确定，所述偏航率正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述偏航率正态分布中的标准差由对应状态模式的经验标准差确定。W表示连接机器人左右车轮的轴长，
Figure PCTCN2021100035-appb-000124
表示t时刻机器人的左车轮线速度估计值，
Figure PCTCN2021100035-appb-000125
表示t时刻机器人的右车轮线速度估计值；其中，在所述正常模式下，t时刻机器人的左车轮线速度估计值为左车轮的编码器测量线速度，t时刻机器人的右车轮线速度估计值为右车轮的编码器测量线速度；在所述左轮编码器故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的驱动速度，t时刻机器人的右车轮线速度估计值为右车轮的编码器测量线速度；在右轮编码器故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的编码器测量线速度，t时刻机器人的右车轮线速度估计值为右车轮的驱动速度；在所述左右轮编码器同时故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的驱动速度，t时刻机器人的右车轮线速度估计值为右车轮的驱动速度。 Wherein, the normal distribution of the robot attitude includes the normal distribution of the linear velocity and the normal distribution of the yaw rate, and the mean value in the normal distribution of the linear velocity is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode, and the The standard deviation in the normal distribution of the linear velocity is determined by the empirical standard deviation of the corresponding state mode, the mean value in the normal distribution of the yaw rate is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode, and the yaw rate is positive The standard deviation in the state distribution is determined from the empirical standard deviation of the corresponding state pattern. W represents the axis length connecting the left and right wheels of the robot,
Figure PCTCN2021100035-appb-000124
Indicates the estimated linear velocity of the left wheel of the robot at time t,
Figure PCTCN2021100035-appb-000125
Represents the estimated value of the linear velocity of the right wheel of the robot at time t; wherein, in the normal mode, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the linear velocity of the right wheel of the robot at time t is estimated is the linear velocity measured by the encoder of the right wheel; in the failure mode of the left wheel encoder, the estimated value of the linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel, and the estimated value of the linear velocity of the right wheel of the robot at time t is the right The encoder of the wheel measures the linear velocity; in the failure mode of the right wheel encoder, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the estimated linear velocity of the right wheel of the robot at time t is the right wheel In the simultaneous failure mode of the left and right wheel encoders, the estimated linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel, and the estimated linear velocity of the right wheel of the robot at time t is the driving speed of the right wheel.
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的 都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。Each embodiment in this specification is described in a progressive manner, and each embodiment focuses on the difference from other embodiments, and the same and similar parts between the various embodiments can be referred to each other. As for the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and for the related information, please refer to the description of the method part.
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。In this paper, specific examples have been used to illustrate the principle and implementation of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea; meanwhile, for those of ordinary skill in the art, according to the present invention Thoughts, there will be changes in specific implementation methods and application ranges. In summary, the contents of this specification should not be construed as limiting the present invention.

Claims (10)

  1. 一种移动机器人故障下同时FDD和SLAM的方法，其特征在于，包括：A method for simultaneous FDD and SLAM under a mobile robot fault, characterized in that it includes:
    获取t时刻机器人的姿态样本；其中，t时刻机器人的姿态样本在各状态模式上的分布概率由t时刻机器人发生各状态模式的经验概率确定，机器人t时刻发生各状态模式的经验概率根据机器人t-1时刻的状态模式估计以及状态转移概率确定；t时刻任一状态模式下的姿态样本分布遵循对应状态模式下机器人姿态的正态分布，所述正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述左右车轮线速度估计值由t时刻左右车轮的编码器测量线速度确定或由t时刻左右车轮的编码器测量线速度以及驱动速度确定，所述正态分布中的标准差由对应状态模式的经验标准差确定；所述状态模式包括正常模式、左轮编码器故障模式、右轮编码器故障模式以及左右轮编码器同时故障模式；Obtain the attitude samples of the robot at time t; among them, the distribution probability of the attitude samples of the robot on each state mode at time t is determined by the empirical probability of each state mode of the robot at time t, and the empirical probability of each state mode of the robot at time t is determined by the robot t - State mode estimation and state transition probability determination at time t; attitude sample distribution in any state mode at time t follows the normal distribution of robot attitude in the corresponding state mode, and the mean value in the normal distribution is determined by the corresponding state mode The estimated values of the left and right wheel linear velocities are determined by the linear velocities measured by the encoders of the left and right wheels at time t or by the linear velocities measured by the encoders of the left and right wheels and the driving speed at time t. The normal distribution The standard deviation in is determined by the empirical standard deviation of the corresponding state mode; The state mode includes normal mode, left wheel encoder failure mode, right wheel encoder failure mode and left and right wheel encoder failure mode simultaneously;
    根据t时刻机器人的姿态样本、t时刻机器人的环境点云数据以及t-1时刻机器人的环境地图估计，确定各姿态样本的权重；其中，环境点云数据为实际测量数据；According to the attitude sample of the robot at time t, the environment point cloud data of the robot at time t, and the environment map estimation of the robot at time t-1, determine the weight of each attitude sample; where the environment point cloud data is the actual measurement data;
    根据t时刻各姿态样本所属的状态模式以及t时刻各姿态样本的权重，计算t时刻发生各状态模式的估计概率，并选取估计概率最大的状态模式作为t时刻的状态模式估计；According to the state pattern to which each attitude sample belongs at t moment and the weight of each attitude sample at t moment, calculate the estimated probability of occurrence of each state pattern at t moment, and select the state pattern with the largest estimated probability as the state pattern estimate at t moment;
    将目标姿态样本中后验概率最大的姿态样本作为t时刻机器人的位姿估计，所述目标姿态样本包括所述t时刻的状态模式估计对应的姿态样本；Using the attitude sample with the largest posterior probability in the target attitude sample as the pose estimation of the robot at time t, the target attitude sample includes the attitude sample corresponding to the state mode estimation at time t;
    根据t时刻机器人的环境点云数据以及t时刻机器人的位姿估计，确定t时刻的环境地图估计。According to the environment point cloud data of the robot at time t and the pose estimation of the robot at time t, the environment map estimation at time t is determined.
  2. 根据权利要求1所述的移动机器人故障下同时FDD和SLAM的方法，其特征在于，所述根据t时刻机器人的姿态样本、t时刻机器人的环境点云数据以及t-1时刻机器人的环境地图估计，确定各姿态样本的权重，具体包括：The method for simultaneous FDD and SLAM under the failure of the mobile robot according to claim 1, characterized in that, according to the attitude sample of the robot at time t, the environment point cloud data of the robot at time t, and the environment map estimation of the robot at time t-1 , to determine the weight of each pose sample, including:
    对t时刻任一所述姿态样本：For any attitude sample at time t:
    以所述姿态样本为初始位姿估计，利用迭代最近点方法确定t时刻环境点云数据与t-1时刻的环境地图估计在配准过程中的配准点对；Using the attitude sample as the initial pose estimation, using the iterative closest point method to determine the registration point pairs between the environmental point cloud data at time t and the environmental map estimation at time t-1 during the registration process;
    计算各所述配准点对中两点的距离；Calculating the distance between two points in each of the registration points;
    确定所述配准点对中所述距离小于设定阈值的配准点对的比例，并将所述比例作为t时刻所述姿态样本的权重。Determining the ratio of the registration point pairs whose distance is smaller than the set threshold among the registration point pairs, and using the ratio as the weight of the attitude sample at time t.
  3. 根据权利要求1所述的移动机器人故障下同时FDD和SLAM的方法，其特征在于，所述根据t时刻各姿态样本所属的状态模式以及t时刻各姿态样本的权重，计算t时刻发生各状态模式的估计概率，具体包括：The method for simultaneous FDD and SLAM under the failure of the mobile robot according to claim 1, characterized in that, according to the state pattern to which each attitude sample belongs at time t and the weight of each attitude sample at time t, each state mode that occurs at time t is calculated The estimated probability of , including:
    根据
    Figure PCTCN2021100035-appb-100001
    计算t时刻发生各状态模式的估计概率
    Figure PCTCN2021100035-appb-100002
    其中，S k表示第k状态模式，s t表示t时刻的状态模式，当t时刻的第i姿态样本
    Figure PCTCN2021100035-appb-100003
    属于状态模式S k时，
    Figure PCTCN2021100035-appb-100004
    为1，否则
    Figure PCTCN2021100035-appb-100005
    为0，
    Figure PCTCN2021100035-appb-100006
    表示t时刻的第i姿态样本归一化后的权重，N表示t时刻姿态样本的数量。     according to
    Figure PCTCN2021100035-appb-100001
    Calculate the estimated probability of occurrence of each state mode at time t
    Figure PCTCN2021100035-appb-100002
    Among them, S k represents the k-th state mode, s t represents the state mode at time t, when the i-th attitude sample at time t
    Figure PCTCN2021100035-appb-100003
    When belonging to the state mode S k ,
    Figure PCTCN2021100035-appb-100004
    is 1, otherwise
    Figure PCTCN2021100035-appb-100005
    is 0,
    Figure PCTCN2021100035-appb-100006
    Indicates the normalized weight of the i-th attitude sample at time t, and N indicates the number of attitude samples at time t.
  4. 根据权利要求1所述的移动机器人故障下同时FDD和SLAM的方法，其特征在于，在所述获取t时刻机器人的姿态样本之前，还包括：The method for simultaneous FDD and SLAM under the failure of the mobile robot according to claim 1, is characterized in that, before the attitude sample of the robot at the moment of the acquisition t, it also includes:
    遵循t时刻各状态模式发生的经验概率以及机器人各状态模式下的线速度正态分布，采样线速度样本；Follow the empirical probability of occurrence of each state mode at time t and the normal distribution of the line speed in each state mode of the robot, and sample line speed samples;
    遵循t时刻各状态模式发生的经验概率以及机器人各状态模式下的偏航率正态分布，采样偏航率样本；Follow the empirical probability of occurrence of each state mode at time t and the normal distribution of yaw rate in each state mode of the robot, and sample the yaw rate samples;
    根据所述线速度样本和所述偏航率样本确定t时刻机器人的姿态样本；Determine the attitude sample of the robot at time t according to the linear velocity sample and the yaw rate sample;
    其中，所述机器人姿态的正态分布包括线速度正态分布和偏航率正态分布，所述线速度正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述线速度正态分布中的标准差由对应状态模式的经验标准差确定，所述偏航率正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述偏航率正态分布中的标准差由对应状态模式的经验标准差确定。Wherein, the normal distribution of the robot attitude includes the normal distribution of the linear velocity and the normal distribution of the yaw rate, and the mean value in the normal distribution of the linear velocity is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode, and the The standard deviation in the normal distribution of the linear velocity is determined by the empirical standard deviation of the corresponding state mode, the mean value in the normal distribution of the yaw rate is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode, and the yaw rate is positive The standard deviation in the state distribution is determined from the empirical standard deviation of the corresponding state pattern.
  5. 根据权利要求4所述的移动机器人故障下同时FDD和SLAM的方法，其特征在于，在采样线速度样本和采样偏航率样本之前，还包括：The method for simultaneous FDD and SLAM under the failure of the mobile robot according to claim 4, wherein, before sampling the linear velocity sample and sampling the yaw rate sample, it also includes:
    获取机器人的左车轮线速度和右车轮线速度；Obtain the linear velocity of the left wheel and the right wheel of the robot;
    根据
    Figure PCTCN2021100035-appb-100007
    计算t时刻线速度正态分布中的均值
    Figure PCTCN2021100035-appb-100008
    和t时刻偏航率正态分布中的均值
    Figure PCTCN2021100035-appb-100009
    其中，W表示连接机器人左右车轮的轴长，
    Figure PCTCN2021100035-appb-100010
    表示t时刻机器人的左车轮线速度估计值，
    Figure PCTCN2021100035-appb-100011
    表示t时刻机器人的右车轮线速度估计值；其中，在所述正常模式下，t时刻机器人的左车轮线速度估计值为左车轮的编码器测量线速度，t时刻机器人的右车轮线速度估计值为右车轮的编码器测量线速度；在所述左轮编码器故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的驱动速度，t时刻机器人的右车轮线速度估计值为右车轮的编码器测量线速度；在右轮编码器故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的编码器测量线速度，t时刻机器人的右车轮线速度估计值为右车轮的驱动速度；在所述左右轮编码器同时故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的驱动速度，t时刻机器人的右车轮线速度估计值为右车轮的驱动速度。     according to
    Figure PCTCN2021100035-appb-100007
    Compute the mean in the normal distribution of the line speed at time t
    Figure PCTCN2021100035-appb-100008
    and the mean value in the normal distribution of the yaw rate at time t
    Figure PCTCN2021100035-appb-100009
    Among them, W represents the axis length connecting the left and right wheels of the robot,
    Figure PCTCN2021100035-appb-100010
    Indicates the estimated linear velocity of the left wheel of the robot at time t,
    Figure PCTCN2021100035-appb-100011
    Represents the estimated value of the linear velocity of the right wheel of the robot at time t; wherein, in the normal mode, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the linear velocity of the right wheel of the robot at time t is estimated is the linear velocity measured by the encoder of the right wheel; in the failure mode of the left wheel encoder, the estimated value of the linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel, and the estimated value of the linear velocity of the right wheel of the robot at time t is the right The encoder of the wheel measures the linear velocity; in the failure mode of the right wheel encoder, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the estimated linear velocity of the right wheel of the robot at time t is the right wheel In the simultaneous failure mode of the left and right wheel encoders, the estimated linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel, and the estimated linear velocity of the right wheel of the robot at time t is the driving speed of the right wheel.
  6. 一种移动机器人故障下同时FDD和SLAM的系统，其特征在于，包括：A system for simultaneous FDD and SLAM under a mobile robot failure, characterized in that it includes:
    姿态样本获取模块，用于获取t时刻机器人的姿态样本；其中，t时刻机器人的姿态样本在各状态模式上的分布概率由t时刻机器人发生各状态模式的经验概率确定，机器人t时刻发生各状态模式的经验概率根据机器人t-1时刻的状态模式估计以及状态转移概率确定；t时刻任一状态模式下的姿态样本分布遵循对应状态模式下机器人姿态的正态分布，所述正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述左右车轮线速度估计值由t时刻左右车轮的编码器测量线速度确定或由t时刻左右车轮的编码器测量线速度以及驱动速度确定，所述正态分布中的标准差由对应状态模式的经验标准差确定；所述状态模式包括正常模式、左轮编码器故障模式、右轮编码器故障模式以及左右轮编码器同时故障模 式；The attitude sample acquisition module is used to acquire the attitude samples of the robot at time t; wherein, the distribution probability of the attitude samples of the robot at each state mode at time t is determined by the empirical probability of each state mode of the robot at time t, and each state mode occurs at time t of the robot. The empirical probability of the mode is determined according to the state mode estimation and state transition probability of the robot at time t-1; the attitude sample distribution in any state mode at time t follows the normal distribution of robot attitude in the corresponding state mode, and the The mean value is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode. The estimated value of the linear velocity of the left and right wheels is determined by the linear velocity measured by the encoder of the left and right wheels at time t or by the linear velocity measured by the encoder of the left and right wheels at time t and the driving speed It is determined that the standard deviation in the normal distribution is determined by the empirical standard deviation of the corresponding state mode; the state mode includes a normal mode, a failure mode of the left wheel encoder, a failure mode of the right wheel encoder and a simultaneous failure mode of the left and right wheel encoders;
    样本权重确定模块，用于根据t时刻机器人的姿态样本、t时刻机器人的环境点云数据以及t-1时刻机器人的环境地图估计，确定各姿态样本的权重；其中，环境点云数据为实际测量数据；The sample weight determination module is used to determine the weight of each attitude sample according to the attitude sample of the robot at time t, the environmental point cloud data of the robot at time t, and the environmental map estimation of the robot at time t-1; wherein, the environmental point cloud data is the actual measurement data;
    状态模式估计确定模块，用于根据t时刻各姿态样本所属的状态模式以及t时刻各姿态样本的权重，计算t时刻发生各状态模式的估计概率，并选取估计概率最大的状态模式作为t时刻的状态模式估计；The state mode estimation determination module is used to calculate the estimated probability of each state mode at time t according to the state mode to which each attitude sample belongs at time t and the weight of each attitude sample at time t, and select the state mode with the largest estimated probability as the state mode at time t state mode estimation;
    位姿估计确定模块，用于将目标姿态样本中后验概率最大的姿态样本作为t时刻机器人的位姿估计，所述目标姿态样本包括所述t时刻的状态模式估计对应的姿态样本；The pose estimation determination module is used to use the pose sample with the largest posterior probability in the target pose sample as the pose estimate of the robot at time t, and the target pose sample includes a pose sample corresponding to the state mode estimation at time t;
    环境地图估计确定模块，用于根据t时刻机器人的环境点云数据以及t时刻机器人的位姿估计，确定t时刻的环境地图估计。The environment map estimation determination module is used to determine the environment map estimation at time t according to the environment point cloud data of the robot at time t and the pose estimation of the robot at time t.
  7. 根据权利要求6所述的移动机器人故障下同时FDD和SLAM的系统，其特征在于，所述样本权重确定模块，具体包括：The system of simultaneous FDD and SLAM under the failure of the mobile robot according to claim 6, wherein the sample weight determination module specifically includes:
    配准点对确定单元，用于以所述姿态样本为初始位姿估计，利用迭代最近点方法确定t时刻环境点云数据与t-1时刻的环境地图估计在配准过程中的配准点对；The registration point pair determination unit is used to use the attitude sample as the initial pose estimation, and use the iterative closest point method to determine the registration point pair between the environmental point cloud data at time t and the environmental map estimation at time t-1 during the registration process;
    距离计算单元，用于计算各所述配准点对中两点的距离；a distance calculation unit, configured to calculate the distance between two points in each of the registration point pairs;
    权重确定单元，用于确定所述配准点对中所述距离小于设定阈值的配准点对的比例，并将所述比例作为t时刻所述姿态样本的权重。A weight determining unit, configured to determine a ratio of the registration point pairs whose distance is smaller than a set threshold among the registration point pairs, and use the ratio as the weight of the posture sample at time t.
  8. 根据权利要求6所述的移动机器人故障下同时FDD和SLAM的系统，其特征在于，所述状态模式估计确定模块，具体包括：The system of simultaneous FDD and SLAM under the mobile robot failure according to claim 6, wherein the state mode estimation and determination module specifically includes:
    状态模式估计概率计算单元，用于根据
    Figure PCTCN2021100035-appb-100012
    计算t时刻发生各状态模式的估计概率
    Figure PCTCN2021100035-appb-100013
    其中，S k表示第k状态模式，s t表示t时刻的状态模式，当t时刻的第i姿态样本
    Figure PCTCN2021100035-appb-100014
    属于状态模式S k时，
    Figure PCTCN2021100035-appb-100015
    为1，否则
    Figure PCTCN2021100035-appb-100016
    为0，
    Figure PCTCN2021100035-appb-100017
    表示t时刻的第i姿态样本归一化后的权重，N表示t时刻姿态样本的数量。     The state mode estimation probability calculation unit is used for
    Figure PCTCN2021100035-appb-100012
    Calculate the estimated probability of occurrence of each state mode at time t
    Figure PCTCN2021100035-appb-100013
    Among them, S k represents the k-th state mode, s t represents the state mode at time t, when the i-th attitude sample at time t
    Figure PCTCN2021100035-appb-100014
    When belonging to the state mode S k ,
    Figure PCTCN2021100035-appb-100015
    is 1, otherwise
    Figure PCTCN2021100035-appb-100016
    is 0,
    Figure PCTCN2021100035-appb-100017
    Indicates the normalized weight of the i-th attitude sample at time t, and N indicates the number of attitude samples at time t.
  9. 根据权利要求6所述的移动机器人故障下同时FDD和SLAM的系统，其特征在于，所述系统还包括：The system of simultaneous FDD and SLAM under the failure of the mobile robot according to claim 6, wherein the system also includes:
    线速度样本采样模块，用于遵循t时刻各状态模式发生的经验概率以及机器人各状态模式下的线速度正态分布，采样线速度样本；The line speed sample sampling module is used to follow the empirical probability of the occurrence of each state mode at time t and the line speed normal distribution under each state mode of the robot, and sample the line speed sample;
    偏航率样本采样模块，用于遵循t时刻各状态模式发生的经验概率以及机器人各状态模式下的偏航率正态分布，采样偏航率样本；The yaw rate sample sampling module is used to sample the yaw rate samples according to the empirical probability of occurrence of each state mode at time t and the normal distribution of the yaw rate under each state mode of the robot;
    姿态样本确定模块，用于根据所述线速度样本和所述偏航率样本确定t时刻机器人的姿态样本；Attitude sample determination module, used to determine the attitude sample of the robot at time t according to the linear velocity sample and the yaw rate sample;
    其中，所述机器人姿态的正态分布包括线速度正态分布和偏航率正态分布，所述线速度正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述线速度正态分布中的标准差由对应状态模式的经验标准差确定，所述偏航率正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述偏航率正态分布中的标准差由对应状态模式的经验标准差确定。Wherein, the normal distribution of the robot attitude includes the normal distribution of the linear velocity and the normal distribution of the yaw rate, and the mean value in the normal distribution of the linear velocity is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode, and the The standard deviation in the normal distribution of the linear velocity is determined by the empirical standard deviation of the corresponding state mode, the mean value in the normal distribution of the yaw rate is determined by the estimated value of the linear velocity of the left and right wheels in the corresponding state mode, and the yaw rate is positive The standard deviation in the state distribution is determined from the empirical standard deviation of the corresponding state pattern.
  10. 根据权利要求9所述的移动机器人故障下同时FDD和SLAM的系统，其特征在于，所述系统还包括：The system of simultaneous FDD and SLAM under the failure of the mobile robot according to claim 9, wherein the system also includes:
    线速度正态分布确定模块，用于：Linear velocity normal distribution determination module for:
    根据
    Figure PCTCN2021100035-appb-100018
    计算t时刻线速度正态分布中的均值
    Figure PCTCN2021100035-appb-100019
    其中，
    Figure PCTCN2021100035-appb-100020
    表示t时刻机器人的左车轮线速度估计值，
    Figure PCTCN2021100035-appb-100021
    表示t时刻机器人的右车轮线速度估计值；其中，在所述正常模式下，t时刻机器人的左车轮线速度估计值为左车轮的编码器测量线速度，t时刻机器人的右车轮线速度估计值为右车轮的编码器测量线速度；在所述左轮编码器故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的驱动速度，t时刻机器人的右车轮线速度估计值为右车轮的编码器测量线速度；在右轮编码器故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的编码器测量线速度，t时刻机器人的右车轮线速度估计值为右车轮的驱动速度；在所述左右轮编码 器同时故障模式下，t时刻机器人的左车轮线速度估计值为左车轮的驱动速度，t时刻机器人的右车轮线速度估计值为右车轮的驱动速度；     according to
    Figure PCTCN2021100035-appb-100018
    Compute the mean in the normal distribution of the line speed at time t
    Figure PCTCN2021100035-appb-100019
    in,
    Figure PCTCN2021100035-appb-100020
    Indicates the estimated linear velocity of the left wheel of the robot at time t,
    Figure PCTCN2021100035-appb-100021
    Represents the estimated value of the linear velocity of the right wheel of the robot at time t; wherein, in the normal mode, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the linear velocity of the right wheel of the robot at time t is estimated is the linear velocity measured by the encoder of the right wheel; in the failure mode of the left wheel encoder, the estimated value of the linear velocity of the left wheel of the robot at time t is the driving speed of the left wheel, and the estimated value of the linear velocity of the right wheel of the robot at time t is the right The encoder of the wheel measures the linear velocity; in the failure mode of the right wheel encoder, the estimated linear velocity of the left wheel of the robot at time t is the linear velocity measured by the encoder of the left wheel, and the estimated linear velocity of the right wheel of the robot at time t is the right wheel drive speed; in the simultaneous failure mode of the left and right wheel encoders, the estimated linear velocity of the left wheel of the robot at time t is the drive speed of the left wheel, and the estimated linear velocity of the right wheel of the robot at time t is the drive speed of the right wheel;
    偏航率正态分布确定模块，用于：The yaw rate normal distribution determination module is used for:
    根据
    Figure PCTCN2021100035-appb-100022
    计算t时刻偏航率正态分布中的均值
    Figure PCTCN2021100035-appb-100023
    其中， W表示连接机器人左右车轮的轴长。     according to
    Figure PCTCN2021100035-appb-100022
    Compute the mean in the normal distribution of the yaw rate at time t
    Figure PCTCN2021100035-appb-100023
    Among them, W represents the axis length connecting the left and right wheels of the robot.
PCT/CN2021/100035 2021-06-15 2021-06-15 Method and system for simultaneously performing fdd and slam under mobile robot fault WO2022261814A1 (en)

Priority Applications (2)

Application Number Priority Date Filing Date Title
PCT/CN2021/100035 WO2022261814A1 (en) 2021-06-15 2021-06-15 Method and system for simultaneously performing fdd and slam under mobile robot fault
ZA2023/07096A ZA202307096B (en) 2021-06-15 2023-07-14 Method and system for simultaneously performing fdd and slam under mobile robot fault

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/CN2021/100035 WO2022261814A1 (en) 2021-06-15 2021-06-15 Method and system for simultaneously performing fdd and slam under mobile robot fault

Publications (1)

Publication Number Publication Date
WO2022261814A1 true WO2022261814A1 (en) 2022-12-22

Family

ID=84526731

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2021/100035 WO2022261814A1 (en) 2021-06-15 2021-06-15 Method and system for simultaneously performing fdd and slam under mobile robot fault

Country Status (2)

Country Link
WO (1) WO2022261814A1 (en)
ZA (1) ZA202307096B (en)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5739657A (en) * 1995-05-10 1998-04-14 Fujitsu Limited Apparatus for controlling motion of normal wheeled omni-directional vehicle and method thereof
CN103795373A (en) * 2013-11-29 2014-05-14 电子科技大学中山学院 Particle filter generating method for incomplete system fault diagnosis
CN109129574A (en) * 2018-11-08 2019-01-04 山东大学 Service robot kinematic system cloud fault diagnosis system and method
CN109669350A (en) * 2017-10-13 2019-04-23 电子科技大学中山学院 A kind of three-wheel omni-directional mobile robots wheel skid quantitative estimation method
CN110216680A (en) * 2019-07-05 2019-09-10 山东大学 A kind of service robot cloud ground collaborative fault diagnosis system and method

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5739657A (en) * 1995-05-10 1998-04-14 Fujitsu Limited Apparatus for controlling motion of normal wheeled omni-directional vehicle and method thereof
CN103795373A (en) * 2013-11-29 2014-05-14 电子科技大学中山学院 Particle filter generating method for incomplete system fault diagnosis
CN109669350A (en) * 2017-10-13 2019-04-23 电子科技大学中山学院 A kind of three-wheel omni-directional mobile robots wheel skid quantitative estimation method
CN109129574A (en) * 2018-11-08 2019-01-04 山东大学 Service robot kinematic system cloud fault diagnosis system and method
CN110216680A (en) * 2019-07-05 2019-09-10 山东大学 A kind of service robot cloud ground collaborative fault diagnosis system and method

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
DUAN ZHUO-HUA, CAI ZI-XING, YU JIN-XIA: "Fault Diagnosis and Fault Tolerant Control for Wheeled Mobile Robots under Unknown Environments: A Survey", ROBOTICS AND AUTOMATION, 2005. PROCEEDINGS OF THE 2005 IEEE INTERNATIONAL CONFERENCE ON BARCELONA, SPAIN 18-22 APRIL 2005, PISCATAWAY, NJ, USA,IEEE, 18 April 2005 (2005-04-18) - 22 April 2005 (2005-04-22), pages 3428 - 3433, XP010871452, ISBN: 978-0-7803-8914-4, DOI: 10.1109/ROBOT.2005.1570640 *

Also Published As

Publication number Publication date
ZA202307096B (en) 2023-09-27

Similar Documents

Publication Publication Date Title
Meng et al. Iterative-learning error compensation for autonomous parking of mobile manipulator in harsh industrial environment
WO2019047641A1 (en) Method and device for estimating orientation error of onboard camera
CN110986968B (en) Method and device for real-time global optimization and error loop judgment in three-dimensional reconstruction
CN110531357A (en) Estimate the method and radar sensing system of mobile target velocity magnitude in a horizontal plane
CN107066806B (en) Data Association and device
CN111024124B (en) Combined navigation fault diagnosis method for multi-sensor information fusion
CN111337018A (en) Positioning method and device, intelligent robot and computer readable storage medium
CN111025250A (en) On-line calibration method for vehicle-mounted millimeter wave radar
CN114396957B (en) Positioning pose calibration method based on vision and map lane line matching and automobile
CN113436274B (en) Calibration method, device and equipment for mobile robot
CN110637209A (en) Method, apparatus, and computer-readable storage medium having instructions for estimating a pose of a motor vehicle
WO2022261814A1 (en) Method and system for simultaneously performing fdd and slam under mobile robot fault
CN113375658B (en) Method and system for simultaneously FDD and SLAM under fault of mobile robot
CN108680162B (en) Human body target tracking method based on progressive unscented Kalman filtering
Liu et al. LGC-Net: A Lightweight Gyroscope Calibration Network for Efficient Attitude Estimation
CN114266776B (en) Digital image correlation method applying composite crack displacement field function
CN111829473B (en) Method and system for ranging moving chassis during traveling
CN114252073A (en) Robot attitude data fusion method
CN113503891A (en) SINSDVL alignment correction method, system, medium and equipment
Mortari et al. Attitude and position estimation from vector observations
CN110749327A (en) Vehicle navigation method in cooperation environment
Xu et al. Automatic and Accurate Vision-Based Measurement of Camber and Toe-In Alignment of Vehicle Wheel
CN114326731B (en) Multi-robot formation tracking sampling control method and system based on laser radar
TWI795306B (en) Localization failure detection system for autonomous mobile robots using deep learning based 6d pose regression
Fazekas et al. Odometry Model Calibration for Self-Driving Vehicles with Noise Correction

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21945407

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE