Intelligent rescue robot trolley and control method

Abstract

The invention relates to an intelligent rescue robot trolley and a control method, and the intelligent rescue robot trolley comprises a chassis moving mechanism which comprises a chassis, omnidirectional wheels and a motor; the holder vision mechanism comprises a holder and a camera; the mechanical claw is arranged on the chassis and is used for grabbing a target object; the mechanical claw comprises a claw finger driving mechanism, a claw finger driving connecting piece and claw fingers, and the claw finger driving mechanism is connected with the claw fingers through the gripper connecting piece; the clamping jaw driving structure comprises a steering engine, and the mechanical jaw is opened or closed by controlling the rotating angle of the steering engine, so that a target object is clamped; and the control system is used for controlling the working states of the chassis moving mechanism, the holder vision module and the mechanical claw. According to the invention, the rescue target can be quickly and directly reached without yawing through attitude control and trajectory planning, so that unnecessary movement and time consumption are reduced, the rescue efficiency is improved, and rescue tasks can be efficiently, accurately and safely executed in various complex environments.

Description

Technical Field

[0001] This invention relates to the field of intelligent robot technology, and in particular to an intelligent rescue robot vehicle and its control method. Background Technology

[0002] With the rapid development of technology, intelligent rescue robots are being used more and more widely in disaster relief, emergency medical care, and industrial accidents. These robots need to perform tasks such as high-speed movement, precise obstacle avoidance, target collection and transportation in complex and ever-changing environments, while possessing the ability to operate autonomously or remotely. In the design and application of intelligent rescue robot vehicles, ensuring that they can accurately follow a predetermined trajectory when performing tasks is crucial.

[0003] When a robotic vehicle moves in a straight line, its initial and final angles must be consistent, and it must not deviate from the centerline of its initial movement. However, in actual testing, various factors can cause the vehicle to deviate from the centerline. The intelligent rescue robotic vehicle may deviate from its intended trajectory during straight-line movement for various reasons, including but not limited to mechanical imbalances, wear and tear, uneven power output of the drive motor, and environmental factors such as uneven ground, slippage, or changes in slope. Traditional control methods may not be effective in handling dynamically changing environmental conditions and lack real-time adjustment and adaptive mechanisms, resulting in insufficient response to emergencies. Summary of the Invention

[0004] Therefore, the technical problem to be solved by the present invention is to overcome the shortcomings of the prior art and provide an intelligent rescue robot vehicle and control method. Through attitude control and trajectory planning, it can reach the rescue target quickly and directly without deviating from the course, reducing unnecessary movement and time consumption, improving rescue efficiency, and enabling it to perform rescue tasks efficiently, accurately and safely in various complex environments.

[0005] In a first aspect, to solve the above-mentioned technical problems, the present invention provides an intelligent rescue robot vehicle, comprising: A chassis moving mechanism includes a chassis, omnidirectional wheels, and a motor; the motor is located on the underside of the chassis, and the output shaft of the motor is connected to the omnidirectional wheels; A gimbal vision mechanism includes a gimbal and a camera; the gimbal is located at the center of the chassis, the camera is mounted on the gimbal, and the gimbal is controlled by two servo motors to achieve horizontal and pitch rotation. A mechanical gripper, mounted on the chassis, is used to grasp a target object. The mechanical gripper includes a gripper finger drive mechanism, a gripper finger drive connector, and grippers. The gripper finger drive mechanism is connected to the grippers via the gripper finger drive connector. The gripper finger drive mechanism includes a servo motor, which provides the opening and closing power for the mechanical gripper. By controlling the rotation angle of the servo motor, the mechanical gripper opens or closes, thereby achieving the gripping of the target object. The control system is used to control the working status of the chassis moving mechanism, the gimbal vision mechanism, and the robotic gripper.

[0006] In one embodiment of the present invention, the control system includes: The Maixcam module is used for real-time image acquisition and processing to identify rescue targets of different colors; The gyroscope module is used to acquire the robot's attitude information for attitude control and navigation assistance. The TOF ranging module is used to measure the distance between the intelligent rescue robot and surrounding obstacles in real time, as well as the distance between the intelligent rescue robot and the rescue target. The optical flow sensor module uses changes in adjacent frames to obtain displacement information, enabling localization and obstacle avoidance. PWM servo drive module, used to simultaneously control the rotation angle and speed of multiple servos; The motor drive module is used to adjust the output torque and speed of the motor; The main control board exchanges data with the Maixcam module, gyroscope module, TOF ranging module, optical flow sensor module, PWM servo drive module, and motor drive module to achieve automated execution of tasks. The power supply voltage regulator module is connected to the main control board and steps down the DC power supply before supplying power to the main control board. The communication module connects to the main control board and is used to enable pairing and communication between the main control board and external devices.

