A follow-up type auxiliary inspection robot and a control method thereof

Abstract

The application relates to the technical field of intelligent mobile robots, in particular to a following type auxiliary inspection robot and a control method thereof, to solve the problems that existing customs on-site inspection work relies on manual carrying tool equipment samples, existing following robots cannot adapt to the inspection start-stop alternating operation rhythm, and robots are prone to position disturbance and heading deviation when personnel stay close to the robot, a specific method comprises the following steps: horizontal relative position estimation based on three-dimensional ranging, relative state construction and filtering processing, following error and motion control quantity calculation, stop intention judgment, following-stop switching control based on a hysteresis state machine, state related motion control, obstacle safety constraint and environment alarm linkage control.

Description

Technical Field

[0001] This invention relates to the field of intelligent mobile robot technology, and in particular to a follow-up assisted inspection robot and its control method. Background Technology

[0002] Currently, customs inspection operations primarily rely on officers carrying a full set of equipment manually. This includes testing instruments, sampling tools, label printing equipment, sample containers, and various auxiliary equipment. When encountering numerous inspection points, open areas, and scattered cargo, officers must repeatedly move equipment between different areas, resulting in heavy physical strain and slowing down overall inspection efficiency due to the excessive carrying of items. Furthermore, in high-risk inspection areas with radiation, toxic gases, or harsh environments, prolonged close-range work with equipment significantly increases personal safety risks.

[0003] With the development of mobile robot technology, most existing follower mobile robots rely on vision, Bluetooth, UWB, and LiDAR for positioning, and motion algorithms to achieve personnel companionship. However, the development of existing equipment is geared towards general delivery and care scenarios, and the control logic only focuses on continuous following accuracy. It lacks supporting functions and control mechanisms designed for customs inspection operations, making it difficult to directly deploy in inspection work and exhibiting several shortcomings. First, it lacks a dedicated integrated module for inspection, missing operational units such as sample storage, weighing, barcode scanning, and environmental monitoring, thus failing to achieve integrated auxiliary inspection. Second, it cannot adapt to the alternating walking and stopping rhythm of operations; when customs officers stop to collect samples or scan barcodes, the robot continuously makes minor adjustments and shakes, interfering with on-site operations. Third, it lacks operational mode switching logic, unable to distinguish between following movement and close-range stopping, failing to simultaneously meet the requirements for tracking sensitivity and stopping stability. Fourth, it lacks environmental risk monitoring and alarm components, unable to identify and warn of dangers such as toxic gases and excessive radiation in real time. Fifth, it only achieves basic following movement and has not established a human-machine collaborative operation system that matches the entire inspection process.

[0004] In summary, current technological development focuses primarily on robot motion control itself, without comprehensively considering the specific needs of customs inspection scenarios, such as human-machine collaboration, stable parking, environmental safety warnings, and adaptation to standardized business procedures. To address the industry pain points of low efficiency and high safety risks associated with manual inspections, and the inability of general-purpose robots to adapt to specialized inspection work, it is necessary to propose a dedicated follow-up auxiliary inspection robot, along with supporting control methods and a complete operational solution. Summary of the Invention

[0005] The problems to be solved by this invention are: the existing customs on-site inspection operations rely on manual handling of tools, equipment and samples; the existing following robots cannot adapt to the alternating walking and stopping rhythm of inspection operations; the robots are prone to positional disturbances and heading deviations when personnel are stationed at close range; and the existing inspection auxiliary equipment does not have environmental safety monitoring and hazard warning functions.

