Autonomous robot control method and system for rail transit vehicle inspection

Abstract

The invention discloses an autonomous robot control method and system for rail transit vehicle inspection, and the method comprises the steps: obtaining a track following instruction, carrying out the track discretization of the track following instruction according to a read-in expected resolution, and obtaining a discretized discrete path; transmitting the discrete path into a vector tracking system, controlling the robot to start vector trajectory following from the starting point coordinate based on a trajectory following control method according to the read starting point coordinate and the discrete path, and calculating a vector angle and a following speed; the following speed is issued to a high real-time motion control system, and the speed of a motion motor is calculated according to a new starting point coordinate read in real time; and the speed of the motion motor is converted into a motor control pulse which is issued to a motor driver to execute and control the motor to rotate. The method solves the problems that local control points oscillate, global navigation control points are not accurate, and the global navigation positioning precision of the robot is affected; and meanwhile, the fitness level of the motion trail is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robot autonomous navigation and control, and in particular to an autonomous robot control method and system for rail transit vehicle inspection. Background Art

[0002] Existing navigation solutions mainly use point following technology or inherent track traction technology; first, point following technology discretizes a complete path into multiple points, and achieves navigation by following point by point. This navigation (track following) method has the disadvantages of insufficient following accuracy and inability to eliminate motion oscillation during following. Second, inherent track traction technology uses inherent markers installed on the ground to achieve navigation. This navigation method has the disadvantages of inflexible tracks, slow movement speed, and a huge workload for implementation and deployment.

[0003] In addition, the prior art also uses Bezier curves to achieve global control of path control points. This method has the problem that if a local control point is out of control, the local control point will also be out of control, thereby affecting the control of the robot and causing errors in the navigation planning path. Summary of the invention

[0004] The technical problem to be solved by the present invention is that the existing robot navigation method has the problem of local control point oscillation, global navigation control point inaccuracy, and the problem of affecting the robot's global navigation positioning accuracy. The purpose of the present invention is to provide an autonomous robot control method and system for rail transit vehicle inspection. The navigation of the present invention mainly includes a high real-time motion control part and a motion trajectory control part based on B-spline curves, which can greatly improve the robot control path trajectory fit and smoothness, solve the problem of local control point oscillation, global navigation control point inaccuracy, and affecting the robot's global navigation positioning accuracy; at the same time, it not only improves the motion trajectory fit level, but also meets the robot's navigation control requirements in complex environments such as rail transit slopes and level crossings.

[0005] The present invention is achieved through the following technical solutions:

[0006] In a first aspect, the present invention provides an autonomous robot control method for rail transit vehicle inspection, the method comprising:

[0007] Obtaining a trajectory following instruction, and discretizing the trajectory following instruction according to the expected resolution read in, to obtain a discretized discrete path;

[0008] The discrete path is passed into the vector tracking system. Based on the read starting point coordinates and the discrete path, the robot is controlled to start vector trajectory following from the starting point coordinates based on the trajectory following control method, and the vector angle and following speed are calculated.

[0009] The following speed is sent to the high real-time motion control system, and the motion motor speed is calculated according to the new starting point coordinates read in real time; and the motion motor speed is converted into motor control pulses and sent to the motor driver to control the motor rotation.

[0010] Furthermore, the trajectory in the trajectory following instruction is a mathematical representation of a path, which is a string of dense coordinates.

[0011] Furthermore, trajectory discretization is to de-densify the dense coordinates to obtain a de-densified discrete path.

[0012] Furthermore, the trajectory following control method is based on B-spline curve to formulate the trajectory of discrete path points in the discrete path and then perform vector trajectory tracking to obtain a better smooth control process to facilitate vector tracking during navigation.

[0013] Furthermore, the expression based on B-spline curve is:

[0014] where t min ≤t≤t max ,2≤d≤n

[0015] in: Represents the coordinate vector of the point on the curve; n represents the control point quantity; is the coordinate of the control point, i starts from 0; B i,d (t) is the polynomial coefficient of the control point coordinates affecting the weight, i represents the index of the coordinate, d is the number of times it affects the B-spline curve, and t is the value taken when drawing the curve.

[0016] Furthermore, vector trajectory following includes:

[0017] Select several forward-looking points and calculate the vector angles between the starting point coordinates and different forward-looking points;

[0018] Select the point with the smallest vector angle for trajectory tracking.

[0019] Furthermore, the vector angle refers to the vector rotation angle, and the calculation formula of the vector rotation angle is:

[0020]

[0021] Where δ is the front wheel vector angle; e is the lateral deviation from the preview point; L is the wheelbase between the wheels; k is the curvature of the turning arc; V is the longitudinal linear speed of the vehicle; and x is the horizontal coordinate of the preview point.

[0022] Furthermore, the calculation formula of the following speed is:

[0023]

[0024] Where ω is the angular velocity corresponding to the front wheel turning angle of the vehicle; v is the longitudinal linear velocity of the vehicle.

[0025] In a second aspect, the present invention further provides an autonomous robot control system for rail transit vehicle inspection, the system using the above-mentioned autonomous robot control method for rail transit vehicle inspection; the system comprises:

[0026] An acquisition unit, used for acquiring a trajectory following instruction;

[0027] A discretization unit is used to discretize the trajectory following instruction according to the expected resolution read in, and obtain a discretized discrete path;

[0028] The trajectory following control unit is used to input the discrete path into the vector tracking system. According to the read starting point coordinates and the discrete path, based on the trajectory following control method, the robot is controlled to start vector trajectory following from the starting point coordinates, and the vector angle and following speed are calculated;

[0029] The high real-time motion control unit is used to send the follow-up speed to the high real-time motion control system, and calculate the motion motor speed according to the new starting point coordinates read in real time; and convert the motion motor speed into a motor control pulse, and send it to the motor driver to control the motor rotation.

[0030] Furthermore, the high real-time motion control system includes an STM32 single-chip microcomputer module, a motor driver module, an encoding deceleration module and a motion wheel module;

[0031] STM32 MCU module, directly runs bare metal programs and directly uses I / O ports to drive motor drivers; STM32 MCU module is used to collect data from gyroscopes and lidars, output PWM waveforms to motor drivers, and achieve high real-time motion control;

[0032] The motor driver module is used to receive data from the STM32 microcontroller module, use the DSP chip to perform high-speed real-time calculations on the data, and output accurate control parameters to adjust the control instructions to the motor and reducer for control;

[0033] The encoder reduction module is used to execute the drive control instructions of the motor driver module, control the motor, and complete the real-time control execution of the encoder and reduction box; this design includes real-time feedback control of one or more motors;

[0034] The motion wheel module, as a wheel execution structure, is used to execute the corresponding wheel movement according to the above control.

[0035] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0036] The present invention discloses an autonomous robot control method and system for rail transit vehicle inspection. The navigation of the present invention mainly includes a high real-time motion control part and a motion trajectory control part based on a B-spline curve, which can greatly improve the robot control path trajectory fit and smoothness, solve the problem of local control point oscillation, global navigation control point inaccuracy, and affect the robot's global navigation positioning accuracy; at the same time, it not only improves the motion trajectory fit level, but also meets the robot's navigation control requirements in complex environments such as rail transit slopes and level crossings;

[0037] (1) The present invention improves the control method based on Bezier curve optimization commonly used in prior engineering technology and uses B-spline curve for optimization to avoid the problem of affecting the global navigation positioning accuracy of the robot after the local control point is out of control;

[0038] (2) The present invention adopts a high-real-time motion system design and curve optimization method to improve the trajectory tracking fit during navigation and the smoothness of the robot's operation and navigation stability during navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] The drawings described herein are used to provide a further understanding of the embodiments of the present invention, constitute a part of this application, and do not constitute a limitation of the embodiments of the present invention. In the drawings:

[0040] Figure 1 This is a flow chart of an autonomous robot control method for rail transit vehicle inspection according to the present invention;

[0041] Figure 2 A detailed flow chart of an autonomous robot control method for rail transit vehicle inspection according to the present invention;

[0042] Figure 3 This is a simplified diagram of the navigation path control of the present invention;

[0043] Figure 4 This is a structural block diagram of the high real-time motion control system of the present invention;

[0044] Figure 5 The present invention is a structural block diagram of an autonomous robot control system for rail transit vehicle inspection. DETAILED DESCRIPTION

