Underground articulated driving vehicle path tracking method

By obtaining the front and rear position information of the articulated vehicle, determining the forward point and anchor point, and designing flexible control strategies, the problem of insufficient path tracking accuracy of articulated vehicles in the existing technology is solved, and higher path tracking accuracy and stability are achieved. Complex terrain and obstacles.

CN118938901APending Publication Date: 2024-11-12QINGDAO FAMBITION HEAVY MASCH CO LTD
View PDF 0 Cites 1 Cited by

Patent Information

Application Number
CN202410975931.7
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-19
Publication Date
2024-11-12

AI Technical Summary

Technical Problem

The existing downhole articulated driving vehicle path tracking method is difficult to accurately predict the driving trajectory when turning, and the sensor data is insufficient and the accuracy and stability are not effective in dealing with complex terrain and obstacles, resulting in path deviation and unstable driving.

Method used

By obtaining the front and rear posture information of the articulated vehicle, determining the forward point and anchor point, designing flexible control strategies, using sensor data for real-time trajectory tracking and path adjustment, ensuring that the vehicle drives safely and stably along the predetermined path.

Benefits of technology

The accuracy and stability of path tracking are improved, and it can better adapt to the needs of articulated vehicles for turning and curved driving, timely discover and correct path deviations, and ensure that the vehicle is driving according to the predetermined trajectory.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118938901A_ABST
    Figure CN118938901A_ABST
Patent Text Reader

Abstract

The invention relates to the technical field of underground vehicle tracking, in particular to an underground articulated driving vehicle path tracking method, which comprises the following steps: S1, in a forward state, acquiring a front posture of an articulated vehicle as an integral posture for trajectory tracking, and acquiring position and posture information of the front part of the vehicle through a vehicle-mounted sensor, taking the position and the posture as the position and the posture of the whole vehicle; s2, a foresight point is obtained according to the front posture, the position of the foresight point is determined by pushing the current position of the vehicle forwards by a certain distance in the current heading direction, the forward pushing distance is determined and selected according to the speed of the vehicle and the control response time, the foresight point is kept in front of the vehicle, and timely adjustment is conducted when the vehicle reaches the foresight point. Trajectory tracking is carried out by obtaining the pose information of the front section and the rear section of the vehicle, the structural characteristics of the articulated vehicle can be better and fully utilized, and the path tracking effect and performance are improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of underground vehicle tracking, and in particular relates to a path tracking method for an underground articulated driving vehicle. Background Art

[0002] At present, the path tracking method of underground articulated driving vehicles mainly adopts technical solutions based on sensor data fusion and control strategies. First, various sensors such as laser radar, camera, inertial navigation, etc. are used to obtain data such as the current position, posture, and environmental information of the vehicle. The data is processed, fused, and corrected in real time through the sensor data fusion algorithm to improve the precision and accuracy of the position and posture. Then, a path tracking control strategy such as model predictive control MPC or classic PID control is adopted to calculate the optimal steering angle and vehicle speed command according to the current state of the vehicle and the pre-planned path, so that the vehicle can travel safely and stably along the predetermined path. In addition, in view of the particularity of the underground environment, taking into account factors such as geological conditions and obstacles, specific path planning algorithms such as genetic algorithms and A* algorithms will be adopted to ensure that the vehicle can avoid obstacles and adapt to complex terrain. The entire system also needs to have the characteristics of strong real-time performance and strong anti-interference ability to cope with the fast-changing and complex characteristics of the underground environment, so as to achieve efficient, stable and safe operation of the path tracking of underground articulated driving vehicles.

[0003] The above trajectory tracking methods have some disadvantages when applied to articulated vehicles. First, since the body of an articulated vehicle is divided into two parts, the front and rear parts, the turning radius changes when turning, which makes it difficult for traditional path tracking control methods to accurately predict the vehicle's driving trajectory, which may lead to path deviation or unstable driving. Secondly, the complexity and uncertainty of the underground environment put forward higher requirements on the collection and processing of sensor data, and the accuracy and stability of sensor data directly affect the effect of path tracking. Therefore, a more reliable and accurate sensor data fusion algorithm is needed. In addition, articulated vehicles may encounter more complex terrain and obstacles under certain circumstances, and traditional path planning algorithms may not be able to cope with them effectively. In this regard, we propose a path tracking method for underground articulated driving vehicles. Summary of the invention

[0004] In view of the deficiencies in the prior art, the present invention provides a path tracking method for an underground articulated driving vehicle to solve the path tracking problem of an articulated vehicle in an underground tunnel.

[0005] To achieve the above object, the present invention provides the following technical solution: a method for tracking the path of an underground articulated driving vehicle, wherein the tracking steps are:

[0006] S1. In the forward state, the front posture of the articulated vehicle is obtained as the overall posture for trajectory tracking, and the position and posture information of the front of the vehicle is obtained through the on-board sensor, and is used as the position and posture of the entire vehicle;

[0007] S2. Obtain the look-ahead point based on the front posture. Determine the position of the look-ahead point by pushing the current position of the vehicle forward a certain distance along the current heading direction. The forward distance is determined based on the vehicle's speed and control response time. The distance is selected to keep the look-ahead point in front of the vehicle, and make timely adjustments when the vehicle reaches the look-ahead point.

[0008] S3, in the backward state, obtaining the rear position posture of the articulated vehicle as the overall posture for trajectory tracking, obtaining the position and posture information of the front part of the vehicle through the on-board sensor, and taking it as the position and posture of the entire vehicle;

[0009] S4. Push the current position of the vehicle back a certain distance along the current heading direction, i.e., the backward direction, to determine the position of the forward-looking point. The backward distance is determined according to the speed of the vehicle and the control response time. The distance is selected so that the forward-looking point can be kept behind the vehicle and timely adjustments are made when the vehicle reaches the forward-looking point.

[0010] Preferably, the main posture is determined according to whether the vehicle is moving forward or backward. If the vehicle is moving forward, the backward posture is the secondary posture. If the vehicle is moving backward, the forward posture is the secondary posture.

[0011] Preferably, in step S2, forward is the main posture, backward is the secondary posture, and the anchor point Panchor is obtained by the distance between the main posture and the anchor point Lanchor. The coordinate position calculation formula of the vehicle is:

[0012] X anchor =X front +L anchor *cos(Θ front )

[0013] Y anchor =Y front +L anchor *sin(Θ front )

[0014] Where X anchor Expressed as the X coordinate of the anchor point, X front Indicates the X coordinate of the main pose, L anchor Denoted as the anchor distance, Θ front Expressed as the heading angle of the front of the articulated vehicle, Y anchor Expressed as the Y coordinate of the anchor point, Y front Represents the Y coordinate of the main pose;

[0015] According to the distance L between the anchor point and the forwardahead Get the forward point P on the planned path ahead .

[0016] Preferably, obtain the forward point P ahead Step is the previous anchor point P anchor is the center of the circle, and the forward distance is L ahead Draw a circle with radius intersecting the planned path at point P intersection , judgment vector P front P anchor With vector P anchor P intersection Whether the absolute value of the intersection angle is less than 90°, the judgment steps are:

[0017] Assume point P front The coordinates are (x f ,y f ), P anchor The coordinates are (x a ,y a ), P intersectio The coordinates are (x i ,y i ), then the vector P front P anchor With vector P anchor P intersection Angle It can be obtained by the following formula:

[0018]

[0019] If so, the intersection is judged to be the correct forward point P ahead , thus obtaining the forward-looking angle Θ head , the obtained anchor point distance L anchor , Look-ahead distance L ahead and the look-ahead angle Θ head Substituting into the lateral controller, the rotation angle at the hinge is δ f , in the case of a curve, only δ f As the final rotation angle output, δ = δ f .

[0020] Preferably, in the case of non-curved roads, the rear vehicle point P is calculated from the beginning of the vehicle operation. rear The closest point P on the trajectory near The distance L near , as the vehicle moves, the distance obtained each time is accumulated and the average value L is obtained agerage , after a certain number of accumulations N, the angle output becomes δ = K * δ f ;

[0021] Where K = (1-(L near / Lagerage )) is the penalty coefficient, L near is the current shortest distance of the following vehicle, L agerage It is the cumulative mean of the closest distances to the following vehicles.

[0022] Preferably, in step S2, according to the main posture P front Get the distance from the target point P target The distance D is calculated to determine the length L of the entire trajectory. First, determine the maximum speed V max , determine the acceleration a, current velocity V current =a*t, until the maximum speed V max , where t is the time interval from vehicle start to the present.

[0023] Preferably, when the distance between the current looking point and the curve point is L circle When the speed is reduced to V circle , ensure that you pass the curve slowly, and calculate the completion degree u=D / L; when u>95%, start to slow down, and ensure that the speed formula is:

[0024] V current =(1-D / L)V max .

[0025] Preferably, the front and rear poses of the articulated vehicle are acquired as the overall pose for trajectory tracking.

[0026] Preferably, the sensor for acquiring the posture information in step S1 is a posture sensor for recording the position and direction information of the vehicle, and the posture sensor includes a three-axis gyroscope, a three-axis accelerometer and a three-axis electronic compass.