[0006] To address the problems in the prior art, the present invention provides a follow-up assisted inspection robot and its control method, specifically including the following steps: S1. Horizontal relative position estimation based on three-dimensional ranging; Establish the robot carrier coordinate system; Get the target location of the tag , No. Base station carrier location and the The ranging value between the base station carrier and the tag target is ; Calculate vertical height difference and horizontal distance ; Based on vertical height difference and horizontal distance Based on the height difference between the tag target and the base station carrier, the planar position of the tag target relative to the base station carrier is calculated. ; The distance measurement value For the first The distance between each base station carrier and the tag target ,in, The label target position in the robot carrier coordinate system. For the first Location of each base station carrier; The vertical height difference For the first The vertical height difference between the base station carrier location and the tag target ,in, This is the height offset of the label target position. For the first The height offset of the base station carrier location; The horizontal distance For the first Horizontal distance between the location of each base station carrier and the tag target ,in, For the first The distance between each base station carrier and the tag target For the first The vertical height difference between the location of the base station carrier and the tag target; The planar position of the tag target relative to the base station carrier , ,in, The label target position in the robot carrier coordinate system. For the first Location of each base station carrier For the first Horizontal distance between the location of each base station carrier and the tag target; S2. Relative state construction and filtering; Setting the first The horizontal position of the tag target calculated within each control cycle ; The filtered horizontal position is calculated based on the exponential smoothing method. ; Based on the filtered horizontal position Calculate the horizontal relative distance between the tag target and the base station carrier. and horizontal relative azimuth ; Based on the horizontal relative distance within adjacent control cycles and horizontal relative azimuth Changes in relative motion trend and And construct relative state vectors ; The target horizontal position of the label , ,in, and yes Planar coordinates within a control cycle; The filtered horizontal position : ,in, For smoothing coefficients; S3. Calculation of following error and motion control quantity; Based on horizontal relative distance and horizontal relative azimuth and expected horizontal distance And the desired azimuth angle is The distance error was calculated. and azimuth error ; Distance error and azimuth error As a linear velocity control model and angular velocity control model enter; Linear velocity control model and angular velocity control model Output the speed control variables separately; S4. Determining the intention to stop; Get horizontal relative distance Relative motion trend and ; Determine horizontal relative distance Relative motion trend and Does it meet the preset conditions? Label target satisfaction , and After the preset conditions are met, a timer is started, and after the preset time threshold is reached, the system enters a pause state. S5. Follow-stop switching control based on hysteresis state machine; Set the base station carrier in the first The state of each control cycle ; Set the dwell threshold and recovery threshold Meeting the recovery threshold Greater than the dwell threshold ; Switch control states according to the parking switching control rules; The state : ,in, Indicates a follow state. Indicates a stationary or holding state; S6, State-related motion control; Get the current control status ; When in follow mode F, motion control variables are generated based on real-time distance error and orientation error; When in the parking hold state H, the robot maintains its current pose, does not perform real-time distance error and orientation error calculation, and sets the control variable output to 0. S7. Linkage control between obstacle safety constraints and environmental alarms; Set the nearest obstacle distance to The safe distance threshold is ; If the nearest obstacle distance is satisfied... Is it less than or equal to the safety threshold? The base station carrier executes speed security control rules.

[0007] Furthermore, the aforementioned This estimates the distance change of the tag target relative to the base station carrier within adjacent control periods. ,in, To control the cycle; The Estimation of the angle change of the tag target relative to the base station carrier within adjacent control cycles. ,in, To control the cycle; The relative state vector As the input for follow-stop switching control based on hysteresis state machine, ,in, yes Filtered planar coordinates within each control cycle Horizontal relative distance Horizontal relative azimuth angle, distance within adjacent control cycles and azimuth Change estimation.

[0008] Furthermore, the distance error The horizontal distance between the labeled target and the robot carrier. ,in, Set the expected safe following distance for the robot carrier; The azimuth error The horizontal azimuth angle of the labeled target relative to the robot carrier. ,in, The desired safe following angle can be set according to the robot's expectations; The linear velocity control model ,in, To control the gain, For the maximum linear velocity, This is a limiting function; The angular velocity control model ,in, To control the gain, For the maximum angular velocity, This is the amplitude limiting function.

[0009] Furthermore, the pause switching control rules , ,in, This indicates the robot's state in the next control cycle. This indicates the duration for which the above-mentioned conditions for determining a standstill are continuously met.

[0010] Furthermore, the execution speed safety control rules Set corresponding safe speeds for deceleration control, stop, and safe avoidance states. ,in, The upper limit of safe speed is determined based on the distance to the obstacle; The Safe distance speed limit ,in To ensure a safe deceleration distance, The distance to the nearest obstacle. This is the safe distance threshold.