[0045] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with embodiments and drawings. The exemplary embodiments of the present invention and their description are only used to explain the present invention and are not intended to limit the present invention.

[0046] Based on the problem that the existing robot navigation methods have local control point oscillations and the global navigation control points are not accurate, which affects the robot's global navigation positioning accuracy. The present invention designs an autonomous robot control method and system for rail transit vehicle inspection. The navigation of the present invention mainly includes a high-real-time motion control part and a motion trajectory control part based on B-spline curves, which can greatly improve the robot control path trajectory fit and smoothness, solve the problem of local control point oscillations and global navigation control points being not accurate, which affects the robot's global navigation positioning accuracy; at the same time, it not only improves the motion trajectory fit level, but also meets the robot's navigation control requirements in complex environments such as rail transit slopes and level crossings.

[0047] Example 1

[0048] like Figure 1 and Figure 2 As shown, the present invention provides an autonomous robot control method for rail transit vehicle inspection, the method comprising:

[0049] Step 1, obtaining a trajectory following instruction, and discretizing the trajectory following instruction according to the expected resolution read in, to obtain a discretized discrete path;

[0050] In this embodiment, the trajectory in the trajectory following instruction is a mathematical representation of a path, which is a string of dense coordinates.

[0051] In this embodiment, the trajectory discretization is to de-densify the dense coordinates to obtain a de-densified discrete path. Specifically, the de-densified input is the dense coordinates and the expected resolution, and the trajectory is not distorted after de-densification.

[0052] The discretized discrete path can be passed into the vector following system to start following;

[0053] Step 2: The discrete path is passed into the vector tracking system. Based on the read starting point coordinates and the discrete path, the robot is controlled to start vector trajectory following from the starting point coordinates based on the trajectory following control method, and the vector angle and following speed are calculated.

[0054] In this embodiment, the trajectory following control method is based on B-spline curve conversion to formulate the trajectory of discrete path points in the discrete path and then perform vector trajectory tracking.

[0055] This is because the discrete trajectory after discretization is still a coordinate, and the coordinate trajectory is formulated. Therefore, the present invention converts the robot motion trajectory into a B-spline curve. In the navigation virtual path planning, the discrete path points are converted into B-spline curves to obtain a better smooth control process, so as to facilitate vector tracking during navigation.

[0056] Specifically, the expression based on B-spline curve is:

[0057] where t min ≤t≤t max ,2≤d≤n

[0058] in: Represents the coordinate vector of the point on the curve; n represents the control point quantity; is the coordinate of the control point, i starts from 0; B i,d (t) is the polynomial coefficient of the control point coordinates affecting the weight, i represents the index of the coordinate, d is the number of times it affects the B-spline curve, and t is the value taken when drawing the curve.

[0059] In this embodiment, the vector trajectory following includes:

[0060] Select several forward-looking points and calculate the vector angles between the starting point coordinates and different forward-looking points;

[0061] Select the point with the smallest vector angle for trajectory tracking.

[0062] Specifically, Figure 3 As shown, Figure 3 Simplified diagram for navigation path control, Figure 3 The parameters in are:

[0063] r(m): turning radius of the vehicle center;

[0064] R(m): wheel turning radius;

[0065] L(m): wheelbase;

[0066] δ(rad): front wheel steering angle;

[0067] α(rad): angle between the vehicle body and the preview point;

[0068] Ld (m): preview distance;

[0069] e(m): lateral deviation from the preview point;

[0070] Xr(m): horizontal coordinate of the preview point;

[0071] according to Figure 3 , vector trajectory tracking, as follows:

[0072] Derived from the law of sines:

[0073] Right now:

[0074] Then the curvature k of the turning arc can be derived:

[0075]

[0076]

[0077]

[0078] It can be deduced that:

[0079] From the above formula, we can see that the essence of this control is to control the turning angle. It can be regarded as the p parameter of the control system. L is the wheelbase of the vehicle, and Ld is the set preview distance. Generally speaking, the longer the preview distance, the smoother the control effect, and the shorter the preview distance, the more precise the control effect (it will also bring certain shocks). The selection of the preview distance is also related to the vehicle speed. The present invention uses the longitudinal linear speed of the vehicle as the pre-taken distance, that is, Ld = kVx, then the front wheel turning angle formula becomes:

[0080]

[0081] Where δ is the front wheel vector angle; e is the lateral deviation from the preview point; L is the wheelbase between the wheels; k is the curvature of the turning arc; V is the longitudinal linear speed of the vehicle; and x is the horizontal coordinate of the preview point.

[0082] Therefore, the parameter adjustment of the pure tracking controller becomes adjusting the foresight coefficient k. Generally speaking, the maximum and minimum foresight distances are used to constrain the foresight distance. A larger foresight distance means smoother tracking of the trajectory, and a smaller foresight distance makes tracking more precise (of course, it will also bring about control shock). The angular velocity ω corresponding to the two wheel angles can be obtained according to the current speed v:

[0083]

[0084] Where ω is the angular velocity corresponding to the front wheel turning angle of the vehicle; v is the longitudinal linear velocity of the vehicle.

[0085] Step 3, send the follow speed to the high real-time motion control system, and calculate the motion motor speed according to the new starting point coordinates read in real time; and convert the motion motor speed into motor control pulses, and send them to the motor driver to control the motor rotation.

[0086] The present invention improves the control method based on Bezier curve optimization commonly used in prior engineering technology, and uses B-spline curve for optimization to avoid the problem of affecting the global navigation and positioning accuracy of the robot after the local control point is out of control; at the same time, the present invention adopts high real-time motion system design and curve optimization method to improve the trajectory tracking fit during navigation and the operation smoothness and navigation stability of the robot during navigation.

[0087] Example 2

[0088] like Figure 5As shown, the difference between this embodiment and embodiment 1 is that this embodiment provides an autonomous robot control system for rail transit vehicle inspection, and the system uses an autonomous robot control method for rail transit vehicle inspection in embodiment 1; the system corresponds one-to-one to an autonomous robot control method for rail transit vehicle inspection in embodiment 1; the system includes:

[0089] An acquisition unit, used for acquiring a trajectory following instruction;

[0090] A discretization unit is used to discretize the trajectory following instruction according to the expected resolution read in, and obtain a discretized discrete path;

[0091] The trajectory following control unit is used to input the discrete path into the vector tracking system. According to the read starting point coordinates and the discrete path, based on the trajectory following control method, the robot is controlled to start vector trajectory following from the starting point coordinates, and the vector angle and following speed are calculated;

[0092] The high real-time motion control unit is used to send the follow-up speed to the high real-time motion control system, and calculate the motion motor speed according to the new starting point coordinates read in real time; and convert the motion motor speed into a motor control pulse, and send it to the motor driver to control the motor rotation.

[0093] As a further implementation, Figure 4 As shown, the high real-time motion control system includes an STM32 single-chip microcomputer module, a motor driver module, an encoder deceleration module and a motion wheel module;

[0094] STM32 MCU module, STM32 MCU design, the MCU has no operating system, and the real-time performance is directly guaranteed by the hardware circuit; it directly runs the bare metal program and directly uses the I / O port to drive the motor driver, thereby ensuring the high real-time motion control of the real-time system; the STM32 MCU module is used to collect data from the gyroscope and laser radar, and output PWM waveforms to the motor driver to achieve high real-time motion control;

[0095] The motor driver module directly accepts PWM control from the lower computer, and its real-time performance is much higher than that of the real-time system controlled by the upper computer. It is used to receive data from the STM32 microcontroller module, use the DSP chip to perform high-speed real-time calculations on the data, and output accurate control parameters to adjust the control instructions to the motor and reducer for control.

[0096] The encoder reduction module is used to execute the drive control instructions of the motor driver module, control the motor, and complete the real-time control execution of the encoder and reduction box; this design includes real-time feedback control of one or more motors;

[0097] The motion wheel module, as a wheel execution structure, is used to execute the corresponding wheel movement according to the above control.

[0098] Among them, the execution process of the above other units can be executed according to the process steps of an autonomous robot control method for rail transit vehicle inspection in Example 1, and will not be repeated in this embodiment.