[0007] Secondly, to solve the above problems, the present invention provides a control method for an intelligent rescue robot vehicle, based on the intelligent rescue robot vehicle described in the first aspect, the control method comprising the following steps: S1: Identify the rescue target and obtain its location; S2: Based on the starting position of the robot car and the position of the rescue target, perform path planning to determine the preset path of the robot car; S3: Based on the preset path, plan the running speed trajectory of the robot car, obtain the current position of the robot car in real time, realize global positioning of the robot car, and then calculate the linear velocity of the car required for the trajectory movement based on the preset path and the current position information of the robot car, so that the actual movement is consistent with the preset path. S4: Real-time acquisition of the robot's current attitude information, including the robot's heading angle; S5: Based on the current attitude information, obtain the angle deviation between the current attitude and the target attitude, and convert the angle deviation into the angular velocity of the trolley required for attitude adjustment; S6: Perform inverse kinematics calculation on the angular velocity of the trolley required for attitude adjustment and the linear velocity of the trolley required for trajectory adjustment to obtain the target speed of each motor; S7: Drive the robot car to move toward the rescue target at the target speed, detect obstacles in real time, generate an obstacle avoidance strategy and control the robot car to avoid obstacles; S8: When the robot car approaches the rescue target, control the mechanical claw on the robot car to grasp the rescue target; S9: Once the rescue target is captured, identify the safe zone and control the robot car to transfer the captured rescue target to the safe zone.

[0008] In one embodiment of the present invention, in step S3, the specific method for planning the running speed trajectory of the robot car according to the preset path includes trapezoidal speed planning, triangular speed planning, and speed planning based on PID output and gradient limiting. The trapezoidal speed planning includes a start-up acceleration phase, a constant speed steady phase, and a deceleration and stopping phase. The triangular speed planning includes a start-up acceleration phase and a deceleration and stopping phase. The speed planning based on PID output and gradient limiting includes generating a target speed through a position loop PID controller based on the position error between the current position and the target position, and limiting the target speed through a gradient limiter.

[0009] In one embodiment of the present invention, the specific method for obtaining the current position of the robot car in real time in step S4 is as follows: The real-time displacement and heading angle of the robot are obtained by performing forward kinematics calculations using the pulse signals fed back from the motor encoder on the motor. The angular velocity integral data of the robot is obtained through the gyroscope module to compensate for the cumulative error in the forward kinematics calculation. The current position information of the robot is obtained by fusing the forward kinematics data and the data acquired by the gyroscope module using the Kalman filter algorithm.

[0010] In one embodiment of the present invention, in step S5, the speed of the motor required for trajectory adjustment is calculated using a PID algorithm based on the preset path and the current position information of the robot car, so that the actual movement is consistent with the preset path. The specific method is as follows: Calculate the velocity value at each time point based on the velocity and acceleration curves generated by the preset path; Real-time acquisition of the robot's current position and speed; Calculate the positional deviation between the current position and the target position; The required motor speed for trajectory adjustment is obtained using a PID algorithm based on the positional deviation.

[0011] The robot car moves toward the target position at the adjusted motor speed; The above process is repeated in each control cycle until the robot reaches the target position or the deviation reaches the preset value.

[0012] In one embodiment of the present invention, in step S5, based on the preset path and the current position information of the robot car, the calculation expression for the motor speed required for trajectory adjustment is calculated using a PID algorithm as follows: ; Where V1 is the control output at time k; The difference between the current position and the target text position. It is the angle difference from the previous control cycle, K. p K is the proportionality coefficient. i K is the integral coefficient; d Δt is the differential coefficient, and Δt is the sampling time interval.

[0013] In one embodiment of the present invention, in step S7, the calculation expression for the motor speed difference is obtained by using a PID algorithm to obtain the angle deviation as follows: ; Where V2 is the control output at time k; The angle difference between the current position and the target position. It is the angle difference from the previous control cycle, K. p K is the proportionality coefficient. i K is the integral coefficient; d Δt is the differential coefficient, and Δt is the sampling time interval.

[0014] In one embodiment of the present invention, when the robot car is controlled to grab the rescue target in step S8, the color of the rescue target is identified by the image processing system, and the target is grabbed in order of its score.

[0015] In one embodiment of the present invention, the specific method for identifying the color of the rescue target through an image processing system and capturing the target in order of its score includes the following steps: Acquire image information of the rescue target; Identify the color of the rescue target using image processing algorithms; Use the YOLOv5 model to identify the location of the rescue target; Based on the score of the rescue target, priority is given to identifying rescue targets with higher scores.

[0016] Compared with the prior art, the above-described technical solution of the present invention has the following advantages: The intelligent rescue robot vehicle described in this invention can move flexibly in complex environments and quickly reach the rescue target location, thereby improving rescue efficiency. The gimbal vision mechanism allows the robot vehicle to freely adjust the camera angle in three-dimensional space to achieve all-round monitoring, which can significantly improve the trajectory tracking ability and task execution accuracy of the intelligent rescue robot vehicle in complex environments, thus playing a greater role in critical tasks such as emergency rescue. The mechanical claw design enables the robot vehicle to grasp and transport rescue targets, enhancing the robot's ability to perform diverse tasks.