[0011] Furthermore, a follow-up assisted inspection robot and its control method also include an operation method: S10. Operation preparation: According to the inspection task requirements, load the corresponding inspection tools, testing equipment, sample containers, label printing equipment, protective equipment and other auxiliary materials onto the robot, and customs officers wear wearable positioning tags and start the robot system. S11. Establish human-machine following relationship; The robot starts the positioning and following function, obtains the distance measurement information between multiple UWB base stations and the tag worn by the customs officer, calculates the positional relationship between the customs officer and the robot, and enters the following control state. S12. Collaborate with customs officers to the inspection area; The robot automatically follows the customs officers based on the linear velocity and angular velocity output by the controller, and uses environmental perception components such as lidar and ultrasonic sensors to detect obstacles and ensure safe passage, collaborating with customs officers to the target inspection area or the location of the designated goods. S13. Enter the parking auxiliary operation state; when the customs officer stops at the target operation point and enters the close inspection operation, the robot automatically stops moving and turning according to the parking and holding control strategy, and maintains the current position and posture stability; in the parking state, the robot serves as a mobile auxiliary operation platform for the customs officer to pick up or return inspection tools, samples, equipment and other auxiliary materials. S14. Conduct auxiliary inspection operations; Customs officers use the auxiliary inspection operation components mounted on the robot to conduct on-site inspection operations; S15. Continuous environmental monitoring and safety early warning; During the robot's following, parking and auxiliary inspection operations, radiation sensors, toxic gas sensors and temperature and humidity sensors continuously collect on-site environmental parameters; When the environmental parameters are detected to reach the preset early warning threshold or danger threshold, the robot triggers an audible and visual alarm to remind customs officers to take protective measures, stay away from dangerous areas, and suspend or terminate the current inspection operation. S16. Resume following and transfer operations; When the customs officer completes the inspection at the current work point and leaves, the robot detects that the horizontal relative distance between the customs officer and the robot exceeds the resume following threshold, exits the parking and holding state and re-enters the following control state to continue following the customs officer to the next work point; For tasks involving multiple inspection points, the robot can repeat steps S3 to S7 until all inspection tasks are completed. S17. Task completed; After completing all inspection tasks, the robot returns to the designated location to standby, or performs closing operations such as tool retrieval, sample retrieval, and task information organization.

[0012] Furthermore, a follow-up assisted inspection robot is characterized by comprising: a controller 200, a follow-up module 201 electrically connected to the controller, a sensing module 202, and an inspection module 203; the follow-up module 201 is used to receive the horizontal distance and azimuth angle of the tag target relative to the base station carrier, the sensing module 202 is used to sense environmental information in real time, obtain obstacle distances and monitor surrounding harmful gases, and the inspection module 203 is used to obtain the weight of the item, output the tag, and input information.

[0013] Furthermore, the following module 201 includes a first base station (123) disposed at the front end of the robot carrier and a second base station 122 and a third base station 121 disposed at both ends of the tail of the robot carrier; the sensing module 202 includes a lidar 131, an ultrasonic sensor 132, and a radiation monitoring sensor 151 distributed at the front end of the robot carrier; the inspection module 203 includes a weighing component 143 embedded in the bottom of the robot carrying platform 141, a label output component 144 disposed at the front end of the robot carrier, and an information input component 145 disposed on the upper end of the ultrasonic sensor 132.

[0014] Compared to existing technologies, this invention can autonomously follow customs officers and carry inspection tools, equipment, samples, and auxiliary materials, reducing manual handling and retracing operations, lowering the labor intensity of customs officers, and improving the efficiency of port inspection operations. This invention sets independent stopping and following recovery thresholds, smoothly switching between following and static stopping modes based on personnel movement status. When personnel are stationary, the robot stops unnecessary position and heading adjustments, adapting to the alternating stop-and-go work rhythm on site. Simultaneously, it converts UWB three-dimensional ranging data into horizontal control quantities, eliminating positioning interference caused by differences in tag and base station installation heights, avoiding robot shaking and frequent starts and stops during close-range operations, and ensuring stable human-robot collaborative operation. Furthermore, this invention integrates radiation, toxic gas, temperature and humidity sensors, and audible and visual alarm modules, providing timely warnings when environmental parameters are abnormal, guiding customs officers to take protective measures and evacuate dangerous areas, enhancing on-site operational safety. In terms of application scenarios, the equipment integrates multiple functions and can be adapted to various inspection scenarios such as yards, docks, and airport cargo terminals. Attached Figure Description

[0015] Figure 1 Flowchart of the control method for a follow-up assisted inspection robot; Figure 2 Schematic diagram of the operation method of the follow-up assisted inspection robot; Figure 3 Schematic diagram of a follow-up assisted inspection robot system; Figure 4 Schematic diagram of a follow-up assisted inspection robot; Figure 5 Front view of the structure of the follow-up assisted inspection robot. Detailed Implementation

[0016] To better understand the purpose, structure, and function of this invention, the following detailed description of a follow-up assisted inspection robot and its control method, in conjunction with the accompanying drawings, is provided.

[0017] like Figure 1 The present invention discloses a follow-up assisted inspection robot and its control method, the method comprising steps S1 to S7, which are described in detail below: Step S1: Estimating the horizontal relative position based on three-dimensional ranging; Specifically, in the robot coordinate system, the position of the tag worn by the customs officer is... , No. The location of each UWB base station is , No. The ranging value between a UWB base station and the customs officer tag is ,satisfy: ,, Furthermore, since the robot primarily moves within the ground plane, the follow control focuses more on the horizontal positional relationship between the operator and the robot. Therefore, height compensation is applied to the 3D ranging results, converting the spatial ranging into a horizontal distance constraint.

