Wheelchair path planning method based on grid map and eye-controlled wheelchair navigation system

Abstract

The invention discloses a wheelchair path planning method based on a grid map and an eye-controlled wheelchair navigation system. Belongs to the technical field of intelligent path planning and particularly relates to the technical field of wheelchair path planning based on a grid map. The method overcomes the defects that according to an existing common path planning method, when the road condition is complex, the path planning quality is low, and the path is too close to an obstacle. An original cost function is formed by respectively weighting existing path cost, heuristic cost and safety cost, the method provides a dynamic weighting strategy, and comprises the following steps: firstly, weighting a w2 heuristic cost weight according to a current distance; specifically, in the initial stage of searching, we hope to converge to the position of a target point as quickly as possible, namely, increase the weight value of w2, and decrease the value of w2 as the w2 approaches to the target point, and at the moment, better path quality is required to reach the target point. In addition, the value of w3 is dynamically adjusted according to the number of obstacles in a certain range where the current node is located, so that a safer path is exchanged in a complex environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent path planning, and in particular to the technical field of wheelchair path planning based on grid maps. Background Art

[0002] Path planning refers to planning an optimal path or one that meets specific requirements through certain intelligent methods based on certain strategies and conditions. Path planning can be applied to various fields, such as autonomous driving, logistics distribution, aviation navigation, etc. In path planning, the following factors are usually considered: map data, starting point and end point, conditions and constraints, objective function and dynamic adjustment. The commonly used path planning methods currently have defects such as low path planning quality when the road conditions are complex and the path being too close to obstacles. Summary of the invention

[0003] The present invention aims to solve the defects of the currently commonly used path planning methods, such as low path planning quality when the road conditions are complicated and the path being too close to obstacles.

[0004] In order to solve the above problems, the present invention provides a wheelchair path planning method based on a grid map, which is based on the Hybrid A* path planning method and improves the Hybrid A* path planning method. The path planning method is specifically as follows: S1. Perform initialization and parameter setting, import the grid map, input the starting node location information, input the target node location information; and set relevant parameters and control commands, including setting data structure, sampling control command combination, etc.; S2, the nodes expand forward, starting from the starting node, and calculating the next set of node position information based on the current node; S3. Calculate the node with the lowest cost in the next set of nodes using the cost function and dynamic weighting method: S4. Select the node with the lowest cost in the next group of nodes as the new starting node, and determine whether the unconstrained obstacle heuristic distance h1 between the new starting node and the target node is less than three times the wheelchair wheelbase. If not, return to step S2 to continue the node forward expansion; if yes, it is considered to have reached the vicinity of the target node. At this time, the RS curve is used to connect to the target node, and it is determined whether there is an obstacle-free path in the generated path. If not, return to step S2 to continue the node forward expansion. If so, the cost function of the RS curve is used to select the lowest cost path; S5. Smoothing the generated path through a Bezier curve to obtain a smooth path, and determining whether a collision occurs on the smooth path. If no collision occurs, the final path is output.

[0005] The present invention also provides an eye-controlled wheelchair navigation system, the system comprising a Hololens 2 device and a smart wheelchair, and the construction process of the eye-controlled wheelchair navigation system is specifically as follows: S21. Perform UI design, create a project in Unity, and design holographic icons in the UI interface, including forward icon, reverse icon, left turn icon, right turn icon, and map icon. The rest of the area is the real environment. S22, fix the holographic icon, and fix the holographic icon somewhere in the field of vision of the person wearing the Hololens 2 device through the solver function in the official development kit MRTK of the Hololens 2 device; S23. Import the ROS# plug-in in Unity and configure it to connect to the ROS Master. Use the ROS# plug-in to create two ROS Nodes, one of which is responsible for monitoring the events in the forward icon, reverse icon, left turn icon, and right turn icon, and converting these events into ROS messages, which are sent to the control topic of the smart wheelchair ROS system to control the forward, reverse, left turn, and right turn of the wheelchair; the other ROS Node is responsible for monitoring the events in the map icon, and converting these events into ROS messages, which are sent to the target node topic of the smart wheelchair ROS system to control the wheelchair to navigate to the target node; S24, updating the navigation stack for the smart wheelchair, wherein the navigation stack has a built-in wheelchair path planning method based on the grid map as described above, so that the smart wheelchair plans a path according to the target node; S25. Add eye-movement interaction related components and event processors to the holographic icon, so that when the person wearing the Hololens2 device stares at the holographic icon, the event processor will call the corresponding event in the ROS Node and send a control command to make the smart wheelchair perform the corresponding operation; S26. Install and use ROS-Bridge in the smart wheelchair ROS system, so that the program in Unity can communicate with the ROS node through the WebSocket protocol and control the smart wheelchair through the Hololens2 device.

