Articulated vehicle control method and articulated vehicle

Abstract

The application provides a kind of articulated vehicle control method and articulated vehicle, it is applicable to the vehicle control technical field of automatic driving, the method comprises: articulated vehicle can calculate the lateral error of the preview point relative to the driving direction of the articulated vehicle itself by obtaining the reference pose information of the preview point in the planning path and the actual pose information of control point, the articulated angle in the current driving process can be calculated based on the lateral error, according to the articulated angle calculated, the steering angle of the articulated vehicle is actively controlled, the accuracy of the tracking driving process of the articulated vehicle when advancing and retreating in automatic driving can be improved, so as to meet the accuracy requirement of position and attitude in any driving scene.

Description

Technical Field

[0001] This application belongs to the field of vehicle control technology, and particularly relates to an articulated vehicle control method and an articulated vehicle. Background Technology

[0002] Articulated vehicles are a versatile type of transport machinery used in large-scale construction projects in the mining industry and other sectors. They consist of two parts connected by an articulator, one in front and one behind. When applied to autonomous driving scenarios in underground mines, these articulated vehicles need to perform forward and backward maneuvers.

[0003] Currently, during the forward or reverse movement of an articulated vehicle, a planning module can provide a planned driving path for the articulated vehicle, which then follows this planned path to move forward or backward during autonomous driving. However, due to the nonlinearity and uncertainty of the articulated vehicle's control system, the accuracy of its forward or reverse tracking is relatively low. Summary of the Invention

[0004] This application provides an articulated vehicle control method and an articulated vehicle, which can improve the accuracy of the forward and backward tracking process during autonomous driving.

[0005] The first aspect of this application provides an articulated vehicle control method, comprising:

[0006] The articulated vehicle acquires reference pose information of the aiming point and actual pose information of the control point in the planned path; the articulated vehicle calculates the lateral error of the aiming point relative to the current driving direction based on the reference pose information and the actual pose information; the articulated vehicle calculates the articulation angle based on the lateral error; when the articulated vehicle controls the control point to track the aiming point, the articulated vehicle controls the steering based on the articulation angle.

[0007] For example, the articulated vehicle includes a front compartment and a rear compartment; wherein the front compartment and the rear compartment are connected by a hinge, and a hinge angle sensor can be installed at the hinge to measure the current actual hinge angle information; the control point can be the midpoint of the front axle corresponding to the front compartment or the midpoint of the rear axle corresponding to the rear compartment.

[0008] For example, a sensor can be installed on the roof of the front axle of the front compartment of the articulated vehicle, or a vision or simultaneous localization and mapping (SLAM) module can be installed directly in front of the front of the articulated vehicle, so as to measure the actual pose information of the midpoint of the front axle of the articulated vehicle; by converting between the front axle and the rear axle, the actual pose information of the midpoint of the rear axle can also be obtained based on the actual pose information of the midpoint of the front axle.

[0009] For example, the aiming points are a series of discrete points on the planned path of the articulated vehicle, which are reference points where the distance between the planned path and the control point meets a distance threshold.

[0010] For example, lateral error is the lateral position or distance of the aiming point relative to the left or right side of the vehicle body.

[0011] For example, the articulation angle calculated based on the lateral error is the steering angle at the articulator during the steering process of the articulated vehicle control point tracking aiming point.

[0012] In one possible implementation of the first aspect, before the articulated vehicle acquires the reference pose information of the pre-aiming point in the planned path and the actual pose information of the control points of the articulated vehicle, the method further includes:

[0013] The articulated vehicle obtains the planned speed of the pre-aimed point in the planned path; based on the planned speed, the driving direction of the articulated vehicle is determined; based on the driving direction, the control point of the articulated vehicle is determined; wherein, the control point includes the midpoint of the front axle or the midpoint of the rear axle of the articulated vehicle.

[0014] For example, if the planned speed is forward, the driving direction is forward, and the control point is the midpoint of the front axle of the articulated vehicle; if the planned speed is reverse, the driving direction is backward, and the control point is the midpoint of the rear axle of the articulated vehicle.

[0015] In one possible implementation of the first aspect, the reference pose information includes the first position coordinates of the aiming point in the global coordinate system, and the actual pose information includes the second position coordinates of the control point in the global coordinate system; the articulated vehicle calculates the lateral error of the aiming point relative to the vehicle's body coordinate system based on the reference pose information and the actual pose information, including:

[0016] The articulated vehicle calculates the third position coordinates of the aiming point in the vehicle coordinate system with the control point as the origin based on the first and second position coordinates; and determines the lateral error based on the third position coordinates.

[0017] For example, there is a transformation relationship between the vehicle coordinate system with the midpoint of the front axle as the origin and the global coordinate system. This transformation relationship can be represented by a rotation matrix. The position of the aiming point relative to the control point can be obtained through the first position coordinate and the second position coordinate. Through further transformation of the rotation matrix, the position coordinates of the aiming point relative to the control point in the vehicle coordinate system can be obtained. Based on these position coordinates, the lateral position or distance of the aiming point relative to the left or right side of the vehicle body can be obtained, i.e., the lateral error.

[0018] In one possible implementation of the first aspect, after calculating the lateral error of the aiming point relative to the current direction of travel of the articulated vehicle, the method further includes:

[0019] The lateral error of the current time is filtered based on the lateral error of the previous time adjacent to the current time.

[0020] For example, the lateral error at the previous time step includes the lateral error before filtering and the lateral error after filtering at the previous time step. The lateral error at the current time step is filtered based on the lateral error before filtering and the lateral error after filtering at the previous time step to obtain the lateral error after filtering at the current time step.

[0021] In one possible implementation of the first aspect, the articulation angle of the articulated vehicle is calculated based on the lateral error, including:

[0022] Based on the geometric relationship between the aiming distance from the aiming point to the control point and the turning radius of the control point, the articulation angle of the articulated vehicle is calculated according to the lateral error.

[0023] In one possible implementation of the first aspect, after calculating the articulation angle of the articulated vehicle based on the lateral error, the method further includes:

[0024] Based on the hinge angle of the first moment preceding the current moment and the hinge angle of the second moment preceding the first moment, the hinge angle of the current moment is adjusted in advance.

[0025] For example, the hinge angle at the first moment includes the hinge angle before and after the advance correction corresponding to the first moment, and the hinge angle at the second moment includes the hinge angle before and after the advance correction corresponding to the second moment.

[0026] In one possible implementation of the first aspect, before the articulated vehicle acquires the reference pose information of the pre-aiming point in the planned path and the actual pose information of the control points of the articulated vehicle, the method further includes:

[0027] Obtain the first pose information of the front axle midpoint of the articulated vehicle; if the driving direction determined by the planned speed is forward, then the front axle midpoint is used as the control point and the first pose information is used as the actual pose information; if the driving direction determined by the planned speed is backward, then the first pose information of the front axle midpoint is converted into the second pose information of the rear axle midpoint, and the rear axle midpoint is used as the control point and the second pose information is used as the actual pose information.

[0028] For example, the first pose information can be obtained by a sensor installed on the roof of the front axle of the front compartment or by a SLAM module installed at the front of the vehicle.

[0029] For example, by using the conversion relationship between the midpoint of the front axle and the midpoint of the rear axle, and by measuring the actual hinge angle using a hinge angle sensor installed at the hinge, the articulated vehicle can convert the measured actual pose information of the midpoint of the front axle into the actual pose information of the midpoint of the rear axle at the current moment. When the articulated vehicle is reversing, it can switch the control point from the midpoint of the front axle to the midpoint of the rear axle, which can improve the accuracy of reverse tracking and ensure that the accuracy of the rear axle midpoint tracking meets the requirements.

[0030] In one possible implementation of the first aspect, before the articulated vehicle acquires the reference pose information of the pre-aiming point in the planned path and the actual pose information of the control points of the articulated vehicle, the method further includes:

[0031] The articulated vehicle acquires reference points on the planned path; target reference points whose distance from the control point meets the distance threshold are used as pre-aiming points.

[0032] For example, the aiming point can be a discrete reference point on the planned path. The distance between the reference point and the control point of the articulated vehicle at the current position meets a distance threshold. This distance threshold can be the aiming distance, that is, the reference point whose distance is greater than or equal to the aiming distance is used as the aiming point corresponding to the control point of the articulated vehicle at the current position.

[0033] A second aspect of this application provides an articulated vehicle control device, the device comprising:

[0034] The acquisition unit is used to acquire the reference pose information of the pre-aiming point in the planned path and the actual pose information of the control point of the articulated vehicle;

[0035] The first calculation unit is used to calculate the lateral error of the aiming point relative to the current driving direction of the articulated vehicle based on the reference pose information and the actual pose information.

[0036] The second calculation unit is used to calculate the articulation angle of the articulated vehicle based on the lateral error.

[0037] A control unit is configured to control the articulated vehicle to steer according to the articulation angle when the articulated vehicle controls the control point to follow the pre-aiming point.

[0038] A third aspect of this application provides an articulated vehicle including a memory and a processor. The memory stores a computer program executable on the processor, which, when executing the computer program, performs the steps of any of the methods described in the first aspect above.

[0039] A fourth aspect of this application provides a computer-readable storage medium comprising: storing a computer program that, when executed by a processor, implements the steps of any of the methods in the first aspect above.

[0040] A fifth aspect of this application provides a computer program product that, when run on a computer, causes the computer to perform the steps of any of the methods in the first aspect described above.

[0041] The beneficial effects of this application compared with the prior art are as follows: In this application, the articulated vehicle can calculate the lateral error of the pre-aiming point relative to the driving direction of the articulated vehicle itself by obtaining the reference pose information of the pre-aiming point in the planned path and the actual pose information of the control point. Based on the lateral error, the articulation angle in the current driving process can be calculated. According to the calculated articulation angle, the steering angle of the articulated vehicle can be actively controlled, which can improve the accuracy of the forward and backward tracking driving process of the articulated vehicle during autonomous driving. It can meet the accuracy requirements of the position and attitude of the articulated vehicle in any driving scenario; it has strong ease of use and practicality. Attached Figure Description

[0042] To more clearly illustrate the technical solutions in the embodiments of this application, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0043] Figure 1 This is a schematic diagram of the architecture of the articulated vehicle provided in the embodiments of this application;

[0044] Figure 2 This is a schematic diagram illustrating the implementation process of the articulated vehicle control method provided in the embodiments of this application;

[0045] Figure 3 This is a schematic diagram illustrating the determination of the hinge angle during forward movement based on geometric relationships, provided in an embodiment of this application.

[0046] Figure 4 This is a schematic diagram illustrating the conversion of the front axle midpoint to the rear axle midpoint according to an embodiment of this application;

[0047] Figure 5 This is a schematic diagram illustrating the determination of the hinge angle during retraction based on geometric relationships, provided in an embodiment of this application.

[0048] Figure 6 This is a schematic diagram of the implementation process for determining the aiming point provided in the embodiments of this application;

[0049] Figure 7This is a schematic diagram of the structure of the articulated vehicle control device provided in the embodiments of this application;

[0050] Figure 8 This is a schematic diagram of the articulated vehicle interior structure provided in the embodiments of this application. Detailed Implementation

[0051] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods have been omitted so as not to obscure the description of this application with unnecessary detail.

[0052] When articulated vehicles are operating autonomously underground, they can use vision or laser SLAM (real-time mapping and localization) modules for mapping and localization. During lateral tracking control, the positioning sensors can be mounted above the vehicle body where the front axle is located, obtaining positioning information as the position of the midpoint of the front axle on the global map, i.e., position information in the global coordinate system. The planned path of the articulated vehicle also uses the midpoint of the front axle as the control point for path planning.