[0027] Preferably, the attitude sensor outputs zero-drift three-dimensional attitude and orientation data represented by quaternions and Euler angles in real time through an embedded low-power ARM processor combined with a quaternion-based three-dimensional algorithm and data fusion technology.

[0028] Compared with the prior art, the present invention has the following beneficial effects:

[0029] The present application can better perform trajectory tracking by acquiring the posture information of the front and rear sections of the vehicle. By acquiring the posture information of the front and rear sections of the vehicle separately, the overall motion state and posture changes of the vehicle can be captured more accurately, thereby improving the accuracy and stability of path tracking. At the same time, the relationship between the postures of the front and rear sections can be used to design more complex and flexible control strategies to adapt to the special needs of articulated vehicles when turning and driving on curves. In addition, through real-time monitoring and analysis of the postures of the front and rear sections of the vehicle, path deviations can be discovered and corrected in a timely manner to ensure that the vehicle can travel safely and stably along the predetermined trajectory. Therefore, by acquiring the posture information of the front and rear sections of the vehicle for trajectory tracking, the structural characteristics of the articulated vehicle can be better utilized to improve the effect and performance of path tracking. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 It is a framework diagram of the tracking step process of the present invention;

[0031] Figure 2 The anchor point P of the present invention anchor Get a schematic diagram;

[0032] Figure 3 Forecast point P of the present invention ahead Get a schematic diagram;

[0033] Figure 4 It is a schematic diagram of speed control of the present invention. DETAILED DESCRIPTION

[0034] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0035] The present invention provides a technical solution: a path tracking method for an underground articulated driving vehicle, wherein the tracking steps are:

[0036] S1. In the forward state, the front posture of the articulated vehicle is obtained as the overall posture for trajectory tracking, and the position and posture information of the front of the vehicle is obtained through the on-board sensor, and is used as the position and posture of the entire vehicle;

[0037] S2. Obtain the look-ahead point based on the front posture. Determine the position of the look-ahead point by pushing the current position of the vehicle forward a certain distance along the current heading direction. The forward distance is determined based on the vehicle's speed and control response time. The distance is selected to keep the look-ahead point in front of the vehicle, and make timely adjustments when the vehicle reaches the look-ahead point.

[0038] By obtaining the position of the look-ahead point, the vehicle's driving direction and speed can be effectively guided, so that it can travel stably and efficiently along the predetermined path, thus achieving the goal of path tracking;

[0039] S3, in the backward state, obtaining the rear position posture of the articulated vehicle as the overall posture for trajectory tracking, obtaining the position and posture information of the front part of the vehicle through the on-board sensor, and taking it as the position and posture of the entire vehicle;

[0040] S4. Push the current position of the vehicle back a certain distance along the current heading direction, i.e., the backward direction, to determine the position of the forward-looking point. The backward distance is determined according to the speed of the vehicle and the control response time. The distance is selected so that the forward-looking point can be kept behind the vehicle and timely adjustments are made when the vehicle reaches the forward-looking point.

[0041] Furthermore, the main posture is determined according to whether the vehicle is moving forward or backward. If moving forward is the main posture, then backward is the secondary posture. If backward is the main posture, then moving forward is the secondary posture.

[0042] Furthermore, in step S2, forward is the main posture, backward is the secondary posture, and the anchor point Panchor is obtained through the distance between the main posture and the anchor point Lanchor. The coordinate position calculation formula of the vehicle is:

[0043] X anchor =X front +L anchor *cos(Θ front )

[0044] Y anchor =Y front +L anchor *sin(Θ front )

[0045] Where X anchor Expressed as the X coordinate of the anchor point, X front Indicates the X coordinate of the main pose, L anchor Denoted as the anchor distance, Θ front Expressed as the heading angle of the front of the articulated vehicle, Y anchor Expressed as the Y coordinate of the anchor point, Y front Represents the Y coordinate of the main pose;

[0046] According to the distance L between the anchor point and the forward ahead Get the forward point P on the planned path ahead .

[0047] Further, obtain the forward point P ahead Step is the previous anchor point P anchor is the center of the circle, and the forward distance is L aheadDraw a circle with radius intersecting the planned path at point P intersection , judgment vector P front P anchor With vector P anchor P intersection Whether the absolute value of the intersection angle is less than 90°, the judgment steps are:

[0048] Assume point P front The coordinates are (x f ,y f ), P anchor The coordinates are (x a ,y a ), P intersectio The coordinates are (x i ,y i ), then the vector P front P anchor With vector P anchor P intersection Angle It can be obtained by the following formula:

[0049]