[0018] Furthermore, let the first The vertical height difference between each UWB base station and the customs officer's tag is: , Then the first The horizontal distance between each UWB base station and the customs officer's tag It can be represented as: , The calculation shows that: , Based on the horizontal distance information between multiple UWB base stations and tags, the position of the customs officer relative to the robot in the horizontal plane can be determined. In one implementation, the solution can be obtained using the following optimization model: , Through the above implementation method, a horizontal relative position suitable for robot ground motion control can be obtained, taking into account the difference between the base station installation height and the tag wearing height, thereby reducing the impact of height changes on follow control.

[0019] S2. Relative state construction and filtering; It should be noted that the controller 200 can perform filtering on the calculated horizontal position.

[0020] Specifically, let the first The horizontal position of the customs officer calculated within each control cycle is: , The filtered horizontal position is: , Furthermore, in the above implementation steps, exponential smoothing can be used for filtering: , in, This is the smoothing coefficient.

[0021] Furthermore, based on the filtered horizontal position, the horizontal relative distance between the customs officer and the robot can be calculated. and horizontal relative azimuth : , , Furthermore, the relative motion trend can be estimated based on the changes in distance and azimuth within adjacent control cycles: , , in, To control the cycle.

[0022] Therefore, a human-machine relative state vector can be constructed: , It should be noted that the human-machine relative state vector is used to determine whether the customs officer is currently in different working states such as moving, stationary, or changing locations, and serves as the input for robot following control and stationary holding control.

[0023] S3. Calculation of following error and motion control quantity; It should be noted that when the robot is in a following state, the controller generates motion control commands based on the horizontal relative distance and horizontal relative azimuth angle between the operator and the robot.

[0024] Let the expected following distance be The expected relative azimuth angle is Then the distance error and azimuth error They are respectively: , , in, The desired position for the robot to follow can be set, for example, it can be selected. This indicates that the customs officer is expected to be positioned in front of the robot.

[0025] In the above embodiment, the robot linear velocity and angular velocity It can be generated based on the error control law with amplitude limiting: , , in, and To control the gain, For the maximum linear velocity, For the maximum angular velocity, This is the amplitude limiting function. The amplitude limiting function can be defined as: , Through the aforementioned control law, the robot can smoothly adjust its movement speed according to the distance and orientation errors between the customs officer and the robot, avoiding excessive speed output that could lead to unstable following or safety risks during on-site operations.

[0026] The aforementioned limiting function can also employ proportional control, PID control, fuzzy control, model predictive control, or other closed-loop control methods to generate the robot's linear velocity based on the distance error and orientation error. and angular velocity control quantity .

[0027] S4. Determining the intention to stop; It should be noted that, in order to avoid the robot mistakenly entering a stationary state due to a momentary proximity, according to the above implementation method, the determination of whether the customs officer has entered a close inspection operation state is based on the horizontal relative distance, relative motion trend and duration.

[0028] Specifically, customs officers can be determined to be in a standby operation state when the following conditions are met: , Furthermore, the time during which the above conditions are continuously met is not less than a preset time. .in, To set the stopping threshold, The distance change rate threshold, This is the threshold for the rate of change of azimuth angle. This is the time for confirmation of the stopover.

[0029] Based on the above criteria, the robot not only judges whether the customs officer is close based on distance, but also judges whether the officer is in a stable parking state based on the relative movement trend of the officer, thereby reducing the situation of accidental parking.

[0030] S5. Follow-stop switching control based on hysteresis state machine; This invention divides the robot's motion states during operation into a following state and a stationary state. Let the robot in the... The state of each control cycle is: , in, Indicates a follow state. This indicates a stationary or held state.

[0031] Furthermore, to prevent the robot from frequently switching states near critical distances, a dwell threshold is set in this step. and recovery follow threshold ,in: > .

[0032] Preferably, the state transition rule can be expressed as: , in, This indicates the robot's state in the next control cycle. This indicates the duration for which the above-mentioned conditions for determining a standstill are continuously met.

[0033] Through the aforementioned state switching mechanism with hysteresis characteristics, the robot can switch to a stationary state when the customs officer stably enters the close-range inspection state, and resume the following state after the customs officer leaves the current work point, thereby effectively avoiding frequent starts and stops and control oscillations near the critical distance.

[0034] S6, State-related motion control; It should be noted that the controller 200 outputs different motion control quantities in real time according to the robot's current state.

[0035] Specifically, when the robot is in the following state F, the controller generates linear velocity and angular velocity based on distance error and orientation error; when the robot is in the parking and holding state H, the controller suppresses the chassis motion output to keep the robot in its current pose.

