An End-to-End Differentiable Trajectory Optimization Method for Safe Interactive Navigation of Robots

Abstract

The present invention discloses an end-to-end differentiable trajectory optimization method for robot safe interaction navigation, which relates to the field of robot navigation and includes the following steps: constructing data containing a rasterized BEV image as the input of a feature vector extractor; using the trained feature vector extractor to extract a hidden feature vector reflecting the current environmental trajectory optimization tendency from the BEV image; optimizing and transforming the hidden feature vector to obtain a feature vector, and setting the feature vector as the parameter of a differentiable planner dLQR; solving the LQR problem to obtain a trajectory, differentiating the trajectory and using arcsine to obtain the orientation angle; forming a gradient descent loop through the feature vector, the feature vector extractor and dLQR, and the planning strategy of dLQR can be dynamically adjusted according to the BEV image. The present invention combines the intelligence and adaptability of data-driven and the safety and efficiency of model-based methods, enabling the robot trajectory optimizer to adjust the behavior strategy according to the current scene and making the trajectory safer.

Description

Technical Field

[0001] The present invention relates to the field of robot navigation, and particularly to an end-to-end differentiable trajectory optimization method for safe interaction navigation of robots. Background Art

[0002] In recent years, robots have been more widely used in complex and open environments, such as shopping malls, museums, streets, or medical care scenarios. These environments pose dynamic risks and uncertainties, presenting a series of challenges for the safe interaction navigation of robots. Integrating robots into such dynamic environments requires the trajectory planner to accurately perceive and make decisions about the dynamic changes in the environment. Data-driven methods can well perceive and predict environmental changes by learning from a large amount of multimodal data, enabling the planner to have better environmental adaptability. However, when understanding the rich constraints of the physical world, data-driven methods require a large amount of data for support, and the number of parameters in the neural network model will also increase accordingly. In addition, due to the inherent black-box nature of the model, for some extreme cases or edge cases that have not appeared in the training set, it may not be able to make correct predictions and decisions, thus posing potential risks.

[0003] After retrieving existing patent documents and materials, for example, the Chinese invention patent with the publication number CN115092178A discloses an end-to-end autonomous driving behavior decision-making method with a multi-perspective attention mechanism. The method takes 4 cameras and a real-time navigation map as inputs, passes through a multi-perspective feature extraction module and a long short-term memory network, and finally directly outputs the steering angle control amount and speed control amount of the vehicle through a multi-layer perceptron. The Chinese invention patent with the publication number CN112212872A discloses an end-to-end autonomous driving method based on lidar and a navigation map. The method predicts the expected steering angle and expected vehicle speed of the car steering wheel at the next moment according to the top view, local navigation map, and the steering angle and vehicle speed information at the past moment. [1] proposes an end-to-end autonomous driving network that takes multi-modal sensing information as input, extracts features through an information fusion encoder, and obtains a semantic segmentation image and vehicle control information through decoding by two branches. The above-mentioned methods all have the black-box effect and lack reliability in terms of safety.

[0004] Based on traditional model-based planning methods, such as model predictive control, incorporating constraint conditions (kinematic constraints, dynamic constraints, boundary conditions, etc.) into the planning process can make the trajectory satisfy physical constraints, thereby reducing the demand for data volume. At the same time, the safety and reliability of the trajectory are also guaranteed. However, the parameters of model-based planning methods are usually not easy to change, and they lack adaptability to dynamic environments. Combining the efficiency and safety of model-based planning methods with the intelligence of data-driven methods is an effective approach and a current research hotspot. Currently, most methods focus on incorporating neural networks as part of the model planner, such as using neural networks to fit cost functions, nonlinear dynamic equations, and using neural networks as the map generator of the planner, etc. There is less work focusing on the deep integration of data-driven methods and model-based planning methods. [2] Utilized neural networks to approximate the nonlinear dynamic equations in model predictive control and constructed a framework to accelerate the solution of problems and achieve real-time performance. [3] Proposed a cost function with a unified form in the behavior planning stage and the trajectory planning stage, and used neural networks to fit this cost function during training. However, in the above methods, the neural network only replaces a part of the model-based method, and the model-based method is not coupled with other perception networks.

[0005] In response to the above problems and requirements, those skilled in the art hope to develop an end-to-end trainable trajectory optimizer, which can have better interpretability, data efficiency, and safety. The optimizer connects the neural network with a differentiable optimizer (at the gradient level) using an embedded feature vector, dynamically adjusts the planning strategy based on the upstream environmental perception results, and the differentiable optimizer can be selected such as the differentiable Linear Quadratic Regulator (dLQR).

[0006] Therefore, those skilled in the art are committed to developing an end-to-end differentiable trajectory optimization method for safe interaction navigation of robots.

[0007] References:

[0008] [1] Z.Huang, C.Lv, Y.Xing, and J.Wu, “Multi-modal sensor fusion-based deep neural network for end-to-end autonomous driving with scene understanding,” IEEE Sensors Journal, vol.21, no.10, pp.11781–11790, 2021.

[0009] [2]T. Salzmann, E. Kaufmann, J. Arrizabalaga, M. Pavone, D. Scaramuzza, and M. Ryll, “Real-time neural mpc: Deep learning model predictive control for quadrotors and agile robotic platforms,” IEEE Robotics and Automation Letters, vol. 8, no. 4, pp. 2397–2404, 2023.

[0010] [3]A. Sadat, M. Ren, A. Pokrovsky, Y.-C. Lin, E. Yumer, and R. Urtasun, “Jointly learnable behavior and trajectory planning for self-driving vehicles,” 2019 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS), p. 3949–3956, 2019. Summary of the Invention

[0011] In view of the above-mentioned defects of the prior art, the technical problem to be solved by the present invention is how to combine the intelligence and adaptability of data-driven methods with the safety and efficiency of model-based methods, so that the robot trajectory optimizer can adjust its behavior strategy according to the current scenario and make the trajectory safer.

[0012] To achieve the above object, the present invention provides an end-to-end differentiable trajectory optimization method for safe robot interaction and navigation, characterized in that the method comprises the following steps:

[0013] S101: Construct data including rasterized BEV images as the input of a feature vector extractor;

[0014] S103: Use the trained feature vector extractor to extract a hidden feature vector reflecting the current environmental trajectory optimization tendency from the BEV image;

