A robot navigation method and system based on dynamic social space cost

Abstract

The application relates to a robot navigation method and system based on a dynamic social space cost, and belongs to the technical field of robot navigation. The method solves the problem that the existing social space cost model in the prior art makes the navigation behavior of a robot not satisfy social norms and increases unnecessary social space of the robot. The method comprises the following steps: performing pedestrian detection by using a 2D laser radar and an RGB-D camera, and calculating the pose and speed of the pedestrians; calculating the mood value of the robot at the current moment based on an emotion model; constructing a dynamic social space cost model, adjusting the parameters of the dynamic social space cost model based on the mood value and the relative speed of the pedestrians and the robot; calculating the cost value of each grid in a social space cost map layer based on the adjusted dynamic social space cost model; integrating the cost value of the social space cost map layer into a cost map; and planning a robot moving path based on the updated cost map.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot navigation technology, and in particular to a robot navigation method and system based on dynamic social space cost. BACKGROUND

[0002] In a social environment, human navigation and obstacle avoidance not only consider the shortest path, but also include the influence of social culture and social norms. For example, to describe how to manage one's own space when communicating, American anthropologist Hall proposed the concept of space distance and four different distance representations: intimate distance (<0.45m), private distance (>0.45m), social area (>1.2m), and general distance (>3.6m). On this basis, the field gradually developed four space relationship models: the concentric circle model, the egg-shaped model, the concentric ellipse model, and the one-end dominant model.

[0003] When a mobile robot is navigated in a factory environment, it only needs to maintain a distance from the object to generate the shortest path and quickly reach the specified pose. As more and more mobile robots enter a human-robot coexistence environment, if humans and objects in the environment are treated equally, the behavior of the robot will not meet the social norms of humans, and the human-robot interaction experience will be reduced.

[0004] In order to make the robot meet the implicit constraints of human space distance in a human-robot coexistence environment, many scholars have established basic cost models representing human social space, and then used factors such as relative speed to achieve dynamic changes in the model.

[0005] Most current research uses a two-Gaussian mixture function to represent the basic cost model. Although the two-Gaussian mixture function can represent the concentric circle, concentric ellipse, and egg-shaped model, this function cannot represent the one-end dominant model. The reason for this is that the function is still symmetric along a certain coordinate axis and cannot meet the requirement of the one-end dominant model for asymmetry in two directions, thereby making the navigation behavior of the robot not meet the social norms and increasing unnecessary social space for the robot. SUMMARY

[0006] In view of the above analysis, the embodiments of the present application aim to provide a robot navigation method and system based on dynamic social space cost, to solve the problem that the existing social space cost model makes the navigation behavior of the robot not meet the social norms and increases unnecessary social space for the robot.

[0007] In one aspect, the embodiments of the present application provide a robot navigation method based on dynamic social space cost, comprising the following steps:

[0008] A 2D laser radar and an RGB-D camera are used for pedestrian detection, and the pose and speed of the pedestrian are calculated.

[0009] calculating a mood value of the robot at the current time based on the emotion model;

[0010] constructing a dynamic social space cost model, adjusting parameters of the dynamic social space cost model based on the mood value and a relative speed between the pedestrian and the robot, calculating a cost value of each grid in a social space cost map layer based on the adjusted dynamic social space cost model, and integrating the cost value of the social space cost map layer into a cost map;

[0011] planning a moving path of the robot based on the updated cost map.

[0012] Further improvement based on the above method, the dynamic social space cost model is:

[0013] θ = atan2((y p -y),(x p -x))-θ p ,

[0014]

[0015]

[0016] c(x,y) = Aexp(-f x -f y ), where (σ f ,σ l ,σ b ,σ r ) are standard deviations of Gaussian functions in front, left, back and right of the pedestrian respectively, (x p ,y p ,θ p ) represents a pose of the pedestrian, c(x,y) represents a cost value at the position (x,y), atan2() represents an inverse tangent function, a,b,fx,fy represent intermediate parameters, and θ represents an angle of the point (x,y) in the pedestrian coordinate system.

[0017] Further, the adjusting of the parameters of the dynamic social space cost model based on the mood value and the relative speed between the pedestrian and the robot comprises:

[0018] calculating an action coefficient based on the mood value and the relative speed between the pedestrian and the robot;

[0019]

[0020] (σ f ,σ l ,σ b ,σ r ) are calculated by the following formula:

[0021]

[0022] wherein w f , w b , w l , w r are the action coefficients of the standard deviations of the Gaussian functions of the front, back, left and right of the pedestrian respectively, σ f , σ b , σ l , σ r are the preset parameters of the Gaussian functions of the front, back, left and right of the pedestrian respectively, w m is the mood action coefficient, and m is the mood value of the robot, is the speed of the pedestrian in the map coordinate system, is the speed of the robot in the map coordinate system.

[0023] Further, the mood value is calculated by the following steps:

[0024] If a stimulating event occurs at the current time, the average value method of stimulation is used to calculate the update of the mood:

[0025]

[0026] wherein Δm is the change amount of the mood in each mood period, n p is the number of mood update periods, w is the number of emotions activated by the stimulating event, k ei is the action coefficient of the i-th emotion on the mood, e ci is the emotion category; a i (t) is the stimulating intensity of the stimulating event on the i-th emotion, and m(t) is the mood value at time t;

[0027] If no stimulating event occurs at the current time, the mood value is calculated according to the following natural decay formula:

[0028] m(t)=m t1 exp(-d m (t-t1))+b,

[0029] wherein t1 is the time of completing the mood update, m t1 is the mood value at time t1, d m is the decay factor of the mood, and b is the bias value of the mood.

[0030] Further, a 2D laser radar and an RGB-D camera are used for pedestrian detection, and the pose and speed of the pedestrian are calculated, including:

[0031] Determine first leg data of the pedestrian based on a grid map and a 2D laser radar;