[0036] Furthermore, in the above implementation, the state-dependent control law can be expressed as: , , Furthermore, in the stationary state, the robot's linear velocity and angular velocity are both zero, that is: , At this point, the robot no longer moves forward, backward, or turns on its own, but maintains its current position and posture, providing a stable platform for customs officers to perform operations such as sampling, scanning, weighing, printing, and recording.

[0037] Furthermore, in the stationary state, the robot's inspection assistance components, data display module, environmental perception components, and environmental monitoring and alarm-related components remain operational, thereby decoupling motion control from inspection assistance functions.

[0038] S7. Linkage control between obstacle safety constraints and environmental alarms; It should be noted that during the robot's following and parking process, the environmental perception components continuously detect surrounding obstacles.

[0039] Specifically, let the distance to the nearest obstacle in front of or around the robot be... The safe distance threshold is Then when the following conditions are met At this time, the robot can perform deceleration, stopping, or safe avoidance control.

[0040] Furthermore, in the above embodiments, the following speed safety constraints can be adopted: , in, This refers to the upper limit of the safe speed determined based on the distance to the obstacle. Preferably: , in, To ensure a safe deceleration distance.

[0041] Furthermore, the robot continuously collects environmental parameters during following, parking, and assisted inspection. Let the radiation detection value be... Given the toxic gas detection value, the comprehensive temperature and humidity index, and the corresponding warning thresholds, an environmental risk index can be constructed: , Furthermore, when the following conditions are met: At that time, the robot triggers the audible and visual alarm 154 to issue an alarm notification.

[0042] Furthermore, when environmental risk indicators reach the danger threshold... At that time, that is: The robot can enter a safe stopping or restricted movement state, enabling: This is to remind customs officers to take protective measures, stay away from dangerous areas, and suspend or terminate the current inspection operation.

[0043] Through the aforementioned obstacle safety constraints and environmental alarm linkage control, the robot can further improve the operational safety of complex inspection sites while achieving human-robot following and parking assistance.

[0044] In addition to the control method described above, the present invention also includes a method for on-site inspection of the following-type assisted inspection robot and its control method.

[0045] like Figure 2 The operation method steps are shown, including steps S10 to S17, which are described in detail below: S1. Preparation for the assignment; According to the requirements of the inspection task, the robot is equipped with the corresponding inspection tools, testing equipment, sample containers, label printing equipment, protective equipment and other auxiliary materials; customs officers wear wearable positioning tags and start the robot system.

[0046] Specifically, the inspection tools, testing equipment, sample containers, etc., can be placed in the cargo platform 141 or the inspection tool and sample storage module 142; the wearable positioning tag can be worn on the waist, chest or other relatively stable position of the customs officer so as to establish a distance measurement relationship with the UWB base stations 121, 122, 123 on the robot.

[0047] S2. Establish a human-machine following relationship; The robot activates its positioning and following function, acquiring distance measurement information between itself and the tag worn by the customs officer through three UWB base stations 121, 122, and 123, and establishing the human-machine relative state of the customs officer relative to the robot based on the distance measurement information.

[0048] S3. Proceed to the inspection area together; The robot automatically follows the customs officer based on the linear and angular velocities output by the controller, and uses environmental perception components such as LiDAR 131 and ultrasonic sensor 132 to detect obstacles and ensure safe passage, cooperating with the customs officer to reach the target inspection area or the designated cargo location.

[0049] During movement, the robot can continuously carry inspection tools, testing equipment, sample containers and other auxiliary materials, reducing the burden of manual handling by customs officers.

[0050] S4. Enter the parking auxiliary operation state; When the robot detects that a customs officer has entered a close inspection situation, the robot determines that the officer has entered a stable stopping operation state based on the relative movement trend and the duration of the stop, and then switches to the stopping and holding state.

[0051] When stationary, the robot stops moving and turning, and maintains its current position and posture, allowing customs officers to easily access or return inspection tools, samples, testing equipment and other auxiliary materials.

[0052] S5. Conduct auxiliary inspection operations; Customs officers used the inspection assistance components mounted on the robot to carry out on-site inspection operations.

[0053] Specifically, customs officers can use the inspection tool and sample storage module 142 to retrieve and place inspection tools or sample containers; use the weighing module 143 to weigh samples or items; use the label printing module 144 to print inspection labels or identification information; use the barcode scanning module 145 to scan barcodes or QR codes and enter inspection information; use the data display module 146 to view weighing results, environmental monitoring parameters, robot working status, or alarm information; and use the law enforcement recorder installation module 148 to install or fix a law enforcement recorder to achieve video recording of the inspection process or to assist in evidence collection.

[0054] In outdoor or mosquito-prone work environments, the mosquito repellent module 147 can also provide auxiliary protection for customs officers.

