Robot positioning and control method and device, storage medium and equipment

Abstract

The invention discloses a robot positioning and control method and device, a storage medium and equipment, and the method comprises the steps: firstly, employing an IMU, a wheel speed meter and a visual sensor which are installed on a target robot in advance, and synchronously obtaining IMU measurement data, wheel speed meter measurement data and two-dimensional code positioning data of the target robot; performing fusion processing on IMU measurement data, wheel speed meter measurement data and two-dimensional code positioning data of the target robot by using ESKF, and determining a pose result of the target robot according to a processing result; and then receiving a preset motion instruction, and controlling the target robot to move to the target final position according to the preset motion instruction and the pose result of the target robot. Therefore, under globally consistent multi-sensor fusion processing, the advantages of all the sensors are fully utilized, the defects of a single sensor are overcome, more accurate and reliable real-time positioning and navigation control are achieved, and then the motion control effect on the target robot can be improved.

Description

Technical Field

[0001] The present application relates to the field of robot technology, and in particular to a robot positioning and control method, device, storage medium and equipment. Background Art

[0002] With the advancement of robotics technology, robots are becoming more and more diverse and their functions are becoming more and more powerful, which can bring great convenience and comfortable experience to people's lives. Among them, autonomous navigation and control of the robot chassis are the key technologies to achieve autonomous movement of robots.

[0003] At present, there are usually two methods to achieve robot positioning: the first is the absolute positioning method, which relies on external information to provide absolute position and posture information, such as vision-based positioning methods, etc., but this method has problems such as long-term drift, high computational cost and inaccurate scale estimation. The second is the relative positioning method, which uses its own sensors, such as inertial measurement units (IMUs) or wheel speed meters, combined with kinematic or dynamic models to obtain the vehicle's position and posture increment information, and then achieve robot positioning. However, due to the existence of integral errors, this method will accumulate positioning errors over time, affecting the positioning and navigation accuracy of the chassis. It can be seen that both of the existing methods for achieving robot positioning have the problem of inaccurate positioning results, which in turn affects the robot's motion control effect. Summary of the invention

[0004] The main purpose of the embodiments of the present application is to provide a robot positioning and control method, device, storage medium and equipment, which can achieve more accurate and reliable real-time positioning and navigation control under globally consistent multi-sensor fusion processing, thereby improving the motion control effect of the robot.

[0005] The present application provides a robot positioning and control method, including:

[0006] Using the inertial measurement unit IMU, wheel speed meter and visual sensor pre-installed on the target robot, the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot are synchronously acquired respectively;

[0007] Using an extended state Kalman filter (ESKF), the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot are fused and processed, and the position and posture result of the target robot is determined according to the processing result;

[0008] Receive a preset motion instruction, and control the target robot to move to a target end position according to the preset motion instruction and the position result of the target robot; wherein the target end position is determined according to the path point and target angle value included in the preset motion instruction.

[0009] In a possible implementation, the extended state Kalman filter ESKF is used to fuse the IMU measurement data, the wheel speed meter measurement data and the QR code positioning data of the target robot, and the position and posture result of the target robot is determined according to the processing result, including:

[0010] Initialize the pose state, covariance matrix and bias parameters according to the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot, and determine the initial pose state of the target robot using the initialized pose state, covariance matrix and bias parameters;

[0011] Perform state prediction based on the IMU measurement data and wheel speed meter measurement data of the target robot, and calculate the state transfer matrix and process noise covariance matrix;

[0012] Update the state estimate according to the two-dimensional code positioning data, and calculate the observation matrix and the observation noise covariance matrix;

[0013] The extended state Kalman filter (ESKF) is used to calculate the Kalman gain according to the state transfer matrix, the process noise covariance matrix, the observation matrix and the observation noise covariance matrix; and the Kalman gain is used to update the initial posture state of the target robot to obtain a posture estimation result as the posture result of the target robot.

[0014] In a possible implementation, after the initial posture state of the target robot is updated by using the Kalman gain to obtain a posture estimation result, the method further includes:

[0015] The pose inference device is used to infer the current pose result of the target robot according to the speed data and the pose estimation result.

[0016] In a possible implementation, after the pose inference device is used to infer the current pose result of the target robot according to the velocity data and the pose estimation result, the method further includes:

[0017] The current posture result of the target robot calculated by the posture estimator is smoothed by using a posture smoother to obtain the processed posture result of the target robot.

[0018] In a possible implementation, controlling the target robot to move to a target end position according to the preset motion instruction and the position result of the target robot includes:

[0019] According to the path points and target angle values ​​included in the preset motion instruction, the linear velocity and angular velocity instructions are determined, and according to the linear velocity and angular velocity instructions and the position and posture results of the target robot, the target robot is controlled to move to the target end position.

[0020] In a possible implementation, before using an inertial measurement unit IMU, a wheel speed meter, and a visual sensor pre-installed on the target robot to synchronously acquire the IMU measurement data, the wheel speed meter measurement data, and the QR code positioning data of the target robot, respectively, the method further includes:

[0021] At preset positions on the motion path of the target robot, N two-dimensional code labels are pre-set; N is a positive integer greater than 0.

[0022] In a possible implementation, the N two-dimensional code tags are arranged at equal intervals; each of the N two-dimensional code tags is arranged in the form of a 3x3 nine-square grid two-dimensional code group.

[0023] The present application also provides a robot positioning and control device, including:

[0024] An acquisition unit is used to synchronously acquire IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot by using an inertial measurement unit IMU, a wheel speed meter and a visual sensor pre-installed on the target robot;

[0025] A fusion unit is used to use an extended state Kalman filter (ESKF) to fuse the IMU measurement data, the wheel speed meter measurement data and the QR code positioning data of the target robot, and determine the position and posture result of the target robot according to the processing result;

[0026] A control unit is used to receive a preset motion instruction and control the target robot to move to a target end position according to the preset motion instruction and the position and posture result of the target robot; wherein the target end position is determined according to the path point and target angle value included in the preset motion instruction.

[0027] In a possible implementation, the fusion unit includes:

[0028] A determination subunit is used to initialize the pose state, covariance matrix and bias parameters according to the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot, and determine the initial pose state of the target robot using the initialized pose state, covariance matrix and bias parameters;

[0029] A first computing subunit is used to perform state prediction based on the IMU measurement data and the wheel speed meter measurement data of the target robot, and calculate a state transfer matrix and a process noise covariance matrix;

[0030] A second computing subunit, used for updating the state estimation according to the two-dimensional code positioning data, and calculating the observation matrix and the observation noise covariance matrix;

[0031] The updating subunit is used to calculate the Kalman gain by using the extended state Kalman filter ESKF according to the state transfer matrix, the process noise covariance matrix, the observation matrix and the observation noise covariance matrix; and to update the initial posture state of the target robot by using the Kalman gain to obtain a posture estimation result as the posture result of the target robot.

[0032] In a possible implementation, the fusion unit further includes:

[0033] The calculation subunit is used to use the posture inference device to calculate the current posture result of the target robot according to the speed data and the posture estimation result.

[0034] In a possible implementation, the fusion unit further includes:

[0035] The smoothing subunit is used to use a posture smoother to smooth the current posture result of the target robot calculated by the posture inference device to obtain the processed posture result of the target robot.

[0036] In a possible implementation, the control unit is specifically configured to:

[0037] According to the path points and target angle values ​​included in the preset motion instruction, the linear velocity and angular velocity instructions are determined, and according to the linear velocity and angular velocity instructions and the position and posture results of the target robot, the target robot is controlled to move to the target end position.

[0038] In a possible implementation manner, the device further includes:

[0039] The setting unit is used to pre-set N two-dimensional code labels at preset positions on the motion path of the target robot; N is a positive integer greater than 0.

[0040] In a possible implementation, the N two-dimensional code tags are arranged at equal intervals; each of the N two-dimensional code tags is arranged in the form of a 3x3 nine-square grid two-dimensional code group.

[0041] The embodiment of the present application also provides a robot positioning and control device, including: a processor, a memory, and a system bus;

[0042] The processor and the memory are connected via the system bus;

[0043] The memory is used to store one or more programs, and the one or more programs include instructions. When the instructions are executed by the processor, the processor executes any one of the implementation methods of the above-mentioned robot positioning and control method.

[0044] An embodiment of the present application further provides a computer-readable storage medium, wherein the computer-readable storage medium stores instructions, and when the instructions are executed on a terminal device, the terminal device executes any one of the implementations of the above-mentioned robot positioning and control method.

[0045] The embodiment of the present application further provides a computer program product, which, when executed on a terminal device, enables the terminal device to execute any one of the implementations of the above-mentioned robot positioning and control method.

[0046] A robot positioning and control method, device, storage medium and equipment provided in an embodiment of the present application first utilizes an IMU, a wheel speed meter and a visual sensor pre-installed on a target robot to synchronously acquire the IMU measurement data, the wheel speed meter measurement data and the QR code positioning data of the target robot respectively, and then utilizes an extended state Kalman filter (ESKF) to fuse the IMU measurement data, the wheel speed meter measurement data and the QR code positioning data of the target robot, and determines the position and posture result of the target robot according to the processing result; then, receives a preset motion instruction, and controls the target robot to move to a target end position according to the preset motion instruction and the position and posture result of the target robot; wherein the target end position is determined according to the path point and the target angle value included in the preset motion instruction.

[0047] It can be seen that since the present application introduces visual sensors for QR code positioning based on the use of wheel speed meters and IMU to estimate the robot's posture, the cumulative error of inertial navigation between two QR code tag sites is eliminated, and the shortcomings of the existing positioning and navigation with low accuracy are improved. Therefore, under the globally consistent multi-sensor fusion processing, the advantages of each sensor can be fully utilized to make up for the shortcomings of a single sensor, thereby achieving more accurate and reliable real-time positioning and navigation control, and thus improving the motion control effect on the target robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0049] Figure 1 A schematic diagram of a flow chart of a robot positioning and control method provided in an embodiment of the present application;

[0050] Figure 2 A schematic diagram of pre-setting N two-dimensional code labels at preset positions on the motion path of the target robot provided in an embodiment of the present application;

[0051] Figure 3 A schematic diagram showing that each QR code label provided in the embodiment of the present application is arranged in the form of a 3x3 nine-square grid QR code group;

[0052] Figure 4 A schematic diagram of the process of generating a map file of the QR code posture information provided in an embodiment of the present application;

[0053] Figure 5 A schematic diagram of a visual positioning process provided in an embodiment of the present application;

[0054] Figure 6 A schematic diagram of a process for determining a position and posture result of a target robot provided in an embodiment of the present application;

[0055] Figure 7 A schematic diagram of a process for controlling a target robot to move to a target end position according to a preset motion instruction and a position result of the target robot provided in an embodiment of the present application;