[0006] The beneficial effects of the method of the present invention are: The heuristic function of the original hybrid A* method is h=max{H1,H2}, and H1 is to estimate the path length from the current node to the end point by the Dijtstra algorithm under the condition of considering obstacles but not considering motion constraints. The Dijtstra algorithm is a shortest path search algorithm for simple maps. The present invention replaces it with the JPS algorithm, which is suitable for more complex maps and greatly improves the search speed.

[0007] The original cost function is composed of the existing path cost, heuristic cost and Voronoi safety cost, which are weighted separately, and the formula is total cost F=w1*g+w2*h+w3*v. The present invention proposes a dynamic weighting strategy. First, the heuristic cost weight of w2 is weighted according to the current distance. Specifically, at the beginning of the search, we hope that the search converges to the position of the target point as quickly as possible, that is, the weight value of w2 is increased. As it approaches the target point, the value of w2 is reduced. At this time, better path quality is required to reach the target point. In addition, the value of w3 is dynamically adjusted according to the number of obstacles within a certain range of the current node to obtain a safer path in a complex environment.

[0008] The hybrid A* method is divided into two parts: forward search and analytical expansion. The analytical expansion connects the current node to the target node through the Reed-Shepp curve (RS curve). This curve can reduce the search volume of the overall algorithm and improve efficiency. However, the path it plans may be too close to obstacles, resulting in a risk of collision when the actual wheelchair moves along the path. In addition, due to the lack of a selection mechanism, the curve will plan a long-distance reversing path in some cases, which is not friendly to wheelchair users. In response to these two situations, the present invention adds a cost function to the RS curve, and by increasing the safety cost and reversing cost, the generated path can reduce the risk of collision and reversing.

[0009] Traditional Bezier curves evenly sample path points as feature points according to time or path length to smooth the path. This method lacks targeted smoothing of key parts because it evenly smoothes the entire path. The present invention intelligently selects the part with larger curvature on the path and uses Bezier curves to smooth the key path, which has higher adaptability. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] Figure 1 This is a flow chart of a wheelchair path planning method based on a grid map in an embodiment of the present invention; Figure 2 This is a schematic diagram of the process of building an eye-controlled wheelchair navigation system in an embodiment of the present invention; Figure 3 Schematic diagram of the UI interface in an embodiment of the present invention. DETAILED DESCRIPTION

[0011] The technical solution of the present invention will be described clearly and completely below in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the protection scope of the present invention.

[0012] Embodiment 1, This embodiment provides a wheelchair path planning method based on a grid map, and its flow chart is as follows: Figure 1 As shown, the path planning method is specifically as follows: S1. Import the grid map, input the starting node location information, and input the target node location information; S2, the nodes expand forward, starting from the starting node, and calculating the next set of node position information based on the current node; S3. Calculate the node with the lowest cost in the next set of nodes using the cost function and dynamic weighting method: S4. Select the node with the lowest cost in the next group of nodes as the new starting node, and determine whether the unconstrained obstacle heuristic distance h1 between the new starting node and the target node is less than three times the wheelchair wheelbase. If not, return to step S2 to continue the node forward expansion; if yes, it is considered to have reached the vicinity of the target node. At this time, the RS curve is used to connect to the target node, and it is determined whether there is an obstacle-free path in the generated path. If not, return to step S2 to continue the node forward expansion. If so, the cost function of the RS curve is used to select the lowest cost path; S5. Smoothing the generated path through a Bezier curve to obtain a smooth path, and determining whether a collision occurs on the smooth path. If no collision occurs, the final path is output.

