Disaster relief material transportation multi-robot cooperation system

Abstract

The present application relates to a kind of disaster relief material transport multi-robot cooperation system, belong to robot technical field.The cooperation system includes power supply, servo motor, track, digital signal processing DSP, wireless communication module, Gauss frequency shift keying GFSK wireless data radio, laser radar, inertial measurement unit IMU, GPS and encoder.The present application adopts distributed control mode in multi-material transport cooperation robot system, so that the autonomy, flexibility, reliability and response speed of each robot in system are improved.The present application designs more suitable for disaster relief scene multi-transport robot cooperation system, designs by the track drive suitable for rugged road, sensor fusion positioning for the environment interference after disaster and multi-transport robot cooperation control under the urgency requirement of disaster relief time.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of robot technology, and relates to a multi-robot cooperation system for disaster relief supplies transportation. BACKGROUND

[0002] The supplies robot not only involves simple fusion of mechanical structure and control algorithm, but also involves multi-sensor fusion technology, information processing technology, automatic control technology, computer technology, multi-system control and many other technologies, and is a comprehensive interdisciplinary subject. The general structure of the robot system includes three parts of mechanical structure, perception system and control system; and six subsystems of driving system, mechanical structure system, sensing system, robot-environment interaction system, human-computer interaction system and control system.

[0003] In recent years, with the improvement of intelligent level and the development of robot technology, robots for rescue have also been gradually developed. The common rescue robots at present mainly use wheel driving, adopt laser radar or GPS positioning, use single ultrasonic obstacle avoidance, and are centrally controlled, that is, through a master control program, the distribution task allocation and work scheduling of all robots are completed; the specific mode of transportation is that the map constructed by using laser radar and the planned transportation route are divided into regions and then respectively imported into the interiors of the robots, and each robot will automatically transport supplies to the designated destination according to the planned route obtained by the robot after starting.

[0004] However, this system also has many defects. First, there are many obstacles and uneven road surfaces in the disaster area, and the wheel driving makes the robot have poor passing performance when coping with steps and rugged road surfaces, and the flexibility of turning and rotating is not high, and it is not easy to pass through some road sections; due to the fact that measurement noise is more likely to occur in disaster areas, the working mode of using a single sensor cannot well perform positioning, and often causes the problem of serious cumulative error; after detecting obstacles, only simple static obstacle avoidance function can be realized, and the position of the obstacle and the obstacle condition image obtained by the camera are transmitted to the control room, and obstacle avoidance is realized under the remote control of the relevant staff, but overall, the flexibility and initiative in obstacle avoidance are lacking; finally, in the multi-robot cooperation control, the centralized control mode makes each individual robot have no ability to select actions and coordinate with each other, so that the inspection efficiency, robustness and scalability of the whole system are poor, and the time optimality and energy optimality of each robot in the transportation process cannot be guaranteed, which reduces the overall endurance and anti-disturbance ability to the outside world, and the autonomous control and intelligent level still need to be improved; in addition, some existing distributed robot control is often updated in time, which is easy to cause waste of communication resources and computing resources and slow robot travel.

[0005] The prior art has less research on rescue robots, and the system integration degree of research results is not high, the control is very unstable, the sensitivity and accuracy are not enough, the real-time of material distribution cannot be guaranteed, and the demand for materials in the disaster area cannot be met in time; The existing robots are still in the semi-intelligent state, the intelligence is not high enough, the processing capacity and initiative of the robots in the disaster area are very limited, and the anti-disturbance ability of the control system in response to the external environment is poor when multiple material transport robots are used for collaborative operation. In addition, the control system usually requires the model of the system to be known for controller design, and in practice, many model parameters cannot be accurately obtained or are unknown, which brings great challenges to the design of the controller. Therefore, the present application designs a new type of multi-material transport collaborative robot system with high system integration, strong peripheral expansion, high inspection efficiency, stable control effect and good robustness to realize the rapid development of material distribution in disaster conditions and reduce the secondary damage after the disaster. SUMMARY

[0006] Therefore, the purpose of the present application is to provide a disaster material transport multi-robot collaborative system.

[0007] To achieve the above purpose, the present application provides the following technical scheme:

[0008] A disaster material transport multi-robot collaborative system, the collaborative system comprising a power supply, a servo motor, a track, a digital signal processing DSP, a wireless communication module, a Gaussian frequency shift keying GFSK wireless data transmission radio, a laser radar, an inertial measurement unit IMU, a GPS and an encoder;

[0009] The laser radar, the inertial measurement unit IMU, the GPS and the encoder upload the collected data to the DSP;

