CN109621260B

CN109621260B - Control system of fire extinguishing vehicle

Info

Publication number: CN109621260B
Application number: CN201811422530.XA
Authority: CN
Inventors: 杨建伟; 姚德臣; 刘恒畅; 寇子明; 杨玉青; 崔晓飞
Original assignee: Beijing University of Civil Engineering and Architecture
Current assignee: Beijing University of Civil Engineering and Architecture
Priority date: 2018-11-27
Filing date: 2018-11-27
Publication date: 2021-03-30
Anticipated expiration: 2038-11-27
Also published as: CN109621260A

Abstract

The invention provides a control system of a fire extinguishing vehicle, which comprises an analysis module, an error term calculation module, a proportional term calculation module, a differential term calculation module, an integral term calculation module and an inner ring calculation module. The main control board is matched with the chassis inertial navigation unit to realize closed-loop control on the chassis motor, the course angle control motor and the pitch angle control motor so as to ensure that the course can be adjusted in a self-adaptive manner and the deviation generated in the running process of the fire extinguishing vehicle is automatically corrected. Therefore, the fire extinguishing vehicle has higher control precision and more convenient operation.

Description

Control system of fire extinguishing vehicle

Technical Field

The invention relates to the field of fire fighting, in particular to a control system of a fire extinguishing vehicle.

Background

The existing fire extinguishing vehicle is limited by a driving wheel, and can realize steering or rotation only by needing a larger turning radius. In practical application, the existing fire extinguishing vehicle is often limited by the space of a fire scene, so that the condition of incapability of steering and movement resistance occurs.

Further, the existing fire extinguishing vehicle is often influenced by the terrain of a fire scene during the traveling process, and a speed difference is generated among the driving wheels. Under the influence of the speed difference, the fire extinguishing vehicle often has course deviation in the advancing process, and a control person in a background needs to pay attention to the advancing direction of the fire extinguishing vehicle all the time. Therefore, the existing fire extinguishing vehicle has low control efficiency in the advancing process, and the control personnel at the background often feel busy and disorderly.

Therefore, a fire extinguishing vehicle which has a small turning radius, is easy to realize comprehensive control, can adaptively adjust the heading and correct the deviation in the advancing process is urgently needed.

Disclosure of Invention

In order to solve the defects of the prior art, the invention aims to provide a control system of a fire extinguishing vehicle. The fire extinguishing vehicle comprises Mecanum wheels, a chassis motor, a chassis, a holder, a water tank, a course angle control motor, a pitch angle control motor, a chassis inertial navigation unit, a course angle control shaft, a pitch angle control shaft, a main control board and a remote control unit, and is characterized in that the system comprises:

the analysis module is used for receiving and analyzing messages fed back by the chassis motor, messages fed back by the course angle control motor or the pitch angle control motor, rotation quantities of the Mecanum wheel, the course angle control shaft or the pitch angle control shaft acquired by the chassis inertial navigation unit and remote control signals of the remote control unit, and acquiring a target rotation speed forward, a current rotation speed of the Mecanum wheel, an expected angle value and a current angle value;

an error term calculation module, which is used for calculating an error term err ═ ((forward-median of chassis motor speed signal)/660) × (maximum of chassis motor speed) -current of chassis motor speed, and an outer ring error term shell _ error ═ desired angle value-current angle value;

a scale term calculation module for calculating a scale term kp _ out ═ a scale coefficient err, and an outer ring scale term shell _ kpout ═ an outer ring scale coefficient shell _ errout;

the differential term calculation module is used for calculating a differential term kd _ out (err-old) which is a differential coefficient, and an outer ring differential term shell _ kdout (shell _ errout-shell _ oldout) which is an outer ring differential coefficient, wherein the shell _ oldout is an outer ring error term obtained by the last calculation, and old is an error term obtained by the last calculation;

the integral term calculating module is used for calculating an integral term ki _ out and an outer ring integral term shell _ kiout, wherein the integral term ki _ out is accumulated by an error term err obtained by each calculation and then multiplied by an integral coefficient to obtain integral term output, and the outer ring integral term shell _ kiout is accumulated by an outer ring error term shell _ errout obtained by each calculation and multiplied by an outer ring integral coefficient;

an inner loop calculation module to perform the steps of: firstly, calculating the expected angular velocity value of the inner ring, namely shell _ kpout + shell _ kdout + shell _ kiout, namely the output value of the outer ring; then, calculating an inner ring error term which is the angular velocity value expected by the inner ring-the current angular velocity value; finally, multiplying the inner ring error term by an inner ring proportion coefficient to obtain an inner ring proportion term; multiplying the inner ring error term obtained by the current calculation by an inner ring integral coefficient and adding the previous inner ring integral term output value to obtain an inner ring integral term; subtracting the last inner ring error value from the inner ring error value calculated this time, and multiplying the inner ring error value by an inner ring differential coefficient to obtain an inner ring differential term;

and the control signal output module is used for calculating and outputting a control signal pidanget to the course angle control motor or the pitch angle control motor so as to control the course angle control motor or the pitch angle control motor to operate, and outputting a control signal pidout to the chassis motor so as to control the chassis motor to operate, wherein pidout is kp _ out + ki _ out + kd _ out, and pidanget is an inner ring proportion term + an inner ring integral term + an inner ring differential term.

Preferably, the frequency of the control signal output by the control signal output module is not lower than 1 kHz.

Preferably, the mecanum wheels are used for driving the fire extinguishing vehicle, the chassis motor is connected with the mecanum wheels, and the chassis is used for fixing the mecanum wheels;

the holder is arranged on the chassis and comprises a course angle control shaft (41) and a pitch angle control shaft (42) which are rotationally connected, the course angle control shaft (41) is driven to rotate by a course angle control motor (411), and the pitch angle control shaft (42) is driven to rotate by a pitch angle control motor (421); and a water cannon (43) and a camera (44) are also arranged on the pitch angle control shaft (42).

Preferably, the chassis inertial navigation unit is respectively connected with the Mecanum wheel, the course angle control shaft and the pitch angle control shaft and is used for acquiring the rotation quantity of the Mecanum wheel, the course angle control shaft and the pitch angle control shaft;