[0053] Under the above planning and positioning modes, when the articulated vehicle moves forward, the midpoint of the front axle can be used as the control point for the vehicle's movement, allowing it to track reference points in the planned path and ensuring that the front axle (i.e., the position and attitude of the vehicle's front end) meets the corresponding position and attitude requirements in the planned path. However, when the articulated vehicle is reversing into a parking space or loading / unloading goods, it needs to ensure the position and attitude accuracy of the cargo box. Since the midpoint of the front axle is used as the control point, and the position of the midpoint of the front axle in the planned path is used as the tracking target, the position and attitude accuracy of the rear axle cannot be guaranteed.

[0054] Currently, to control the rear axle of articulated vehicles, the steering axle actuator is controlled to passively move the vehicle's articulation angle, thus controlling the rear axle. This requires pre-recording the articulation angles at a predetermined number of positions. However, this method suffers from low control accuracy and is difficult to operate because the articulation angles are hard to measure. Another approach relies on inertial navigation to acquire data on the vehicle's front center of gravity, including pose, yaw rate, and velocity, while simultaneously acquiring the articulation rate. This motion information is then transferred to the rear center of gravity for state feedback control. This method requires measuring multiple state parameters and calculating the articulation rate, and it depends on inertial navigation. The control process is complex and has limited applicability, making it unsuitable for scenarios like mines where inertial navigation is not feasible.

[0055] Regarding the sensor layout, mapping, and positioning methods of the articulated vehicle, and in order to meet the lateral tracking accuracy requirements of the vehicle when reversing into a parking space and loading and unloading goods, this application provides a method for pre-aiming lateral tracking control of articulated vehicles based on geometric relationships.

[0056] In this application, when the articulated vehicle moves forward, the positioning point information (front axle midpoint) acquired by SLAM is used as the reference point (pre-aiming point) for the control point to track the planned path, ensuring that the front axle midpoint (corresponding to the preceding vehicle) meets the position and attitude requirements. When reversing, the path reference point corresponding to the planned front axle midpoint and the control point (front axle midpoint) information acquired by SLAM during vehicle movement are converted to the rear axle midpoint of the vehicle; during reversing, this is converted to driving process pre-aiming lateral tracking control with the rear axle midpoint as the target control point, ensuring that the position and attitude accuracy of the rear axle midpoint (corresponding to the following vehicle, which is the cargo compartment of the vehicle) meets the requirements during reversing.

[0057] This application directly changes the vehicle's driving direction by controlling the articulation angle and switches the reference point and control point of the planned path when moving forward and backward, achieving precise tracking of the reference point's pose. By adopting geometrically based pre-aiming control, it does not need to rely on inertial navigation devices to obtain motion state data such as vehicle front speed and heading angular velocity. Instead, it obtains vehicle front pose information through visual or laser positioning. At the same time, it only needs the articulation angle data between the front and rear of the vehicle and does not need to calculate the articulation angular velocity. Therefore, it requires less state information and has a better control effect. The sensor layout is simple and can be applied in places where inertial navigation is not suitable, such as in mines.

[0058] To illustrate the technical solution described in this application, specific embodiments are provided below.

[0059] This application applies to autonomous driving scenarios for articulated vehicles in mines. It employs SLAM (Simultaneous Mapping and Localization) for mapping and localization. The SLAM localization module can acquire the vehicle's actual position and attitude information during vehicle operation. Furthermore, since sensors are typically mounted on the front axle roof, the acquired vehicle pose information is essentially the pose information of the front axle midpoint. When moving forward, the front axle midpoint can be used as a control point to track the path (pre-aiming point). However, if the front axle midpoint is still used as the control point when reversing, the position and attitude of the rear cargo box cannot be guaranteed, and reversing often requires a specific pose for the rear axle (i.e., the rear cargo box).

[0060] Therefore, in view of the positioning method of articulated vehicles in mines and the control requirements of the rear axle posture when reversing, this application proposes a lateral control method based on geometric relationship pre-aiming, and meets the control requirements of different control point postures of the vehicle under different driving scenarios by changing the control points.

[0061] Please see Figure 1 ,Figure 1 This is a schematic diagram of the architecture of an articulated vehicle provided in an embodiment of this application. For example... Figure 1 As shown, the articulated vehicle may include a front end, a front compartment, a rear compartment, an articulator, sensors, and a SLAM positioning module. Only one of the sensors or the SLAM positioning module needs to be installed; the sensor can be positioned on the roof corresponding to the midpoint of the front axle in the front compartment, and the SLAM positioning module can be positioned directly in front of the front end. The articulated vehicle can acquire the actual pose information corresponding to the midpoint of the front axle through either the sensor or the SLAM positioning module. The front and rear compartments are connected by the articulator, which also has an articulation angle sensor that acquires the actual articulation angle at any given time.

[0062] It should be noted that the articulated vehicle also includes a path planning module, which can provide information such as the planned path, planned speed, and planned articulation angle. Based on the planned speed provided by the planning module, the articulated vehicle can determine whether to travel forward or backward. When traveling forward, the articulated vehicle uses the midpoint of its front axle as the control point; when traveling backward, the midpoint of its rear axle is used as the control point. The planned path includes a series of reference points. After determining the control point, it can be controlled to track the reference points on the planned path that meet the pre-aiming distance.

[0063] In addition, during the turning process, by calculating the hinge angle at the articulation point and actively controlling the hinge angle during the driving process, the control accuracy can be improved. Especially for reversing scenarios, by controlling the vehicle to drive according to the calculated hinge angle, the position of the rear compartment can be more accurately positioned, thereby meeting the requirements for vehicle position and attitude accuracy.

[0064] Through the embodiments of this application, the control point of the articulated vehicle is switched according to the forward or reverse driving scenario, so that the control point tracks and controls the pre-aiming point, which can meet the accuracy requirements of the position and attitude of the articulated vehicle in different scenarios; when the articulated vehicle is driving in reverse, the control point is switched from the midpoint of the front axle to the midpoint of the rear axle, which can improve the accuracy when tracking driving in reverse, so that the accuracy of tracking driving at the midpoint of the rear axle meets the requirements.