[0010] The GFSK wireless data transmission radio transmits signals to the wireless communication module, and the wireless communication module sends the signals to the DSP after receiving the signals, and the DSP controls the power supply to supply power to the servo motor to drive the track to walk;

[0011] The IMU generates acceleration and angular velocity measurement values along multiple axes or degrees of freedom, which are combined by 3 accelerometers and 3 gyroscopes, and the accelerometers and gyroscopes are installed on mutually perpendicular measurement axes;

[0012] The GP includes a reference station and a mobile station, which are used to observe satellite data, the reference station transmits the received carrier phase signal or carrier phase difference correction signal through its transmitting radio; the mobile station receives the satellite signal and the radio signal of the reference station at the same time; the solidification software on the mobile station realizes difference calculation to determine the spatial relative position relationship between the reference station and the mobile station;

[0013] An IMU integrates multiple inertial sensors to generate measurements of acceleration and angular rate along multiple axes or degrees of freedom; typically a combination of 3 accelerometers and 3 gyroscopes mounted on mutually perpendicular axes of measurement;

[0014] When there is an obstacle in front of the robot, the laser radar detects the distance and angle between the robot and the obstacle, and sends the obstacle information to the DSP, and calculates the specific coordinates of the obstacle with the obtained odometer and gyroscope data; after obtaining the coordinates, an obstacle avoidance path is designed, which keeps a minimum distance from the obstacle, and after obstacle avoidance, it returns to the previously planned optimal inspection path;

[0015] The cooperative system uses an extended Kalman filter (EKF) model to fuse the data of the IMU and GPS, and Kalman filtering includes two stages, namely prediction and update; in the prediction stage, the filter makes an estimate of the state at this time X(k|k-1) based on the estimate of the last state X(k-1|k-1); in the update stage, the filter optimizes the predicted value X(k|k) of the prediction stage according to the actual observation value Z(k) of the current state;

[0016] The GPS data is used as the observation value Z(k) of the filter, the GPS sample data satisfies the normal distribution, and the probability distribution satisfies the chi-square distribution, and the likelihood estimate of the position error and the heading error of the GPS is calculated as l xy , l θ ,

[0017]

[0018] where p=(P gps (k)-P odo (k)), θ=(θ gps (k)-θ odo (k)), respectively representing the probability difference between GPS and IMU; ∑ odo (k) and ∑ gps are the position error covariance matrices of IMU and GPS at time k; when the calculated l xy and l θ are smaller than the set threshold l xyth and l θth , it is considered that the current data meets the requirements, and the GPS data is input as the observation value Z of the filter, and the robot position X(k|k) and error covariance matrix P(k|k) are updated;

[0019] Under the definition of EKF, the error covariance matrix R of GPS is composed of the position error covariance ∑ gps and the heading error covariance ,

[0020]

[0021] R is defined in different cases -1 :

[0022] 1. When l xy (k) < l xyth and l θ > l θth ,

[0023]

[0024] 2. When l xy (k) < l xyth and l θ < l θth ,

[0025]

[0026] 3. When l xy (k) > l xyth and l θ < l θth ,

[0027]

[0028] At this time, the position error is expected to be too large, and the heading error is not fused with the IMU data.

[0029] Optionally, the data of fusing the IMU and the GPS is specifically:

[0030] Initialize X(0), P(0), k = 1;

[0031] S11: In the prediction stage, the current state X(k|k-1) is predicted according to the previous state:

[0032] X(k|k-1) = F(X(k-1|k-1), U(k)) (6)

[0033] S12: Update the system prediction covariance matrix P(k|k-1):

[0034]

[0035]

[0036] wherein is the Jacobian matrix of F on X at k-1 time, Q(k), R(k) are system noise and observation noise, that is, GPS and IMU observation noise;

[0037] S13: Calculate the maximum likelihood estimation l of the GPS heading error and the position error according to formula (1)θ (k) and l xy (k), select the corresponding R according to the conditions of equations (3) and (4). -1 (k) Calculate the Kalman filter gain K(k):

[0038] K(k)=P(k|k-1)H(k) T [H(k)P(k|k-1)H(k) T +R(k)] -1 (9)

[0039] H(k) = diag{1, 1, 1};

[0040] S14: Calculate the optimized state X(k|k) at time k based on the filter gain, and update the error P(k|k).

[0041] X(k|k)=X(k|k-1)+K(k)(Z(k)-H(k)X(k|k-1)) (10)

[0042] P(k|k)=[EK(k)H(k)]P(k|k-1) (11)

[0043] S15: k = k + 1;

