Narrow channel trajectory planning method for multiple unmanned autonomous robots

Abstract

The invention belongs to the field of track planning of unmanned autonomous robots, and particularly relates to a narrow channel track planning method for multiple unmanned autonomous robots, which comprises the following steps: independently searching a discrete path in a target narrow channel for each robot to obtain a discrete motion track; comparing the motion trails of the robots, and determining interaction events; for the two robots corresponding to each meeting interaction event, whether meeting is allowed at the position of each robot is judged, a pair of positions meeting the transverse safety of the two robots is obtained by segmenting the road width of the positions allowing meeting, and each position serves as a middle point, used for re-path planning, of the corresponding robot; based on all the middle points of each robot, path segmentation planning is carried out on the robot, trajectory planning is carried out after the planned paths are combined, a quadratic planning trajectory is obtained, and trajectory planning is completed through nonlinear optimization. According to the method, mobile robot interaction and narrow environment trajectory planning can be processed at the same time.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned autonomous robot trajectory planning, and more specifically, relates to a narrow channel trajectory planning method for multiple unmanned autonomous robots. Background Art

[0002] Trajectory planning is one of the core technologies for realizing autonomous driving. Planning a suitable trajectory is the key to the safe driving of unmanned autonomous robots. Trajectory planning mainly includes path planning and motion control, which are responsible for obtaining the state and control quantity of the mobile robot at each moment, respectively, to ensure that the mobile robot can reach the destination safely and smoothly.

[0003] At present, some patents have studied trajectory planning in narrow environments. Patent "CN117873108A" proposes a tractor-trailer trajectory planning method and controller based on building a driving corridor, which can quickly generate a safe and optimal trajectory in a complex and narrow environment. Patent "CN115447612A" proposes a narrow road meeting method based on priority, which determines the respective meeting strategies of the two vehicles according to the priority of the two vehicles. The former can solve the trajectory planning problem in narrow areas, but cannot solve the situation where there are other mobile robots. Once other mobile robots interact with the current trajectory planning mobile robot, they will not be able to generate a trajectory smoothly. The latter can use priority to meet in narrow roads, but it can only handle the situation of meeting, and cannot distinguish whether meeting is necessary, and there is no trajectory planning ability, so the actual use scenarios are very limited.

[0004] The above patents either solve the trajectory planning problem in a narrow environment or solve the logic problem of meeting vehicles, but both have their limitations and cannot combine the two aspects to obtain a method that can simultaneously handle mobile robot interaction and narrow environment trajectory planning. Summary of the invention

[0005] In view of the defects of the prior art and the need for improvement, the present invention provides a method for narrow channel trajectory planning of multiple unmanned autonomous robots, the purpose of which is to propose a method that can simultaneously handle mobile robot interaction and narrow environment trajectory planning.

[0006] To achieve the above object, according to one aspect of the present invention, a method for narrow channel trajectory planning of multiple unmanned autonomous robots is provided, comprising:

[0007] S1. independently searching for a discrete path in a target narrow channel for each unmanned autonomous robot, so as to obtain a discrete motion trajectory of the robot through motion planning;

[0008] S2. Compare the motion trajectories of the unmanned autonomous robots to determine the robot information and interaction type of each interaction event; for each interaction event of the type of interaction being a meeting, determine the preset discrete time sequence containing the interaction time of the interaction event on the discrete motion trajectory, and at each robot position corresponding to each discrete time point in the sequence, perform left and right parallel expansion of the left and right boundaries of the robot according to the position of the nearest static obstacle to obtain the maximum expansion distance d of the left and right boundaries. left and d right , when the robot's d left and d right The sum of is not less than the robot width B a The preset minimum safety distance δ min When the sum of , the robot position allows the vehicle to meet, and a pair of positions that meet the lateral safety of the two robots are obtained by dividing the road width at the robot position, and each position is used as the middle point for re-path planning of the corresponding robot;

[0009] S3. Based on all the intermediate points of each robot, segment planning is performed on the path of the robot and the planned segments are combined to perform trajectory planning to obtain a secondary planning trajectory; nonlinear optimization of the motion trajectory is performed on the secondary planning trajectory of each robot to complete the trajectory planning.

[0010] Furthermore, before obtaining the secondary planning trajectory of each robot, S3 further includes:

[0011] For the two robots corresponding to each interaction event whose interaction type is non-car interaction, the speeds of the two robots are adjusted separately according to the first-come-first-go method in the interaction time and space of the interaction event, and finally the secondary planning trajectory of each robot is obtained.

[0012] Furthermore, in S2, by dividing the road width at the position of the robot, a pair of positions that meet the lateral safety of the two robots is obtained as follows:

[0013] Re-adjust the distribution of the road width R at the robot position, and split the safety distance δ into three segments: δ1, δ2, and δ3, which are respectively used as the distance between the robot on the left side of the road and the obstacle on the left, the distance between the two robots, and the distance between the robot on the right side of the road and the obstacle on the right; where the safety distance δ satisfies:

[0014] The positions of the two robots are determined according to δ1, δ2, and δ3 as the pair of positions.

[0015] Furthermore, in S1, when each unmanned autonomous robot independently searches for a discrete path, each side of the vehicle boundary defined by the robot is retracted inward by a preset length, and the obtained new vehicle boundary is used as the vehicle boundary used for path planning, wherein the ratio of the preset length to the shortest side of the vehicle boundary before retraction is not greater than 20%, so as to relax the collision constraint between the robot and the static obstacle.

[0016] Furthermore, the optimization model for the nonlinear optimization of the motion trajectory of each robot is:

[0017]

[0018] In the formula, is the state of the robot at time k, N is the maximum sequence number of the time series, is the control quantity of the robot, x k+1 =f(x k ,u k ) represents the state equation of the kinematic model of the vehicle, m represents the number of the static obstacle, and M represents the total number of static obstacles in the target narrow channel; represents the space occupied by the robot at time k, represents the space occupied by the convex obstacle, Represents the distance between the actual robot contour and the convex obstacle, d min is the minimum distance allowed between the robot and the obstacle, represents the space occupied by the robot interacting with the controlled robot at time k; h(x k ,u k )≤0 is acceleration constraint; as well as Constitute collision constraints; J(x k ,u k ) is the cost function.

[0019] Furthermore, it also includes: adjusting the control amount of each mobile robot in real time by predicting its own future state and the driving trajectory of other mobile robots until it reaches the designated destination.

[0020] Furthermore, the control variable includes a steering angle. After the steering angle at the current time is predicted, the steering angle is corrected in the following manner:

[0021] According to the position of each obstacle on the road where the controlled robot is located, calculate the allowable robot turning radius R k , according to the relationship between the turning radius R and the steering angle θ: Get the steering angle limit value θ allowed by the obstacle k , using the steering angle limit value θ allowed for each obstaclek The upper and lower limits of the preset steering angle range are adjusted to obtain a feasible steering angle range with a reduced range; where L is the wheelbase of the vehicle;

[0022] If the steering angle obtained by MPC is not in this range, a new steering angle is obtained from the feasible range for tracking.

[0023] The present invention also provides an electronic device, comprising a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of the above method when executing the computer program.

[0024] The present invention also provides a computer-readable storage medium, which includes a stored computer program, wherein when the computer program is executed by a processor, the device where the storage medium is located is controlled to execute the steps of the method described above.

[0025] The present invention also provides a computer program product, comprising a computer program or instructions, which implement the steps of the method described above when executed by a processor.

[0026] In general, the above technical solutions conceived by the present invention can achieve the following beneficial effects:

[0027] (1) The present invention proposes a trajectory planning method for a narrow environment considering multi-vehicle interaction. First, a discrete path in the target narrow channel is independently searched for each robot to obtain a discrete motion trajectory; the motion trajectories of each robot are compared to determine each interaction event, and for each two robots corresponding to each vehicle-meeting interaction event, it is determined whether the robot position allows the vehicle to meet, and the road width at the position where the vehicle is allowed to meet is divided to obtain a pair of positions that meet the lateral safety of the two robots, and each position is used as the middle point for the corresponding robot to re-plan the path; based on all the middle points of each robot, the robot is segmented and the planned segments are merged for trajectory planning to obtain a secondary planning trajectory, and the trajectory planning is completed through nonlinear optimization. The present invention proposes a complete trajectory planning process for a narrow environment with multi-vehicle interaction, which can plan a trajectory from the starting point to the end point for the mobile robot in a narrow road environment without colliding with static obstacles or other moving mobile robots (i.e., dynamic obstacles) throughout the entire process. The method introduces mobile robot interaction into the trajectory planning and tracking controller to ensure that the mobile robot can complete trajectory planning and tracking on a narrow road in the presence of other mobile robots affecting the passage.

[0028] (2) In order to improve the solution efficiency, the present invention proposes an initial planning method that appropriately relaxes the collision constraints between the mobile robot and the static obstacles. That is, through a threshold, the robot and the obstacle polygons can be allowed to partially invade each other to ensure that a feasible solution can be obtained quickly. The relaxation of the collision constraints here will be corrected in the subsequent solution of the nonlinear optimization model to ensure that there is no collision.

[0029] (3) The present invention also proposes an optimization model for nonlinear optimization of motion trajectories, which takes into account maximum acceleration constraints and collision constraints. The collision constraints correct the robot boundary relaxation introduced during initial planning, thereby improving the solution efficiency while ensuring the solution accuracy.