[0065] The specific implementation process and implementation principle of the present application solution are further described below through specific embodiments.

[0066] Please see Figure 2 , Figure 2 This is a schematic diagram illustrating the implementation flow of the articulated vehicle control method provided in an embodiment of this application. For example... Figure 2 As shown, the implementation process of this articulated vehicle control method may include the following steps:

[0067] S201, the articulated vehicle acquires the reference pose information of the pre-aiming point and the actual pose information of the control point in the planned path.

[0068] In some embodiments, the articulated vehicle includes a path planning module that can provide information related to the planned path. The planned path includes a series of discrete reference points, and the relevant information includes the position and attitude information of the reference points. The path planning module can also provide the theoretical articulation angle of each reference point and the planned velocity of each reference point. The aiming point is a reference point whose distance from the control point satisfies the aiming distance requirement; the reference pose information includes the position and attitude information of the reference point. The actual pose information of the control point includes the position and attitude information of the control point, which can be specifically measured by sensors or a SLAM positioning module.

[0069] It should be noted that if the control point is the midpoint of the rear axle corresponding to the rear compartment of the articulated vehicle, then the pose information of the front axle midpoint collected by the sensor or SLAM positioning module is converted into the actual pose information of the rear axle midpoint through a rotation matrix. This rotation matrix can be further determined by the hinge angle measured by the hinge angle sensor at the hinge.

[0070] For example, the working principle of the path planning module of an articulated vehicle can be implemented using existing path planning algorithms, such as sampling-based path planning algorithms: Probabilistic Road Map (PRM) and Rapidly-exploring Random Tree (RRT); or through graph search methods, convex optimization methods, etc., or by using multiple cameras and distance sensors to obtain the articulation angle at each reference point, or by calculating the theoretical articulation angle at each reference point based on the planned path.

[0071] Furthermore, when the articulated vehicle has multiple articulated compartments, multiple control points can be set. For example, when the articulated vehicle includes a front compartment, a middle compartment, and a rear compartment, there are two articulation angles: a first articulation angle at the articulation connection between the front and middle compartments, and a second articulation angle at the articulation connection between the middle and rear compartments. In this case, when the articulated vehicle is moving forward, the midpoint of the front axle corresponding to the front compartment can be the primary control point, and the midpoint of the middle axle corresponding to the middle compartment can be the secondary control point. Similarly, when the articulated vehicle is reversing, the midpoint of the rear axle corresponding to the rear compartment can be the primary control point, and the midpoint of the middle axle corresponding to the middle compartment can be the secondary control point. The path planning module of the articulated vehicle can provide planned paths corresponding to the primary and secondary control points. These planned paths can include primary and secondary reference points, thereby controlling the primary and secondary control points of the articulated vehicle to track the primary and secondary reference points respectively.

[0072] It should be noted that both the reference pose information and the actual pose information mentioned above are information in the global coordinate system.

[0073] In some embodiments, before the articulated vehicle acquires the reference pose information of the pre-aiming point in the planned path and the actual pose information of the control point of the articulated vehicle, the method further includes:

[0074] The articulated vehicle obtains the planned speed of the pre-aiming point in the planned path; based on the planned speed, the driving direction of the articulated vehicle is determined; based on the driving direction, the control point of the articulated vehicle is determined.

[0075] For example, the control point of an articulated vehicle can be the midpoint of the front axle corresponding to the front compartment or the midpoint of the rear axle corresponding to the rear compartment. When the planned speed of the aiming point is in the forward direction of travel, the midpoint of the front axle corresponding to the front compartment is determined as the control point; when the planned speed of the aiming point is in the backward direction of travel (i.e., reversing), the midpoint of the rear axle corresponding to the rear compartment is determined as the control point.

[0076] For example, when the articulated vehicle has multiple articulated compartments, when the articulated vehicle is moving forward, the midpoint of the front axle corresponding to the front compartment can be the primary control point, and the midpoint of the middle axle corresponding to the middle compartment can be the secondary control point; when the articulated vehicle is reversing, the midpoint of the rear axle corresponding to the rear compartment can be the primary control point, and the midpoint of the middle axle corresponding to the middle compartment can be the secondary control point.

[0077] S202, the articulated vehicle calculates the lateral error of the aiming point relative to the current driving direction based on the reference pose information and the actual pose information.

[0078] In some embodiments, the reference pose information includes the position coordinates and attitude information of the aiming point in the global coordinate system, and the actual pose information includes the measured or converted position coordinates and attitude information of the control point of the articulated vehicle in the global coordinate system. The lateral error is the lateral position of the aiming point relative to the left or right side of the vehicle body (or the current driving direction of the vehicle body).

[0079] In some embodiments, the reference pose information includes the first position coordinates of the aiming point in the global coordinate system, and the actual pose information includes the second position coordinates of the control point in the global coordinate system; the articulated vehicle calculates the lateral error of the aiming point relative to the vehicle's body coordinate system based on the reference pose information and the actual pose information, including:

[0080] The articulated vehicle calculates the third position coordinates of the aiming point in the vehicle coordinate system with the control point as the origin based on the first and second position coordinates; and determines the lateral error based on the third position coordinates.

[0081] In some embodiments, when the articulated vehicle is moving forward, the midpoint of the front axle corresponding to the front compartment is used as the control point, and the articulated vehicle controls its tracking of the reference point in the planned path.

[0082] When the articulated vehicle moves forward, the path planning module provides the reference pose information of the pre-aiming point corresponding to the midpoint of the front vehicle in the global coordinate system, as well as the actual pose information of the midpoint of the front axle in the global coordinate system collected by the sensor or SLAM positioning module; thereby calculating the lateral error of the pre-aiming point relative to the body coordinate system of the articulated vehicle according to formula (1).

[0083]

