Movement planning method, system and equipment of disabled-helping robot, storage medium and computer program

Abstract

The invention discloses a movement planning method, system and device for a disabled assisting robot, a storage medium and a computer program. The method comprises the steps that S1, a language instruction is received and processed; s2, sensing an environment and constructing a map; s3, analyzing data and planning a path; and S4, state estimation and dynamic obstacle avoidance. Through voice input, a laser SLAM technology and a grid map optimization method, a high-precision two-dimensional map of the disabled assisting robot is constructed, and the accuracy of navigation planning is remarkably improved; and a more optimized path planning method is adopted, so that the path planning efficiency is higher and more stable.

Description

Technical Field

[0001] The present invention belongs to the technical field related to robot control, and particularly relates to a method, system, device, storage medium and computer program for the movement planning of a disabled-assistance robot. Background Art

[0002] In the context of the rapid social development in recent years, with the rapid changes in science and technology and the change of the population structure, the quality of life and self-care ability of the disabled group have attracted increasing attention. Especially for those who are mobility-impaired due to accidents, diseases or congenital conditions, how to provide efficient and intelligent auxiliary devices has become a research hotspot. As a new type of auxiliary technology, the disabled-assistance robot provides a more convenient and safe movement solution for the disabled by combining advanced sensors, artificial intelligence algorithms and precise mechanical structures.

[0003] Although the movement planning system of the disabled-assistance robot has shown great potential in improving the quality of life of the disabled, it still faces a series of challenges and problems in the actual application process. Fu Xiang, Ruan Qianfeng, Zhou Junyu et al. proposed a path planning method, system, device and storage medium based on RRT (Fu Xiang, Ruan Qianfeng, Zhou Junyu, Wan Jiaqi, Tang Maojia, Li Fuqiang, Path planning method, system, device and storage medium based on RRT, CN118730151A, 2024-10-1). In a highly dynamic or complex environment, the real-time performance and accuracy of path planning may still face challenges. Xu Zhengbo, Luo Hongsi, Chen Jie proposed a method and system for intelligent planning of the movement path of a robot (Xu Zhengbo, Luo Hongsi, Chen Jie, A method and system for intelligent planning of the movement path of a robot, CN118424297A, 2024-8-2). The optimal path is evaluated through a path planning index. However, when this system faces a complex and changeable environment, if the processing speed is not fast enough, it may lead to delays or mistakes in path planning, affecting the navigation efficiency and safety of the robot. Summary of the Invention

[0004] To solve the above technical problems existing in the existing movement planning system of the disabled-assistance robot, the present invention provides a method and system for the movement planning of a disabled-assistance robot. The present invention integrates voice recognition, laser SLAM (Simultaneous Localization and Mapping) navigation, ultrasonic obstacle avoidance and speed odometer technology; after the user's voice command is input, the system uses SLAM to perceive the environment in real time and build a map, and accurately plans the navigation path; during movement, the system continuously collects state and position information, and the ultrasonic sensor detects and avoids dynamic obstacles in real time to ensure stable and accurate movement in a complex environment.

[0005] The technical problems adopted by the present invention are:

[0006] The first aspect of the present invention relates to a method for mobile planning of a disabled - assistance robot, including the following steps:

[0007] S1: Receive and process language instructions;

[0008] The system receives the user's voice instruction, creates an untrained voice model, converts the training data set into a feature sequence through a pre - processing module, adds position encoding, extracts information through an encoder - decoder structure, optimizes the model in combination with a loss function, and uses the trained model to convert the voice instruction into a text instruction for output;

[0009] S2: Sense the environment and construct a map;

[0010] Use a lidar to obtain 3D point clouds, filter and denoise them, project them onto a 2D plane, divide the grid and mark the status to construct an initial map, correct the error through loop detection and optimization, and output the final map;

[0011] S3: Analyze data and plan a path;

[0012] Extract free space based on the grid map and analyze the connectivity, divide the free space into convex polygons, and construct a topological map; Initialize the search tree, set the starting point and the ending point according to the instruction; Expand the tree using a random sampling and connection strategy, optimize the path nodes, and then adjust the path iteratively; Backtrack to generate the optimal path and output the path;

[0013] S4: Estimate the state and perform dynamic obstacle avoidance;

[0014] Fuse multi - sensor information through a filtering algorithm, update the robot's pose in real - time to ensure that the robot travels along the path; When encountering a dynamic obstacle, first determine the safe speed range according to the robot's kinematic constraints and the distance to the obstacle, and then predict the motion trajectory in the next few seconds based on the kinematic model; Comprehensively consider the azimuth deviation, the distance to the obstacle, and the priority of the moving speed, and select the optimal local obstacle - avoidance path; Output the adjusted local path and control instructions. Further, in step S1, receiving and processing language instructions specifically includes:

[0015] S11: Wait for the patient to take a seat, the system enters the standby state, and is ready to receive the user's instruction;

[0016] S12: The patient issues a voice instruction through the voice module, inputs instructions to the disabled - assistance robot through speech, that is, transmits the voice signal to the CPU processor;

[0017] S14: Construct an untrained speech recognition model. The pre - processing module uses a predefined relevant voice instruction library as the training data set and processes it into an acoustic feature vector that can be recognized by the encoder and converts the real - text label corresponding to the voice signal into a text embedding sequence;

[0018] S15: The sequences obtained after being processed by the preprocessing module are all added to their respective position encodings;

[0019] S16: Use the encoder-decoder structure to train the model to accurately convert the acoustic features into the target of text instructions;

[0020] S17: Use the temporal connection classification algorithm to assist in training;

[0021] When calculating the overall loss evaluation, fuse two different types of loss values, and at the same time fuse the temporal loss and the attention loss to calculate the total loss. Optimize the model parameters through backpropagation, and continuously train the model to improve the conversion accuracy;

[0022] S18: Use the trained model to convert the voice instructions into text control instructions and output the instructions.

[0023] Further, in step S2, perceiving the environment and building the map specifically includes:

[0024] S21: Collect data;

[0025] Use the lidar sensor to scan the surrounding environment at a set frequency range to obtain detailed three-dimensional point cloud data;

[0026] S22: Preprocess the data;

[0027] Filter and denoise the collected three-dimensional point cloud data.

[0028] S23: Perform the point cloud projection operation;

[0029] Accurately project the preprocessed three-dimensional point cloud data onto a two-dimensional plane to generate the corresponding two-dimensional point cloud map;

[0030] S24: Build a grid map;

[0031] Reasonably divide the two-dimensional plane into a series of grids, and each grid represents a specific position; according to the generated two-dimensional point cloud map, accurately determine the state of each grid, that is, free, occupied or unknown;

[0032] S25: Implement the map optimization step, and comprehensively optimize the map through loop detection and backend optimization to improve its accuracy and consistency, and ensure that the generated 2D map has high practical value.

[0033] Further, in step S3, analyzing the data and planning the path specifically includes:

[0034] S31: Obtain the free space X of the agent free ;

