Following type motion chassis control system

Abstract

The invention discloses a following type motion chassis control system which comprises a central processor and a handheld device, the handheld device comprises a label module, and the central processor comprises a chassis control module. The chassis control module is composed of a base station module, an obstacle avoidance module, a control module, a steering module, a driving module, a braking module, a power module and a system program. The UWB positioning technology is adopted, compared with WIFI, GPS, Bluetooth and other technologies, high-precision measurement and tracking of the position of the label equipment can be achieved, real-time position tracking and control can be achieved, the method is suitable for application scenes needing quick response, the positioning function is achieved under the condition that a large number of base stations do not need to be deployed in advance, and the positioning efficiency is improved. The flexibility and deployment efficiency of the system are effectively improved, accurate positioning can be realized in the presence of obstacles or in a complex environment, and the method is suitable for positioning and tracking tasks in the complex environment.

Description

Technical Field

[0001] The invention relates to the technical field of motion chassis, in particular to a follow-up motion chassis control system. Background Art

[0002] The following mobile chassis technology is a technology similar to unmanned driving. It mainly guides the chassis to repeat the same moving trajectory by tracking the moving trajectory of the target material. In the actual application scenario, the user holds a device with a built-in UWB tag, and there is a UWB base station device on the chassis. The base station communicates with the tag. During the user's movement, the base station can always locate the position of the tag and notify the chassis controller of the location information. The controller drives the chassis to move and follow the user. This technology is mainly composed of UWB technology and wire-controlled chassis technology.

[0003] UWB-AOA (Ultra-Wideband-Arrival Angle Ranging) technology is used in target positioning. It realizes data transmission through extremely narrow pulses in nanoseconds, and uses a bandwidth of more than 500mHz. It is cheaper, has low signal power spectrum density, is insensitive to channel fading, and can perform high-precision distance measurement with an accuracy of centimeters. The disadvantage of UWB is that the transmission distance is not far enough and the signal is easily interfered by the external environment. Therefore, this technology is not suitable for use in the field of unmanned driving. The drive, steering, parking and other operations of the wire-controlled chassis are all controlled by electronic signals. Compared with the traditional mechanical chassis, it has better accuracy, but it is highly dependent on electronic components, electronic failures may occur, and it is highly dependent on power supply. In the event of power failure, it cannot work normally. The complexity is much higher than that of the traditional mechanical chassis. Summary of the invention

[0004] The object of the present invention is to provide a follow-up motion chassis control system to solve the problems raised in the above background.

[0005] To achieve the above-mentioned purpose, the present invention provides the following technical solutions: a follow-up motion chassis control system, including a central processor and a handheld device, the handheld device includes a tag module, the central processor includes a chassis control module, and the chassis control module is composed of a base station module, an obstacle avoidance module, a control module, a steering module, a drive module, a braking module, a power supply module, and a system program; the chassis control module is used to identify the location of the handheld device through UWB communication technology, and send an operating instruction to the drive system to automatically move the chassis to the location of the handheld device.

[0006] Preferably, the power module is used to provide power to all modules included in the chassis, the input end of the power module is connected to an external power supply, the output end of the power module is connected to a 48V power converter, and the power required by the 48V power converter is separated into 4 output ends through wiring terminals; the output end 1 of the 48V power converter is connected to a 12V power converter, the output end 2 of the 48V power converter is connected to the input end of the steering system, the output end 3 of the 48V power converter is connected to the input end of the drive system, and the output end 4 of the 48V power converter is connected to the input end of the braking system; the 48V direct current connected to the 12V power converter is stepped down and converted into 12V direct current, and the 12V power converter is separated into 3 output ends through wiring terminals, which are respectively connected to the 5V power converter of the obstacle avoidance module, the 5V power converter of the control module, and the 5V power converter of the base station module.