[0030] (4) The present invention proposes a method for correcting the steering angle in trajectory tracking optimization. The method provides a steering angle interval, and the steering angles in the interval are all selectable. The selection method can be given according to actual needs. In practice, the method can choose to use a conservative steering angle or a more radical steering angle, giving the executor room to decide for himself, with good adaptability and guaranteed accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 A flowchart of a method for narrow channel trajectory planning of multiple unmanned autonomous robots provided by an embodiment of the present invention;

[0032] Figure 2 A schematic diagram of vehicle relaxation processing provided by an embodiment of the present invention;

[0033] Figure 3 A speed curve diagram during initial motion control provided by an embodiment of the present invention;

[0034] Figure 4 A schematic diagram of parallel expansion of vehicles provided in an embodiment of the present invention;

[0035] Figure 5 A road width distribution map provided by an embodiment of the present invention;

[0036] Figure 6 A schematic diagram of vehicle interaction provided by an embodiment of the present invention;

[0037] Figure 7 Another vehicle interaction schematic diagram provided by an embodiment of the present invention;

[0038] Figure 8 A schematic diagram of steering angle correction provided by an embodiment of the present invention;

[0039] Fig. 9 A flowchart of the overall implementation of a method for narrow channel trajectory planning for multiple unmanned autonomous robots provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0040] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0041] Embodiment 1

[0042] A narrow channel trajectory planning method for multiple unmanned autonomous robots, such as Figure 1 As shown, including:

[0043] S1. independently searching for a discrete path in the target narrow channel for each unmanned autonomous robot and obtaining a discrete motion trajectory of the robot through motion planning based on the discrete path;

[0044] S2. By comparing the motion trajectories of the unmanned autonomous robots for collision detection, it is determined whether the robots interact in time and space and the types of interaction, including interaction without encountering other vehicles and interaction with other vehicles. For two robots whose interaction type is interaction with other vehicles, the left and right boundaries of the robots are extended in parallel at each robot position corresponding to each discrete time point. The maximum extension distances of the left and right boundaries are determined according to the position of the nearest static obstacle to the robot on the road at the robot position, and the maximum extension distance d of the left and right boundaries is obtained. left and d right ; When the robot's d left d right The sum of is not less than the robot width B a The preset minimum safety distance δ min When the sum of , the robot position allows the vehicle to meet, and by dividing the road width at the robot position, a pair of positions that meet the lateral safety of the two robots are obtained, and each position is used as the replanning intermediate point of the corresponding robot; wherein, each discrete time point is a discrete motion trajectory containing the interaction time t c Each point in the preset discrete time series;

[0045] S3. Based on all the intermediate points of each robot, segment planning is performed on the path of the robot and the planned segments are combined to perform trajectory planning to obtain a secondary planning trajectory; nonlinear optimization of the motion trajectory is performed on the secondary planning trajectory of each robot to complete the trajectory planning.

[0046] It should be noted that the discrete time points of the discrete motion trajectories of the unmanned autonomous robots are consistent, so the two robots that meet share a preset discrete time sequence.

[0047] As a preferred implementation, before obtaining the secondary planning trajectory of each robot, S3 further includes:

[0048] For each interaction event with the interaction type of no-car interaction, the speed of the two robots is adjusted according to the first-come-first-go method at the interaction time and space of the interaction event, and finally the secondary planning trajectory of each robot is obtained. Specifically, according to the first-come-first-go principle, a priority relationship can be specified at the interaction point according to the current position of the mobile robot and the future turning direction. For example, the car that passes first passes at the original speed or at a speed that is appropriately decelerated, and the car that passes later decelerates or brakes, waits for the other car to pass, and then accelerates to pass.

[0049] As a preferred implementation, in S1, when independently searching for a path for each unmanned autonomous robot, each side of the vehicle boundary defined for the robot is retracted inward by a preset length, and the obtained new vehicle boundary is used as the vehicle boundary used for path planning, wherein the ratio of the preset length to the shortest side is not greater than 20%, so as to relax the collision constraint between the robot and the static obstacle.

[0050] It can be further preferred that the optimization model for nonlinear optimization of the motion trajectory of each robot is:

[0051]

[0052] In the formula, is the state of the robot at time k, N is the maximum sequence number of the time series, is the control quantity of the robot, m represents the number of the static obstacle, and M represents the total number of static obstacles in the target narrow channel; represents the space occupied by the robot at time k, represents the space occupied by the convex obstacle, Represents the distance between the actual robot contour and the convex obstacle, d min is the minimum distance allowed between the robot and the obstacle, represents the space occupied by the robot interacting with the controlled robot at time k; h(x k ,u k )≤0 is acceleration constraint; as well as Constitute collision constraints; J(x k ,u k ) is the cost function.