[0055] S6. Continuous environmental monitoring and safety early warning; During the robot's following, parking, and assisted inspection operations, radiation sensor 151 and toxic gas and temperature and humidity sensor 152 continuously collect on-site environmental parameters.

[0056] When environmental parameters are detected to reach the preset warning threshold or danger threshold, the robot triggers an alarm via the audible and visual alarm 154 to remind customs officers to take protective measures, move away from dangerous areas, and suspend or terminate the current inspection operation.

[0057] Preferably, different types of abnormal environmental risks can correspond to different alarm methods. For example, a continuous high-frequency audible and visual alarm is triggered when radiation is abnormal; an intermittent audible and visual alarm is triggered when the concentration of toxic gases exceeds the limit; and a warning alarm is triggered when temperature and humidity are abnormal.

[0058] Furthermore, when environmental parameters reach a dangerous threshold, the robot can remain stationary and restrict further movement into dangerous areas to improve the safety of on-site inspection operations.

[0059] S7. Resume following and relocation operations; When the customs officer completes the inspection at the current work point and leaves, the robot detects that the horizontal relative distance between the customs officer and the robot exceeds the recovery follow threshold, exits the parking auxiliary operation state and re-enters the follow control state.

[0060] The robot then continues to follow the customs officer to the next work point. For tasks involving multiple inspection points, the robot can repeat steps S3 to S7 until all inspection tasks are completed.

[0061] Through the aforementioned cyclical operation method, the robot can adapt to the actual needs of frequent transfers between multiple work points during on-site inspection.

[0062] S8, Mission complete; After completing all inspection tasks, the robot returns to the designated location to standby, or performs finishing operations such as tool retrieval, sample retrieval, task information organization, and test data uploading.

[0063] Through the above-mentioned operating methods, the robot can continuously perform functions such as accompanying assistance, stable parking, inspection support, and environmental safety early warning throughout the entire inspection process, thereby providing continuous assistance and support for customs officers' on-site inspection operations, improving inspection efficiency, reducing labor intensity, and enhancing on-site operation safety.

[0064] In addition, the present invention provides a follow-up assisted inspection robot and its control method, which, in addition to the control method described above, also includes a follow-up assisted inspection robot.

[0065] like Figure 3 , 4As shown in Figure 5, the follow-up assisted inspection robot includes a controller 200, a follow-up module 201 electrically connected to the controller, a sensing module 202, and an inspection module 203; the follow-up module 201 is used to receive the horizontal distance and azimuth angle of the tag target relative to the base station carrier, the sensing module 202 is used to sense environmental information in real time, obtain the distance of obstacles and monitor the surrounding harmful gases, and the inspection module 203 is used to obtain the weight of the item, output the tag, and input information.

[0066] It should be noted that the follower module 201 includes a first base station 123 disposed at the front end of the robot carrier and a second base station 122 and a third base station 121 disposed at both ends of the tail of the robot carrier.

[0067] The first base station 123, the second base station 122, and the third base station 121 are deployed at the front and rear ends of the robot carrier. They perform distance perception and azimuth angle calculation with the target tags worn by customs officers. The relative position of the target personnel with respect to the robot carrier is determined by the distance and angle information perceived by the three base stations.

[0068] Furthermore, the controller 200 acquires the distance and angle information of the first base station 123, the second base station 122, and the third base station 121 relative to the tag target. Under the premise of meeting the safe following distance, it calculates the linear velocity and angular velocity control information that can be read by the mobile chassis 110 and updates it in real time according to the position change of the customs officer wearing the tag target relative to the robot carrier.

[0069] It should be noted that the perception module 202 includes a lidar 131, an ultrasonic sensor 132, and a radiation monitoring sensor 151, which are distributed and set at the front end of the robot carrier.

[0070] Furthermore, during the inspection process, the robot carrier equipped with a base station follows customs officers wearing tags to perform the inspection. During the following process, the perception module 202 perceives the surrounding environment data in real time. When the distance exceeds the preset safety distance, the lidar 131 and the ultrasonic sensor 132 jointly determine whether there is obstacle information on the following path. If there is obstacle information, the lidar 131 obtains point cloud information, performs obstacle contour expansion inference, and the controller 200 calculates the obstacle avoidance angle and controls the mobile chassis 110 to adjust for obstacle avoidance.

[0071] Furthermore, during the execution of the task, in order to ensure the safety and reliability of the inspection environment, a radiation sensor 151 is installed at the front end of the robot carrier to monitor the surrounding environment in real time. When the radiation threshold is exceeded, the controller 200 sends an alarm message, and at the same time, the robot carrier's audible and visual alarm 153 is triggered to sound an alarm.