[0084] Among them, [xf p yf p zf p ] T Pre-aiming point P f The first position coordinate in the global coordinate system, [xf r yf r zf r ] T L is the midpoint of the front axle f The second position coordinates in the global coordinate system Let [e] be the rotation matrix corresponding to the attitude of the front axle midpoint vehicle coordinate system relative to the global coordinate system. f l f w f ] T Pre-aiming point P when moving forward f e f This represents the lateral error corresponding to forward movement.

[0085] When the articulated vehicle is reversing, the path planning module provides the reference pose information of the pre-aiming point corresponding to the midpoint of the front vehicle in the global coordinate system, as well as the actual pose information of the midpoint of the front axle in the global coordinate system collected by the sensor or SLAM positioning module; by converting the reference pose information and the actual pose information into the reference pose information of the pre-aiming point corresponding to the midpoint of the rear axle and the actual pose information of the midpoint of the rear axle, the lateral error of the pre-aiming point relative to the body coordinate system of the articulated vehicle is calculated according to formula (2).

[0086]

[0087] Among them, [xr p yr p zr p ] T Pre-aiming point P r The first position coordinate in the global coordinate system, [xr r yrr zr r ] T Let [e] be the second position coordinate of the rear axis midpoint in the global coordinate system. r l r w r ] T Pre-aiming point P when reversing r e r This refers to the lateral error corresponding to reversing. This is the rotation matrix corresponding to the attitude of the rear axle vehicle body coordinate system relative to the global coordinate system.

[0088] It should be noted that the vehicle body coordinate system of this articulated vehicle is a coordinate system in which the origin moves. Taking the control point of the articulated vehicle as the origin, when moving forward, the midpoint of the front axle is the origin of the vehicle body coordinate system, that is, the midpoint of the front axle vehicle body coordinate system; when moving backward, the midpoint of the rear axle is the origin of the vehicle body coordinate system, that is, the midpoint of the rear axle vehicle body coordinate system.

[0089] In some embodiments, after calculating the lateral error of the aiming point relative to the current direction of travel of the articulated vehicle, the method further includes:

[0090] The lateral error of the current time is filtered based on the lateral error of the previous time adjacent to the current time.

[0091] In some embodiments, the lateral error of the above calculation is filtered to make the subsequent calculated hinge angle smoother.

[0092] For example, the lateral error at the previous time step includes the filtered lateral error and the unfiltered lateral error at that previous time step. The filtered lateral error at the current time step is calculated according to formula (3).

[0093] e' k =k1e' k-1 +k2e k +k3e k-1 (3)

[0094] Among them, e' k e' represents the filter output value at the current moment, i.e., the filtered lateral error corresponding to the current moment. k-1 e represents the output value of the filter at the previous time step, i.e., the filtered lateral error corresponding to the previous time step. k e represents the input value of the filter at the current moment, i.e., the transverse error before filtering at the current moment; k-1 Let e ​​be the input value of the filter at the previous time step, i.e., the transverse error before filtering at the previous time step; where e kThis refers to the lateral error calculated above when moving forward or reversing. k1, k2, and k3 are filter parameters, which are calibrated according to the actual situation.

[0095] S203, Based on the lateral error, the articulated vehicle calculates the articulation angle.

[0096] In some embodiments, calculating the articulation angle of an articulated vehicle based on lateral error includes:

[0097] Based on the geometric relationship between the aiming distance from the aiming point to the control point and the turning radius of the control point, the articulation angle of the articulated vehicle is calculated according to the lateral error.

[0098] like Figure 3 The diagram shown illustrates how the articulated vehicle determines its forward hinge angle based on geometric relationships, as provided in this embodiment. The hinge angle corresponding to the forward movement of the articulated vehicle can be calculated using formulas (4), (5), and (6).

[0099]

[0100]

[0101]

[0102] Among them, e' f This represents the lateral error during forward movement; ld is the midpoint L of the front axle. f With the aiming point P f The path planning module can provide the pre-aiming distance between them; R f The front axle steering radius (which can be provided by the path planning module), and lf is the midpoint L of the front axle. f The distance between the hinge point and the rear axle midpoint, lr, is the distance between the hinge point and the rear axle midpoint. r The distance between the hinge point and point O, R is the distance from the hinge point to point O, θ f The hinge angle is calculated when the vehicle is moving forward.

[0103] The articulation angle of the articulated vehicle when moving forward can be calculated using the above method, which allows for more accurate control of the position and posture of the rear compartment during driving.

[0104] In some embodiments, before the articulated vehicle acquires reference pose information of a pre-aiming point in the planned path and actual pose information of the control points of the articulated vehicle, the method further includes:

[0105] Obtain the first pose information of the front axle midpoint of the articulated vehicle; if the driving direction determined by the planned speed is forward, then the front axle midpoint is used as the control point and the first pose information is used as the actual pose information; if the driving direction determined by the planned speed is backward, then the first pose information of the front axle midpoint is converted into the second pose information of the rear axle midpoint, and the rear axle midpoint is used as the control point and the second pose information is used as the actual pose information.

[0106] For example, the first pose information can be obtained by a sensor installed on the roof of the front axle of the front compartment or by a SLAM module installed at the front of the vehicle.

[0107] For example, by using the conversion relationship between the midpoint of the front axle and the midpoint of the rear axle, and by using the actual hinge angle measured by the hinge angle sensor set at the hinge, the articulated vehicle can convert the measured actual pose information of the midpoint of the front axle into the actual pose information of the midpoint of the rear axle at the current moment.

[0108] like Figure 4 As shown, this application provides a schematic diagram of the conversion of the front axle midpoint to the rear axle midpoint. Figure 4 The path diagram shown in Figure (a) illustrates that when the articulated vehicle is reversing, the pose information of the rear axle midpoint corresponding to the rear compartment can be calculated based on the reference point in the planned reversing path (shown by dashed line ①) of the front compartment, combined with the reference articulation angle (the pose is the position and direction shown by the arrow in dashed line ②). Similarly, the actual pose information collected by the sensor or SLAM positioning module can also be converted. Based on the actual pose information of the front axle midpoint during the operation of the articulated vehicle, it can be converted to the rear axle, and the rear axle midpoint can be controlled to track the reference point in the planned path. This reference point is the reference pose information of the reference point corresponding to the rear axle midpoint of the rear compartment (shown by dashed line ②) calculated based on the reference pose information of the reference point in the planned reversing path (shown by dashed line ①).