the water tank is arranged at the front part of the chassis and is connected with the water cannon through a water pipe so as to supply water to the water cannon.

Preferably, the main control board is arranged at the rear part of the chassis, and a data transmission module (61) and a picture transmission module (62) are also arranged in the main control board; the data transmission module is in communication connection with the remote control unit and is used for interacting with the remote control unit;

the remote control unit also comprises an image display module and an image processing module, wherein the image transmission module is connected with the camera and wirelessly connected with the image processing module and is used for transmitting image signals acquired by the camera to the remote control unit;

the main control board is electrically connected with the chassis motor, the course angle control motor and the pitch angle control motor through a CAN bus, and is also electrically connected with the chassis inertial navigation unit so as to output control signals to the chassis motor, the course angle control motor and the pitch angle control motor according to the rotation quantity of the Mecanum wheel, the course angle control shaft and the pitch angle control shaft and remote control signals of a remote control unit and receive messages fed back by the chassis motor, the course angle control motor and the pitch angle control motor;

the main control board is also electrically connected with the water cannon and used for controlling the water cannon to spray water.

Preferably, four corners of the chassis are respectively provided with a group of Mecanum wheels, the Mecanum wheels arranged at the upper left corner and the lower right corner of the chassis are of a left-handed structure, the Mecanum wheels arranged at the upper right corner and the lower left corner of the chassis are of a right-handed structure, and the 4 groups of Mecanum wheels are respectively and independently driven by 4 chassis motors;

the 4 chassis motors respectively output corresponding driving force according to the control signals of the main control board to drive a group of Mecanum wheels connected with the chassis motors, the Mecanum wheels decompose the driving force output by the chassis motors to a driving direction and/or a driven direction, and the driving forces of the 4 groups of Mecanum wheels in the driving direction and the driven direction are matched with each other to realize the omnidirectional driving of the fire extinguishing vehicle.

Preferably, each set of Mecanum wheels is secured to the chassis by a separate suspension structure comprising at least one coil spring connected between the chassis and the set of Mecanum wheels.

Preferably, the main control board is configured to output a control signal to the chassis motor according to the rotation amount of the mecanum wheel acquired by the chassis inertial navigation unit (5) and a remote control signal of a remote control unit, according to the following steps:

step S1, receiving and analyzing a message fed back by the chassis motor, the rotation quantity of the Mecanum wheel obtained by the chassis inertial navigation unit and a remote control signal of the remote control unit, and obtaining a target rotation speed forward and a current rotation speed of the Mecanum wheel;

step S2, calculating an error term err ((forward-median of chassis motor speed signal)/660) maximum of chassis motor speed-current of chassis motor speed;

step S3, calculating: the scaling term kp _ out is a scaling factor err; a differential term kd _ out is a differential coefficient (err-old), wherein old is an error term obtained in the last calculation; the integral term ki _ out is accumulated for the error term err obtained by each calculation, and then multiplied by an integral coefficient to obtain integral term output;

and step S4, calculating and outputting a control signal pidout ═ kp _ out + ki _ out + kd _ out to the chassis motor.

Preferably, the main control board is configured to output a control signal to the heading angle control motor or the pitch angle control motor according to the rotation amount of the heading angle control shaft or the pitch angle control shaft acquired by the chassis inertial navigation unit (5) and a remote control signal of a remote control unit, according to the following steps:

step T1, receiving and analyzing the message fed back by the course angle control motor or the pitch angle control motor, the rotation quantity of the course angle control shaft or the pitch angle control shaft acquired by the chassis inertial navigation unit and the remote control signal of the remote control unit, and acquiring an expected angle value and a current angle value;

step T2, calculating the outer-loop error term shell _ error as the desired angle value — the current angle value;

step T3, calculating: an outer ring proportion term shell _ kpout is an outer ring proportion coefficient shell _ errout; an outer ring differential term shell _ kdout is an outer ring differential coefficient (shell _ error-shell _ oldout), wherein the shell _ oldout is an outer ring error term obtained by the last calculation; the outer ring integral term shell _ kiout is obtained by multiplying an outer ring error term shell _ error obtained by each calculation by an outer ring integral coefficient and then accumulating, and the outer ring integral term needs to be limited;

step T4, calculating the expected angular velocity value of the inner ring, namely shell _ kpout + shell _ kiout + shell _ kdout;

step T5, calculating an inner ring error term, which is the desired angular velocity value of the inner ring — the current angular velocity value; clipping the inner loop error term;

step T6, multiplying the inner ring error term by an inner ring proportion coefficient to obtain an inner ring proportion term; multiplying the inner ring error term obtained by the current calculation by an inner ring integral coefficient and adding the previous integral term output to obtain an inner ring integral term, wherein the inner ring integral term needs to be limited, and the limiting range is 500; taking the difference between the inner ring error term obtained by the calculation and the inner ring error term obtained by the previous calculation, and multiplying the difference by an inner ring differential coefficient to obtain an inner ring differential term;

step T7, calculating and outputting control signals to the course angle control motor or the pitch angle control motor

And (3) Pidangle is an inner ring proportion term + an inner ring integral term + an inner ring differential term.

The invention has the following beneficial effects:

according to the invention, 4 sets of Mecanum wheels are used, the chassis motor which is independently driven is arranged for each set of Mecanum wheels, and the control module coordinates the driving of the 4 sets of Mecanum wheels, so that the turning radius of the fire extinguishing vehicle can be effectively reduced to 0. The invention can realize the rotation and the oblique movement of the fire extinguishing vehicle in situ through the cooperative control of the control module on 4 groups of Mecanum wheels so as to adapt to the movement in a narrow fire scene space. In addition, the fire extinguishing vehicle is also provided with a data transmission module and a picture transmission module, so that the fire scene image and the fire extinguishing vehicle data can be acquired in real time, and the real-time comprehensive control is realized.

Furthermore, the invention realizes closed-loop control on the chassis motor, the course angle control motor and the pitch angle control motor by matching the main control board and the chassis inertial navigation unit so as to ensure that the course can be adaptively adjusted and the deviation generated in the traveling process of the fire extinguishing vehicle can be corrected. Therefore, the fire extinguishing vehicle has higher control precision and more convenient operation, and the deviation in the advancing process does not need to be corrected artificially.