[0035] According to the characteristics of the grid map, that is, the map is divided into small cells, each small cell represents a specific area size and is marked as an obstacle or free space; traverse the map, traverse every cell on the entire map. Then identify the free space. If a cell is not an obstacle, mark it as free space; this can be achieved by checking the value of the cell. Finally, perform connectivity analysis to determine which free space areas are connected to each other;

[0036] S32: Perform convex segmentation on X free to obtain a list of polygons;

[0037] S33: Create a graph structure to store topological information, add nodes and edges, and construct the topological graph X topo ;

[0038] S34: First, perform semantic parsing, extract the key parameters in the instruction, match the predefined task template, and then perform coordinate mapping. According to the pre-stored environmental map, convert the semantic target point into two-dimensional coordinates (x, y) in the map; if the target point is temporarily adjusted due to environmental changes, the system corrects the coordinates in real time through laser SLAM; determine the starting point X init and the target point X goal , and initialize the tree T with X init as the root; then set up three empty queues ξold, Qr, Qg. The queue ξold is used to record the homotopy path classes that have been searched, while Qr is used to store the re-wiring process, and Qg is used to store the randomly sampled points close to the target point

[0039] After setting up the basic variables, perform pruning on X topo using X init and X goal as the basis to remove the parts unrelated to the path between the two points; input the topological graph X topo , the starting point X init and the target point X goal . First, make a copy X′ topo of the topological graph X topo , and then use the function Γ to map the starting point X init and the target point X goal to the corresponding nodes in the topological graph; where the function Γ is a mapping that converts geometric coordinates into a sequence of nodes in the topological graph;

[0040] S36: Then traverse all the simply connected nodes in X topo , that is, the nodes with only one neighbor, set up a temporary variable X temp , and assign the current simply connected node x to the temporary variable X temp ;

[0041] S37: Then enter a loop until Xtemp until it is no longer a simply - connected node; if X temp is not the starting point or the target point, then remove it from X′ topo ; otherwise, exit the loop, that is, if X temp is the starting point or the target point, then do not remove it; update X temp to its neighbor nodes and continue to check whether the new X temp is still a simply - connected node; finally, return the copy X′ topo ;

[0042] S38: Start a loop that performs multiple iterations to expand the tree structure; in the loop, first sample on the secant line of X topo using formula (6) to generate a random sample point X rand ; this step uses the roulette wheel selection process to select the secant line and randomly generate a point X rand on the selected secant line;

[0043] S39: Find the set of nearby nodes Q near using an efficient data structure to find a set of nodes closer to the random X rand and find the nearest node X closest in this set of nodes,

[0044] S311: Use the Euclidean distance method to set a threshold range to determine whether to check if it is close to the target point X goal ; if X rand is close enough to the target point X goal , then add it to the queue Qg, indicating that a path close to the target has been found;

[0045] S313: Perform backtracking and construct the complete path. For each node Xe in the queue Qg, use the tree T to backtrack to obtain the path f from the starting point X init to this node Xe; use the mapping function Γ to map the path f to the corresponding path in the topological graph and consider the direct connection from the current node to the target point If the path f is not in the existing set of homotopy classes ξold, then add it;

[0046] S314: Calculate the shortest version f * of the current path f, and make a judgment. If ξold is empty or the cost S(f * ) of the new path f * is less than the cost S(f) of the current optimal path f, then update the optimal path to f * ; when the loop ends, the obtained path f is the optimal path planning obtained by this algorithm, and output the path f.

[0047] Further, in step S4, the estimation state and dynamic obstacle avoidance specifically include:

[0048] S41: Sample the speed data and delimit the sampleable speed range V m ;

[0049] S42: Then delimit the sampleable acceleration range V d ;

[0050] S43: Finally, delimit the speed range V under the condition of no collision; the final speed sampling space of the mobile robot is the intersection of the three speed spaces, that is, V a = V s ∩ V m ∩ V d ∩ V a ;

[0051] S44: Predict the trajectory. After determining the speed sampling space V s , sample uniformly in this space at a certain sampling interval, that is, the resolution. In the speed space, set the resolution for the linear velocity and angular velocity respectively, and use E v , E ω to represent the sampling resolution. Then the number of the sampling speed group n can be determined;

[0052] S45: When a group of (v, ω) is sampled, perform trajectory prediction through the kinematic model of the differential drive mobile robot, that is, position update, and then save the predicted trajectory within the prediction time for subsequent processing;

[0053] S46: After completing the trajectory prediction, perform the final trajectory evaluation, select the speed corresponding to the optimal trajectory as the driving speed, and execute the trajectory obstacle avoidance;;

[0054] S47: Normalization processing, that is, smoothing processing, where σ represents normalization;

[0055] S48: Collect data through the speed and acceleration odometer to obtain the state vector X k = [x, y, θ, v x , v y , ω] T , perform state prediction, and based on the current state and the control input u k (such as the linear velocity v and the angular velocity ω), use the nonlinear state transition function f for prediction;

[0056] S49: According to the state prediction equation, in order to locally linearize the nonlinear model, it is necessary to calculate the Jacobian matrix F k of the state transition function f with respect to the state x, and then predict the covariance matrix;

[0057] S410: During the update phase, use the sensor measurement value Z k to correct the predicted state; it is necessary to calculate the Jacobian matrix H of the observation function h with respect to the state x k , and then calculate the Kalman gain;

[0058] S411: Finally, update the state estimate and covariance matrix to obtain the estimated quantity of the state; if an anomaly occurs, trigger path replanning to ensure task continuity.

[0059] The second aspect of the present invention relates to a mobile planning system for a disabled-assisting robot, including:

[0060] Data acquisition and preprocessing module: used to collect environmental data and user voice commands, filter and denoise the laser point cloud data, and extract key features of obstacles; perform noise reduction and feature encoding on the voice signal, and convert it into an instruction that can be processed by a machine, and output clean point cloud data and parsed text control instructions;

[0061] Environmental perception and map construction module: used to construct a two-dimensional environmental map, update the obstacle state in real time, and improve the map accuracy and consistency through loop detection and global optimization techniques, output a high-resolution grid map, and mark static and dynamic obstacle areas;

[0062] Data analysis and path planning module: used to obtain the free space X of the agent free , perform convex segmentation on X free to obtain a list of polygons, create a graph structure to store topological information, add nodes and edges, and construct a topological graph X topo , search for a global path in the topological graph, and generate a safe and smooth trajectory through progressive expansion and path optimization strategies. Output the global optimal path from the starting point to the target point;

[0063] State estimation and dynamic obstacle avoidance module: used to detect dynamic obstacles in real time, delimit a safe speed range; predict future trajectories and evaluate priorities, and dynamically adjust local paths; fuse multi-sensor data to correct the robot pose to ensure path execution accuracy; output the local path after obstacle avoidance and real-time motion control instructions;

[0064] Execution control and feedback module: used to drive the robot to move along the planned path and monitor the execution state; trigger path replanning when an abnormal interruption occurs to ensure task continuity.

[0065] The third aspect of the present invention relates to a mobile planning device for a disabled-assisting robot, including a memory and one or more processors. An executable code is stored in the memory. When the one or more processors execute the executable code, it is used to implement the above-mentioned mobile planning method for a disabled-assisting robot.

