Indoor robot dynamic path planning method for mobile target pursuit

Abstract

The application relates to a dynamic path planning method for an indoor robot facing a moving target, which comprises the following steps: constructing a dynamic grid map according to current observation data and historical observation data collected by a multi-source sensor; updating a path planning terminal point according to the motion state of the moving target; generating a single-frame dynamic path according to the current position of the robot, the dynamic grid map and the path planning terminal point; updating the current position of the robot, the dynamic grid map and the path planning terminal point frame by frame to determine a final execution trajectory for following the moving target; and improving the map updating efficiency and the target tracking accuracy, and realizing safe, smooth and low-energy continuous following of the moving target in an indoor dynamic environment.

Description

Technical Field

[0001] This invention relates to the field of robotics applications, and in particular to a dynamic path planning method for indoor robots that are designed for following moving targets. Background Technology

[0002] With the continuous development of the smart home industry, indoor robots are widely used in scenarios such as home inspection, delivery of personal items, care for elderly people living at home, and following people. Stable and safe dynamic tracking of moving targets indoors is an indispensable core function of service robots. Current indoor robot path planning technologies generally use a fixed refresh rate for global updates of grid maps, target trajectory prediction often employs a single uniform motion model, and path optimization is primarily based on reducing the human-robot distance and minimizing motion energy consumption. The above solution has the following drawbacks: it does not divide the indoor space into static and dynamic areas; frequent updates of static areas consume redundant computing power; and the map updates of areas where people are active are delayed, which may cause collision hazards; a single prediction model cannot adapt to moving targets with multiple motion modes, and the long-term trajectory prediction error is large; the path optimization target is one-sided, ignoring the impact of the robot's position on sensor observations, which may lead to a decrease in the accuracy of target state estimation and problems such as target tracking loss and follow-up task interruption. Currently, no effective solutions have been proposed to address the problems of unreasonable allocation of computing power for map updates, poor adaptability of target trajectory prediction, and failure to consider tracking accuracy in path optimization. Summary of the Invention

[0003] The purpose of this invention is to address the shortcomings of existing technologies by providing a dynamic path planning method for indoor robots that follows moving targets, thereby solving the technical problems of unreasonable allocation of computing power for map updates, poor adaptability of target trajectory prediction, and failure to consider tracking accuracy in path optimization.

[0004] To achieve the above objectives, the technical solution adopted by the present invention is as follows: This invention provides a dynamic path planning method for indoor robots to follow moving targets, comprising: constructing a dynamic grid map based on current observation data and historical observation data collected by multi-source sensors; updating the path planning endpoint based on the obtained motion state of the moving target; generating a single-frame dynamic path based on the robot's current position, the dynamic grid map, and the path planning endpoint; updating the robot's current position, the dynamic grid map, and the path planning endpoint frame by frame to determine the final execution trajectory and follow the moving target.

[0005] Optionally, constructing a dynamic grid map based on current and historical observation data collected by multi-source sensors includes: analyzing the evolution patterns of obstacles in different areas of the indoor environment at different times based on current and historical observation data, dividing the indoor environment into static and dynamic areas; constructing a dynamic grid map based on the static and dynamic areas; wherein, the static areas are updated at a first frequency, and the dynamic areas are updated incrementally frame by frame at a second frequency greater than the first frequency; the incremental update includes: adding obstacle grid labels and releasing disappearing obstacle grids.

[0006] Further, optionally, the method also includes: dynamically adjusting the weight of each grid region in the dynamic grid map according to the time dimension, wherein the grid region includes: grids occupied by obstacles, grids that have not reached the safe avoidance distance, and passable grids that meet the safe distance; wherein the weights of grids occupied by obstacles and grids that have not reached the safe avoidance distance are set to a first value; the weights of passable grids that meet the safe distance are set to a second value; the safe avoidance distance is the minimum allowable measurement distance from the ranging sensor detection point to the obstacle region, and the safe avoidance distance is denoted as d. s The formula for calculating the safe avoidance distance includes: d s =d e +d r +d v +d o ; where d e d represents the sensor measurement error margin. r To compensate for the deviation between the sensor mounting point and the outermost edge of the robot body, the body contour compensation distance is d. v d represents the inertial gliding distance required for the robot to brake. o Reserved risk for the protruding corners of obstacles.

[0007] Optionally, the method further includes: using a region-specific incremental update of the dynamic grid map; wherein, using a region-specific incremental update of the dynamic grid map includes: periodically verifying the map status of the dynamic grid map for static areas at a first frequency, and correcting the corresponding grid information when facility changes are detected; periodically performing incremental refreshes for dynamic areas at a second frequency; the region-specific incremental update includes: marking the vacant grids corresponding to newly added obstacles as obstacle-prohibited grids, and restoring the obstacle grids corresponding to removed obstacles to passable grids; the passability weight of each grid is dynamically adjusted in sequence according to the change in the obstacle occupancy status.

[0008] Optionally, updating the path planning endpoint based on the obtained motion state of the moving target includes: matching the corresponding motion prediction model for the type of the moving target and predicting the target's motion trajectory; and using the end point of the target's motion trajectory as the path planning endpoint; wherein, the motion prediction model includes: a uniform motion model and a multi-trajectory probability candidate prediction model.

[0009] Further, optionally, matching the corresponding motion prediction model to the type of moving target, the prediction of the target motion trajectory includes: defining the moving target with a motion speed less than or equal to a preset value as a low-speed stable target, using a uniform motion model and setting a long prediction time domain to calculate the position at each time, and obtaining the target motion trajectory based on the position at each time. Uniform motion models include: in, and This represents the horizontal and vertical plane coordinates of a pedestrian in the indoor global coordinate system during the k-th control cycle; k = 1, 2, ..., K1, where K1 represents the total number of prediction steps. and This represents the initial velocity of the target along the x and y directions. The time step for a moving target that is moving at a low speed and maintaining a steady pace; Moving targets with a speed greater than a preset value are defined as highly maneuverable targets. A multi-trajectory probability candidate prediction model is used to calculate the target's trajectory based on the weighted sum of the trajectories formed by the superposition of l initial velocities and multiple sets of velocity disturbances. A short prediction time domain, which is less than the long prediction time domain, is combined to avoid long-term prediction bias. Multi-trajectory probabilistic candidate prediction models include: in, and This represents the horizontal and vertical plane coordinates of a pedestrian in the indoor global coordinate system during the k-th control cycle. This represents the weight of the l-th candidate trajectory. =1,0 , and This represents the l-th candidate trajectory; in, and Represented as: in, and Indicates the first The velocity perturbations in the x and y directions on the candidate trajectories. This represents the time step for a highly maneuverable reciprocating moving target.