[0053] In addition, as a preferred implementation, in S2, by dividing the road width at the position of the robot, a pair of positions that meet the lateral safety of the two robots is obtained as follows:

[0054] Re-adjust the distribution of the road width R at the robot position, and split the safety distance δ into three segments: δ1, δ2, and δ3, which are respectively used as the distance between the robot on the left side of the road and the obstacle on the left, the distance between the two robots, and the distance between the robot on the right side of the road and the obstacle on the right; where the safety distance δ satisfies:

[0055] The positions of the two robots are determined according to δ1, δ2, and δ3 as the pair of positions.

[0056] Vehicles are a type of mobile robots, and their motion patterns are not complete. Compared with complete robots, it is more difficult for them to handle interaction problems in a narrow environment. This embodiment takes an unmanned vehicle in the field of autonomous driving (hereinafter referred to as a vehicle) as an example to explain the above solution.

[0057] The above S1 includes:

[0058] (S11) Initial path planning: For example, HybridA* is used to independently search for a path for each vehicle in a narrow map, and the paths of other mobile robots are not considered at this time. In order to improve the solution efficiency, the collision constraint between the mobile robot and the static obstacle will be appropriately relaxed, that is, through the threshold, the robot and the obstacle polygon can be allowed to partially invade each other to ensure that a feasible solution can be obtained quickly. Preferably, the relaxation of the collision constraint here will be corrected in the subsequent solution of the nonlinear optimization model to ensure no collision.

[0059] Specifically, the existing HybridA* algorithm is used to search for a discrete path P = (p1, p2, p3, ..., p n ), where each path point p i The vehicle's position coordinates (x i ,y i ) and the orientation angle At this time, other robots (such as vehicles) are temporarily ignored and only the surrounding static obstacles are considered.

[0060] Because HybridA* belongs to the path planning method of graph search, the smaller the step size is, the easier it is to search for the optimal path without collision with static obstacles in a narrow environment. At the same time, the smaller the search step size is, the slower the planning speed will be. In order to ensure the solution quality and solution efficiency, it is preferred that the HybridA* vehicle model is relaxed in this embodiment, such as Figure 2 As shown, the actual vehicle boundary is represented by a rectangular box that completely envelops the vehicle. Based on the actual vehicle boundary, the four boundaries of the rectangular box are relaxed inward by a length ζ (no more than 20%) to obtain a new relaxed vehicle boundary, which is the vehicle boundary used in HybridA*.

[0061] (S12) Initial motion control: The total length of the path is obtained through the initial path, and the speed of each path point is fitted with a sine function. Then, a set of rough (wheel) steering angles is obtained using a simple differential method, thereby converting the path planned in the previous step S11 into a trajectory with time information.

[0062] Specifically, based on the path obtained in the previous step, the estimated length S of the path is obtained by calculating the Euclidean distance between the path points:

[0063]

[0064] The vehicle speed follows the rule of accelerating first, then maintaining a constant speed, and finally decelerating. The total estimated distance S is reached when the vehicle reaches the destination, and the speed curve is fitted using a sine function.

[0065] During the driving time t, the speed curve is roughly as follows Figure 3 As shown, by integrating the speed within time t, the driving length corresponding to this speed curve can be obtained. When the driving length is defined to be equal to the estimated length S, the specific parameters of the speed curve (i.e., fitting) can be solved with the shortest driving time as the constraint.

[0066] The vehicle steering angle is obtained by using the vehicle model to solve the change in the vehicle's heading angle. In this step, only a series of initial values ​​are required. The specific calculation formula is as follows:

[0067]

[0068] Where L is the vehicle wheelbase, v is the vehicle speed, is the change in the orientation angle within the time Δt.

[0069] The above S2 performs interaction checking and classification: by comparing the trajectories of multiple robots for collision detection, it is roughly determined whether interaction occurs in time and space; if there is no interaction between the robots in time and space, it directly jumps to the execution step of nonlinear optimization of the motion trajectory; if there is interaction but there is no need to meet on the road, a speed planning is performed according to the first-come-first-go method to ensure the passage of the mobile robot; if there is interaction and meeting is required, the surrounding map is analyzed near the location where the interaction occurs, and a waypoint suitable for the two vehicles to pass at the same time is determined as the midpoint of the path of the two vehicles.

[0070] Specifically, after step S1, the motion trajectory of each vehicle in time and space has been obtained. By searching the time and the corresponding vehicle position in the trajectory of each vehicle and performing collision detection between different vehicles at the same time, it is found whether there is a certain time t c , the two cars collided.