[0066] The fourth aspect of the present invention relates to a computer-readable storage medium, on which a program is stored. When the program is executed by a processor, the above-mentioned method for mobile planning of a disabled-assisting robot is implemented.

[0067] The fifth aspect of the present invention relates to a computer program product, including a computer program, which implements the above-mentioned method for mobile planning of a disabled-assisting robot when executed by a processor.

[0068] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0069] 1. The present invention solves the technical problems of the traditional mobile planning of disabled-assisting robots, such as the lack of efficient environment perception and efficient path generation methods, inaccurate path planning, and susceptibility to environmental changes; at the same time, the present invention solves the technical problems of the traditional obstacle avoidance method, such as slow response in a dynamic environment and increased movement risks.

[0070] 2. The present invention constructs a high-precision two-dimensional map of a disabled-assisting robot through voice input, laser SLAM technology, and grid map optimization method, significantly improving the accuracy of navigation planning; and adopts a more optimized path planning method, making the path planning more efficient and stable.

[0071] 3. The present invention proposes a dynamic path planning strategy that combines ultrasonic obstacle avoidance and local path planning algorithms, significantly enhancing the obstacle avoidance ability and path planning flexibility of the disabled-assisting robot in a complex environment.

[0072] 4. By adopting advanced speech recognition technology, especially the speech recognition module based on the optimized model of the hybrid loss function, the present invention realizes efficient and accurate speech command recognition, significantly improving the interaction experience between the user and the disabled-assisting robot, enabling the patient to more conveniently and quickly control the movement and operation of the robot through voice commands. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] Figure 1 It is a schematic diagram of the steps of a method for mobile planning of a disabled-assisting robot in Embodiment 1 of the present invention;

[0074] Figure 2 It is a schematic diagram of the process of a mobile planning system of a disabled-assisting robot of the present invention;

[0075] Figure 3 It is a schematic diagram of the process of training and using the optimized model of the hybrid loss function of the present invention;

[0076] Figure 4 It is a schematic diagram of the process of a laser sensor establishing a 2D map of the present invention;

[0077] Figure 5Schematic diagram of the process of converting the environmental map of the present invention into a convex topological map and finding the optimal path;

[0078] Figure 6 Schematic diagram of the process of measuring state estimation of the present invention;

[0079] Figure 7 Schematic diagram of the process of realizing dynamic obstacle avoidance in local path planning of the present invention.

[0080] Figure 8 Schematic diagram of the planning system of the present invention.

[0081] Figure 9 Block diagram of the structure of the planning device of the present invention. Detailed implementation manners

[0082] The following will describe in detail the specific implementation manners of the embodiments of the present invention with reference to the accompanying drawings. It should be understood that the specific implementation manners described herein are only used to illustrate and explain the embodiments of the present invention, and are not used to limit the embodiments of the present invention.

[0083] It should be noted that, without conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other.

[0084] The present invention will be described in detail below with reference to the accompanying drawings and in conjunction with exemplary embodiments.

[0085] Embodiment 1

[0086] Reference Figures 1 to 7 , a method for mobile planning of a disabled-assisting robot according to the present invention, includes the following steps:

[0087] S1: The system waits for the patient to issue a voice command. The patient issues the command through a 6-microphone array voice module, and these voice signals are then transmitted to the CPU for processing. The system uses the CTC-Transformer model to process the voice command. This model includes three parts: preprocessing, encoder-decoder, and loss calculation, and has the characteristics of low latency and high accuracy. The preprocessing converts the voice features into acoustic feature vectors and adds position encoding to retain the sequence information. The encoder-decoder structure uses the multi-head attention mechanism to capture the key information of the input sequence. To optimize the recognition accuracy, the system combines the advantages of the CTC and Transformer loss functions, and calculates the final loss value by weighted summation, thereby improving the model training effect. Finally, the voice command is converted into a text control command, enabling the disabled-assisting robot to accurately execute tasks according to the user's command.

[0088] Specifically, step S1 includes:

[0089] S11: Wait for the patient to take his place, and the system enters the standby state, ready to receive the user's command.

[0090] S12: The patient issues a voice command through the voice module. This step is the part where the patient or user directly interacts with the system, inputting commands to the disability assistance robot through spoken language, that is, transmitting the voice signal to the CPU processor.

[0091] S13: The language recognition model we use is the CTC-Transformer model, which consists of three parts, including a preprocessing module, an encoder-decoder module, and a hybrid CTC-Transformer loss calculation module, and has the characteristics of low latency, low error rate, and strong parallelization ability.

[0092] S14: The preprocessing module contains K acoustic preprocessing modules (including a 2D convolutional layer and a Relu activation layer) and a text preprocessing module, and the tasks are to process the input voice features into acoustic feature vectors that the encoder can recognize and to convert the true text labels corresponding to the voice signal into text embedding sequences.

[0093] S15: The sequences obtained after being processed by the preprocessing module need to be added to their respective position encodings before entering the Transformer encoder and decoder. The specific formula for the position encoding:

[0094]

[0095] k is the position of the previous label in the input sequence; i is the index of each value in the quantity; d is the dimension of the position vector, p k,2i is the 2i-th element in the encoding vector at position k, p k,2i+1 is the (2i + 1)-th element in the encoding vector at position k.

[0096] S16: The encoder-decoder structure is composed of N identical modules stacked together, and each module contains two main sub-layers: a multi-head attention layer and a feed-forward neural network layer. During the training process, the encoder maps the acoustic feature vector to a hidden layer vector, which on the one hand serves as the input to the CTC decoding end, and on the other hand serves as the value vector and key vector in the self-attention calculation process of the second layer of the decoder. The formula for multi-attention is:

[0097]

[0098] In the formula: is the query vector, is the key vector, is the value vector, where t is the number of inputs, d is the dimension of the corresponding elements, is the scaling factor. h represents the number of attention heads, head is the attention head, and the trainable parameter matrix is the output weight matrix, is the weight matrix, is the dimension of Key, i is the index, soft max() is the activation function, Attention() is the attention function, MultiHead() is the multi - head attention function, and Concat is the concatenation operation.

[0099] S17: Based on the Transformer model, the temporal connection classification algorithm is used to assist in training. Therefore, when calculating the loss function, the loss values of CTC and Transformer should be considered separately, and finally the weighted sum of the two obtained loss values is calculated. After the model is processed, the voice command is converted into a text control command. The combined loss formula is:

[0100]

[0101] Among them, a(t, i) is the forward decoding probability, β(t, u) is the backward decoding probability, u ∈ [1, 2l + 1], F(t, u) represents the set of all possible paths passing through node u at time t, B(t, u) is the set of all subsequent paths passing through node u at time t, S is the labeled training set, P CTC (Y|X) is the probability of the output sequence Y given the input sequence X, y i is the probability distribution of the true label, is the probability distribution of the predicted label, λ ∈ [0, 1] is the weight coefficient, L CTC is the loss function of connectionist temporal classification, L att is the attention - based loss function, T loss is the total loss function. The trained model is used to convert the voice command into a text control command.