[0013] The unconstrained obstacle heuristic distance h1 is specifically: the distance from the starting node to the target node calculated using the JPS jumping point search algorithm under obstacles and without motion constraints.

[0014] The incoming starting node location information is S start (x start ,y start ,θ start ), x start and start Represents the position of the wheelchair mass center at the starting node in the plane coordinate system, θ start Indicates the wheelchair heading angle of the starting node; the position information of the incoming target node is P goal (x goal ,y goal ,θ goal ), x goal and goal represents the position of the target node wheelchair mass center in the plane coordinate system, θ goal Indicates the wheelchair heading angle at the target node position.

[0015] The calculation of the next set of node position information based on the current node is as follows: new=V / L*tanδ to obtain the wheelchair heading angle of the next set of node positions, where V represents the linear speed of the wheelchair, L represents the wheelbase of the wheelchair, and δ represents the sampled front wheel steering angle sequence, including several sampled values; each sampled value of δ corresponds to the position x of a node in the plane coordinate system. new and new , x new =V *cosθ new ,y new =V * sinθ new .

[0016] The cost function is: F=w1*g+w2*h+w3*v, where g represents the path cost, whose value is the distance from the starting point to the current node, h represents the heuristic cost, whose value is max{h1,h2}, where h2 is the distance from the starting node to the target node calculated using the RS curve under the constraint of barrier-free motion, and v is the safety cost generated using the Voronoi diagram. ,in, and are constants, representing the potential energy decrease rate and the control range of potential energy, respectively. is the distance from the current node to the nearest obstacle, is the shortest distance from the current node to the edge of the Voronoi diagram; w1, w2 and w3 are the path cost weight, heuristic cost weight and safety cost weight, respectively, which are initialized to 0.5, 0.5 and 0.5.

[0017] The dynamic weighting method is specifically as follows: setting a distance threshold d and a safety threshold m; if the distance between the current node and the target node is greater than the distance threshold d, then increasing the heuristic cost weight w2 to 0.8 to speed up the search process until the distance between the current node and the target node gradually decreases to below the distance threshold d, and then lowering the heuristic cost weight w2 to 0.5, so that the generation of the path balances the consideration of quality and speed, and calculates the proportion of obstacles within a circle with a radius of 3L around the current node. If it exceeds the safety threshold m, it means that the current node is in a complex environment. At this time, the safety cost weight w3 is increased to 0.8 to ensure that this section of the path has a sufficient safety distance from obstacles, until the proportion of obstacles within a circle with a radius of 3L around the current node is lower than the safety threshold m. At this time, the safety cost weight w3 is lowered to 0.5, so that the generation of the path balances the consideration of quality and safety.

[0018] The cost function of the RS curve is given by C length +C obstacle +C reverse Composition, among which, , n The number of path segments output for the RS curve, is the number of path points on the current path segment, ( , ) indicates the i The first path segment j coordinate points, ( , ) indicates the i The first path segment j+ 1 coordinate point; ,in For the i The first path segment j The distance from a path point to the nearest obstacle, The preset safety distance; ,in, p is the cost coefficient of the reversing operation, Output for RS curve n The number of reverse path segments in the path segment.

[0019] The method of smoothing the generated path through the Bezier curve is as follows: setting the curvature ,pass Find the path point whose curvature is higher than the preset curvature threshold S, and smooth the curve part where it is located by Bezier curve; where ( , ) represents the coordinates of the current point position, and the coordinates of the previous point position are ( , ), and the coordinates of the next point are ( , ).

[0020] The generated path is formed by connecting the path generated by forward expansion of the node and the RS curve minimum cost path.