[0044] Design a distributed control law using the Euler-Lagrange model as the robot's dynamics model;

[0045] The leader model is:

[0046]

[0047] q0=Co (12)

[0048] q0∈R n This represents the position signal followed by each material transport robot, which is generated by the leader system in equation (12), where o∈R m , S∈R m×m , C∈R n×m It is a constant matrix;

[0049] Suppose there are N robots cooperating in transportation. The Lagrange model of N robots is represented as:

[0050]

[0051] in, These are the generalized position and velocity vectors, M. i (q i )∈R n×n It is a mass matrix that describes the distribution of mass across each independent coordinate axis; It is the vector of the Coriolis force and the centripetal force, G i (q i )∈R n It is the gravity vector; u i ∈R n It is a generalized force vector;

[0052] In a scenario where each transport robot can only obtain its own state information and that of its communicable neighbors, and the robot model information is uncertain, a control system is designed for each transport robot. i Such that at any initial q i (0), o(0), q i (t), o(t) all exist and are bounded, satisfying:

[0053]

[0054] When the expected event occurs, the system control policy is updated. The triggering policy is as follows:

[0055]

[0056] in κ and β are positive real numbers, and the subscript k represents time k; when the above conditions are met, each transport robot will update its controller at adjacent trigger times. The control strategies for each robot remain unchanged; the update law of the controller is as follows:

[0057]

[0058] It is the observation of the leader's internal state o by each robot, achieved by designing a state observer η for each robot. i This enables the robot to perform position tracking even when it cannot communicate with the leader; by assumption 1, S is denoted as

[0059]

[0060] Design an ω-observer for each robot:

[0061]

[0062] Construct the observation matrix S for each robot. i The leader's state is measured by the observation matrix, thus enabling tracking.

[0063] Optionally, for each of the robots, we have:

[0064] Arbitrary initialization of q i (0), o(0);

[0065] S21: Determine whether the event triggering condition is met by (15);

[0066] If the condition is not met, proceed to S24;

[0067] If the condition is met, proceed to S22;

[0068] S22: The S matrix of the observed leader is calculated by solving equation (17).

[0069] S23: Observe the leader's state o, and calculate it by solving equation (16).

[0070] S24: Controller (16) implement.

[0071] The beneficial effects of this invention are as follows:

[0072] In terms of robot drive, in order to avoid the problems of poor performance and inflexible steering of wheeled drive when facing steps or rough roads, this invention adopts tracked drive and selects a two-stage design in the track structure. The design of the front track can increase the operating radius of the robotic arm and further enhance its adaptability to complex terrain.

[0073] Regarding positioning:

[0074] Inertial navigation positioning suffers from large drift over long periods, which can accumulate a significant error in position estimation. However, it offers advantages such as high accuracy and frequency. GPS positioning, on the other hand, cannot obtain high-precision position information but does not accumulate errors. To address the problems of using GPS sensors and inertial navigation alone, a tightly coupled fusion approach is used to fuse the data from the two positioning models. Several optimization methods are employed to jointly optimize GPS and inertial navigation, thereby filtering out noise disturbances and outputting more accurate positioning information.

[0075] In terms of collaborative control of multiple material transport robots:

[0076] To enable collaborative operation of multiple transport robots, a distributed control approach is adopted, allowing each robot to solve sub-problems independently and autonomously, thereby improving the overall system's efficiency, robustness, and scalability. Furthermore, an event-driven consistency control algorithm is designed to address the uncertainty of robot models, enabling multiple transport robots to collaboratively deliver supplies with fewer computations while following the lead robot, ultimately reaching the disaster relief destination.

[0077] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0078] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings, wherein:

[0079] Figure 1 This is a system structure diagram of the present invention;

[0080] Figure 2 This is a schematic diagram of the track control principle of the present invention;

[0081] Figure 3 This is a schematic diagram of the EKF-based fusion scheme.

[0082] Figure 4 This is a schematic diagram of obstacle avoidance principle based on lidar. Detailed Implementation

[0083] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0084] The accompanying drawings are for illustrative purposes only and are schematic diagrams, not actual pictures. They should not be construed as limiting the invention. To better illustrate the embodiments of the invention, some parts in the drawings may be omitted, enlarged, or reduced, and do not represent the actual product dimensions. It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings.

[0085] In the accompanying drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components. In the description of the present invention, it should be understood that if terms such as "upper," "lower," "left," "right," "front," and "rear" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, the terms used to describe positional relationships in the drawings are only for illustrative purposes and should not be construed as limiting the present invention. For those skilled in the art, the specific meaning of the above terms can be understood according to the specific circumstances.