[0032] Determine second leg data and body joint information of the pedestrian based on an RGB-D camera;

[0033] Determine pedestrian position information and pedestrian speed information based on Kalman filtering according to the first leg data and the second leg data;

[0034] Determine a pedestrian posture angle according to the body joint information.

[0035] Further, the first leg data is determined based on the grid map and the 2D laser radar, comprising:

[0036] Collect leg laser data by the 2D laser radar;

[0037] Determine at least one cluster point set from the leg laser data according to a preset laser data gradient and a preset leg diameter, the cluster point set comprising at least one laser point;

[0038] Determine obstacle position information according to the grid map;

[0039] For each cluster point set, determine the distance between each laser point in the cluster point set and the obstacle according to the obstacle position information, and calculate the proportion of laser points in the corresponding cluster point set whose distance is less than a first preset value;

[0040] Delete the cluster point set whose proportion is greater than a second preset value to obtain the first leg data.

[0041] Further, the second leg data and the body joint information are determined based on the RGB-D camera, comprising:

[0042] Determine a skeleton point of an observation target and a pixel coordinate of the skeleton point based on a picture taken by the RGB-D camera;

[0043] Determine a three-dimensional coordinate of the skeleton point according to the pixel coordinate of the skeleton point, a depth image and a back projection model of the RGB-D camera;

[0044] Determine position information of a leg skeleton point and the body joint information according to the three-dimensional coordinate of the skeleton point;

[0045] Generate second leg data according to the position information of the leg skeleton point.

[0046] Further, the body joint information comprises shoulder joint position information and hip joint position information.

[0047] The human joint information is used to determine a pedestrian posture angle, which includes:

[0048] The shoulder joint position information is used to determine a shoulder joint posture angle, the hip joint position information is used to determine a hip joint posture angle, and the shoulder joint posture angle and the hip joint posture angle are used to determine the pedestrian posture angle.

[0049] Further, the shoulder joint posture angle and the hip joint posture angle are used to determine the pedestrian posture angle, which includes:

[0050]

[0051] wherein θ shoulder is used to represent the joint posture angle, θ hip is used to represent the hip joint posture angle, θ' leg is used to represent the human leg posture angle, (x Rshoulder , y Rshoulder ) is right shoulder joint position information, (x Lshouler , y Lshouler ) is left shoulder joint position information, (x Rhip , y Rhip ) is right hip joint position information, and (x Lhip , y Lhip ) is left hip joint position information.

[0052] In another aspect, an embodiment of the present application provides a robot navigation system based on dynamic social space cost, which includes the following modules:

[0053] A pedestrian detection module is used to detect pedestrians by using a 2D laser radar and an RGB-D camera, and to calculate the pose and speed of the pedestrians.

[0054] A mood calculation module is used to calculate the mood value of the robot at the current time based on an emotion model.

[0055] A cost map updating module is used to construct a dynamic social space cost model, to adjust the parameters of the dynamic social space cost model based on the mood value and the relative speed between the pedestrians and the robot, to calculate the cost value of each grid in a social space cost map layer based on the adjusted dynamic social space cost model, and to integrate the cost value of the social space cost map layer into a cost map.

[0056] A navigation module is used to plan a moving path of the robot based on the updated cost map.

[0057] Compared with the prior art, the application realizes a basic cost model of four spatial relationship models of a concentric circle model, an egg-shaped model, a concentric ellipse model and a one-end dominant model by adopting a four-Gaussian mixed function, not only makes the navigation behavior of the robot meet social norms, but also can reduce unnecessary social space of the robot and improve the navigation efficiency of the robot. Through the adjustment of the relative speed and emotion on the basic model, the dynamic change of the basic social cost model is realized, so that the navigation behavior of the robot can more comply with the social norms of human beings compared with the method of regarding human beings as objects.

[0058] By fusing 2D laser radar and RGB-D camera for pedestrian detection and tracking, based on Kalman filtering algorithm, the first person leg data obtained by 2D laser radar and the second person leg data obtained by RGB-D camera are verified with each other, the problem of missed detection or false detection of 2D laser radar is solved to a certain extent by using RGB-D, meanwhile, the visual blind area of RGB-D camera is supplemented by 2D laser sensor, the accuracy is improved and the detection range is expanded. By comparing the detection target corresponding to the state quantity of the next moment and the detection target corresponding to the observation quantity, it is determined in real time whether the detection target has left the monitoring area and whether a new detection target appears in the monitoring area, so as to improve the efficiency of tracking multiple targets and the accuracy of tracking detection. Based on the obstacles in the grid map, the error data in the laser data obtained by the 2D laser radar is removed, so as to improve the accuracy of the first person leg data, and further improve the accuracy of tracking detection.

[0059] The technical solutions described above can also be combined with each other in the application to realize more preferred combination solutions. Other features and advantages of the application will be described in the subsequent description, and some advantages will become apparent from the description or be understood by implementing the application. The purposes and other advantages of the application can be realized and obtained from the contents specifically pointed out in the description and the drawings. BRIEF DESCRIPTION OF DRAWINGS

[0060] The accompanying drawings are included to provide a further understanding of the application and are incorporated in and constitute a part of this specification, illustrate embodiments of the application and together with the description serve to explain the principles of the application.

[0061] Figure 1 Flow chart of the robot navigation method based on dynamic social space cost of the embodiment of the application;

[0062] Figure 2 Block diagram of the robot navigation system based on dynamic social space cost of the embodiment of the application;

[0063] Figure 3 Social space cost model generated based on four Gaussian mixed functions of the embodiment of the application;

[0064] Figure 4 A social space cost model for merging two leg features of an embodiment of the present application;

[0065] Figure 5 A social space cost model for different speed parameters of an embodiment of the present application;

[0066] Figure 6 A social space cost model for different emotional parameters of an embodiment of the present application;