[0099] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0100] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0101] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0102] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0103] The specific implementation methods described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific implementation method of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. An autonomous robot control method for rail transit vehicle inspection, It is characterized in that The method includes: Acquire a trajectory following instruction, and discretize the trajectory following instruction according to the read expected resolution to obtain a discretized discrete path; The discrete path is transmitted to the vector tracking system, and the robot is controlled to start vector trajectory following from the starting point coordinates based on the trajectory following control method according to the read starting point coordinates and the discrete path, and the vector angle and the following speed are calculated; The following speed is sent to the high real-time motion control system, and the motion motor speed is calculated according to the new starting point coordinates read in real time; and the motion motor speed is converted into a motor control pulse and sent to the motor driver to execute the control of the motor rotation.

2. The autonomous robot control method for rail transit vehicle inspection according to claim 1, It is characterized in that The trajectory in the trajectory following instruction is a mathematical representation of a path, which is a string of dense coordinates.

3. The autonomous robot control method for rail transit vehicle inspection according to claim 2, It is characterized in that The trajectory discretization is to de-densify the dense coordinates to obtain a de-densified discrete path.

4. The autonomous robot control method for rail transit vehicle inspection according to claim 1, It is characterized in that The trajectory following control method is based on B-spline curve to formulate the trajectory of discrete path points in the discrete path and then perform vector trajectory tracking.

5. The autonomous robot control method for rail transit vehicle inspection according to claim 4, It is characterized in that The expression based on B-spline curve is: where t min ≤t≤t max ,2≤d≤n in: Represents the coordinate vector of the point on the curve; n represents the control point quantity; is the coordinate of the control point, i starts from 0; B i,d (t) is the polynomial coefficient of the control point coordinates affecting the weight, i represents the index of the coordinate, d is the number of times it affects the B-spline curve, and t is the value taken when drawing the curve.

6. The autonomous robot control method for rail transit vehicle inspection according to claim 1, It is characterized in that The vector trajectory following includes: Select several forward-looking points, and calculate the vector angles between the starting point coordinates and different forward-looking points; The point with the smallest vector angle is selected for trajectory tracking.

7. The autonomous robot control method for rail transit vehicle inspection according to claim 6, It is characterized in that The vector angle refers to the vector rotation angle, and the calculation formula of the vector rotation angle is: Where δ is the front wheel vector angle; e is the lateral deviation from the preview point; L is the wheelbase between the wheels; k is the curvature of the turning arc; V is the longitudinal linear speed of the vehicle; and x is the horizontal coordinate of the preview point.

8. The autonomous robot control method for rail transit vehicle inspection according to claim 7, It is characterized in that The calculation formula of the following speed is: Where ω is the angular velocity corresponding to the front wheel turning angle of the vehicle; v is the longitudinal linear velocity of the vehicle.

9. An autonomous robot control system for rail transit vehicle inspection, It is characterized in that The system includes: An acquisition unit, used for acquiring a trajectory following instruction; A discretization unit, used for discretizing the trajectory of the trajectory following instruction according to the expected resolution read in, to obtain a discretized discrete path; A trajectory following control unit is used to input the discrete path into a vector tracking system, and based on the trajectory following control method, control the robot to start vector trajectory following from the starting point coordinates according to the read starting point coordinates and the discrete path, and calculate the vector angle and following speed; The high real-time motion control unit is used to send the following speed to the high real-time motion control system, and calculate the motion motor speed according to the new starting point coordinates read in real time; and convert the motion motor speed into a motor control pulse, and send it to the motor driver to control the motor rotation.

10. An autonomous robot control system for rail transit vehicle inspection according to claim 9, It is characterized in that The high real-time motion control system includes an STM32 single-chip microcomputer module, a motor driver module, an encoding deceleration module and a motion wheel module; The single-chip microcomputer module directly runs the bare metal program and directly uses the I / O port to drive the motor driver; the single-chip microcomputer module is used to collect data from the gyroscope and the laser radar, output PWM waveforms to the motor driver, and realize high real-time motion control; The motor driver module is used to receive data from the single-chip microcomputer module, use a DSP chip to perform high-speed real-time calculations on the data, and output control parameter adjustment control instructions to the motor and the reduction box for control; The encoding and deceleration module is used to execute the driving control instructions of the motor driver module, control the motor, and complete the real-time control execution of the encoder and the reduction box; The moving wheel module, as a wheel execution structure, is used to execute the corresponding wheel movement according to the above control.

Images