[0086] This invention consists of the following four parts: hardware layer design, multi-sensor fusion positioning design, obstacle avoidance system design, and multi-transport robot collaborative controller design.

[0087] Part 1: Hardware Design of the Material Transport Robot

[0088] The hardware layer uses a DSP as the controller, and the host computer sends speed commands to the DSP. The servo motor is powered by 24V. The DSP encodes and processes the wheel speed information issued by the upper layer before execution. The robot uses tracked drive, which aims to improve the transportation robot's ability to pass through complex road sections (such as gravel) and its turning flexibility during disaster relief. It is also equipped with sensors such as lidar, ultrasonic sensors, GPS receiver, and IMU.

[0089] Hardware layer and control module diagram as follows Figure 1 As shown.

[0090] 1. Track drive system

[0091] To adapt to various inspection environments and improve flexibility and maneuverability during inspections, this inspection robot adopts a tracked drive system. It features a long-stroke adjustable suspension, a balanced road wheel assembly with four small road wheels, an adaptive tensioning idler wheel adjustment structure, a lightweight tracked structure, and an intelligent suspension control system. This allows for continuous adjustment of the suspension height, giving the platform a comprehensive advantage in high mobility, high maneuverability, strong adaptability, and high reliability. Figure 2 This is a schematic diagram of the track control principle of the present invention.

[0092] 2. LiDAR

[0093] It can be used for obstacle avoidance and position recognition in high-speed motion robots; its high-precision, high-resolution, and wide-field-of-view design provides autonomous navigation robots with excellent environmental recognition capabilities; its compact design saves installation space, and it is lightweight and consumes little power. In this invention, a lidar is used to detect obstacles ahead with high sensitivity, uploads the size and distance information of the obstacles to a host computer for processing, and finally outputs a certain control signal to achieve obstacle avoidance.

[0094] 3. GPS and IMU

[0095] GPS positioning is susceptible to various factors, resulting in significant positioning errors for mobile robots. RTK technology addresses these issues by eliminating ionospheric and tropospheric delays, multipath propagation, and satellite clock and ephemeris errors (errors caused by the GPS receiver's use of satellite positions in its location calculations). A GPS base station (whose position can be pre-set to a fixed precision) transmits its carrier wave observations and station coordinates to the robot in real-time via a data link. The robot then processes the carrier wave phases from the GPS satellites it receives and those from the base station, combining them into phase difference observations for real-time analysis, providing centimeter-level positioning results.

[0096] Therefore, this invention uses higher-precision RTK for outdoor positioning, employing two GPS receivers (one base station and one rover) to observe satellite data. Simultaneously, the base station transmits the received carrier phase signal (or carrier phase differential correction signal) through its transmitting radio. Meanwhile, the rover receives the satellite signal and the base station's radio signal through its receiving radio. Based on these two signals, the software embedded in the rover can perform differential calculations, thereby accurately determining the spatial relative position between the base station and the rover.

[0097] An IMU is an electronic module that integrates multiple inertial sensors to generate acceleration and angular velocity measurements along multiple axes or degrees of freedom. It typically consists of a combination of three accelerometers and three gyroscopes, mounted on mutually perpendicular measurement axes.

[0098] Part Two: Multi-sensor Fusion Positioning Design

[0099] Localization is the first problem a mobile robot must solve to complete its navigation task; knowing its current location is a prerequisite for navigation. The accuracy of localization directly impacts the success of material transportation tasks. Mobile robot localization methods can be divided into relative localization and absolute localization.

[0100] Compared to relative positioning, absolute positioning obtains the robot's actual position. Its error is bounded and does not accumulate with movement; each positioning attempt has its own error. GPS is a type of absolute positioning. GPS uses precise time and location information transmitted by receivers and satellites for positioning, but it can only be used in outdoor environments and is easily affected by buildings and clouds. Disaster relief transport robots often operate in environments with buildings and trees nearby, resulting in varying numbers of satellites and significant variations in GPS positioning accuracy. GPS differential positioning requires simultaneous observation of at least four satellites, and both the mobile station and base station must simultaneously observe the same four satellites. When the number of satellites is insufficient, GPS differential positioning may fail or even fail altogether.

[0101] In simple terms, an IMU (Integrated Measurement Unit) can obtain the acceleration of three axes. By integrating the acceleration once and twice, the robot's velocity and displacement can be obtained in a very short time. By integrating the angular velocity, the changes in the angles of the three axes can be obtained, and thus the robot's directional changes can be inferred. However, due to the integration method, the odometry will accumulate a large error over a long period of use and may eventually fail.

[0102] For the reasons mentioned above, neither IMU nor GPS alone can fulfill the positioning requirements. The dead reckoning system based on inertial sensors in this invention has good high-frequency characteristics but poor low-frequency characteristics; while GPS has complementary characteristics. By combining the advantages of both, a combined positioning system is constructed. An extended Kalman filter (EKF) model is used to fuse the data from both systems, resulting in a significant improvement in the accuracy, performance, and reliability of the entire system compared to a single system.

[0103] Kalman filtering consists of two phases: prediction and update. In the prediction phase, the filter estimates the state at the current moment, X(k|k-1), based on the estimate of the previous state X(k-1|k-1). In the update phase, the filter optimizes the predicted value X(k|k) from the prediction phase based on the actual observed value Z(k) of the current state, aiming to obtain a more accurate state estimate. Kalman filtering is a mathematical mechanism that generates the optimal estimate of the system state based on the system state and its measurement devices; it describes system noise, measurement errors, and uncertainties in the dynamic model. However, for robotic systems, the system model is usually nonlinear. Therefore, the model is typically linearized before filtering, which is known as extended Kalman filtering.

[0104] Specifically, GPS data is used as the observation value Z(k) for the filter. Because GPS data has low accuracy and exhibits abrupt changes, it needs to be inspected to remove data that does not meet the fusion requirements. First, the GPS sample data follows a normal distribution, and its probability distribution follows a chi-square distribution. The likelihood estimates of the GPS position error and heading error can be calculated as l. xy , l θ ,

[0105]

[0106] Where p = (P gps (k)-P odo (k)), θ=(θ) gps (k)-θ odo (k)), representing the probability differences between GPS and IMU, respectively. ∑ odo (k) and ∑ gps These are the position error covariance matrices of the IMU and GPS at time k, respectively. When the calculated l xy With l θ Compared to the set threshold l xyth and l θth When the value is small, the current data can be considered to meet the requirements. In this case, the GPS data can be used as the input of the filter observation value Z to update the robot position X(k|k) and the error covariance matrix P(k|k).