[0067] Figure 7 The detection result of OpenPose provided by an embodiment of the present application;

[0068] Figure 8 A flowchart for tracking a detection target based on Kalman filtering provided by an embodiment of the present application;

[0069] Figure 9 An emotional model schematic diagram provided by an embodiment of the present application. DETAILED DESCRIPTION

[0070] The preferred embodiments of the present application will be specifically described below in conjunction with the accompanying drawings, which form a part of this application, and are used to explain the principles of the present application together with the embodiments of the present application, and are not used to limit the scope of the present application.

[0071] One specific embodiment of the present application discloses a robot navigation method based on dynamic social space cost, as shown in Figure 1 The method comprises the following steps:

[0072] S1, a 2D laser radar and an RGB-D camera are used for pedestrian detection, and the pose and speed of the pedestrian are calculated;

[0073] S2, the mood value of the robot at the current time is calculated based on an emotional model;

[0074] S3, a dynamic social space cost model is constructed, the parameters of the dynamic social space cost model are adjusted based on the mood value and the relative speed of the pedestrian and the robot, the cost value of each grid in the social space cost map layer is calculated based on the adjusted dynamic social space cost model, and the cost value of the social space cost map layer is integrated into a cost map;

[0075] S4, the moving path of the robot is planned based on the updated cost map.

[0076] Compared with the prior art, the application realizes a basic cost model capable of simultaneously representing four spatial relationship models of concentric circle model, egg-shaped model, concentric ellipse model and one-end dominant model by adopting a four-Gaussian mixed function, not only makes the navigation behavior of the robot meet social norms, but also reduces unnecessary social space of the robot and improves the navigation efficiency of the robot. Through the adjustment of the relative speed and emotion on the basic model, the dynamic change of the basic social cost model is realized, so that the navigation behavior of the robot can more comply with the social norms of humans compared with the method of regarding humans as objects.

[0077] Specifically, the 2D laser radar and the RGB-D camera are used for pedestrian detection in step S1, and the pose and speed of the pedestrian are calculated, including:

[0078] S11, determine the first leg data of the pedestrian based on the grid map and the 2D laser radar.

[0079] Specifically, the 2D laser radar is used to collect leg laser data. According to the preset laser data gradient and the preset leg diameter, at least one cluster point set is determined from the leg laser data.

[0080] In the embodiment of the application, the cluster point set includes at least one laser point.

[0081] According to the grid map, the obstacle position information is determined;for each cluster point set, the distance between each laser point in the cluster point set and the obstacle is determined according to the obstacle position information, and the proportion of the laser points with a distance less than a first preset value in the corresponding cluster point set is calculated;the cluster point set with a proportion greater than a second preset value is deleted, and the first leg data is obtained.

[0082] Since the distance between adjacent laser points of the leg part features changes little, the gradient of adjacent points is less than the gradient threshold g leg . For laser data S=[r1..,r i ,...r Nbeam ], the gradient g i of each laser point is obtained by using the median difference method, and the calculation method is as follows:

[0083]

[0084] Then, according to the laser scanning order, at least one cluster point set is formed with points g i >g leg as the dividing points, wherein C′ is a set of cluster point sets, c′1, c′2, …, c′ beam is a cluster point set.

[0085] To improve the accuracy of the position of the human leg, the application gives the following constraints, and optimizes the collected laser data according to the constraints:

[0086] Firstly, the shape of the human leg is assumed to be a circle in the human leg feature detection algorithm, and at least three laser data points are needed to fit the shape, so the set that does not meet the number of data is removed as a constraint. Secondly, the diameter of the human leg is usually less than a preset threshold, and in the actual scene, the data points of the walls, tables and other objects scanned by the laser are located on the same straight line, resulting in the detection of the human leg diameter being greater than the preset threshold. Therefore, based on the diameter constraint of the human leg feature, the set that does not meet the constraint is further removed. The distance between the first and last points in each cluster point set is calculated, and if the distance is greater than the threshold of the diameter of the human leg, the cluster point set is removed, and the final set is where C is a set of cluster point sets, is a cluster point set.

[0087] For each cluster point set c i , a least squares method is used to fit a circle.

[0088] In the tracking detection process, if the human leg is close to the obstacle, a part of the laser points collected by the 2D laser radar come from the obstacle. In addition, the human leg is described as a circle in the application, which will cause a very large error in the position of the center of the circle, and thus an incorrect human leg position is obtained. In order to solve the above problem, the grid map created by the robot during navigation is used to remove part of the false positive detection results. The specific process is as follows:

[0089] The occupancy grid map is refined into a certain precision grid, and the probability of each grid in the occupancy grid map being occupied is estimated according to the environmental information returned by the laser sensor, which provides a basis for path planning. In implementation, first, the occupancy grid map is converted into a distance transform map, then the distance of each laser point in each cluster point set to the obstacle is counted, and if 85% of the points in the set are close to the obstacle, the set is removed to eliminate the influence of the obstacle on the leg position. Finally, considering that the position of the center of the circle fitted by the circle may be too large, in order to improve the accuracy of the position of the human leg, the centroid belonging to the cluster point set is used as the coordinate of the detected human leg, and the first human leg data where l i = (x leg,i , y leg,i ), that is, an l i represents the position of one human leg. S12, based on the RGB-D camera, determines the second human leg data and human joint information of the pedestrian

[0090] Specifically, based on images captured by an RGB-D camera, the skeleton points and pixel coordinates of the observed target are determined. Using the pixel coordinates of the skeleton points and the back-projection model of the RGB-D camera, the 3D coordinates of the skeleton points are determined. Based on the 3D coordinates of the skeleton points, the positional information of the human leg skeleton points and human joint information are determined. Based on the positional information of the human leg skeleton points, second human leg data is generated.