[0015] S105: Optimize and transform the hidden feature vector to obtain a feature vector, and set the feature vector as the parameter of a differentiable planner dLQR;

[0016] S107: Solve the LQR problem to obtain a trajectory, differentiate the trajectory and use arcsine to obtain the orientation angle;

[0017] S109: Through the feature vector, the feature vector extractor and the differentiable planner dLQR form a gradient descent loop, and the planning strategy of the differentiable planner dLQR can be dynamically adjusted according to the BEV image.

[0018] Further, in S101, the BEV image is segmented into a plurality of small squares, and the values corresponding to the objects are filled in the small squares; the BEV image is composed of 6 channels, namely the ego layer, the agents layer, the agents forward speed layer, the agents side shift speed layer, the road map layer, and the lane center line layer.

[0019] Further, the values filled in the agents forward speed layer and the agents side shift speed layer are the mapping of the object speed between 0 and 1, and the filling contour is based on the shape of the agents; the agents forward speed layer and the agents side shift speed layer are obtained by transformation according to the following formula:

[0020]

[0021] In the formula, is the representation of the speed in the x direction of the agents forward direction in the ego coordinate system, is the representation of the speed in the x direction of the agents forward direction in the world coordinate system, is the homogeneous transformation matrix of the ego coordinate system relative to the world coordinate system, is the representation of the speed in the y direction of the agents side shift direction in the ego coordinate system, is the representation of the speed in the y direction of the agents side shift direction in the world coordinate system, and the subscript Wld is the world coordinate system.

[0022] Further, in S103, the feature vector extractor is set as a perception neural network based on a convolutional neural network, and the hidden feature vector is obtained by the feature vector extractor through the following structure:

[0023] p = ConvEncoder(X)

[0024] q = ReLU(MultiLinear(p))

[0025] In the formula, ConvEncoder(·) is a pre-trained resnet50 convolutional neural network, X is the input BEV image, MultiLinear(·) is the fully connected layer in the resnet50 convolutional neural network, ReLU(·) is the noise rectified linear activation function, and q is the hidden feature vector.

[0026] Further, in the step S105, the long short-term memory layer is used to optimize the hidden feature vector, so that the feature vector components at different times are correlated with each other, and the acceleration fluctuation of the trajectory is weakened. Subsequently, the optimized feature vector is transformed into the parameters of the differentiable planner dLQR through a multi-layer perceptron and an activation function layer. The specific processing process is as follows:

[0027] q * = ReLU(MLP(LSTM(q)))

[0028] In the formula, q is the hidden feature vector obtained by the feature vector extractor, and q * is the optimized feature vector. LSTM(·) is the long short-term memory network, which is used to establish a connection for the feature vectors at different time points. MLP(·) is the linear transformation layer, and ReLU(·) is the activation function layer.

[0029] Further, the differentiable planner dLQR is set as follows:

[0030]

[0031] F t τ t + f t = u t dt + p t = p t+1 t = 1, 2, 3,..., T - 1