[0010] Optionally, generating a single-frame dynamic path based on the robot's current position, dynamic grid map, and path planning endpoint includes: using the robot's linear velocity and angular velocity within the current control cycle as optimization variables, keeping the linear velocity and angular velocity constant within the prediction time window, and recursively generating candidate motion trajectories based on the robot's kinematic model; constructing a comprehensive trajectory evaluation function that integrates target tracking accuracy cost and motion energy consumption cost, and constructing a trajectory optimization problem by combining robot motion constraints and dynamic obstacle avoidance constraints; solving the trajectory optimization problem using a constraint pre-verification two-dimensional variable iterative optimization algorithm to obtain the optimal motion parameters and corresponding dynamic path for the current control cycle; and obtaining the single-frame dynamic path based on the optimal motion parameters and corresponding dynamic path for the current control cycle.

[0011] Furthermore, optionally, candidate motion trajectories can be recursively generated based on the robot's kinematics model, including: in, This represents the robot's pose state at the nth prediction time. and Let x and y represent the horizontal and vertical plane coordinates of the robot in the indoor global coordinate system at the nth prediction time. Let be the robot's heading angle in this coordinate system at the nth predicted time, i.e., the robot's current orientation; The predicted trajectory of the robot, derived recursively from the robot's motion model, includes: in, This represents the linear velocity of the robot in the current k-th control cycle. This represents the robot's angular velocity in the current k-th control cycle; n = 0, 1, ..., N-1; Indicates the discrete time step between adjacent control cycles; For the candidate motion trajectories, a comprehensive trajectory evaluation function is constructed for the k-th control cycle: in, This represents the cost of target tracking accuracy, used to guide the robot in choosing motion directions that improve the accuracy of target state estimation. This represents the energy cost of motion, used to reduce the robot's movement distance and energy consumption. Let $\mathbf$ represent the weighting coefficient of the cost in the evaluation function, and satisfy $\mathbf$. ; , , , , in, These represent the minimum and maximum values ​​of the linear velocity constraint, respectively. This represents the maximum rate of change of linear velocity during adjacent control cycles. These represent the minimum and maximum values ​​of the angular velocity constraint, respectively. This represents the maximum rate of change of angular velocity during adjacent control cycles; The trajectory optimization problem satisfies the robot's dynamic obstacle avoidance constraints: The comprehensive trajectory evaluation function includes: Based on the target observation information obtained from the robot's vision sensors, lidar, and inertial measurement unit, a target state observation model is established: in, Let m be the observation function of the m-th sensor. The target state for the (n-1)th predicted location includes the position and velocity in the xy direction , This represents the observation noise of the m-th sensor at the n-th predicted location, and satisfies the characteristics of Gaussian white noise. , Represents the noise covariance matrix; Construct the target state Fisher information matrix based on the multi-sensor fusion results: in, This represents the Jacobian matrix of the m-th sensor at the n-th predicted location. Represents the inverse of a matrix; Based on the target state estimation error , in, Let the trace of the matrix be the target tracking accuracy cost: Energy expenditure cost of exercise for: in, Represents the weighting coefficients, satisfying .

[0012] Optionally, the two-dimensional variable iterative optimization algorithm with constraint pre-verification includes: reading the optimal linear velocity and angular velocity from the previous planning cycle as the initial values ​​for this iteration; deducing the robot's predicted motion trajectory in the complete prediction time domain based on the current iteration's linear velocity and angular velocity; traversing trajectory points to verify the robot's indoor obstacle avoidance constraints; when a trajectory touches a dangerous collision area or exceeds a velocity threshold, it is determined as an infeasible trajectory and the corresponding set of velocity variables is discarded; participating in the total value calculation of feasible trajectories that have passed all constraint pre-verification; performing neighborhood perturbation search for the two optimization variables, linear velocity and angular velocity, generating multiple sets of candidate velocity variables, and repeatedly executing the deduction of the robot's prediction in the complete prediction time domain. The motion trajectory is used to calculate the total value of the feasible trajectory that has passed all constraints pre-validation. The total value of all feasible trajectories is compared, and the velocity variable with the smaller total cost is selected as the update amount for this iteration. It is determined that the decrease in the cost value of the comprehensive trajectory evaluation function after multiple consecutive iterations is less than or equal to the preset convergence threshold. If the convergence condition is not met, the robot's estimated motion trajectory in the complete prediction time domain is deduced and the iterative search continues. The iteration stops when the convergence condition is met. The optimal velocity command after convergence is output to drive the robot to run. After the grid map and moving target information are refreshed, a new round of iteration is started to complete the online optimization of the indoor path.

[0013] Optionally, determining the final execution trajectory and following the moving target includes: calculating path planning at fixed time intervals; the robot completes the following task when the constraints for determining whether it has completed the following task are satisfied; wherein, satisfying the constraints for determining whether the robot has completed the following task through trajectory optimization includes: in,( , ) represents the predicted position of the target in the k-th control cycle, and d represents the farthest distance at which the robot catches up with the target.

[0014] This invention employs the above technical solution, constructing a dynamic grid map based on current and historical observation data collected by multi-source sensors; updating the path planning endpoint based on the obtained motion state of the moving target; generating a single-frame dynamic path based on the robot's current position, the dynamic grid map, and the path planning endpoint; updating the robot's current position, the dynamic grid map, and the path planning endpoint frame by frame to determine the final execution trajectory and track the moving target. Compared with existing technologies, this invention has the following technical effects: improving map update efficiency and target tracking accuracy, and achieving safe, smooth, and low-energy continuous tracking of moving targets in indoor dynamic environments. Attached Figure Description

[0015] Figure 1This is a flowchart illustrating a dynamic path planning method for an indoor robot oriented towards moving target following, according to an embodiment of the present invention. Figure 2 This is a flowchart illustrating another dynamic path planning method for indoor robots oriented towards moving target following according to an embodiment of the present invention. Figure 3 This is a schematic diagram of a grid map in an indoor robot dynamic path planning method for following moving targets according to an embodiment of the present invention. Detailed Implementation

[0016] To make the objectives, technical solutions, and advantages of this application clearer, the application is described and illustrated below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application. All other embodiments obtained by those skilled in the art based on the embodiments provided in this application without inventive effort are within the scope of protection of this application.

[0017] Obviously, the accompanying drawings described below are merely some examples or embodiments of this application. Those skilled in the art can apply this application to other similar scenarios based on these drawings without any inventive effort. Furthermore, it is understood that although the efforts made in this development process may be complex and lengthy, for those skilled in the art related to the content disclosed in this application, any changes to design, manufacturing, or production based on the technical content disclosed in this application are merely conventional technical means and should not be construed as insufficient disclosure of the content of this application.