[0091] Specifically, the RGB-D camera uses the open-source skeleton detection algorithm OpenPose to identify key points of a person. OpenPose is a human skeleton detector based on RGB images that can detect 25 skeleton points. Based on the pixel coordinates of the skeleton points, and then combining the pixel coordinates of the OpenPose skeleton points, the depth image, and the camera's back-projection model, the 3D coordinates of the skeleton points in the camera coordinate system are obtained. This is achieved by using the homogeneous pixel coordinates u of a skeleton point. key =[u,v,1] T This allows us to obtain the corresponding spatial point P. key c ,

[0092] P key c =z c K -1 u key (2)

[0093] Among them, z c is the observation value from the RGB-D camera, and K is the calibration matrix of the RGB-D camera.

[0094] The pose of the human legs is calculated by combining the shoulder and hip joints detected by OpenPose. The local coordinate system for the human is defined as follows: the front of the person is the x-axis, the top is the z-axis, and the y-axis is defined using the right-hand rule. The pose of the person is the angle between the x-axis and the positive x-axis of the map coordinate system. The visually detected leg features are (x′... leg ,y′ leg ,θ′ leg ), where (x′ leg ,y′ leg The pose angle θ′ of the human leg joint can be calculated by using the average values ​​of the directions of the shoulder and hip joints, obtained through the back projection model of the camera and the human leg skeleton points. leg .like Figure 7 As shown in the figure, this embodiment of the invention provides a schematic diagram of human leg feature detection based on an RGB-D camera and OpenPose.

[0095] S13. Based on the first leg data and the second leg data, determine the pedestrian's position information and speed information using Kalman filtering. Specifically, this includes:

[0096] S131, taking the first received first human leg data or second human leg data as a state quantity, and taking the second human leg data or first human leg data received at the next time as an observation quantity.

[0097] In the embodiment of the present application, the first human leg data and the second human leg data are not received at the same time, but are received alternately. Therefore, the Kalman filter is initialized by the first received first human leg data or second human leg data. It should be noted that, Figure 8 The initialization in the embodiment of the present application is the initialization of the observation target, and is not the initialization of the entire detection process. For example, there is only detection target A at the last time, and detection target B appears at the next time. At this time, the detection of target B is initialized by the first human leg data or second human leg data of the first received detection target B.

[0098] S132, performing Kalman filter state prediction according to the state quantity to obtain the next time state quantity.

[0099] S133, comparing the detection target corresponding to the next time state quantity and the detection target corresponding to the observation quantity, when the two are matched, optimizing and updating the next time state quantity according to the current observation quantity.

[0100] In the embodiment of the present application, in order to realize the detection of multiple targets, the matching degree of the detection target corresponding to the next time state quantity and the detection target corresponding to the observation quantity is detected by global data association, and the detection result is specifically three kinds: in order to facilitate the description, it is assumed that the detection target corresponding to the next time state quantity is M, and the detection target corresponding to the observation quantity is N.

[0101] The first kind is that M and N correspond to each other, at this time, step S134 is executed.

[0102] The second kind is that there is M without corresponding N, which means that the detection target has left the detection area, at this time, the tracking of the detection target corresponding to the next time state quantity is ended, that is, the Kalman filter deletion strategy is executed.

[0103] The third kind is that there is N without corresponding M, which means that a new detection target appears in the detection area, and the observation quantity is set as the state quantity of the corresponding detection target to realize the Kalman filter initialization of the detection target.

[0104] It should be noted that in the embodiment of the present application, the first human leg data and the second human leg data are both position information, therefore, when optimizing and updating the next time state quantity, the human body speed information is determined according to the position information and time of the observation quantity, and the human body speed information is added to the state quantity.

[0105] S134, then, alternately taking the received first human leg data or second human leg data as an observation quantity to constantly optimize the corresponding next time state quantity.

[0106] Specifically, the state variables of the Kalman filter are: Where, x k and y k Let k be the position of the person's leg in the map coordinate system at time k. and Let be the velocity of the human leg in the map coordinate system at time k. A uniform velocity model is used as the motion model for the Kalman filter. The observation model is the observed position of the human leg. Global nearest neighbor data association is used to confirm the correspondence between state variables and observation variables, and the Kalman filter is updated accordingly.

[0107] S14. Determine the pedestrian posture angle based on the human joint information.

[0108] The human joint information includes: shoulder joint position information and hip joint position information;

[0109] Determining the pedestrian posture angle based on the human joint information includes:

[0110] Based on the shoulder joint position information, determine the shoulder joint posture angle; based on the hip joint position information, determine the hip joint posture angle; based on the shoulder joint posture angle and the hip joint posture angle, determine the pedestrian posture angle.

[0111] Specifically, determining the pedestrian posture angle based on the shoulder joint posture angle and the hip joint posture angle includes:

[0112]

[0113] Where, θ shoulder θ is used to characterize the joint attitude angle. hip θ′ is used to characterize the hip joint posture angle. leg Used to characterize the posture angle of the human leg, (x) Rshoulder y Rshoulder (x) represents the position information of the right shoulder joint. Lshouler y Lshouler (x) represents the position information of the left shoulder joint. Rhip y Rhip (x) represents the position information of the right side of the hip joint. Lhip y Lhip ) represents the left side position information of the hip joint.

[0114] In an embodiment of the present invention, θ′ leg This applies to the leg posture angles of a person when they are at rest. During the detection process, a Kalman filter is used to determine whether the person is in motion; if so, the updated state variables are used. and Calculate the leg posture angle; otherwise, use θ′. legis the total leg posture angle of the person leg The calculation process is specifically as follows:

[0115]

[0116] wherein, V thres is a speed threshold for determining whether the human body is in a motion state, and are the components of the leg in the x direction and the y direction, and thus and the square root of the sum of the squares of and is the actual moving speed of the leg. That is, when the actual speed is less than V thres , it is determined that the human body is in a static state, and when the actual speed is greater than or equal to V thres , it is determined that the human body is in a moving state.