[0050] If so, the intersection is judged to be the correct forward point P ahead , thus obtaining the forward-looking angle Θ head , the obtained anchor point distance L anchor , Look-ahead distance L ahead and the look-ahead angle Θ head Substituting into the lateral controller, the rotation angle at the hinge is δ f , in the case of a curve, only δ f As the final rotation angle output, δ = δ f .

[0051] Furthermore, in the case of non-curved roads, the rear vehicle point P is calculated from the beginning of the vehicle operation. rear The closest point P on the trajectory near The distance L near , as the vehicle moves, the distance obtained each time is accumulated and the average value L is obtained agerage , after a certain number of accumulations N, the angle output becomes δ = K * δ f ;

[0052] Where K = (1-(L near / L agerage )) is the penalty coefficient, L near is the current shortest distance of the following vehicle, L agerage It is the cumulative mean of the closest distances to the following vehicles.

[0053] Furthermore, in step S2, according to the main pose P frontGet the distance from the target point P target The distance D is calculated to determine the length L of the entire trajectory. First, determine the maximum speed V max , determine the acceleration a, current velocity V current =a*t, until the maximum speed V max , where t is the time interval from vehicle start to the present.

[0054] Furthermore, when the distance between the current look-ahead point and the curve point is L circle When the speed is reduced to V circle , ensure that you pass the curve slowly, and calculate the completion degree u=D / L; when u>95%, start to slow down, and ensure that the speed formula is:

[0055] V current =(1-D / L)V max .

[0056] Furthermore, the front and rear poses of the articulated vehicle are obtained as the overall poses for trajectory tracking.

[0057] Furthermore, the sensor for acquiring the attitude information in step S1 is an attitude sensor for recording the position and direction information of the vehicle. The attitude sensor includes a three-axis gyroscope, a three-axis accelerometer and a three-axis electronic compass.

[0058] Furthermore, the attitude sensor uses an embedded low-power ARM processor, combined with quaternion-based three-dimensional algorithms and data fusion technology, to output zero-drift three-dimensional attitude and orientation data expressed in quaternions and Euler angles in real time;

[0059] The attitude sensor uses a combination of multiple sensors and data integration technology to eliminate the deviation and noise of the sensor itself, provide accurate attitude status information, and can immediately obtain the attitude status of the object, including attitude viewing angle, attitude speed and attitude acceleration. The measured power consumption is 17mA, showing its true low power consumption characteristics. It supports multiple data output formats such as Euler angles, quaternions, and rotation matrices, and has the characteristics of strong resistance to external electromagnetic interference and ability to withstand vibration and shock.

[0060] Case 1: Tracking on a map generated underground in Luohe Mine

[0061] In the scenario of trajectory tracking in Luohe Mine, articulated vehicles need to travel in narrow and complex underground tunnels to ensure safety and efficiency. In order to achieve this goal, the posture information of the front and rear sections of the vehicle can be used for trajectory tracking. First, the posture data of the front and rear sections of the vehicle, including position and posture information, are obtained through the on-board sensors. Then, the dynamic model of the vehicle is established using these data, and the path planning is carried out in combination with the map information of the underground tunnel. In the path planning process, considering the articulated structure of the vehicle, the path can be dynamically adjusted according to the position and direction of the front and rear of the vehicle to adapt to the curves and turns of the underground tunnel. Then, by real-time monitoring of the posture of the front and rear sections of the vehicle, the control strategy is continuously adjusted so that the vehicle can travel safely and stably according to the planned path. During the driving process, the path deviation can also be timely discovered and corrected according to the relative relationship between the postures of the front and rear sections of the vehicle to ensure that the vehicle always stays on the predetermined trajectory. By using the posture information of the front and rear sections of the vehicle for trajectory tracking, the driving safety and efficiency of articulated vehicles in Luohe Mine can be improved, and the smooth progress of production operation can be guaranteed.

[0062] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0063] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A path tracking method for an underground articulated driving vehicle, characterized in that: The tracking steps are: S1. In the forward state, the front posture of the articulated vehicle is obtained as the overall posture for trajectory tracking, and the position and posture information of the front of the vehicle is obtained through the on-board sensor, and is used as the position and posture of the entire vehicle; S2. Obtain the look-ahead point based on the front posture. Determine the position of the look-ahead point by pushing the current position of the vehicle forward a certain distance along the current heading direction. The forward distance is determined based on the vehicle's speed and control response time. The distance is selected to keep the look-ahead point in front of the vehicle, and make timely adjustments when the vehicle reaches the look-ahead point. S3, in the backward state, obtaining the rear position posture of the articulated vehicle as the overall posture for trajectory tracking, obtaining the position and posture information of the front part of the vehicle through the on-board sensor, and taking it as the position and posture of the entire vehicle; S4. Push the current position of the vehicle back a certain distance along the current heading direction, i.e., the backward direction, to determine the position of the forward-looking point. The backward distance is determined according to the speed of the vehicle and the control response time. The distance is selected so that the forward-looking point can be kept behind the vehicle and timely adjustments are made when the vehicle reaches the forward-looking point.

