Pedestrian navigation system and path planning method based on environmental perception and human kinematics

Abstract

The present invention discloses a pedestrian navigation system and path planning method based on environmental perception and human kinematics. The pedestrian navigation system includes: an environmental information acquisition module, a positioning module, a gait planning module, and a motion planning module. The path planning method includes: establishing a Euclidean signed distance field map through environmental perception, establishing a continuous motion model using a linear inverted pendulum model, generating gait points using linear gait point placement control and solving the LIP model, generating a safe trajectory through a search algorithm, and guiding pedestrians to walk according to the optimal gait trajectory. The pedestrian navigation system and method proposed in the present invention can perceive rich environmental information, process dynamic information in the environment, consider the kinematic constraints of the human body, and plan safe and barrier-free walking trajectories suitable for special people such as the visually impaired and the intellectually disabled, thereby achieving autonomous and safe guidance of pedestrians.

Description

Technical Field

[0001] The present invention relates to the technical field of pedestrian positioning and autonomous navigation, and in particular to a pedestrian navigation system and a path planning method based on environmental perception and human kinematics. Background Art

[0002] For pedestrians with visual impairments, the difficulty in perceiving the environment has seriously affected their normal lives. The visually impaired are in urgent need of tools that can help them perceive the surrounding environment. However, most of the guide tools used by the visually impaired are simple devices such as canes with simple functions. In today's rapidly developing society, simple canes can no longer adapt to the current complex environment. However, biological guide methods represented by guide dogs have never been widely popularized due to the high training costs, long training cycles and lifespan.

[0003] Alzheimer's disease manifests itself as progressively more severe cognitive impairment (memory impairment, learning impairment, attention deficit, spatial cognition, and problem-solving impairment). These symptoms can cause patients to have difficulty locating and navigating, making them prone to wandering. Furthermore, because the symptoms are not obvious, it can be difficult for rescuers to discern the patient's condition and severity, hindering effective assistance.

[0004] Based on the above problems, as current intelligent equipment technology becomes more and more mature, an autonomous navigation system based on environmental perception and human kinematic constraints is a better choice to solve the travel and daily life of special people such as the visually impaired and intellectually disabled.

[0005] Currently, the more comprehensive intelligent navigation systems can be categorized as wearable navigation devices, handheld navigation devices, navigation systems based on smart terminals, and mobile navigation robots. Intelligent navigation systems build upon traditional navigation systems by adding multiple sensors and computing platforms to provide pedestrians with road surface information, but they lack specific navigation information. Wearable navigation devices attach navigation devices to pedestrians' jackets, glasses, backpacks, shoes, and other items. They use audio prompts in the left and right ears to provide direction, but lack a physical sense of traction and are excessively heavy, leading to fatigue. Handheld navigation devices generate vibrations at the pedestrian's wrist and thumb to indicate the distance between two points. The closer the distance, the stronger the vibration. Pedestrians can avoid obstacles by continuously rotating their wrists to scan their surroundings, but this can significantly impact accuracy.

[0006] For autonomous navigation systems to adapt to complex, unknown environments, they must possess the ability to perceive and explore the environment. Current navigation systems are still at a relatively rudimentary stage in their environmental awareness and are unable to process rich, dynamic information. Furthermore, visually impaired individuals must navigate densely populated, dynamically changing environments quickly and smoothly, requiring path planning capabilities that are also a prerequisite for autonomous pedestrian guidance. Current navigation systems and devices lack this capability. Summary of the Invention

[0007] In order to solve the above problems, the present invention proposes a pedestrian navigation system and path planning method based on environmental perception and human kinematics. It can perceive rich environmental information, process dynamic information in the environment, consider the kinematic constraints of the human body, and plan a safe and barrier-free walking plan suitable for visually impaired people, thereby realizing autonomous and safe guidance of pedestrians.

[0008] To achieve the above objectives, the method adopts the following technical solutions: In a first aspect, the present invention provides a pedestrian navigation system based on environmental perception and human kinematics, comprising:

[0009] Environmental information acquisition module, used to obtain three-dimensional information of the environment;

[0010] The environmental perception module is used to semantically model the environment based on three-dimensional information to form a composite environmental map;

[0011] Positioning module, used to obtain the current position of pedestrians and the system;

[0012] The gait planning module is used to analyze the composite map of the environment, obtain suitable walking path plans, and select the best plan from all plans;

[0013] The motion planning module is used to track and control the trajectory obtained by the gait planning module to guide pedestrians to walk.