[0017] The present invention discloses a control method for an intelligent rescue robot vehicle. Through real-time attitude control, the robot vehicle's attitude is adjusted to ensure that it can travel precisely along a predetermined path. This achieves the goal of maintaining consistent starting and ending angles and correcting yaw during straight-line travel. Through trajectory planning control, the robot vehicle is ensured to travel accurately to the predetermined target point, enabling the robot to reach the rescue target quickly and directly, reducing unnecessary movement and time consumption, thereby improving rescue efficiency and enabling it to perform rescue tasks efficiently, accurately, and safely in various complex environments. Attached Figure Description

[0018] To make the content of this invention easier to understand, the invention will be further described in detail below with reference to specific embodiments and accompanying drawings.

[0019] Figure 1 This is a schematic diagram of the structure of the intelligent rescue robot vehicle in a preferred embodiment of the present invention; Figure 2 This is a structural block diagram of the control system in a preferred embodiment of the present invention; Figure 3 This is a flowchart of the control method for the intelligent rescue robot vehicle in a preferred embodiment of the present invention; Figure 4 This is a schematic diagram of trapezoidal trajectory planning in a preferred embodiment of the present invention; Figure 5This is a schematic diagram of the triangular velocity planning in a preferred embodiment of the present invention; Figure 6 This is a block diagram illustrating the principle of trajectory control and attitude control in a preferred embodiment of the present invention; Figure 7 This is a schematic diagram of the rescue target, departure area, and safe zone in a preferred embodiment of the present invention; Figure 8 This is a functional flowchart of the intelligent rescue robot vehicle of the present invention; Explanation of reference numerals in the accompanying drawings: 1. Chassis moving mechanism; 11. Chassis; 12. Omnidirectional wheel; 13. Motor; 2. Gimbal vision mechanism; 21. Gimbal; 22. Camera; 31. Claw drive mechanism; 32. Claw drive connector; 33. Claw. Detailed Implementation

[0020] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand and implement the present invention. However, the embodiments described are not intended to limit the present invention.

[0021] Reference Figure 1 As shown, the present invention provides an intelligent rescue robot vehicle, including: a chassis moving mechanism 1, including a chassis 11, omnidirectional wheels 12 and a motor 13; the motor 13 is disposed on the lower side of the chassis 11, and the output shaft of the motor 13 is connected to the omnidirectional wheels 12; wherein the omnidirectional wheels 12 include 3, and the included angle between the axes of adjacent omnidirectional wheels 12 is 120°; The gimbal vision mechanism 2 includes a gimbal 21 and a camera 22. The gimbal 21 is positioned at the center of the chassis 11, and the camera 22 is mounted on the gimbal 21. The gimbal 21 is controlled by two servo motors to achieve horizontal and vertical rotation. The center position of the gimbal 21 stabilizes the vehicle structure, reduces the vehicle's rotational inertia about its central axis, lowers energy consumption, and ensures the vehicle's balance. The gimbal 21 can freely adjust the pitch angle and horizontal rotation of the camera 22 as needed, providing a more flexible shooting perspective to adapt to different scenarios. It is also equipped with shock absorption and vibration damping functions to maintain camera stability during movement or environmental vibrations, ensuring image quality. By rotating and tilting the gimbal 21, the camera 22 can achieve 360-degree all-round monitoring, covering a wider area, reducing blind spots, and ensuring that the center position of the camera 22 is always facing the straight-line direction of the robot car, which facilitates accurate determination of the location of the rescue target and makes it easy to adjust the direction of the robot car's movement. The gimbal 21 is cleverly controlled by two servo motors and has two degrees of freedom in space, which can capture all targets within the field of view and provide clear and accurate visual guidance for the movement of the car. A mechanical gripper 3, mounted on the chassis 11, is used to grasp a target object. The mechanical gripper 3 includes a gripper finger drive mechanism 31, a gripper finger drive connector 32, and grippers 33. The gripper finger drive mechanism 31 is connected to the grippers 32 via the gripper finger drive connector 32. The gripper finger drive mechanism 31 includes a servo motor, which provides the opening and closing power for the mechanical gripper. By controlling the rotation angle of the servo motor, the mechanical gripper opens or closes, thereby achieving the gripping of the target object. The control system is used to control the working status of the chassis moving mechanism 1, the gimbal vision mechanism 2, and the mechanical claw 3.