[0109] like Figure 4 Figure (b) shows a schematic diagram of the coordinate system of the front axle midpoint and the coordinate system of the rear axle midpoint when the articulated vehicle is moving forward and backward, respectively, as well as the corresponding articulation angle θ when moving forward or backward.

[0110] For example, the reference pose information of the reference point corresponding to the midpoint of the rear axle of the rear compartment is calculated based on the reference pose information of the reference point in the planned reversing path (shown by dashed line ①) corresponding to the front compartment using formulas (7) and (8), and is converted to the rear axle based on the actual pose information of the midpoint of the front axle when the articulated vehicle is running.

[0111]

[0112] q r =q f*q φ (8)

[0113] Among them, when [xf yf zf] T When using the position coordinates of the reference pose information for the reference point in the planned reversing path corresponding to the front carriage, [xr yr zr] T Let θ be the reference position coordinates in the planned reversing path corresponding to the rear cargo compartment, and θ be the planned reference hinge angle; when [xf yf zf] T When the position coordinates are in the actual pose information of the midpoint of the front axle corresponding to the front compartment, [xr yr zr] T T represents the actual position coordinates of the midpoint of the rear axle corresponding to the rear cargo compartment, where θ is the actual hinge angle collected by the hinge angle sensor; b w The rotation matrix (or the rotation matrix of the vehicle coordinate system relative to the global coordinate system) that transforms the attitude information of the front axle midpoint (planned reference attitude information or measured actual attitude information) to the rear axle midpoint is obtained by multiplying the rotation matrices represented by three Euler angles; T θ For [0 0 1] T The rotation axis is defined by the reference hinge angle or the current actual hinge angle measured by sensors during vehicle control, which forms the rotation matrix corresponding to the rotation angle; lf and lr are the distances from the midpoint of the front axle and the midpoint of the rear axle to the hinge point, respectively; q f q represents the attitude quaternion at the midpoint of the front axle (either the planned reference attitude quaternion or the measured actual attitude quaternion); r q represents the attitude quaternion of the midpoint of the rear axle after transformation; φ The quaternion corresponding to the rotation matrix that transforms the attitude from the midpoint of the front axis to the midpoint of the rear axis.

[0114] For example, the quaternion q corresponding to the rotation matrix φ rotation angle φ and direction vector It is obtained from formulas (9) and (10).

[0115] φ=π+θ (9)

[0116]

[0117] Where θ is the planned reference hinge angle or the actual hinge angle collected by the hinge angle sensor.

[0118] In some embodiments, the conversion from the front compartment to the rear compartment is realized according to the above method. The control point is converted from the midpoint of the front axle to the midpoint of the rear axle. The reference pose information of the reference point (pre-aiming point) corresponding to the front compartment is converted to the reference pose information of the reference point (pre-aiming point) corresponding to the rear compartment. The actual pose information of the midpoint of the front axle measured by the sensor or SLAM positioning module is converted to the actual pose information corresponding to the midpoint of the rear axle. Thus, the reference pose information and actual pose information corresponding to the midpoint of the rear axle in the planned path can be obtained when reversing or driving backward. The lateral error of the pre-aiming point relative to the vehicle body when reversing is calculated by formula (2).

[0119] like Figure 5 The diagram shown is a schematic of determining the hinge angle when reversing based on geometric relationships provided in this application embodiment. The hinge angle corresponding to the articulated vehicle when reversing can be calculated using formulas (11), (12), and (13).

[0120]

[0121]

[0122]

[0123] Among them, e' r This represents the lateral error during reverse driving; ld represents the midpoint L of the rear axle. r With the aiming point P r The path planning module can provide the pre-aiming distance between them; R r The rear axle steering radius (which can be provided by the path planning module), and lf is the midpoint L of the front axle. f The distance between the hinge point and the rear axle midpoint, lr, is the distance between the hinge point and the rear axle midpoint. r The distance between the hinge point and point O, R is the distance between the hinge point and point O, θ r The hinge angle calculated when the vehicle is reversing.

[0124] In some embodiments, after calculating the articulation angle of the articulated vehicle based on the lateral error, the method further includes:

[0125] Based on the hinge angle of the first moment preceding the current moment and the hinge angle of the second moment preceding the first moment, the hinge angle of the current moment is adjusted in advance.

[0126] For example, the hinge angle at the first moment includes the hinge angle before and after the advance correction corresponding to the first moment, and the hinge angle at the second moment includes the hinge angle before and after the advance correction corresponding to the second moment.

[0127] In some embodiments, the process of the articulated vehicle's articulation angle being executed to the vehicle control point for lateral tracking is a lag process. Therefore, advance correction is used to increase the phase margin of the system at low speeds. The calculated articulation angle corresponding to forward or reverse driving is advanced by formula (14) to improve the accuracy of control during the driving process of the articulated vehicle.

[0128] θ′ k =k4θ′ k-1 +k5θ′ k-2 +k6θ k +k7θ k-1 +k8θ k-2 (14)

[0129] Where, θ′ k The output value of the hinge angle at the current moment, i.e., the hinge angle after advance correction at the current moment; θ′ k-1 , θ′ k-2 These are the output values ​​of the previous two time points, namely the lead-corrected hinge angle corresponding to the first time point adjacent to the current time point, and the lead-corrected hinge angle corresponding to the second time point adjacent to the first time point, θ. k θ k-1 and θ k-2 These are the input values ​​of the hinge angle corresponding to the current time, the hinge angle of the previous first time adjacent to the current time, and the hinge angle of the previous second time adjacent to the first time, respectively. That is, θ calculated by formula (6) before advance correction. f θ can be calculated using formula (13). r k4, k5, k6, k7 and k8 are the control parameters obtained by the lead compensator based on the bilinear transformation, which can be calibrated according to the actual situation.