Additional features and advantages of the invention will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention.

Drawings

The accompanying drawings are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the principles of the invention and not to limit the invention. In the drawings:

fig. 1 is a schematic view of an overall structure of a fire extinguishing vehicle according to the present invention;

FIG. 2 is a block diagram of a system of fire suppression vehicles according to the present invention;

FIG. 3 is a schematic diagram of a Mecanum wheel in a fire fighting vehicle according to the present invention;

FIG. 4 is a schematic diagram of the forces applied to a 4-set Mecanum wheel in a fire fighting vehicle according to the present invention as it moves longitudinally;

FIG. 5 is a schematic diagram of forces experienced during lateral movement of 4 sets of Mecanum wheels in a fire fighting vehicle according to the present invention;

FIG. 6 is a schematic diagram of forces applied to 4 Mecanum wheels moving diagonally in a fire fighting vehicle according to the present invention;

FIG. 7 is a schematic illustration of the forces applied to a 4-set Mecanum wheel in a fire fighting vehicle according to the present invention during in-situ rotation;

FIG. 8 is a schematic view of a chassis suspension structure in the fire fighting vehicle according to the present invention;

FIG. 9 is a schematic view of the configuration of the pan and tilt head in the fire fighting vehicle according to the present invention;

fig. 10 is a front view of the overall structure of the fire extinguishing vehicle according to the present invention;

fig. 11 is a left side view of the overall structure of the fire extinguishing vehicle according to the present invention;

fig. 12 is a plan view of the overall structure of the fire extinguishing vehicle according to the present invention;

fig. 13 is a channel map of a remote control unit in the fire extinguishing vehicle of the present invention;

FIG. 14 is a schematic view of thermal imaging of the fire suppression vehicle of the present invention;

fig. 15 is a schematic diagram of a voltage acquisition circuit in the fire extinguishing vehicle of the present invention.

Detailed Description

The preferred embodiments of the present invention will be described in conjunction with the accompanying drawings, and it will be understood that they are described herein for the purpose of illustration and explanation and not limitation.

The invention mainly discloses a Mecanum wheel-based omnidirectional fire extinguishing vehicle design, which comprises five systems, namely a Mecanum wheel motor driving system, a wireless image transmission system, a wireless control system, a holder control system and a visual identification system. The chassis system adopts a four-wheel drive Mecanum wheel structure, and four brushless speed reduction motors drive Mecanum wheels, so that the forward, transverse, diagonal, rotary and equal movement modes of the patrol car can be realized. The wireless image transmission system consists of a high-definition micro camera, a receiving screen and a TS835 image transmission module, so that an operator can remotely observe the state and the surrounding environment of the fire extinguishing vehicle, and then the advancing of the fire extinguishing vehicle, the rotation of a cradle head and the emission of a water cannon are controlled by the wireless control system, the wireless control system consists of a seven-way professional remote controller and a receiver, and the effective control distance is within 1 km. The visual recognition system can automatically recognize personnel or special objects in the camera image, detection information is transmitted to the upper computer through wireless data transmission, and the upper computer can display the traveling speed and the battery voltage of the trolley and the identification information of the visual recognition module.

The omnidirectional fire extinguishing vehicle provided by the invention can replace a fireman to go deep into a fire scene when the fire scene with unknown detection condition is detected, and the danger of the fireman during fire extinguishing is avoided. The fire extinguishing vehicle can meet the requirement of free movement in a narrow space in function, and meets the requirements of zero-radius rotation, movement in any direction, remote control, real-time image return to a command center and the like.

Table 1 shows the main performance indexes of the fire extinguishing vehicle provided by the present invention. The specific implementation thereof will be discussed in detail in the present embodiment.

TABLE 1 Omnidirectional deration Performance index based on Mecanum wheels

Fig. 1 is a schematic view showing an overall structure of a fire extinguishing vehicle according to the present invention, and a front view, a left side view and a top view thereof can be referred to fig. 10 to 12, respectively. This fire extinguishing vehicle includes: a Mecanum wheel 1 for driving the fire extinguishing vehicle; the chassis motor 2 is connected with the Mecanum wheels and used for providing driving force for the Mecanum wheels; a chassis 3 for fixing the Mecanum wheels; the cloud deck 4 is arranged on the chassis and comprises a course angle control shaft 41 and a pitch angle control shaft 42 which are rotationally connected, the course angle control shaft 41 is driven to rotate by a course angle control motor, and the pitch angle control shaft 42 is driven to rotate by a pitch angle control motor 4; the pitch angle control shaft 42 is also provided with a water cannon 43 and a camera 44; the chassis inertial navigation unit is respectively connected with the Mecanum wheels, the course angle control shaft and the pitch angle control shaft and is used for acquiring the rotation quantity of the Mecanum wheels, the course angle control shaft and the pitch angle control shaft; the water tank 45 is arranged at the front part of the chassis, is connected with the water cannon through a water pipe and is used for supplying water to the water cannon; the main control board 6 is arranged at the rear part of the chassis, and a data transmission module and a picture transmission module are also arranged in the main control board; the data transmission module is in communication connection with the remote control unit and is used for interacting with the remote control unit; the remote control unit also comprises an image display module and an image processing module, wherein the image transmission module is connected with the camera and wirelessly connected with the image processing module and is used for transmitting image signals acquired by the camera to the remote control unit; the main control board is electrically connected with the chassis motor, the course angle control motor and the pitch angle control motor through a CAN bus, and is also electrically connected with the chassis inertial navigation unit so as to output control signals to the chassis motor, the course angle control motor and the pitch angle control motor according to the rotation quantity of the Mecanum wheel, the course angle control shaft and the pitch angle control shaft and remote control signals of a remote control unit and receive messages fed back by the chassis motor, the course angle control motor and the pitch angle control motor; the main control board is also electrically connected with the water cannon and used for controlling the water cannon to spray water.