[0018] In this application, the reference to "embodiment" means that a specific feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places in the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment that is mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described in this application may be combined with other embodiments without conflict.

[0019] Unless otherwise defined, the technical or scientific terms used in this application shall have the ordinary meaning understood by one of ordinary skill in the art to which this application pertains. The terms “a,” “an,” “an,” “the,” and similar words used in this application do not indicate quantity limitation and may indicate singular or plural. The terms “comprising,” “including,” “having,” and any variations thereof used in this application are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that includes a series of steps or units (elements) is not limited to the listed steps or units, but may also include steps or units not listed, or may include other steps or units inherent to these processes, methods, products, or apparatus. The terms “connected,” “linked,” “coupled,” and similar words used in this application are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. The terms “multiple” / “several” used in this application refer to two or more. “And / or” describes the relationship between related objects, indicating that three relationships may exist; for example, “A and / or B” can indicate: A alone, A and B simultaneously, or B alone. The character " / " generally indicates that the preceding and following objects are in an "or" relationship. The terms "first," "second," and "third" used in this application are merely to distinguish similar objects and do not represent a specific ordering of the objects.

[0020] An illustrative embodiment of the present invention, such as Figure 1 As shown, Figure 1 This is a flowchart illustrating a dynamic path planning method for an indoor robot oriented towards moving target following according to an embodiment of the present invention. The dynamic path planning method for an indoor robot oriented towards moving target following provided in this application embodiment includes: Step S100: Construct a dynamic grid map based on the current observation data and historical observation data collected by the multi-source sensors; In this embodiment, the multi-source sensors include: a visual camera and a lidar; during operation, the current observation point cloud and image information of the indoor environment are collected in real time (i.e., the current observation data in this embodiment), while continuously storing multiple frames of historical sensor observation data (i.e., the historical observation data in this embodiment).

[0021] Optionally, the step S100 of constructing a dynamic grid map based on the current and historical observation data collected by the multi-source sensors includes: analyzing the evolution patterns of obstacles in different areas of the indoor space at different times based on the current and historical observation data, dividing the indoor space into static and dynamic areas; constructing a dynamic grid map based on the static and dynamic areas; wherein the static area is updated at a first frequency, and the dynamic area is updated incrementally frame by frame at a second frequency greater than the first frequency; the incremental update includes: adding obstacle grid markers and releasing disappearing obstacle grids.

[0022] Specifically, by statistically analyzing the temporal changes in the appearance and disappearance of obstacles in different home areas, the spatiotemporal evolution patterns of static facilities such as walls and fixed cabinets versus variable obstacles such as pedestrian movement and temporary storage of clutter are summarized. This allows for the construction of a binary dynamic grid map adapted to dynamic home scenarios, fully covering the entire space where the robot can move. Figure 2 This is a flowchart illustrating another dynamic path planning method for indoor robots oriented towards moving target following according to an embodiment of the present invention. Figure 3 This is a schematic diagram of a grid map in an indoor robot dynamic path planning method for moving target tracking according to an embodiment of the present invention. The dynamic grid map constructed by the indoor robot dynamic path planning method for moving target tracking provided in this application embodiment based on current observation data and historical observation data collected by multi-source sensors is as follows: like Figure 2 As shown, current and historical observation data from multiple sensors are acquired to construct a dynamic grid map, with weights updated in real time. Based on the temporal evolution differences of obstacles, the entire grid area is divided into static and dynamic regions, as shown below. Figure 3 As shown, the static area includes the grid area containing facilities such as walls, fixed wardrobes, and sofas that do not change position in a long time, while the dynamic area includes leisure activity areas such as aisles where people walk frequently and areas where children's toys are placed.

[0023] Optionally, the indoor robot dynamic path planning method for moving target following provided in this application embodiment further includes: using regional differentiated incremental updates of the dynamic grid map; wherein, using regional differentiated incremental updates of the dynamic grid map includes: periodically verifying the map status of the dynamic grid map for static areas at a first frequency, and correcting the corresponding grid information when facility changes are detected; periodically performing incremental updates for dynamic areas at a second frequency; regional differentiated incremental updates include: marking the vacant grids corresponding to newly added obstacles as obstacle-prohibited grids, and restoring the obstacle grids corresponding to removed obstacles as passable grids; the passability weight of each grid is dynamically adjusted in sequence according to the change of obstacle occupancy status.

[0024] Specifically, a differentiated incremental update strategy is implemented for two types of areas: the static area periodically verifies the map status at a low update frequency f1 (i.e., the first frequency period in this embodiment of the application), and only corrects the corresponding grid information when the sensor data detects abnormal changes in the facility. For example, f1 can be 0.01Hz to 1Hz. The dynamic region completes incremental refreshes cycle by cycle at an update frequency f2 (i.e., the second frequency cycle in this embodiment) that is higher than the frequency f1. For example, f2 can be from 10Hz to 50Hz. Incremental updates specifically include two types of operations: when a new obstacle is detected, the corresponding free grid is marked as an obstacle-free grid; after an existing obstacle disappears, the corresponding obstacle grid is released in a timely manner and restored to a passable state. This avoids redundant embedded computing power caused by high-frequency repeated updates of static areas, and ensures that changes in the environment of personnel activity areas can be perceived by the map in a timely manner.

[0025] Further, optionally, the indoor robot dynamic path planning method for moving target tracking provided in this application embodiment further includes: dynamically adjusting the weight of each grid region in the dynamic grid map according to the time dimension. The grid region includes: obstacle-occupied grids, grids that have not reached the safe avoidance distance, and passable grids that meet the safe distance. The weights of obstacle-occupied grids and grids that have not reached the safe avoidance distance are set to a first value; the weight of passable grids that meet the safe distance is set to a second value; the safe avoidance distance is the minimum allowable measurement distance from the ranging sensor detection point to the obstacle region, denoted as d. s The formula for calculating the safe avoidance distance includes: d s =d e +d r +d v +d o ; where d e d represents the sensor measurement error margin. r To compensate for the deviation between the sensor mounting point and the outermost edge of the robot body, the body contour compensation distance is d. v d represents the inertial gliding distance required for the robot to brake. o Reserved risk for the protruding corners of obstacles.