[0107] Under the EKF definition, the error covariance matrix R of GPS is composed of the position error covariance ∑ gps and heading error covariance composition,

[0108]

[0109] When the GPS data error is too large, according to the meaning of the filter gain K(k) in EKF, it can be regarded as ∑ gps and heading covariance It is extremely large, and can be considered to approach infinity, therefore R needs to be defined for different cases. -1 :

[0110] 1. When l xy (k)<l xyth And l θ <l θth ,

[0111]

[0112] 2. When l xy (k)<l xyth And l θ >lθth ,

[0113]

[0114] 3. When l xy (k)>l xyth And l θ <l θth ,

[0115]

[0116] 3. When l xy (k)>l xyth And l θ >l θth At this point, both the position error and the expected heading error are too large to be fused with the IMU data.

[0117] Based on the above discussion, the fusion scheme diagram of the present invention is shown below. Figure 3 As shown.

[0118] Specifically, the steps of the EKF-based GPS and IMU fusion algorithm are as follows:

[0119] Initialize X(0), P(0), k = 1;

[0120] Step 1: Prediction phase, predict the current state X(k|k-1) based on the state at the previous time step.

[0121] X(k|k-1)=F(X(k-1|k-1),U(k)) (6)

[0122] Step 2: Update the system prediction covariance matrix P(k|k-1),

[0123]

[0124]

[0125] in Let F be the Jacobian matrix of X at time k-1, and Q(k) and R(k) be the system noise and observation noise (i.e., GPS and IMU observation noise).

[0126] Step 3: Calculate the maximum likelihood estimates of GPS heading error and position error according to equation (1). θ (k) and l xy (k), select the corresponding R according to the conditions of equation (3). -1 (k) Calculate the Kalman filter gain K(k),

[0127] K(k)=P(k|k-1)H(k) T[H(k)P(k|k-1)H(k) T +R(k)] -1 (9)

[0128] H(k) = diag{1, 1, 1};

[0129] Step 4: Calculate the optimized state X(k|k) at time k based on the filter gain, and update the error P(k|k).

[0130] X(k|k)=X(k|k-1)+K(k)(Z(k)-H(k)X(k|k-1)) (10)

[0131] P(k|k)=[EK(k)H(k)]P(k|k-1) (11)

[0132] Step 5: k = k + 1.

[0133] During the navigation process after obtaining location information, obstacle avoidance is an essential part of transportation because disasters can easily lead to encountering many obstacles during the journey.