[0056] Figure 8 A schematic diagram of the composition of a robot positioning and control device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0057] With the rapid development of robotics, sensor technology and modern control technology, the types of robots are becoming more and more diverse, and they are gradually moving from traditional industrial fields into people's daily lives, with more and more applications in industries, services, warehousing and homes. Autonomous navigation and control of the robot chassis is the key technology to achieve autonomous mobility of robots.

[0058] At present, the methods for achieving robot positioning usually include absolute positioning technology and relative positioning technology. Among them, absolute positioning technology relies on external information to provide absolute position and posture information, such as vision-based positioning methods, but this method has problems such as long-term drift, high computational cost and inaccurate scale estimation. Relative positioning technology uses its own sensors (such as IMU or wheel speed meter, etc.) combined with kinematic or dynamic models to obtain the vehicle's position and posture increment information, thereby achieving robot positioning. Although relative positioning technology is not affected by factors such as uneven external lighting, uneven texture distribution, and the presence of dynamic targets compared to absolute positioning technologies such as visual sensors, the collected data is only related to the information of the body itself, but due to the existence of integral errors, its positioning error will continue to accumulate over time, affecting the positioning and navigation accuracy of the chassis.

[0059] Specifically, the robot's navigation and positioning technology mainly includes the following methods:

[0060] (1) Global Navigation Satellite System (GNSS)

[0061] Among them, GNSS technology refers to the measurement technology of obtaining absolute positioning coordinates in the coordinate system by observing GNSS satellites. The GNSS system can be divided into three parts: satellites, ground observation stations, and user receivers. There are precision atomic clocks on the satellites, which can synchronize the time of all satellites with an accuracy of nanoseconds. The ground observation station conducts long-term and precise observations of the satellite position and motion state to correct the satellite's motion orbit information. The user receiver is used to receive satellite or base station signals for analysis. There are two main modes of GNSS positioning. The first is the single point positioning mode (Single Point Position). This positioning method has an error of less than 5m in open areas, but the error may reach more than 50m when there is multipath interference. The second is differential global navigation satellite system positioning, which includes three categories, namely position differential positioning, pseudorange differential positioning, and carrier phase differential positioning. The differential positioning method can achieve a positioning accuracy of less than 1m in open areas, but it is easy to cause an error of more than 50m when there is multipath interference. GNSS is now often combined with IMU to achieve inertial navigation, which can improve the robustness of GNSS navigation.

[0062] (2) Magnetic navigation

[0063] Magnetic navigation includes electromagnetic guidance, magnetic tape guidance, magnetic nail guidance, etc., and optical navigation includes color ribbon guidance, laser navigation, etc. Magnetic navigation is a technology that lays magnetic materials on the robot's path as a guide. Magnetic materials include buried magnetic strips, buried magnetic nails, buried wires, and ground-attached magnetic strips. The magnetic navigation sensor continuously senses magnetic signals, allowing the vehicle body to remain on the magnetic track for navigation, and identifies the current position by reading pre-buried road signs. The biggest advantage of magnetic navigation is that it is stable and reliable, and is not affected by natural conditions such as weather. It is equally effective even if the road surface is buried by sand or heavy snow.

[0064] (3) LiDAR Navigation

[0065] The robot chassis uses laser sensors to scan the surrounding environment and determine its own position and path through reflected signals. The laser navigation system builds maps with high accuracy, mature technology, intuitive mapping, no cumulative errors, and can be used for path planning. However, in narrow sections such as corridors and aisles, the robot chassis with laser navigation may deviate from the set path when encountering large moving objects. In addition, the monitoring range of the laser sensor is limited, which affects the navigation accuracy.

[0066] (4) QR code navigation

[0067] The service robot industry uses custom reflective codes attached to the ceiling. The robot uses the fisheye camera, infrared transmitter and infrared filter on the top to detect the QR code information on the ceiling in real time to achieve navigation. In industrial scenarios, QR code navigation is to scan the QR code placed on the ground, analyze the QR code information to obtain the current location information, determine the robot's position in the map, and send navigation path instructions. According to the received path instructions, the robot drives to each QR code image label in the navigation path instruction sequence in turn.

[0068] (5) Simultaneous Localization And Mapping (SLAM) navigation technology

[0069] Specifically, the robot locates its own position and posture by repeatedly observing map features (such as corners, pillars, etc.) during the movement, and then incrementally constructs the map based on its own position, thereby achieving simultaneous positioning and map construction. The core steps of SLAM technology include three processes: perception, positioning, and mapping. Perception means that the robot can obtain information about the surrounding environment through sensors; positioning means that the robot obtains current and historical information through sensors to infer its own position and posture; mapping is to depict the appearance of the environment in which it is located based on its own posture and information obtained by sensors.

[0070] Perception is a necessary condition for SLAM. Only by perceiving the information of the surrounding environment can we reliably locate and build a map of the environment. Positioning and mapping are interdependent processes: positioning depends on known map information, and mapping depends on reliable positioning. The SLAM problem can basically be divided into two parts: the front end and the back end. The front end mainly processes the data obtained by the sensor and converts it into relative posture or other forms that can be understood by other robots; the back end mainly calculates and analyzes its own posture and synchronously builds maps.