[0071] If there is no time t c , it means that there is no collision between the vehicles in time and space, that is, when driving at normal speed, there is no interaction between the two vehicles, and it jumps directly to the execution step of nonlinear optimization of motion trajectory; if there is a time t c , then it means that the distance between vehicles is at time t c When there is a collision in time and space, it is necessary to analyze the interaction mode in detail and identify the interaction mode between the two vehicles so as to adopt the corresponding non-collision trajectory planning method to re-plan the trajectory (that is, secondary trajectory planning).

[0072] The interaction mode can be roughly divided into two categories, namely, interactive meeting and interactive but not meeting, depending on whether the vehicles need to meet each other. The judgment is based on the time t c and its subsequent trajectory within a period of time Δt. c ~t c During the period of +Δt, the interaction mode can be determined according to the relative positions of the two vehicles and the changing trends of the heading angles.

[0073] When the interaction mode is the interactive meeting mode, at the interaction point (t c Time) to analyze the surrounding map, such as Figure 4 As shown in the figure, for the interacting vehicles, the maximum extension distances are expanded outwards in parallel to the left and right of each vehicle to be interacted. The maximum extension distances of the left and right boundaries of the vehicle are determined according to the position of the static obstacle closest to the vehicle in the current position of the road where the vehicle is located, and the maximum extension distance d of the left and right boundaries of the vehicle is obtained. left and d right If two cars want to meet here, they need to meet the safety distance conditions:

[0074] d left +d right ≥B a +δ min

[0075] In the formula, B a is the vehicle width, δ min It is the preset minimum safety distance.

[0076] When left and d righy When the safety distance condition is met, the road width R and the safety distance δ satisfy the following relationship:

[0077]

[0078] In order to obtain a feasible solution, c The search is performed on the preset discrete time sequence at the time and its vicinity to obtain the vehicle position corresponding to each time point, that is, the current position, historical position and future position of the interacting vehicles on the road are obtained respectively. Based on the discrete time points and their corresponding vehicle positions, the above parallel expansion operation is performed to determine whether the maximum expansion distance of any interacting vehicle at each search time meets the above safety distance condition. If not, it is considered that the road environment when the two vehicles interact is too narrow, and it is determined that there is no meeting condition at the search time. If so, it is considered that the road environment when the two vehicles interact allows the two vehicles to meet.

[0079] For each search time t that satisfies the safety distance condition i and vehicle location, such as Figure 5 As shown in the figure, by re-adjusting the distribution of the road width R, the safety distance δ is split into three segments: δ1, δ2, and δ3, which are respectively used as the distance to the obstacle on the left, the distance between the two vehicles, and the distance to the obstacle on the right. The vehicle positions that meet the safety distance conditions are stored as a pair of waypoints, which serve as constraints for subsequent re-trajectory planning.

[0080] The interactive mode is interactive but not meeting mode: no meeting means that the vehicles do not need to travel in the opposite direction, and can be planned by passing in sequence to complete the meeting. There is no need to calculate waypoints, and the passing order is only judged and adjusted in real time based on the driving trend (speed and distance between the two vehicles, etc.).

[0081] The above S3 performs trajectory replanning: For the case of interaction but no meeting, a first-come-first-go principle is followed, and a priority relationship is specified at the interaction point according to the current position of the mobile robot and the future turning direction: the first car passing through passes at the original speed or a speed after appropriate deceleration, and the car passing later decelerates or brakes, waits for the other car to pass, and then accelerates to pass. For the case of interaction and meeting, the calculated waypoint is used as the middle point of the path planning, and step S1 is re-executed.

[0082] Specifically, if there is an optional waypoint, that is, the type of interaction is interactive meeting, then the waypoint is used as the middle point of the path planning, and discrete path planning is performed in segments and merged into a discrete path. The purpose of this step is to ensure that there is a feasible path solution for the two vehicles, that is, the interaction requirements of the two vehicles can be achieved by controlling the speed. After obtaining the new vehicle path, continue to execute trajectory planning to roughly plan the speed and steering angle of the entire path.

[0083] If the type of interaction is interaction but not meeting, then according to the collision time and the subsequent period (t c ~tc +Δt), and re-determine the speed according to the priority rules. The rules are mainly divided into two categories based on the direction of vehicle travel: when two vehicles are traveling in the same direction, the first-come-first-go principle is followed; when two vehicles are traveling in opposite directions, the judgment is made based on the driving trends and relative position relationship of the two vehicles. In the interaction area, the vehicle with the smaller vehicle path space will have priority over other vehicles.

[0084] like Figure 6 As shown, this scenario is a case of driving in the same direction. At time t c , Car A and Car B collided. Before the collision, the two cars were not traveling in the same direction, but according to the planned paths, the future driving trends of the vehicles can be known: Car A plans to turn left, and Car B plans to go straight. After the two cars interacted, the driving trajectories of Car A and Car B overlapped, and both needed to travel on the same narrow road. When replanning the trajectory, each car will slow down before the interaction occurs. When tracking and driving subsequently, they will pass in order according to the first-come, first-served method: the car that arrives at the interaction position first will accelerate first, and other vehicles will slow down and perform emergency stops when necessary.