[0021] Embodiment 2, This embodiment provides an eye-controlled wheelchair navigation system. The construction process of the eye-controlled wheelchair navigation system is as follows: Figure 2 As shown, specifically: S1. Perform UI design, create a project in Unity, and design holographic icons in the UI interface, including forward icon, reverse icon, left turn icon, right turn icon, and map icon. The rest of the area is the real environment; Figure 3 The following is a schematic diagram of the UI interface; S2. Fix the holographic icon. Use the solver function in the official development kit MRTK of the Hololens 2 device to fix the holographic icon somewhere in the field of vision of the person wearing the Hololens 2 device. S3. Import the ROS# plug-in in Unity and configure it to connect to the ROS Master. Use the ROS# plug-in to create two ROS Nodes, one of which is responsible for listening to events in the forward icon, reverse icon, left turn icon, and right turn icon, and converting these events into ROS messages, which are sent to the control topic of the smart wheelchair ROS system to control the wheelchair's forward, reverse, left turn, and right turn; the other ROS Node is responsible for listening to events in the map icon, and converting these events into ROS messages, which are sent to the target node topic of the smart wheelchair ROS system to control the wheelchair to navigate to the target node; S4. Update the navigation stack for the smart wheelchair, wherein the navigation stack has a built-in wheelchair path planning method based on a grid map as described in Example 1, so that the smart wheelchair plans a path according to the target node; S5. Add eye-movement interaction related components and event processors to the holographic icon, so that when the person wearing the Hololens2 device stares at the holographic icon, the event processor will call the corresponding event in the ROS Node and send a control command to make the smart wheelchair perform the corresponding operation; S6. Install and use ROS-Bridge in the smart wheelchair ROS system, so that the program in Unity can communicate with the ROS node through the WebSocket protocol and control the smart wheelchair through the Hololens2 device.

Claims

1. A wheelchair path planning method based on a grid map, characterized in that: The method is based on the Hybrid A* path planning method and improves the Hybrid A* path planning method. The path planning method is specifically as follows: S1. Import the grid map, input the starting node location information, and input the target node location information; S2, the nodes expand forward, starting from the starting node, and calculating the next set of node position information based on the current node; S3. Calculate the node with the lowest cost in the next set of nodes using the cost function and dynamic weighting method: S4. Select the node with the lowest cost in the next group of nodes as the new starting node, and determine whether the unconstrained obstacle heuristic distance h1 between the new starting node and the target node is less than three times the wheelchair wheelbase. If not, return to step S2 to continue the node forward expansion; if yes, it is considered to have reached the vicinity of the target node. At this time, the RS curve is used to connect to the target node, and it is determined whether there is an obstacle-free path in the generated path. If not, return to step S2 to continue the node forward expansion. If so, the cost function of the RS curve is used to select the lowest cost path; S5. Smoothing the generated path through a Bezier curve to obtain a smooth path, and determining whether a collision occurs on the smooth path. If no collision occurs, the final path is output.

2. The wheelchair path planning method based on grid map according to claim 1, characterized in that: The unconstrained obstacle heuristic distance h1 is specifically: the distance from the starting node to the target node calculated using the JPS jumping point search algorithm under obstacles and without motion constraints.

3. The wheelchair path planning method based on grid map according to claim 2, characterized in that: The incoming starting node location information is S start (x start ,y start ,θ start ), x start and start Represents the position of the wheelchair mass center at the starting node in the plane coordinate system, θ start Indicates the wheelchair heading angle of the starting node; the position information of the incoming target node is P goal (x goal ,y goal ,θ goal ), x goal and goal represents the position of the target node wheelchair mass center in the plane coordinate system, θ goal Indicates the wheelchair heading angle at the target node position.

4. The wheelchair path planning method based on grid map according to claim 3, characterized in that: The calculation of the next set of node position information based on the current node is as follows: new =V / L*tanδ to obtain the wheelchair heading angle of the next set of node positions, where V represents the linear speed of the wheelchair, L represents the wheelbase of the wheelchair, and δ represents the sampled front wheel steering angle sequence, including several sampled values; each sampled value of δ corresponds to the position x of a node in the plane coordinate system. new and new , x new =V *cosθ new ,y new =V * sinθ new .