[0007] Preferably, the obstacle avoidance module includes a control center and a power supply, the control center is internally provided with a central processing unit, a memory, an asynchronous serial communication interface and a serial peripheral interface, the 5V power converter of the power supply is respectively connected to the ultrasonic sensor, the STM32G030C8T6 processor and the communication module, the input port of the ultrasonic sensor is connected to the signal output port 1 of the STM32G030C8T6 processor, the output port of the ultrasonic sensor is connected to the signal input port of the STM32G030C8T6 processor, and the signal output port 2 of the STM32G030C8T6 processor is connected to the communication module.

[0008] Preferably, the 5V power converter of the control module is respectively connected to the STM32F407ZET6 processor, the first communication module 1 and the second communication module, the first communication module is connected to the communication module in the base station module by TTL communication technology, the second communication module is connected to the communication module in the obstacle avoidance module by TTL communication technology, the output end of the first communication module is connected to the first signal input port of the STM32F407ZET6 processor, the input end of the first communication module is connected to the first signal output port of the STM32F407ZET6 processor, the input end of the second communication module is connected to the second signal output port of the STM32F407ZET6 processor, and the output end of the second communication module is connected to the second signal input port of the STM32F407ZET6 processor.

[0009] Preferably, the 5V power converter in the base station module is respectively connected to a signal transceiver module, an STM32G030C8T6 processor, and a communication module; the signal transceiver module is connected to a tag module in a handheld device through an antenna array using UWB communication technology; the output end of the communication module is connected to a signal input port 1 of the STM32G030C8T6 processor, and the input end of the first communication module is connected to a signal output port 1 of the STM32G030C8T6 processor.

[0010] Preferably, the obstacle avoidance module transmits an ultrasonic signal through an ultrasonic sensor, and the STM32G030C8T6 processor in the obstacle avoidance module calculates the distance of the obstacle by measuring the time difference between transmitting the ultrasonic wave and receiving the reflected ultrasonic wave.

[0011] Preferably, the STM32F407ZET6 processor in the control module sends a movement instruction to the drive system after receiving the data given by the coordinate determination. Preferably, the system program consists of a first initialization program, a first interrupt program, and a first control program; the first initialization program is used to control the system state, timer, crystal oscillator, and I / O for initialization settings; the first interrupt program consists of an obstacle avoidance serial port interrupt and a positioning serial port interrupt, the obstacle avoidance serial port interrupt is used to receive data from the obstacle avoidance module, and the positioning serial port interrupt is used to receive positioning data from the base station module; the first control program is used to drive the PID control algorithm and the mechanical movement of driving, braking, and stopping.

[0012] Preferably, the positioning program is composed of a second initialization program, a second interrupt program, and a second control program; the second initialization program is used to initialize the system status, timer, crystal oscillator, I / O, etc.; the second interrupt program is mainly used to receive positioning data from the tag, and the second control program is used to receive instructions from the control program.

[0013] Preferably, the obstacle avoidance program is composed of a third initialization program, a third interruption program, and a third control program; the third initialization program has the same function as the first initialization program, and the second interruption program is mainly used to receive obstacle avoidance signals.

[0014] The beneficial effects of the present invention are as follows: The present invention adopts a multi-processor distributed control mode, with an STM32F407ZET6 chip as the central processor of a chassis control module, an STM32G030C8T6 chip as the processor of an obstacle avoidance module, and an STM32G030C8T6 chip as the processor of a base station module. The advantage of this distributed architecture is that the work division of each processor can be clarified, each processor performs its own duties in the data processing process, burr data is filtered, calculation errors are reduced, the accuracy of data processing is increased, the calculation process is simplified, the burden of the chip is reduced, and the effects of improving efficiency, reducing response time, and reducing power consumption are achieved.

[0015] The positioning technology of the present invention adopts UWB positioning technology. Compared with WIFI, GPS, Bluetooth and other technologies, it can not only realize high-precision measurement and tracking of the location of tag devices, but also realize real-time location tracking and control. It is suitable for application scenarios that require rapid response, and does not require the deployment of a large number of base stations in advance to realize the positioning function, effectively improving the flexibility and deployment efficiency of the system, and can achieve accurate positioning even in the presence of obstacles or complex environments. It is suitable for positioning and tracking tasks in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 This is a schematic diagram of the modules of the motion chassis control system proposed by the present invention; Figure 2 A schematic diagram of the system program proposed by the present invention; DETAILED DESCRIPTION