[0032] In the formula, τ t ∈R 4 is the combination of the state quantity p t and the control quantity u t at time t. is the reference state quantity and control quantity of the ego at time t. is the state quantity and control quantity of the i-th agent at time t. p t = [x t y t T is a 2D state quantity, and u t = [u xt u yt T is a 2D control quantity. is the weight of the negative distance term between the ego and the i-th agent. is the quadratic coefficient of the tracking error term. C t ∈R 4 ×R 4 is the quadratic coefficient of the combined term of the tracking error term and the risk term. c t ∈R 4 ​​​is the first-order coefficient of the combined term of the tracking error term and the risk term, p1 is the initial state quantity of the ego, dt is the control step size, F t is the first-order coefficient, f t is the constant term, x t is the state quantity p t The state component in the x direction, and the x direction is the forward direction of the ego at the initial moment, y t is the state quantity p t The state component in the y direction, and the y direction is the lateral movement direction of the ego at the initial moment, u xt is the control quantity u t The control component in the x direction, u yt is the control quantity u t The control component in the y direction, T is the number of planned time points, N is the number of agents considered, and ⊙ is the Hadamard product.

[0033] Furthermore, the conversion relationship between the parameters of the differentiable planner dLQR and the eigenvector is:

[0034]

[0035] In the formula, is the quadratic coefficient of the tracking error term, is the quadratic coefficient of the risk term, is the first-order coefficient of the tracking error term, is the first-order coefficient of the risk term, is the weight of the quadratic control penalty term, and is the weight corresponding to the i-th agent, is the eigenvector at time t, is the state component in the x direction of the reference state quantity at time t, is the state component in the y direction of the reference state quantity at time t, is the state component in the x direction of the state quantity of the i-th agent at time t, is the state component in the y direction of the state quantity of the i-th agent at time t, is the control component u xt The corresponding quadratic control penalty term, is the control component u yt The corresponding quadratic control penalty term.

[0036] Furthermore, the perception neural network and the differentiable planner form a gradient descent loop through the eigenvector, and the loop is as follows:

[0037]

[0038] In the formula, θ is the weight to be trained in the perception neural network, loss is the loss function, and W is the parameter of the differentiable planner dLQR. W = {C, c, F, f}, where C is the quadratic coefficient of the combined term of the tracking error term and the risk term, c is the linear coefficient of the combined term of the tracking error term and the risk term, F is the linear coefficient, and f is the constant term.

[0039] Furthermore, the loss function loss consists of two parts, namely the mean squared error MSE of the trajectory points and the L1 norm of the orientation angle. The loss function loss is:

[0040] loss = Mean(MSE(P, P target ) + L1(H, H target ))

[0041] In the formula, P is the trajectory point, and P target is the training target value of the trajectory point, H is the orientation angle, and H target is the training target value of the orientation angle.

[0042] Furthermore, the training of the feature vector extractor includes the following steps:

[0043] S1011: Sample data of multiple batches as the training data set. The training data set includes a rasterized 6-channel BEV image, the lane center line intercepted at the lane speed limit as the reference trajectory, the future trajectories of multiple surrounding agents, the ego initial state, and the learning rate;

[0044] S1012: Extract a batch from the training data set, and input the BEV image in the batch into the feature vector extraction network to obtain the hidden feature vector;

[0045] S1013: Input the hidden feature vector into the LSTM for optimization to obtain the feature vector;

[0046] S1014: Convert the feature vector, the reference trajectory, and the future trajectories of the multiple agents into the parameter W of the differentiable planner dLQR;

[0047] S1015: Use the iLQR method to solve the LQR problem under the parameter W to obtain the trajectory;

[0048] S1016: Perform differential calculation on the trajectory and use arcsine to obtain the orientation angle;

[0049] S1017: Calculate the loss function;

[0050] S1018: According to the gradient descent loop, perform one backpropagation to update the weight θ;

[0051] S1019: Repeat the steps from S1012 to S1018 until the training is completed.

[0052] In a preferred embodiment of the present invention, compared with the prior art, the present invention has the following beneficial effects:

[0053] 1. The robot behavior planning model proposed in the present invention, which combines data-driven and model-based optimizers, can extract information in the environment as feature vectors, and adjust the behavior planning parameters to adapt to the dynamic environment accordingly. It combines the intelligence and adaptability of data-driven methods with the safety and efficiency of model-based methods, enabling the robot trajectory optimizer to adjust the behavior strategy according to the current scenario, making the trajectory safer. Moreover, compared with other planners, the planner trained on a small training set by this method has performance close to that trained on a complete training set, showing data efficiency.

[0054] 2. For dLQR, the present invention adopts a cost function that takes into account the collision risks of multiple agents around the robot body. By optimizing and transforming the hidden feature vectors extracted by the perception neural network and integrating them into the dLQR parameters, the feature vectors become interpretable.

[0055] 3. The present invention proposes a fusion mechanism between the perception neural network and the differentiable planner, namely the embedded vector. Optimizing this embedded vector can reduce the acceleration fluctuations of the planner and improve comfort.

[0056] The following will further illustrate the concept, specific structure and technical effects of the present invention with reference to the accompanying drawings, so as to fully understand the purpose, features and effects of the present invention. Description of the Drawings

[0057] Figure 1 is a schematic flowchart of the trajectory optimization method according to an embodiment of the present invention;

[0058] Figure 2 is a schematic diagram of the architecture of the differentiable trajectory optimization model according to an embodiment of the present invention;

[0059] Figure 3 is a schematic diagram of the rasterized 6-channel BEV image according to an embodiment of the present invention;

[0060] Figure 4 is a schematic diagram of the structure of the feature vector extractor according to an embodiment of the present invention;

[0061] Figure 5 is a schematic diagram of the feature vector optimization and transformation process according to an embodiment of the present invention;

[0062] Figure 6 is a schematic flowchart of the training process of the feature vector extractor according to an embodiment of the present invention;

[0063] Figure 7 It is a schematic diagram for the research of data efficiency in the embodiments of the present invention.

[0064] The descriptions of each label in the figure are as follows:

[0065] 1 - rasterized BEV image, 2 - feature vector extraction module, 3 - optimized feature vector, 4 - differentiable planner parameters, 5 - differentiable planner dLQR, 6 - generated trajectory. Specific embodiments

[0066] The following introduces multiple preferred embodiments of the present invention with reference to the accompanying drawings of the specification, making its technical content clearer and easier to understand. The present invention can be embodied in many different forms of embodiments, and the protection scope of the present invention is not limited to the embodiments mentioned in the text.

[0067] In the accompanying drawings, components with the same structure are denoted by the same numerical labels, and components with similar structures or functions everywhere are denoted by similar numerical labels. The size and thickness of each component shown in the drawings are arbitrarily shown, and the present invention does not limit the size and thickness of each component. To make the illustration clearer, the thickness of some components in the drawings is appropriately exaggerated.

[0068] As Figure 1 shown, an end-to-end differentiable trajectory optimization method for robot safe interaction navigation provided by the embodiments of the present invention constructs an end-to-end trajectory optimization model for robot safe interaction navigation. This model is a differentiable architecture, combining the intelligence and adaptability of data-driven and the safety and efficiency of model-based methods, and can adjust the behavior strategy according to the current scenario, making the trajectory safer.

[0069] The above method specifically includes the following steps:

[0070] Step 1: Construct data including rasterized BEV images as the input of the feature vector extractor.

[0071] The bird's-eye view BEV (Bird’s-Eye-View, BEV) is a raster image containing rich environmental information. The feature vector extractor to be trained can extract the environmental information in the bird's-eye view BEV image to generate a hidden feature vector. In this embodiment, the BEV image is divided into multiple small squares, and the small squares are filled with the values corresponding to the objects, such as Figure 3As shown. These BEV images consist of 6 channels, namely the ego layer, the agents layer, the agents forward speed layer, the agents lateral movement speed layer, the road map layer, and the lane center line layer. Among them, the values filled in the agents forward speed layer and the agents lateral movement speed layer are the mapping of the object speed to the range between 0 and 1, and the filled contours are based on the shapes of the agents. The agents forward speed layer and the agents lateral movement speed layer are transformed according to the following formula:

[0072]

[0073] In the formula, is the representation of the speed in the x direction of the agents forward direction in the ego coordinate system, is the representation of the speed in the x direction of the agents forward direction in the world coordinate system, is the homogeneous transformation matrix of the ego coordinate system relative to the world coordinate system, is the representation of the speed in the y direction of the agents lateral movement direction in the ego coordinate system, is the representation of the speed in the y direction of the agents lateral movement direction in the world coordinate system, and the subscript Wld represents the world coordinate system.

[0074] Step 2: Use the trained feature vector extractor to extract the hidden feature vector reflecting the current environmental trajectory optimization tendency from the BEV image.

[0075] The feature vector extractor to be trained is a perception neural network based on a convolutional neural network (CNN). After training, this network can extract the environmental information in the bird's-eye view BEV image to generate a hidden feature vector. The feature vector extractor obtains the hidden feature vector through the structure shown in Figure 4 as follows:

[0076] p = ConvEncoder(X)

[0077] q = ReLU(MultiLinear(p))

[0078] In the formula, ConvEncoder(·) is the pre-trained resnet50 convolutional neural network, X is the input BEV image, MultiLinear(·) is the fully connected layer in the resnet50 convolutional neural network, ReLU(·) is the noisy rectified linear activation function, and q is the hidden feature vector.

[0079] Step 3: Optimize and transform the hidden feature vector to obtain the feature vector, and set the feature vector as the parameter of the differentiable planner dLQR.

[0080] After obtaining the hidden feature vector through the feature vector extractor, it is necessary to optimize and transform the hidden feature vector to obtain the feature vector. In the process of optimizing and transforming the hidden feature vector, it is necessary to first define the interpretable dLQR 5.

[0081] This embodiment considers the collision risks of multiple surrounding intelligent agents, and defines the dLQR problem as follows:

[0082]

[0083] s.t. f0 = p1

[0084] F t τ t + f t = u t dt + p t = p t+1 t = 1, 2, 3,..., T - 1

[0085] In the formula, τ t ∈ R 4 is the combination of the state quantity p t and the control quantity u t at time t, is the reference state quantity and control quantity of the ego at time t, is the state quantity and control quantity of the i-th agent at time t, p t = [x t y t T is a 2D state quantity, u t = [u xt u yt T is a 2D control quantity, is the weight of the negative distance term between the ego and the i-th agent, is the quadratic term coefficient of the tracking error term, C t ∈ R 4 × R 4 is the quadratic term coefficient of the combined term of the tracking error term and the risk term, c t ∈ R 4 is the linear term coefficient of the combined term of the tracking error term and the risk term, p1 is the initial state quantity of the ego, dt is the control step size, F t is the linear term coefficient, f t is the constant term, x t is the state component of the state quantity p t in the x direction, and the x direction is the forward direction of the ego at the initial moment, y t is the state quantity p tThe state component in the y direction, where the y direction is the ego side shift direction at the initial moment, u xt is the control quantity u t The control component in the x direction, u yt is the control quantity u t The control component in the y direction, T is the number of planned time points, N is the number of agents considered, and ⊙ is the Hadamard product.

[0086] The hidden feature vector obtained by the feature vector extractor cannot be directly converted into the dLQR parameters because the parameters of dLQR are correlated at different time points. Directly using the hidden feature vectors with independent components at each time point output by the neural network will cause the trajectory acceleration generated by the planner to oscillate. In this embodiment, the above hidden feature vector is processed as follows Figure 5 shown in the process, and the optimized feature vector can be obtained:

[0087] Use the long short-term memory layer to optimize the hidden feature vector, make the feature vector components at different times correlated, and weaken the acceleration fluctuation of the trajectory. Then, the optimized feature vector is transformed into the parameters of the differentiable planner dLQR through the multi-layer perceptron and the activation function layer. The specific processing process is as follows:

[0088] q * = ReLU(MLP(LSTM(q)))

[0089] In the formula, q is the hidden feature vector obtained by the feature vector extractor, and q * is the optimized feature vector. LSTM(·) is the long short-term memory network, which is used to establish connections for the feature vectors at different time points. MLP(·) is the linear transformation layer, and ReLU(·) is the activation function layer.

[0090] The conversion relationship between the parameters of the differentiable planner dLQR and the feature vector is:

[0091]

[0092] In the formula, is the quadratic coefficient of the tracking error term, is the quadratic coefficient of the risk term, is the linear coefficient of the tracking error term, is the linear coefficient of the risk term, is the weight of the quadratic control penalty term, and are the weights corresponding to the i-th agent,

[0093] is the feature vector at time t, is the state component of the reference state quantity in the x direction at time t, where the x direction is the forward direction of ego at the initial time. is the state component of the reference state quantity in the y direction at time t, where the y direction is the lateral movement direction of ego at the initial time. is the state component of the state quantity of the i-th agent in the x direction at time t. is the state component of the state quantity of the i-th agent in the y direction at time t. is the control component corresponding quadratic control penalty term. is the control component corresponding quadratic control penalty term.

[0094] Step 4: Solve the LQR problem to obtain the trajectory, differentiate the trajectory, and use arcsine to obtain the heading angle.

[0095] Step 5: Form a gradient descent loop through the eigenvector, eigenvector extractor, and differentiable planner dLQR, and the planning strategy of dLQR can be dynamically adjusted according to the environmental information (BEV image).

[0096] In this embodiment, the perception neural network and the differentiable planner dLQR form a gradient descent loop through the eigenvector, and the loop is as follows:

[0097]

[0098] In the formula, θ is the weight to be trained in the perception neural network, loss is the loss function, W is the parameter of the differentiable planner dLQR, W = {C, c, F, f}, C is the quadratic coefficient of the combined term of the tracking error term and the risk term, x is the linear coefficient of the combined term of the tracking error term and the risk term, F is the linear coefficient, and f is the constant term.

[0099] The above loss function loss consists of two parts, namely the mean square error MSE of the trajectory points and the L1 norm of the heading angle, and the loss function loss is:

[0100] loss = Mean(MSE(P, P target ) + L1(H, H target ))

[0101] In the formula, P is the trajectory point, P target is the training target value of the trajectory point, H is the heading angle, H target is the training target value of the heading angle.

[0102] As Figure 6 shown, the eigenvector extractor in the embodiment of the present invention is trained using the following training process:

[0103] 1) Sample data of multiple batches as the training dataset, where the training dataset includes rasterized 6-channel BEV images, the lane centerline intercepted at the lane speed limit as the reference trajectory, the future trajectories of multiple surrounding agents, the initial state of ego, and the learning rate;

[0104] 2) Extract a batch from the training dataset, and input the BEV image in the batch into the feature vector extraction network to obtain the hidden feature vector;

[0105] 3) Pass the hidden feature vector into the LSTM for optimization to obtain the feature vector;

[0106] 4) Convert the feature vector, the reference trajectory, and the future trajectories of multiple agents to the parameter W of the differentiable planner dLQR;