[0130] S204, when the articulated vehicle controls the control point to follow the aiming point, the articulated vehicle controls the steering according to the articulation angle.

[0131] In some embodiments, during the turning process, the articulated vehicle sends the aforementioned advanced corrected articulation angle to the control mechanism of the vehicle chassis for execution, controlling the articulation angle during driving. This enables the front and rear compartments to track reference points of the planned path when driving forward or backward, meeting the vehicle's position and attitude requirements in different application scenarios, reducing the probability of folding, collisions, or inaccurate positioning, and improving the accuracy of autonomous driving control.

[0132] In some embodiments, before the articulated vehicle acquires reference pose information of a pre-aiming point in the planned path and actual pose information of the control points of the articulated vehicle, the method further includes:

[0133] The articulated vehicle acquires reference points on the planned path; target reference points whose distance from the control point meets the distance threshold are used as pre-aiming points.

[0134] For example, the aiming point can be a discrete reference point on the planned path. The distance between the reference point and the control point of the articulated vehicle at the current position meets a distance threshold. This distance threshold can be the aiming distance, that is, the reference point whose distance is greater than or equal to the aiming distance is used as the aiming point corresponding to the control point of the articulated vehicle at the current position.

[0135] like Figure 6 As shown in the figure, the implementation process of determining the aiming point provided in this application embodiment is illustrated.

[0136] For example, the articulated vehicle obtains a reference point P on the planned path. k And calculate control point P c To reference point P k The distance L (k is an integer greater than or equal to 0) is determined; it is then determined whether the distance L is greater than or equal to the distance threshold ld (pre-aiming distance); if so, the reference point P is moved to the next reference point. k As a pre-aiming point; if not, continue to increment the count of k by k = k + 1, and calculate and determine the control point P. c To reference point P k Is the distance L greater than or equal to the distance threshold ld?

[0137] In some embodiments, the above implementation can also be applied to articulated vehicles including two or more articulated joints. Taking two articulated joints as an example, an articulated vehicle may include a front compartment, a middle compartment, and a rear compartment. The front compartment and the middle compartment are connected by a first articulated joint (or articulator), and the middle compartment and the rear compartment are connected by a second articulated joint (or articulator). Based on the same implementation principle as above, and combining the geometric relationship between parameters such as the turning radius of the front compartment, the middle compartment, and the rear compartment in the planned path, the aiming distance of the aiming point, and the lateral error of the aiming point relative to the vehicle body, the size of the two articulation angles is calculated. Thus, during the driving process of the articulated vehicle, the position and attitude of the compartment are controlled according to the calculated two articulation angles to meet the position and attitude requirements of the articulated vehicle in the planned path, thereby improving the control accuracy of the articulated vehicle in the autonomous driving process.

[0138] Corresponding to the method in the above embodiments, Figure 7 A structural block diagram of the articulated vehicle control device provided in the embodiments of this application is shown. For ease of explanation, only the parts related to the embodiments of this application are shown. Figure 7 The articulated vehicle control device in the example can be the execution subject of the articulated vehicle control method provided in the foregoing embodiments.

[0139] Reference Figure 7 The articulated vehicle control unit includes:

[0140] The acquisition unit is used to acquire the reference pose information of the pre-aiming point in the planned path and the actual pose information of the control point of the articulated vehicle;

[0141] The first calculation unit is used to calculate the lateral error of the aiming point relative to the current driving direction of the articulated vehicle based on the reference pose information and the actual pose information.

[0142] The second calculation unit is used to calculate the articulation angle of the articulated vehicle based on the lateral error.

[0143] A control unit is configured to control the articulated vehicle to steer according to the articulation angle when the articulated vehicle controls the control point to follow the pre-aiming point.

[0144] The process by which each module in the articulated vehicle control device provided in this application implements its respective function can be specifically referred to the foregoing. Figure 1 and Figure 2 The description of the illustrated embodiment will not be repeated here.

[0145] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0146] It should be understood that, when used in this application specification and the appended claims, the term "comprising" indicates the presence of the described features, integrals, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or a collection thereof.

[0147] It should also be understood that the term “and / or” as used in this application specification and the appended claims means any combination of one or more of the associated listed items and all possible combinations, and includes such combinations.

[0148] As used in this application specification and the appended claims, the term "if" may be interpreted, depending on the context, as "when," "once," "in response to determination," or "in response to detection." Similarly, the phrases "if determined" or "if detected [the described condition or event]" may be interpreted, depending on the context, as meaning "once determined," "in response to determination," "once detected [the described condition or event]," or "in response to detection [the described condition or event]."

[0149] Furthermore, in the description of this application and the appended claims, the terms "first," "second," "third," etc., are used only for distinguishing descriptions and should not be construed as indicating or implying relative importance. It should also be understood that although the terms "first," "second," etc., are used in the text to describe various elements in some embodiments of this application, these elements should not be limited by these terms. These terms are merely used to distinguish one element from another. For example, a first table may be named a second table, and similarly, a second table may be named a first table, without departing from the scope of the various described embodiments. Both the first table and the second table are tables, but they are not the same table.

[0150] References to "one embodiment" or "some embodiments" as described in this specification mean that one or more embodiments of this application include a specific feature, structure, or characteristic described in connection with that embodiment. Therefore, the phrases "in one embodiment," "in some embodiments," "in other embodiments," "in still other embodiments," etc., appearing in different parts of this specification do not necessarily refer to the same embodiment, but rather mean "one or more, but not all, embodiments," unless otherwise specifically emphasized. The terms "comprising," "including," "having," and variations thereof mean "including but not limited to," unless otherwise specifically emphasized.