[0026] Specifically, such as Figure 3As shown, the passage weight of each grid is dynamically adjusted according to the time dimension: grids occupied by obstacles and grids that are not within the preset safe avoidance distance from obstacles are uniformly assigned a grid weight of 1 (i.e., the first value in this embodiment), which means that the robot is prohibited from entering the area; passable grids that meet the safe avoidance distance and are not obstructed by obstacles are assigned a weight of 0 (i.e., the second value in this embodiment); as time goes by, the weight values ​​of each grid are continuously updated according to the obstacle occupancy status, providing a basis for judgment of obstacle avoidance constraints in subsequent path planning, and realizing efficient and accurate time-series construction and maintenance of dynamic grid maps throughout the process.

[0027] In this embodiment, the safe avoidance distance is the minimum permissible measurement distance from the detection point of the ranging sensor to the obstacle area, and the safe avoidance distance is denoted as d. s The formulas for calculating safe avoidance distance include: d s =d e +d r +d v +d o ; Where, d e d represents the sensor measurement error margin. r To compensate for the deviation between the sensor mounting point and the outermost edge of the robot body, the body contour compensation distance is d. v d represents the inertial gliding distance required for the robot to brake. o Reserved risk for the protruding corners of obstacles.

[0028] The indoor robot dynamic path planning method for moving target following provided in this application divides the indoor grid map into static and dynamic regions and adapts to differentiated incremental update frequencies. Combined with a time-series dynamic grid weight adjustment strategy, it not only saves the computing power resources of the embedded robot, but also updates the dynamic obstacle information in the home in a timely manner, effectively improving the real-time performance and safety of environmental modeling.

[0029] Step S102: Update the destination of the path planning based on the obtained motion state of the moving target; Optionally, step S102, which updates the path planning endpoint based on the obtained motion state of the moving target, includes: matching the corresponding motion prediction model to the type of the moving target and predicting the target's motion trajectory; and using the end point of the target's motion trajectory as the path planning endpoint; wherein the motion prediction model includes: a uniform motion model and a multi-trajectory probability candidate prediction model.

[0030] Further, optionally, matching the corresponding motion prediction model to the type of moving target, the prediction of the target motion trajectory includes: defining the moving target with a motion speed less than or equal to a preset value as a low-speed stable target, using a uniform motion model and setting a long prediction time domain to calculate the position at each time, and obtaining the target motion trajectory based on the position at each time. Uniform motion models include: in, and This represents the horizontal and vertical plane coordinates of a pedestrian in the indoor global coordinate system during the k-th control cycle; k = 1, 2, ..., K1, where K1 represents the total number of prediction steps. and This represents the initial velocity of the target along the x and y directions. The time step for a moving target that is moving at a low speed and maintaining a steady pace; Moving targets with a speed greater than a preset value are defined as highly maneuverable targets. A multi-trajectory probability candidate prediction model is used to calculate the target's trajectory based on the weighted sum of the trajectories formed by the superposition of l initial velocities and multiple sets of velocity disturbances. A short prediction time domain, which is less than the long prediction time domain, is combined to avoid long-term prediction bias. Multi-trajectory probabilistic candidate prediction models include: in, and This represents the horizontal and vertical plane coordinates of a pedestrian in the indoor global coordinate system during the k-th control cycle. This represents the weight of the l-th candidate trajectory. =1,0 , and This represents the l-th candidate trajectory; in, and Represented as: in, and Indicates the first The velocity perturbations in the x and y directions on the candidate trajectories. This represents the time step for a highly maneuverable reciprocating moving target.

[0031] Specifically, such as Figure 2As shown, in acquiring a moving target, predicting its trajectory, and generating a dynamic path endpoint, a two-dimensional plane pedestrian motion state vector is selected, denoted as: in, and This represents the horizontal and vertical plane coordinates of a pedestrian in the indoor global coordinate system during the k-th control cycle. This represents the velocity components of the pedestrian along the x and y directions during the k-th control cycle.

[0032] The initial state q0 at the current starting time t0 is obtained by time-series fusion of observation data from devices such as lidar and vision cameras using Kalman filtering, and is used as the initial value of the pedestrian target trajectory endpoint.

[0033] The unit discrete time step of the control period is set to T, and the total prediction time is: T total =KT; Where K represents the total number of predicted steps, which is related to the time it takes for the robot to complete the target pursuit task; the value of the unit discrete time step T varies depending on the target's motion mode, with the corresponding time step for a target moving at low speed and with stable motion being... and the time step corresponding to a highly maneuverable reciprocating motion target It should meet ,For example, , .

[0034] Among them, the motion prediction models for different targets are: Specifically, it includes: Uniform motion prediction model: Applicable to pedestrians who are taking a leisurely stroll or walking slowly at home, whose speed and heading remain basically constant. Their position at each time step is: in, and This represents the horizontal and vertical plane coordinates of a pedestrian in the indoor global coordinate system during the k-th control cycle; k = 1, 2, ..., K1, where K1 represents the total number of prediction steps. and This represents the initial velocity of the target along the x and y directions. The time step for a moving target that is moving at a low speed and maintaining a steady pace; In a preferred example, a moving target with a speed less than or equal to 0.5 m / s (i.e., the preset value in the embodiments of this application) is defined as a low-speed stable target. The elderly and children moving slowly are low-speed stable targets. The low-speed stable target adopts a uniform motion model and sets a long prediction time domain.

[0035] Multi-trajectory probabilistic weighted candidate prediction model: Applicable to highly maneuverable targets such as those that frequently turn back and forth, pace back and forth, and randomly turn. The time-series position is obtained by weighting the motion trajectory formed by superimposing multiple sets of velocity perturbations based on l initial velocities, specifically: in, and This represents the horizontal and vertical plane coordinates of a pedestrian in the indoor global coordinate system during the k-th control cycle. This represents the weight of the l-th candidate trajectory. =1,0 , and This represents the l-th candidate trajectory; in, and Represented as: in, and Indicates the first The velocity perturbations in the x and y directions on the candidate trajectories. This represents the time step for a highly maneuverable reciprocating moving target.

[0036] In a preferred example, a moving target with a speed greater than 0.5 m / s (i.e., the preset value in the embodiments of this application) is defined as a highly maneuverable target. People walking quickly and pets running are highly maneuverable targets. A multi-trajectory probability candidate prediction model is used for highly maneuverable targets, with a shorter prediction time domain to avoid long-term prediction bias; the endpoint of the target's predicted trajectory is used as the endpoint of the path planning in this frame.

[0037] The indoor robot dynamic path planning method for moving target following provided in this application adaptively matches multiple trajectory prediction models and provides differentiated prediction time domains for different pedestrian motion characteristics. It can significantly reduce the target trajectory prediction error under different motion modes and dynamically generate the planning endpoint to adapt to the real-time motion state of the target.

