Traffic emergency robot with double functions of independent walking and autonomous driving and method
By designing a traffic emergency robot with both independent walking and autonomous driving functions, the problem of existing emergency robots being unable to efficiently complete traffic evacuation and rescue has been solved. This enables efficient traffic management and rescue tasks in dangerous environments and provides high-precision road condition perception and real-time road condition reporting.
Patent Information
- Application Number
- CN202410929780.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-11
- Publication Date
- 2024-11-08
- Estimated Expiration
- 2044-07-11
AI Technical Summary
Existing emergency robots cannot simultaneously achieve independent walking and autonomous driving functions, cannot efficiently complete traffic evacuation and rescue tasks in dangerous environments, and cannot perform route planning when traffic is congested.
A traffic emergency robot was designed, which has dual functions of independent walking and autonomous driving. It is installed in the driver's seat of a vehicle and is equipped with a power system, communication system, perception system and control system. It uses multi-sensor fusion technology to achieve high-precision road condition perception and uses PID control algorithm to realize automatic control of steering wheel angle and throttle depth. It has the ability to switch between independent walking and autonomous driving.
It enables efficient traffic evacuation and rescue in dangerous environments, can independently clear traffic during traffic jams, has high-precision road condition perception and real-time road condition broadcasting, ensuring the safety and efficiency of rescue missions, and has the advantages of easy operation, low cost and portability.
Smart Images