[0072] It should be noted that the inspection module 203 includes a weighing component 143 embedded in the bottom of the robot carrying platform 141, a label output component 144 set at the front end of the robot carrier, and an information input component 145 set on the upper end of the ultrasonic sensor 132.

[0073] Furthermore, in the inspection task, if the inspection target involves weighing, it can be weighed by the weighing component 143 at the bottom of the robot carrying platform 141. After weighing, the label output component 144 at the front end of the robot carrier prints out the weighing information and number of the inspection component, and enters it into the inspection system for storage by the information input component 145 set on the upper end of the ultrasonic sensor 132.

[0074] It is understood that the present invention has been described through some embodiments, and those skilled in the art will recognize that various changes or equivalent substitutions can be made to these features and embodiments without departing from the spirit and scope of the invention. Furthermore, under the teachings of the present invention, these features and embodiments can be modified to adapt to specific situations and materials without departing from the spirit and scope of the invention. Therefore, the present invention is not limited to the specific embodiments disclosed herein, and all embodiments falling within the scope of the claims of this application are within the protection scope of the present invention.

Claims

1. A follow-type assisted inspection robot and its control method, characterized in that, The method includes: S1. Horizontal relative position estimation based on three-dimensional ranging; Establish the robot carrier coordinate system; Get the target location of the tag , No. Base station carrier location and the The ranging value between the base station carrier and the tag target is ; Calculate vertical height difference and horizontal distance ; Based on vertical height difference and horizontal distance Based on the height difference between the tag target and the base station carrier, the planar position of the tag target relative to the base station carrier is calculated. ; The distance measurement value For the first The distance between each base station carrier and the tag target ,in, The label target position in the robot carrier coordinate system. For the first Location of each base station carrier; The vertical height difference For the first The vertical height difference between the base station carrier location and the tag target ,in, This is the height offset of the label target position. For the first The height offset of the base station carrier location; The horizontal distance For the first Horizontal distance between the location of each base station carrier and the tag target ,in, For the first The distance between each base station carrier and the tag target For the first The vertical height difference between the location of the base station carrier and the tag target; The planar position of the tag target relative to the base station carrier , ,in, The label target position in the robot carrier coordinate system. For the first Location of each base station carrier For the first Horizontal distance between the location of each base station carrier and the tag target; S2. Relative state construction and filtering; Setting the first The horizontal position of the tag target calculated within each control cycle ; The filtered horizontal position is calculated based on the exponential smoothing method. ; Based on the filtered horizontal position Calculate the horizontal relative distance between the tag target and the base station carrier. and horizontal relative azimuth ; Based on the horizontal relative distance within adjacent control cycles and horizontal relative azimuth Changes in relative motion trend and And construct relative state vectors ; The target horizontal position of the label , ,in, and yes Planar coordinates within a control cycle; The filtered horizontal position : ,in, For smoothing coefficients; S3. Calculation of following error and motion control quantity; Based on horizontal relative distance and horizontal relative azimuth and expected horizontal distance And the desired azimuth angle is The distance error was calculated. and azimuth error ; Distance error and azimuth error As a linear velocity control model and angular velocity control model enter; Linear velocity control model and angular velocity control model Output the speed control variables separately; S4. Determining the intention to stop; Get horizontal relative distance Relative motion trend and ; Determine horizontal relative distance Relative motion trend and Does it meet the preset conditions? Label target satisfaction , and After the preset conditions are met, a timer is started, and after the preset time threshold is reached, the system enters a pause state. S5. Follow-stop switching control based on hysteresis state machine; Set the base station carrier in the first The state of each control cycle ; Set the dwell threshold and recovery threshold Meeting the recovery threshold Greater than the dwell threshold ; Switch control states according to the parking switching control rules; The state : ,in, Indicates a follow state. Indicates a stationary or holding state; S6, State-related motion control; Get the current control status ; When in follow mode F, motion control variables are generated based on real-time distance error and orientation error; When in the parking hold state H, the robot maintains its current pose, does not perform real-time distance error and orientation error calculation, and sets the control variable output to 0; S7. Linkage control between obstacle safety constraints and environmental alarms; Set the nearest obstacle distance to The safe distance threshold is ; If the nearest obstacle distance is satisfied... Is it less than or equal to the safety threshold? The base station carrier executes speed security control rules.

2. The follow-up assisted inspection robot and its control method according to claim 1, characterized in that, The This estimates the distance change of the tag target relative to the base station carrier within adjacent control periods. ,in, To control the cycle; The Estimation of the angle change of the tag target relative to the base station carrier within adjacent control cycles. ,in, To control the cycle; The relative state vector As the input for follow-stop switching control based on hysteresis state machine, ,in, yes Filtered planar coordinates within each control cycle Horizontal relative distance Horizontal relative azimuth angle, distance within adjacent control cycles and azimuth Change estimation.