[0038] Step S104: Generate a single-frame dynamic path based on the robot's current position, dynamic grid map, and path planning endpoint; Optionally, step S104, which generates a single-frame dynamic path based on the robot's current position, dynamic grid map, and path planning endpoint, includes: using the robot's linear velocity and angular velocity within the current control cycle as optimization variables, keeping the linear velocity and angular velocity constant within the prediction time window, and recursively generating candidate motion trajectories based on the robot's kinematic model; constructing a comprehensive trajectory evaluation function that integrates target tracking accuracy cost and motion energy consumption cost, and constructing a trajectory optimization problem by combining robot motion constraints and dynamic obstacle avoidance constraints; solving the trajectory optimization problem using a two-dimensional variable iterative optimization algorithm with constraint pre-verification to obtain the optimal motion parameters and corresponding dynamic path for the current control cycle; and obtaining the single-frame dynamic path based on the optimal motion parameters and corresponding dynamic path for the current control cycle.

[0039] Furthermore, optionally, candidate motion trajectories can be recursively generated based on the robot's kinematics model, including: in, This represents the robot's pose state at the nth prediction time. and Let x and y represent the horizontal and vertical plane coordinates of the robot in the indoor global coordinate system at the nth prediction time. Let be the robot's heading angle in this coordinate system at the nth predicted moment, used to determine the robot's current orientation; The predicted trajectory of the robot, derived recursively from the robot's motion model, includes: in, This represents the linear velocity of the robot in the current k-th control cycle. This represents the robot's angular velocity in the current k-th control cycle; n = 0, 1, ..., N-1; Indicates the discrete time step between adjacent control cycles; For the candidate motion trajectories, a comprehensive trajectory evaluation function is constructed for the k-th control cycle: in, This represents the cost of target tracking accuracy, used to guide the robot in choosing motion directions that improve the accuracy of target state estimation. This represents the energy cost of motion, used to reduce the robot's movement distance and energy consumption. Let $\mathbf$ represent the weighting coefficient of the cost in the evaluation function, and satisfy $\mathbf$. ; , , , , in, These represent the minimum and maximum values ​​of the linear velocity constraint, respectively. This represents the maximum rate of change of linear velocity during adjacent control cycles. These represent the minimum and maximum values ​​of the angular velocity constraint, respectively. This represents the maximum rate of change of angular velocity during adjacent control cycles; The trajectory optimization problem satisfies the robot's dynamic obstacle avoidance constraints: The comprehensive trajectory evaluation function includes: Based on the target observation information obtained from the robot's vision sensors, lidar, and inertial measurement unit, a target state observation model is established: in, Let m be the observation function of the m-th sensor. The target state for the (n-1)th predicted location includes the position and velocity in the xy direction , This represents the observation noise of the m-th sensor at the n-th predicted location, and satisfies the characteristics of Gaussian white noise. , Represents the noise covariance matrix; Construct the target state Fisher information matrix based on the multi-sensor fusion results: in, This represents the Jacobian matrix of the m-th sensor at the n-th predicted location. Represents the inverse of a matrix; Based on the target state estimation error , in, Let the trace of the matrix be the target tracking accuracy cost: Energy expenditure cost of exercise for: in, Represents the weighting coefficients, satisfying .

[0040] Specifically, such as Figure 2As shown, based on the robot's current position, and combining trajectory smoothing, motion parameter planning, and dynamic obstacle avoidance mechanisms, the single-frame path planning is solved using the robot's linear velocity and angular velocity within the current control cycle as optimization variables. Control variables are defined. : in, This represents the linear velocity of the robot in the current k-th control cycle. This represents the robot's angular velocity in the current k-th control cycle. Indicates transpose; the prediction time window T=N in the current k-th control cycle. While maintaining constant linear and angular velocities, candidate motion trajectories are generated based on the robot's kinematic model prediction. in, This represents the robot's pose state at the nth prediction time. and Let x and y represent the horizontal and vertical plane coordinates of the robot in the indoor global coordinate system at the nth prediction time. Let be the robot's heading angle in this coordinate system at the nth predicted moment, used to determine the robot's current orientation.

[0041] The robot's predicted trajectory is obtained recursively using the following kinematic model: in, This represents the linear velocity of the robot in the current k-th control cycle. This represents the robot's angular velocity in the current k-th control cycle; n = 0, 1, ..., N-1; Indicates the discrete time step between adjacent control cycles; To effectively improve the accuracy of target state estimation by multi-source sensors throughout the robot's pursuit of a moving target, while reducing the travel energy consumption generated by the robot's reciprocating motion, this embodiment constructs a comprehensive multi-objective evaluation function that integrates the cost of target tracking perception accuracy and the cost of robot motion energy consumption. Based on this evaluation function, a trajectory optimization problem with motion constraints and obstacle avoidance constraints is established. The specific expression of the comprehensive objective function is as follows: in, Let $\mathbf$ represent the weighting coefficient of the cost in the evaluation function, and satisfy $\mathbf$. , This represents the cost of target tracking accuracy, used to guide the robot in choosing motion directions that improve the accuracy of target state estimation. This represents the energy cost of motion, used to reduce the robot's movement distance and energy consumption.

[0042] Specifically, the cost of target tracking accuracy Based on the target observation information obtained by the robot's vision sensors, LiDAR, and inertial measurement unit, a target state observation model is established: in, Let m be the observation function of the m-th sensor. The target state for the (n-1)th predicted location includes the position and velocity in the xy direction , This represents the observation noise of the m-th sensor at the n-th predicted location, and satisfies the characteristics of Gaussian white noise. , This represents the noise covariance matrix.

[0043] Construct the target state Fisher information matrix based on the multi-sensor fusion results: in, This represents the Jacobian matrix of the m-th sensor at the n-th predicted location. Represents the inverse of a matrix; Error estimated based on target state: , in, Let the trace of the matrix be the target tracking accuracy cost: Energy expenditure cost of exercise for: in, Represents the weighting coefficients, satisfying .

[0044] The total mileage cost term within the current control cycle is used to constrain the total length of the trajectory generated by the planning to be as short as possible, so as to avoid the robot detouring on redundant routes. As a linear speed constraint term, it suppresses the robot's high-speed sprinting, which can reduce the power consumption of the motor when running at a constant speed and avoid the risk of human-robot collision caused by excessive speed. The angular velocity constraint reduces the steering energy consumption caused by frequent reversals and deflections of the chassis differential wheels, while effectively improving the smoothness of robot movement.