[0017] The present invention provides a technical solution: a follow-up motion chassis control system, comprising a central processor and a handheld device, the handheld device comprising a tag module, the central processor comprising a chassis control module, the chassis control module comprising a base station module, an obstacle avoidance module, a control module, a steering module, a drive module, a brake module, a power supply module, and a system program; the chassis control module is used to identify the location of the handheld device through UWB communication technology, and send an operating instruction to the drive system to automatically move the chassis to the location of the handheld device.

[0018] The power module is used to provide power to all modules included in the chassis. The input end of the power module is connected to an external power supply, and the output end of the power module is connected to a 48V power converter. The power required by the 48V power converter is divided into 4 output ends through the wiring terminals; output end 1 of the 48V power converter is connected to the 12V power converter, output end 2 of the 48V power converter is connected to the input end of the steering system, output end 3 of the 48V power converter is connected to the input end of the drive system, and output end 4 of the 48V power converter is connected to the input end of the braking system; the 48V DC power connected to the 12V power converter is stepped down and converted into 12V DC power. The 12V power converter is divided into 3 output ends through the wiring terminals, which are respectively connected to the 5V power converter of the obstacle avoidance module, the 5V power converter of the control module, and the 5V power converter of the base station module.

[0019] The obstacle avoidance module includes a control center and a power supply. The control center is equipped with a central processing unit, a memory, an asynchronous serial communication interface and a serial peripheral interface. The 5V power converter of the power supply is respectively connected to the ultrasonic sensor, the STM32G030C8T6 processor and the communication module. The input port of the ultrasonic sensor is connected to the signal output port 1 of the STM32G030C8T6 processor, the output port of the ultrasonic sensor is connected to the signal input port of the STM32G030C8T6 processor, and the signal output port 2 of the STM32G030C8T6 processor is connected to the communication module.

[0020] The 5V power converter of the control module is respectively connected to the STM32F407ZET6 processor, the first communication module 1 and the second communication module, the first communication module is connected to the communication module in the base station module through TTL communication technology, the second communication module is connected to the communication module in the obstacle avoidance module through TTL communication technology, the output end of the first communication module is connected to the first signal input port of the STM32F407ZET6 processor, the input end of the first communication module is connected to the first signal output port of the STM32F407ZET6 processor, the input end of the second communication module is connected to the second signal output port of the STM32F407ZET6 processor, and the output end of the second communication module is connected to the second signal input port of the STM32F407ZET6 processor.

[0021] The 5V power converter in the base station module is respectively connected to the signal transceiver module, the STM32G030C8T6 processor, and the communication module. The signal transceiver module is connected to the tag module in the handheld device through the antenna array using UWB communication technology. The output end of the communication module is connected to the signal input port 1 of the STM32G030C8T6 processor, and the input end of the first communication module is connected to the signal output port 1 of the STM32G030C8T6 processor.

[0022] The obstacle avoidance module transmits ultrasonic signals through ultrasonic sensors. The STM32G030C8T6 processor in the obstacle avoidance module calculates the distance of the obstacle by measuring the time difference between transmitting ultrasonic waves and receiving reflected ultrasonic waves.

[0023] The STM32F407ZET6 processor in the control module sends movement instructions to the drive system after receiving the data given by the coordinate determination.

[0024] The system program consists of a first initialization program, a first interrupt program, and a first control program; the first initialization program is used to control the system state, timer, crystal oscillator, and I / O for initialization settings; the first interrupt program consists of an obstacle avoidance serial port interrupt and a positioning serial port interrupt, the obstacle avoidance serial port interrupt is used to receive data from the obstacle avoidance module, and the positioning serial port interrupt is used to receive positioning data from the base station module; the first control program is used to drive the PID control algorithm and the mechanical movement of driving, braking, and stopping.

