Recently, the safety of workers has gained increasing attention due to the applications of collaborative robots (cobot). However, there is no quantitative research on the impact of cobot behavior on humans’ psychological reactions, and these results are not applied to the cobot motion planning algorithms. Based on the concept of the gravity field, this paper proposes a model of the psychological safety field (PSF), designs a comprehensive experiment on different speeds and minimum distances when approaching the head, chest, and abdomen, and obtains the ordinary surface equation of psychological stress about speed and minimum distance by using data fitting. By combining social rules and PSF models, we improve the robot motion planning algorithm based on behavioral dynamics. The validation experiment results show that our proposed improved robot motion planning algorithm can effectively reduce psychological stress. Eighty-seven point one percent (87.1%) of the experimental participants think that robot motion planned by improved robot motion planning algorithms is more “friendly”, can effectively reduce psychological stress, and is more suitable for human–robot interaction scenarios.