[0045] In this embodiment, the dynamic trajectory planning optimization problem satisfies the robot motion constraints: , , , , in, These represent the minimum and maximum values ​​of the linear velocity constraint, respectively. This represents the maximum rate of change of linear velocity during adjacent control cycles. These represent the minimum and maximum values ​​of the angular velocity constraint, respectively. This represents the maximum rate of change of angular velocity during adjacent control cycles; Constraints Represents the linear velocity of the kth control cycle. The upper and lower bound constraints limit the robot's forward speed to always stay within the rated safe range, ensuring that the speed does not exceed the motor load limit or stop moving. Constraints It indicates the limit on the increase or decrease of forward speed between each frame, avoiding sudden acceleration and braking, improving motion smoothness, and reducing mechanical shock and energy consumption; Constraints Represents the angular velocity in the kth control cycle. The upper and lower bound constraints restrict the robot's turning speed, preventing it from spinning rapidly in place and adapting to the walking restrictions in small home spaces. Constraints It represents the limit on the change in turning rate between adjacent cycles, preventing sudden and large-scale changes in course, ensuring smooth and stable movement, and meeting the human-machine comfort needs of indoor companionship scenarios.

[0046] To ensure the robot's safety throughout the entire tracking process, dynamic obstacle avoidance constraints are introduced to optimize the problem: in, For the binary indicator function of dynamic grid map access determination, if the robot's position... Landing within the obstacle area or the prohibited area within the safe avoidance distance, 1 means that the location is impassable; If the location is in a passable area with no obstacles and a safe distance, then .

[0047] This constraint requires that every discrete sub-time point on the predicted trajectory meets the grid passability conditions, and that no part of the entire trajectory may intrude into obstacles or safety restricted areas. Unlike the traditional obstacle avoidance method that only verifies the first and last points of the trajectory, this method can effectively avoid the mid-journey collision risk caused by large strides across the grid, and achieve all-round safety verification of the entire motion trajectory, adapting to the obstacle avoidance needs in complex dynamic home environments.

[0048] Considering the multi-dimensional cost optimization objective, target prediction state, robot motion constraints, dynamic obstacle avoidance constraints, and following distance determination constraints, this embodiment ultimately constructs a nonlinear minimization optimization problem with complete inequality constraints, the complete form of which is as follows: , , , , , , , , , , Among them, constraints Specifically, it refers to predicting the target's trajectory in step S2.

[0049] Optionally, the two-dimensional variable iterative optimization algorithm with constraint pre-verification includes: reading the optimal linear velocity and angular velocity from the previous planning cycle as the initial values ​​for this iteration; deducing the robot's predicted motion trajectory in the complete prediction time domain based on the current iteration's linear velocity and angular velocity; traversing trajectory points to verify the robot's indoor obstacle avoidance constraints; when a trajectory touches a dangerous collision area or exceeds a velocity threshold, it is determined as an infeasible trajectory and the corresponding set of velocity variables is discarded; participating in the total value calculation of feasible trajectories that have passed all constraint pre-verification; performing neighborhood perturbation search for the two optimization variables, linear velocity and angular velocity, generating multiple sets of candidate velocity variables, and repeatedly executing the deduction of the robot's prediction in the complete prediction time domain. The motion trajectory is used to calculate the total value of the feasible trajectory that has passed all constraints pre-validation. The total value of all feasible trajectories is compared, and the velocity variable with the smaller total cost is selected as the update amount for this iteration. It is determined that the decrease in the cost value of the comprehensive trajectory evaluation function after multiple consecutive iterations is less than or equal to the preset convergence threshold. If the convergence condition is not met, the robot's estimated motion trajectory in the complete prediction time domain is deduced and the iterative search continues. The iteration stops when the convergence condition is met. The optimal velocity command after convergence is output to drive the robot to run. After the grid map and moving target information are refreshed, a new round of iteration is started to complete the online optimization of the indoor path.

[0050] Specifically, the two-dimensional variable iterative optimization algorithm with constraint pre-verification provided in this application embodiment is as follows: Step S1, Iterative Initialization: Read the optimal linear velocity and optimal angular velocity obtained from the previous planning cycle, and use them as the initial values ​​of the initial iteration variables for this iteration optimization; Step S2, Trajectory Deduction: Based on the linear velocity and angular velocity of the current iteration, deduce the predicted motion trajectory of the robot in the complete prediction time domain; Step S3, Constraint Pre-verification: Traverse all trajectory points of the predicted trajectory and verify the robot's motion constraints and dynamic obstacle avoidance constraints in turn; if any point on the trajectory touches the collision-prone area of ​​an obstacle or exceeds the threshold of the motion parameters, the trajectory is determined to be an infeasible trajectory and the set of two-dimensional variables is directly discarded. Step S4, Feasible trajectory cost calculation: Only feasible trajectories that have passed all constraint pre-verifications participate in the total cost calculation; Step S5, Two-dimensional variable update: Perform neighborhood perturbation search for the two optimization variables, linear velocity and angular velocity, to generate multiple sets of candidate velocity variables. Repeat steps S2 to S4, compare the total cost value corresponding to all feasible trajectories, and select the two-dimensional velocity variable with the smaller total cost as the update amount for this iteration. Step S6, Iteration Termination Judgment: Determine if the decrease in the cost value of the comprehensive objective function after multiple consecutive iterations is less than or equal to a preset convergence threshold; if the convergence condition is not met, return to step S2 to continue the iterative search; if the convergence condition is met, stop the iteration; specifically, the convergence condition is defined as: in, This represents the cost of the comprehensive objective function after the (t+1)th iteration. Let (t) represent the cost of the comprehensive objective function after the (t)th iteration. This indicates a preset convergence threshold, for example... .

[0051] Step S7, Command Output and Cyclic Loop: After the output converges, the optimal linear velocity and angular velocity are sent to the robot motion execution unit; after waiting for the next frame of grid map and moving target prediction information to be updated, a new round of cyclic iteration optimization is started to realize online optimization of dynamic paths in indoor scenes.

[0052] The dynamic path planning method for indoor robots oriented to follow moving targets provided in this application constructs a multi-objective optimization function that integrates target perception and tracking accuracy and motion energy consumption, taking into account both pursuit efficiency and sensor tracking quality, and uses a two-dimensional variable iterative optimization algorithm with constraint pre-verification to solve the dynamic path of a single frame. Step S106: Update the robot's current position, dynamic grid map, and path planning endpoint frame by frame to determine the final execution trajectory and follow the moving target.

[0053] Optionally, in step S106, determining the final execution trajectory and following the moving target includes: calculating path planning at fixed intervals; when the robot completes the following task and the constraint is satisfied, it indicates that the following task is completed; wherein, satisfying the constraint for determining whether the robot has completed the following task through trajectory optimization includes: in,( , ) represents the predicted position of the target in the k-th control cycle, and d represents the farthest distance at which the robot catches up with the target.