[0022] like Figure 2 As shown, the control system includes: The Maixcam module is used for real-time image acquisition and processing, identifying rescue targets of different colors. Within the Maixcam module, the LAB color space is used for color recognition and tracking tasks. The LAB color space can better distinguish colors because it processes color information and brightness information separately, making color recognition more stable under different lighting conditions. By setting thresholds for a and b values, specific colors can be effectively identified, effectively handling color recognition in complex environments. Using the Find blobs application in Maixcam, a suitable color threshold range can be quickly determined. After testing under different lighting conditions, the corresponding color thresholds for three objects are obtained. The gyroscope module is used to acquire the robot's attitude information for attitude control and navigation assistance. It integrates a high-precision crystal gyroscope, employs a high-performance microprocessor and advanced dynamic calculation and Kalman dynamic filtering algorithms, enabling rapid calculation of the module's current real-time motion attitude. Advanced digital filtering technology effectively reduces measurement noise and improves measurement accuracy. The gyroscope module integrates an attitude solver, which, combined with the dynamic Kalman filtering algorithm, accurately outputs the module's current attitude in dynamic environments, achieving an attitude measurement accuracy of 0.1 degrees and extremely high stability. The TOF ranging module is used for real-time measurement of the distance between the intelligent rescue robot and surrounding obstacles, as well as the distance between the intelligent rescue robot and the rescue target. The preferred TOF ranging module uses the TOF050C laser rangefinder, which provides measurement accuracy up to millimeter level, making it ideal for applications requiring precise distance measurement. The TOF050C offers fast measurement speed, completing multiple measurements in a short time, suitable for real-time monitoring and measurement in dynamic environments. The laser rangefinder effectively resists ambient light interference, especially in environments with strong light or multiple reflections, maintaining measurement stability and reliability. The optical flow sensor module uses changes in adjacent frames to acquire displacement information, enabling positioning and obstacle avoidance. The optical flow sensor can quickly process image information and provide real-time feedback, allowing the device to respond instantly to environmental changes and achieve smoother motion control. In addition to positioning, the optical flow sensor can also be used to detect motion and achieve obstacle avoidance, improving the intelligence level of the device. The PWM servo drive module is used to simultaneously control the rotation angle and speed of multiple servos. The PWM servo drive module uses the PCA9685, and the multi-channel PWM output enables the simultaneous control of multiple servos, enhancing the scalability and flexibility of the system. The motor drive module is used to adjust the output torque and speed of the motor. It employs the FOC vector closed-loop control algorithm, with three-loop control of torque, speed, and position. It features an onboard industrial-grade high-precision 16384-line magnetic encoder and a precision current sensor, supporting pulse control and serial communication control. The closed-loop control system monitors the motor's position in real time through a feedback mechanism, ensuring that the motor accurately reaches the set position.

[0023] The main control board exchanges data with the Maixcam module, gyroscope module, TOF ranging module, optical flow sensor module, PWM servo drive module, and motor drive module to achieve automated execution of tasks. The power supply voltage regulator module is connected to the main control board and steps down the DC power supply before supplying power to the main control board. The communication module connects to the main control board and is used to enable pairing and communication between the main control board and external devices. Preferably, a Bluetooth communication module is used.

[0024] The main control board includes: The trajectory planning and control module is used to calculate the output speed based on the error between the target position and the current position using a PID algorithm, so that the robot can accurately reach the target position. The attitude control module is used to calculate the output speed based on the deviation between the target angle and the current angle of the robot car using a PID algorithm, so that the robot car can adjust to the target angle. The forward kinematics module is used to calculate the velocity components of the robot car in the car coordinate system based on the robot car's orientation angle and target angle, and further calculate the velocity of each omnidirectional wheel; The global positioning module is used to calculate the displacement of the robot in the global coordinate system based on the displacement of the motor encoder, using transformation and rotation matrices.

[0025] like Figure 3As shown, the present invention provides a control method for an intelligent rescue robot vehicle, implemented based on the aforementioned intelligent rescue robot vehicle. The control method includes the following steps: S1: Identify the rescue target and obtain its location; S2: Based on the starting position of the robot car and the position of the rescue target, perform path planning to determine the preset path of the robot car; S3: Based on the preset path, plan the running speed trajectory of the robot car, obtain the current position of the robot car in real time, realize global positioning of the robot car, and then calculate the linear velocity of the car required for the trajectory movement based on the preset path and the current position information of the robot car, so that the actual movement is consistent with the preset path. S4: Real-time acquisition of the robot's current attitude information, including the robot's heading angle; S5: Based on the current attitude information, obtain the angle deviation between the current attitude and the target attitude, and convert the angle deviation into the angular velocity of the trolley required for attitude adjustment; S6: Perform inverse kinematics calculation on the angular velocity of the trolley required for attitude adjustment and the linear velocity of the trolley required for trajectory adjustment to obtain the target speed of each motor; S7: Drive the robot car to move toward the rescue target at the target speed, detect obstacles in real time, generate an obstacle avoidance strategy and control the robot car to avoid obstacles; S8: When the robot car approaches the rescue target, the mechanical claw 3 on the robot car is controlled to grasp the rescue target; the specific method includes the following steps: Acquire image information of the rescue target; Identify the color of the rescue target using image processing algorithms; Use the YOLOv5 model to identify the location of the rescue target; Based on the scores of the rescue targets, prioritize identifying rescue targets with higher scores; Control the robot car to move to the rescue target location and drive the mechanical claw 3 to grab the rescue target.

[0026] S9: Once the rescue target is captured, identify the safe zone and control the robot car to transfer the captured rescue target to the safe zone.