[0025] The positioning program is composed of a second initialization program, a second interrupt program, and a second control program; the second initialization program is used to initialize the system status, timer, crystal oscillator, I / O, etc.; the second interrupt program is mainly used to receive positioning data from the tag, and the second control program is used to receive instructions from the control program; the positioning algorithm mainly realizes filtering of positioning data and calculating the coordinates of the tag.

[0026] The obstacle avoidance program consists of a third initialization program, a third interrupt program, and a third control program; the third initialization program has the same function as the first initialization program, and the second interrupt program is mainly used to receive obstacle avoidance signals; the obstacle avoidance algorithm mainly realizes the functions of filtering obstacle avoidance signals and calculating obstacle distances.

[0027] Working principle: The positioning tag and the chassis base station obtain the time t when the signal reaches the target through wireless communication. The base station communicates with the STM32G030C8T6 processor through TTL communication, and transmits the time t to the STM32G030C8T6 processor in real time. The processor regards the base station chassis as the origin coordinate and calculates the relative position of the tag. Since the antenna array in the base station is composed of three antennas, the processor assumes that the distance between each antenna is d, the propagation speed of the signal is the speed of light c, and Δt is the time difference of the signal, that is, the time difference between the two antennas receiving the signal. Then the distance d=c*Δt / 2, the signal arrival angle θ=arcsin(λ*ΔΦ / 2π*d); then the polar coordinates of the tag are (c*Δt / 2,arcsin(λ*ΔΦ / 2π*d)). Since there are three antennas, three sets of polar coordinate data are obtained, and then the final coordinate data is obtained according to the trilateral measurement method. This data enters the STM32F407ZET6 processor in the control module through the communication module; Ultrasonic sensors emit ultrasonic signals, which propagate in space in the form of sound waves. The frequency of ultrasonic waves is usually between 20kHz and 200kHz, which is beyond the range of human hearing. Once ultrasonic waves encounter obstacles, part of them will be reflected back by the obstacles. The ultrasonic sensor receives these reflected ultrasonic signals. The STM32G030C8T6 processor in the obstacle avoidance module calculates the distance of the obstacle by measuring the time difference between the emission of ultrasonic waves and the reception of reflected ultrasonic waves. The calculation formula is: distance s=v*Δt, v ​​is the speed of sound waves in the air, which is about 343 meters per second. The distance s enters the STM32F407ZET6 processor in the control module through the communication module. The STM32F407ZET6 processor in the control module will send a moving instruction to the drive system when receiving the data given by the coordinate judgment, and the drive system will drive the chassis to move to the current position; at the same time, the control module will judge the steering angle. When the steering angle is too small, no instruction will be sent to the steering system. On the contrary, a steering instruction will be sent to the steering system, and the steering system will drive the chassis to turn; when the chassis is moving, when the obstacle avoidance judgment gives the obstacle coordinates, the processor will determine whether the obstacle coordinates are lower than the preset value of the obstacle distance in the system. When it is lower than the preset value, the control module will send a stop instruction to the braking system, and the braking system will perform a braking operation to stop the chassis from moving; when it is higher than the preset value, the control system will not interfere with the movement of the chassis; While embodiments of the present invention have been shown and described, those skilled in the art will appreciate that numerous changes, modifications, substitutions and variations may be made to the embodiments without departing from the spirit and scope of the present invention, the scope of which is defined by the claims and their equivalents.

Claims

1. A following motion chassis control system, comprising a central processor and a handheld device, characterized in that: The handheld device includes a tag module, the central processor includes a chassis control module, and the chassis control module is composed of a base station module, an obstacle avoidance module, a control module, a steering module, a drive module, a brake module, a power module, and a system program; the chassis control module is used to identify the location of the handheld device through UWB communication technology, and send an operating instruction to the drive system to automatically move the chassis to the location of the handheld device.