[0054] Specifically, such as Figure 2 As shown, the grid map, robot position, and target endpoint are updated cyclically to determine the final execution trajectory, achieving moving target tracking. A unified control cycle duration is preset. The robot periodically initiates a complete closed-loop path planning operation at a constant time interval. Within each cycle, the robot first collects the latest environmental data using multi-source sensors and incrementally updates the occupancy status of each area in the dynamic grid map. Then, based on the updated environmental information, it re-completes the motion state filtering estimation of the moving target, combines the pedestrian motion characteristics with an adaptive matching trajectory prediction model, and updates the target endpoint for the next stage of path planning. Subsequently, it constructs a multi-constraint optimization problem that integrates tracking accuracy and motion energy consumption, and solves the optimal linear velocity and angular velocity control quantities through a two-dimensional variable iterative optimization algorithm with constraint pre-verification. These values ​​are then sent to the robot control system to drive the robot to travel a short distance.

[0055] After each planning and solving process, the system verifies the following task decision constraints: calculating the Euclidean distance between the robot's end-point position and the predicted target position. in,( , ) represents the predicted position of the target in the k-th control cycle, and d represents the farthest distance at which the robot catches up with the target. If the distance between the robot and the human remains less than the preset safe following threshold d, it is determined that the robot has stably reached the following range and the following task has been accomplished, and the loop iteration can be terminated; if the constraint conditions are not met, the robot waits for the next cycle to arrive, and restarts the entire process of map updating, target prediction, and trajectory optimization, continuously correcting the motion trajectory until the indoor pedestrian dynamic following task is successfully completed.

[0056] This application provides a dynamic path planning method for indoor robots that track moving targets. By fusing observation information from multiple sensors, a grid environment model with spatiotemporal dynamic update capabilities is constructed. A target motion prediction model is established based on the motion characteristics of the moving target to achieve real-time prediction of the moving target's position and motion trend. Furthermore, based on the robot's current position, dynamic environment information, and the predicted target trajectory, a dynamic trajectory planning optimization model is constructed that integrates target tracking accuracy, motion distance cost, trajectory smoothness, and dynamic obstacle avoidance constraints. By optimizing the robot's motion parameters in real time, the optimal motion trajectory within the current control cycle is obtained, enabling the robot to quickly, safely, and with low energy consumption continuously track moving targets in complex indoor dynamic environments.

[0057] This invention employs the above technical solution, constructing a dynamic grid map based on current and historical observation data collected by multi-source sensors; updating the path planning endpoint based on the obtained motion state of the moving target; generating a single-frame dynamic path based on the robot's current position, the dynamic grid map, and the path planning endpoint; updating the robot's current position, the dynamic grid map, and the path planning endpoint frame by frame to determine the final execution trajectory and track the moving target. Compared with existing technologies, this invention has the following technical effects: improving map update efficiency and target tracking accuracy, and achieving safe, smooth, and low-energy continuous tracking of moving targets in indoor dynamic environments.

[0058] The above are merely preferred embodiments of the present invention and are not intended to limit the implementation methods and protection scope of the present invention. Those skilled in the art should recognize that any equivalent substitutions and obvious changes made based on the description and illustrations of the present invention should be included within the protection scope of the present invention.

Claims

1. A dynamic path planning method for indoor robots oriented towards moving target following, characterized in that, include: A dynamic grid map is constructed based on current and historical observation data collected by multiple sensors; Update the destination of the path planning based on the obtained motion state of the moving target; A single-frame dynamic path is generated based on the robot's current position, the dynamic grid map, and the path planning endpoint; The robot's current position, the dynamic grid map, and the path planning endpoint are updated frame by frame to determine the final execution trajectory and to follow the moving target.

2. The indoor robot dynamic path planning method for moving target following according to claim 1, characterized in that, The construction of a dynamic raster map based on current and historical observation data collected by multi-source sensors includes: Based on the current observation data and the historical observation data, the evolution patterns of obstacles in various indoor areas at different times are analyzed, and the indoor areas are divided into static areas and dynamic areas. The dynamic raster map is constructed based on the static region and the dynamic region; The static region is updated at a first frequency, and the dynamic region is updated incrementally frame by frame at a second frequency greater than the first frequency; the incremental update includes adding obstacle grid labels and releasing disappearing obstacle grids.

3. The indoor robot dynamic path planning method for moving target following according to claim 2, characterized in that, The method further includes: The weight of each grid region in the dynamic grid map is dynamically adjusted according to the time dimension. The grid region includes: grids occupied by obstacles, grids that have not reached the safe avoidance distance, and passable grids that meet the safe distance. The weights of the grid cells occupied by the obstacles and the grid cells that do not reach the safe avoidance distance are set to a first value; the weights of the passable grid cells that meet the safe distance are set to a second value. The safe avoidance distance is the minimum permissible measurement distance from the detection point of the ranging sensor to the obstacle area, and the safe avoidance distance is denoted as d. s The formula for calculating the safe avoidance distance includes: d s =d e +d r +d v +d o ; Where, d e d represents the sensor measurement error margin. r To compensate for the deviation between the sensor mounting point and the outermost edge of the robot body, the body contour compensation distance is d. v d represents the inertial gliding distance required for the robot to brake. o Reserved risk for the protruding corners of obstacles.

4. The indoor robot dynamic path planning method for moving target following according to claim 2, characterized in that, The method further includes: The dynamic raster map is updated incrementally with regional differences. The method of using differentiated incremental updates for the dynamic grid map by region includes: periodically verifying the map status of the dynamic grid map for the static area at the first frequency, and correcting the corresponding grid information when facility changes are detected; periodically performing incremental updates for the dynamic area at the second frequency; the differentiated incremental updates by region include: marking the empty grid corresponding to newly added obstacles as obstacle-prohibited grids, and restoring the obstacle grids corresponding to removed obstacles as passable grids; the passability weight of each grid is dynamically adjusted in sequence according to the change in the obstacle occupancy status.

5. The indoor robot dynamic path planning method for moving target following according to claim 1, characterized in that, The step of updating the path planning endpoint based on the obtained motion state of the moving target includes: Match the corresponding motion prediction model to the type of the moving target and predict the target's trajectory; The endpoint of the target motion trajectory is taken as the endpoint of the path planning; The motion prediction model includes a uniform motion model and a multi-trajectory probability candidate prediction model.