3. The follow-up assisted inspection robot and its control method according to claim 1, characterized in that, The distance error The horizontal distance between the labeled target and the robot carrier. ,in, Set the expected safe following distance for the robot carrier; The azimuth error The horizontal azimuth angle of the labeled target relative to the robot carrier. ,in, The desired safe following angle can be set according to the robot's expectations; The linear velocity control model ,in, To control the gain, For the maximum linear velocity, This is a limiting function; The angular velocity control model ,in, To control the gain, For the maximum angular velocity, This is the amplitude limiting function.

4. The follow-up assisted inspection robot and its control method according to claim 1, characterized in that, The pause switching control rules : ,in, This indicates the robot's state in the next control cycle. This indicates the duration for which the above-mentioned conditions for determining a standstill are continuously met.

5. The follow-up assisted inspection robot and its control method according to claim 1, characterized in that, The execution speed safety control rules Set corresponding safe speeds for deceleration control, stop, and safe avoidance states. ,in, The upper limit of safe speed is determined based on the distance to the obstacle; The Safe distance speed limit ,in To ensure a safe deceleration distance, The distance to the nearest obstacle. This is the safe distance threshold.

6. A follow-up assisted inspection robot and its control method, characterized in that, It also includes a work method: S10. Operation preparation: According to the inspection task requirements, load the corresponding inspection tools, testing equipment, sample containers, label printing equipment, protective equipment and other auxiliary materials onto the robot. Customs officers wear wearable positioning tags and start the robot system. S11. Establish a human-machine following relationship. The robot starts the positioning and following function, obtains the distance measurement information between multiple UWB base stations and the tag worn by the customs officer, calculates the positional relationship between the customs officer and the robot, and enters the following control state. S12. Cooperate with customs officers to proceed to the inspection area. The robot automatically follows the customs officers based on the linear velocity and angular velocity output by the controller. It also uses environmental perception components such as lidar and ultrasonic sensors to detect obstacles and ensure safe passage, and cooperates with customs officers to proceed to the target inspection area or the location of the designated goods. S13. Entering the parking auxiliary operation state: When the customs officer stops at the target operation point and enters the close inspection operation, the robot automatically stops moving and turning according to the parking and holding control strategy, and maintains the current position and posture stability. In the parking state, the robot serves as a mobile auxiliary operation platform for the customs officer to pick up or return inspection tools, samples, equipment and other auxiliary materials. S14. Conduct auxiliary inspection operations. Customs officers use the inspection auxiliary operation components mounted on the robot to carry out on-site inspection operations. S15. Continuous environmental monitoring and safety early warning: During the robot's following, parking, and auxiliary inspection operations, radiation sensors, toxic gas sensors, and temperature and humidity sensors continuously collect on-site environmental parameters; when the environmental parameters are detected to reach the preset early warning threshold or danger threshold, the robot triggers an audible and visual alarm to remind customs officers to take protective measures, stay away from dangerous areas, and suspend or terminate the current inspection operation. S16. Resume following and transfer operations. When the customs officer completes the inspection at the current work point and leaves, the robot detects that the horizontal relative distance between the customs officer and the robot exceeds the resume following threshold. It then exits the parking and holding state and re-enters the following control state to continue following the customs officer to the next work point. For tasks with multiple inspection points, the robot can repeat steps S3 to S7 until all inspection tasks are completed. S17. After completing all inspection tasks, the robot returns to the designated location to standby, or performs follow-up operations such as tool retrieval, sample retrieval, and task information organization.

7. A follow-up assisted inspection robot, characterized in that, include: The system includes a controller (200), a follower module (201) electrically connected to the controller, a sensing module (202), and an inspection module (203); the follower module (201) is used to receive the horizontal distance and azimuth angle of the tag target relative to the base station carrier, the sensing module (202) is used to sense environmental information in real time, obtain the distance to obstacles and monitor the surrounding harmful gases, and the inspection module (203) is used to obtain the weight of the item, output the tag, and input information.

8. The follow-up assisted inspection robot according to claim 7, characterized in that, The following module (201) includes a first base station (123) located at the front end of the robot carrier and a second base station (122) and a third base station (121) located at both ends of the tail of the robot carrier; the sensing module (202) includes a lidar (131) and an ultrasonic sensor (132) and a radiation monitoring sensor (151) distributed at the front end of the robot carrier; the inspection module (203) includes a weighing component (143) embedded in the bottom of the robot carrying platform (141), a label output component (144) located at the front end of the robot carrier, and an information input component (145) located on the upper end of the ultrasonic sensor (132).

Images