Intelligent rescue robot dolly and control method

Abstract

The application relates to an intelligent rescue robot trolley and a control method, the intelligent rescue robot trolley comprising: a chassis moving mechanism comprising a chassis, an omni-directional wheel and a motor; a cloud platform vision mechanism comprising a cloud platform and a camera; a 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 a claw finger, the claw finger driving mechanism is connected with the claw finger through the claw finger driving connecting piece; the claw finger driving mechanism comprises a rudder motor, the rotation angle of the rudder motor is controlled, the mechanical claw is opened or closed, and the mechanical claw is used for clamping a target object; a control system is used for controlling the working state of the chassis moving mechanism, the cloud platform vision module and the mechanical claw. The application can reach a rescue target quickly and directly without yawing through posture control and trajectory planning, unnecessary movement and time consumption are reduced, rescue efficiency is improved, rescue tasks can be efficiently, accurately and safely executed in various complex environments.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of intelligent robots, in particular to an intelligent rescue robot vehicle and a control method. BACKGROUND

[0002] With the rapid development of technology, intelligent rescue robots are increasingly used in disaster rescue, emergency medical care, industrial accidents and other fields. These robots need to perform high-speed movement, precise obstacle avoidance, target collection and transportation in complex and variable environments, while having the ability of autonomous or remote control. In the design and application of intelligent rescue robot vehicles, it is crucial to ensure that they can accurately follow the predetermined trajectory when performing tasks.

[0003] When the robot vehicle moves in a straight line, it needs to ensure that its starting angle and ending angle are consistent and do not deviate from the center line of the robot vehicle's initial movement. However, there are many factors that can cause the vehicle to deviate from the center line during actual testing; intelligent rescue robot vehicles may deviate from the predetermined trajectory due to various reasons during straight-line movement; these reasons include but are not limited to mechanical structure imbalance, wear and tear, uneven power output of the drive motor, as well as environmental factors such as uneven ground, sliding or slope changes. Traditional control methods may not effectively handle dynamic environmental conditions and lack real-time adjustment and adaptive mechanisms, resulting in a slow response when facing unexpected situations. SUMMARY

[0004] To this end, the technical problem to be solved by the present application is to overcome the shortcomings of the prior art and provide an intelligent rescue robot vehicle and a control method that can reach the rescue target quickly and directly without deviation through posture control and trajectory planning, reducing unnecessary movement and time consumption, improving rescue efficiency, and enabling efficient, accurate and safe execution of rescue tasks in various complex environments.

[0005] In a first aspect, to solve the above technical problems, the present application provides an intelligent rescue robot vehicle, comprising:

[0006] A chassis moving mechanism comprising a chassis, an omni-directional wheel and a motor; the motor is arranged on the lower side of the chassis, and the output shaft of the motor is connected with the omni-directional wheel;

[0007] A gimbal vision mechanism comprising a gimbal and a camera; the gimbal is arranged at the center position of the chassis, the camera is installed on the gimbal, and the gimbal is controlled by two servos to realize horizontal rotation and pitch rotation;

[0008] A mechanical claw is arranged on the chassis and used for grabbing a target object; the mechanical claw comprises a claw finger driving mechanism, a claw finger driving connecting piece and a claw finger, and the claw finger driving mechanism is connected with the claw finger through the claw finger driving connecting piece; the claw finger driving mechanism comprises a rudder machine, the rudder machine provides the opening and closing power of the mechanical claw, the mechanical claw is opened or closed by controlling the rotation angle of the rudder machine, so that the target object is clamped;

[0009] A control system is used for controlling the working states of the chassis moving mechanism, the holder visual mechanism and the mechanical claw.

[0010] In an embodiment of the present application, the control system comprises:

[0011] A Maixcam module is used for real-time image acquisition and processing, and identification of different colors of rescue targets;

[0012] A gyroscope module is used for acquiring the attitude information of the robot car, and performing attitude control and navigation assistance;

[0013] A TOF ranging module is used for measuring the distance between the intelligent rescue robot car and surrounding obstacles in real time, and measuring the distance between the intelligent rescue robot car and a rescue target;

[0014] An optical flow sensor module is used for acquiring displacement information through the change of adjacent frame images, and realizing positioning and obstacle avoidance;

[0015] A PWM rudder machine driving module is used for controlling the rotation angle and speed of multiple rudder machines at the same time;

[0016] A motor driving module is used for adjusting the output torque and speed of the motor;

[0017] A main control board exchanges data with the Maixcam module, the gyroscope module, the TOF ranging module, the optical flow sensor module, the PWM rudder machine driving module and the motor driving module, and realizes automatic execution of tasks;

[0018] A power stabilizing module is connected with the main control board, and supplies power to the main control board after step-down processing of a direct current power supply;

[0019] A communication module is connected with the main control board, and is used for realizing the pairing and communication of the main control board and external equipment.

[0020] The second aspect is to solve the above problems, and the present application provides a control method of an intelligent rescue robot car, which is realized based on the intelligent rescue robot car of the first aspect, and the control method comprises the following steps:

[0021] S1: identifying a rescue target and acquiring the position of the rescue target;

[0022] S2: Path planning according to the starting position of the robot car and the position of the rescue target to determine the preset path of the robot car;

[0023] S3: According to the preset path, the running speed trajectory of the robot car is planned, and the current position of the robot car is obtained in real time, the global positioning of the robot car is realized, then according to the preset path and the current position information of the robot car, the required car linear speed of trajectory motion is calculated, so that the actual motion is consistent with the preset path;

[0024] S4: Real-time acquisition of the current attitude information of the robot car, the attitude information includes the heading angle of the robot car;

[0025] S5: According to the current attitude information, the angle deviation between the current attitude and the target attitude is obtained, and the angle deviation is converted into the required car angular velocity of attitude adjustment;

[0026] S6: Kinematics inverse solution of the required car angular velocity of attitude adjustment and the required car linear speed of trajectory adjustment is obtained to obtain the target speed of each motor;

[0027] S7: Drive the robot car to move towards the rescue target according to the target speed, and detect obstacles in real time, generate obstacle avoidance strategy and control the robot car to avoid obstacles;

[0028] S8: When the robot car approaches the rescue target, control the mechanical claw on the robot car to grab the rescue target;

[0029] S9: When the rescue target is grabbed in place, identify the safety zone and control the robot car to transfer the grabbed rescue target to the safety zone.

[0030] In one embodiment of the application, in step S3, the specific method of 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, wherein the trapezoidal speed planning includes startup acceleration stage, uniform speed stable stage and deceleration stop stage; the triangular speed planning includes startup acceleration stage and deceleration stop stage; the speed planning based on PID output and gradient limiting includes generating target speed through position loop PID controller based on position error between current position and target position and limiting target speed through gradient limiter.

[0031] In one embodiment of the application, in step S4, the specific method of obtaining the current position of the robot car in real time is as follows:

[0032] Through the pulse signal feedback by the motor encoder on the motor, kinematics forward solution is performed to obtain the real-time displacement and heading angle of the robot car;

[0033] Obtaining the angular velocity integral data of the robot trolley through the gyroscope module, and compensating the accumulated error in the kinematics forward solution;

[0034] Fusing the kinematics forward solution data and the data obtained by the gyroscope module through the Kalman filtering algorithm to obtain the current position information of the robot trolley.

[0035] In an embodiment of the present application, in step S3, the speed of the motor required for trajectory adjustment is calculated through the PID algorithm according to the preset path and the current position information of the robot trolley, and the specific method for making the actual movement consistent with the preset path is as follows:

[0036] According to the speed curve and the acceleration curve generated according to the preset path, the speed value of each time node is calculated;

[0037] The current position and speed of the robot trolley are obtained in real time;

[0038] The position deviation between the current position and the target position is calculated;

[0039] The speed of the motor required for trajectory adjustment is obtained through the PID algorithm based on the position deviation.

[0040] The robot trolley moves to the target position at the adjusted motor speed;

[0041] The above process is repeated in each control cycle until the robot trolley reaches the target position or the deviation reaches the preset value.

[0042] In an embodiment of the present application, in step S3, the calculation expression of the speed of the motor required for trajectory adjustment calculated through the PID algorithm according to the preset path and the current position information of the robot trolley is as follows:

[0043] ;

[0044] Wherein, V1 is the control output at the kth moment; is the angle difference between the current position and the target position, is the angle difference of the last control cycle; denotes the cumulative sum of the angle difference from j=0 to j=k; K p is the proportional coefficient; K i is the integral coefficient; K d is the differential coefficient, and Δt is the sampling time interval;

[0045] In an embodiment of the present application, in step S5, the calculation expression of the motor speed difference obtained through the PID algorithm based on the angle deviation is as follows:

[0046] ;

[0047] V2 is the control output at the k moment; is the angle difference between the current position and the target position, is the angle difference of the last control cycle; represents the cumulative sum of the angle difference from j=0 to j=k; p is the proportional coefficient; i is the integral coefficient; d is the differential coefficient, and Δt is the sampling time interval.

[0048] In an embodiment of the present application, when the robot car is controlled to grab the rescue target in step S8, the color of the rescue target is recognized by the image processing system, and the grabbing is performed in the order of the score size of the rescue target.

[0049] In an embodiment of the present application, the specific method for recognizing the color of the rescue target by the image processing system and performing the grabbing in the order of the score size of the rescue target includes the following steps:

[0050] Obtain image information of the rescue target;

[0051] Recognize the color of the rescue target by an image processing algorithm;

[0052] Recognize the position of the rescue target using a YOLOv5 model;

[0053] According to the score size of the rescue target, the rescue target with a high score is preferentially recognized.

[0054] The above technical solutions of the present application have the following beneficial effects compared with the prior art:

[0055] The intelligent rescue robot car can move flexibly in a complex environment, quickly reach the rescue target position, and improve the rescue efficiency. The gimbal vision mechanism allows the robot car to freely adjust the angle of the camera in three-dimensional space, realizes omnidirectional monitoring, can significantly improve the trajectory tracking ability and task execution accuracy of the intelligent rescue robot car in a complex environment, and thus plays a greater role in critical tasks such as emergency rescue. The design of the mechanical claw enables the robot car to grab and transport the rescue target, and enhances the ability of the robot to perform diversified tasks.

[0056] The control method of the intelligent rescue robot trolley provided by the application can ensure that the robot trolley can accurately travel along the predetermined path by real-time posture control and posture adjustment of the robot trolley, so that the robot trolley can keep the starting and ending angles consistent and correct the yaw during straight travel, and can ensure the accurate travel of the robot trolley at the predetermined target point through trajectory planning control, so that the robot can quickly and directly reach the rescue target, reduces unnecessary movement and time consumption, thereby improving the rescue efficiency and enabling the robot to efficiently, accurately and safely perform the rescue task in various complex environments. BRIEF DESCRIPTION OF DRAWINGS

[0057] In order to make the content of the application more easily understood, the application will be further described in detail below according to specific embodiments of the application and in conjunction with the drawings.

[0058] Figure 1 Fig. 1 is a structural schematic diagram of an intelligent rescue robot trolley in a preferred embodiment of the application;

[0059] Figure 2 Fig. 3 is a structural block diagram of a control system in a preferred embodiment of the application;

[0060] Figure 3 Fig. 5 is a flowchart of a control method of an intelligent rescue robot trolley in a preferred embodiment of the application;

[0061] Figure 4 Fig. 7 is a schematic diagram of trapezoidal trajectory planning in a preferred embodiment of the application;

[0062] Figure 5 Fig. 9 is a schematic diagram of triangular velocity planning in a preferred embodiment of the application;

[0063] Figure 6 Fig. 11 is a principle block diagram of trajectory control and posture control in a preferred embodiment of the application;

[0064] Figure 7 Fig. 13 is a schematic diagram of a rescue target, a departure area and a safety area in a preferred embodiment of the application;

[0065] Figure 8 Fig. 15 is a functional flowchart of an intelligent rescue robot trolley of the application;

[0066] The drawing of the specification is explained as follows: 1, chassis moving mechanism; 11, chassis; 12, omni-directional wheel; 13, motor; 2, cloud platform vision mechanism; 21, cloud platform; 22, camera; 31, claw finger driving mechanism; 32, claw finger driving connecting piece; 33, claw finger. DETAILED DESCRIPTION

[0067] The application will be further described below in conjunction with the drawings and specific embodiments, so that those skilled in the art can better understand the application and implement it, but the embodiments are not limiting to the application.

[0068] Referring to Figure 1 As shown in the drawings, the application provides an intelligent rescue robot car, comprising: a chassis moving mechanism 1, comprising a chassis 11, an omni-directional wheel 12 and a motor 13; the motor 13 is arranged on the lower side of the chassis 11, and the output shaft of the motor 13 is connected with the omni-directional wheel 12; wherein the omni-directional wheel 12 comprises three, and the included angle between the shaft centers of adjacent omni-directional wheels 12 is 120°;

[0069] A gimbal vision mechanism 2, comprising a gimbal 21 and a camera 22; the gimbal 21 is arranged at the center position of the chassis 11, and the camera 22 is installed on the gimbal 21, and the gimbal 21 is controlled to realize horizontal rotation and pitching rotation by two rudders; the gimbal 21 is installed at the center of the chassis 11, so that the car structure is stable, the rotational inertia of the car to the center shaft is reduced, the energy consumption is reduced, and the balance of the car is ensured; the gimbal 21 can freely adjust the pitching angle and horizontal rotation of the camera 22 as needed, provide more flexible shooting angles, and adapt to the needs of different scenes. At the same time, it is equipped with shockproof and shock absorption functions, which can maintain the stability of the camera in the case of movement or environmental vibration, and ensure the image quality. Through the rotation and inclination of the gimbal 21, the camera 22 can realize 360-degree omnidirectional monitoring, cover a wider area, reduce the blind area, and ensure that the center position of the camera 22 always faces the straight line advancing direction of the robot car, which is convenient for accurately determining the position of the rescue target and adjusting the moving direction of the robot car; the gimbal 21 is cleverly controlled by two rudders, which has two degrees of freedom in space, can capture all targets in the field of view, and provides clear and accurate visual guidance for the action of the car;

[0070] A mechanical claw 3 is arranged on the chassis 11 and is used for grabbing a target object; the mechanical claw 3 comprises a claw finger driving mechanism 31, a claw finger driving connecting piece 32 and a claw finger 33, the claw finger driving mechanism 31 is connected with the claw finger 32 through the claw finger driving connecting piece 32; the claw finger driving mechanism 31 comprises a rudder, which provides the opening and closing power of the mechanical claw, and through controlling the rotation angle of the rudder, the mechanical claw is opened or closed, so as to realize the clamping of the target object;