[0102] S2: Use the lidar sensor to scan the surrounding environment to obtain detailed three - dimensional point cloud data. Then, filter and denoise these point cloud data to improve the data quality. Next, project the pre - processed three - dimensional point cloud onto a two - dimensional plane to generate a two - dimensional point cloud map. Then, divide the two - dimensional plane into multiple grids, determine the state (idle, occupied or unknown) of each grid according to the two - dimensional point cloud map, and construct a grid map. Finally, optimize the map through loop detection and backend optimization techniques.

[0103] Specifically, step S2 includes:

[0104] S21: First, in the data acquisition stage. Use the lidar sensor to scan the surrounding environment at a set frequency range (for example, 10Hz to 40Hz) to obtain detailed three - dimensional point cloud data.

[0105] S22: Next, enter the data preprocessing stage. Perform necessary processing such as filtering and denoising on the collected three-dimensional point cloud data to effectively improve the data quality and lay a solid foundation for subsequent steps. The filtering and denoising use Statistical Outlier Removal (SOR), and the specific formula is as follows:

[0106]

[0107] where, is the average distance of all points; p i is the reference point, p j is the j-th nearest neighbor point of, d(p i , p j ) is the Euclidean distance between two points, μ is the average distance of all points is the average value of, n is the total number of points in the point cloud, α is the user-defined standard deviation multiple threshold, k is the number of neighbors or points considered when calculating the average distance of each point, and σ is the standard deviation of these average distances.

[0108] S23: Subsequently, perform the point cloud projection operation. Accurately project the preprocessed three-dimensional point cloud data onto a two-dimensional plane to generate the corresponding two-dimensional point cloud map.

[0109] S24: Immediately afterwards, construct a grid map. Reasonably divide the two-dimensional plane into a series of grids, and each grid represents a specific location. Based on the generated two-dimensional point cloud map, accurately determine the state of each grid, i.e., free, occupied, or unknown.

[0110] S25: Finally, implement the map optimization step. Through advanced technical means such as loop detection and backend optimization, comprehensively optimize the map to improve its accuracy and consistency, and ensure that the generated 2D map has high practical value.

[0111] S3: After establishing the 2D map, the route planning method we adopt is CDT-RRT route planning, that is, first use the convex segmentation technology of the two-dimensional environmental map to segment the environment, divide the complex environment into multiple simple convex polygon regions, and according to the convex segmentation result, construct a topological graph X topo , where the nodes represent convex polygons and the edges represent the connection relationships between adjacent convex polygons. Then use the RRT algorithm (Rapidly-Exploring Random Tree Star algorithm) to perform path search to find the optimal path, and this algorithm is divided into a map initialization stage and a path search stage.

[0112] Step S3 specifically includes:

[0113] S31: Obtain the free space X freeAccording to the characteristics of the grid map, that is, the map is divided into small cells, each cell represents a specific-sized area and is marked as an obstacle or free space. Traverse the map, traverse every cell on the entire map. Then identify the free space. If a cell is not an obstacle, mark it as free space. This can be achieved by checking the value of the cell. For example, in a binary map, 0 may represent free space while 1 represents an obstacle. Finally, perform connectivity analysis. The purpose is for further optimization. Connectivity analysis can be carried out to determine which free space regions are connected to each other.

[0114] S32: Perform convex partitioning on X free to obtain a list of polygons. The convex partitioning uses the Hertel-Mehlhorn algorithm. The diagonal generation and polygon triangulation optimization method involves the following steps:

[0115] S321: First, identify all possible diagonals in the simple polygon, that is, line segments connecting two non-adjacent vertices and not intersecting the polygon boundary (except at the endpoints). An effective method to generate these diagonals is to start from each vertex, try to connect to other non-adjacent vertices, and verify whether these connections meet the conditions.

[0116] S322: Subsequently, use these diagonals to divide the polygon into multiple triangles. This process is usually achieved through the ear-cutting method technology, resulting in a triangulation graph composed of diagonals. The specific formula for judging whether a vertex can form a triangle in the ear-cutting method is:

[0117]

[0118] In the formula, the vertex coordinates are (x i , y i ), is the vector from the point (x i-1 , y i-1 ) to the point (x i , y i ), is the vector from the point (x i , y i ) to the point (x i+1 , y i+1 ). If the cross product result is greater than 0, then these three points are arranged in counterclockwise order, that is, they form a triangle.

[0119] S323: Next, remove unnecessary diagonals. The specific approach is that for each pair of adjacent triangles (sharing a side), if they can be merged into a convex quadrilateral, then remove the diagonal between them. This ensures that all the final polygons are convex polygons.

[0120] S324: Additionally, the Hertel-Mehlhorn algorithm is used for optimization to ensure that the number of output convex polygons does not exceed twice the number of vertices of the input polygon. Since removing each diagonal reduces the number of convex polygons by one while maintaining the convexity of the remaining part. In terms of specific implementation details, the diagonal validity check needs to confirm that it is completely inside the polygon, which can be achieved through geometric algorithms such as ray intersection testing or plane sweep; and the merging condition is to check whether the sum of the two angles at both ends of the shared edge is less than or equal to 180 degrees to determine whether two triangles can be merged into a convex quadrilateral.

[0121] S33: Construct a topological graph X based on the segmentation result topo , from the above two steps, we obtain the result of the convex decomposition of the free map. Store all the obtained convex polygons in a list, and then create an empty graph structure to store topological information. Traverse the list and add a node for each convex polygon. Each node can be attached with some attributes, such as the vertex coordinates of the polygon. Then detect and add edges. For each pair of polygons, check whether they are adjacent by checking whether their boundaries overlap. Finally, to further optimize the topological graph, remove those nodes with only one neighbor (referred to as simply connected nodes) to construct the topological graph X topo .

[0122] S34: First, perform semantic parsing to extract the key parameters in the instruction, match the predefined task template, and then perform coordinate mapping. According to the pre-stored environmental map, convert the semantic target point into two-dimensional coordinates (x, y) in the map. If the target point is temporarily adjusted due to environmental changes, the system corrects the coordinates in real time through laser SLAM. Determine the starting point X init and the target point X goal , and initialize the tree T with X init as the root. Then set up three empty queues ξold, Qr, Qg. The queue ξold is used to record the homotopy path classes that have been searched, while Qr is used to store the re-routing process, and Qg is used to store the randomly sampled points close to the target point

[0123] S35: After setting up the basic variables, prune X topo to remove the parts unrelated to the path between the two points with X init and X goal as the basis. Input the topological graph X topo , the starting point X init and the target point X goal . First, make a copy X′ topo of the topological graph X topo , and then use the function Γ to connect the starting point X init and the target point X goalMap to the corresponding nodes in the topological graph. The Γ function here is a mapping that converts geometric coordinates into a sequence of nodes in the topological graph.

[0124] S36: Then traverse all the simply connected nodes (nodes with only one neighbor) in X topo and set up a temporary variable X temp and assign the current simply connected node x to the temporary variable X temp .

[0125] S37: Then enter a loop until X temp is no longer a simply connected node. If X temp is not the starting point or the target point, then remove it from X′ topo . Otherwise, exit the loop, that is, if X temp is the starting point or the target point, then do not remove it. Update X temp to its neighbor node and continue to check whether the new X temp is still a simply connected node. Finally, return the copy X′ topo .