[0071] Although SLAM technology can complete the robot's autonomous positioning, effectively track and operate the target, plan the path, and automatically avoid obstacles in a completely unknown indoor environment without relying on auxiliary navigation signs, it has autonomous navigation with higher flexibility, higher precision, and stronger adaptability. However, the navigation and positioning accuracy of the SLAM system will be affected by factors such as the accuracy of the sensor itself, which may cause the construction of the map to be inaccurate. It is necessary to process the environmental information, or use multiple sensors to obtain information, and fuse and analyze the data to eliminate noise and improve navigation accuracy. This will result in a large amount of calculation, high real-time requirements, and high requirements for sensor accuracy and algorithm stability.

[0072] (6) Visual Simultaneous Localization and Mapping (VSLAM) navigation technology

[0073] Specifically, it combines vision and SLAM technology to perform positioning and map construction through image recognition and feature point matching. It has high accuracy and good real-time performance, but is greatly affected by lighting and scene changes.

[0074] For the navigation and positioning methods of the above-mentioned robots, GNSS signals are easily interfered and blocked in urban environments, and the number of visible satellites, positioning accuracy and availability of a single GPS system will decrease. The magnetic navigation system has poor flexibility in path setting, and the implementation process is cumbersome. Changing the operating line requires re-burying the navigation equipment. At the same time, the magnetic strip is easy to break. Since the magnetic strip is laid on the ground, it will have a certain impact on the overall aesthetics. In addition, the magnetic strip will attract metal substances, which may cause robot equipment failure, and other sensors are required to realize the positioning site function; there are obvious disadvantages based on the QR code navigation posted on the ground: that is, if the QR code distribution is not dense enough, the error will still accumulate; QR code positioning depends on the clarity of the QR code and the ambient light conditions, and the QR code is easy to be contaminated and requires regular maintenance. If the site is complex, the QR code needs to be replaced more frequently. At the same time, the positioning accuracy is also strongly related to the accuracy of the QR code pasting position. In the case of insufficient light or the QR code is blocked, the reliability of visual positioning will be greatly reduced. The cost of laser navigation is relatively high and the signal is easily lost in the closed place of the shelf.

[0075] Therefore, the existing methods for realizing robot navigation and positioning all have the problem of inaccurate positioning results, which in turn affects the motion control effect of the robot.

[0076] To solve the above-mentioned defects, the present application provides a robot positioning and control method, which first uses the IMU, wheel speed meter and visual sensor pre-installed on the target robot to synchronously acquire the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot respectively, and then uses the extended state Kalman filter (ESKF) to fuse the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot, and determines the position and posture result of the target robot according to the processing result; then, receives the preset motion instruction, and controls the target robot to move to the target end position according to the preset motion instruction and the position and posture result of the target robot; wherein the target end position is determined according to the path point and target angle value included in the preset motion instruction.

[0077] It can be seen that since the present application introduces visual sensors for QR code positioning based on the use of wheel speed meters and IMU to estimate the robot's posture, the cumulative error of inertial navigation between two QR code tag sites is eliminated, and the shortcomings of the existing positioning and navigation with low accuracy are improved. Therefore, under the globally consistent multi-sensor fusion processing, the advantages of each sensor can be fully utilized to make up for the shortcomings of a single sensor, thereby achieving more accurate and reliable real-time positioning and navigation control, and thus improving the motion control effect on the target robot.

[0078] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0079] First embodiment

[0080] See also Figure 1 , is a flow chart of a robot positioning and control method provided in this embodiment, the method comprising the following steps:

[0081] S101: using the inertial measurement unit IMU, wheel speed meter and visual sensor pre-installed on the target robot, synchronously acquiring the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot respectively.

[0082] In this embodiment, any robot that needs to be positioned and motion controlled is defined as a target robot. It should be noted that this embodiment does not limit the specific composition structure of the target robot.

[0083] Furthermore, in order to achieve more accurate and reliable real-time positioning and navigation control of the target robot, and thus improve the motion control effect of the target robot, an inertial measurement unit (IMU), a wheel speed meter and a visual sensor can be pre-installed on the target robot. The specific structure and installation position are not limited. For example, a camera can be fixed at the bottom of the chassis of the target robot as a visual sensor with the field of view facing vertically downward. The inertial measurement unit (IMU) and the wheel speed meter can also be fixedly installed on the chassis of the target robot.

[0084] In addition, the present application will pre-set N (the specific value is not limited, and the value can be any positive integer greater than 0) two-dimensional code labels at a preset position (the specific position is not limited, and can be set according to actual conditions and experience) on the motion path of the target robot; wherein the N two-dimensional code labels are set at equal intervals, and the intervals are adjustable, such as Figure 2 In addition, the size of each of the N QR code labels is consistent, and the coordinate system is consistent. In order to avoid using only a single label for each QR code, each QR code label can also be arranged in the form of a 3x3 nine-square grid QR code group, such as Figure 3 As shown, accurate and reliable real-time positioning and navigation control of the target robot can be achieved under the globally consistent multi-sensor fusion framework.

[0085] On this basis, the IMU, tachometer and visual sensor (such as camera) pre-installed on the target robot are used to synchronously obtain the IMU measurement data, tachometer measurement data and QR code positioning data of the target robot, that is, to ensure that the timestamps of the IMU and tachometer data are synchronized with the timestamps of the camera pose data, so as to execute the subsequent step S102.

[0086] The specific method of determining the QR code positioning data is as follows: based on the QR code of any ID in the space, a distribution map of all QR codes randomly distributed in the entire space is constructed to specify the QR code as the reference coordinate system, and a map file is generated. The distribution of the QR code is reflected in the three-dimensional coordinates of the four corner points of each QR code relative to the specified QR code coordinate system, that is, the position distribution of all QR codes in the reference world coordinate system is obtained.