[0071] A control system is used for controlling the working state of the chassis moving mechanism 1, the gimbal vision mechanism 2 and the mechanical claw 3.

[0072] As Figure 2 shown, the control system comprises:

[0073] Maixcam module for real-time image acquisition and processing to identify different colors of rescue targets; in 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 separates color information from brightness information, making color recognition more stable under different lighting conditions. By setting threshold values for a and b values, specific colors can be effectively identified, effectively dealing with color recognition in complex environments. The Find blobs application in Maixcam can quickly determine the appropriate color threshold range. After testing under different lighting conditions, three corresponding object color thresholds are obtained;

[0074] Gyroscope module for obtaining attitude information of the robot car for attitude control and navigation assistance; the gyroscope module integrates a high-precision crystal gyroscope, uses a high-performance microprocessor and advanced dynamic solving and Kalman dynamic filtering algorithms to quickly solve the real-time motion attitude of the module. Advanced digital filtering technology is used to effectively reduce measurement noise and improve measurement accuracy. The gyroscope module integrates an attitude solver internally, which, combined with the dynamic Kalman filtering algorithm, can accurately output the current attitude of the module in a dynamic environment. The attitude measurement accuracy is 0.1 degrees, and the stability is extremely high;

[0075] TOF ranging module for real-time measurement of the distance between the intelligent rescue robot car and surrounding obstacles and for measuring the distance between the intelligent rescue robot car and the rescue target; the TOF ranging module optimally selects the TOF050C laser rangefinder, which can provide measurement accuracy up to millimeters, making it very suitable for applications requiring precise distance measurement. The TOF050C has fast measurement speed and can complete multiple measurements in a short time, making it suitable for real-time monitoring and measurement in dynamic environments. The laser rangefinder can effectively resist environmental light interference, especially in strong light or multiple reflection environments, maintaining measurement stability and reliability;

[0076] Optical flow sensor module for obtaining displacement information by changes in adjacent frame images to achieve positioning and obstacle avoidance; the optical flow sensor can quickly process image information and provide real-time feedback, allowing the device to respond to environmental changes in real time, resulting in smoother motion control. The optical flow sensor can not only be used for positioning but also for motion detection, obstacle avoidance, and other functions, improving the intelligence level of the device;

[0077] PWM servo drive module for simultaneously controlling the rotation angle and speed of multiple servos; the PWM servo drive module selects PCA9685, which allows simultaneous control of multiple servos, enhancing the system's scalability and flexibility;

[0078] A motor driving module is used to adjust the output torque and speed of the motor; the motor driving module adopts a FOC vector closed-loop control algorithm, three-loop control torque, speed and position, an on-board industrial high-precision 16384 line magnetic encoder, an on-board precision current sensor, supports pulse control, and supports serial communication control.

[0079] A main control board exchanges data with the Maixcam module, the gyroscope module, the TOF distance measuring module, the optical flow sensor module, the PWM steering engine driving module and the motor driving module, and realizes automatic execution of tasks.

[0080] A power stabilizing module is connected with the main control board, and supplies power to the main control board after step-down processing of a direct current power supply.

[0081] A communication module is connected with the main control board, and is used for realizing pairing and communication of the main control board and external equipment.

[0082] The main control board comprises:

[0083] A trajectory planning control module is used to calculate an output speed through a PID algorithm according to an error between a target position and a current position, so that the robot trolley can accurately reach the target position.

[0084] A posture control module is used to calculate an output speed through a PID algorithm according to a deviation between a target angle and a current angle of the trolley, so that the robot trolley can be adjusted to the target angle.

[0085] A kinematics forward solution module is used to calculate a speed component of the robot trolley in a trolley coordinate system according to an orientation angle and a target angle of the robot trolley, and further calculate a speed of each omnidirectional wheel.

[0086] A global positioning module is used to calculate a displacement of the robot trolley in a global coordinate system through a conversion matrix and a rotation matrix according to a displacement of a motor encoder.

[0087] As shown in Figure 3 The present application provides a control method of an intelligent rescue robot trolley, which is realized based on the above intelligent rescue robot trolley, and comprises the following steps:

[0088] S1: identifying a rescue target and obtaining a position of the rescue target;

[0089] S2: performing path planning according to a starting position of the robot trolley and the position of the rescue target, and determining a preset path of the robot trolley.

[0090] S3: According to the preset path, the running speed trajectory of the robot trolley is planned, the current position of the robot trolley is obtained in real time, the global positioning of the robot trolley is realized, and then according to the preset path and the current position information of the robot trolley, the linear speed of the trolley required by the trajectory motion is calculated, so that the actual motion is consistent with the preset path;

[0091] S4: Real-time acquisition of the current attitude information of the robot trolley, the attitude information including the heading angle of the robot trolley;

[0092] S5: According to the current attitude information, the angle deviation between the current attitude and the target attitude is obtained, and the angle deviation is converted into the angular velocity of the trolley required for attitude adjustment;