[0014] In a second aspect, the present invention provides a path planning method based on environmental perception and human kinematics, comprising the following steps:

[0015] Establishing Euclidean signed distance field map through environmental perception;

[0016] A linear inverted pendulum model is used to establish a continuous motion model;

[0017] Generate gait points using linear gait point placement control and solve the LIP model;

[0018] Generate safe trajectories through search algorithms.

[0019] In a third aspect, the present invention provides a navigation system device, including a mechanical structure of the device and an environmental information acquisition module and a positioning module arranged on the device, which meets the requirements of an intelligent navigation system and the steps of the navigation method.

[0020] In a fourth aspect, the present invention provides an electronic device comprising a memory and a processor, and computer instructions stored in the memory and executed on the processor, wherein when the computer instructions are executed by the processor, the steps of the path planning method in the second aspect are completed.

[0021] Compared with the prior art, the present invention has the following beneficial effects:

[0022] (1) The present invention can help special people meet their safe passage needs in home and outdoor environments through the system's autonomous perception of the environment, planning walking plans in complex environments, and intelligent voice and vibration reminder functions. It can also autonomously guide pedestrians to walk and improve the ability of special people such as the visually impaired and intellectually disabled to live independently.

[0023] (2) The present invention can perform path planning in a dynamic environment, allowing pedestrians to travel through a dynamically changing environment with dense crowds. It can also quickly and smoothly plan an optimal walking trajectory. Under the constraints of the human kinematic model, dynamic environment, obstacles, etc., it can achieve autonomous guidance of pedestrians through its own navigation method.

[0024] (3) The present invention has intelligent voice and vibration interaction functions. The voice and vibration interaction module of the present invention can receive service instructions from pedestrians and provide corresponding feedback. When encountering dangerous scenes, the system reminds pedestrians to pay attention to safety through intelligent voice and vibration intensity prompts. To meet the requirements of smooth service, the voice interaction module can complete accurate voice recognition, multi-round voice dialogue and other functions.

[0025] (4) The present invention adopts environmental perception and a navigation method that takes into account the human body dynamics model, which greatly reduces the probability of pedestrian injury and significantly improves the efficiency and safety of special people such as the visually impaired and intellectually disabled when walking indoors and outdoors. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] The accompanying drawings, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0027] Figure 1 This is an overall system block diagram of a multi-sensor based intelligent navigation system according to embodiment 1 of the present invention.

[0028] Figure 2 This is a planning flow chart of a navigation method based on an intelligent navigation system according to embodiment 2 of the present invention.

[0029] Figure 3 This is a schematic diagram of an intelligent navigation system device according to embodiment 3 of the present invention. DETAILED DESCRIPTION

[0030] In order to make the technical solutions, objectives and advantages of the present invention clearer, the present invention is described in detail below with reference to the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, and are not used to limit the scope of the present invention.

[0031] Example 1

[0032] The present invention discloses an intelligent navigation system based on multiple sensors. The overall design diagram of the system is as follows: Figure 1 As shown, specifically including:

[0033] Environmental information acquisition module, used to obtain three-dimensional point cloud information of the environment;

[0034] The Environmental Perception Module observes the environment based on the 3D point cloud information acquired by the Environmental Information Acquisition Module, performs semantic modeling, and forms an incremental grid map. Each grid in this incremental grid map contains semantic information. Based on this semantic information, it identifies areas in the environment where parallel traversal is possible. Dynamic obstacles are then considered, and their motion information is added to the base layer and updated in real time to form a composite map of the environment. Finally, the 3D map is compressed to a bird's-eye view using a Euclidean signed distance field for path and gait planning. The Environmental Information Acquisition Module includes an RGBD depth camera and an RGB camera, which acquire 3D information about the environment using visual data and depth images captured by the cameras.

[0035] The positioning module uses visual motion estimation, inertial navigation motion estimation, and satellite navigation system to fuse the output results of each module to obtain the current position of pedestrians and the system;

[0036] The gait planning module is used to analyze the composite map of the environment, obtain suitable walking path solutions, and select the optimal global path from all solutions. The specific method execution process is described in detail in Example 2.

[0037] The motion planning module and the gait planning module track and control the trajectory obtained to guide the pedestrian to walk.

[0038] Furthermore, the multi-sensor based intelligent navigation system disclosed in this embodiment is also provided with a voice and vibration interaction module, which includes a microphone array, a voice player, a vibration motor and a voice processing module.