6. The indoor robot dynamic path planning method for moving target following according to claim 5, characterized in that, The motion prediction model, which matches the type of the moving target, predicts the target's trajectory, including: The moving target with a speed less than or equal to a preset value is defined as a low-speed stable target. The uniform motion model is used and a long prediction time domain is set to calculate the position at each time. The target's motion trajectory is obtained based on the position at each time. The uniform motion model includes: in, and This represents the horizontal and vertical plane coordinates of a pedestrian in the indoor global coordinate system during the k-th control cycle; k = 1, 2, ..., K1, where K1 represents the total number of prediction steps. and This represents the initial velocity of the target along the x and y directions. The time step corresponding to the moving target walking at a low speed and smoothly; The moving target with a speed greater than the preset value is defined as a highly maneuverable target. The multi-trajectory probability candidate prediction model is used to calculate the target's trajectory based on the weighted sum of the trajectories formed by the superposition of l initial velocities and multiple sets of velocity disturbances. A short prediction time domain smaller than the long prediction time domain is combined to avoid long-term prediction bias. The multi-trajectory probability candidate prediction model includes: in, and This represents the horizontal and vertical plane coordinates of a pedestrian in the indoor global coordinate system during the k-th control cycle. This represents the weight of the l-th candidate trajectory. =1,0 , and This represents the l-th candidate trajectory; in, and Represented as: in, and Indicates the first The velocity perturbations in the x and y directions on the candidate trajectories. The time step is the time step corresponding to the highly maneuverable reciprocating target.

7. The indoor robot dynamic path planning method for moving target following according to claim 1, characterized in that, The process of generating a single-frame dynamic path based on the robot's current position, the dynamic grid map, and the path planning endpoint includes: Using the robot's linear velocity and angular velocity within the current control cycle as optimization variables, and keeping the linear velocity and angular velocity constant within the prediction time window, candidate motion trajectories are recursively generated based on the robot's kinematic model. A comprehensive trajectory evaluation function is constructed that integrates the cost of target tracking accuracy and the cost of motion energy consumption. The trajectory optimization problem is then constructed by combining robot motion constraints and dynamic obstacle avoidance constraints. The trajectory optimization problem is solved by a two-dimensional variable iterative optimization algorithm with constraint pre-verification to obtain the optimal motion parameters and corresponding dynamic path for the current control cycle. The single-frame dynamic path is obtained based on the optimal motion parameters and corresponding dynamic path of the current control cycle.

8. The indoor robot dynamic path planning method for moving target following according to claim 7, characterized in that, The recursive generation of candidate motion trajectories based on the robot kinematics model includes: in, This represents the robot's pose state at the nth prediction time. and Let x and y represent the horizontal and vertical plane coordinates of the robot in the indoor global coordinate system at the nth prediction time. Let be the robot's heading angle in this coordinate system at the nth predicted time, i.e., the robot's current orientation; The predicted trajectory of the robot, derived recursively from the robot motion model, includes: in, This represents the linear velocity of the robot in the current k-th control cycle. This represents the robot's angular velocity in the current k-th control cycle; n = 0, 1, ..., N-1; Indicates the discrete time step between adjacent control cycles; For the candidate motion trajectories, a comprehensive trajectory evaluation function is constructed for the k-th control cycle: in, This represents the cost of target tracking accuracy, used to guide the robot in choosing motion directions that improve the accuracy of target state estimation. This represents the energy cost of motion, used to reduce the robot's movement distance and energy consumption. Let $\mathbf$ represent the weighting coefficient of the cost in the evaluation function, and satisfy $\mathbf$. ; The trajectory optimization problem satisfies the robot motion constraints: ， ， ， ， in, These represent the minimum and maximum values ​​of the linear velocity constraint, respectively. This represents the maximum rate of change of linear velocity during adjacent control cycles. These represent the minimum and maximum values ​​of the angular velocity constraint, respectively. This represents the maximum rate of change of angular velocity during adjacent control cycles; The trajectory optimization problem satisfies the robot's dynamic obstacle avoidance constraints: The comprehensive trajectory evaluation function includes: Based on the target observation information obtained from the robot's vision sensors, lidar, and inertial measurement unit, a target state observation model is established: in, Let m be the observation function of the m-th sensor. The target state for the (n-1)th predicted location includes the position and velocity in the xy direction , This represents the observation noise of the m-th sensor at the n-th predicted location, and satisfies the characteristics of Gaussian white noise. , Represents the noise covariance matrix; Construct the target state Fisher information matrix based on the multi-sensor fusion results: in, This represents the Jacobian matrix of the m-th sensor at the n-th predicted location. Represents the inverse of a matrix; Based on the target state estimation error , in, Let the trace of the matrix be the target tracking accuracy cost: Energy expenditure cost of exercise for: in, Represents the weighting coefficients, satisfying .

9. The indoor robot dynamic path planning method for moving target following according to claim 7, characterized in that, The two-dimensional variable iterative optimization algorithm with constraint pre-verification includes: Read the optimal linear velocity and angular velocity from the previous planning cycle as the initial values ​​for this iteration; Based on the current iteration of linear velocity and angular velocity, the complete prediction of the robot's motion trajectory in the time domain is derived; Traverse the trajectory points and verify the robot's indoor obstacle avoidance constraints. If the trajectory touches a dangerous collision area or exceeds the speed threshold, it is determined to be an infeasible trajectory and the corresponding group of speed variables is discarded. Feasible trajectories that have undergone full constraint pre-verification are used in the total agency value calculation; For the two optimization variables of linear velocity and angular velocity, a neighborhood perturbation search is performed to generate multiple sets of candidate velocity variables. The prediction of the robot's motion trajectory in the time domain is repeatedly executed until the feasible trajectory that has passed all constraints pre-verification is used to participate in the total value calculation. The total value corresponding to all feasible trajectories is compared, and the velocity variable with the smaller total cost is selected as the update amount for this iteration. If the decrease in the cost value of the comprehensive trajectory evaluation function after multiple consecutive iterations is less than or equal to the preset convergence threshold, and the convergence condition is not met, return to the simulation of the robot's predicted motion trajectory in the complete prediction time domain and continue iterative search; if the convergence condition is met, stop iterating. The optimal speed command after convergence is output to drive the robot to run; after the grid map and moving target information are refreshed, a new round of iteration is started to complete the online optimization of the indoor path.

10. The indoor robot dynamic path planning method for moving target following according to claim 7, characterized in that, The determination of the final execution trajectory and the tracking of the moving target include: Calculate path planning at fixed intervals; When the robot completes the following task determination constraint, it indicates that the following task has been completed. The constraints for determining whether the robot has completed the following task, which are satisfied through trajectory optimization problems, include: in,( , ) represents the predicted position of the target in the k-th control cycle, and d represents the farthest distance at which the robot catches up with the target.

Images