[0093] S6: Kinematic inverse solution of the trolley angular velocity required for attitude adjustment and the linear velocity of the trolley required for trajectory adjustment is carried out to obtain the target speed of each motor;

[0094] S7: Drive the robot trolley to move towards the rescue target according to the target speed, and real-time detect obstacles, generate obstacle avoidance strategy and control the robot trolley to avoid obstacles;

[0095] S8: When the robot trolley approaches the rescue target, control the mechanical claw 3 on the robot trolley to grab the rescue target; The specific method comprises the following steps:

[0096] Obtain image information of the rescue target;

[0097] Recognize the color of the rescue target through image processing algorithm;

[0098] Use YOLOv5 model to identify the position of the rescue target;

[0099] According to the score size of the rescue target, the rescue target with high score is preferentially identified;

[0100] Control the robot trolley to move to the position of the rescue target, and drive the mechanical claw 3 to grab the rescue target.

[0101] S9: When the rescue target is grabbed in place, identify the safety zone and control the robot trolley to transfer the grabbed rescue target to the safety zone.

[0102] In order for the robot trolley to accurately stop at the predetermined target point during the movement process and reduce the time required for visual recognition correction, the robot trolley needs to accelerate and decelerate smoothly, and through straight line trajectory planning, the problem that the current position of the robot trolley is inconsistent with the target position of the trajectory planning can be solved. When the current position lags behind the target position, it needs to accelerate to catch up, and when the current position leads the target position, it needs to accelerate to adjust.

[0103] 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.

[0104] The specific method for planning the trapezoidal and triangular velocities of the robot car based on the preset path is as follows:

[0105] Obtain the distance between the rescue target and the starting position of the robot vehicle, and label it as s;

[0106] Set the maximum speed and acceleration of the robot car;

[0107] The acceleration distance s1 during the acceleration phase is calculated based on the maximum speed and acceleration.

[0108] Determine if the distance s is greater than twice the value of s1;

[0109] If so, then perform trapezoidal trajectory planning;

[0110] If not, then execute triangle velocity planning;

[0111] 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.

[0112] 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.

[0113] 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:

[0114] ;

[0115] 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.

[0116] When distance At this time, the robot car has enough acceleration distance to accelerate to the maximum speed, which meets the characteristics of the trajectory planning trapezoidal diagram. At this time, the specific value of each time node can be calculated, as shown in the following formula:

[0117]

[0118] wherein is the acceleration time, is the distance of the constant speed stage, is the total time of acceleration and constant speed, is the total motion time, then:

[0119] Acceleration stage , target speed , target position ;

[0120] Constant speed stage , target speed , target position ;

[0121] Deceleration stage , target speed , target position .

[0122] When the distance , the robot car cannot accelerate to the maximum speed according to the predetermined trajectory planning, for this special short distance motion, the triangular velocity planning is adopted as shown in Figure 5 , its motion only contains acceleration and deceleration two stages, set the acceleration of the acceleration stage and the deceleration stage , the direction is opposite, the highest speed is .

[0123] Acceleration stage , target speed , target position .

[0124] Deceleration stage , target speed , target position .

[0125] The above trapezoidal / triangular velocity planning gives the shortest trajectory under the constraint of limit acceleration and speed, the acceleration is bounded and continuous, no impact, the whole curve is completely predefined, which is convenient for real-time tracking and monitoring.

[0126] Based on the PID output and gradient limiting speed planning, the position loop PID controller outputs the theoretical target speed; then the gradient limiter completes the "trapezoidal planning" at one time to get the real-time, smooth and physically executable speed v_cmd, and the specific method is as follows:

[0127] Sampling the current position error: at the beginning of each control period ΔT, the planned position and the measured position are read, and the error e(k) is calculated;

[0128] Calculating the theoretical target speed: the error e(k) is sent to the position loop PID controller, and the position loop PID controller calculates an original control output u_pid(t). The output value u_pid(t) is directly interpreted and used as the theoretical target speed v_target(t) required to eliminate the error, that is

[0129] ;

[0130] Wherein, the output dimension of the position loop PID controller is speed, such as m / s or rpm;

[0131] Calculating the speed increment: v_target(k) is subtracted from the actual speed v_cmd(k−1) issued in the last period to obtain the required speed change Δv(k); that is

[0132] Δv(k) = v_target [k] - v_cmd [k-1]

[0133] Gradient limiting:

[0134] If Δv>Δv_max, then v_cmd[k] = v_cmd[k−1]+Δv_max, which realizes positive limiting and uniform upward;

[0135] If Δv<-Δv_max, then v_cmd[k] = v_cmd[k−1]−Δv_max, which realizes negative limiting and uniform downward;

[0136] Otherwise, v_cmd[k] = v_target[k], which does not need to be limited.

[0137] Wherein, k is the discrete time period index; ΔT is the control period; e[k] is the position error in the kth 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; Δv_max is the maximum allowed speed change in a single period.