[0107] 5) Use the iLQR method to solve the LQR problem under the parameter W to obtain the trajectory;

[0108] 6) Perform differential calculation on the trajectory and use arcsine to obtain the heading angle;

[0109] 7) Calculate the loss function;

[0110] 8) According to the gradient descent loop, perform one backpropagation to update the weight θ;

[0111] 9) Repeat steps 2) to 8) until the training ends.

[0112] Compared with the prior art, the end-to-end differentiable trajectory optimization method for robot safe interaction navigation provided by the embodiments of the present invention has the following advantages:

[0113] 1) In view of the problems existing in data-driven methods and model-based methods in the prior art, a robot behavior planning model integrating data-driven and model-based optimizers is proposed, which can extract information in the environment as a feature vector and adjust the behavior planning parameters to adapt to the dynamic environment. The data-driven method is an effective method for robot navigation in a dynamic open environment. However, there are many physical constraints in the real world. Understanding these constraints to generate kinematically or dynamically feasible trajectories requires a large amount of data. Due to the black-box effect of the data-driven model, there are challenges in ensuring the safety of the trajectories planned by it. Although the existing model-based methods have high efficiency and safety, they lack adaptability to the dynamic environment. The robot trajectory optimization model proposed in the embodiments of the present invention uses a differentiable linear quadratic regulator dLQR, derives the gradient expression form of the loss function loss with respect to the dLQR parameters, and connects the perception neural network with the dLQR parameters by using the embedded feature vector, thereby forming a gradient propagation loop from loss to the neural network parameters, and can be trained end-to-end. This innovation combines the intelligence and adaptability of the data-driven method and the safety and efficiency of the model-based method, enabling the robot trajectory optimizer to adjust the behavior strategy according to the current scenario and making the trajectory safer; moreover, compared with other planners, this method has a similar performance to the planner trained on the complete training set when trained on a small training set (such as 25% of the complete training set), showing data efficiency.