[0085] like Figure 7 As shown, this scenario is a case of driving in opposite directions. At time t c , Car A and Car B collided. Before the collision, the two cars were basically in opposite directions. At the same time, the future driving trends of the vehicles are: Car A plans to turn left later, and Car B plans to go straight later. Both cars need to interact at a narrow fork. After the interaction, Car A drives in the direction without other vehicles, and Car B will drive along the path before Car A interacted. When replanning the trajectory, Car A needs to pass in front of Car B, otherwise Car B cannot pass. Therefore, Car A passes as quickly as possible, and Car B will start to slow down or even stop before the interaction until Car A passes and then accelerates to pass. Figure 6 and Figure 7 O1, O2, and O3 represent different obstacles.

[0086] The above S4 optimization obtains the optimal trajectory: by establishing and solving a nonlinear optimization problem, a partial area of ​​the trajectory is optimized to maximize the distance to the obstacle and improve the error tolerance space of the tracking trajectory in the next step.

[0087] Specifically, the motion trajectories of each robot obtained in S3 are optimized. At this time, the trajectory optimization problem is modeled as a nonlinear optimization problem, and a time-space collision-free trajectory that is more consistent with the vehicle kinematics is solved. The relaxation in S1 will be removed in this step, and the collision boundary uses the actual vehicle envelope boundary instead of the relaxed boundary, and is put into the optimizer for trajectory correction.

[0088] The state equation of the vehicle's kinematic model is simply expressed as the following formula, where is the state of the vehicle, The control quantity of the vehicle:

[0089] x k+1 =f(x k ,u k )

[0090] Assuming there is no disturbance, the vehicle's state and control quantities satisfy physical constraints, such as maximum acceleration:

[0091] h(x k ,u k )≤0

[0092] For a given state x k ,use represents the space occupied by the vehicle at time k, and Represents the space occupied by convex obstacles (non-convex obstacles need to be split into multiple convex obstacles in advance). Represents the distance between the actual vehicle profile and the convex obstacle, d min is the minimum distance allowed between the vehicle and the obstacle, represents the space occupied by the vehicle that interacts with the controlled vehicle at time k, then the collision constraint can be expressed as:

[0093]

[0094] The nonlinear problem can be expressed as:

[0095]

[0096] The cost function J(x k ,u k ) is related to the state and control amount of the vehicle. By setting different cost functions, different optimal paths can be obtained. Preferably, the cost function is set with the goal of making the trajectory as smooth as possible. The optimized trajectory is safer than the initial trajectory, and it is allowed to deviate from the trajectory appropriately without collision during driving.

[0097] Preferably, the method further comprises real-time trajectory tracking: by predicting its own future state and the driving trajectory of other mobile robots, the control amount of the mobile robot itself is adjusted in real time until the designated destination is reached.

[0098] The above S5 real-time tracking trajectory: the mobile robot tracks along the calculated trajectory. There will be a certain deviation between the actual driving and the planned trajectory, so it is necessary to predict its own future state and the driving trajectory of other mobile robots, and adjust the control amount of the mobile robot in real time until it reaches the designated destination.

[0099] That is, after obtaining the optimal trajectory, it is necessary to track and drive in real time. The optimal trajectory is the optimal under ideal conditions. In actual driving, it is difficult to ensure that the control amount is completely consistent with the calculated optimal control amount. Therefore, during driving, appropriate corrections need to be made to ensure that the error between the actual driving trajectory of the vehicle and the optimal trajectory is as small as possible.

[0100] This embodiment mainly uses model predictive control (hereinafter referred to as MPC) to obtain the steering angle and acceleration in actual driving, discretizes time, and predicts the state of the vehicle for n steps each time: r is the optimal trajectory for n steps as a reference, is the predicted trajectory for n steps, is the predicted control quantity for n steps, and the cost function is optimized by rolling The optimal predictive control quantity is obtained, and the value of the first step in its n steps is used as the steering angle and acceleration at the current time.

[0101] The vehicle state equation in MPC is related to the reference trajectory x r Regarding, through the reference trajectory point x when the vehicle is traveling r(k) Linearize near to obtain the linearization matrix A k , B k , C k , then the vehicle state equation is:

[0102] x k+1 =A r ·x k +B r ·u k +C r

[0103] Using MPC to obtain the steering angle and acceleration does not take into account the interaction between obstacle avoidance and the vehicle, so corrections need to be made based on actual conditions to correct the acceleration and steering angle to ensure safety.