[0087] Specifically, the AprilTag visual benchmark library can be used. First, when detecting the AprilTag code, if it is detected, the homography matrix corresponding to the four corner points of the AprilTag code on the image will be calculated. This homography matrix maps these points to the standard (-1,1), (1,1), (1,-1), (-1,-1) vertices; by giving the camera's intrinsic parameter K, the homography matrix can be used to estimate the camera's position relative to the AprilTag code. The main steps are as follows:

[0088] (1) Adaptive threshold segmentation;

[0089] (2) Find the contour and use Union-find to find the connected domain;

[0090] (3) Perform straight line fitting on the contour to find candidate convex quadrilaterals;

[0091] (4) Decode the quadrilateral and identify the tag;

[0092] (5) Coordinate transformation: convert to the world coordinate system.

[0093] In this way, after determining the position of the origin QR code, in order to facilitate rapid deployment, the position information of the QR code at the center of the nine-square grid at the four corner points on the rectangular map relative to the origin is determined in the QR code distribution map configuration file.

[0094] Since the nine-square grid layout is selected, it is only necessary to determine the position information of the QR code at the center of the nine-square grid. According to the distance between the QR code groups and the distance between adjacent QR codes in the nine-square grid (expressed as tag_d), a global map generation module is written. According to the QR code distribution map configuration file, a map file containing the position information of all QR codes is generated. The specific process is as follows: Figure 4 shown.

[0095] like Figure 4 As shown, the QR codes at the center of each nine-square grid at the four corners of the rectangular map (marked by boxes) are corner_center_tag1, corner_center_tag2, corner_center_tag3, and corner_center_tag4, and the position and posture information of these four small QR codes based on the origin QR code are measured and written into the configuration file as part of the input of the map generation module. After geometric transformation calculation, the map generation module can generate a map file marker map, which contains the posture information of a single small QR code (the four small QR codes all_corner_tag at the corners of the rectangle originally input, and the small QR codes all_marker_tag at all non-corner points).

[0096] When the camera acquires an image containing QR code information, such as Figure 5 As shown in the figure, combined with the deployed QR code printing size marker size, the pose information of the QR code in the map file marker map is queried, and the detection information of the QR code can be obtained by solving the homography matrix, including: (1) the QR code number marker ID; (2) the pixel coordinates of the QR code corner points; (3) the rotation vector Rvec and translation vector Tvec of the QR code based on the camera coordinate system, that is, the pose; combined with the pose of the QR code in the camera coordinate system, the pose of the camera in the global coordinate system can be converted to complete the visual positioning.

[0097] S102: Using an extended state Kalman filter (ESKF), the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot are fused and processed, and the position and posture result of the target robot is determined according to the processing result.

[0098] In this embodiment, after the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot are synchronously acquired through step S101, in order to achieve more accurate and reliable real-time positioning and navigation control of the target robot, and thus improve the motion control effect of the target robot, the extended state Kalman filter (ESKF) can be further used to perform data fusion processing and state estimation on the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot, and according to the processing result, the position and posture result of the target robot is determined to achieve high-precision positioning of the target robot, so as to execute the subsequent step S103.

[0099] Specifically, an optional implementation method is that the implementation process of step S102 may include steps S1021-S1024:

[0100] S1021: Initialize the pose state, covariance matrix and bias parameters according to the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot, and determine the initial pose state of the target robot using the initialized pose state, covariance matrix and bias parameters.

[0101] In this implementation, in order to achieve high-precision positioning of the target robot, it is first necessary to initialize parameters such as the pose state (including position and attitude, etc.), covariance matrix (used to describe the uncertainty of the initial state), bias (for example, zero bias of gyroscope and accelerometer) according to the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot, and then use the initialized pose state, covariance matrix and bias parameters to determine the initial pose state of the target robot to execute the subsequent step S1022.

[0102] The posture state may include but is not limited to position p, attitude q (rotation matrix), velocity v, acceleration, etc.; the bias may include but is not limited to the IMU gyroscope bias b g and the accelerometer bias b v , after processing and format conversion, these data can be initialized into a large diagonal matrix x, as shown in the following formula, which is used to indicate that the uncertainty of the initial state is high.

[0103]

[0104] S1022: Perform state prediction based on the IMU measurement data and wheel speed meter measurement data of the target robot, and calculate the state transfer matrix and process noise covariance matrix.

[0105] Among them, the state transfer matrix (defined as F) describes the change of the system state within a time step. It shows how the current state is converted to the next state. For example, when fusing IMU data, the state transfer matrix can describe how acceleration and angular velocity affect the change of position and attitude. The process noise covariance matrix (defined as Q) describes the uncertainty in the system process model, which helps to describe the random fluctuations in the system and the impact of imperfect models.

[0106] In this implementation, when performing state prediction, the state transition equation can be used to describe the transition from the current state x k Transition to next state x k+1 , the specific calculation formula is as follows:

[0107] x k+1 =f(x k ,u k )+w k

[0108] Among them, f represents the state transfer function, which describes the evolution of the state; u k represents the control input; w k represents process noise, which is usually assumed to have a mean of zero and a covariance of Q k .

[0109] Then, the IMU measurement data and wheel speed meter measurement data can be processed to calculate the angular velocity and linear velocity, minus the corresponding bias. The specific calculation formula is as follows:

[0110] ω=ω-b g

[0111] v=vb v

[0112] Next, the state transfer matrix F and the process noise covariance matrix Q are calculated. The state transfer matrix F is used to describe how the state changes within a time step. The specific calculation formula is as follows:

[0113]

[0114] Among them, I 33 represents the 3x3 unit matrix; R represents the rotation matrix of the current posture; (vb v )× is the antisymmetric matrix of the linear velocity vector, which is used to represent the cross product operation; Δt represents the time step, which represents the time difference between two moments; exp(-(ω-bg)Δt) is the exponential mapping of the rotation matrix, which represents the update of the angular velocity; b v represents the bias of the accelerometer; b g Indicates the bias of the gyroscope; 0 33 Represents a 3x3 zero matrix.

[0115] On this basis, the calculation formula of the process noise covariance matrix Q is as follows:

[0116]

[0117] Among them, σ v represents the standard deviation of the accelerometer noise; σ g represents the standard deviation of gyroscope noise; σ bv represents the standard deviation of the accelerometer bias noise; σ bg Represents the standard deviation of the gyroscope bias noise.

[0118] The calculation formula for further updating the state covariance matrix is ​​as follows:

[0119]

[0120] S1023: Update the state estimation according to the QR code positioning data, and calculate the observation matrix and the observation noise covariance matrix.

[0121] Among them, the observation matrix (defined as H) describes how the system state is mapped to the measurement space. It converts the state vector into a measurement vector so that the measurement value can reflect the current system state. The observation matrix is ​​used in the observation update step to convert the predicted state into a measurement value for comparison with the actual measurement and update the state estimate. The observation noise covariance matrix (defined as R) measures uncertainty in the process. It should be noted that the state transfer matrix and process noise covariance matrix mentioned in the above step S1022 are used in the state prediction step to help predict the future state of the system and its uncertainty. The observation matrix and the observation noise covariance matrix are used in the observation update step to convert the predicted state into a measurement value and update the state estimate in combination with the actual measurement data.

[0122] In this implementation, when performing observation update, the observation matrix H is usually the Jacobian matrix of the state vector to the observation value, and the specific calculation formula is as follows:

[0123]

[0124] Among them, h(x) is the observation equation, which represents the mapping from the state vector x to the observation value z.

[0125] The observation noise covariance matrix R is usually defined as follows:

[0126]

[0127] Among them, σ p represents the position noise standard deviation; σ y Represents the standard deviation of yaw angle noise.

[0128] If the camera's yaw angle is used, the observation matrix and the noise covariance matrix contain both position and rotation parts. Otherwise, they contain only the position part.

[0129] S1024: Using the extended state Kalman filter ESKF, the Kalman gain is calculated according to the state transfer matrix, the process noise covariance matrix, the observation matrix and the observation noise covariance matrix; and the Kalman gain is used to update the initial posture state of the target robot to obtain a posture estimation result as the posture result of the target robot.

[0130] Among them, the Kalman gain K4 is used to balance the weight between the predicted state and the observed value. The specific calculation formula is as follows:

[0131]

[0132] On this basis, the Kalman gain K and the bias value y can be used k Correct the state vector x, the specific calculation formula is as follows:

[0133] x k+1 =x k +K k y k

[0134] Furthermore, the state covariance matrix can be updated to reflect the new uncertainty. The specific calculation formula is as follows:

[0135] P k+1 =(IK k H k ) k

[0136] On this basis, an optional implementation method is to use a pose inference device to infer the current pose result of the target robot based on the velocity data and pose estimation results. That is, the high-precision state estimation provided by the above ESKF, including position, attitude, velocity and other information, can be passed to the pose extrapolator for further processing and extrapolation, such as Figure 6 shown.

[0137] Furthermore, an optional implementation method is to use a posture smoother to smooth the current posture result of the target robot calculated by the posture inference device to reduce noise and mutation, provide a smoother path, and obtain a processed and better posture result of the target robot (i.e., the smoothed posture), such as Figure 6 shown.

[0138] S103: receiving a preset motion instruction, and controlling the target robot to move to a target end position according to the preset motion instruction and the position and posture result of the target robot; wherein the target end position is determined according to the path point and target angle value included in the preset motion instruction.

[0139] In this embodiment, after the position and posture results of the target robot are determined by step S102, a preset motion instruction can be further received, and a linear velocity and angular velocity instruction can be determined according to the path points and target angle values ​​included in the preset motion instruction. Then, a moving route can be planned according to the linear velocity and angular velocity instructions and the position and posture results of the target robot to avoid obstacles in the route, and then the target robot can be controlled to move to the target terminal position according to the moving route, thereby improving the motion control effect of the target robot.

[0140] Specifically, the preset motion instructions can be the path points and target angle values ​​requested by the application layer, and then the target robot is controlled to move by returning the linear velocity and angular velocity instructions. The specific control process is as follows: Figure 7As shown: First, when the target robot is located at the starting point, the positioning method mentioned in the above steps S101-S102 is used, and the QR code positioning data, IMU measurement data, and wheel speed meter measurement data are used to initialize the posture to obtain positioning information. Then, after receiving the preset motion instruction (including a series of path points and target points), a single path point can be taken out in turn as a waypoint. If the point is a corner point on the rectangular map, it is necessary to slow down to stop and turn when reaching the point; if it is not a corner point, there is no need to slow down, just go straight through. When the target robot passes each tag, the positioning data is obtained by the fusion of the QR code positioning data, IMU measurement data, and wheel speed meter measurement data; when the target robot travels between two QR code tags, the positioning data can be obtained by the fusion of IMU measurement data and wheel speed meter measurement data, that is, a state update is performed each time a QR code tag is passed.