[0114] 2) In most of the existing work that combines data-driven methods and model-based methods, the neural network is either part of the planner or the output of the neural network is used as the parameter or input of the planner. The two methods are not coupled, there is no gradient conduction, or in a small part of the work, there is coupling but no interpretability. For dLQR, the present invention adopts a cost function that considers the collision risks of multiple agents around the robot body. After optimizing and transforming the hidden feature vector extracted by the perception neural network, it is incorporated into the dLQR parameters, making the feature vector interpretable. The dLQR used in this method has a cost function that includes a trajectory tracking error term and a risk term. The risk term refers to the negative distance between the robot body ego and the surrounding agents. This cost function is interpretable. The components of the hidden feature vector extracted by the perception neural network are independent and cannot be directly incorporated into the dLQR parameters. In this embodiment, through the long short-term memory layer, the feature vector components at each future moment are connected to avoid excessive acceleration. Subsequently, the optimized feature vector is transformed into the cost function of dLQR through a multi-layer perceptron MLP and a RuLU activation function layer. The embodiment of the present invention proposes a fusion mechanism between the perception neural network and the differentiable planner, that is, the embedded vector. Optimizing this embedded vector can reduce the acceleration fluctuation of the planner and improve comfort, and the derivation of the cost function and the transformation of the feature vector into the cost function parameters reflect the interpretability of this method.

[0115] The present invention will be described in detail below in conjunction with the preferred embodiments of the present invention.

[0116] As Figure 1 、 Figure 2 shown, the end-to-end differentiable trajectory optimization method for robot safe interaction navigation provided by the preferred embodiment of the present invention adopts an end-to-end trajectory optimization model for robot safe interaction navigation. This model is a differentiable architecture, which includes a rasterized BEV image 1 input to the network, a feature vector extraction module 2, an optimized feature vector 3, differentiable planner parameters 4, a differentiable planner dLQR 5, and a generated trajectory 6.

[0117] As Figure 3 shown, the rasterized BEV image 1 is used as the input of the present invention because it contains rich environmental information and is regarded as an effective representation form between perception and planning. The BEV image is represented in a rasterized manner, that is, the image is divided into small squares of 150×150, and the small squares are filled with the values corresponding to the objects. The BEV image consists of 6 channels, namely the ego layer, the agents layer, the forward speed layer of the agents, the side shift speed layer of the agents, the road map layer, and the lane center line layer. The values filled in the speed layer are the mapping of the object speed to between 0 and 1, and the filled contour is based on the shape of the agents.

[0118] The forward speed layer and the lateral movement speed layer of the agents are transformed according to the following formula:

[0119]

[0120] In the formula, is the representation of the speed in the x direction of the forward direction of the agents in the ego coordinate system, W is the world coordinate system, is the representation of the speed in the x direction of the forward direction of the agents in the world coordinate system, is the homogeneous transformation matrix of the ego coordinate system relative to the world coordinate system, is the representation of the speed in the y direction of the lateral movement direction of the agents in the ego coordinate system, is the representation of the speed in the y direction of the lateral movement direction of the agents in the world coordinate system.

[0121] As Figure 4 shown, the feature vector extraction module 2 is used to extract the hidden feature vector reflecting the trajectory optimization tendency of the current environment from the BEV image.

[0122] The feature vector extractor obtains the hidden feature vector through the following structure in sequence.

[0123] p = ConvEncoder(X)

[0124] q = ReLU(MultiLinear(p))

[0125] In the formula, CoonvEncoder(·) is a pre-trained resnet50 convolutional neural network, X is the input BEV image, MultiLinear(·) is the fully connected layer in resnet50, whose function is to regress the required physical meaning, ReLU(·) is the noise linear rectifier activation function, and q is the hidden feature vector.

[0126] For the optimization and transformation process of the hidden feature vector, it is necessary to first define the interpretable dLQR 5. This method considers the collision risks of multiple surrounding intelligent agents, and defines the dLQR problem as follows:

[0127]

[0128] s.t. f0 = p1

[0129] F t τ t + f t = u t dt + p t = p t+1 t = 1, 2, 3,..., T - 1