[0138] The posture control and trajectory planning in this embodiment are simultaneously performed in actual control, which belongs to parallel type PID control. After the posture control and trajectory planning control, the target speed instruction of each omni-directional wheel 12 set is sent to the motor control module, and then the speed control of the motor 13 is realized. The control flow is shown in Figure 6 .

[0139] In step S4, the specific method of acquiring the current position of the robot trolley in real time is as follows:

[0140] S41: kinematic forward solution is performed through the pulse signal feedback by the motor encoder on the motor 13 to obtain the real-time displacement and heading angle of the robot trolley;

[0141] S42: the angular velocity integral data of the robot trolley is obtained through the gyroscope module to compensate the angle accumulation error in the kinematic forward solution;

[0142] S43: the current position information of the robot trolley is obtained by fusing the kinematic forward solution data and the data obtained by the gyroscope module through Kalman filtering algorithm.

[0143] In S41, the specific steps of kinematic forward solution through the pulse signal feedback by the motor encoder on the motor 13 to obtain the real-time displacement and heading angle of the robot trolley are as follows:

[0144] Data acquisition: read the encoder increment values of the three drive motors in unit time: Δθ1, Δθ2, Δθ3.

[0145] Body coordinate system increment calculation: use the linear transformation matrix M (based on the 120° symmetrical distribution kinematics constraint of three wheels) to convert the encoder increment into the displacement and rotation increment in the robot body coordinate system:

[0146] ;

[0147] World coordinate system conversion: according to the current heading angle θ of the robot, the translation increment in the body coordinate system is converted to the world coordinate system through the rotation matrix R(Δθ):

[0148] ;

[0149] Pose update: the calculated world coordinate system displacement increment ΔXw, ΔYw and rotation increment Δθ are used to update the real-time pose of the robot in the world coordinate system.

[0150] Through the above fusion kinematics forward and inverse solution and coordinate transformation, real-time, high-frequency autonomous pose estimation is realized only by relying on motor encoder. The core algorithm naturally supports three degrees of freedom motion, and easily realizes complex actions such as side shift, diagonal movement, rotation in place and "walking and turning" in any combination.

[0151] In step S3, according to the preset path and the current position information of the robot car, the speed of the motor 13 required for trajectory adjustment is calculated by PID algorithm, so that the actual motion is consistent with the specific method of the preset path as follows:

[0152] According to the speed curve and acceleration curve generated according to the preset path, the speed value of each time node is calculated;

[0153] The current position and speed of the robot car are obtained in real time;

[0154] The position deviation between the current position and the target position is calculated;

[0155] The speed of the motor 13 required for trajectory adjustment is obtained based on the position deviation by PID algorithm.

[0156] The robot car moves to the target position at the adjusted speed of the motor 13;

[0157] Repeat the above process in each control cycle until the robot car reaches the target position or the deviation reaches the preset value.

[0158] In step S3, the calculation expression of the speed of the motor required for trajectory adjustment calculated by PID algorithm according to the preset path and the current position information of the robot car is as follows:

[0159] ;

[0160] Wherein, V1 is the control output at the k time; is the angle difference between the current position and the target position, is the angle difference of the last control cycle; denotes the cumulative sum of the angle difference from j=0 to j=k; K p is the proportional coefficient; K i is the integral coefficient; K d is the differential coefficient, and Δt is the sampling time interval;

[0161] In actual operation, it is found that too fast speed or non-smooth acceleration and deceleration will cause the robot car to be difficult to maintain straight driving. In order to make the robot car maintain execution, the robot car that has deviated needs to adjust the speed difference of the wheels to change the angle of the robot car.

[0162] The data collected by the gyroscope module is used to obtain the angle of deviation of the robot trolley. The deviation between the obtained angle and the target angle is taken as the input of the PID controller, and the deviation value is converted into a speed difference to be taken as the input value of the motor controller, so as to change the speed of the motor and realize real-time speed adjustment.

[0163] In order to periodically obtain the speed of the motor 13, the gyroscope deviation angle, and adjust the speed of the motor 13, a timer in the STM32 needs to be enabled. The angle PID control function is called by the timer interrupt every certain period of time. In each period, the current deviation angle and the current speed are obtained, and the target speed of the omnidirectional wheel 12 is calculated, and the set target speed instruction is sent to the motor control module.

[0164] In step S6, the trolley angular velocity required for the posture adjustment and the trolley linear velocity required for the trajectory adjustment are kinematically solved to obtain the specific method of the target speed of each motor as follows:

[0165] The forward speed aim_speed, the rotation speed spin_speed and the direction angle yaw_h in the chassis coordinate system are set

[0166] First, the combined speed aim_speed is decomposed into X and Y axis speeds (vx, vy) in the chassis coordinate system according to the direction angle yaw_h.

[0167] Then, according to the mechanical structure of the 120° uniform distribution of the three wheels, the linear speed and the angular speed are superimposed to calculate the target speed of each wheel:

[0168] wheel1=Vx+ spin_speed;

[0169] wheel2=-sin(30°)*vx+ cos(30°)*vy+ spin_speed;

[0170] wheel3= -sin(30°)*vx- cos(30°)*vy+ spin_speed;