[0039] The microphone array, voice player and vibration motor are located at the top of the navigation system device, providing an interface for human-computer voice and touch interaction, receiving the user's voice information and playing the voice reminder instructions generated during movement to realize voice reminders.

[0040] The voice processing module is used to recognize the voice information received by the microphone array and then specify specific navigation tasks based on the voice recognition results. In addition, it will generate voice reminder instructions for perceived danger factors and play them through a voice player, and use a vibration motor to generate vibrations to remind pedestrians, thereby improving the walking safety of special people such as the visually impaired and intellectually disabled.

[0041] The risk factors during the movement are determined by the semantic information of the environment obtained by the environmental information acquisition module and the risk information predicted by dynamic obstacles.

[0042] The navigation system of this embodiment can help the visually impaired, intellectually disabled and other special people meet their safe passage needs in home and outdoor environments through the system's autonomous perception of the environment, planning walking plans in complex environments, force feedback mechanism, intelligent voice and vibration reminders, etc. It can autonomously guide pedestrians to walk and improve the ability of the visually impaired, intellectually disabled and other special people to live independently.

[0043] The navigation system of this embodiment can perform path planning in a dynamic environment, allowing pedestrians to travel through a dynamically changing environment with dense crowds. It can also quickly and smoothly plan an optimal walking trajectory. Under the constraints of the human kinematic model, dynamic environment, obstacles, etc., it can achieve autonomous guidance of pedestrians through its own navigation method.

[0044] The navigation system of this embodiment features intelligent voice and vibration interaction capabilities. This voice and vibration interaction module receives pedestrian service instructions and provides corresponding feedback. In dangerous situations, the system uses intelligent voice and vibration intensity prompts to remind pedestrians to pay attention to safety. To ensure smooth service, the voice interaction module is capable of performing accurate voice recognition and multi-round voice dialogue.

[0045] The navigation system of this embodiment adopts environmental perception and takes into account the navigation method of the human body dynamic model, which greatly reduces the probability of injury to special people such as the visually impaired and intellectually disabled, and significantly improves the efficiency and safety of special people such as the visually impaired and intellectually disabled when walking indoors and outdoors.

[0046] Example 2

[0047] In this embodiment, a gait generation algorithm based on environmental perception and human kinematic constraints is disclosed, and the execution flow chart is as follows: Figure 2 As shown, the steps of the method include:

[0048] Step 1: Establish a Euclidean Signed Distance Field (ESDF) map through environmental perception;

[0049] First, based on the current walking scene, the module observes the environment using 3D point cloud information acquired by the RGBD depth camera, RGB camera, and inertial measurement unit (IMU) visual information and inertial sensors. This generates an incremental grid map. Based on the map's occupancy information, it determines traversable areas within the environment. Furthermore, considering dynamic obstacles, the module adds motion predictions of these obstacles to the feasible map and updates it in real time to form a composite map of the environment. Finally, the 3D map is compressed to a bird's-eye view using the Euclidean Signed Distance Field (ESDF).

[0050] First, we need to construct an occupancy grid map using the three-dimensional point cloud data detected by the sensor. However, due to sensor errors, this approach often results in erroneous obstacle information. Therefore, we often use a probabilistic occupancy grid map. In this probabilistic occupancy grid map, we use the probability of a point being in the free state to represent the probability of its grid being occupied. During motion, the point cloud information read by the sensor is accumulated and the state voxels of the probabilistic occupancy grid map are updated. However, for planning and obstacle avoidance, the occupancy information in the map is not sufficient; information such as obstacle distance and direction is also required. The Euclidean Signed Distance Field (ESDF) is very useful for online motion planning for navigation because it can easily query obstacle distance and gradient information. It generates an ESDF from the grid occupancy map and then uses a ray casting algorithm to calculate the ESDF value for each voxel.

[0051] Step 2: Use the LIP model (LIPM, Linear inverted pendulum model) to establish a continuous motion model;

[0052] The basic principle of the LIP model is: the linear inverted pendulum consists of a mass with a fixed height and two legs with negligible mass. In this model, the mass moves at a constant speed in the horizontal direction, and the length of the legs can be adjusted according to the control strategy. When one leg leaves the ground, the other leg contacts the ground, forming a new support triangle. This situation is called leg switching, which is an instantaneous event and does not affect the speed of the mass. It is assumed that there is no double support stage during the switching process and there is no foot sliding. Through mathematical description: In one step of the continuous phase, the system follows the dynamics of the inverted pendulum, and the equation can be established:

[0053]