[0027] In order for the robot car to stop accurately at the predetermined target point during its movement and reduce the time required for visual recognition correction, the robot car needs to accelerate and decelerate smoothly. By using straight trajectory planning, the problem of inconsistency between the robot car's current position and the target position of the trajectory planning can be solved. When the current position is behind the target position, it needs to accelerate to catch up; when the current position is ahead of the target position, it needs to accelerate to adjust.

[0028] In step S3, the specific methods for planning the running speed trajectory of the robot car according to the preset path include trapezoidal speed planning, triangular speed planning, and speed planning based on PID output and gradient limiting. The trapezoidal speed planning includes a start-up acceleration phase, a constant speed steady phase, and a deceleration and stopping phase. The triangular speed planning includes a start-up acceleration phase and a deceleration and stopping phase. The speed planning based on PID output and gradient limiting includes generating a target speed through a position loop PID controller based on the position error between the current position and the target position, and limiting the target speed through a gradient limiter.

[0029] The specific method for planning the trapezoidal and triangular velocities of the robot car based on the preset path is as follows: Obtain the distance between the rescue target and the starting position of the robot vehicle, and label it as s; Set the maximum speed and acceleration of the robot car; The acceleration distance s1 during the acceleration phase is calculated based on the maximum speed and acceleration. Determine if the distance s is greater than twice the value of s1; If so, then perform trapezoidal trajectory planning; If not, then execute triangle velocity planning; like Figure 4 As shown, a trapezoidal trajectory planning diagram is displayed; in trajectory planning, the time nodes of each stage of trajectory planning are deduced based on the velocity value, acceleration value and target distance.

[0030] Known maximum allowable acceleration The distance s between the rescue target and the starting position of the robot car is divided into three stages: the starting acceleration stage, the uniform and stable stage, and the deceleration and stopping stage. In order for the robot car to accelerate to its maximum speed with a specified acceleration, a certain acceleration distance s1 is required, as shown in the following formula: ; Because the acceleration is the same during the acceleration and deceleration phases, a distance twice the required distance needs to be reserved for the robot car's acceleration and deceleration.

[0031] When distance At this point, the robot has sufficient acceleration distance to reach its maximum speed, conforming to the characteristics of a trapezoidal graph for trajectory planning. The specific values ​​at each time point can then be calculated, as shown in the following formula:

[0032] in To speed up the process, This is the distance during the uniform velocity phase. The total time for acceleration and constant speed. Let the total motion time be: Acceleration phase Target speed Target location ; Uniform speed phase Target speed Target location ; deceleration phase Target speed Target location .

[0033] When distance At times, the robotic car cannot accelerate to its maximum speed according to the predetermined trajectory. For this special short-distance movement, a triangular velocity planning method is used, such as... Figure 5 As shown, its motion consists of only two phases: acceleration and deceleration. The magnitudes of the acceleration and deceleration phases are set to be... In opposite directions, the maximum speed is .

[0034] Acceleration phase Target speed Target location .

[0035] deceleration phase Target speed Target location .

[0036] The trapezoidal / triangle velocity planning described above provides the shortest trajectory under extreme acceleration and velocity constraints. The acceleration is bounded, continuous, and shock-free. The entire curve is completely predefined, facilitating real-time tracking and monitoring.

[0037] Speed ​​planning based on PID output and gradient limiting is achieved by using a position loop PID controller to output the theoretical target speed. Subsequently, a gradient limiter is used to complete the trapezoidal programming in one step, yielding a real-time, smooth, and physically executable speed v_cmd. The specific method for obtaining v_cmd is as follows: Sample the current position error: At the beginning of each control cycle ΔT, read the planned position and the measured position, and calculate the error e(k); Calculating the theoretical target speed: The error e(k) is fed into the position loop PID controller, which calculates a raw control output u_pid(t). This output value u_pid(t) is directly interpreted and used as the theoretical target speed v_target(t) required to eliminate the error, i.e. ; The output of the position loop PID controller is in the dimension of speed, such as m / s or rpm; To calculate the velocity increment: Subtract the actual velocity v_cmd(k−1) emitted in the previous cycle from v_target(k) to obtain the required velocity change Δv(k); that is... Δv(k) = v_target [k] - v_cmd [k-1]

[0038] Gradient limiting: If Δv > Δv_max, then v_cmd[k] = v_cmd[k-1] + Δv_max achieves positive amplitude limiting and uniform upward movement; If Δv <- Δv_max, then v_cmd[k] = v_cmd[k-1] - Δv_max, achieving negative amplitude limiting and uniform descent; In other cases, v_cmd[k] = v_target[k], and no amplitude limiting is required.

[0039] Where k is the discrete time period index; ΔT is the control period; e[k] is the position error of the k-th period; Kp is the proportional coefficient; Ki is the integral coefficient; Kd is the differential coefficient; v_target[k] is the theoretical target speed; v_cmd[k] is the actual speed; Δv is the speed command increment; and Δv_max is the maximum allowable speed change per period.