[0171] This solution enables the robot to perform translational and rotational motion simultaneously, i.e. to realize "walking while turning". Since the kinematic inverse solution belongs to the prior art, it will not be described here.

[0172] In step S5, the angle deviation is obtained by the PID algorithm to obtain the calculation expression of the motor speed difference as follows:

[0173] ;

[0174] Wherein, V2 is the control output at the kth moment; is the angle difference between the current position and the target position, is the angle difference of the last control cycle; represents the cumulative sum of the angle difference from j=0 to j=k; K p is the proportional coefficient; K i is the integral coefficient; K d is the differential coefficient, and Δt is the sampling time interval.

[0175] In this embodiment, the omni-directional wheels 12 include three, each of which is independently controlled by a motor 13. Taking the three omni-directional wheels 12 as an example, the obtaining of the target speed of the motor 13 is explained:

[0176] The current orientation angle α of the trolley is obtained by gyroscopic integration, and the angle γ in the trolley coordinate system is obtained by subtracting the trolley orientation angle α from the target angle β.

[0177] The speed is decomposed:

[0178] vx=v1×sin(γ);

[0179] vy=v1×cos(γ);

[0180] The speed of the three omni-directional wheels 12 is calculated:

[0181] ;

[0182] Then the target speed of the three motors 13 of the robot trolley is obtained by linear superposition of the speeds of the three omni-directional wheels.

[0183] Then the target speed of the three motors 13 of the robot trolley is obtained by linear superposition of the speeds of the three omni-directional wheels.

[0184] Figure 7 A schematic diagram of a rescue scene is shown, which is used to plan and simulate rescue operations. The robot trolley departs from the departure area and reaches the rescue target. After grabbing the rescue target, it is transported to the safety area; Figure 8 The complete decision-making process of the robot trolley from image recognition, motion control to global positioning is shown, which ensures that the robot can effectively perform emergency rescue tasks. The decision-making process of the intelligent rescue robot trolley starts from the start and is divided into three main steps:

[0185] First, the robot uses YOLOv5 for target recognition through the image processing system, judges whether the recognized target is the rescue target of the team, and if so, the system will pass the target position information through the serial port. If not, no further action will be taken. Color recognition is very important in computer vision and image processing, and common methods include using threshold segmentation techniques to identify and distinguish different colors;

[0186] Secondly, the control system is responsible for the posture control and trajectory planning of the chassis 11 of the robot car. The posture control is achieved by angle PID control to stabilize the center of the robot. The trajectory planning includes trapezoidal and acceleration / deceleration as well as speed trajectory offset correction. According to the correction result, if the speed trajectory offset is less than 5 revolutions / second, the system will perform speed PID control; if the offset is greater than or equal to 5 revolutions / second, the three motors 13 will be independently controlled to obtain equal proportion of torque and avoid slipping, thereby realizing the straight-line operation of the robot car.

[0187] Finally, the positioning is performed by integrating the positive solution value of the motor feedback and the displacement value integrated by the gyroscope, and the accurate coordinates of the robot car are obtained through data fusion.

[0188] Obviously, the above embodiments are only examples for the purpose of clarity, and are not limiting to the embodiments. For those skilled in the art, other different forms of changes or variations can be made on the basis of the above description. It is not necessary and impossible to enumerate all the embodiments. The obvious changes or variations derived therefrom are still within the protection scope of the present application.

Claims

1. A control method of an intelligent rescue robot vehicle, characterized in that: The control method comprises the following steps: S1: identifying a rescue target and obtaining the position of the rescue target; S2: path planning according to the starting position of the robot car and the position of the rescue target to determine the preset path of the robot car; S3: according to the preset path, planning the running speed trajectory of the robot car, and obtaining the current position of the robot car in real time to realize global positioning of the robot car, and then calculating the speed of the motor required for trajectory motion according to the preset path and the current position information of the robot car, so that the actual motion is consistent with the preset path; S4: obtaining the current attitude information of the robot car in real time, and the attitude information comprises the heading angle of the robot car; S5: obtaining the angle deviation between the current attitude and the target attitude according to the current attitude information, and converting the angle deviation into the speed difference of the motor required for attitude adjustment; S6: combining the speed difference of the motor required for attitude adjustment with the speed of the motor required for trajectory adjustment to obtain the target speed of each motor; S7: driving the robot car to move towards the rescue target at the target speed, and detecting obstacles in real time to generate an obstacle avoidance strategy and control the robot car to avoid obstacles; S8: when the robot car approaches the rescue target, controlling the mechanical claw on the robot car to grab the rescue target; S9: when the rescue target is grabbed in place, identifying a safe area and controlling the robot car to transfer the grabbed rescue target to the safe area; In step S3, the specific method for planning the running speed trajectory of the robot car according to the preset path comprises trapezoidal speed planning, triangular speed planning, and speed planning based on PID output and gradient limiting, wherein the trapezoidal speed planning comprises a startup acceleration stage, a uniform speed stable stage, and a deceleration stop stage; the triangular speed planning comprises a startup acceleration stage and a deceleration stop stage; and the speed planning based on PID output and gradient limiting comprises 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; In step S3, the specific method for obtaining the current position of the robot car is as follows: obtaining real-time displacement and heading angle of the robot car through kinematic forward calculation of pulse signals fed back by a motor encoder on the motor; obtaining angular velocity integral data of the robot car through a gyroscope module to compensate for cumulative errors in the kinematic forward calculation; obtaining current position information of the robot car by fusing the kinematic forward calculation data and the data obtained by the gyroscope module through a Kalman filtering algorithm; According to the preset path and the current position information of the robot car, the calculation expression of the motor speed required for trajectory adjustment calculated through the PID algorithm is as follows: ； wherein V1 is the control output at the kth moment; is the angle difference between the current position and the target position, is the angle difference of the last control cycle; represents the cumulative sum of the angle difference from j=0 to j=k; K p is a proportional coefficient; K i is an integral coefficient; K d is a differential coefficient, and Δt is a sampling time interval; In step S5, the calculation expression of the motor speed difference obtained by the PID algorithm from the angle deviation is as follows: ； wherein V2 is the control output at the kth moment; is the angle difference between the current position and the target position, is the angle difference of the last control cycle; denotes the cumulative sum of the angle difference from j = 0 to j = k; K p is a proportional coefficient; K i is an integral coefficient; K d is a differential coefficient, and Δt is a sampling time interval.