[0130] In the formula, the vector is defaulted to be a column vector. In the cost function, T is the number of planned time points, and τ t ∈R 4 is the 2D state quantity p t =[x t y t T and the 2D control quantity u t =[u xt u yt T at time t. is the reference state quantity and control quantity of the ego at time t, N is the number of considered agents, is the state quantity and control quantity of the i-th agent at time t. The cost function consists of two terms, namely the reference trajectory tracking error term and the negative distance risk term between the ego and the agents. is the weight of the negative distance term between the ego and the i-th agent, ⊙ is the Hadamard product, and x t is the state component of the state quantity p t in the x direction, and the x direction is the forward direction of the ego at the initial moment. y t is the state component of the state quantity p t in the y direction, and the y direction is the lateral displacement direction of the ego at the initial moment. u xt is the control component of the control quantity u t in the x direction, and u yt is the control component of the control quantity u t in the y direction. The cost function is expanded and simplified. Among them, is the quadratic term coefficient of the tracking error term, and C t ∈R 4 ×R 4 is the quadratic term coefficient of the combined term of the tracking error term and the risk term, and c t ∈R 4 is the linear term coefficient of the combined term of the tracking error term and the risk term. In the constraint conditions, p1 is the initial state quantity of the ego, dt is the control step size, and the kinematic equation is arranged in a linear form. F t is the linear term coefficient, and f t is the constant term.

[0131] The hidden feature vector cannot be directly converted to the dLQR parameters because the dLQR parameters are correlated at different time points. Directly using the hidden feature vectors that are independent of each time point component output by the neural network will cause the trajectory acceleration generated by the planner to oscillate. The hidden feature vector can obtain the optimized feature vector 3 through the processing process shown in Figure 5 . The processing process is shown in the following formula.​​

[0132] q * = ReLU(MLP(LSTM(q)))

[0133] In the formula, q is the hidden feature vector obtained by the CNN feature vector extractor, and q * is the optimized feature vector. LSTM(·) is the long short-term memory network, which is used to establish connections for feature vectors at different time points The MLP(·) is the linear transformation layer.

[0134] In the dLQR problem Among them contains four weight components, and these four weight components act on the negative distance risk term of the i-th agent. In addition, the reference control quantity of the reference trajectory is usually not available. Therefore, in the cost function The quadratic control penalty term coefficient of is also two weights. At time t, the parameters of dLQR require a feature vector of 4×N + 2 dimensions Then q * has a dimension of T×(4×N + 2). According to the above analysis, the conversion relationship between the dLQR parameters and the feature vector is as follows.

[0135]

[0136] In the formula, the feature vector at time t is as follows:

[0137]

[0138] In the formula is the quadratic term coefficient of the tracking error term, is the quadratic term coefficient of the risk term, is the linear term coefficient of the tracking error term, is the linear term coefficient of the risk term, is the weight of the quadratic control penalty term, and are the weights corresponding to the i-th agent, is the feature vector at time t, is the state component of the reference state quantity in the x direction at time t, is the state component of the reference state quantity in the y direction at time t, is the state component of the i-th agent in the x direction at time t, is the state component of the i-th agent in the y direction at time t, is the control component u xt corresponding to the quadratic control penalty term, To control the component u yt The corresponding quadratic control penalty term. Each component of this eigenvector reflects the attitude of the ego towards tracking the reference trajectory and the agents at risk. The weight of the quadratic control penalty term The larger it is, the more the planner prefers a small control amount, and the more conservative the ego's trajectory is. The weight corresponding to the i-th agent and The larger they are, it means that the ego is more inclined to stay away from this agent. The weight corresponding to the i-th agent and The smaller they are, from the above conversion relationship, it can be seen that and The larger they are, the more conservative the ego's trajectory is. To sum up, and The larger they are, and The smaller they are, the safer and more conservative the ego's trajectory is. From the above description, the method proposed by the present invention can change the attitude of the planner towards different agents according to the environmental information, so as to switch different driving styles. Compared with the existing methods, it is more interpretable.

[0139] The parameters of the differentiable planner dLQR can be controlled by the eigenvector, which is the basis for the differentiability of the overall framework and end-to-end training.

[0140] The following is the derivation process of the derivative of the loss function loss with respect to the dLQR parameters. This process uses the Lagrangian function, the KKT conditions, and the implicit function differentiation theorem.

[0141]

[0142] A*Γ = -B

[0143]

[0144] In the formula, L(τ,λ) is the Lagrangian equation, and A is a symmetric matrix. Define the KKT conditions as an implicit function G(W,Γ) = 0 about the dLQR parameters W = {C, c, F, f} = [vec(C) T vec(c) T vec(F) T vec(f) T T and Γ, where vec(·) is the operator that expands a matrix into a vector.

[0145] According to the implicit function differentiation theorem and the chain rule, the derivative of loss with respect to the dLQR parameter W can be obtained.

[0146] ​

[0147] In the formula, is the Kronecker product. Define

[0148]

[0149] In the formula, d can be obtained by solving the LQR problem. The LQR problem has the same A matrix as the original problem, and the B matrix of the LQR problem is where

[0150]

[0151]

[0152] The dLQR planner solves the optimal trajectory 6 as P according to the current parameters. The orientation angle H can be obtained based on the trajectory point P. Through imitating and learning the trajectory and orientation angle of the expert, (P target , H target ) is trained. The loss function loss consists of two parts, namely the mean square error MSE of the trajectory points and the L1 norm of the orientation angle, which are defined as follows.

[0153] loss = Mean(MSE(P, P target ) + L1(H, H target ))

[0154] The CNN perception neural network and the differentiable planner of the present invention form a gradient descent loop through the feature vector, and the loop is as follows.

[0155]

[0156] In the formula, θ is the weight to be trained in the CNN feature extraction network and the LSTM, which is the only part to be trained in the framework of the present invention.

[0157] As Figure 6 shown, the training process steps of the present invention are as follows:

[0158] 1. Input the rasterized 6-channel BEV image X, intercept the center line of the lane at the lane speed limit as the reference trajectory the future trajectories of the surrounding N agents annotated in the dataset the initial state p1 of the ego and the learning rate γ.

[0159] 2. Sample n batches of data for training, and the size of each batch is m.

[0160] 3. Extract a batch, and input the BEV image X in this batch into the feature vector extraction network to obtain the hidden feature vector q.

[0161] 4. Input the hidden feature vector into the LSTM for optimization to obtain the feature vector q. * 。

[0162] 5. For the feature vector q * , refer to the trajectory The future trajectories of N agents Convert to the parameter W of dLQR.

[0163] 6. Solve the LQR problem under the current parameter W. The solving method used is the iLQR method to obtain the trajectory P.

[0164] 7. Differentiate P and use arcsine to obtain the orientation angle H.

[0165] 8. Calculate the loss function loss.

[0166] 9. According to the gradient descent loop, perform one backpropagation to update the weight θ.

[0167] 10. Repeat steps 3 to 9 until the training ends.

[0168] Due to the incorporation of the model-based method, the end-to-end trajectory optimization model for robot safe interaction navigation provided by the embodiments of the present invention has better safety and data efficiency compared with existing planners through comparative experiments and data efficiency research.

[0169] Compared with the prior art, the end-to-end differentiable trajectory optimization method for robot safe interaction navigation provided by the embodiments of the present invention has certain advantages in terms of technology and performance indicators.

[0170] 1. Technical advantages

[0171] The method proposed by the present invention mainly includes three parts in architecture:

[0172] (1) A perception network based on the convolutional neural network CNN.

[0173] This network can extract the environmental information in the BEV image to generate a hidden feature vector. BEV is a grid image containing rich environmental information. The present invention adopts a 6-channel grid image and obtains a hidden feature vector through CNN.

[0174] (2) Used to optimize the hidden feature vector and transform it into the parameters of the downstream differentiable planner.

[0175] The present invention uses a long short-term memory layer (LSTM) to optimize the hidden feature vectors, enabling the feature vector components at different times to be correlated with each other, thereby reducing the acceleration fluctuations of the trajectory. Additionally, the cost function of the differentiable planner in the present invention has a trajectory tracking error term and a risk term. The risk term refers to the negative distance between the ego and the surrounding agents, which is interpretable. The present invention converts the corresponding components of the optimized feature vectors into parameters with physical meanings in the cost function. This method can analyze different strategy conversions of the planner through the changes in the feature vectors. Compared with existing technologies, this method has stronger interpretability.

[0176] (3) A differentiable planner, whose parameters are controlled by the feature vectors, thus forming a gradient loop.

[0177] The differentiable planner dLQR is the basis for the end-to-end training of the method of the present invention. dLQR forms a gradient descent loop with the CNN feature extractor by means of the feature vectors. The differentiable planner can change the planning strategy according to the environmental information to cope with the dynamically changing interactive environment. Because it integrates a model-based module, which itself has the ability of kinematic constraints and tracking the reference trajectory, the present invention has better safety and higher data efficiency compared with data-driven methods.

[0178] 2. Performance Metrics

[0179] The method proposed by the present invention is evaluated for performance on the publicly available autonomous driving dataset nuPlan. nuPlan is a large-scale autonomous driving dataset that provides a training framework, a closed-loop simulator, motion planning metrics, and an interactive page for visualizing the results for learning-based planners. This dataset collects nearly 1,200 hours of driving data in 4 cities. nuPlan provides 14 low-level metrics and 11 high-level metrics during the simulation process.

[0180] The present invention uses the following four metrics to evaluate the method of the present invention and the comparative methods. All metrics are better when they are smaller.

[0181] 1) Collision rate: The collision rate is a safety metric that counts the proportion of scenarios in which the ego and other agents collide among all the scenarios of the simulation.

[0182] 2) Acceleration: The acceleration is a comfort metric, which is a statistic of the absolute value of the maximum acceleration of the ego in each scenario during the simulation.

[0183] 3) Human driving similarity: It belongs to a driving style metric, which is a statistic of the Euclidean distance between the trajectory points generated by the planner and the expert trajectory points.