Referring to fig. 10, 11 or 12, the front part of the fire extinguisher is provided with a square box capable of placing a water tank, the size of the square box is 45 x 200 x 86mm, two battery racks for fixing batteries are placed on two sides of a holder, and a main control panel and a water pump are arranged on the rear part of a trolley. The upper portion of cloud platform has installed the camera base, can be used for installing the picture and pass the camera.

The main control board comprises a pan-tilt main control and a chassis main control shown in fig. 2.

The chassis master control respectively outputs control signals to four motors a, b, c and d connected with the chassis according to the instruction of the remote control unit and the rotation quantity acquired by the chassis inertial navigation unit so as to drive the chassis motors to drive the Mecanum wheels to run, thereby realizing the driving of the fire extinguishing vehicle. The chassis motor reports the running state of the chassis motor to an upper computer through a CAN bus through a chassis master controller.

Referring to fig. 3, a mecanum wheel is shown that is shaped like a bevel gear except that the helical teeth on the bevel gear are replaced by a roller that is free to rotate about its axis. Multiple mecanum wheels are typically combined in a fixed manner to provide the motion mechanism with such omni-directional motion capability. The Mecanum wheel is an omnidirectional wheel which is technically mature in the aspect of omnidirectional wheels. Which is mounted on the chassis of the fire extinguishing vehicle of the present invention in the manner shown in fig. 4. The body of the mecanum wheel has a left-handed and right-handed version (right-handed wheel and left-handed wheel in the upper right corner of fig. 4), respectively, and the diagonal lines shown in the wheel are the axial directions of the wheel roller structure that contact the ground. Each wheel is independently driven by a direct current motor, so that the omnidirectional mobile robot can move on the ground plane with three degrees of freedom. The force condition and the motion analysis of the robot body formed by the four omnibearing wheels are shown in figures 4 to 7, wherein Fa is the axial friction force applied to the wheel rollers when the wheels roll; fr is the friction experienced by the wheel rollers in driven motion (negligible compared to Fa); is the angular velocity vector at which each wheel rotates. If the wheels are ordinary wheels, the mounting combination shown in figures 4 to 7 can only drive the fire extinguishing vehicle to move back and forth, and if the fire extinguishing vehicle is required to turn, a steering mechanism is required to be mounted. However, the omni-directional moving wheel of the Mecanum wheel is characterized in that an axial force relative to the wheel body is generated, a fixed coordinate system is formed with the ground through the steering and speed control of each wheel, and the omni-directional movement of the whole train is realized at a certain angle. For the four combined motion modes shown in fig. 4 to 7, taking the clockwise rotation motion shown in fig. 7 as an example for analysis, in this case, the right two-wheel rotation vector direction is the right direction (the wheel turns to the rear), the left two-wheel direction is the left direction (the wheel turns to the front), and the rotation speeds are the same, we use newton's law and so on to judge the force receiving directions of the wheel roller axial force Fa and the wheel roller rolling friction force Fr experienced by each wheel as shown in fig. 7, and Fr and Fa of each wheel are respectively identical. The resultant force formed by the forces is a counterclockwise moment around the central axis of the robot structure body, so that the robot structure body is represented as a counterclockwise rotation, the rotation center of the robot structure body is on the central axis of the body of the omnidirectional mobile robot, so that the rotation radius is 0, and the flexible movement with the performance can be well utilized in places with narrow space.

In order to enable the trolley to run under various terrains and reduce the shaking of a chassis in the working process as much as possible, the design adopts an independent suspension structure and uses a spiral spring for damping. The specific installation of the damping system can be seen in fig. 8.

The 4 sets of Mecanum wheels are independently controlled by using RM3510 motors as chassis motors for driving the Mecanum wheels. RM3510 is produced by Dajiang corporation with a reduction ratio of 1: the brushless speed reducing motor of 19 has the characteristic of large torque, so that the trolley has good bearing capacity. Since 3510 is a brushless motor, it requires electrical modulation for driving. The RM3510 is generally connected with the 820R electric regulation, the input of the RM3510 is different from the input of a common electric regulation into a PWM signal, the 820R electric regulation uses a CAN bus for communication, and the ID of the electric regulation CAN be changed through a dial switch. The master control can identify different electric tunes through the ID value of the electric tunes. The electric speed of the motor is changed by controlling the current, and the electric speed can also feed back mechanical angle data and rotating speed data.

In the invention, the four Mecanum wheels can be independently controlled in the following modes: step S1, receiving and analyzing a message fed back by the chassis motor, the rotation quantity of the Mecanum wheel obtained by the chassis inertial navigation unit and a remote control signal of the remote control unit, and obtaining a target rotation speed forward and a current rotation speed of the Mecanum wheel; step S2, calculating an error term err ((forward-median of chassis motor speed signal)/660) maximum of chassis motor speed-current of chassis motor speed; step S3, calculating: the scaling term kp _ out is a scaling factor err; a differential term kd _ out is a differential coefficient (err-old), wherein old is an error term obtained in the last calculation; the integral term ki _ out is the accumulation of the error term err obtained by each calculation and then multiplied by an integral coefficient; and step S4, calculating and outputting a control signal pidout ═ kp _ out + ki _ out + kd _ out to the chassis motor. Therefore, the chassis motor can correct the running deviation of the chassis motor in time according to the control signal pidout and drive the Mecanum wheels in a preset mode, so that the control over the 4 Mecanum wheels is realized. The 4 Mecanum wheels are controlled by the main control board and are matched with each other to realize the omnidirectional drive of the fire extinguishing vehicle.

