Intelligent tracked robot motion planning control method and system based on improved CILQR

Abstract

The invention provides an improved CILQR-based intelligent tracked robot motion planning control method and system, and constructs a motion planning control framework which comprises four core modules including an A * algorithm, an improved discrete point quadratic smoothing algorithm, an improved CILQR algorithm and an LQR controller. According to the framework, a motion planning task from a starting point to a target point can be realized based on an environment map fusing static prior information and dynamic real-time information, continuous curvature, safety and no collision of a planned trajectory are ensured, kinematics constraints of an intelligent tracked robot are met, and a finally generated trajectory can be accurately controlled through an LQR controller.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robots, in particular to an intelligent tracked robot motion planning control method and system based on improved CILQR. BACKGROUND

[0002] The task of intelligent tracked robot motion planning is to generate a collision-free optimal path from the starting point to the end point in the given environment map, and dynamically adjust the path through real-time perception of the robot pose information to avoid obstacles, ensuring the robot to safely reach the target point. At present, most of the researches on motion planning tasks divide them into two parts: global path planning and local motion planning. The former generates a globally feasible path based on the prior environment map, and the latter optimizes the local path in real time through sensors.

[0003] In the prior art, it is difficult to balance the path continuity and safety of intelligent tracked robot motion planning, and the ability to cope with environmental changes is weak, resulting in the planning trajectory having feasibility risks and safety hazards. SUMMARY

[0004] (I) Technical problem to be solved

[0005] Therefore, the present application provides an intelligent tracked robot motion planning control method and system based on improved CILQR, which completes the motion planning task from the starting point to the target point, while ensuring the curvature continuity, safety and collision-free of the planning result, and meeting the actual kinematic characteristics of the intelligent tracked robot. The generated trajectory can be precisely controlled by a controller (such as LQR), solving the problems mentioned in the background art.

[0006] (II) Technical solution

[0007] In order to achieve the above purpose, the present application provides an intelligent tracked robot motion planning control method based on improved CILQR, comprising:

[0008] S1: Constructing an environment model: obtaining obstacle information and robot pose information from sensors and positioning systems, and generating an environment representation in combination with a grid map;

[0009] S2: Based on the environment model, using A* algorithm based on Chebyshev distance for global path planning to calculate the global reference path from the starting point to the target point;

[0010] S3: Based on the improved discrete point quadratic smoothing algorithm of adaptive offset distance function, the global reference path is smoothed and optimized to obtain a smoothed reference path under prior environmental information;

[0011] S4: under the guidance of the smooth reference path, for local motion planning in a dynamic environment, a CILQR algorithm based on a hybrid obstacle representation framework is used to generate a collision-free path with continuous curvature and kinematically feasible;

[0012] S5: based on the collision-free path, path tracking control is performed.

[0013] In another aspect, the application also provides an intelligent tracked robot motion planning control system based on improved CILQR, comprising: an intelligent tracked robot experimental platform hardware system and an intelligent tracked robot experimental platform software architecture; the intelligent tracked robot experimental platform hardware system comprises a tracked mobile platform, a high-precision positioning system and an intelligent control host computer; the intelligent tracked robot experimental platform software architecture adopts a hierarchical design and specifically comprises a positioning node, a planning node and a lower control node.

[0014] (Three) beneficial effects

[0015] From the above technical solution, the intelligent tracked robot motion planning control method and system based on improved CILQR have the beneficial effects that:

[0016] 1. The KD-Tree index optimized grid map representation can frequently and quickly perform distance query and collision check when optimizing the trajectory, thereby ensuring the real-time performance and reliability of the entire motion planning control process.

[0017] 2. The reference path is smoothed by using an improved discrete point quadratic smoothing algorithm, a multi-index cost function (smoothness, similarity, compactness) and an adaptive safety constraint are introduced, and the path curvature smoothness and safety are effectively balanced.

[0018] 3. The improved CILQR algorithm based on hybrid obstacle representation and single circle envelope constraint is used for local motion planning, supports real-time obstacle avoidance in a dynamic environment, and generates a collision-free path in a dynamic environment map. BRIEF DESCRIPTION OF DRAWINGS