[0134] LiDAR can detect the distance and angle between the robot and surrounding signals that can reach the target. Therefore, this invention utilizes LiDAR to detect obstacles encountered by the inspection robot during transportation in real time. When an obstacle is encountered, the LiDAR detects the distance and angle between the robot and the obstacle, sends the relevant obstacle information to the DSP, and, together with the obtained odometer and gyroscope data, calculates the specific coordinates of the obstacle. After obtaining the coordinates, the control algorithm can immediately design an obstacle avoidance path. This obstacle avoidance path is usually arc-shaped and requires maintaining a minimum distance from the obstacle throughout the process. After obstacle avoidance, the robot must immediately return to the previously planned optimal inspection path. The obstacle avoidance process is as follows: Figure 4 As shown.

[0135] Part Three: Design of a Collaborative Controller for Multi-Transport Robots

[0136] In practice, transportation tasks are typically carried out simultaneously by multiple transport robots. The materials transported by these robots may not be at the same location, and the scenarios they face may differ. Furthermore, due to distance and other factors, each robot cannot directly obtain information about the destination location and must rely on exchanging information with nearby communicable robots to complete the task. Therefore, collaborative transportation in this scenario cannot be considered a single system for task planning; it is a distributed collaborative task. In reality, the robot models are not always fully known, which adds further difficulty to designing distributed collaborative control. To address the control requirements of the aforementioned multi-material transportation collaborative robot system, a distributed control scheme with partially unknown system models and limited inputs needs to be designed. This scheme allows each robot to ultimately follow the leader (the navigator) and reach the destination based on information from its neighbors or a lead robot.

[0137] Eulerian and Lagrange models are generally applicable to mechanical systems and robot systems. This model is simple in form, has good control performance, and is more realistic than other models. Therefore, this invention uses the Eulerian and Lagrange models as the robot dynamics model and designs distributed control laws.

[0138] Consider the above leader model as follows:

[0139]

[0140] q0=Co (12)

[0141] q0∈R n This indicates the position signal that each material transport robot needs to follow, which is generated by the (12) leader system, where o∈R m , S∈R m×m , C∈R n×m It is a constant matrix.

[0142] Suppose there are N robots cooperating in transportation. The Lagrange model of N robots can be expressed as:

[0143]

[0144] in, These are the generalized position and velocity vectors, M. i (q i )∈R n×n It is the mass matrix, which describes the distribution of mass across each independent coordinate axis; It is the vector of the Coriolis force and the centripetal force, G i (q i )∈R n It is the gravity vector; u i ∈R nIt is a generalized force vector. Because the Lagrange model is built upon actual physical systems, Lagrange systems also possess favorable properties:

[0145] Property 1: It is a positive definite symmetric matrix;

[0146] Property 2: It is an antisymmetric matrix;

[0147] Property 3: For all x, y∈R n They all in Given the regression matrix Θ i ∈R p It is a constant vector that is related to system (13) but is unknown.

[0148] Systems (12) and (13) are typically referred to as a multi-agent system with N+1 agents, where (12) is the leader and the N subsystems are followers. The communication topology between the agents can be described by a directed graph G = (V, E, A). Here, it is assumed that the directed graph of communication between the N followers contains a directed spanning tree, and that the leader can send information to at least one follower called the root node.

[0149] This multi-agent system must satisfy several assumptions:

[0150] Assumption 1: The leader's dynamics matrix S is diagonalizable;

[0151] Assumption 2: The communication topology of a multi-agent system is an undirected graph.

[0152] Based on the above, the control objective of this invention is: in a scenario where each transport robot can only obtain its own and its communicable neighbor's state information, and the robot model information is uncertain, to design a control system for each transport robot. i Such that at any initial q i (0), o(0), q i (t), o(t) all exist and are bounded, satisfying:

[0153]

[0154] Therefore, each follower's position can eventually track the leader's position, meaning they can all eventually reach the disaster destination. This invention addresses the issue of uncertain transport robot models and the unavailability of the leader's dynamics matrix by designing an event-triggered distributed controller for each transport robot. Compared to traditional distributed controllers that require information exchange at all times, leading to wasted system resources, the event-triggered approach significantly reduces communication frequency. The system control strategy is only updated when the expected event occurs. The triggering strategy is as follows:

[0155]

[0156] in κ and β are positive real numbers, and the subscript k represents time k. When the above conditions are met, each transport robot will update its controller, while at adjacent trigger times... The control strategies for each robot remain unchanged; the update law of the controller is as follows:

[0157]