The main control system of the cradle head is mainly used for controlling a course angle control motor and a pitch angle control motor so as to drive a course angle control shaft and a pitch angle control shaft which are connected with the main control system of the cradle head, and the control of the water spraying direction of a water cannon and the shooting direction of a camera is realized. The specific control mode is as follows: step T1, receiving and analyzing the message fed back by the course angle control motor or the pitch angle control motor, the rotation quantity of the course angle control shaft or the pitch angle control shaft acquired by the chassis inertial navigation unit and the remote control signal of the remote control unit, and acquiring an expected angle value and a current angle value; step T2, calculating the outer-loop error term shell _ error as the desired angle value — the current angle value; step T3, calculating: an outer ring proportion term shell _ kpout is an outer ring proportion coefficient shell _ errout; an outer ring differential term shell _ kdout is an outer ring differential coefficient (shell _ error-shell _ oldout), wherein the shell _ oldout is an outer ring error term obtained by the last calculation; the outer ring integral term shell _ kiout is obtained by multiplying an outer ring error term shell _ error obtained by each calculation by an outer ring integral coefficient and then accumulating, and the outer ring integral term needs to be limited; step T4, calculating the expected angular velocity value of the inner ring, namely shell _ kpout + shell _ kiout + shell _ kdout; step T5, calculating an inner ring error term, which is the desired angular velocity value of the inner ring — the current angular velocity value; carrying out amplitude limiting on the inner ring error term, wherein the amplitude limiting range is plus or minus 100; step T6, multiplying the inner ring error term by an inner ring proportion coefficient to obtain an inner ring proportion term; multiplying the inner ring error term obtained by the current calculation by an inner ring integral coefficient and adding the previous inner ring integral term to obtain an inner ring integral term, wherein the inner ring integral term needs to be limited, and the limiting range is 500; the difference between the inner ring error term obtained by the calculation and the inner ring error term obtained by the previous calculation is multiplied by an inner ring differential coefficient to obtain an inner ring differential term; and step T7, calculating and outputting a control signal yaw to the heading angle control motor or the pitch angle control motor, wherein the control signal yaw is equal to an inner ring proportion term, an inner ring integral term and an inner ring differential term. Therefore, the course angle control motor or the pitch angle control motor is controlled by the main control panel according to the control signal, and the rotation quantity of the main control panel is corrected to a required value in time so as to keep the directions of the holder, the water cannon and the camera stable.

Here, a two-axis pan-tilt is used to control the angle between the water cannon and the camera, wherein, referring to fig. 9, a 6025 deceleration motor is used at the bottom of the pan-tilt to control the course angle of the water cannon, and a 6025 deceleration motor is also used at the right side of the pan-tilt to control the pitch angle of the water cannon as the course angle control motor and the pitch angle control motor on the pan-tilt. The RM6025 motor selected here is a control motor specially designed for two-shaft and three-shaft pan-tilt in the company of Da Jiang, and has the characteristics of high precision, fast response, large torque and the like. Meanwhile, the HG900 electric controller is arranged in the electric controller, and the integrated design of the motor and the electric controller provides convenience for the electric wiring of the vehicle body. 6025 the theoretical angle control precision of the motor can reach plus or minus 0.01 degree. The RM6025 motor and the chassis motor adopt CAN protocol for communication. The main control controls the rotating speed of the motor by sending the current value through a CAN protocol. The electric regulation can also feed back mechanical angle data, an actual torque current measured value and a torque current given value, and the frequency of sending data frames can reach 1 khz. Control of the speed loop or the position loop may be effected in dependence on the feedback data. In order to obtain a good fire extinguishing effect, a water cannon on a holder pumps water in a water tank by using an agricultural high-pressure high-power water pump with 12V power supply to form the water cannon to extinguish fire. The water pump draws water the water spray distance can reach 5-8 meters, has high-efficient pressure boost's effect, is fit for various atomizer, and the water yield of per minute can reach 4L in the actual test, and the water smoke fire extinguishing effect that the water cannon sent is also very outstanding. The pure copper core motor adopted by the water pump has nearly three times longer service life than the common aluminum wire motor, and is durable in use.

The camera arranged on the holder, the image transmission module arranged in the main control board, the image display module and the image processing module arranged at one end of the remote control unit jointly form a machine vision module. The machine vision module comprises a camera arranged on the cloud deck, an image transmission module arranged in the main control board, an image display module and an image processing module, wherein the image display module and the image processing module are arranged at one end of the remote control unit, and the image transmission module is connected with the camera and is wirelessly connected with the image processing module and used for transmitting image signals acquired by the camera to the remote control unit.

In order to realize the above control, in this embodiment, the main control board of the fire extinguishing vehicle selects stm32f407 as the main control chip, and as the design has many control tasks including a car chassis control task, a remote control signal capturing task, a visual recognition task, and a pan-tilt control task. The required task control frequency is also higher, and the stm32f407 with the highest running frequency of 168Mhz can meet the requirement of the design on the control task. stm32f4 has more outstanding advantages compared with other common chips, namely that the floating point operation unit is provided, so that hardware can directly process floating point operation, and the efficiency of codes is improved. Because the motor that this design used all uses the CAN agreement to communicate, and stm32f4 has two CAN bus transceivers, CAN build the CAN network with the motor, controls the motor, receives the data of motor feedback.

The design uses two stm32f407 chips, one chip is mainly used for a control task of a trolley chassis, a control signal of a remote controller is received, a gyroscope is used for resolving a task to obtain a direction angle of the trolley, so that the chassis can change the direction angle along with the horizontal rotation of the trolley holder, and the communication task with the holder master control is used for sending a course angle and a pitch angle to the holder master control to change the angle of the holder. The other is a cradle head main control chip which is mainly responsible for the control tasks of the cradle head and the water cannon, the attitude calculation task of the gyroscope and the communication with the machine vision module.

The main control board realizes wireless data interaction with the Da Jiang DT7 remote control system through the data transmission module so as to remotely control the fire extinguishing vehicle. The remote control distance can reach 1000m outdoors, the DBUS protocol is used for data transmission, 7 paths of remote control signals are combined on one signal line, and occupation of a signal port of the main control chip is saved. Six channels of remote control information are used in the design, and the mapping of 6 channels is shown in fig. 13.