[0019] The features and advantages of the present application will be more clearly understood through reference to the accompanying drawings, which are schematic and should not be understood as limiting the present application, in which:

[0020] Figure 1 is a structure block diagram of the intelligent tracked robot motion planning control method based on improved CILQR of the embodiments of the present application;

[0021] Figure 2 is a comparative schematic diagram of the global reference path smoothing optimization processing of the embodiments of the present application;

[0022] Figure 3A mixed obstacle characterization schematic diagram for an embodiment of the present application;

[0023] Figure 4 A flowchart of iterative solution of the LQR controller for an embodiment of the present application;

[0024] Figure 5 A software architecture diagram of the intelligent tracked robot motion planning control system based on improved CILQR for an embodiment of the present application. DETAILED DESCRIPTION

[0025] To make the objectives, technical solutions, and advantages of the embodiments of the present application clearer, the following will clearly and completely describe the technical solutions in the embodiments of the present application with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are some but not all of the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative work fall within the protection scope of the present application.

[0026] As shown in Figure 1 The present application provides an intelligent tracked robot motion planning control method based on improved CILQR, which comprises:

[0027] S1: Constructing an environment model: obtaining obstacle information and pose information of the robot itself according to sensors and a positioning system, and generating an environment representation in combination with a grid map;

[0028] Obtaining obstacle information and pose information of the robot itself in a working environment of the robot according to sensors and a positioning system, and making the data information in a same global coordinate system.

[0029] A grid method is used to construct a map of the working environment of the robot, a continuous space is divided into regular grids, each grid (i.e., a grid node) uses a binary value to represent an obstacle occupancy state, 1 represents an obstacle, and 0 represents a feasible space (i.e., no obstacle). The obstacle information in the global coordinate system is fused into the grid map, and is updated according to real-time sensor information.

[0030] A grid node space indexing method based on KD-Tree is used to map the nodes in the grid map into a KD-Tree structure, and constraints are generated by using the space indexing relationship of the grid nodes.

[0031] The generated environment representation is not only a two-dimensional array of a grid map, but also a composite data structure with enhanced efficient query capability. This environment representation optimized by indexing can frequently and quickly use KD-Tree indexing to perform distance query and collision checking when optimizing a trajectory, thereby ensuring the real-time performance and reliability of the entire motion planning control process.

[0032] S2: based on the environment model, using A* algorithm based on Chebyshev distance for global path planning, calculating the global reference path from the starting point to the target point;

[0033] The expression of A* algorithm is:

[0034] F(n)=G(n)+H(n) (1)

[0035] Wherein, F(n) represents the total cost of the tracked robot from the starting point to the target node via node n; G(n) represents the actual cost of the tracked robot from the starting point to node n; H(n) is the cost estimated from node n to the target node by heuristic function, and in this embodiment, the heuristic function is Chebyshev distance.

[0036] Chebyshev distance is expressed as:

[0037] H Chebyshev (n)=M×max(|x n -x goal |,|y n -y goal |) (2)

[0038] Wherein, M represents the distance between two adjacent grid nodes; x n , y n respectively represent the horizontal coordinate and vertical coordinate of the grid node n; x goal , y goal respectively represent the horizontal coordinate and vertical coordinate of the target node.

[0039] In the 8-connected (allowing diagonal movement) grid map, Chebyshev distance is more accurate than traditional Manhattan distance, which can guide the algorithm to search the target point faster, reduce the number of nodes to be expanded, and improve the search efficiency.

[0040] S3: improved discrete point quadratic smoothing algorithm based on adaptive offset distance function is used to smooth and optimize the global reference path to obtain a smooth reference path under prior environment information; specifically including:

[0041] S301: a multi-objective cost function including smoothness, similarity and compactness is constructed, and adaptive safety constraints are introduced to convert the path optimization task into an optimization problem under the constraint condition;

[0042] The smoothness cost is represented by the sum of the square of the second-order difference between adjacent path points after smoothing:

[0043]

[0044] The similarity cost is represented by the sum of squares of the offset distances between the original path points and the smoothed path points:

[0045]

[0046] The compactness cost is represented by the sum of squares of the distances between the smoothed path points:

[0047]

[0048] In formulas (3)-(5), (x i ,y i ) represents the coordinates of the original path points; (x i ′,y i ′) represents the coordinates of the smoothed path points; and subscript i represents the number of the path points. The path points are the grid nodes in the global reference path.

[0049] The overall cost expression of the global reference path smoothing algorithm is obtained as follows:

[0050] Cost = w1Cost Smooth + w2Cost Similar + w3Cost Length (6)

[0051] where w1, w2, and w3 are weight coefficients.

[0052] In view of the safety requirement of the global reference path, a safety constraint is constructed. The top view projection of the tracked robot can be simplified as a square with a side length of L, and the equivalent envelope radius R is Based on this feature, an adaptive dynamic offset constraint is constructed, and the upper limit of the offset distance is designed as an adaptive function related to the distance to the obstacle:

[0053]

[0054] where k is a constant, representing the fixed upper limit of the offset distance; R represents the envelope radius; and (x obs ,y obs ) represents the coordinates of the nearest obstacle point to the path point (x i ′,y i ′).

[0055] S302: Convert the smoothness, similarity, and compactness costs into quadratic cost functions, and represent the safety constraint as a linear constraint, thereby converting the reference path smoothing problem into a standard quadratic programming problem for solving;

[0056] The conversion form of the smoothness cost converted into a quadratic cost function is as follows:

[0057]

[0058] wherein,

[0059]

[0060] The transformation of the similarity cost into a quadratic cost function is as follows:

[0061]

[0062] wherein,

[0063]

[0064] The transformation of the compactness cost into a quadratic cost function is as follows:

[0065]

[0066] wherein,

[0067]

[0068] Thus, the construction of the quadratic cost function in the optimization problem is completed.

[0069] The safety constraint is constructed to control the upper limit of the offset distance of the path points from the original path points, and to ensure the safety of path driving. The form of the safety constraint is as follows:

[0070]

[0071] Thus, the improved discrete point quadratic smoothing algorithm based on the adaptive offset distance function is constructed.