[0158] It is the observation of the leader's internal state o by each robot, achieved by designing a state observer η for each robot. i This allows the robot to track its location even when it cannot communicate with the leader. From Assumption 1, we know that S can be represented as...

[0159]

[0160] Therefore, as long as you know This allows us to construct matrix S and design an ω-observer for each robot:

[0161]

[0162] Based on this, the observation matrix S for each robot is constructed. i The leader's state can be measured from the observation matrix, thus enabling tracking.

[0163] Algorithm: Consistency Control Algorithm for Transport Robots with Unknown Model

[0164] For each transport robot:

[0165] Initialization: Arbitrarily initialize q i (0), o(0);

[0166] Step 1: Determine whether the event triggering condition is met by (15);

[0167] a. If the condition is not met, proceed to step 4;

[0168] b. If satisfied, proceed to step 2;

[0169] Step 2: Observe the leader's S matrix and calculate it using equation (17).

[0170] Step 3: Observe the leader's state o, and calculate using equation (16).

[0171] Step 4: Control by (16) implement.

[0172] This invention employs a distributed control method in a multi-material transportation collaborative robot system, which improves the autonomy, flexibility, reliability, and response speed of each robot in the system.

[0173] This invention designs a multi-transport robot collaborative system that is more suitable for disaster relief scenarios. It features a system with tracked drive suitable for rugged roads, sensor fusion positioning to address environmental interference after a disaster, and collaborative control of the multi-transport robots under the urgency of disaster relief.

[0174] This invention provides an obstacle avoidance scheme for transport robots, enabling the robot system to stably perform navigation, walking, and material transport in complex disaster situations with a high frequency of obstacles.

[0175] This invention fuses GPS and IMU positioning data, combining the advantages of absolute and relative positioning to eliminate their shortcomings, achieving more robust and accurate positioning results that meet the positioning requirements in actual transportation processes.

[0176] This invention uses the Lagrange dynamics model, which is commonly used in practice, to design the controller algorithm. The resulting distributed controller is closer to actual engineering and easier to deploy.

[0177] This invention uses an observer in a multi-robot control algorithm, enabling robots that cannot communicate with the lead transport robot to observe the lead system matrix and system state. It also solves the problem of uncertainty in the transport robot model, and further enables each transport robot to reach its final destination, significantly improving the walking efficiency and robustness of the multi-material transport collaborative robot system.

[0178] This invention uses an event-driven approach to update the controller during the collaborative control of a transport robot, which greatly reduces communication resources and the frequency of robot control strategy updates, thereby avoiding problems such as robot lag and unsmooth movement caused by overly rapid and unnecessary updates.

[0179] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A multi-robot collaborative system for transporting disaster relief supplies, characterized in that: The collaborative system includes a power supply, servo motors, tracks, a digital signal processing (DSP), a wireless communication module, a Gaussian frequency shift keying (GFSK) wireless data radio, a lidar, an inertial measurement unit (IMU), a GPS, and an encoder. The lidar, inertial measurement unit (IMU), GPS, and encoder upload the collected data to the DSP. The GFSK wireless data radio transmits signals to the wireless communication module. After receiving the signals, the wireless communication module sends them to the DSP. The DSP controls the power supply to power the servo motor, driving the track to move. The IMU generates acceleration and angular velocity measurements along multiple axes or degrees of freedom. It consists of three accelerometers and three gyroscopes, which are mounted on mutually perpendicular measurement axes. The GPS includes a base station and a rover station for observing satellite data. The base station transmits the received carrier phase signal or carrier phase differential correction signal through its transmitting radio. The rover station receives the radio signal from the base station while receiving the satellite signal. The fixed software on the rover station performs differential calculations to determine the spatial relative position relationship between the base station and the rover station. An IMU integrates multiple inertial sensors to generate acceleration and angular velocity measurements along multiple axes or degrees of freedom; it typically consists of a combination of three accelerometers and three gyroscopes, with the accelerometers and gyroscopes mounted on mutually perpendicular measurement axes. When there is an obstacle in front, the lidar detects the distance and angle between the robot and the obstacle, sends the obstacle information to the DSP, and calculates the specific coordinates of the obstacle with the obtained odometer and gyroscope data; after obtaining the coordinates, an obstacle avoidance path is designed, which maintains a minimum distance from the obstacle; after the obstacle avoidance is completed, the robot returns to the previously planned optimal inspection path. The collaborative system uses an extended Kalman filter (EKF) model to fuse IMU and GPS data. The Kalman filter consists of two phases: prediction and update. In the prediction phase, the filter estimates the state at the current moment X(k-1|k-1) based on the estimate of the previous state X(k-1|k-1). In the update phase, the filter optimizes the predicted value X(k|k) from the prediction phase based on the actual observed value Z(k) of the current state. Using GPS data as the observation Z(k) for the filter, the GPS sample data follows a normal distribution and a chi-square probability distribution. The likelihood estimates of the GPS position and heading errors are calculated as l. xy , l θ , Where p = (P gps (k)-P odo (k)), θ=(θ) gps (k)-θ odo (k)), representing the probability difference between GPS and IMU, respectively; ∑ odo (k) and ∑ gps These are the position error covariance matrices of the IMU and GPS at time k; when the calculated l xy With l θ Compared to the set threshold l xyth and l θth When the value is small, the current data is considered to meet the requirements. The GPS data is used as the input of the filter observation value Z to update the robot position X(k|k) and the error covariance matrix P(k|k). Under the EKF definition, the error covariance matrix R of GPS is composed of the position error covariance ∑ gps and heading error covariance composition, Define R according to different cases -1 :