[0184] 4) Target distance: Belongs to the compliance index and is the distance between the final position of the ego and the final position of the expert.

[0185] The evaluation process uses closed-loop simulation. The trajectory of the ego is generated by the planner, and the trajectories of the agents are given by the annotations in the dataset. The two are aligned on the time scale. In the comparative experiment, the final presented index is the average result on the validation set. It is worth mentioning that the four indexes involved in the present invention aim at balanced evaluation rather than achieving the optimum simultaneously. It is difficult to achieve the optimum for these four indexes at the same time. For example, a relatively safe driving strategy will cause an increase in acceleration, while a more comfortable planner may cause the performance of the other three indexes to deteriorate. The present invention pays more attention to the safety index and the driving style index in dynamic scenarios.

[0186] A comparative experiment was conducted on five planners, among which:

[0187] 1) Ours: Refers to the method proposed in the present invention;

[0188] 2) Human: Refers to the driving data of humans in the dataset;

[0189] 3) CNN: Refers to a data-driven method that directly controls the trajectory through BEV;

[0190] 4) LQR: Refers to a model-based planner only without perception information;

[0191] 5) UrbanDriver: Refers to the benchmark model proposed by nuPlan.

[0192] For Ours and CNN, the same feature extraction module is used to extract features from BEV.

[0193] For CNN, BEV is used as the input, and features are extracted through a pre-trained resnet50 (CNN neural network). The features are output as a 3×T-dimensional vector through a fully connected layer, where T refers to the number of future time points. This vector is transformed to obtain trajectory points and orientations.

[0194] For LQR, only a section of the lane centerline intercepted at the current lane speed limit is used as the reference trajectory, and other information in the scene is not used.

[0195] UrbanDriver inputs ego information, agents information, and map information into the PointNetLayers, and the output features pass through a multi-head attention layer to generate trajectories.

[0196] The performance index comparison is shown in Table 1.

[0197] Table 1 Comparative experiment performance index

[0198]

[0199] As can be seen from the comparison data in Table 1, the method proposed in the present invention has the best safety performance and compliance performance. The improvement of safety performance will inevitably lead to an increase in acceleration, but the method also achieves satisfactory results in terms of similarity to human driving. UrbanDriver performs best in terms of comfort index and similarity to human driving index, but the low acceleration of this planner results in reduced safety and poor compliance.

[0200] Due to the incorporation of dLQR, the present invention already has the ability to track the reference trajectory even without training. Figure 7 It is a comparison of the performance indicators of 3 learning-based planners after training on 4 training sets of different scales on the same validation set. The method proposed in the present invention can still show the best similarity to human driving, safety performance, and the smallest target distance even when trained on 25% of the training set. Even though the acceleration index is not the lowest, the lower acceleration of other methods will cause the other three indicators to deteriorate.

[0201] The preferred specific embodiments of the present invention have been described in detail above. It should be understood that those of ordinary skill in the art can make many modifications and variations based on the concept of the present invention without creative efforts. Therefore, all technical solutions that can be obtained by those skilled in the art in the technical field of the present invention through logical analysis, reasoning, or limited experiments based on the concept of the present invention on the basis of the prior art should fall within the protection scope determined by the claims.

Claims