[0117] The detected position and posture angle of the leg are the pose of the pedestrian, and the speed of the leg is the speed of the pedestrian.

[0118] Through fusion of 2D laser radar and RGB-D camera for pedestrian detection and tracking, based on Kalman filtering algorithm, the first leg data obtained by 2D laser radar and the second leg data obtained by RGB-D camera are verified with each other, so as to solve the problem of missed detection or false detection of 2D laser radar to a certain extent by using RGB-D, and at the same time, the visual blind area of RGB-D camera is supplemented by 2D laser sensor, so as to improve the accuracy and expand the detection range. By comparing the detected target corresponding to the state quantity of the next moment and the detected target corresponding to the observation quantity, it is determined in real time whether the detected target has left the monitoring area and whether a new detected target appears in the monitoring area, so as to improve the efficiency of tracking multiple targets and the accuracy of tracking and detection. Based on the obstacles in the grid map, the error data in the laser data obtained by the 2D laser radar is removed, so as to improve the accuracy of the first leg data, and further improve the accuracy of tracking and detection.

[0119] Specifically, the emotion model in step S2 is an emotion model based on Plutchik theory, as shown in Figure 9 (a), and emotions mainly include mood, mood and personality.

[0120] Table 1 Classification of emotional concepts

[0121]

[0122] The model consists of 8 basic bipolar emotions, emotion balls, difference thresholds, and an emotional ball. The 8 basic bipolar emotions are used to express the emotional state of the robot, describe the interaction with emotions, such as positive emotions can increase positive emotions. The emotion ball moves on the corresponding emotion curve and is used to visualize the dynamics of emotion. The model directly uses the difference threshold to represent the personality traits of the robot and the different emotional intensity. Finally, the emotional ball is used to represent the emotional state of the robot, such as positive, neutral and negative. In addition, the emotional ball can use different colors to show different states. In this way, not only can emotions, emotions and personality traits be integrated into a two-dimensional emotional space, but also the dynamic changes of emotions can be visualized and described in an easy-to-understand way. For example, Figure 9 (b) For example, the happy emotion moves on the corresponding emotion curve. Under the stimulation of a specific event, it is assumed to be located at the position of the dashed happy ball, however if the effect of the negative mood is considered, it is possible to make the robot experience a lower happy emotion, making the emotion ball located at the position of the solid ball closer to the mood. In addition, the robot in this figure has a positive personality because it can easily experience emotions in the positive category and is not easily prone to negative emotions.

[0123] The model takes the emotional stimulus of the robot as input, and describes the change of the emotional state of the robot by establishing the stimulation update and natural decay process of emotion and mood. Specifically, emotional stimulus is an event that causes the emotional state of the robot to change. This event may come from the internal state of the robot or the external environment. For example, when the robot performs too many tasks, it may produce a sad emotion; when the robot successfully completes the navigation task, it will be stimulated by a happy emotion. The stimulus not only includes the action intensity, but also includes the action effect generated by the stimulus.

[0124] Because the mood lasts for a long time and is less related to specific events, in order not to make the robot experience strong emotions frequently and produce unnatural behavior, the mood is used as a regulating factor for the emotional cost of the robot.

[0125] The mood update is affected by the previous mood and the current stimulus. Because the mood lasts for a long time and is usually unrelated to specific events, it is modeled as a slowly changing state quantity. Unlike emotions, which can complete updates within a single computation period, the slow change process of mood is simulated by completing updates to the emotional state over multiple computation periods. Katsimerou et al. of Delft University of Technology proposed five methods to map emotions to mood: emotion average, emotion maximum, long-lasting emotion, first-stimulus emotion, and last-stimulus emotion. The present invention uses the average of the stimulus to update the mood.

[0126]

[0127] where Δm is the change of mood in each mood cycle, n p is the number of mood update cycles, w is the number of emotions activated by the stimulus event, k ei represents the effect coefficient of the ith emotion on mood, e ci represents the emotion category; a i (t) represents the stimulus intensity of the stimulus event on the ith emotion, m(t) represents the mood value at time t.

[0128] Specifically, the emotion categories are divided into positive emotions and negative emotions. The positive emotions are happiness, trust, and expectation. It should be noted that, unlike the definition of the two polarities of emotions in Plutchik's theory, the division is based on the degree of friendliness that the robot's emotions show to humans. For example, although anger and fear are two polar emotions, they both belong to the negative category. Therefore, the category of emotions is represented as:

[0129]

[0130] After the mood completes the stimulus update, it will decay over time to return to a calm state. Many studies have shown that emotions decay exponentially. For example, a study of life satisfaction showed that after pleasant and unpleasant events occur, life satisfaction returns to a stable state in the form of an exponential function. Therefore, the model adopts an exponential decay form. That is, if there is no stimulus event at the current time, the mood value is calculated according to the following natural decay formula:

[0131]

[0132] where t1 represents the time when the mood update is completed, m(t1) represents the mood value at time t1, d m is the decay factor of the mood, and b is the bias value of the mood.

[0133] Specifically, the emotion update equation includes four cases.

[0134] The first case is as formula (7), which indicates that the robot's mood cannot ignore this stimulus event, and the stimulus value and mood value are greater than the emotion value at the previous time. The emotion model update equation is as formula (8), which indicates that in this case, the current emotion value of the robot is the comprehensive effect of the previous mood and the current stimulus, and the duration is the action time of the stimulus.

[0135]

[0136]

[0137] where e ai (t) is the intensity value of emotion i at the stimulus time, e di(t) is the duration of the stimulus event on emotion i, t i0 is the time of stimulus occurrence, e ci is the emotion category, k mi is the effect coefficient of the mood on emotion i, a i (t) represents the stimulus intensity of the i-th emotion by the stimulus event, m(t-1) represents the mood value at time t-1, e i (t) represents the value of the i-th emotion at time t, d i represents the decay factor of the i-th emotion, h i1 represents the threshold value of the i-th emotion in a calm state.