[0040] In this embodiment, attitude control and trajectory planning are executed simultaneously in actual control, belonging to parallel PID control. After attitude control and trajectory planning are completed, the pre-set target speed command for each omnidirectional wheel 12 is sent to the motor control module, thereby realizing the speed control of the motor 13. The control flow is as follows: Figure 6 As shown.

[0041] In step S4, the specific method for obtaining the current position of the robot car in real time is as follows: S41: The forward kinematics calculation is performed using the pulse signal fed back from the motor encoder on motor 13 to obtain the real-time displacement and heading angle of the robot car; S42: Obtain the integral data of the robot's angular velocity through the gyroscope module to compensate for the cumulative angle error in the forward kinematics calculation; S43: By using the Kalman filter algorithm, the current position information of the robot is obtained by fusing the forward kinematics data and the data acquired by the gyroscope module.

[0042] The specific steps in S41 for calculating the real-time displacement and heading angle of the robot car using the pulse signal fed back from the motor encoder on motor 13 are as follows: Data acquisition: Read the encoder increment values ​​of the three drive motors per unit time: Δθ1, Δθ2, Δθ3.

[0043] Incremental calculation in the robot's body coordinate system: Using the linear transformation matrix M (based on the kinematic constraints of the three wheels with a 120° symmetrical distribution), the encoder increment is converted into displacement and rotation increments in the robot's body coordinate system. ; World coordinate system transformation: Based on the robot's current heading angle θ, the translation increment in the body coordinate system is transformed to the world coordinate system using the rotation matrix R(Δθ). ; Pose update: The calculated world coordinate system displacement increments ΔXw, ΔYw and rotation increments Δθ are used to update the robot's real-time pose in the world coordinate system.

[0044] By integrating forward and inverse kinematics calculations and coordinate transformations, real-time, high-frequency autonomous pose estimation relying solely on a motor encoder is achieved. The core algorithm inherently supports three-degree-of-freedom motion, easily realizing complex movements including lateral movement, oblique movement, in-situ rotation, and arbitrary combinations of "walking and turning."

[0045] In step S5, based on the preset path and the current position information of the robot car, the speed of motor 13 required for trajectory adjustment is calculated using a PID algorithm to ensure that the actual movement is consistent with the preset path. The specific method is as follows: Calculate the velocity value at each time point based on the velocity and acceleration curves generated by the preset path; Real-time acquisition of the robot's current position and speed; Calculate the positional deviation between the current position and the target position; The speed of motor 13 required for trajectory adjustment is obtained using a PID algorithm based on the position deviation.

[0046] The robot car moves toward the target position at the adjusted speed of motor 13; The above process is repeated in each control cycle until the robot reaches the target position or the deviation reaches the preset value.

[0047] In step S5, based on the preset path and the current position information of the robot, the calculation expression for the motor speed required for trajectory adjustment is calculated using a PID algorithm as follows: ; Where V1 is the control output at time k; The difference between the current position and the target position. It is the angle difference from the previous control cycle, K. p K is the proportionality coefficient. i K is the integral coefficient; d Δt is the differential coefficient, and Δt is the sampling time interval.

[0048] In actual operation, it was found that excessive speed or uneven acceleration and deceleration would make it difficult for the robot to maintain a straight line. In order to keep the robot running and allow the robot that has deviated to readjust its posture and resume straight-line movement, it is necessary to adjust the speed difference of the wheels to change the angle of the robot. The robot car's offset angle is obtained by collecting data from the gyroscope module. The deviation between the obtained angle and the target angle is used as the input of the PID controller. This deviation value is converted into a speed difference and used as the input value of the motor controller, thereby changing the motor speed and realizing real-time speed regulation.

[0049] In order to periodically obtain the speed of motor 13 and the gyroscope offset angle and adjust the speed of motor 13, the timer in STM32 needs to be enabled. Every once in a while, the timer interrupt calls the angle PID control function. In each cycle, the current offset angle and current speed are obtained and the target speed of omnidirectional wheel 12 is calculated. The command to set the target speed is then sent to the motor control module.

[0050] In step S6, the inverse kinematics calculation is performed on the angular velocity of the trolley required for attitude adjustment and the linear velocity of the trolley required for trajectory adjustment to obtain the target velocity of each motor. The specific method is as follows: Set the forward speed (aim_speed), rotation speed (spin_speed), and heading angle (yaw_h) in the chassis coordinate system. First, the resultant velocity aim_speed is decomposed into X and Y axis velocities (vx, vy) in the chassis coordinate system according to the direction angle yaw_h.

[0051] Based on the mechanical structure of the three wheels evenly distributed at 120°, the linear velocity and angular velocity are superimposed to calculate the target velocity of each wheel: wheel1 = Vx + spin_speed; wheel2=-sin(30°)*vx+ cos(30°)*vy+ spin_speed; wheel3= -sin(30°)*vx- cos(30°)*vy+ spin_speed; This solution enables the robot to perform translational and rotational movements simultaneously, achieving "walking and turning at the same time." Since inverse kinematics is existing technology, it will not be elaborated upon in this application.