Because the fire extinguishing vehicle all need be with cloud platform direction automatic calibration as current locomotive direction when starting at every turn, in order to guarantee the stability of water cannon and camera passback data when the dolly marchs, when the remote controller did not control the cloud platform, the cloud platform can keep at the horizontal angle to the rotation of cloud platform course angle can be followed to the dolly chassis. In order to achieve the functions, a GY-521mpu6050 inertial measurement unit is respectively used on a chassis and a pan-tilt, mpu6050 on the chassis is used for obtaining a heading angle of the chassis, mpu6050 on the pan-tilt is used for obtaining a pitch angle and a heading angle of the pan-tilt, and after 6050 is calibrated, coordinate systems of the two 6050 are basically consistent. The chassis can be adjusted according to the error of two 6050 course angles, so that the trolley chassis can follow the rotation of the holder. The 6050 of the pan-tilt not only keeps the same course angle of the chassis and the pan-tilt, but also needs to obtain the pitch angle, and the pan-tilt is ensured not to shake when the trolley travels through a control algorithm and can accurately respond to a signal for changing the pitch angle by a remote controller.

The fire extinguishing vehicle in the embodiment uses the openmv machine vision module to achieve the function of visual identification, so that the trolley can automatically identify a fire source in a fire scene, and sends an identification success signal to the holder main control through a serial port, and the openmv is an open-source, low-cost and powerful machine vision module. The OpenMV3 Cam M7 used in the design is a latest generation product of OpenMV series, and the Openmv3 adopts stm32f765vit6 as a main control chip and has 2M internal flash, which means that compared with the Openmv2, the image fluency and frame rate of the image recognition function of the Openmv3 are greatly improved. openmv uses C language to implement the core machine vision algorithm, providing Python programming interface. The simplification of the Python language in actual programming reduces the workload of programming, and compared with the C language, the Python language is easier to use and convenient to develop. openmv3 can also install various expansion boards additional such as wifi expansion board, infrared thermal imaging expansion board, realizes many extra functions, has just used infrared thermal imaging expansion board to carry out the task of detecting flame in this design, and infrared thermal imaging expansion board can detect the existence condition of surrounding environment heat to can judge whether someone or the existence of flame exist in the detection field of vision. The formed thermal imaging data is superposed on the image information collected by the camera through the middle strip of the graph 14, and background personnel can be more intuitively helped to judge the fire scene situation.

In the design, both the chassis motor and the holder motor are controlled by using a can protocol, the stm32 needs to use a can controller interface chip when accessing a can bus network, a module with a TJA1050 chip is used in the design to complete the function, the TJA1050 chip is the most common can controller interface chip, a 120-ohm terminal resistor is added in the circuit design, impedance matching is realized, the driving capability is ensured, and signal radiation is prevented during long-distance signal transmission. In the design, the cloud platform main control and the chassis main control respectively use a TJA1050 module to enable the cloud platform main control and the chassis main control to be accessed to a can network.

This design has the rotational speed value with dolly chassis motor, the attitude angle of cloud platform, and the battery voltage and the current detection state of burning things which may cause a fire disaster transmit the function of host computer, also can calibrate mpu6050 of cloud platform through the host computer simultaneously. The wireless data transmission module created by anonymous science is used for completing functions, the anonymous data transmission supports duplex communication, and the wireless data transmission system has the characteristics of being bidirectional, high-speed and stable, and the baud rate can reach 500000. The data transmission working mode, the air rate, the wireless channel, the retransmission times and the serial port rate can be configured, and the conflict can be effectively prevented when a plurality of machines work simultaneously. The anonymous data transmission uses STM32F103 as a main control chip, and uses NRF24l01, PA and LNA to complete the task of wireless communication.

The design has a remote control function, so that personnel can control the trolley at a place far away from a fire scene, and a reliable remote image transmission system is needed to achieve the function. Through experimental comparison, the TS835 image transmission system is adopted to transmit the image of the camera back in real time, and the image video acquisition card is also adopted to display the image transmitted and received on the computer. The effective mapping distance of the TS835 can reach 1km-2km, a mushroom antenna can be arranged on a transmitter, a 5.8G 14DB panel antenna is arranged on a receiver, and the transmission distance can be extended to 5 km.

The design can record video on a fire scene in real time and remotely transmit the video back to a computer through image transmission. The design requirement can be as clear as possible to collect fire scene image information, the size and the power consumption of the camera are not too large, and therefore a GoPro3 motion camera is selected as image collection equipment. When the device is used, the GoPro3 is provided with the waterproof shell to prevent the water cannon from influencing the camera, the mounting seat is designed above the turret, and the camera can be directly mounted above the turret after the waterproof shell is arranged.

The state of the battery voltage determines whether the cart can work normally. When the battery voltage is too low, the system will not work properly. Therefore, we need to detect the battery voltage in real time. Because the full-power voltage of the battery TB47d in Da Jiang used by the power supply battery in the design is 25.2v which is far higher than the voltage range acquired by the adc at the stm32 port, a resistance voltage-dividing circuit is adopted to divide the voltage of the power supply firstly, acquire the voltage and finally calculate the actual voltage value of the battery according to the attenuation proportion. Fig. 15 is a schematic diagram of a voltage acquisition circuit, which divides the voltage of the battery pack through resistors R1 and R3, inputs the divided voltage to a voltage interface in a control board through a simple voltage stabilizing circuit composed of a current limiting resistor and a diode, and samples a voltage signal to correspondingly control the fire extinguishing vehicle to start protection in time when the battery pack is abnormal.

Based on the hardware, when the control of each motor is specifically realized, the specific algorithm design is as follows:

the remote control receiver communicates via the DBUS protocol. Unlike conventional receivers that output multiple pwm signals, the DBUS protocol integrates multiple lines into one data line for communication. The analysis of the DBUS signal needs to firstly manufacture a level inverting circuit to convert the RS232 level of the DBUS signal into the TTL level, so that the single chip microcomputer can process the signal. After the inversion of the signal is completed, the single chip microcomputer receives data by using a serial port DMA. The length of the data frame received by the DMA is 18, and the data of the remote controller is stored in the array from low order to high order. Table 2 is remote controller information. Wherein sbus _ rx _ buffer is an unsigned character type array for setting the receiving remote control signal. Taking the data of the analysis channel 0 as an example, it can be seen from the figure that the valid data of the t channel 0 is 11 bits. According to the rules of the DBUS protocol, these 11 bits require the full 8 bits of sbus _ rx _ buffer [0] as the lower 8 bits and the lower 3 bits of sbus _ rx _ buffer [1] as the upper 3 bits. According to the rule, the sbus _ rx _ buffer [1] is shifted to the left by 8 bits and then forms 16-bit data with the sbus _ rx _ buffer [0] according to the bits or the bits, and finally all the high 5 bits of the 16-bit data are cleared to zero, so that the output value of the channel can be obtained