[0138] The second case is represented by equation (9), which indicates that the mood of the robot cannot allow it to ignore the stimulus event, and the stimulus value and the mood value are not greater than the emotion value at the previous time. The update equation is represented by equation (10), which indicates that the intensity of the emotional experience does not change, but the emotional experience time is extended.

[0139]

[0140] where f i is the extension coefficient of the emotional experience time, h i1 represents the threshold value of the i-th emotion in a calm state.

[0141] The third case is represented by equation (11), which indicates that although the mood of the robot can allow it to ignore the stimulus event, the current stimulus and the emotion value at the previous time are greater than the mood value. The update equation is represented by equation (12), which indicates that the intensity of the emotion is updated to the maximum emotion intensity that the robot can experience.

[0142]

[0143]

[0144] where abs(·) represents the absolute value.

[0145] The last case is represented by equation (13), which means that due to the existence of the mood, the stimulus is not enough to cause the emotional change of the robot. The update equation is represented by equation (14).

[0146]

[0147]

[0148] Specifically, the natural decay of the emotion is:

[0149]

[0150] The cost model is adjusted by the current mood value of the robot to improve flexibility of the model and enrich behavior performance of the robot.

[0151] The dynamic social space cost model constructed in step S3 is a four Gaussian mixture model, which is composed of four Gaussian functions. Each Gaussian function corresponds to the cost value of a quadrant in the two-dimensional Cartesian coordinate system. In addition, like the two Gaussian mixture model, it also represents the influence of different human poses on the model. Assuming that the pose of a person is (x p ,y p ,θ p ), the social cost c(x,y) at the coordinate (x,y) is:

[0152] θ=atan2((y p -y),(x p -x))-θ p ,

[0153]

[0154]

[0155] c(x,y)=Aexp(-f x -f y ), where (σ f ,σ l ,σ b ,σ r ) are the standard deviations of the Gaussian functions in front, left, back and right of the pedestrian respectively, (x p ,y p ,θ p ) represents the pose of the pedestrian, c(x,y) represents the cost value at the position (x,y), atan2(·) represents the arctangent function, a, b, fx, fy represent intermediate parameters, and θ represents the angle of the point (x,y) in the pedestrian coordinate system.

[0156] The social space cost model realizes flexible representation of social cost, and the dynamic social space cost model is based on this to realize dynamic change of the model. The parameters of the four Gaussian mixture model are the pose (x p ,y p ,θ p ) of the pedestrian and the standard deviation (σ f ,σ b ,σ l ,σ r ). The pose parameter is mainly used to distinguish humans and obstacles in the environment, and the standard deviation parameter is used to ensure the safety of the pedestrian and the diversity of the robot behavior. Specifically, since the humans in the environment may be in motion, the robot must avoid humans in time, and therefore the standard deviation σ fThe relative motion speed of the human and the robot is considered in the parameters. In addition, in a human-robot symbiotic environment, the robot should enrich the behavior of the robot on the basis of meeting the navigation requirements. Otherwise, the robot will always move along a fixed trajectory, giving people a stiff impression. Therefore, in order to dynamically adjust the model, the model has high flexibility, and at the same time, the navigation movement of the robot can also comply with the norms of spatial relationship science, and the model size is dynamically and reasonably adjusted through emotion and pedestrian state.

[0157] Specifically, the parameters of the dynamic social space cost model are adjusted based on the mood value and the relative speed of the pedestrian and the robot, including:

[0158] An action coefficient is calculated based on the mood value and the relative speed of the pedestrian and the robot:

[0159]

[0160] The following formula is used to calculate (σ f ,σ l ,σ b ,σ r ):

[0161]

[0162] wherein w f , w b , w l , w r respectively represent the action coefficients of the standard deviations of the Gaussian functions in the front, rear, left and right of the pedestrian, σ′ f , σ′ b , σ′ l , σ′ r respectively represent preset parameters of the Gaussian functions in the front, rear, left and right of the pedestrian, w m represents the mood action coefficient, m represents the mood value of the robot, represents the speed of the pedestrian in the map coordinate system, represents the speed of the robot in the map coordinate system.

[0163] After obtaining the action coefficients, the coefficients are applied to the standard deviations, thereby realizing the dynamic change of the model.

[0164] The cost value of each grid in the social space cost map layer is calculated based on the adjusted dynamic social space cost model, and the cost value of the social space cost map layer is integrated into the cost map. Specifically, the cost map is a local cost map, which is different from the global cost map. The local cost map is only a rectangular window centered on the robot as the map size, which is a subgraph of the global cost map. The size of the local cost map can be set according to the performance of the robot and the detection device, for example, it can be set to 10m*10m, so that the updating speed is higher. The local cost map of the application comprises an obstacle layer, an inflation layer and a social space cost layer. The obstacle layer detects obstacle data through a sensor. In implementation, the cost addition form can be used to integrate the cost value of the social space cost map layer into the cost map by merging multiple cost values located at the same grid.

[0165] The moving path of the robot is planned based on the updated cost map. Specifically, the existing moving path planning algorithm can be used for robot path planning and navigation, for example, the A* algorithm.

[0166] In order to verify the representation ability and the relative speed and emotion of the model, a simulation experiment based on the control variable method is used to verify the effectiveness of the dynamic social space cost model proposed in the application.

[0167] In the field of spatial relationship, commonly used models include concentric circle model, elliptical model, egg-shaped model, and one-end station model. Since the two Gaussian mixture model cannot represent all the models at the same time, the basic model parameters shown in Table 2 are used to verify the flexibility of the four Gaussian mixture function model.

[0168] Table 2 Parameter values in basic model experiment

[0169]