[0054] Where g is the acceleration due to gravity, x is the component in the horizontal axis direction, and h is the height of the center of mass of the human body. The following solution can be obtained from this equation:

[0055]

[0056]

[0057] Where t is time,

[0058] The LIP model can be extended to three-dimensional cases because the two-axis model can be decoupled. The model can be written as follows:

[0059]

[0060]

[0061] Step 3: Generate gait points using linear foot placement control (LFPC) and solve the LIP model.

[0062] The next body position is calculated based on the known controller parameters and the body's falling speed. The next foot landing position is predicted based on a linear function that is related to the body's speed. The landing points can be designed separately along the x-axis and y-axis:

[0063]

[0064]

[0065]

[0066]

[0067] in, Represents the placement of the two legs in the x-axis direction, Represents the placement of the two legs in the y-axis direction, a w 、a l and b are controller parameters, v x 、v y are the velocity components in the x and y directions.

[0068] When the walking direction is taken into consideration, let the angle between the walking angle and the positive direction of the x-axis be θ, then the walking gait can be represented by the triple (d l ,d w ,θ) to describe, where d l represents the step size, d w Represents the step width. Therefore, the original controller needs to be modified using the rotation matrix. The modified controller is as follows:

[0069]

[0070]

[0071]

[0072]

[0073] In this controller, we can select the desired step period T and choose b as a parameter. The above equation becomes a three-parameter (a l ,a w ,θ) controller, each parameter determines (d l ,d w ,θ) in the corresponding gait parameters, where d l represents the step size, d w represents the step width, and θ represents the angle between the walking direction and the positive direction of the x-axis. Through the controller, we can easily adjust the step length, step width, and walking direction of each step.

[0074] Step 4: Generate safe trajectories through search algorithms;

[0075] Based on the current starting and target points of the human body, the starting and target points are determined in the ESDF map. Based on the starting and target point poses and obstacle information in the current driving scene, the guidance path is established by using the LFPC extended motion primitives and a human kinematics-constrained search algorithm.

[0076] Based on the established guidance path, the valid neighboring nodes corresponding to the starting point are expanded based on the LFPC loop starting from the starting point. The parent node is determined according to the rules in the valid nodes to be expanded, and the valid neighboring nodes corresponding to the parent node are expanded again, and so on, and the neighboring node expansion is performed cyclically. After each determination of the parent node, the human body center of mass trajectory is calculated using LFPC as a new node to be expanded outward, and added to the set of nodes to be expanded. The length of the path is determined by calculating the cost function of the LFPC trajectory. The loop is performed until the shortest valid path exists between the current valid neighboring node and the target point posture. It can be determined that the path planning is successful. This method outputs all path states (paths on the XY-YAW plane) generated by LFPC from the starting point to the target point, and the planned path is used as the global reference path.

[0077] A collision check is performed on the target starting point posture and the target point posture to determine whether there is an obstacle between the starting point posture and the target point posture that prevents the pedestrian from walking, and a collision detection result is obtained. If the collision detection result indicates that there is no collision between the starting point posture and the target point posture, the starting point posture and the target point posture are determined to be valid.

[0078] Example 3

[0079] In this embodiment, a navigation system device is disclosed, including a mechanical structure of the device and an environmental information acquisition module and a positioning module provided on the device, which meet the requirements of the navigation system and the steps of the navigation method. The specific steps are as follows:

[0080] like Figure 3 As shown, the navigation system device mainly includes a loading platform, an environmental information acquisition module and a positioning module arranged on the device.

[0081] Among them, the loading platform is composed of 3D printed structural parts.

[0082] The environmental information acquisition module includes a depth camera 1, which is located at the head of the loading platform. A screw passes through the through hole in the center of the front loading plate and connects to the threaded hole at the bottom of the depth camera to fix the depth camera on the front loading plate to obtain image information of the environment;

[0083] The positioning module includes a satellite navigation system and an inertial measurement unit. The inertial measurement unit is used to obtain the speed, acceleration, and direction of the device's movement, and can also be used for positioning. The satellite navigation system is used to obtain the device's positioning;

[0084] And a high-performance edge computing platform 2, and the electronic device shown in Example 4.

[0085] Example 4

[0086] In this embodiment, an electronic device is disclosed, including a memory and a processor, and computer instructions stored in the memory and executed on the processor, to complete the steps of the navigation method based on the intelligent navigation system disclosed in Example 2.

[0087] These computer program instructions can be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce computer-implemented processing, whereby the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one or more processes of Example 2.