TABLE 2 remote controller information

The trolley chassis uses an RM3510 motor to provide power and is controlled through 820R electricity, the 3510 motor communicates through a CAN bus, and a main control chip sends a data frame with current data to control four motors of the chassis. Different bits in the DATA frame represent current set-points for different motors, e.g., the DATA [0] and DATA [1] bits in the DATA field represent the 8 higher and 8 lower bits, respectively, of a current set-point with a motor ID of 0x 201. Meanwhile, the motor CAN also feed back the mechanical angle and the actual rotating speed of the current motor through a CAN bus, and the fed back data frame identifies different motors through identifiers. Table 3 is a frame format of the 820R electrical tilt receive message, and table 4 is an electrical tilt feedback message format. The sending frequency of the message is 1kHz, the unit of the rotating speed value is RPM, and the numerical range of the mechanical angle of the motor is 0-8191.

Frame format of table 3820R electric receiving message

Table 4820R electric tuning feedback message

Due to the hardware, the rotation speed of each motor is not necessarily the same, and each motor has the same current value. In fact, even if the motors of the same model are different in rotating speed, in an actual experimental phenomenon, the trolley cannot run straight, deviates from a specified route and the like. To solve this problem we use the PID algorithm in the car chassis control.

PID is short for proportion, integral and differential, and is a common algorithm in closed-loop automatic control technology. The design adopts a position type PID algorithm.

The remote controller used in the design sends signals in the range of 364-. forward signal: forward or reverse. rotate signal: and controlling the trolley to rotate in situ. left _ right signal: and controlling the trolley to translate leftwards or rightwards. Taking the control of the wheel a by receiving the forward semaphore as an example, when the forward semaphore is in the median value of 1024, the trolley stops moving forward, and when the forward semaphore is smaller than 1024, the command of backing the trolley is equivalent to the command of backing the trolley. In practical tests, the maximum feedback speed value of the electric regulation is 1980, so that the speed loop PID control of the chassis of the trolley is determined. The specific flow of the speed loop PID control is as follows.

1) A proportional term output is obtained. Taking the control of the forward signal to any chassis motor as an example, after the remote controller signal is acquired, the difference is made with a median value 1024, and the difference is divided by 660 and finally multiplied by the maximum value 1980 of the rotating speed (namely, the expected value of the rocker signal is converted into the expected value of the speed). And obtaining an error value obtained by subtracting the rotating speed value fed back by the motor from the expected speed value, and multiplying the error value by a proportional coefficient to obtain an output value of a proportional term. The following equation is a calculation equation of the error term of the motor a.

forward.err_a＝((forward-1024)/660)*1980-speed_a

forward.kp_out_a＝forward.kp_a*forward.err_a

2) A derivative term output is obtained. And (3) subtracting the error obtained in the step (1) from the error obtained in the previous control, and multiplying the error by a differential coefficient to obtain a differential term output.

forward.kd_out_a＝forward.kd_a*(forward.err_a-forward.old_err_a)

3) An integral term output is obtained. And (3) accumulating the errors obtained in the step (1), and multiplying the errors by an integral coefficient to obtain an integral term output, wherein the integral term output needs to be added with an integral limiting slice to limit the size of the output. The problem that the integration effect is too large, the system oscillates, the integration amplitude limiting cannot be too small, the integration cannot be played due to too small integration, and static errors cannot be eliminated is solved.

4) A forward signal is obtained. And adding the proportional term output, the differential term output and the integral term output, namely the output of the forward signal on the motor 1.

forward.pid_out_a＝forward.kp_out_a+forward.kd_out_a+forward.ki_out_a

The final current set value of each chassis motor is determined by the common superposition of the respective speed ring output values of a forward signal forward _ out, a side shift signal left _ right _ out and a rotation signal rotate _ pid _ out, and the following formula is a formula of the current set value of the chassis motor a.

forward＝forward.pid_out_a-left_right.pid_out_a-rotate.pid_out_a

Similarly, the pan-tilt motor uses an RM6025 motor similar to the chassis motor, and the RM6025 is also a CAN bus communication. And controlling the motor to rotate by sending a data frame with the current value of the holder motor, wherein the ID of the Pitch axis motor is 0x201, and the ID of the Yaw axis motor is 0x 203. The cradle head is provided with an MPU650 inertia measurement unit which can monitor the angles of the cradle head on a Pitch axis and a Yaw axis in real time. Table 5 is a 6025 motor reception data protocol table, table 6 is a 6025 motor feedback data protocol table, and the current range of the motor is-5000 to + 5000.

Table 56025 Motor reception data protocol table

Table 66025 motor feedback data protocol table

When the motor feeds back data, the frequency of a message is 1kHz, the feedback value range of the mechanical angle of the motor is 0-8191 (ox1FFF), the measurement value range of the actual current of the motor is-13000, and the numerical range of the Hall switch is 1-6.

The cloud deck influences the accuracy of fire extinguishing of a water cannon and the stability of a returned image. Therefore, the tripod head must be kept stable in the traveling process, so that the jolt caused by the terrain is prevented, and the control of the remote controller can be accurately and sensitively responded. In order to meet the requirements, a cascade PID control algorithm is used for controlling a pan-tilt motor.

The cascade PID consists of an outer ring angle ring and an inner ring speed ring. Both rings are implemented using position-based PIDs.

Taking the pitch axis pan-tilt motor as an example, the input of the outer ring angle ring is the desired angle value and the current motor feedback angle value. The output is then the desired angular velocity as the inner ring. And (3) subtracting the input angle from the current angle to obtain an angle difference, and then carrying out amplitude limiting on the angle difference, wherein the amplitude limiting is positive or negative 100. The current error is multiplied by a proportional coefficient, an integral coefficient is multiplied by the current error and then accumulated with the previous integral term output, and a differential coefficient is multiplied by the difference between the current error and the last error and then added to obtain the output of the angular velocity loop. It should be noted that the derivative term must be added with a low pass filter to prevent abrupt changes in the derivative. Finally, a maximum value of the clipped rotational speed value must be added to the output to be set to 1500, so that the output clipping is 1500.