1. When l xy (k) <l xyth And l θ <l θth , 2. When l xy (k) <l xyth And l θ >l θth , 3. When l xy (k)>l xyth And l θ <l θth , At this point, the position error is predicted to be too large for the heading error, and therefore it is not fused with the IMU data.

2. The multi-robot collaborative system for transporting disaster relief supplies according to claim 1, characterized in that: The data from the fused IMU and GPS are specifically as follows: Initialize X(0), P(0), k = 1; S11: Prediction phase, predict the current state X(k|k-1) based on the state of the previous time step: X(k∣k-1)=F(X(k-1∣k-1),U(k)) (6) S12: Update the system prediction covariance matrix P(k|k-1): in It is the Jacobian matrix of F to X at time k-1, and Q(k) and R(k) are the system noise and observation noise, i.e. GPS and IMU observation noise; S13: Calculate the maximum likelihood estimates of GPS heading error and position error according to equation (1). θ (k) and l xy (k), select the corresponding R according to the conditions of equations (3) and (4). -1 (k) Calculate the Kalman filter gain K(k): K(k)=P(k∣k-1)H(k) T [H(k)P(k∣k-1)H(k) T +R(k)] -1 (9) H(k) = diag{1,1,1}; S14: Calculate the optimized state X(k|k) at time k based on the filter gain, and update the error P(k|k). X(k∣k)=X(k∣k-1)+K(k)(Z(k)-H(k)X(k∣k-1)) (10) P(k∣k)=[Ek(k)H(k)]P(k∣k-1) (11) S15: k = k + 1; Design a distributed control law using the Euler-Lagrange model as the robot's dynamics model; The leader model is: q0=Co (12) q0∈R n This represents the position signal followed by each material transport robot, which is generated by the leader system in equation (12), where o∈R m , S∈R m×m , C∈R n×m It is a constant matrix; Suppose there are N robots cooperating in transportation. The Lagrange model of N robots is represented as: in, These are the generalized position and velocity vectors, M. i (q i )∈R n×n It is a mass matrix that describes the distribution of mass across each independent coordinate axis; It is the vector of the Coriolis force and the centripetal force, G i (q i )∈R n It is the gravity vector; u i ∈R n It is a generalized force vector; In a scenario where each transport robot can only obtain its own state information and that of its communicable neighbors and followers, and the robot model information is uncertain, a control system is designed for each transport robot. i Such that at any initial q i (0), o(0), q i (t), o(t) all exist and are bounded, satisfying: When the expected event occurs, the system control policy is updated. The triggering policy is as follows: in κ and β are positive real numbers, and the subscript k represents time k; when the above conditions are met, each transport robot will update its controller at adjacent trigger times. The control strategies for each robot remain unchanged; the update law of the controller is as follows: It is the observation of the leader's internal state o by each robot, achieved by designing a state observer η for each robot. i This enables the robot to perform position tracking even when it cannot communicate with the leader; by assumption 1, S is denoted as Design an ω-observer for each robot: Construct the observation matrix S for each robot. i The leader's state is measured by the observation matrix, thus enabling tracking.

3. The multi-robot collaborative system for transporting disaster relief supplies according to claim 2, characterized in that: For each of the robots, we have: Arbitrary initialization of q i (0), o(0); S21: Determine whether the event triggering condition is met by (15); If the condition is not met, proceed to S24; If the condition is met, proceed to S22; S22: The S matrix of the observed leader, calculated by solving equation (18). S23: Observe the leader's state o, and calculate it by solving equation (16). S24: Controller (16) implement.

Images