[0072] The purpose of the improved discrete point quadratic smoothing algorithm is to eliminate the zigzag turning of the global reference path, make it smooth and shortened, and keep a reasonable distance from the obstacles, so as to ensure that the path points do not intrude into the obstacles. As shown in FIG. 1, the left graph is the path (P0→P1→P2) without smoothing processing, and the right graph is the path (P0'→P1'→P2') after smoothing processing. Figure 2

[0073] S4: Under the guidance of the smoothed reference path, a collision-free path with continuous curvature and kinematically feasible is generated for local motion planning in a dynamic environment by using the CILQR algorithm based on a hybrid obstacle representation framework.

[0074] ​The CILQR (Constrained Iterative Linear Quadratic Regulator) algorithm is a commonly used optimal control algorithm, which is good at dealing with finite time domain, constrained nonlinear optimization problems, and can constantly optimize the trajectory path, so that the cost (including reference point deviation cost function, curvature cost function and curvature rate cost function) is minimized under the premise of meeting all constraints (including curvature amplitude constraint, curvature rate amplitude constraint and hybrid obstacle avoidance constraint based on grid network). Specifically, it includes:

[0075] S401: Convert the general local motion planning problem of the tracked robot into a discrete finite time domain motion planning problem;

[0076] First, model the problem as follows:

[0077]

[0078] Where N represents the prediction time domain; x k represents the state variable of the kth step; u k represents the control variable of the kth step; L N (x N ) represents the cost function of the end point; L k (x k , u k ) represents the cost function of the kth step; x k+1 = f k (x k , u k ) represents the state space equation of the kth step; x0 = x init represents the initial state equation constraint; p k (x k ) < 0, p k (u k ) < 0 represents inequality constraints.

[0079] Inequality constraints that need to be considered, such as curvature constraints, curvature rate constraints, and safety distance constraints with obstacles, etc. The first-order Taylor expansion of the nonlinear inequality constraint is carried out to realize the approximate linearization:

[0080]

[0081] The linearized inequality constraint is converted into a cost function in the form of a penalty function, and the exponential form of the penalty function is as follows:

[0082]

[0083] Where q1 and q2 are parameters for adjusting the linearity of the penalty function.

[0084] The local motion planning problem of the above tracked robot is converted into an unconstrained motion planning problem by the method of penalty function, as follows:

[0085]

[0086] S402: Establish a motion model of the tracked robot;

[0087] The discrete robot motion model is as follows:

[0088]

[0089] Wherein, X = [x y θ κ] represents the state quantity; represents the control quantity; x represents the horizontal coordinate of the robot mass center in the Cartesian coordinate system; y represents the vertical coordinate of the robot mass center in the Cartesian coordinate system; θ represents the yaw angle; κ represents the curvature; represents the curvature rate of change; v i dt represents the distance walked by the robot between two time steps.

[0090] The Taylor expansion is performed at the reference point, and the nonlinear terms in the matrix are eliminated to obtain the linearized model:

[0091]

[0092] The overall cost function is designed, including the reference point deviation cost function, the curvature cost function and the curvature rate of change cost function three parts:

[0093] The reference point deviation cost function is expressed as:

[0094]

[0095] Wherein, are weight matrices; X i represents the state of the intermediate point, represents the state of the intermediate reference point; X N represents the state of the target point, X goal represents the state of the target reference point.

[0096] J ref is the path tracking deviation cost function, and its construction includes the intermediate point path deviation and the terminal strong attraction term The former represents the state deviation between the path points except the starting point and the ending point and the nearest point of the global path; by constructing the terminal attraction domain of , the optimization algorithm is forced to preferentially converge to the target neighborhood, significantly reducing the number of iterative solutions, effectively improving the global path following accuracy and path terminal convergence speed.

[0097] The curvature cost function is represented as:

[0098]

[0099] where w κ represents the curvature weight coefficient.

[0100] The curvature rate cost function is represented as:

[0101]

[0102] where represents the curvature rate weight coefficient.

[0103] In the present embodiment, the overall cost function is a weighted sum of the reference point deviation cost function, the curvature cost function, and the curvature rate cost function.

[0104] Inequality constraints are set, including the curvature amplitude constraint, the curvature rate amplitude constraint, and the hybrid obstacle avoidance constraint based on the grid network.

[0105] The curvature amplitude constraint is represented as:

[0106]

[0107] where κ low and κ high represent the minimum and maximum values of the curvature, respectively.

[0108] The curvature rate amplitude constraint is represented as:

[0109]

[0110] where and represent the minimum and maximum values of the curvature rate, respectively.

[0111] To ensure the self-consistency of the above two constraints, it is set that

[0112]

[0113] where The curvature κ is defined as the rate of change of the tangent angle θ with respect to the arc length s, i.e.

[0114] The hybrid obstacle avoidance constraint based on the grid network is represented as:

[0115]

[0116] where O p represents the occupied space of the dynamic obstacle in the uncertain environment in the grid network, as shown in Figure 3 qc centroid coordinates of the circular envelope of the tracked robot.

[0117] For dynamic obstacles in uncertain environment, a grid network occupancy model (such as Figure 3 shown in the left figure) is used to represent the occupancy space, and the grid network nodes are stored in a KD-Tree, and an obstacle avoidance constraint is constructed based on the grid network occupancy model. If the distance between the optimized path and the obstacle is less than a safety threshold, a convex polygon envelope (such as Figure 3 shown in the right figure) is used to represent the occupancy space, and accurate collision detection is performed.

[0118] At this point, the entire unconstrained problem is constructed.

[0119] S403: using CILQR algorithm to solve, output the optimal trajectory (i.e. collision-free path);

[0120] S5: based on the collision-free path, path tracking control is performed;

[0121] S501: construct a linear error kinematics model of the intelligent tracked robot and discretize it:

[0122]

[0123] wherein, represents the error state quantity; v l and v r are control input quantities, representing the linear velocities of the left and right tracks respectively; v lr and v rr are reference quantities, representing the reference linear velocities of the left and right tracks respectively; θ r represents the centroid yaw angle of the intelligent tracked robot; B d represents the center distance between the left and right tracks; I is an identity matrix; T is a constant, representing the discrete time step.

[0124] S502: construct the objective function:

[0125]

[0126] wherein, Q is a state weight matrix, and R is a control input weight matrix.

[0127] S503: construct the optimal control rate, and use the LQR controller to complete path tracking;

[0128] The discrete-time Riccati equation is represented as:

[0129]

[0130] wherein, K tis the state feedback gain matrix at time step t, A is the system state transition matrix, B is the system input matrix, P t+1 is the Ricatti equation solution of the next time step, U e (t) is the control input at time t, X e (t) is the system state error at time t.

[0131] As Figure 4 shown, by iteratively solving the discrete-time Ricatti equation, the optimal control rate corresponding to the minimum value of the constraint condition is obtained, and then the optimal control input is obtained to execute the tracked robot.

[0132] On the other hand, the present application provides an improved CILQR-based intelligent tracked robot motion planning control system for executing the above method, comprising an intelligent tracked robot experimental platform hardware system and an intelligent tracked robot experimental platform software architecture.

[0133] The intelligent tracked robot experimental platform hardware system comprises a tracked mobile platform, a high-precision positioning system and an intelligent control host computer.

[0134] The tracked mobile platform, as an actuator, carries all devices and performs mobile tasks, including an ECU (Electronic Control Unit) and a communication network. Two-way CAN bus is used for communication inside: one connects the ECU and the motor controllers of the left and right wheels; the ECU sends speed, steering and other instructions to the motor controllers through this bus, and receives the state feedback (such as actual speed) of the motor, realizing precise motor control. The other connects the ECU and extends out, finally connecting to the host computer through a conversion interface; this bus is used for the intelligent control host computer to issue high-level control instructions (such as target point, speed command) to the robot.

[0135] The high-precision positioning system uses a combined inertial navigation system. This system integrates an inertial measurement unit (IMU, including a three-axis fiber-optic gyroscope, an accelerometer and a navigation computer) and a satellite navigation system (such as GPS), utilizes the complementary advantages of both, uses high-precision, low-drift inertial sensors, realizes accurate calculation and output of attitude, position and velocity, and can obtain centimeter-level positioning information.

[0136] The intelligent control host computer calculates and decides according to the real-time pose information fed back by the positioning system, generates corresponding motion control instructions, and thus realizes autonomous navigation, path tracking and other high-level intelligent functions. It includes an industrial computer, a CAN-to-USB card, an RS485-to-USB cable and other devices. The industrial computer runs the robot operating system (such as ROS), perception algorithms, decision-making and planning algorithms, etc.

[0137] As Figure 5As shown, the intelligent tracked robot experimental platform software architecture adopts a layered design (including application layer software based on an industrial computer Linux system and bottom layer control software based on an ECU), the software architecture realizes complete decoupling, so that hardware, algorithms and control can be independently developed and upgraded. Specifically, it includes a positioning node, a planning node and a lower control node.

[0138] The positioning node part is based on a multi-source data fusion framework, receives raw data streams from a combined positioning system (through RS485), can further fuse data of other sensors (such as a laser radar), realizes centimeter-level positioning accuracy through an internal global latitude and longitude coordinate analysis function, and has a local positioning information acquisition function and a frequency adjustment function.

[0139] The planning node part constructs a grid map based on a dynamic environment, uses a KD-Tree for efficient indexing, quickly queries environmental information, uses an A* algorithm to search on the known grid map, generates a global rough path from the starting point to the target point, this path is feasible but not smooth and is not suitable for direct tracking, optimizes the global path, generates a safe and smooth reference path in combination with dynamic obstacle avoidance constraints, uses an improved CILQR algorithm to consider the current dynamic obstacles and vehicle model on the basis of the reference path, and calculates a directly executable optimal local path in real time. The planning time of the planning node part is less than 500 ms, which guarantees the fast response capability of the system to the dynamic environment.

[0140] The control node part calculates the optimal control output based on the intelligent tracked robot model through an LQR controller according to the current pose of the robot (from the positioning node) and the expected pose (from the planning node), so as to minimize the tracking error and control energy consumption.

[0141] The lower control node part is responsible for communication between ROS and CAN, realizes lossless bidirectional conversion of the two;

[0142] The bottom layer control node part realizes the function of controlling the motor speed through the CAN bus command through the secondary development of the ECU.

[0143] The above is only a specific embodiment of the present application, but the protection scope of the present application is not limited thereto, any person skilled in the art can easily think of changes or replacements within the technical range disclosed by the present application, which should be covered within the protection scope of the present application.

Claims

1. An improved CILQR-based intelligent tracked robot motion planning control method, characterized in that, Comprise: S1: build an environment model: according to the sensor and positioning system to obtain the obstacle information and the pose information of the robot itself, combined with the grid map, generate the environment representation; S2: based on the environment model, using the A* algorithm based on Chebyshev distance for global path planning, calculate the global reference path from the starting point to the target point; S3: based on the improved discrete point quadratic smoothing algorithm of adaptive offset distance function, the global reference path is optimized and processed to obtain the smooth reference path under the prior environment information; Specifically, it includes: S301: construct a multi-objective cost function including smoothness, similarity and compactness, and introduce adaptive safety constraints to convert the path optimization task into an optimization problem under the constraint condition; The smoothness cost is represented by the sum of squares of second-order differences between adjacent path points after smoothing: wherein (x i ,y i ) represents the coordinates of the original path point; (x i ′,y i ′) represents the coordinates of the smoothed path point; subscript i represents the number of the path point; the path point is a grid node in the global reference path; The similarity cost is represented by the sum of squares of offset distances between the original path points and the smoothed path points: The compactness cost is represented by the square of the distance between the smoothed path points: The overall cost expression of the global reference path smoothing algorithm is obtained: Cost = w1Cost Smooth + w2Cost Similar + w3Cost Length Wherein, w1, w2, w3 are weight coefficients; The tracked robot is simplified as a square with side length L, and the equivalent envelope radius R is An adaptive dynamic offset constraint, i.e., a safety constraint, is constructed, in which the upper limit of the offset distance is designed as an adaptive function related to the distance to the obstacle: where k is a constant representing a fixed offset distance upper limit; R represents an envelope radius; (x obs ,y obs ) represents the coordinates of the obstacle point closest to the path point (x i ′,y i ′); S302: convert the smoothness, similarity and compactness cost into a quadratic cost function, and express the safety constraint as a linear constraint, convert the reference path smoothing problem into a standard quadratic programming problem for solving; S4: under the guidance of the smooth reference path, for local motion planning in dynamic environment, using CILQR algorithm based on hybrid obstacle representation framework, generate a collision-free path with continuous curvature and kinematically feasible; Specifically, it includes: S401: convert the general local motion planning problem of tracked robot into a discrete finite time domain motion planning problem; S402: establish the motion model of tracked robot; S403: solve by using CILQR algorithm, output the optimal path, that is, the collision-free path.

2. The method of claim 1, wherein, In S1, according to the sensor and positioning system, the obstacle information and the pose information of the robot in the robot working environment are obtained, and these data information is in the same global coordinate system; The grid method is used to construct the map of the robot working environment, and the continuous space is divided into regular grids. Each grid uses binary value to represent the obstacle occupancy state, 1 represents obstacle, 0 represents no obstacle; Wherein, the grid is a grid node; The obstacle information in the global coordinate system is fused into the grid map, and updated according to the real-time sensor information; The KD-Tree based grid node space indexing method is used to map the nodes in the grid map to the KD-Tree structure, and the constraints are generated by using the spatial indexing relationship of the grid nodes.

3. The method of claim 2, wherein, In S302, the conversion form of the smoothness cost converted into the quadratic cost function is as follows: wherein, The conversion form of the similarity cost converted into the quadratic cost function is as follows: wherein, The conversion form of the compactness cost converted into the quadratic cost function is as follows: wherein, Thus, the construction of the quadratic cost function in the optimization problem is completed; The safety constraint is constructed to control the upper limit of the offset distance of the path point from the original path point, and to ensure the safety of path driving; The form of safety constraint is as follows: Up to now, the improved discrete point quadratic smoothing algorithm based on adaptive offset distance function is constructed.

4. The method of claim 3, wherein, In S401, the problem is first modeled as follows: s.t.x k+1 = f k (x k , u k ), k = 0, 1, …, N - 1 x0 = x init p k (x k )<0,k=0,1,…,N-1 p k (u k )<0,k=0,1,…,N-1 p N (x N )=0 where N denotes the prediction horizon; x k denotes the state variable at step k; u k denotes the control variable at step k; L N (x N ) denotes the cost function at the end point; L k (x k , u k ) denotes the cost function at step k; x k+1 = f k (x k , u k ) denotes the state space equation at step k; x0= x init denotes the initial state equality constraint; p k (x k ) < 0, p k (u k ) < 0 denotes the inequality constraint; Inequality constraints that need to be considered, such as curvature constraint, curvature rate constraint and safety distance constraint with obstacles, etc., are applied; the first-order Taylor expansion is performed on the nonlinear inequality constraint to realize approximate linearization: The linearized inequality constraint is converted into a cost function in the form of a penalty function, and the exponential form penalty function is as follows: Where q1 and q2 are parameters for adjusting the linearity of the penalty function. The above local motion planning problem of the tracked robot is converted into an unconstrained motion planning problem by the method of penalty function, as follows: s.t.x k+1 = f k (x k , u k ), k = 0, 1,..., N - 1 x0 = x init .

5. The method of claim 4, wherein, In S402, the discrete robot motion model is as follows: where X = [x y θ κ] represents the state quantities; represents the control quantities; x represents the lateral coordinate of the robot's center of mass in the Cartesian coordinate system; y represents the longitudinal coordinate of the robot's center of mass in the Cartesian coordinate system; θ represents the yaw angle; and κ represents the curvature. represents the rate of change of curvature; v i dt represents the distance traveled by the robot between two time steps. The Taylor expansion is performed at the reference point to eliminate the nonlinear terms in the matrix to obtain the linearized model: The overall cost function is designed, including three parts of the reference point deviation cost function, the curvature cost function and the curvature rate cost function: The reference point deviation cost function is expressed as: wherein, are weight matrices; X i denotes the state of an intermediate point, denotes the state of an intermediate reference point; X N denotes the state of a target point, X goal denotes the state of a target reference point; The curvature cost function is expressed as: where w κ represents the curvature weight coefficient; The curvature rate cost function is expressed as: wherein w κ represents the curvature rate of change weight coefficient; The overall cost function is the weighted sum of the reference point deviation cost function, the curvature cost function and the curvature rate cost function; The inequality constraints are set, including the curvature amplitude constraint, the curvature rate amplitude constraint and the hybrid obstacle avoidance constraint based on the grid network; The curvature amplitude constraint is expressed as: where κ low and κ high denote the minimum and maximum of the curvature, respectively; The curvature rate amplitude constraint is expressed as: wherein and respectively represent the minimum and maximum of the rate of change of curvature; To ensure the self-consistency of the above two constraints, it is set that wherein κ i ∈ [κ low , κ high ]; the curvature κ is defined as the rate of change of the tangent angle θ with respect to the arc length s, i.e. The hybrid obstacle avoidance constraint based on the grid network is expressed as: where O p represents the occupied space of dynamic obstacles in uncertain environments in the grid network; q c represents the centroid coordinates of the circular envelope of the tracked robot Up to now, the entire unconstrained problem is constructed.

6. The method of claim 5, wherein, It also includes S5: based on the collision-free path, path tracking control is performed; specifically including: S501: construct a linear error kinematics model of an intelligent tracked robot and discretize it: where, denotes the error state variable; v l and v r are control inputs, representing the linear velocities of the left and right tracks, respectively; v lr and v rr are reference variables, representing the reference linear velocities of the left and right tracks, respectively; θ r denotes the centroid yaw angle of the intelligent tracked robot; B d denotes the center-to-center distance between the left and right tracks; I is the identity matrix; T is a constant, representing the discrete time step. S502: construct an objective function: Where Q is a state weight matrix and R is a control input weight matrix; S503: construct an optimal control rate, and adopt an LQR controller to complete path tracking; The discrete-time Riccati equation is expressed as: K t = (R + B T P t+1 B) -1 B T P t+1 A U e (t) = -K t X e (t) P t-1 = Q + A T P t A-A T P t B(R+B T P t+1 B) -1 B T P t A where K t is the state feedback gain matrix at time step t, A is the system state transition matrix, B is the system input matrix, P t+1 is the Riccati equation solution for the next time step, U e (t) is the control input at time t, X e (t) is the system state error at time t; By iteratively solving the discrete-time Riccati equation, the optimal control rate corresponding to the minimum cost value satisfying the constraint condition is obtained, and then the optimal control input is obtained to execute the tracked robot.

7. An improved CILQR based intelligent tracked robot motion planning control system for performing the above method, adopting the method of any one of claims 1-6, characterized in that, It includes: an intelligent tracked robot experimental platform hardware system and an intelligent tracked robot experimental platform software architecture; The intelligent tracked robot experimental platform hardware system includes a tracked mobile platform, a high-precision positioning system and an intelligent control host computer; The tracked mobile platform serves as an execution mechanism, carries all devices and executes a mobile task, including an ECU and a communication network, where the ECU represents an electronic control unit; two-way CAN buses are used for communication inside the ECU: one is connected to the motor controllers of the left and right wheels; the ECU sends speed and steering instructions to the motor controllers through this bus and receives state feedback from the motor, realizing precise motor control; the other is connected to the ECU and extends out, and is finally connected to the host computer through a conversion interface; this bus is used for the intelligent control host computer to issue high-level control instructions to the robot; The high-precision positioning system adopts a combined inertial navigation system. The system integrates an inertial measurement unit and a satellite navigation system, utilizes the complementary advantages of the two systems, adopts high-precision and low-drift inertial sensors, realizes accurate calculation and output of attitude, position and velocity, and obtains positioning information with a precision of centimeters; The intelligent control host computer calculates and decides according to the real-time position information fed back by the positioning system, generates corresponding motion control instructions, and thus realizes autonomous navigation and path tracking. The intelligent control host computer includes an industrial computer, a CAN-to-USB card and an RS485-to-USB cable. The industrial computer runs a robot operating system (ROS), a perception algorithm and a decision-making and planning algorithm. The software architecture of the intelligent tracked robot experimental platform adopts a layered design and specifically includes a positioning node, a planning node and a lower control node. The positioning node part is based on a multi-source data fusion framework, receives original data streams from the combined positioning system, further fuses data from other sensors, realizes centimeter-level positioning accuracy through an internal global latitude and longitude coordinate analysis function, and has a local positioning information acquisition function and a frequency adjustment function. The planning node part constructs a grid map based on a dynamic environment, uses a KD-Tree for efficient indexing and fast query of environmental information, uses an A* algorithm to search on the known grid map, generates a global rough path from a starting point to a target point, optimizes the global path, generates a safe and smooth reference path in combination with dynamic obstacle avoidance constraints, uses an improved CILQR algorithm to consider current dynamic obstacles and a vehicle model on the basis of the reference path, and calculates a directly executable optimal local path in real time. The control node part calculates optimal control output based on an intelligent tracked robot model through an LQR controller to minimize tracking error and control energy consumption according to a current position and a desired position of the robot. The lower control node part is responsible for communication between the ROS and the CAN and realizes lossless bidirectional conversion between the two. The bottom control node part realizes the function of controlling motor speed through CAN bus instructions through secondary development of an ECU.

Images