[0104] The acceleration correction is mainly related to the interaction mode. By predicting the trajectory of other vehicles, it is determined whether interaction is needed. Only when the trajectories of the interacting vehicles are close, the speed is adjusted according to the interaction mode. Otherwise, it drives as fast as possible according to the acceleration obtained by MPC.

[0105] The correction of the steering angle requires the obstacle information around the vehicle. According to the different positions of the obstacles, the feasible range of the steering angle is calculated using the vehicle boundary. The steering angle θ of the robot corresponds to the turning radius R of the robot. The turning radius is the distance from the midpoint of the rear axle of the vehicle to the turning center. Different steering angles correspond to different turning radius R. This correspondence is only related to the wheelbase L of the vehicle:

[0106]

[0107] If the turning radius R of the vehicle at the current moment is known k , then the current steering angle θ can be calculated k Through this correspondence, the allowable turning radius R can be calculated according to the position of the obstacle. k , thereby obtaining the steering angle limit value θ k , adjust the preset steering angle range, that is, the calculated θ k It can be a new minimum steering angle or a maximum steering angle, and then the preset range is narrowed. Considering all obstacles around the vehicle, a feasible range of steering angles can be obtained. If the steering angle obtained by MPC is not in this range, a new steering angle is obtained from the feasible range for tracking.

[0108] Figure 8 This shows a situation where an obstacle is directly in front of the vehicle. In this case, the vehicle boundary BC is used to calculate the turning radius R. k (OP). is the distance from the rotation center O to the vehicle boundary point C (on the vehicle), and the distance from the rear axle center point P to the front of the vehicle is d froni , the distance to the side is d side , at this time:

[0109]

[0110] at the same time It is also equal to the distance d from the obstacle to the center of rotation obstacle , the relative position relationship between the obstacle and the vehicle center point P is known, then according to the equation The coordinates of the center of rotation can be calculated in the vehicle coordinate system, and then the current rotation radius R can be obtained. k , and thus calculate the steering angle θ k .

[0111] The corrected acceleration and steering angle can track the trajectory without collision, so that the target can be reached smoothly while considering the interaction.

[0112] It should be noted that different trajectory planning schemes are carried out for different interaction types. The complete flowchart can be seen Fig. 9 .

[0113] In general, considering that the traditional trajectory planning method will obtain a more conservative trajectory (as far away as possible) when dealing with dynamic obstacles such as other mobile robots, it means that it is difficult to achieve a feasible solution in a narrow environment (it is difficult to solve). The present invention proposes a method that can adapt to narrow road environments and solve the interaction problems therein. The present invention first plans a path for each vehicle in advance as an initial guess based on the idea of ​​collaborative planning, and solves the nonlinear optimization problem to obtain the time-space collision-free trajectory of each vehicle. Then, based on this trajectory, the mobile robot is model-predicted and tracked, and the control amount of the mobile robot is calculated in real time. If other mobile robots change their trajectories and affect normal driving, the control amount is corrected to achieve avoidance.

[0114] Embodiment 2

[0115] An electronic device comprises a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of the above method when executing the computer program.

[0116] The electronic device may be a computing device such as a desktop computer, a notebook, a PDA, a cloud server, etc. The processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA), or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. The memory may be used to store computer programs and / or modules, and the processor may perform various functions of the electronic device by running or executing computer programs and / or modules stored in the memory, and calling data stored in the memory.

[0117] The relevant technical solution is the same as that in Embodiment 1 and will not be described in detail here.

[0118] Embodiment 3

[0119] A computer-readable storage medium includes a stored computer program, wherein when the computer program is executed by a processor, the device where the storage medium is located is controlled to execute the steps of the method described above.

[0120] Specifically, the memory may include a high-speed random access memory, and may also include a non-volatile memory, such as a hard disk, an internal memory, a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card (Flash Card), at least one disk storage device, a flash memory device, or other volatile solid-state storage devices.

[0121] The relevant technical solution is the same as that in Embodiment 1 and will not be described in detail here.

[0122] Embodiment 4

[0123] A computer program product comprises a computer program or instructions, wherein when the computer program or instructions are executed by a processor, the steps of the method described above are implemented.

[0124] The relevant technical solution is the same as that in Embodiment 1 and will not be described in detail here.