5. The wheelchair path planning method based on grid map according to claim 4, characterized in that: The cost function is: F=w1*g+w2*h+w3*v, where g represents the path cost, whose value is the distance from the starting point to the current node, h represents the heuristic cost, whose value is max{h1,h2}, where h2 is the distance from the starting node to the target node calculated using the RS curve under the constraint of barrier-free motion, and v is the safety cost generated using the Voronoi diagram. ,in, and are constants, representing the potential energy decrease rate and the control range of potential energy, respectively. is the distance from the current node to the nearest obstacle, is the shortest distance from the current node to the edge of the Voronoi diagram; w1, w2 and w3 are the path cost weight, heuristic cost weight and safety cost weight, respectively, which are initialized to 0.5, 0.5 and 0.

5.

6. The wheelchair path planning method based on grid map according to claim 5, characterized in that: The dynamic weighting method is specifically as follows: setting a distance threshold d and a safety threshold m; if the distance between the current node and the target node is greater than the distance threshold d, then increasing the heuristic cost weight w2 to 0.8, until the distance between the current node and the target node gradually decreases to below the distance threshold d, and then lowering the heuristic cost weight w2 to 0.5, and calculating the proportion of obstacles within a circle with a radius of 3L around the current node. If it exceeds the safety threshold m, it means that the current node is in a complex environment. At this time, the safety cost weight w3 is increased to 0.8, until the proportion of obstacles within a circle with a radius of 3L around the current node is lower than the safety threshold m, and then the safety cost weight w3 is lowered to 0.

5.

7. The wheelchair path planning method based on grid map according to claim 6, characterized in that: The cost function of the RS curve is given by C length +C obstacle +C reverse Composition, among which, , n The number of path segments output for the RS curve, is the number of path points on the current path segment, ( , ) indicates the i The first path segment j coordinate points, ( , ) indicates the i The first path segment j+ 1 coordinate point; ,in For the i The first path segment j The distance from a path point to the nearest obstacle, The preset safety distance; ,in, p is the cost coefficient of the reversing operation, Output for RS curve n The number of reverse path segments in the path segment.

8. The wheelchair path planning method based on grid map according to claim 7, characterized in that: The method of smoothing the generated path through the Bezier curve is as follows: setting the curvature ,pass Find the path point whose curvature is higher than the preset curvature threshold S, and smooth the curve part where it is located by Bezier curve; where ( , ) represents the coordinates of the current point position, and the coordinates of the previous point position are ( , ), and the coordinates of the next point are ( , ).

9. Eye-controlled wheelchair navigation system, characterized in that: The system includes a Hololens 2 device and a smart wheelchair. The construction process of the eye-controlled wheelchair navigation system is as follows: S21. Perform UI design, create a project in Unity, and design holographic icons in the UI interface, including forward icon, reverse icon, left turn icon, right turn icon, and map icon. The rest of the area is the real environment. S22, fix the holographic icon, and fix the holographic icon somewhere in the field of vision of the person wearing the Hololens 2 device through the solver function in the official development kit MRTK of the Hololens 2 device; S23. Import the ROS# plug-in in Unity and configure it to connect to the ROS Master. Use the ROS# plug-in to create two ROS Nodes. One of the ROS Nodes is responsible for listening to the events in the forward icon, reverse icon, left turn icon, and right turn icon, and converting these events into ROS messages and sending them to the control topic of the smart wheelchair ROS system to control the forward, reverse, left turn, and right turn of the wheelchair. Another ROS Node is responsible for listening to events in the map icon and converting these events into ROS messages, which are sent to the target node topic of the smart wheelchair ROS system to control the wheelchair to navigate to the target node. S24, updating a navigation stack for the smart wheelchair, wherein the navigation stack has a built-in wheelchair path planning method based on a grid map as claimed in claim 8, so that the smart wheelchair plans a path according to the target node; S25. Add eye-movement interaction related components and event processors to the holographic icon, so that when the person wearing the Hololens2 device stares at the holographic icon, the event processor will call the corresponding event in the ROS Node and send a control command to make the smart wheelchair perform the corresponding operation; S26. Install and use ROS-Bridge in the smart wheelchair ROS system, so that the program in Unity can communicate with the ROS node through the WebSocket protocol and control the smart wheelchair through the Hololens2 device.

Images