[0151] Figure 8 This is a schematic diagram of the structure of an articulated vehicle provided in one embodiment of this application. Figure 8 As shown, the articulated vehicle 8 of this embodiment includes: at least one processor 80 ( Figure 8 (Only one is shown in the image), a memory 81, which stores a computer program 82 that can run on the processor 80. When the processor 80 executes the computer program 82, it implements the steps in the various articulated vehicle control method embodiments described above, for example... Figure 2 S201 to S204 are shown. Alternatively, when the processor 80 executes the computer program 82, it implements the functions of each module / unit in the above-described device embodiments, for example... Figure 7 The functions of units 71 to 74 shown.

[0152] The articulated vehicle 8 can be a tractor, semi-trailer, or a combination of car and trailer, bus and trailer, or tractor and semi-trailer, etc., with one or two joints. The articulated vehicle may include, but is not limited to, a processor 80 and a memory 81. Those skilled in the art will understand that... Figure 8This is merely an example of the articulated vehicle 8 and does not constitute a limitation on the articulated vehicle 8. It may include more or fewer components than shown, or combine certain components, or different components. For example, the terminal device may also include input transmission devices, network access devices, buses, etc.

[0153] The processor 80 may be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor.

[0154] In some embodiments, the memory 81 may be an internal storage unit of the articulated vehicle 8, such as a hard disk or memory of the articulated vehicle 8. The memory 81 may also be an external storage device of the articulated vehicle 8, such as a plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, etc., equipped on the articulated vehicle 8. Furthermore, the memory 81 may include both internal storage units and external storage devices of the articulated vehicle 8. The memory 81 is used to store operating systems, applications, bootloaders, data, and other programs, such as the program code of computer programs. The memory 81 can also be used to temporarily store data that has been sent or will be sent.

[0155] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0156] This application also provides an articulated vehicle, which includes at least one memory, at least one processor, and a computer program stored in the at least one memory and executable on the at least one processor. When the processor executes the computer program, it causes the articulated vehicle to perform the steps in any of the above-described method embodiments.

[0157] This application also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps described in the various method embodiments above.

[0158] This application provides a computer program product that, when run on a computer, enables the computer to execute the steps described in the various method embodiments above.

[0159] If the integrated module / unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, a recording medium, a USB flash drive, a portable hard drive, a magnetic disk, an optical disk, a computer memory, a read-only memory (ROM), a random access memory (RAM), an electrical carrier signal, a telecommunication signal, and a software distribution medium, etc.

[0160] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0161] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0162] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0163] The above-described embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application.

Claims

1. A method for controlling an articulated vehicle, characterized in that, The articulated vehicle is equipped with a vision module or a SLAM module. The vision module or SLAM module is used to acquire the actual pose information of the midpoint of the front axle of the articulated vehicle. The actual pose information of the midpoint of the rear axle is obtained by converting the actual pose information of the midpoint of the front axle. The midpoints of the front and rear axles are the control points for the articulated vehicle when it moves forward and backward, respectively. The method includes: Obtain the reference pose information of the pre-aiming points in the planned path and the actual pose information of the control points of the articulated vehicle; Based on the reference pose information and the actual pose information, calculate the lateral error of the aiming point relative to the current driving direction of the articulated vehicle; Based on the lateral error, calculate the articulation angle of the articulated vehicle; When the articulated vehicle controls the control point to follow the pre-aiming point, the articulated vehicle is steered according to the articulation angle. The method further includes: The position coordinate difference between the reference pose information and the actual pose information is transformed by the rotation matrix of the vehicle coordinate system relative to the global coordinate system to determine the position coordinates of the aiming point relative to the vehicle coordinate system. The position coordinates include the lateral error. The vehicle coordinate system is a translation coordinate system with the control point as the origin. Based on the geometric relationship between the aiming distance, lateral error, distance between the midpoint of the front axle and the hinge point, and distance between the midpoint of the rear axle and the hinge point, the hinge angle is calculated; based on the hinge angle of the previous first moment adjacent to the current moment and the hinge angle of the previous second moment adjacent to the first moment, the hinge angle of the current moment is subjected to advance correction processing.

2. The method as described in claim 1, characterized in that, Before acquiring the reference pose information of the pre-aiming points in the planned path and the actual pose information of the control points of the articulated vehicle, the method further includes: Obtain the planning speed of the pre-aiming points in the planned path; The direction of travel of the articulated vehicle is determined based on the planned speed. The control point of the articulated vehicle is determined based on the direction of travel; The control point includes the midpoint of the front axle or the midpoint of the rear axle of the articulated vehicle.

3. The method as described in claim 1, characterized in that, The reference pose information includes the first position coordinates of the pre-aiming point in the global coordinate system, and the actual pose information includes the second position coordinates of the control point in the global coordinate system; the method includes: Based on the first position coordinates and the second position coordinates, calculate the position coordinates of the aiming point in the vehicle coordinate system with the control point as the origin.

4. The method as described in claim 1, characterized in that, After calculating the lateral error of the aiming point relative to the current direction of travel of the articulated vehicle, the method further includes: The lateral error of the current moment is filtered based on the lateral error of the previous moment adjacent to the current moment.

5. The method as described in claim 2, characterized in that, Before acquiring the reference pose information of the pre-aiming points in the planned path and the actual pose information of the control points of the articulated vehicle, the method further includes: Obtain the first pose information of the midpoint of the front axle of the articulated vehicle; If the driving direction determined according to the planned speed is forward, then the first pose information is used as the actual pose information; If the driving direction determined according to the planned speed is backward, then the first pose information of the front axle midpoint is converted into the second pose information of the rear axle midpoint, and the second pose information is used as the actual pose information.

6. The method according to any one of claims 1 to 5, characterized in that, Before obtaining the reference pose information of the pre-aiming points in the planned path and the actual pose information of the control points of the articulated vehicle, the method further includes: Obtain reference points along the planned path; The target reference point whose distance from the control point meets the distance threshold among the reference points is used as the aiming point.

7. An articulated vehicle, characterized in that, The articulated vehicle includes a memory and a processor, the memory storing a computer program executable on the processor, the processor executing the computer program to implement the steps of the method as described in any one of claims 1 to 6.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the steps of the method as described in any one of claims 1 to 6.

Images