[0052] In step S7, the calculation expression for the motor speed difference obtained by the attitude control PID control of the angle deviation is as follows: ; Among them, u k This is the control output at time k; The angle difference between the current position and the target position. It is the angle difference from the previous control cycle, K. p K is the proportionality coefficient. i K is the integral coefficient; d Δt is the differential coefficient, and Δt is the sampling time interval.

[0053] In this embodiment, there are three omnidirectional wheels 12, each of which is independently controlled by a motor 13. Taking the three omnidirectional wheels 12 as an example, the acquisition of the target speed of the motor 13 is explained as follows: The current orientation angle α of the vehicle is obtained by integrating the gyroscope, and the angle γ in the vehicle coordinate system is obtained by subtracting the vehicle orientation angle α from the target angle β.

[0054] Decompose the velocity: vx = v1 × sin(γ); vy = v1 × cos(γ); Speed ​​calculation for the three omnidirectional wheels 12: ; Then, the speeds of the three omnidirectional wheels are linearly superimposed using v2 to obtain the target speeds of the three motors 13 of the robot car.

[0055] Then, the speeds of the three omnidirectional wheels are linearly superimposed using v2 to obtain the target speeds of the three motors 13 of the robot car.

[0056] Figure 7 The diagram illustrates a rescue scenario for planning and simulating rescue operations. The robotic vehicle departs from the starting area, reaches the rescue target, grabs the target, and then transports it to a safe area. Figure 8 This demonstrates the complete decision-making process of the robotic vehicle, from image recognition and motion control to global positioning, ensuring that the robot can effectively perform emergency rescue tasks. The decision-making process of the intelligent rescue robotic vehicle, starting from startup, consists of three main steps: First, the robot uses the YOLOv5 image processing system to perform target recognition, determining whether the identified target is the team's rescue objective. If so, the system transmits the target's location information via serial port; otherwise, no further action is taken. Color recognition is crucial in computer vision and image processing, and common methods include using thresholding techniques to identify and distinguish different colors. Secondly, the control system is responsible for the attitude control and trajectory planning of the robot's chassis 11. The attitude control achieves the robot's center stability through angle PID control. The trajectory planning includes trapezoidal and acceleration / deceleration, as well as speed trajectory deviation correction. According to the correction results, if the speed trajectory deviation is less than 5 revolutions per second, the system will perform speed PID control; if the deviation is greater than or equal to 5 revolutions per second, the three motors 13 will be independently controlled to obtain proportional torque and avoid slippage, so as to achieve the straight-line movement of the robot.

[0057] Finally, the robot is positioned by using the positive solution value fed back from the integrating motor and the displacement value integrated by the gyroscope, and the precise coordinates of the robot are obtained through data fusion.

[0058] Obviously, the above embodiments are merely illustrative examples for clear explanation and are not intended to limit the implementation. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is neither necessary nor possible to exhaustively list all possible implementations here. However, obvious variations or modifications derived therefrom are still within the scope of protection of this invention.

Claims

1. An intelligent rescue robot vehicle, characterized in that: include: A chassis moving mechanism includes a chassis, omnidirectional wheels, and a motor; the motor is located on the underside of the chassis, and the output shaft of the motor is connected to the omnidirectional wheels; A gimbal vision mechanism includes a gimbal and a camera; the gimbal is located at the center of the chassis, the camera is mounted on the gimbal, and the gimbal is controlled by two servo motors to achieve horizontal and pitch rotation. A mechanical gripper, mounted on the chassis, is used to grasp a target object. The mechanical gripper includes a gripper finger drive mechanism, a gripper finger drive connector, and grippers. The gripper finger drive mechanism is connected to the grippers via the gripper finger drive connector. The gripper finger drive mechanism includes a servo motor, which provides the opening and closing power for the mechanical gripper. By controlling the rotation angle of the servo motor, the mechanical gripper opens or closes, thereby achieving the gripping of the target object. The control system is used to control the working status of the chassis moving mechanism, the gimbal vision mechanism, and the robotic gripper.

2. The intelligent rescue robot vehicle according to claim 1, characterized in that: The control system includes: The Maixcam module is used for real-time image acquisition and processing to identify rescue targets of different colors; The gyroscope module is used to acquire the robot's attitude information for attitude control and navigation assistance. The TOF ranging module is used to measure the distance between the intelligent rescue robot and surrounding obstacles in real time, as well as the distance between the intelligent rescue robot and the rescue target. The optical flow sensor module uses changes in adjacent frames to obtain displacement information, enabling localization and obstacle avoidance. PWM servo drive module, used to simultaneously control the rotation angle and speed of multiple servos; The motor drive module is used to adjust the output torque and speed of the motor; The main control board exchanges data with the Maixcam module, gyroscope module, TOF ranging module, optical flow sensor module, PWM servo drive module, and motor drive module to achieve automated execution of tasks. The power supply voltage regulator module is connected to the main control board and steps down the DC power supply before supplying power to the main control board. The communication module connects to the main control board and is used to enable pairing and communication between the main control board and external devices.