[0125] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for trajectory planning of multiple unmanned autonomous robots in narrow channels, It is characterized in that include: S1. independently searching for a discrete path in a target narrow channel for each unmanned autonomous robot, so as to obtain a discrete motion trajectory of the robot through motion planning; S2, comparing the motion trajectories of each unmanned autonomous robot to determine the robot information and interaction type of each interaction event; For each interaction event of the type of vehicle-meeting, the preset discrete time sequence containing the interaction time of the interaction event on the discrete motion trajectory is determined, and at each robot position corresponding to each discrete time point in the sequence, the left and right boundaries of the robot are extended in parallel according to the position of the nearest static obstacle to obtain the maximum extension distance d of the left and right boundaries. left and d right , when the robot's d left and d right The sum of is not less than the robot width B a The preset minimum safety distance δ min When the sum of , the robot position allows the vehicle to meet, and a pair of positions that meet the lateral safety of the two robots are obtained by dividing the road width at the robot position, and each position is used as the middle point for re-path planning of the corresponding robot; S3, based on all the intermediate points of each robot, perform segmented path planning for the robot and merge the planned segments of the path to perform trajectory planning to obtain a secondary planning trajectory; The motion trajectory of each robot's quadratic planning trajectory is nonlinearly optimized to complete the trajectory planning.

2. A method for narrow channel trajectory planning of multiple unmanned autonomous robots according to claim 1, It is characterized in that Before obtaining the secondary planning trajectory of each robot, S3 further includes: For the two robots corresponding to each interaction event whose interaction type is non-car interaction, the speeds of the two robots are adjusted separately according to the first-come-first-go method in the interaction time and space of the interaction event, and finally the secondary planning trajectory of each robot is obtained.

3. A method for narrow channel trajectory planning of multiple unmanned autonomous robots according to claim 1, It is characterized in that In S2, by dividing the road width at the position of the robot, a pair of positions that meet the lateral safety of the two robots are obtained as follows: At this robot position, the road width R is readjusted and the safety distance δ is split into δ 1 , δ 2 , δ 3 The three segments are the distance between the robot on the left side of the road and the obstacle on the left, the distance between the two robots, and the distance between the robot on the right side of the road and the obstacle on the right. The safety distance δ satisfies: According to δ 1 , δ 2 , δ 3 The positions of the two robots are determined as the pair of positions.

4. A method for narrow channel trajectory planning of multiple unmanned autonomous robots according to claim 1, It is characterized in that In S1, when independently searching for discrete paths for each unmanned autonomous robot, each side of the vehicle boundary defined by the robot is retracted inward by a preset length, and the obtained new vehicle boundary is used as the vehicle boundary used for path planning, wherein the ratio of the preset length to the shortest side of the vehicle boundary before retraction is no more than 20%, so as to relax the collision constraint between the robot and the static obstacle.

5. A method for narrow channel trajectory planning of multiple unmanned autonomous robots according to claim 4, It is characterized in that The optimization model for nonlinear optimization of the motion trajectory of each robot is: In the formula, is the state of the robot at time k, N is the maximum sequence number of the time series, is the control quantity of the robot, x k+1 =f(x k ,u k ) represents the state equation of the kinematic model of the vehicle, m represents the number of the static obstacle, and M represents the total number of static obstacles in the target narrow channel; represents the space occupied by the robot at time k, represents the space occupied by the convex obstacle, Represents the distance between the actual robot contour and the convex obstacle, d min is the minimum distance allowed between the robot and the obstacle, represents the space occupied by the robot interacting with the controlled robot at time k; h(x k ,u k )≤0 is acceleration constraint; as well as Constitute collision constraints; J(x k ,u k ) is the cost function.

6. A method for narrow channel trajectory planning of multiple unmanned autonomous robots according to claim 1, It is characterized in that Also includes: By predicting its own future state and the driving trajectory of other mobile robots, each mobile robot can adjust its own control amount in real time until it reaches the designated destination.

7. A method for narrow channel trajectory planning of multiple unmanned autonomous robots according to claim 6, It is characterized in that The control quantity includes the steering angle. After the steering angle at the current time is predicted, the steering angle is corrected in the following manner: According to the position of each obstacle on the road where the controlled robot is located, calculate the allowable robot turning radius R k , according to the relationship between the turning radius R and the steering angle θ: Get the steering angle limit value θ allowed by the obstacle k , using the steering angle limit value θ allowed for each obstacle k The upper and lower limits of the preset steering angle range are adjusted to obtain a feasible steering angle range with a reduced range; where L is the wheelbase of the vehicle; If the steering angle obtained by MPC is not in this range, a new steering angle is obtained from the feasible range for tracking.

8. An electronic device comprising a memory and a processor, wherein the memory stores a computer program, It is characterized in that When the processor executes the computer program, the steps of the method according to any one of claims 1 to 7 are implemented.

9. A computer-readable storage medium, It is characterized in that The computer-readable storage medium includes a stored computer program, wherein when the computer program is executed by a processor, the device where the storage medium is located is controlled to execute the steps of the method according to any one of claims 1 to 7.

10. A computer program product comprising a computer program or instructions, It is characterized in that When the computer program or instruction is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images