[0088] The above-described embodiments are only specific implementation methods of the present invention, which are used to illustrate the technical solutions of the present invention, rather than to limit them. The protection scope of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the above-described embodiments, it should be understood by those skilled in the art that any person skilled in the art can still modify the technical solutions described in the above-described embodiments within the technical scope disclosed by the present invention, or perform equivalent replacements on some of the technical features thereof. However, these modifications, changes, or replacements do not deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims

1. A pedestrian navigation path planning method based on environmental perception and human kinematics, characterized in that: The following steps are involved: A Euclidean signed distance field map is established through environmental perception. Specifically, the following steps are used: Based on the visual and inertial information provided by the RGBD depth camera, RGB camera, and inertial measurement unit, and the 3D point cloud information obtained by the environmental information acquisition module, an incremental grid map is formed. The passable areas in the environment are obtained based on the occupancy information of the map. Furthermore, considering dynamic obstacles, the motion prediction information of dynamic obstacles is added to the feasible map and updated in real time to form a composite map of the environment. Finally, the 3D map is compressed to a bird's-eye view using the Euclidean signed distance field. A linear inverted pendulum model is used to establish a continuous motion model; Generate gait points using linear gait point placement control and solve the LIP model; Generate safe trajectories through search algorithms.

2. The method according to claim 1, characterized in that The LIP model is used to establish a continuous motion model, as follows: During one step of the continuous phase, the system follows the dynamics of an inverted pendulum, establishing the equation: Where g is the acceleration due to gravity, x is the component in the horizontal axis direction, and h is the height of the center of mass of the human body; the solution of this equation is as follows: Where t is time, The LIP model is extended to three dimensions and is expressed as follows:

3. The method according to claim 2, characterized in that Use LFPC to generate gait points and solve the LIP model as follows: Based on the known controller parameters and the body's falling speed, a linear function is used to predict the next foot landing position. This function is related to the body's speed. The landing points along the x-axis and y-axis are designed separately: in, Represents the placement of the two legs in the x-axis direction, Represents the placement of the two legs in the y-axis direction, a w 、a l and b are controller parameters, v x 、v y are the velocity components in the x and y directions; When the walking direction is taken into account, let the angle between the walking angle and the positive direction of the x-axis be θ, and the walking gait is composed of the triplet (d l ,d w ,θ) to describe, where d l represents the step size, d w Represents the step width. The controller is modified using the rotation matrix. The modified controller is as follows: In this controller, the desired step period T is selected and b is selected as a parameter; the above equation becomes a three-parameter (a l ,a w ,θ) controller, each parameter determines (d l ,d w ,θ) corresponding gait parameters; through the controller, adjust the step length, step width and walking direction of each step.

4. The method according to claim 3, characterized in that Generate a safe trajectory through a search algorithm. The specific method is as follows: A safe trajectory is generated through a search algorithm. The starting and target points are determined in the ESDF map based on the current starting and target points of the human body. Based on the starting and target poses and obstacle information in the current driving scene, the LFPC motion primitives are extended and a human kinematics-constrained search algorithm is used to establish a guidance path. The guidance path is used as a global reference path, and the global reference path is used to provide a reference during the pedestrian walking guidance process.

5. A pedestrian navigation system based on environmental perception and human kinematics, characterized in that: For implementing the method described in any one of claims 1 to 4, the system comprises: Environmental information acquisition module, used to obtain three-dimensional information of the environment; The environmental perception module is used to semantically model the environment based on three-dimensional information to form a composite environmental map; Positioning module, used to obtain the current position of pedestrians and the system; The gait planning module is used to analyze the composite map of the environment, obtain suitable walking path plans, and select the best plan from all plans; The motion planning module is used to track and control the trajectory obtained by the gait planning module to guide pedestrians to walk.

6. The pedestrian navigation system based on environmental perception and human kinematics according to claim 5, characterized in that: It also includes a voice and vibration interaction module for obtaining voice information, recognizing the voice information, and determining the system's navigation task based on the recognition results of the voice information.

7. The pedestrian navigation system based on environmental perception and human kinematics according to claim 6, characterized in that: The voice and vibration interaction module is also used to generate voice and vibration alerts to pedestrians regarding dangerous information that appears during navigation.

8. A pedestrian navigation device based on the pedestrian navigation system according to claim 5, characterized in that: It includes a mechanical structure of the device and an environmental information acquisition module and a positioning module arranged on the device.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the steps of the method according to any one of claims 1 to 4 are implemented.

Images