3. A control method for an intelligent rescue robot vehicle, implemented based on the intelligent rescue robot vehicle according to any one of claims 1-2, characterized in that: The control method includes the following steps: S1: Identify the rescue target and obtain its location; S2: Based on the starting position of the robot car and the position of the rescue target, perform path planning to determine the preset path of the robot car; S3: Based on the preset path, plan the running speed trajectory of the robot car, obtain the current position of the robot car in real time, realize global positioning of the robot car, and then calculate the linear velocity of the car required for the trajectory movement based on the preset path and the current position information of the robot car, so that the actual movement is consistent with the preset path. S4: Real-time acquisition of the robot's current attitude information, including the robot's heading angle; S5: Based on the current attitude information, obtain the angle deviation between the current attitude and the target attitude, and convert the angle deviation into the angular velocity of the trolley required for attitude adjustment; S6: Perform inverse kinematics calculation on the angular velocity of the trolley required for attitude adjustment and the linear velocity of the trolley required for trajectory adjustment to obtain the target speed of each motor; S7: Drive the robot car to move toward the rescue target at the target speed, detect obstacles in real time, generate an obstacle avoidance strategy and control the robot car to avoid obstacles; S8: When the robot car approaches the rescue target, control the mechanical claw on the robot car to grasp the rescue target; S9: Once the rescue target is captured, identify the safe zone and control the robot car to transfer the captured rescue target to the safe zone.

4. The control method for an intelligent rescue robot vehicle according to claim 3, characterized in that: In step S3, the specific methods for planning the running speed trajectory of the robot car according to the preset path include trapezoidal speed planning, triangular speed planning, and speed planning based on PID output and gradient limiting. The trapezoidal speed planning includes a start-up acceleration phase, a constant speed steady phase, and a deceleration and stopping phase. The triangular speed planning includes a start-up acceleration phase and a deceleration and stopping phase. The speed planning based on PID output and gradient limiting includes generating a target speed through a position loop PID controller based on the position error between the current position and the target position, and limiting the target speed through a gradient limiter.

5. The control method for an intelligent rescue robot vehicle according to claim 3, characterized in that: In step S3, the specific method for obtaining the current position of the robot car in real time is as follows: The real-time displacement and heading angle of the robot are obtained by performing forward kinematics calculations using the pulse signals fed back from the motor encoder on the motor. The angular velocity integral data of the robot is obtained through the gyroscope module to compensate for the cumulative error in the forward kinematics calculation. The current position information of the robot is obtained by fusing the forward kinematics data and the data acquired by the gyroscope module using the Kalman filter algorithm.

6. The control method for an intelligent rescue robot vehicle according to claim 3, characterized in that: In step S3, based on the preset path and the current position information of the robot, the speed of the motor required for trajectory adjustment is calculated using a PID algorithm to ensure that the actual movement is consistent with the preset path. The specific method is as follows: Calculate the velocity value at each time point based on the velocity and acceleration curves generated by the preset path; Real-time acquisition of the robot's current position and speed; Calculate the positional deviation between the current position and the target position; Based on the position deviation, a PID algorithm is used to obtain the motor speed required for trajectory adjustment; The robot car moves toward the target position at the adjusted motor speed; The above process is repeated in each control cycle until the robot reaches the target position or the deviation reaches the preset value.

7. The control method for an intelligent rescue robot vehicle according to claim 6, characterized in that: In step S3, based on the preset path and the current position information of the robot, the calculation expression for the motor speed required for trajectory adjustment is calculated using a PID algorithm as follows: ； Where V1 is the control output at time k; The angle difference between the current position and the target position. It is the angle difference from the previous control cycle, K. p K is the proportionality coefficient. i K is the integral coefficient; d Δt is the differential coefficient, and Δt is the sampling time interval.

8. The control method for an intelligent rescue robot vehicle according to claim 3, characterized in that: In step S5, the calculation expression for the motor speed difference is obtained by using a PID algorithm to obtain the angle deviation, as follows: ； Where V2 is the control output at time k; The angle difference between the current position and the target position. It is the angle difference from the previous control cycle, K. p K is the proportionality coefficient. i K is the integral coefficient; d Δt is the differential coefficient, and Δt is the sampling time interval.

9. The control method for an intelligent rescue robot vehicle according to claim 3, characterized in that: In step S8, when the robot car is controlling the rescue target to grab it, the color of the rescue target is identified by the image processing system, and the target is grabbed in order of its score.

10. The control method for an intelligent rescue robot vehicle according to claim 9, characterized in that: The specific method for identifying the color of rescue targets using an image processing system and capturing them in order of their scores includes the following steps: Acquire image information of the rescue target; Identify the color of the rescue target using image processing algorithms; Use the YOLOv5 model to identify the location of the rescue target; Based on the score of the rescue target, priority is given to identifying rescue targets with higher scores.

Images