Figure CN118906067A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of traffic management, and in particular to a traffic emergency robot and method having dual functions of independent walking and autonomous driving. Background Art
[0002] At present, conventional special vehicles do not have the ability of intelligent driving. Rescue and special operations in dangerous environments are very dangerous for operators. For example, when encountering mountain torrents, explosions, nuclear leaks, epidemics, etc., traffic is very easy to be blocked, and rescuers face huge risks of casualties. The operation tasks are greatly hindered, and traffic emergency management is particularly important. If the existing unmanned vehicles with intelligent driving functions are used for operations, although rescuers can be prevented from being injured, they cannot enter the rescue site when the road is congested or blocked. At this time, in the state of traffic paralysis caused by sudden accidents, there is an urgent need for traffic emergency robots with the following functions: (1) Independent walking function form, requiring a small and flexible intelligent robot that can identify vehicles around the road and the drivable area, enter the accident center and transmit sensor data to the traffic command center, and conduct traffic command to relieve traffic; (2) Autonomous driving function form, anthropomorphic driving of accident vehicles or rescue vehicles, so as to be competent for the evacuation of ordinary conventional vehicles or the operation of rescue vehicles. Therefore, there is an urgent need for a traffic emergency robot with the dual functions of independent walking and autonomous driving to achieve traffic emergency management, vehicle evacuation or rescue operations in the case of traffic congestion caused by dangerous environments.
[0003] However, currently available emergency robots cannot realize the above functions at the same time, and cannot perform path planning, and cannot complete emergency work well. Summary of the invention
[0004] In view of the above content, the present invention provides a traffic emergency robot and method with the dual functions of independent walking and autonomous driving. The robot can be installed on the main driving seat of a vehicle and cooperate with special vehicles to realize functions such as anthropomorphic intelligent driving and vehicle-road collaboration, so that rescue missions are safe and rapid, and vehicles in traffic jams can be easily evacuated. At the same time, multi-sensor fusion technology can achieve high-precision and high-efficiency road condition perception, and can still perform efficient operations in environments such as blocked vision, poor signals, and high burial. In addition, the robot also has the function of independent walking. When vehicles in traffic jams are difficult to enter, the robot can walk independently, realize environmental exploration, and display and broadcast the current road conditions in real time, so as to realize traffic supervision, patrol, and command, which is conducive to the smooth implementation of rescue missions, traffic command and supervision, and has the advantages of easy operation, efficient operation, low cost, and portability.
[0005] The technical solution adopted by the present invention is a traffic emergency robot with the dual functions of independent walking and autonomous driving vehicles, which includes a power supply system, a communication system, a perception system, a control system and a robot body. The communication system includes a wireless communication module, a wired communication interface and an encoder. The wireless communication module is installed near the main control unit of the robot body, the wired communication interface is installed on the robot body, and the encoder is installed on the robot body and connected to the moving parts of the robot body; the perception system includes a visual sensor module, a millimeter wave radar module, a laser radar module and an RFID module. The visual sensor module, the millimeter wave radar module and the laser radar module are all installed on the sensor bracket of the robot body. The RFID module includes an RFID smart tag and an RFID tag reader / writer, the RFID smart tag is arranged at the target intersection, the RFID tag reader / writer is arranged on the sensor bracket of the robot body, the control system includes a motor controller, a motor sensor, a motor, a servo driver, a single-chip microcomputer and a communication expansion unit; the motor controller, the servo driver and the communication expansion unit are all arranged on the single-chip microcomputer, the motor includes a DC servo electric cylinder, a DC joint motor, a DC high-torque reduction motor and a DC hub motor, the motor sensor is arranged on the motor, the servo driver is respectively arranged on two servo motor cylinders, and the communication expansion unit is connected to the visual sensor module, the millimeter wave radar module and the laser radar module;
[0006] The robot body includes a robot frame, a three-jaw chuck, a mechanical arm, a sensor bracket, a folding plate, a rear wing, a front wheel frame, a rear wheel frame and a driving wheel. The three-jaw chuck is located at the top of the mechanical arm and is connected to a DC high-torque reduction motor. The three-jaw chuck can be clamped on the outside of the vehicle steering wheel. The mechanical arm includes a first connecting rod and a second connecting rod. The first end of the first connecting rod is fixedly connected to the three-jaw chuck, the second end of the first connecting rod is rotatably connected to the first end of the second connecting rod, and the second end of the second connecting rod is fixedly connected to the robot frame. The sensor bracket is slidably connected to the first connecting rod. The sensor bracket is installed on The sensing system, the folding plate is rotatably connected to the front wheel frame, two servo motor cylinders are fixed on the folding plate, the rear wing is rotatably connected to the rear wheel frame, the driving wheels include two front driving wheels and two rear driving wheels, the front driving wheels are connected to the DC joint motors, and the rear driving wheels are connected to the DC hub motors; the folding plate and the driving wheels both have upper and lower limits, when the vehicle is driven autonomously, the folding plate is flipped to the lower limit to control the throttle and brakes, and the driving wheels are folded to the upper limit; when walking independently, the folding plate is flipped to the upper limit, and the driving wheels are folded to the lower limit for independent walking;
[0007] When the vehicle is driven autonomously, the current drivable area is obtained through the road condition perception algorithm, and the central axis of the current drivable area is calculated. The robot's next movement is determined based on the angle between the central axis of the drivable area and the direction of travel of the vehicle body. The steering wheel angle and throttle depth are used as control variables to make the vehicle steering angle follow the angle change and maintain a constant distance from the vehicle body side to the central axis of the drivable area.
[0008] When clearing roads independently, the current position is determined based on radar information. If the designated position has been reached, the road clearing is directed through the intercom voice monitoring system. After the clearing is completed, the vehicle returns to the dedicated lane where it can be driven. If there are still points that need to be evacuated, the vehicle enters the next road clearing position and determines whether it can move forward based on the road condition perception algorithm. If not, the intercom voice monitoring system is used to guide vehicles on both sides to give up the drivable area. Based on the road condition label results obtained by the main control chip based on the data prediction, the vehicle determines whether it is currently moving forward, turning left or right, and then controls the speed and direction based on the set thresholds.
[0009] Preferably, the visual sensor module includes two monocular cameras, one rear-view camera and four surround-view cameras.
[0010] Preferably, the millimeter wave radar module includes four lateral radars and two forward radars.
[0011] Preferably, the laser radar module includes a multi-threaded speed measurement laser radar and a multi-threaded ranging laser radar.
[0012] Preferably, the length of the first connecting rod is equal to the length of the second connecting rod.
[0013] Preferably, the sensor bracket and the fixed end of the first connecting rod are provided with movable buckles.
[0014] Preferably, the power supply system includes a lithium battery module, and the lithium battery module includes a voltage transformation module.
[0015] Another aspect of the present invention provides an autonomous driving method for a traffic emergency robot, which comprises the following steps:
[0016] S1. Obtain the current drivable area through the road condition perception algorithm;
[0017] S2, calculate the central axis of the current drivable area, and determine the next movement of the robot according to the angle between the central axis of the drivable area and the traveling direction of the vehicle body;
[0018] S3. With the steering wheel angle and throttle depth as control variables, the controller outputs execution commands to the actuator based on the PID algorithm to control the action of the motor so as to maintain the steering angle of the vehicle following the change of the included angle and maintain the distance from the side of the vehicle body to the center axis of the drivable area constant.
[0019] Another aspect of the present invention provides an independent walking method of a traffic emergency robot, which comprises the following steps:
[0020] S1. Determine whether the vehicle can move forward based on the road condition perception algorithm. If not, guide the vehicles on both sides to leave the driving area through the intercom voice monitoring system.
[0021] S2. According to the road condition label result obtained by the main control chip from the data prediction, it is determined whether to move forward, turn left or turn right, and then the speed and direction are controlled using the PID algorithm according to the set threshold;
[0022] S3. Determine the current position based on radar information. If the designated position has been reached, direct the road through the intercom voice monitoring system. After the evacuation is completed, return through the drivable area. The mission is completed. If there is still a position that needs to be evacuated, proceed to the next evacuation position.
[0023] Preferably, the specific steps of the road condition perception algorithm are as follows:
[0024] S1. Plan the motion trajectory with time. The specific formula is:
[0025]
[0026] Among them, u(t) is the motion trajectory with time, k p is the proportionality coefficient, T i is the integration time constant, T d is the differential time constant;
[0027] S2, using the output of the lateral controller as the steering wheel angle control amount to perform lateral control, including the following sub-steps:
[0028] S21. Analyze the lateral controller error:
[0029] In the Frenet coordinate system, calculate the relative position of the robot:
[0030] dχ=χ-χ des
[0031] dy=yy des
[0032] At time t, the trajectory route function gives the time χ r With y r The value of, that is:
[0033] χ r (t) = χ(t)
[0034] y r (t) = y(t)
[0035] θ r (t) = arctan{y′[χ(t)]}
[0036]
[0037] The coordinates of the vehicle in the frenet coordinate system are obtained by the two-dimensional coordinate system rotation formula:
[0038]
[0039] e=d
[0040]
[0041] Among them, e is the lateral error, θ is the heading angle error, is the lateral error change rate, is the rate of change of heading angle error, is the robot’s current heading angle, is the heading angle of the path reference point, v is the current speed of the robot, is the current yaw angular velocity of the robot, k des is the curvature of the path reference point;
[0042] The state quantity of the control system is expressed as:
[0043]
[0044] S22, LQR lateral feedback control, specifically includes the following sub-steps:
[0045] S221. Establish state space model:
[0046] Assuming the state vector is x and the control input vector is u, the state space model can be expressed as:
[0047]
[0048] Among them, A and B are constant matrices, representing the state transition and input matrices of the system.
[0049] S222. Define the cost function:
[0050]
[0051] Among them, Q and R are positive definite matrices, representing the weight coefficients of the state and input.
[0052] S23, calculate the optimal controller:
[0053] The optimal controller is expressed as a linear state feedback controller:
[0054] u=-Kx
[0055] Where K is the state feedback matrix. Substituting the above controller into the state space model, we get:
[0056]
[0057] Substituting into the cost function, we get:
[0058]
[0059] By deriving the cost function, we get the analytical expression of the state feedback matrix K:
[0060] K=(R+B T PB) -1 B T PA
[0061] where P is a positive definite matrix that satisfies the algebraic Riccati equation:
[0062] A T P+PA-PBR -1 B T P+Q=0
[0063] The Riccati equation is solved by repeated iterations;
[0064] S24, solve the control quantity:
[0065] According to the state feedback matrix K, the optimal control input is calculated to achieve the optimal lateral control
[0066] δ=u=-Kx
[0067] Among them, δ is the calculated rotation angle;
[0068] S3, using the longitudinal position-speed PID controller to calculate the throttle depth for longitudinal control:
[0069] The longitudinal position error is calculated as:
[0070]
[0071] The speed error is:
[0072]
[0073] The acceleration is:
[0074]
[0075] Longitudinal control uses dual PID to control speed, that is, using e sAs the input of position PID, the speed difference between the expected speed and the current speed and the output of position PID are used as the input of speed PID, and the output of speed PID and acceleration are used as acceleration input together.
[0076] The characteristics and beneficial effects of the present invention are:
[0077] (1) The traffic emergency robot of the present invention has the dual functions of independent walking and autonomous driving, which can be installed on the main driving seat of a vehicle to realize functions such as anthropomorphic intelligent driving and vehicle-road collaboration. It can not only realize the evacuation of vehicles in traffic jams, but also operate rescue vehicles to perform anthropomorphic operations, thereby ensuring the safe and rapid execution of rescue tasks in dangerous environments.
[0078] (2) The traffic emergency robot of the present invention has the dual functions of independent walking and autonomous driving. With the help of multi-sensor fusion technology, it can achieve high-precision and high-efficiency road condition perception, and can still perform efficient operations in environments such as obstructed vision, poor signals, and high burial depth.
[0079] (3) The traffic emergency robot of the present invention has the dual functions of independent walking and autonomous driving. It has the function of independent walking. When traffic congestion makes it difficult for vehicles to enter, the robot can walk independently and freely shuttle through the space gaps between congested vehicles to guide traffic, clear traffic congestion, and flexibly serve as an emergency traffic police.
[0080] (4) The traffic emergency robot of the present invention has the dual functions of independent walking and autonomous driving, which can display and broadcast the current road conditions in real time, realize traffic supervision, patrol and command, and is conducive to the smooth implementation of rescue tasks, traffic command and supervision, etc. It has the advantages of easy operation, efficient operation, low cost and portability.
[0081] (5) The traffic emergency robot of the present invention has the dual functions of independent walking and autonomous driving, and has the function of autonomous driving lane keeping. The robot's driving steering wheel angle and throttle depth are determined by the distance from the side of the car to the axis of the drivable area, the angle between the car's travel direction and the tangent direction of the axis in the drivable area, and the automatic control of the steering wheel angle and vehicle speed is achieved based on the PID control algorithm to ensure the accuracy of the control. BRIEF DESCRIPTION OF THE DRAWINGS
[0082] Figure 1 It is a schematic diagram of the overall structure of the present invention;
[0083] Figure 2 The intercom voice monitoring system of the present invention;
[0084] Figure 3 is a schematic diagram of the autonomous driving posture of the present invention;
[0085] Figure 4This is a schematic diagram of the initial posture of independent walking of the present invention;
[0086] Figure 5 This is a schematic diagram of the posture after independent walking adjustment of the present invention;
[0087] Figure 6 It is a schematic diagram of the topological structure diagram of the present invention;
[0088] Figure 7 is a layout diagram of the sensor of the present invention in the sensor bracket;
[0089] Figure 8 It is a schematic diagram of the direction-speed combined control autonomous driving strategy of the present invention;
[0090] Fig. 9 It is a control scheme diagram of the direction-speed combined control autonomous driving of the present invention;
[0091] Fig.10 It is a block diagram of the PID-based path planning and driving control algorithm of the present invention;
[0092] Fig.11 It is a block diagram of the PID closed-loop control system of the present invention;
[0093] Fig.12 It is a schematic diagram of the PID longitudinal control position error of the present invention;
[0094] Fig.13 It is a schematic diagram of the longitudinal control of the present invention. DETAILED DESCRIPTION
[0095] In order to fully describe the technical content, structural features, objectives and effects of the present invention, the following will be described in detail with reference to the accompanying drawings.
[0096] The present invention provides a traffic emergency robot with dual functions of independent walking and autonomous driving. Figure 1The overall structure diagram of the present invention includes a power supply system, a communication system, a perception system, a control system and a robot body. The communication system includes a wireless communication module, a wired communication interface and an encoder. The wireless communication module is installed near the main control unit of the robot body, the wired communication interface is installed on the robot body, and the encoder is installed on the robot body and connected to the moving parts of the robot body; the perception system includes a visual sensor module, a millimeter wave radar module, a laser radar module and an RFID module, and the visual sensor module, the millimeter wave radar module and the laser radar module are all installed on the sensor bracket of the robot body, the RFID module includes an RFID smart tag and an RFID tag reader, the RFID smart tag is arranged at the target intersection, and the RFID tag reader is arranged on the sensor bracket of the robot body, and the control system includes a motor controller, a motor sensor, a motor, a servo driver, a single-chip microcomputer and a communication expansion unit; the motor controller, the servo driver and the communication expansion unit are all arranged on the single-chip microcomputer, the motor includes a DC servo electric cylinder, a DC joint motor, a DC high torque reduction motor and a DC hub motor, the motor sensor is arranged on the motor, and the servo driver is arranged on two servo motor cylinders respectively, and the communication expansion unit connects the visual sensor module, the millimeter wave radar module and the laser radar module.
[0097] The power system used in this embodiment is a lithium battery module A. The communication system and control system are installed inside the robot, and the current road information is obtained through the traffic network. The communication system includes Figure 2 The intercom voice monitoring system shown in the figure can be used by the monitoring center to command the traffic road in real time through the intercom voice monitoring system. The information is fed back to the mechanical structure after being processed by the control system, so as to coordinate the vehicle and the road and command the current traffic. The visual sensor, millimeter wave radar, laser radar and RFID tag reader / writer inside the perception system are all arranged on the sensor bracket 1. The visual sensor recognition is mainly used, and the millimeter wave radar and laser radar are used as auxiliary. The reliability of target detection and road recognition is guaranteed when the line of sight is blocked and the environment is dim. After entering the working environment, the road condition perception algorithm can obtain sensor information in real time, identify vehicles and drivable areas. When there is no drivable area ahead, it is fed back to the monitoring center in real time. The monitoring center shouts through the intercom voice monitoring system to let the vehicles on both sides of the road give up the drivable area. The RFID tags are arranged at key road intersections where accidents frequently occur. When passing through the road intersection, the RFID tag reader / writer installed on the sensor bracket 1 reads the tag information, quickly obtains the current road information, and shortens the rescue time.
[0098] like Figure 1As shown, the robot body includes a robot frame, a three-jaw chuck 2, a mechanical arm 3, a sensor bracket 1, a folding plate 4, a rear wing 5, a driving wheel 6, a front wheel frame and a rear wheel frame. The three-jaw chuck 2 is located at the top of the mechanical arm 3 and is connected to a DC high-torque reduction motor a, so that it can achieve 360-degree rotation. In autonomous driving, the three-jaw chuck 2 clamps the steering wheel, and the control system controls the DC high-torque reduction motor to drive the three-jaw chuck 2 to achieve control of the steering wheel.
[0099] Figure 2 The invention discloses an intercom voice monitoring system, which can be used for voice broadcasting and reminding. Figure 3 Schematic diagram of autonomous driving posture of the present invention, as shown in Figure 3 As shown, at this time, the rear wing rotates to the upper limit position, and the sensor bracket 1 senses the road condition in the positive direction. Figure 4 This is a schematic diagram of the initial posture of independent walking of the present invention. At this time, the three-jaw chuck 2 is used to fix the display screen, which displays the current road information in real time according to the traffic network and is used to direct traffic. Figure 5 This is a schematic diagram of the posture after independent walking adjustment of the present invention. By adjusting the sensor bracket 1, the three-claw chuck 2 and the mechanical arm 3, the Figure 3 to Figure 4 In the autonomous driving posture, the three-jaw chuck 2 is used to control the steering wheel rotation, and the torque T=35Nm. In the independent walking posture, it is used to fix the display screen that displays the current road conditions and direct traffic. The robot arm 3 includes a first connecting rod 31 and a second connecting rod 32. The second end of the first connecting rod 31 and the first end of the second connecting rod 32 are rotatably connected. The first connecting rod 31 and the second connecting rod 32 have the same rod length. The first end of the first connecting rod 31 is fixedly connected to the three-jaw chuck 2, and the second end of the second connecting rod 32 is fixedly connected to the robot frame. The height of the robot arm can be adjusted and locked according to the working space. The sensor bracket 1 is slidably connected to the first connecting rod 31. The sensor bracket 1 is equipped with a visual sensor module, a millimeter wave radar module and a laser radar module. The sensor bracket 1 can slide and adjust on the first connecting rod 31 to meet different working modes. The folding plate 4 is rotatably connected to the front wheel frame, and two servo motor cylinders 101 and 102 are fixed on it. When the vehicle is driven autonomously, the folding plate 4 is flipped to the lower limit position to control the throttle and brake. The throttle pedal and the brake pedal are equipped with pressure sensors. The pedal control is accurate and fast, and the thrust F=350N can achieve the full stroke movement of the throttle and brake within 0.8s. When walking independently, the folding plate 4 is flipped to the upper limit position, and the driving wheel is folded to the lower limit position and walks independently. The rear wing 5 is rotatably connected to the rear wheel frame, and the driving wheel 6 includes two front driving wheels 61, 62 and two rear driving wheels 63, 64. The front driving wheel is connected to a DC joint motor 103 and a DC joint motor 104 to synchronously control the steering. The rear driving wheel is connected to a DC hub motor 105 and a DC hub motor 106, and the maximum speed is 10km / h.
[0100] Figure 6 The topological structure diagram of the present invention is shown as a schematic diagram. Its topological structure is composed of a main control chip that sends and receives various signals and protocols, including CAN signals of DC joint motor 103, DC joint motor 104 and DC large torque reduction motor a, DC hub motor 105 and DC hub motor 106, and servo motor cylinder 101 and servo motor cylinder 102 drivers. The data prediction stage is online on the main control chip. After reading and saving the weight file obtained in the data training module, the current road condition image of the robot's location is obtained from the visual sensor in real time, and the prediction program function is called to use the weight file for real-time calculation, prediction and classification recognition of road conditions. After obtaining the road condition label result, the information of the robot's travel direction is obtained and handed over to the controller algorithm for further processing. The signal receiver receives the SBUS signal, and the communication expansion unit has a UART protocol. The communication expansion unit is used to receive data information of the radar and the camera. The information on the main control chip is output to the control system to control the mechanical structure.
[0101] Figure 7 The figure shows the arrangement of the sensors of the present invention in the sensor bracket. To ensure safety, each area needs to be covered by two or more sensors for mutual verification. The visual sensor is installed by installing four surround cameras on all sides. At the same time, two monocular cameras are installed for the front view and one rear view camera is installed for the rear view. The millimeter wave radar includes two front and rear forward millimeter wave radars and four lateral millimeter wave radars. The two front lateral millimeter wave radars are arranged at 45 degrees to the driving direction of the robot, and the two rear lateral millimeter wave radars are arranged at 30 degrees to the driving direction of the robot. At the same time, a ranging laser radar and a lateral laser radar are arranged. When encountering an operating environment with obstructed vision, the laser radar can assist in sensing the environment to adapt to extreme environments. Reasonable arrangement of sensors can ensure the accuracy of target recognition and distance measurement.
[0102] Figure 8 This is a schematic diagram of the direction-speed joint control strategy in the robot's autonomous driving. Fig. 9 This is a diagram of the direction-speed joint control scheme for the robot's autonomous driving. The current drivable area is obtained through the road condition perception algorithm, and the central axis of the current drivable area is calculated. The next movement of the robot is determined according to the angle between the central axis of the drivable area and the direction of travel of the vehicle body. The steering wheel angle and throttle depth are used as control variables, and a controller is designed based on PID to maintain the car's steering angle following the angle change and maintain a constant distance from the vehicle body side to the central axis of the drivable area.
[0103] like Fig.10The figure shows the block diagram of the path planning and driving control algorithm based on PID of the present invention. The current position is determined according to the radar information. If the designated position has been reached, the road is directed through the intercom voice monitoring system. After the dredging is completed, the vehicle returns to the drivable dedicated lane. The task is completed. If there are still evacuation points, the vehicle enters the next evacuation position. The road condition perception algorithm is used to determine whether the vehicle can move forward at present. If not, the intercom voice monitoring system is used to guide the vehicles on both sides to give up the drivable area. The main control chip predicts the road condition label result to determine whether the vehicle is moving forward or turning left or right, and then the PID controller is used to control the speed and direction according to the set threshold.
[0104] Fig.11 The block diagram of the PID closed-loop control system is shown, where u(t) represents the output of the controller, that is, the signal between the controller link and the actuator. The parameter е represents the error, which is also E in the control system in the figure (equal to the expected output minus the actual output).
[0105] Fig.12 The relative position of the robot in the Frenet coordinate system is shown. Fig.13 The PID longitudinal control position error is shown. The longitudinal control is performed using dual PID. The longitudinal position error is used as the input of the position PID, the speed difference and the output of the position PID are used as the input of the speed PID, and the output of the speed PID and the actual acceleration are used together as the acceleration to perform longitudinal control of the vehicle.
[0106] The specific operation steps of the present invention are as follows:
[0107] like Figures 1 to 12 As shown, the specific operation steps of the traffic emergency robot with independent walking and autonomous driving functions during use of the present invention are as follows:
[0108] After installing and arranging the robot's communication system, perception system, and control system according to the topological structure diagram and the sensor layout diagram, turn on the robot's power system. The present invention completes communication with external devices through the communication system, accurately perceives the road conditions through the perception system, identifies targets on the road, extracts lane lines, and divides the drivable area. By adjusting the sensor bracket 1, three-claw chuck 2, mechanical arm 3, folding plate 4, and rear wing 5 of the present invention, the transition between autonomous driving and independent walking can be achieved. A variety of sensors are arranged on the sensor bracket 1. By adjusting the angle of the sensor bracket 1, the advantages of multiple sensors are brought into play to achieve accurate road condition perception. The three-claw chuck 2 clamps the vehicle steering wheel during autonomous driving, and the steering wheel is controlled by the DC high-torque reduction motor a. During autonomous driving, the display screen can be fixed to complete the implementation and broadcast of the current road conditions. The folding plate 4 is rotatably connected to the front wheel frame, and a servo motor cylinder 101 and a servo motor cylinder 102 are fixed thereon. When the vehicle is driven autonomously, the folding plate 4 flips to the lower limit position to control the throttle and brake, with high precision and high speed, and a thrust of F=350N, which can achieve full stroke movement of the throttle and brake within 0.8s. When walking independently, the folding plate 4 flips to the upper limit position, and the rear wing 5 is rotatably connected to the rear wheel frame. The driving wheel 6 includes two front driving wheels 61, 62 and two rear driving wheels 63, 64. The two front driving wheels 61, 62 are respectively connected to DC joint motors 103, 104 to synchronously control the steering. The two rear driving wheels 63, 64 are respectively connected to DC hub motors 105, 106, with a maximum speed of 10km / h.
[0109] Among them, the specific steps of the road condition perception algorithm are as follows:
[0110] First, use PID to control the longitudinal speed and automatically match the acceleration. Specifically, the difference between the expected speed and the current speed is used as the acceleration signal input. Speed - speed = acceleration (ignore the dimension), which is considered as a number minus a number. As long as it satisfies the requirement that the acceleration is very large at the beginning when it is far away from the target, the acceleration gradually slows down when it approaches the target, and the acceleration becomes 0 when it reaches the target. The planned speed is 10km / h, so we hope that the car will run at 10km / h. The starting acceleration is 0, and the initial acceleration is large. The acceleration gradually decreases during the acceleration process. When the speed reaches 10km / h, the acceleration is 0. When the speed does not reach 10km / h, there is always acceleration, and it is constantly accelerating to match the acceleration. The formula is:
[0111]
[0112] Among them, k p is the proportionality coefficient, T i is the integration time constant, T dis the differential time constant. Plan the trajectory with time, assuming the initial conditions x(0), y(0), and termination condition x(T), y(x end )、y'(x end )、y)(x end ), through these boundary conditions, we can calculate the quintic polynomial (six unknowns) x(t) and y(x) (y(x) represents the requirement for the slope of the curve), and through the derivation of the intermediate variables, we can get:
[0113] y(t)=y(x(t))
[0114] y'(t)=y'(x(t))·x'(t)
[0115]
[0116] Then y(t) can be obtained. The planning module calculates a reference trajectory. The actual driving trajectory of the robot needs to fit the reference trajectory as closely as possible. For this purpose, the control module needs to calculate the control information of the robot's throttle / brake and turning angle. The output of the lateral LQR controller is the turning angle, and the longitudinal position-speed PID controller is used to calculate the throttle / brake.
[0117] Lateral Control:
[0118] S1. Analyze the lateral controller error.
[0119] In the Frenet coordinate system, Fig.12 As shown, calculate the relative position of the robot:
[0120] dx=χ-x des
[0121] dy=yy des
[0122] At time t, the trajectory route function gives the time x r With y r The value of, that is:
[0123] x r (t) = x(t)
[0124] y r (t) = y(t)
[0125] θ r (t) = arctan{y'[x(t)]}
[0126]
[0127] The coordinates of the vehicle in the frenet coordinate system can be obtained by the two-dimensional coordinate system rotation formula:
[0128]
[0129] e=d
[0130]
[0131] Among them, e is the lateral error, θ is the heading angle error, is the lateral error change rate, is the rate of change of heading angle error, is the robot’s current heading angle, is the heading angle of the path reference point, v is the current speed of the robot, is the current yaw angular velocity of the robot, k des is the curvature of the path reference point.
[0132] The state quantity of the control system can be expressed as:
[0133]
[0134] S2, LQR lateral feedback control
[0135] (1) Establish a state-space model.
[0136] Assuming the state vector is x and the control input vector is u, the state space model can be expressed as:
[0137]
[0138] Among them, A and B are constant matrices, representing the state transition and input matrices of the system.
[0139] (2) Define the cost function.
[0140]
[0141] Among them, Q and R are positive definite matrices, representing the weight coefficients of the state and input.
[0142] S3. Calculate the optimal controller.
[0143] The goal of the LQR algorithm is to find an optimal controller that minimizes the cost function. The optimal controller can be expressed as a linear state feedback controller:
[0144] u=-Kx
[0145] Where K is the state feedback matrix, which needs to be calculated. Substituting the above controller into the state space model, we can get:
[0146]
[0147] Substituting into the cost function, we can get:
[0148]
[0149] By taking the derivative of the cost function, we can get the analytical expression of the state feedback matrix K:
[0150] K=(R+B T PB) -1 B T PA
[0151] where P is a positive definite matrix that satisfies the algebraic Riccati equation:
[0152] A T P+PA-PBR -1 B T P+Q=0
[0153] The Riccati equation can be solved by repeated iterations.
[0154] S4. Solve the control quantity.
[0155] According to the state feedback matrix K, the optimal control input can be calculated to achieve the optimal lateral control.
[0156] δ=u=-Kx
[0157] Here, δ is the calculated rotation angle.
[0158] Longitudinal control:
[0159] like Fig.13 As shown, the longitudinal position error is calculated as:
[0160]
[0161] The speed error is:
[0162]
[0163] The acceleration is:
[0164]
[0165] Longitudinal control uses dual PID to control speed, that is, use e s As the input of position PID, the speed difference and the output of position PID are used as the input of speed PID. The output of speed PID and acceleration are used together as acceleration to the car, so as to realize the speed control of the robot.
[0166] The steering angle obtained above can achieve lateral control, and the acceleration can achieve longitudinal control.
[0167] The present invention provides a traffic emergency robot with the dual functions of independent walking and autonomous driving. The robot can be installed on the main driving seat of a vehicle and cooperate with special vehicles to realize functions such as intelligent driving and vehicle-road collaboration, so that rescue missions can be safe and rapid. At the same time, multi-sensor fusion technology can realize high-precision and high-efficiency road condition perception, and can still perform efficient operations in environments such as blocked vision, poor signals, and high burial degrees. In addition, the robot also has the function of independent walking. When vehicles are difficult to enter, it can walk independently to realize environmental exploration, and can display and broadcast the current road conditions in real time to realize traffic supervision, patrol, and command, which is conducive to the smooth implementation of rescue missions, traffic command and supervision, and has the advantages of easy operation, efficient operation, low cost, and portability.
[0168] The embodiments described above are only descriptions of the preferred implementation modes of the present invention, and are not intended to limit the scope of the present invention. Without departing from the design spirit of the present invention, various modifications and improvements made to the technical solutions of the present invention by ordinary technicians in this field should all fall within the protection scope determined by the claims of the present invention.
Claims
1. A traffic emergency robot with the dual functions of independent walking and autonomous driving, characterized in that: It includes a power supply system, a communication system, a perception system, a control system and a robot body. The communication system includes a wireless communication module, a wired communication interface and an encoder. The wireless communication module is installed near the main control unit of the robot body, the wired communication interface is installed on the robot body, and the encoder is installed on the robot body and connected to the moving parts of the robot body; the perception system includes a visual sensor module, a millimeter wave radar module, a laser radar module and an RFID module. The visual sensor module, the millimeter wave radar module and the laser radar module are all installed on the sensor bracket of the robot body. The RFID module includes an RFID smart tag and an RFID tag reader / writer. The RFID smart tag is arranged at the target intersection, and the RFID tag reader / writer is arranged on the sensor bracket of the robot body. The control system includes a motor controller, a motor sensor, a motor, a servo driver, a single-chip microcomputer and a communication expansion unit; the motor controller, the servo driver and the communication expansion unit are all arranged on the single-chip microcomputer, the motor includes a DC servo electric cylinder, a DC joint motor, a DC high-torque reduction motor and a DC hub motor, the motor sensor is arranged on the motor, and the servo driver is arranged on two servo motor cylinders respectively. The communication expansion unit is connected to the visual sensor module, the millimeter wave radar module and the laser radar module; The robot body includes a robot frame, a three-jaw chuck, a mechanical arm, a sensor bracket, a folding plate, a rear wing, a front wheel frame, a rear wheel frame and a driving wheel. The three-jaw chuck is located at the top of the mechanical arm and is connected to a DC high-torque reduction motor. The three-jaw chuck can be clamped on the outside of the vehicle steering wheel. The mechanical arm includes a first connecting rod and a second connecting rod. The first end of the first connecting rod is fixedly connected to the three-jaw chuck, the second end of the first connecting rod is rotatably connected to the first end of the second connecting rod, and the second end of the second connecting rod is fixedly connected to the robot frame. The sensor bracket is slidably connected to the first connecting rod. The sensor bracket is installed on The sensing system, the folding plate is rotatably connected to the front wheel frame, two servo motor cylinders are fixed on the folding plate, the rear wing is rotatably connected to the rear wheel frame, the driving wheels include two front driving wheels and two rear driving wheels, the front driving wheels are connected to the DC joint motors, and the rear driving wheels are connected to the DC hub motors; the folding plate and the driving wheels both have upper and lower limits, when the vehicle is driven autonomously, the folding plate is flipped to the lower limit to control the throttle and brakes, and the driving wheels are folded to the upper limit; when walking independently, the folding plate is flipped to the upper limit, and the driving wheels are folded to the lower limit for independent walking; When the vehicle is driven autonomously, the current drivable area is obtained through the road condition perception algorithm, and the central axis of the current drivable area is calculated. The robot's next movement is determined based on the angle between the central axis of the drivable area and the direction of travel of the vehicle body. The steering wheel angle and throttle depth are used as control variables to make the vehicle steering angle follow the angle change and maintain a constant distance from the vehicle body side to the central axis of the drivable area. When clearing the road independently, the current position is determined based on the radar information. If the designated position has been reached, the road clearing is directed through the intercom voice monitoring system. After the clearing is completed, the vehicle returns to the dedicated lane where it can be driven. If there are still points that need to be evacuated, the vehicle enters the next road clearing position and determines whether it can move forward based on the road condition perception algorithm. If not, the intercom voice monitoring system is used to guide the vehicles on both sides to give up the drivable area. Based on the road condition label results obtained by the main control chip based on the data prediction, the vehicle determines whether it is currently moving forward, turning left or right, and then controls the speed and direction based on the set thresholds.
2. The traffic emergency robot with the dual functions of independent walking and autonomous driving according to claim 1 is characterized in that: The visual sensor module includes two monocular cameras, a rear-view camera and four surround-view cameras.
3. The traffic emergency robot with the dual functions of independent walking and autonomous driving according to claim 1 is characterized in that: The millimeter wave radar module includes four side radars and two front radars.
4. The traffic emergency robot with dual functions of independent walking and autonomous driving according to claim 1 is characterized in that: The laser radar module includes a multi-threaded speed measurement laser radar and a multi-threaded ranging laser radar.
5. The traffic emergency robot with dual functions of independent walking and autonomous driving according to claim 1 is characterized in that: The rod length of the first connecting rod is equal to the rod length of the second connecting rod.
6. The traffic emergency robot with independent walking and autonomous driving dual functions according to claim 1 is characterized in that: The sensor bracket and the fixed end of the first connecting rod are provided with movable buckles.
7. The traffic emergency robot with independent walking and autonomous driving dual functions according to claim 1 is characterized in that: The power supply system includes a lithium battery module, and the lithium battery module includes a voltage transformation module.
8. An autonomous driving method for a traffic emergency robot having the dual functions of independent walking and autonomous driving according to claim 1, characterized in that: It includes the following steps: S1. Obtain the current drivable area through the road condition perception algorithm; S2, calculate the central axis of the current drivable area, and determine the next movement of the robot according to the angle between the central axis of the drivable area and the traveling direction of the vehicle body; S3. With the steering wheel angle and throttle depth as control variables, the controller outputs execution commands to the actuator based on the PID algorithm to control the action of the motor so as to maintain the steering angle of the vehicle following the change of the included angle and maintain the distance from the side of the vehicle body to the center axis of the drivable area constant.
9. An independent walking method for a traffic emergency robot having the dual functions of independent walking and autonomous driving according to claim 1, characterized in that: S1. Determine whether the vehicle can move forward based on the road condition perception algorithm. If not, guide the vehicles on both sides to leave the driving area through the intercom voice monitoring system. S2. According to the road condition label result obtained by the main control chip from the data prediction, it is determined whether to move forward, turn left or turn right, and then the speed and direction are controlled using the PID algorithm according to the set threshold; S3. Determine the current position based on radar information. If the designated position has been reached, direct the road through the intercom voice monitoring system. After the evacuation is completed, return through the drivable area. The mission is completed. If there is still a position that needs to be evacuated, proceed to the next evacuation position.
10. The autonomous driving method of the traffic emergency robot according to claim 8, characterized in that: The specific steps of the road condition perception algorithm are as follows: S1. Plan the motion trajectory with time. The specific formula is: Among them, u(t) is the motion trajectory with time, k p is the proportionality coefficient, T i is the integration time constant, T d is the differential time constant; S2, using the output of the lateral controller as the steering wheel angle control amount to perform lateral control, including the following sub-steps: S21. Analyze the lateral controller error: In the Frenet coordinate system, calculate the relative position of the robot: dx=x-x des dy=yy des At time t, the trajectory route function gives the time x r With y r The value of, that is: x r (t)=x(t) y r (t)=y(t) θ r (t)=arctan{y'[x(t)]} The coordinates of the vehicle in the frenet coordinate system are obtained by the two-dimensional coordinate system rotation formula: e=d Among them, e is the lateral error, θ is the heading angle error, is the lateral error change rate, is the rate of change of heading angle error, is the robot’s current position heading angle, is the heading angle of the path reference point, v is the current speed of the robot, is the current yaw angular velocity of the robot, k des is the curvature of the path reference point; The state quantity of the control system is expressed as: S22, LQR lateral feedback control, specifically includes the following sub-steps: S221. Establish state space model: Assuming the state vector is x and the control input vector is u, the state space model can be expressed as: Among them, A and B are constant matrices, representing the state transition and input matrices of the system; S222. Define the cost function: J=∫0 ∞ (x T Qx+u T Ru)dt Among them, Q and R are positive definite matrices, representing the weight coefficients of state and input; S23, calculate the optimal controller: The optimal controller is expressed as a linear state feedback controller: u=-Kx Where K is the state feedback matrix. Substituting the above controller into the state space model, we get: Substituting into the cost function, we get: J=∫0 ∞ (x T Qx+x T K T RKx)dt By deriving the cost function, we get the analytical expression of the state feedback matrix K: K=(R+B T PB) -1 B T PA where P is a positive definite matrix that satisfies the algebraic Riccati equation: A T P+PA-PBR -1 B T P+Q=0 The Riccati equation is solved by repeated iterations; S24, solve the control quantity: According to the state feedback matrix K, the optimal control input is calculated to achieve the optimal lateral control δ=u=-Kx Among them, δ is the calculated rotation angle; S3, using the longitudinal position-speed PID controller to calculate the throttle depth for longitudinal control: The longitudinal position error is calculated as: The speed error is: The acceleration is: Longitudinal control uses dual PID to control speed, that is, using e s As the input of position PID, the speed difference between the expected speed and the current speed and the output of position PID are used as the input of speed PID, and the output of speed PID and acceleration are used as acceleration input together.
Citation Information
Patent Citations
Designated driving robot
CN107775641A
Quickly-installed automobile driving robot steering mechanical arm
CN107962575A
Intelligent traffic accident processing method and device
CN112712691A
Underground rail vehicle automatic driving robot and control method thereof
CN113467346A
Rapid centering slice type multi-jaw chuck for steering robot
CN114986537A