[0126] S38: Start a loop that performs N iterations to expand the tree structure. In the loop, first sample on the secant line of X topo using formula (6) to generate a random sample point X rand . This step uses the roulette wheel selection process to select the secant line and randomly generate a point X rand on the selected secant line. The specific formula is as follows:

[0127]

[0128] ρ 1 and ρ 2 are random numbers between [0, 1], c 1 , c 2 ,..., c n are the corresponding secant lines in X topo , th 1 , th 2 ,..., th k and th n-1 are the thresholds used to select the secant line during sampling, α is a very large number, β is (0, 1], μ i is the number of sampling points adjacent to the i-th sampling point, η i is the number of the i-th sampling point, κ i is the number of times the i-th occurrence in the homotopy class (list ξold) during sampling.

[0129] S39: Subsequently, find the set of nearby nodes Q near, use the KD - tree to efficiently find a set of nodes closer to the random X rand Then we find the nearest node X in this set of nodes closest , and adopt formula (7), which is as follows:

[0130]

[0131] where ‖·‖ is the 2 - norm in Euclidean space, and N is a sufficiently large positive integer. Here, Cost(x) is the total path of Sf(x), and f(x) is the path from x to x using T init to x

[0132] S310: After finding it, connect X rand to X closest , add this new edge to the tree T, and then add the newly generated random point X rand to the end of the queue Qr for possible subsequent re - wiring operations

[0133] S311: After completion, use the Euclidean distance method to set a threshold range to determine whether to check if it is close to the target point X goal , if X rand is close enough to the target point X goal , then add it to the queue Qg, indicating that a path close to the target has been found. The basic formula is:

[0134]

[0135] where d(x,y) is the distance between two points, (x 2 ,y 2 ) and (x 1 ,y 1 ) are two points

[0136] S312: Next, try to re - wire the nodes in the queue Qr to optimize the structure of the tree to obtain a shorter path. The specific operation is as follows: First, take out a node Xr from the queue Qr. This queue contains the new nodes that have been recently added to the tree T. Then find the nearby nodes, and find all the sets of nodes Q near that are closer to Xr in the tree T, which is efficiently implemented using the KD - tree. Then for each nearby node X near , calculate the change in the path cost if the parent node of X near is changed to Xr. If the new path cost is lower, then update the parent node of X near to Xr and add it to Qr for further optimizing the connection relationship of its child nodes. Repeat the above process until the queue Qr is empty, indicating that all nodes have been optimized for re - wiring

[0137] S313: Then perform backtracking and construct the complete path. For each node Xe in the queue Qg, use the tree T to backtrack to obtain the path f from the starting point X init to this node Xe. Use the mapping function Γ to map the path f to the corresponding path in the topological graph, and consider the direct connection from the current node to the target point If the path f is not in the existing homotopy class set ξold, add it to it.

[0138] S314: Finally, calculate the shortest version f of the current path f * , and make a judgment. If ξold is empty or the cost S(f * ) of the new path f * is less than the cost S(f) of the current optimal path f, then update the optimal path to f * . When the loop ends, the obtained path f is the optimal path planning obtained by this algorithm.

[0139] S4: After we have planned the route on the fixed map through the algorithm, the disabled assistance robot will move along the established route. To determine whether it executes according to the established route, we use a speed-accelerated odometer to obtain the current position of the disabled assistance robot, and then determine whether it is close to the destination. During the dynamic process, if dynamic obstacles appear, we will use an ultrasonic radar for local path planning to avoid obstacles so that the disabled assistance robot can run to the destination normally according to the specified planned route. The specific method used is the DWA algorithm (Dynamic Window Algorithm). The core idea is to determine a sampled speed space that satisfies the hardware constraints of the mobile robot in the speed space (v, ω) according to the current position state and speed state of the mobile robot, then calculate the trajectory of the mobile robot moving for a certain period of time under these speed conditions, and evaluate the trajectory through an evaluation function. Finally, select the speed corresponding to the trajectory with the best evaluation as the movement speed of the mobile robot, and so on until the mobile robot reaches the target point.

[0140] Specifically, step S4 includes:

[0141] S41: First, we need to sample speed data. In this operation, the data sampling is all completed by the speed-accelerated odometer, and the sampleable speed range V m is delimited as follows:

[0142] V m ={(v, ω)|v ∈ [v min , v max , ω ∈ [ω min , ω max} (9)

[0143] In the formula, v min , v maxare the minimum linear velocity and the maximum linear velocity of the disabled-assisting robot, ω min , ω max are the minimum angular velocity and the maximum angular velocity of the disabled-assisting robot respectively.

[0144] S42: Then delimit the range of the accelerations that can be sampled V d :

[0145]

[0146] In the formula, v c , ω c are the linear velocity and the angular velocity of the disabled-assisting robot at the current moment respectively, and a v max , a ωmax are the maximum linear acceleration and the maximum angular acceleration of the disabled-assisting robot respectively.

[0147] S43: Finally, delimit the velocity range V under the condition of no collision a :

[0148]

[0149] In the formula, dist(v, ω) represents the closest distance between the corresponding simulated trajectory and the obstacle at the current velocity. The ultrasonic sensor emits ultrasonic signals from the transmitting end. After the ultrasonic waves encounter the measured object, they will form reflections and are received by the receiving end of the ultrasonic sensor. By calculating the time from the emission of the ultrasonic signal to the reception of the return signal, the distance between the ultrasonic sensor and the reflecting object can be calculated. c is the sound wave velocity. In the case of no obstacle, dist(v, ω) is a very large constant value. When the running sampling speed of the robot is within the range of formula (11), it can be constrained by the maximum deceleration. The final velocity sampling space of the mobile robot is the intersection of the three velocity spaces, that is, V s = V m ∩V d ∩V a .

[0150] S44: Then we conduct trajectory prediction. After determining the velocity sampling space V s , the DWA algorithm uniformly samples in this space at a certain sampling interval (resolution). In the velocity space, the resolutions are set for the linear velocity and the angular velocity respectively, and are represented by E v , E ω respectively. Then the number of sampling velocity groups can be determined as follows:

[0151]

[0152] In the formula, V high , V low , ω high , ωlow are the upper and lower limits of the velocity space. The above equation shows that for the linear velocity, a value is taken at each interval of E v in magnitude, and for the angular velocity, a value is taken at each interval of E ω in magnitude, thus forming a series of velocity groups. When

[0153] S45: After sampling a group of (v, ω), trajectory prediction (i.e., position update) is performed through the kinematic model of the differential drive mobile robot, and then the predicted trajectory is saved within the prediction time for subsequent processing. The specific trajectory prediction is as follows:

[0154]

[0155] In the formula, (x, y, θ) represents the pose of the robot, k represents the sampling moment, and Δt represents the sampling interval.

[0156] S46: After completing the trajectory prediction, we perform the final trajectory evaluation, then select the velocity corresponding to the optimal trajectory as the driving velocity, and execute obstacle avoidance for this trajectory. The evaluation function is as follows:

[0157] G(v, ω) = σ(α·heading(v, ω)) + σ(β·dist(v, ω)) + σ(γ·velocity(v, ω)(15)

[0158] heading(v, ω) is the azimuth evaluation function, used to evaluate the error Δθ of the angle between the direction of the end position of the trajectory generated at the current sampling velocity and the line connecting to the target point; since we want a larger evaluation function value to indicate better performance, so π - Δθ is used for evaluation, that is, heading(v, ω) = π - Δθ. dist(v, ω) is the distance evaluation function, representing the closest distance between the simulated trajectory corresponding to the current velocity and the obstacle; if there is no obstacle or the closest distance is greater than the set threshold, then its value is set to a relatively large constant value. velocity(v, ω) is the velocity evaluation function, representing the magnitude of the current velocity, which can be directly represented by the magnitude of the current linear velocity. The larger it is, the faster the velocity on the planned trajectory, and the higher the evaluation score. α, β, and γ are all coefficients of the evaluation function.

[0159] S47: Since the process of local path planning requires the acquisition of multiple sensors, and the acquired information cannot be continuous, which will also result in relatively large differences after evaluation, so normalization processing (smoothing processing) can be performed, where σ represents normalization, and the specific operation is as follows:

[0160]

[0161] Among them, i represents the i-th simulated trajectory, and n is the total number of all sampled trajectories under the constraints. From the above formulas (15) and (16), a path that satisfies avoiding obstacles and traveling quickly towards the target point can be obtained, enabling the robot to complete the local path optimal planning.

[0162] S48: For the current state of the disability assistance robot, we use the Extended Kalman Filter (EKF) for state estimation, collect data through the velocity-accelerated odometer, and obtain the state vector X k =[x, y, θ, v x , v y , ω] T , perform state prediction, based on the current state and the control input u k (such as the linear velocity v and the angular velocity ω), use the non-linear state transition function f for prediction:

[0163]

[0164] S49: According to the state prediction equation, in order to locally linearize the non-linear model, it is necessary to calculate the Jacobian matrix F of the state transition function f with respect to the state x k , and then predict the covariance matrix. The formula is as follows:

[0165]

[0166] Among them, Q is the covariance matrix of the process noise.

[0167] S410: In the update stage, use the sensor measurement value Z k to correct the predicted state. It is necessary to calculate the Jacobian matrix H of the observation function h with respect to the state x k , and calculate the Kalman gain:

[0168]

[0169] Among them, R is the covariance matrix of the measurement noise.

[0170] S411: Finally, perform the update of the state estimation and the covariance matrix. The specific formula is as follows:

[0171]

[0172] Obtain the estimated quantity of the state. If an anomaly occurs, trigger path replanning to ensure task continuity.

[0173] Embodiment 2

[0174] Reference Figure 8, A mobile planning system for a disabled - assistance robot of the present invention includes:

[0175] Data acquisition and pre - processing module: It is used to collect environmental data and user voice commands, filter and denoise the laser point - cloud data, and extract key features of obstacles; denoise and perform feature encoding on the voice signal, and convert it into instructions that can be processed by a machine, and output clean point - cloud data and parsed text control instructions;

[0176] Environmental perception and map - building module: It is used to build a two - dimensional environmental map, update the state of obstacles in real time, and improve the accuracy and consistency of the map through closed - loop detection and global optimization techniques, output a high - resolution grid map, and mark static and dynamic obstacle areas;

[0177] Data analysis and path - planning module: It is used to obtain the free space X of the agent free , for X free perform convex segmentation to obtain a list of polygons, create a graph structure to store topological information, add nodes and edges, and construct a topological graph X topo , search for a global path in the topological graph, and generate a safe and smooth trajectory through progressive expansion and path - optimization strategies. Output the global optimal path from the starting point to the target point;

[0178] State estimation and dynamic obstacle - avoidance module: It is used to detect dynamic obstacles in real time, delimit a safe speed range; predict future trajectories and evaluate priorities, and dynamically adjust local paths; fuse multi - sensor data to correct the robot's pose to ensure path - execution accuracy; output the local path after obstacle - avoidance and real - time motion control instructions;

[0179] Execution control and feedback module: It is used to drive the robot to move along the planned path, monitor the execution state; trigger path replanning during abnormal interruption to ensure task continuity.

[0180] Embodiment 3

[0181] A mobile planning device for a disabled - assistance robot of the present invention includes a memory and one or more processors. When the one or more processors execute the executable code stored in the memory, it is used to implement the above - mentioned mobile planning method for the disabled - assistance robot.

[0182] Embodiment 4

[0183] The present invention also provides a computer - readable storage medium, on which a program is stored. When the program is executed by a processor, it is used to implement the above - mentioned Figures 1 to 7 mobile planning method for the disabled - assistance robot provided.

[0184] Embodiment 5

[0185] The present invention also provides Figure 9A schematic structural diagram of a planning device corresponding to Figure 1 is shown. As Figure 9 described, at the hardware level, the planning device includes a processor, an internal bus, a network interface, a memory, and a non-volatile memory. Of course, it may also include other hardware required for other services. The processor reads the corresponding computer program from the non-volatile memory into the memory and then runs it to implement the Figure 1 data acquisition method described above. Of course, in addition to the software implementation method, the present invention does not exclude other implementation methods, such as logical devices or a combination of software and hardware, etc. That is to say, the execution subject of the following processing flow is not limited to each logical unit, and can also be hardware or logical devices.

[0186] The above are only embodiments of the present invention and are not used to limit the present invention. For those skilled in the art, the present invention can have various changes and modifications. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included within the scope of the claims of the present invention.

Claims

1. A method for planning the movement of a robot for assisting the disabled, characterized in that: The following steps are involved: S1: receiving and processing language instructions; The system receives user voice commands, creates an untrained voice model, converts the training data set into a feature sequence through the front-end module, adds position encoding, extracts information through the encoder-decoder structure, optimizes the model with the loss function, and converts the voice command into a text command output using the trained model; S2: Perceiving the environment and building maps; Use LiDAR to obtain 3D point clouds, filter and denoise them, and then project them onto a 2D plane. Divide the grid and mark the states to build an initial map. Correct errors through loop detection and optimization to output the final map. S3: Analyze data and plan routes; Extract free space based on raster map and analyze connectivity, divide free space into convex polygons and construct topological map; Initialize the search tree and set the starting point and end point according to the instructions; use random sampling and connection strategies to expand the tree, optimize the path nodes, and then adjust the path through iteration; backtrack to generate the optimal path and output the path; S4: Estimated state and dynamic obstacle avoidance; The robot's position and posture are updated in real time through the filtering algorithm to ensure that the robot moves along the path. When encountering a dynamic obstacle, the safe speed range is first determined based on the robot's kinematic constraints and the obstacle distance, and then the motion trajectory in the next few seconds is predicted based on the kinematic model. The optimal local obstacle avoidance path is selected by comprehensively considering the azimuth deviation, obstacle distance and moving speed priority. The adjusted local path and control instructions are output.

2. The method for mobile planning of a disabled assistive robot according to claim 1, characterized in that: In step S1, receiving and processing language instructions specifically includes: S11: Waiting for the patient to be in place, the system enters a standby state, and is ready to receive instructions from the user; S12: The patient issues a voice command through the voice module and inputs a command to the assistive robot through oral language, that is, the voice signal is transmitted to the CPU processor; S14: Build an untrained speech recognition model. The pre-processing module uses the predefined related speech command library as a training data set and processes it into an acoustic feature vector that can be recognized by the encoder and converts the real text label corresponding to the speech signal into a text embedding sequence; S15: The sequences obtained by the pre-processing module are added to their respective position codes; the specific formula of the position code is shown in formula (1): Among them, k is the position of the previous label in the input sequence; i is the index of each value in the quantity; d is the dimension of the vector, p k,2i is the 2ith element in the encoding vector at position k, p k,2i+1 is the 2i+1th element in the encoding vector at position k; S16: Use the encoder-decoder structure to train the model and accurately convert the acoustic features into the target of text instructions, as shown in formula (2): Where: is the query vector, is the key vector, is a vector of values, where t is the number of inputs and d is the element size. is the scaling factor; h represents the number of attention heads, head is the attention head, and the trainable parameter matrix is the output weight matrix, is the weight matrix, is the dimension of Key, i is the index, softmax() is the activation function, Attention() is the attention function, MultiHead() is the multi-head attention function, and Concat is the connection operation; S17: Use the temporal connection classification algorithm to assist training; When calculating the overall loss evaluation, two different types of loss values ​​are fused. At the same time, the temporal loss and attention loss are fused to calculate the total loss. The model parameters are optimized through back propagation, and the model is continuously trained to improve the conversion accuracy. The fused loss formula is: Among them, a(t,u) is the forward decoding probability, β(t,u) is the backward decoding probability, u∈[1,2l+1], F(t,u) represents the set of all possible paths passing through node u at time t, B(t,u) is the set of all subsequent paths passing through node u at time t, S is the labeled training set, P CTC (Y|X) is the probability of output sequence Y given input sequence X, y i is the probability distribution of the true label, is the probability distribution of the predicted label, λ∈[0,1] is the weight coefficient, L CTC is the loss function for connection time series classification, L att is the attention-based loss function, T loss is the total loss function; S18: Use the trained model to convert voice commands into text control commands and output the commands.

3. The method for mobile planning of a disabled assistive robot according to claim 1, characterized in that: In step S2, sensing the environment and building a map specifically include: S21: collect data; Use the LiDAR sensor to scan the surrounding environment within a set frequency range to obtain detailed 3D point cloud data; S22: preprocessing data; The collected 3D point cloud data is filtered and denoised. The specific formula is as follows: in, is the average distance of all points, p i is the reference point, p j is the jth nearest neighbor, d(p i ,p i ) is the Euclidean distance between two points, μ is the average distance of all points The average value of n is the total number of points in the point cloud, α is the user-defined standard deviation multiple threshold, k is the number of neighbors or points considered when calculating the distance average of each point, and σ is the standard deviation of these distance averages; S23: Execute point cloud projection operation; Accurately project the pre-processed 3D point cloud data onto a 2D plane, and then generate a corresponding 2D point cloud image; S24: constructing a grid map; The two-dimensional plane is scientifically and rationally divided into a series of grids, each grid representing a specific location; based on the generated two-dimensional point cloud, the status of each grid is accurately determined, that is, free, occupied or unknown; S25: Implement the map optimization step, and comprehensively optimize the map through loop detection and backend optimization to improve its accuracy and consistency, so as to ensure that the generated 2D map has high practical value.

4. The method for mobile planning of a disabled assistive robot according to claim 1, characterized in that: In step S3, analyzing data and planning paths specifically include: S31: Get the free space of the agent X free ; According to the characteristics of the raster map, the map is divided into small cells, each of which represents an area of ​​a certain size and is marked as an obstacle or free space; each cell on the entire map is traversed to identify the free space. If a cell is not an obstacle, it is marked as free space by checking the value of the cell; finally, a connectivity analysis is performed to determine which free space areas are connected to each other; S32: To X free Perform convex segmentation to obtain a polygon list; S33: Create a graph structure to store topological information, add nodes and edges, and construct a topological graph X topo ; S34: First, perform semantic analysis, extract key parameters in the instruction, match the predefined task template, and then perform coordinate mapping. According to the pre-stored environment map, the semantic target point is converted into a two-dimensional coordinate (x, y) in the map; if the target point is temporarily adjusted due to environmental changes, the system uses laser SLAM to correct the coordinates in real time; determine the starting point X of the task init and the target point X goal , and X init Initialize the tree T as the root; then set up three empty queues ξold, Qr, Qg, the queue ξold is used to record the searched homology path class, while Qr is used to store the rewiring process, and Qg is used to store random sampling points close to the target point; S35: After setting up the basic variables, topo To trim, use X init and X goal To remove the parts irrelevant to the two-point path; input the topology graph X topo 、Initial point X init and the target point X goal , first put the topology map X topo Make a copy X′ topo , and then use the function Γ to convert the starting point X init and the target point X goal Mapped to the corresponding nodes in the topological graph; wherein the function Γ is a mapping that converts geometric coordinates into a sequence of nodes in the topological graph; S36: Traverse X again topo For all the simply connected nodes in the graph, that is, nodes with only one neighbor, set up a temporary variable X temp , assign the current single-connected node x to the temporary variable X temp ; S37: Then enter a loop until X temp is no longer a simply connected node; if X temp If it is not the starting point or the target point, remove it from X′ topo Remove; otherwise, exit the loop, that is, if X temp If it is the starting point or the target point, do not remove it; update X temp For its neighbor nodes, continue to check the new X temp Is it still a single connected node? Finally, return the copy X′ topo ; S38: Start a loop and execute multiple iterations to expand the tree structure; in the loop, first use formula (6) to calculate the value of X topo Sampling is performed on the secant line to generate random sample points X rand ; This step uses a roulette wheel selection process to select a secant line and randomly generates a point X on the selected secant line rand , the specific formula is as follows: Where ρ1 and ρ2 are random numbers between [0, 1], c1, c2, ..., c n Yes X topo The corresponding secant lines in th1, th2, ..., th k and n-1 are the thresholds used to select the secant line during sampling. α is a very large number, β is (0,1], μ i is the number of sampling points adjacent to the i-th sampling point, η i is the number of the i-th sampling points, κ i is the number of occurrences of listξold) for the ith time in the sampled homology class; S39: Find nearby node set Q near , using efficient data structures to find the distance to a random X rand A group of nodes that are closer, find the closest node X in this group of nodes closest , using formula (7), as follows: Where ‖·‖ is the 2-norm in Euclidean space, and N is a sufficiently large positive integer. Here Cost(x) is S(fx), which is the length of the path fx, where fx is the path from x_init to x using T; S310: X rand Connect to X closest , and add this new edge to the tree T, and then add the newly generated random point X rand Add to the end of the queue Qr for possible subsequent reconnection operations; S311: Use the Euclidean distance method to set the threshold range to determine whether the check is close to the target point X goal , if X rand Close enough to the target point X goal , then add it to the queue Qg, indicating that a path close to the target has been found; the basic formula is: Among them, d(x,y) is the distance between two points, (x2,y2) and (x1,y1) are two points S312: Try to reconnect the nodes in the queue Qr to optimize the tree structure to obtain a shorter path; S313: Backtrack and construct a complete path. For each node Xe in the queue Qg, use the tree T to backtrack and obtain the complete path from the starting point Xe. init Path f to the node Xe; use the mapping function Γ to map the path f to the corresponding path in the topology graph, and consider the direct connection from the current node to the target point If the path f is not in the existing homology class set ξold, add it; S314: Calculate the shortest version f of the current path f * , and make a judgment, if ξold is empty or the new path f * The cost S(f * ) is less than the cost S(f) of the current optimal path f, then the optimal path is updated to f * ; When the loop ends, the obtained path f is the optimal path planning, and the path f is output.

5. The method for planning the movement of a disabled assistive robot according to claim 1, characterized in that: In step S4, the estimated state and dynamic obstacle avoidance specifically include: S41: Sampling the speed data and defining the sampleable speed range V m : V m ={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max ]} (9) Where v min ,v max are the minimum and maximum linear speeds of the assistive robot, ω min ,ω max are the minimum angular velocity and maximum angular velocity of the assistive robot respectively; S42: Re-define the range V of the acceleration that can be sampled d : Where v c ,ω c are the linear velocity and angular velocity of the assistive robot at the current moment, a vmax ,a ωmax They are the maximum linear acceleration and maximum angular acceleration of the assistive robot respectively; S43: Finally, the speed range V is defined under the condition that no collision occurs a : Where dist(v, ω) represents the shortest distance between the corresponding simulation trajectory and the obstacle at the current speed, c is the ultrasonic speed, and t is the time from the ultrasonic wave is emitted to the ultrasonic wave is received. When the robot running sampling speed is within the range of formula (11), the maximum deceleration constraint can be used. The final mobile robot speed sampling space is the intersection of the three speed spaces, that is, V s =V m ∩V d ∩V a ; S44: predicting the trajectory, determining the speed sampling space V s After that, the space is sampled uniformly with a certain sampling interval, that is, the resolution; in the velocity space, the resolution is set for the linear velocity and angular velocity respectively, using E v , E ω The sampling resolution is shown, then the number of sampling speed groups n can be determined as follows: V in the formula high , V low ,ω high ,ω low is the upper and lower limits of the velocity space. The above formula (13) shows that the linear velocity is E v The size takes a value, and the angular velocity is E ω The size takes a value, thus forming a series of speed groups; S45: After sampling a set of (V, ω), the kinematic model of the differentially driven mobile robot is used to predict the trajectory, that is, the position is updated, and then the predicted trajectory is saved within the prediction time for later processing; the specific trajectory prediction is as follows: In the formula, (x, y, θ) represents the robot's position, k represents the sampling time, and Δt represents the sampling interval; S46: After completing the trajectory prediction, the final trajectory evaluation is performed, the speed corresponding to the optimal trajectory is selected as the driving speed, and the obstacle avoidance of the trajectory is executed; the evaluation function G(v,ω) is as follows: G(v,ω)=σ(α·heading(v,ω))+σ(β·dist(v,ω))+σ(γ·velocity(v,ω) (15) Among them, heading(v,ω) is the azimuth evaluation function, which is used to evaluate the error Δθ between the angle between the trajectory endpoint position direction and the target point line generated at the current sampling speed; dist(v,ω) is the distance evaluation function, which represents the closest distance between the corresponding simulated trajectory and the obstacle at the current speed; velocity(v,ω) is the speed evaluation function, which represents the current speed, which is directly expressed by the current linear speed. The larger it is, the faster the speed on the planned trajectory and the higher the evaluation score; α, β and γ are all coefficients of the evaluation function; S47: Normalization processing is smoothing processing, where σ represents normalization, and the specific operation is: Where i represents the i-th simulation trajectory, and n is the total number of all sampled trajectories under the constraint conditions. From the above formulas (15) and (16), a path that avoids obstacles and moves quickly toward the target point is obtained, so that the robot completes the optimal local path planning; S48: Collect data through the speed acceleration odometer to obtain the state vector X k =[x,y,θ,v x ,v y ,ω] T , perform state prediction based on the current state and control input u k (such as linear velocity v and angular velocity ω), and are predicted using the nonlinear state transfer function f: S49: According to the state prediction equation, in order to locally linearize the nonlinear model, it is necessary to calculate the Jacobian matrix F of the state transfer function f with respect to the state x k , and then predict the covariance matrix, the formula is as follows: Where Q is the covariance matrix of process noise; S410: Update phase using sensor measurement value Z k To correct the predicted state; it is necessary to calculate the Jacobian matrix H of the observation function h with respect to the state x k , in calculating the Kalman gain: Where R is the covariance matrix of the measurement noise; S411: Finally, update the state estimation and covariance matrix. The specific formula is as follows: Obtain an estimate of the state; if an exception occurs, trigger path replanning to ensure task continuity.

6. A mobility planning system for a disabled-assisting robot, characterized in that: include: Data acquisition and preprocessing module: used to collect environmental data and user voice commands, filter and denoise the laser point cloud data, and extract key features of obstacles; De-noise and feature encode the voice signal, and convert it into machine-processable instructions, outputting clean point cloud data and parsed text control instructions; Environmental perception and map construction module: used to build a two-dimensional environmental map, update the obstacle status in real time, improve the map accuracy and consistency through closed-loop detection and global optimization technology, output high-resolution raster maps, and mark static and dynamic obstacle areas; Data analysis and path planning module: used to obtain the agent's free space X free , for X free Perform convex segmentation to obtain a polygon list, create a graph structure to store topological information, add nodes and edges, and construct a topological graph X topo , search for global paths in the topology graph and generate safe and smooth trajectories through progressive expansion and path optimization strategies; Output the global optimal path from the starting point to the target point; State estimation and dynamic obstacle avoidance module: used to detect dynamic obstacles in real time and define the safe speed range; predict future trajectories and evaluate priorities, dynamically adjust local paths; fuse multi-sensor data to correct robot posture and ensure path execution accuracy; output local paths and real-time motion control instructions after obstacle avoidance; Execution control and feedback module: used to drive the robot to move along the planned path and monitor the execution status; trigger path re-planning in case of abnormal interruption to ensure task continuity.

7. A mobile planning device for a disabled-assisting robot, characterized in that: It comprises a memory and one or more processors, wherein the memory stores executable codes, and when the one or more processors execute the executable codes, they are used to implement the mobility planning method for a disabled assistive robot as described in any one of claims 1-5.

8. A computer-readable storage medium, characterized in that: A program is stored thereon, and when the program is executed by a processor, the mobility planning method for a disabled-assisting robot described in any one of claims 1-5 is implemented.

9. A computer program product, characterized in that It includes a computer program, which, when executed by a processor, implements the mobility planning method of an assistive robot as described in any one of claims 1 to 5.

Images