2. The control method of claim 1, wherein: In step S3, according to the preset path and the current position information of the robot car, the specific method for calculating the motor speed required for trajectory adjustment through the PID algorithm so that the actual motion is consistent with the preset path is as follows: According to the preset path generated speed curve and acceleration curve, the speed value of each time node is calculated; Real-time acquisition of the current position and speed of the robot trolley; Calculate the position deviation between the current position and the target position; Based on the position deviation, the speed of the motor required for trajectory adjustment is obtained by using the PID algorithm; The robot trolley moves to the target position at the adjusted motor speed; Repeat the above process in each control cycle until the robot trolley reaches the target position or the deviation reaches the preset value.

3. The control method of claim 1, wherein: In step S8, when the robot trolley is controlled to grab the rescue target, the color of the rescue target is recognized by the image processing system, and the rescue target is grabbed in the order of the score size of the rescue target.

4. The control method of claim 3, wherein: The specific method of recognizing the color of the rescue target by the image processing system and grabbing the rescue target in the order of the score size of the rescue target includes the following steps: Obtain image information of the rescue target; Recognize the color of the rescue target by image processing algorithm; Use YOLOv5 model to identify the position of the rescue target; According to the score size of the rescue target, the rescue target with high score is preferentially identified.

5. An intelligent rescue robot vehicle for performing the control method according to any one of claims 1 to 4, characterized in that: It includes: The chassis moving mechanism includes a chassis, an omni-directional wheel and a motor; The motor is arranged on the lower side of the chassis, and the output shaft of the motor is connected with the omni-directional wheel; The holder visual mechanism includes a holder and a camera; the holder is arranged at the center position of the chassis, the camera is installed on the holder, and the holder is controlled to rotate horizontally and vertically by two rudders; The mechanical gripper is arranged on the chassis and is used for grabbing the target object; the mechanical gripper includes a gripper finger driving mechanism, a gripper finger driving connecting piece and a gripper finger, the gripper finger driving mechanism is connected with the gripper finger through the gripper finger driving connecting piece; the gripper finger driving mechanism includes a rudder, the rudder provides the opening and closing power of the mechanical gripper, and by controlling the rotation angle of the rudder, the mechanical gripper is opened or closed, so as to realize the clamping of the target object; The control system is used for controlling the working state of the chassis moving mechanism, the holder visual module and the mechanical gripper.

6. The intelligent rescue robot vehicle according to claim 5, characterized in that: The control system includes: The Maixcam module is used for real-time image acquisition and processing, and different colors of rescue targets are identified; The gyroscope module is used for obtaining the attitude information of the robot trolley, and is used for attitude control and navigation assistance; The TOF distance measuring module is used for real-time measurement of the distance between the intelligent rescue robot trolley and the surrounding obstacles, and is used for measurement of the distance between the intelligent rescue robot trolley and the rescue target; The optical flow sensor module is used for obtaining displacement information by the change of adjacent frame images, realizing positioning and obstacle avoidance; The PWM rudder driving module is used for controlling the rotation angle and speed of multiple rudders at the same time; The motor driving module is used for adjusting the output torque and speed of the motor; The main control board exchanges data with the Maixcam module, the gyroscope module, the TOF distance measuring module, the optical flow sensor module, the PWM rudder driving module and the motor driving module, and realizes automatic execution of tasks; The power stabilizing module is connected with the main control board, and the DC power supply is processed by voltage reduction to supply power to the main control board; The communication module is connected with the main control board, and is used for realizing the pairing and communication of the main control board and external devices.

Images