[0141] In this way, by executing the robot positioning and control method of the above steps S101-S103, on the first hand, by combining IMU, wheel speed meter and QR code tag for positioning, the advantages of each sensor can be fully utilized to make up for the shortcomings of a single sensor, thereby improving the robustness of the positioning algorithm; on the second hand, by introducing QR code tag positioning on the basis of inertial navigation, the accumulated error of inertial navigation between two QR code tag sites is regularly eliminated, which provides stronger and more stable constraints for nonlinear optimization and improves positioning accuracy; on the third hand, the adaptability of the target robot to complex working environments is improved, ensuring that it can maintain a high positioning accuracy in the case of insufficient light or the QR code is blocked, while controlling the cost of the system and being suitable for a variety of application scenarios; on the fourth hand, a general strategy for motion control under arbitrary rectangular paths is proposed, and the overall maintenance and deployment costs are also low, which facilitates rapid deployment and application.

[0142] In summary, the robot positioning and control method provided in this embodiment first uses the IMU, wheel speed meter and visual sensor pre-installed on the target robot to synchronously obtain the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot, and then uses the extended state Kalman filter (ESKF) to fuse the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot, and determines the position and posture result of the target robot according to the processing result; then, receives the preset motion instruction, and controls the target robot to move to the target end position according to the preset motion instruction and the position and posture result of the target robot; wherein the target end position is determined according to the path point and target angle value included in the preset motion instruction.

[0143] It can be seen that since the present application introduces visual sensors for QR code positioning based on the use of wheel speed meters and IMU to estimate the robot's posture, the cumulative error of inertial navigation between two QR code tag sites is eliminated, and the shortcomings of the existing positioning and navigation with low accuracy are improved. Therefore, under the globally consistent multi-sensor fusion processing, the advantages of each sensor can be fully utilized to make up for the shortcomings of a single sensor, thereby achieving more accurate and reliable real-time positioning and navigation control, and thus improving the motion control effect on the target robot.

[0144] Second embodiment

[0145] This embodiment will introduce a robot positioning and control device. For related content, please refer to the above method embodiment.

[0146] See also Figure 8 , is a schematic diagram of the composition of a robot positioning and control device provided in this embodiment, the device 800 includes:

[0147] The acquisition unit 801 is used to synchronously acquire the IMU measurement data, the wheel speed meter measurement data and the QR code positioning data of the target robot respectively by using the inertial measurement unit IMU, the wheel speed meter and the visual sensor pre-installed on the target robot;

[0148] The fusion unit 802 is used to use an extended state Kalman filter ESKF to fuse the IMU measurement data, the wheel speed meter measurement data and the QR code positioning data of the target robot, and determine the position and posture result of the target robot according to the processing result;

[0149] The control unit 803 is used to receive a preset motion instruction and control the target robot to move to a target end position according to the preset motion instruction and the position and posture result of the target robot; wherein the target end position is determined according to the path point and target angle value included in the preset motion instruction.

[0150] In an implementation of this embodiment, the fusion unit 802 includes:

[0151] A determination subunit is used to initialize the pose state, covariance matrix and bias parameters according to the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot, and determine the initial pose state of the target robot using the initialized pose state, covariance matrix and bias parameters;

[0152] A first computing subunit is used to perform state prediction based on the IMU measurement data and the wheel speed meter measurement data of the target robot, and calculate a state transfer matrix and a process noise covariance matrix;

[0153] A second computing subunit, used for updating the state estimation according to the two-dimensional code positioning data, and calculating the observation matrix and the observation noise covariance matrix;

[0154] The updating subunit is used to calculate the Kalman gain by using the extended state Kalman filter ESKF according to the state transfer matrix, the process noise covariance matrix, the observation matrix and the observation noise covariance matrix; and to update the initial posture state of the target robot by using the Kalman gain to obtain a posture estimation result as the posture result of the target robot.

[0155] In an implementation of this embodiment, the fusion unit 802 further includes:

[0156] The calculation subunit is used to use the posture inference device to calculate the current posture result of the target robot according to the speed data and the posture estimation result.

[0157] In an implementation of this embodiment, the fusion unit 802 further includes:

[0158] The smoothing subunit is used to use a posture smoother to smooth the current posture result of the target robot calculated by the posture inference device to obtain the processed posture result of the target robot.

[0159] In an implementation of this embodiment, the control unit 803 is specifically configured to:

[0160] According to the path points and target angle values ​​included in the preset motion instruction, the linear velocity and angular velocity instructions are determined, and according to the linear velocity and angular velocity instructions and the position and posture results of the target robot, the target robot is controlled to move to the target end position.

[0161] In an implementation of this embodiment, the device further includes:

[0162] The setting unit is used to pre-set N two-dimensional code labels at preset positions on the motion path of the target robot; N is a positive integer greater than 0.

[0163] In an implementation of this embodiment, the N two-dimensional code tags are arranged at equal intervals; each of the N two-dimensional code tags is arranged in the form of a 3x3 nine-square grid two-dimensional code group.

[0164] Furthermore, the embodiment of the present application also provides a robot positioning and control device, including: a processor, a memory, and a system bus;