1. An end-to-end differentiable trajectory optimization method for robot safe interactive navigation, characterized in that, The method includes the following steps: S101: Construct data including a rasterized BEV image as the input of a feature vector extractor; S103: Use the trained feature vector extractor to extract a hidden feature vector reflecting the current environmental trajectory optimization tendency from the BEV image; S105: Optimize and transform the hidden feature vector to obtain a feature vector, and set the feature vector as the parameter of the differentiable planner dLQR; S107: Solve the LQR problem to obtain a trajectory, differentiate the trajectory and use arcsine to obtain the orientation angle; S109: A gradient descent loop is formed by the feature vector, the feature vector extractor and the differentiable planner dLQR, and the planning strategy of the differentiable planner dLQR is dynamically adjusted according to the BEV image; Wherein, The differentiable planner dLQR is set as: s.t.f0 = p1 F t τ t +f t =u t dt+p t =p t+1 t = 1, 2, 3, ..., T - 1 where τ t ∈R 4 is the combination of the state variable p t and the control variable u t at time t, is the reference state variable and control variable of ego at time t, is the state variable and control variable of the i-th agent at time t, p t = [x t y t T is a 2D state variable, u t = [u xt u yt T is a 2D control variable, is the weight of the negative distance term between ego and the i-th agent, is the quadratic term coefficient of the tracking error term, C t ∈R 4 ×R 4 is the quadratic term coefficient of the combined term of the tracking error term and the risk term, c t ∈R 4 is the linear term coefficient of the combined term of the tracking error term and the risk term, p1 is the initial state variable of ego, dt is the control step size, F t is the linear term coefficient, f t is the constant term, x t is the state component of the state variable p t in the x direction, and the x direction is the forward direction of ego at the initial time, y t is the state component of the state variable p t in the y direction, and the y direction is the lateral movement direction of ego at the initial time, u xt is the control component of the control variable u t in the x direction, u yt is the control component of the control variable u t in the y direction, T is the number of planned time points, N is the number of agents considered, and ⊙ is the Hadamard product;​​ The conversion relationship between the parameter of the differentiable planner dLQR and the feature vector is: In the formula, is the quadratic coefficient of the tracking error term, is the quadratic coefficient of the risk term, is the linear coefficient of the tracking error term, is the linear coefficient of the risk term, is the weight of the quadratic control penalty term, and are the weights corresponding to the i-th agent, is the feature vector at time t after optimization, is the state component of the reference state quantity in the x direction at time t, is the state component of the reference state quantity in the y direction at time t, is the state component of the state quantity of the i-th agent in the x direction at time t, is the state component of the state quantity of the i-th agent in the y direction at time t, is the control component u xt corresponding to the quadratic control penalty term, is the control component u yt corresponding to the quadratic control penalty term; A gradient descent loop is formed by the feature vector, the feature vector extractor and the differentiable planner. The feature vector extractor is configured as a perception neural network. The perception neural network and the differentiable planner form a gradient descent loop through the feature vector. The loop is as follows: Where, θ is the weight to be trained in the perception neural network, loss is the loss function, W is the parameter of the differentiable planner dLQR, W = {C, c, F, f}, and q * is the optimized feature vector, C is the quadratic coefficient of the combined term of the tracking error term and the risk term, c is the linear coefficient of the combined term of the tracking error term and the risk term, F is the linear coefficient, and f is the constant term.

2. The method according to claim 1, wherein In S101, the BEV image is divided into multiple small squares, and the values corresponding to the objects are filled in the small squares; the BEV image consists of 6 channels, namely the ego layer, the agents layer, the agents forward speed layer, the agents lateral speed layer, the road map layer and the lane center line layer.

3. The method according to claim 2, wherein The values filled in the agents forward speed layer and the agents lateral speed layer are the mapping of the object speed between 0 and 1, and the filling contour is based on the shape of the agents; the agents forward speed layer and the agents lateral speed layer are transformed according to the following formula: In the formula, ego V x represents the velocity of the agents in the forward direction x in the ego coordinate system, Wld V x represents the velocity of the agents in the forward direction x in the world coordinate system, is the homogeneous transformation matrix of the ego coordinate system relative to the world coordinate system, ego V y represents the velocity of the agents in the lateral displacement direction y in the ego coordinate system, Wld V y is the velocity of the agents in the lateral displacement direction y in the world coordinate system, and the subscript Wld represents the world coordinate system.

4. The method according to claim 3, wherein In S103, the feature vector extractor is set as a perception neural network based on a convolutional neural network. The feature vector extractor obtains the hidden feature vector through the following structure: p = ConvEncoder(X) q = ReLU(MultiLinear(p)) In the formula, ConvEncoder(·) is a pre-trained resnet50 convolutional neural network, X is the input BEV image, MultiLinear(·) is the fully connected layer in the resnet50 convolutional neural network, ReLU(·) is the noise linear rectifier activation function, and q is the hidden feature vector.

5. The method according to claim 4, wherein In S105, a long short-term memory layer is used to optimize the hidden feature vector to make the feature vector components at different times correlated with each other and weaken the acceleration fluctuation of the trajectory. Subsequently, the optimized feature vector is transformed into the parameter of the differentiable planner dLQR through a multi-layer perceptron and an activation function layer. The specific processing process is: q * = ReLU(MLP(LSTM(q))) where q is the hidden feature vector obtained by the feature vector extractor, and q * is the optimized feature vector, LSTM(·) is the long short-term memory network, which is used to establish connections for feature vectors at different time points ; MLP(·) is the linear transformation layer, and ReLU(·) is the activation function layer.

6. The method according to claim 5, characterized in that, The loss function loss consists of two parts, namely the mean squared error MSE of the trajectory points and the L1 norm of the orientation angle. The loss function loss is as follows: loss = Mean(MSE(P, P target ) + L1(H, H target )) Wherein, P is the trajectory point, and P target is the training target value of the trajectory point, H is the orientation angle, and H target is the training target value of the orientation angle.

7. The method according to claim 6, wherein The training process of the feature vector extractor includes the following steps: S1011: Sample data of multiple batches as the training data set, where the training data set includes a rasterized 6-channel BEV image, the lane center line intercepted at the lane speed limit as the reference trajectory, the future trajectories of multiple surrounding agents, the ego initial state, and the learning rate; S1012: Extract a batch from the training data set, and input the BEV image in the batch into the feature vector extraction network to obtain the hidden feature vector; S1013: Pass the hidden feature vector into the LSTM for optimization to obtain the feature vector; S1014: Convert the feature vector, the reference trajectory, and the future trajectories of the multiple agents to the parameter W of the differentiable planner dLQR; S1015: Use the iLQR method to solve the LQR problem under the parameter W to obtain the trajectory; S1016: Perform a difference calculation on the trajectory and use arcsine to obtain the orientation angle; S1017: Calculate the loss function; S1018: According to the gradient descent loop, perform one backpropagation to update the weight θ; S1019: Repeat S1012 to S1018 until the training ends.

Images