The inputs to the speed ring are the output of the outer ring and the x-axis angular speed value of mpu 6050. Similar to the angular velocity loop, the angular velocity value of 6050 is different from the output of the outer loop, the output of the inner loop, i.e. the current, also needs to be limited, and the limited value is given to 4600. And finally, adding a first-order low-pass filter to the current value output by the inner ring, and setting the cut-off frequency to be 1 Hz.

In order to realize automatic identification of a fire source or personnel in a fire scene and reduce the difficulty of searching for the fire source or trapped personnel under the condition of a complex fire scene, the invention uses an Openmv machine vision module and a matched special infrared thermal imaging expansion board to complete the function of visual identification of the fire source, the effective identification distance of the Openmv module can reach 3m after a telephoto lens is additionally arranged, and the detection range of the temperature of a target object is between-50 ℃ and 300 ℃. When detecting a fire source in an environment, specifically, a photosensitive element driver (sensor) and a thermal imaging expansion board driver (fir) in a MicroPython function library can be called, and the fire source is identified by detecting heat in the surrounding environment, we need to set a pixel mode of a camera module to an RGB565 format, set a frame size to QQVGA (160 x 120), and then initialize a thermal imaging expansion board, wherein the initialization uses a default configuration, that is, a power-2 update rate is 64Hz, and a measurement resolution is set to 18 bits. The measurement resolution during initialization influences the maximum value of the measured temperature, and the higher the resolution digit is, the lower the maximum value of the measured temperature is. Because the ignition point of paper is only about 130 degrees, the ignition point of wood is between 200-300 degrees according to different environments, and therefore the highest value of the measured temperature is only about 400 degrees. Therefore, the design selects the measurement resolution of 18 bits, the resolution can reach the highest, and the maximum measurement temperature value is 450 degrees.

The lowest temperature and the highest temperature can be obtained through a fir _ ir () function, when the highest temperature is detected to exceed 250 ℃, alarm information is sent to a holder main control board, then information is sent to a chassis main control by the holder main control, and finally the chassis main control sends the alarm information to an upper computer through wireless data transmission

The technical scheme of the invention has the advantages that: the chassis based on Mecanum wheels is adopted, the function of all-directional movement can be realized, and the damping design can work on various terrains. In the aspect of the power of the trolley, a large-torque speed reducing motor is adopted as a chassis driving motor, so that the trolley can carry a large-capacity water tank or other fire extinguishing materials. The trolley adopts a two-shaft cradle head to carry a small water cannon, a picture transmission camera and a machine vision module. The cradle head is controlled by adopting a cascade pid algorithm, so that the stability of the water cannon in the advancing process of the trolley is ensured, and meanwhile, the gun turret can quickly and accurately respond to the response of the remote controller. The trolley also uses an openmv machine vision module to identify and automatically detect human bodies or objects in a fire scene according to requirements. The real-time image transmission system can transmit images back to the upper computer, so that personnel can control the trolley in a safety region far away from a fire scene through a remote controller. The upper computer can display the running speed and state of the trolley and the detection of the object.

Those of ordinary skill in the art will understand that: although the present invention has been described in detail with reference to the foregoing embodiments, it will be apparent to those skilled in the art that changes may be made in the embodiments and/or equivalents thereof without departing from the spirit and scope of the invention. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. The utility model provides a control system of car of putting out a fire, the car of putting out a fire includes mecanum wheel, chassis motor, chassis, cloud platform, water tank, course angle control motor, pitch angle control motor, chassis inertial navigation unit, course angle control shaft pitch angle control shaft, main control board, remote control unit, its characterized in that, this system includes:

2. The control system of claim 1, wherein the control signal output module outputs the control signal at a frequency of not less than 1 kHz.

3. The control system according to claim 1 or 2,

the Mecanum wheels are used for driving the fire extinguishing vehicle, the chassis motor is connected with the Mecanum wheels, and the chassis is used for fixing the Mecanum wheels;

4. The control system of claim 3,

the chassis inertial navigation unit is respectively connected with the Mecanum wheels, the course angle control shaft and the pitch angle control shaft and is used for acquiring the rotation quantities of the Mecanum wheels, the course angle control shaft and the pitch angle control shaft;

5. The control system of claim 4,

the main control board is arranged at the rear part of the chassis, and a data transmission module (61) and a picture transmission module (62) are also arranged in the main control board; the data transmission module is in communication connection with the remote control unit and is used for interacting with the remote control unit; the remote control unit also comprises an image display module and an image processing module, wherein the image transmission module is connected with the camera and wirelessly connected with the image processing module and is used for transmitting image signals acquired by the camera to the remote control unit;

6. The control system of claim 5, wherein a set of Mecanum wheels are disposed at each of four corners of the chassis, the Mecanum wheels disposed at the upper left corner and the lower right corner of the chassis are in a left-handed configuration, the Mecanum wheels disposed at the upper right corner and the lower left corner of the chassis are in a right-handed configuration, and the 4 sets of Mecanum wheels are independently driven by 4 chassis motors;

7. The control system of claim 6, wherein each set of Mecanum wheels is secured to the chassis by a separate suspension structure comprising at least one coil spring coupled between the chassis and the set of Mecanum wheels.

8. The control system according to claim 7, wherein the main control board is configured to output a control signal pidout to the chassis motor based on the rotation amount of the mecanum wheel obtained by the chassis inertial navigation unit (5) and a remote control signal of a remote control unit, according to the following steps:

9. The control system of claim 8, wherein the main control board is configured to output a control signal to the course angle control motor or the pitch angle control motor according to the rotation amount of the course angle control shaft or the pitch angle control shaft acquired by the chassis inertial navigation unit (5) and a remote control signal of a remote control unit, according to the following steps:

and step T7, calculating and outputting a control signal yaw to the heading angle control motor or the pitch angle control motor, wherein the control signal yaw is equal to an inner ring proportion term, an inner ring integral term and an inner ring differential term.