[0170] Figure 3 The four Gaussian mixture model generates a pedestrian cost. Among them, Figure 3 (a) is a concentric circle model generated by using the parameters of experiment 1 in Table 2; Figure 3 (b) is a concentric ellipse model generated by using the parameters of experiment 2 in Table 2; Figure 3 (c) is an egg-shaped model generated by using the parameters of experiment 3 in Table 2; Figure 3 (d) is a one-end dominant model generated by using the parameters of experiment 4 in Table 2. The four different social costs are derived from the cost model based on the four Gaussian mixture function proposed in the application, that is, the model can represent the four spatial relationship models at the same time, thereby verifying the flexibility of the model.

[0171] Since the application detects and tracks the leg features of a pedestrian when detecting and tracking the pedestrian, two social costs are generated for one pedestrian in actual observation, but only one cost value can be contained in one grid position in the cost map, so different costs need to be merged. The application adopts the form of cost addition to merge multiple cost values located in the same grid. Figure 4 The cost model after synthesizing two human legs is shown. Among them, Figure 4 (a) shows the synthesized concentric circle model; Figure 4 (b) shows the synthesized concentric ellipse model; Figure 4 (c) shows the synthesized egg-shaped model; Figure 4 (d) shows the synthesized one-end-dominant model. The arrow indicates the posture of the leg. Overall, the synthesized model can meet the expected shape, proving the feasibility of using the leg as a pedestrian feature and using the superposition method to merge the cost when detecting and tracking the pedestrian.

[0172] The dynamic social space cost model should automatically adjust the size of the model cost with the relative speed of the human and the robot, so that the robot can avoid obstacles in time. The application adopts four groups of parameters shown in Table 3 to generate social costs, and then proves the effectiveness of the dynamic model through comparison of experimental results.

[0173] Figure 5 The results of generating social costs based on four groups of parameters are shown. By comparing Figure 5 (a) and (b), and Figure 5 (c) and (d), it can be seen that as the speed effect coefficient increases, the social cost in front of the pedestrian increases. By comparing Figure 5 (a) and (c), and Figure 5 (c) and (d), it can be seen that under the condition that the speed effect coefficient is the same, when the speed of the pedestrian increases, the social cost in front of the pedestrian also increases.

[0174] Through the above comparative analysis, it can be seen that the speed effect parameter can dynamically adjust the model cost to meet the needs of robot navigation and obstacle avoidance.

[0175] Table 3 Parameter values in the experiment of the effect of speed on the cost model

[0176]

[0177] Emotion is mainly used to adjust the social cost model to achieve the purpose of enriching the behavior of the robot. The application generates social cost models using four groups of experimental parameters in Table 4, and then verifies the adjusting effect of emotion on the cost model by comparing the experimental results.

[0178] Figure 6 The results of generating social costs based on four groups of parameters are shown. By comparingFigure 6 As can be seen from (a) and (b), when the robot is in a positive mood, the social cost of the pedestrian decreases as the mood action coefficient decreases. Figure 6 As can be seen from (c) and (d), when the robot is in a negative mood, the social cost of the pedestrian increases as the mood action coefficient decreases. Figure 6 As can be seen from (a) and (c), and Figure 6 As can be seen from (c) and (d), under the condition of the same mood action coefficient, the social cost of the robot in a positive mood is less than the social cost of the robot in a negative mood. In other words, when the mood of the robot is better, the robot is more willing to approach the human, and vice versa, the robot tends to keep a relatively far distance from the human. As the mood action coefficient decreases, the social cost of the pedestrian decreases.

[0179] Through the above comparative analysis, it can be seen that when the mood of the robot is different or the action coefficient is different, the social cost of the human in the map also changes, so that the navigation technology based on the cost model can generate different paths, thereby enriching the behavior of the robot.

[0180] Table 4 Parameter values in the experiment of the effect of emotion on the cost model

[0181]

[0182] One specific embodiment of the present application discloses a robot navigation system based on a dynamic social space cost, comprising the following modules:

[0183] A pedestrian detection module is configured to detect pedestrians by using a 2D laser radar and an RGB-D camera, and calculate the pose and speed of the pedestrians.

[0184] A mood calculation module is configured to calculate the mood value of the robot at the current time based on an emotion model.

[0185] A cost map updating module is configured to construct a dynamic social space cost model, adjust parameters of the dynamic social space cost model based on the mood value and the relative speed between the pedestrian and the robot, calculate the cost value of each grid in a social space cost map layer based on the adjusted dynamic social space cost model, and integrate the cost value of the social space cost map layer into a cost map.

[0186] A navigation module is configured to plan a moving path of the robot based on the updated cost map.

[0187] The method embodiment and the system embodiment described above are based on the same principle, and can be mutually referred to and achieve the same technical effects. For specific implementation processes, refer to the foregoing embodiments, which will not be described here again.

[0188] Those skilled in the art can understand that all or part of the processes of the above-mentioned embodiment methods can be completed by instructing the relevant hardware by a computer program, and the program can be stored in a computer readable storage medium. The computer readable storage medium is a disk, an optical disk, a read-only memory, a random access memory, etc.

[0189] The above description is merely preferred specific embodiments of the present application, but the protection scope of the present application is not limited thereto, and any person skilled in the art can easily think of changes or replacements within the technical scope disclosed by the present application, which should be covered within the protection scope of the present application.

Claims

1. A method for robot navigation based on dynamic social space cost, the method comprising: Includes the following steps: Pedestrian detection is performed using 2D LiDAR and RGB-D cameras to calculate pedestrian pose and velocity. The robot's current mood value is calculated based on an emotion model. A dynamic social space cost model is constructed, and the parameters of the dynamic social space cost model are adjusted based on the mood value and the relative speed between pedestrians and robots; the cost value of each grid in the social space cost map layer is calculated based on the adjusted dynamic social space cost model; and the cost value of the social space cost map layer is integrated into the cost map. The robot's movement path is planned based on the updated cost map.