[0165] The processor and the memory are connected via the system bus;

[0166] The memory is used to store one or more programs, and the one or more programs include instructions. When the instructions are executed by the processor, the processor executes any one of the implementation methods of the above-mentioned robot positioning and control method.

[0167] Furthermore, an embodiment of the present application also provides a computer-readable storage medium, in which instructions are stored. When the instructions are executed on a terminal device, the terminal device executes any one of the implementation methods of the above-mentioned robot positioning and control method.

[0168] Furthermore, an embodiment of the present application also provides a computer program product, which, when executed on a terminal device, enables the terminal device to execute any one of the implementation methods of the above-mentioned robot positioning and control method.

[0169] It can be known from the description of the above implementation mode that those skilled in the art can clearly understand that all or part of the steps in the above-mentioned embodiment method can be implemented by means of software plus a necessary general hardware platform. Based on such an understanding, the technical solution of the present application can be essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product can be stored in a storage medium such as ROM / RAM, a disk, an optical disk, etc., including several instructions for enabling a computer device (which can be a personal computer, a server, or a network communication device such as a media gateway, etc.) to execute the methods described in the various embodiments of the present application or certain parts of the embodiments.

[0170] It should be noted that the various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments, and the same or similar parts between the various embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part description.

[0171] It should also be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the presence of other identical elements in the process, method, article or device including the elements.

[0172] The above description of the disclosed embodiments enables those skilled in the art to implement or use the present application. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present application. Therefore, the present application will not be limited to the embodiments shown herein, but will conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A robot positioning and control method, characterized in that: include: Using the inertial measurement unit IMU, wheel speed meter and visual sensor pre-installed on the target robot, the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot are synchronously acquired respectively; Using an extended state Kalman filter (ESKF), the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot are fused and processed, and the position and posture result of the target robot is determined according to the processing result; Receive a preset motion instruction, and control the target robot to move to a target end position according to the preset motion instruction and the position result of the target robot; wherein the target end position is determined according to the path point and target angle value included in the preset motion instruction.

2. The method according to claim 1, characterized in that: The extended state Kalman filter ESKF is used to fuse the IMU measurement data, the wheel speed meter measurement data and the two-dimensional code positioning data of the target robot, and the position and posture result of the target robot is determined according to the processing result, including: Initialize the pose state, covariance matrix and bias parameters according to the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot, and determine the initial pose state of the target robot using the initialized pose state, covariance matrix and bias parameters; Perform state prediction based on the IMU measurement data and wheel speed meter measurement data of the target robot, and calculate the state transfer matrix and process noise covariance matrix; Update the state estimate according to the two-dimensional code positioning data, and calculate the observation matrix and the observation noise covariance matrix; The extended state Kalman filter (ESKF) is used to calculate the Kalman gain according to the state transfer matrix, the process noise covariance matrix, the observation matrix and the observation noise covariance matrix; and the Kalman gain is used to update the initial posture state of the target robot to obtain a posture estimation result as the posture result of the target robot.

3. The method according to claim 2, characterized in that After the initial posture state of the target robot is updated by using the Kalman gain to obtain a posture estimation result, the method further includes: The pose inference device is used to infer the current pose result of the target robot according to the speed data and the pose estimation result.

4. The method according to claim 3, characterized in that: After the pose inference device is used to infer the current pose result of the target robot according to the speed data and the pose estimation result, the method further comprises: The current posture result of the target robot calculated by the posture estimator is smoothed by using a posture smoother to obtain the processed posture result of the target robot.

5. The method according to claim 1, characterized in that: The step of controlling the target robot to move to a target end position according to the preset motion instruction and the position result of the target robot comprises: According to the path points and target angle values ​​included in the preset motion instruction, the linear velocity and angular velocity instructions are determined, and according to the linear velocity and angular velocity instructions and the position and posture results of the target robot, the target robot is controlled to move to the target end position.

6. The method according to any one of claims 1 to 5, characterized in that: Before using the inertial measurement unit IMU, wheel speed meter and visual sensor pre-installed on the target robot to synchronously acquire the IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot respectively, the method further includes: At preset positions on the motion path of the target robot, N two-dimensional code labels are pre-set; N is a positive integer greater than 0.

7. The method according to claim 6, characterized in that The N two-dimensional code labels are arranged at equal intervals; each of the N two-dimensional code labels is arranged in the form of a 3x3 nine-square grid two-dimensional code group.

8. A robot positioning and control device, characterized in that: include: An acquisition unit is used to synchronously acquire IMU measurement data, wheel speed meter measurement data and QR code positioning data of the target robot by using an inertial measurement unit IMU, a wheel speed meter and a visual sensor pre-installed on the target robot; A fusion unit is used to use an extended state Kalman filter (ESKF) to fuse the IMU measurement data, the wheel speed meter measurement data and the QR code positioning data of the target robot, and determine the position and posture result of the target robot according to the processing result; A control unit is used to receive a preset motion instruction and control the target robot to move to a target end position according to the preset motion instruction and the position and posture result of the target robot; wherein the target end position is determined according to the path point and target angle value included in the preset motion instruction.

9. A robot positioning and control device, characterized in that: include: Processor, memory, system bus; The processor and the memory are connected via the system bus; The memory is used to store one or more programs, wherein the one or more programs include instructions, and when the instructions are executed by the processor, the processor executes the method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores instructions, and when the instructions are executed on a terminal device, the terminal device executes the method according to any one of claims 1 to 7.

Images