2. The following motion chassis control system according to claim 1, characterized in that: The power module is used to provide power to all modules included in the chassis. The input end of the power module is connected to an external power supply, and the output end of the power module is connected to a 48V power converter. The power required by the 48V power converter is separated into 4 output ends through a wiring terminal; the output end 1 of the 48V power converter is connected to a 12V power converter, the output end 2 of the 48V power converter is connected to the input end of the steering system, the output end 3 of the 48V power converter is connected to the input end of the drive system, and the output end 4 of the 48V power converter is connected to the input end of the braking system; the 48V direct current connected to the 12V power converter is stepped down and converted into 12V direct current. The 12V power converter is separated into 3 output ends through wiring terminals, which are respectively connected to the 5V power converter of the obstacle avoidance module, the 5V power converter of the control module, and the 5V power converter of the base station module.

3. The following motion chassis control system according to claim 2, characterized in that: The obstacle avoidance module includes a control center and a power supply. The control center is internally provided with a central processing unit, a memory, an asynchronous serial communication interface and a serial peripheral interface. The 5V power converter of the power supply is respectively connected to an ultrasonic sensor, an STM32G030C8T6 processor and a communication module. The input port of the ultrasonic sensor is connected to a signal output port 1 of the STM32G030C8T6 processor, the output port of the ultrasonic sensor is connected to a signal input port of the STM32G030C8T6 processor, and the signal output port 2 of the STM32G030C8T6 processor is connected to a communication module.

4. The following motion chassis control system according to claim 3, characterized in that: The 5V power converter of the control module is respectively connected to the STM32F407ZET6 processor, the first communication module 1 and the second communication module, the first communication module is connected to the communication module in the base station module through TTL communication technology, the second communication module is connected to the communication module in the obstacle avoidance module through TTL communication technology, the output end of the first communication module is connected to the first signal input port of the STM32F407ZET6 processor, the input end of the first communication module is connected to the first signal output port of the STM32F407ZET6 processor, the input end of the second communication module is connected to the second signal output port of the STM32F407ZET6 processor, and the output end of the second communication module is connected to the second signal input port of the STM32F407ZET6 processor.

5. The following motion chassis control system according to claim 4, characterized in that: The 5V power converter in the base station module is respectively connected to the signal transceiver module, the STM32G030C8T6 processor, and the communication module. The signal transceiver module is connected to the tag module in the handheld device through the antenna array using UWB communication technology. The output end of the communication module is connected to the signal input port 1 of the STM32G030C8T6 processor, and the input end of the first communication module is connected to the signal output port 1 of the STM32G030C8T6 processor.

6. The following motion chassis control system according to claim 5, characterized in that: The obstacle avoidance module transmits ultrasonic signals through an ultrasonic sensor, and the STM32G030C8T6 processor in the obstacle avoidance module calculates the distance of the obstacle by measuring the time difference between transmitting the ultrasonic wave and receiving the reflected ultrasonic wave.

7. The following motion chassis control system according to claim 6, characterized in that: The STM32F407ZET6 processor in the control module sends a movement instruction to the drive system after receiving the data given by the coordinate determination.

8. The following motion chassis control system according to claim 7, characterized in that: The system program consists of a first initialization program, a first interrupt program, and a first control program; the first initialization program is used to control the system state, timer, crystal oscillator, and I / O for initialization settings; the first interrupt program consists of an obstacle avoidance serial port interrupt and a positioning serial port interrupt, the obstacle avoidance serial port interrupt is used to receive data from the obstacle avoidance module, and the positioning serial port interrupt is used to receive positioning data from the base station module; the first control program is used to drive the PID control algorithm and the mechanical movement of driving, braking, and stopping.

9. The following motion chassis control system according to claim 8, characterized in that: The positioning program is composed of a second initialization program, a second interrupt program, and a second control program; the second initialization program is used to initialize the system status, timer, crystal oscillator, I / O, etc.; the second interrupt program is mainly used to receive positioning data from the tag, and the second control program is used to receive instructions from the control program.

10. The following motion chassis control system according to claim 9, characterized in that: The obstacle avoidance program is composed of a third initialization program, a third interruption program, and a third control program; the third initialization program has the same function as the first initialization program, and the second interruption program is mainly used to receive obstacle avoidance signals.

Images