2. The path tracking method for an underground articulated driving vehicle according to claim 1, characterized in that: The main posture is determined based on whether the vehicle is moving forward or backward. If the vehicle is moving forward, the backward posture is the secondary posture. If the vehicle is moving backward, the forward posture is the secondary posture.

3. The path tracking method for an underground articulated driving vehicle according to claim 1, characterized in that: In step S2, the main posture is forward, and the secondary posture is backward. The anchor point Panchor is obtained by the distance between the main posture and the anchor point Lanchor. The coordinate position calculation formula of the vehicle is: X anchor =X front +L anchor *cos(θ front ) AND anchor =And front +L anchor *sin(Θ front ) Where X anchor Expressed as the X coordinate of the anchor point, X front Indicates the X coordinate of the main pose, L anchor Denoted as the anchor distance, Θ front Expressed as the heading angle of the front of the articulated vehicle, Y anchor Expressed as the Y coordinate of the anchor point, Y front Represents the Y coordinate of the main pose; According to the distance L between the anchor point and the forward ahead Get the forward point P on the planned path ahead .

4. The path tracking method for an underground articulated driving vehicle according to claim 3, characterized in that: Get the forward point P ahead Step is the previous anchor point P anchor is the center of the circle, and the forward distance is L ahead Draw a circle with radius intersecting the planned path at point P intersection , judgment vector P front P anchor With vector P anchor P intersection Whether the absolute value of the intersection angle is less than 90°, the judgment steps are: Assume point P front The coordinates are (x f ,y f ), P anchor The coordinates are (x a ,y a ), P intersectio The coordinates are (x i ,y i ), then the vector P front P anchor With vector P anchor P intersection Angle It can be obtained by the following formula: If so, the intersection is judged to be the correct forward point P ahead , thus obtaining the forward-looking angle Θ head , the obtained anchor point distance L anchor , Look-ahead distance L ahead and the look-ahead angle Θ head Substituting into the lateral controller, the rotation angle at the hinge is δ f , in the case of a curve, only δ f As the final rotation angle output, δ = δ f .

5. The path tracking method for an underground articulated driving vehicle according to claim 4, characterized in that: In the case of non-curves, the rear vehicle point P is calculated from the beginning of the vehicle operation. rear The closest point P on the trajectory near The distance L near , as the vehicle moves, the distance obtained each time is accumulated and the average value L is obtained agerage , after a certain number of accumulations N, the angle output becomes δ = K * δ f ; Where K = (1-(L near / L agerage )) is the penalty coefficient, L near is the current shortest distance of the following vehicle, L agerage It is the cumulative mean of the closest distances to the following vehicles.

6. The path tracking method for an underground articulated driving vehicle according to claim 1, characterized in that: In step S2, according to the main pose P front Get the distance from the target point P target The distance D is calculated to determine the length L of the entire trajectory. First, determine the maximum speed V max , determine the acceleration a, current velocity V current =a*t, until the maximum speed V max , where t is the time interval from vehicle start to the present.

7. The path tracking method for an underground articulated driving vehicle according to claim 6, characterized in that: When the distance between the current looking point and the curve point is L circle When the speed is reduced to V circle , ensure that you pass the curve slowly, and calculate the completion degree u=D / L; when u>95%, start to slow down, and ensure that the speed formula is: V current =(1-D / L)V max 。 8. The path tracking method for an underground articulated driving vehicle according to claim 1, characterized in that: The front and rear poses of the articulated vehicle are obtained as the overall pose for trajectory tracking.

9. The path tracking method for an underground articulated driving vehicle according to claim 1, characterized in that: The sensor for acquiring the attitude information in step S1 is an attitude sensor, which is used to record the position and direction information of the vehicle. The attitude sensor includes a three-axis gyroscope, a three-axis accelerometer and a three-axis electronic compass.

10. The path tracking method for an underground articulated driving vehicle according to claim 9, characterized in that: The attitude sensor uses an embedded low-power ARM processor, combined with quaternion-based three-dimensional algorithms and data fusion technology, to output zero-drift three-dimensional attitude and orientation data represented by quaternions and Euler angles in real time.

Citation Information

Cited By

  • Articulated vehicle kinematic model modeling method

    CN121133740A