2. The method for robot navigation based on dynamic social space cost according to claim 1, wherein, The cost model for the dynamic social space is as follows: θ = atan2((y p -y),(x p -x))-θ p , c(x, y) = A exp(-f x -f y ), where (σ f ,σ l ,σ b ,σ r ) are the standard deviations of the Gaussian functions for the front, left, back, and right of the pedestrian, respectively, (x p ,y p ,θ p ) represents the pose of the pedestrian, c(x, y) represents the cost value at the (x, y) position, atan2(·) represents the arctangent function, a, b, fx, fy represent intermediate parameters, and θ represents the angle of the (x, y) point in the pedestrian coordinate system.

3. The method for robot navigation based on dynamic social space cost of claim 2, wherein, The adjustment of the parameters of the dynamic social space cost model based on the mood value and the relative speed between the pedestrian and the robot includes: The action coefficient is calculated based on the mood value and the relative speed between the pedestrian and the robot: The following formula is used to calculate (σ f ,σ l ,σ b ,σ r ): wherein w f , w b , w l , w r represent the action coefficients of the standard deviations of the Gaussian functions in front, behind, left and right of the pedestrian respectively, σ′ f , σ′ b , σ′ l , σ′ r represent the preset parameters of the Gaussian functions in front, behind, left and right of the pedestrian respectively, w m represents the mood action coefficient, and m represents the mood value of the robot, represents the speed of the pedestrian in the map coordinate system, represents the speed of the robot in the map coordinate system.

4. The method of claim 1, wherein, The mood value is calculated through the following steps: If a stimulus event occurs at the current moment, the mood update is calculated using the average value of the stimulus: where Δm is the change of mood in each mood cycle, n p is the number of mood update cycles, w is the number of emotions activated by the stimulus event, k ei represents the effect coefficient of the ith emotion on mood, e ci represents the emotion category; a i (t) represents the stimulation intensity of the stimulus event on the ith emotion, and m(t) represents the mood value at time t. If no stimulating event occurs at the current moment, the mood value is calculated according to the following natural decay formula: m(t) = m t1 exp(-d m (t-t1))+b, where t1 represents the time at which the mood update is completed, m t1 represents the mood value at time t1, d m is a decay factor for the mood, and b is a bias value for the mood.

5. The method for robot navigation based on dynamic social space cost of claim 1, wherein, Pedestrian detection is performed using 2D LiDAR and an RGB-D camera to calculate pedestrian pose and velocity, including: Based on grid maps and 2D LiDAR, the first leg data of a pedestrian is determined; Based on an RGB-D camera, determine the second leg data and human joint information of a pedestrian; Based on the first and second leg data, pedestrian location and speed information are determined using Kalman filtering. Based on the human joint information, the pedestrian posture angle is determined.

6. The method for robot navigation based on dynamic social space cost of claim 5, wherein, The determination of the first human leg data based on grid maps and 2D LiDAR includes: Laser data of the human leg is collected using the 2D lidar; Based on a preset laser data gradient and a preset human leg diameter, at least one cluster point set is determined from the human leg laser data, and the cluster point set includes at least one laser point; Based on the grid map, determine the location information of the obstacles; For each cluster point set, based on the obstacle location information, the distance between each laser point in the cluster point set and the obstacle is determined, and the proportion of laser points whose distance is less than a first preset value in the corresponding cluster point set is calculated; Delete the cluster set whose proportion is greater than the second preset value to obtain the first human leg data.

7. The method for robot navigation based on dynamic social space cost of claim 5, wherein, The determination of the second human leg data and human joint information based on the RGB-D camera includes: Based on the images captured by the RGB-D camera, determine the skeleton points of the observed target and the pixel coordinates of the skeleton points; The three-dimensional coordinates of the skeleton points are determined based on the pixel coordinates of the skeleton points, the depth image, and the back projection model of the RGB-D camera. Based on the three-dimensional coordinates of the skeletal points, determine the position information of the human leg skeletal points and the information of the human joints; Based on the position information of the human leg skeleton points, second human leg data is generated.

8. The method for robot navigation based on dynamic social space cost of claim 5, wherein, The human joint information includes: shoulder joint position information and hip joint position information; Determining the pedestrian posture angle based on the human joint information includes: According to the shoulder joint position information, a shoulder joint posture angle is determined; according to the hip joint position information, a hip joint posture angle is determined; and according to the shoulder joint posture angle and the hip joint posture angle, the pedestrian posture angle is determined.

9. The method for robot navigation based on dynamic social space cost of claim 8, wherein, The determination of the pedestrian posture angle according to the shoulder joint posture angle and the hip joint posture angle comprises: wherein θ shoulder for representing the joint posture angle θ hip for representing the hip joint posture angle θ' leg for representing the human leg posture angle (x Rshoulder , y Rshoulder ) is right shoulder joint position information, (x Lshouler , y Lshouler ) is left shoulder joint position information, (x Rhip , y Rhip ) is right hip joint position information, and (x Lhip , y Lhip ) is left hip joint position information.

10. A robot navigation system based on dynamic social space cost, the system comprising: The method comprises the following modules: A pedestrian detection module is configured to detect pedestrians by using a 2D laser radar and an RGB-D camera, and to calculate the pose and speed of the pedestrians; A mood calculation module is configured to calculate the mood value of the robot at the current time based on an emotion model; A cost map updating module is configured to construct a dynamic social space cost model, to adjust the parameters of the dynamic social space cost model based on the mood value and the relative speed between the pedestrians and the robot, to calculate the cost value of each grid in a social space cost map layer based on the adjusted dynamic social space cost model, and to integrate the cost value of the social space cost map layer into a cost map; A navigation module is configured to plan a moving path of the robot based on the updated cost map.

Images