Safety-Oriented Motion Planning for a Wheeled Humanoid Robot Operating in Environments with Stochastically Moving Humans

Abstract

With the advancement of humanoid robotics, human–robot collaboration has emerged as a prominent research focus. Ensuring the safety of both humanoid robots and humans remains a critical challenge. In this paper, we address conflict resolutions at the planning level and propose a safety-oriented motion planning (SOMP) algorithm for a wheeled humanoid robot operating in environments with unknown human motions. In the proposed SOMP algorithm, we employ Monte Carlo simulations to predict trajectories of stochastically moving humans and formulate both hard and soft constraints. A dynamic-quadrant stochastic sampling policy, integrated with a rapidly exploring random tree method, is proposed to generate diverse initial paths. Building upon this, we develop a constraint-fusion mechanism that combines hard constraints for safety guarantees and soft constraints for path optimization, thereby effectively resolving potential conflicts between wheeled humanoid robots and stochastically moving humans. We evaluate the proposed algorithm under different configurations of conflict numbers, task success rates, and path rewards. The proposed method outperforms A*, RRT, and MDP in terms of conflict numbers (−77.8%, −76.6%, and −71.4%) and task success rates (+168.0%, +109.4%, and +91.4%). Our simulation results prove the efficiency and robustness of our algorithm in safe motion planning with stochastically moving humans.

1. Introduction

As robotics technology continues to evolve, an increasing number of robots are being integrated into daily human activities, leading to more frequent collaboration between humans and robots [1]. The wheeled humanoid robot (WHT), a hybrid design that integrates a bipedal upper body with a wheeled mobile base, has garnered significant attention from both academia and industry (Figure 1). The configuration of WHT combines the advantages of humanoid morphology, which enables natural collaboration with human-centric environments (e.g., operating tables and door handles) and facilitates dexterous manipulation tasks, with the efficiency and stability of wheeled locomotion. The wheeled base alleviates the inherent limitations of traditional bipedal robots, such as gait instability, low locomotion speed, and high energy consumption, thereby enabling fast, efficient, and stable motion on flat terrain. These capabilities make WHT highly promising for a wide range of applications, including warehouse robots, service robotics, medical rehabilitation, and public assistance.

However, this vision of human–robot collaboration also poses a fundamental challenge: path safety. As robots operate in dynamic human-shared spaces, their path planning is no longer merely finding the shortest path, but rather a dynamic decision-making process that must prioritize human safety above all else. Robot motion must not only be efficient, but also highly reliable, predictable, and collision-free. Even minor incidents can result in physical harm and severely undermine public trust in robotic systems. Hence, ensuring path safety amid unpredictable human movements is a critical prerequisite for unlocking the potential of such robots.

Motion planning in dynamic, uncertain human-shared environments represents a fundamental challenge in the field of mobile robotics. Unlike static or deterministically moving obstacles, human motion is inherently stochastic and impossible to predict with perfect accuracy. This uncertainty stems from the complex intentions, social conventions, and reactive decision-making of humans. For instance, a person may suddenly change his/her direction, stop without obvious cues, or interact with other individuals. Traditional planning approaches, such as velocity obstacles [2,3] and the dynamic window approach [4], often rely on the assumption that dynamic obstacles move in a deterministic manner (e.g., at constant velocity). However, this assumption frequently fails in human-shared environments where human motion is inherently stochastic and unpredictable. Traditional methods perform real-time obstacle avoidance [5,6]; however, those methods lack foresight and are easily trapped in local optima. Safe motion planning must explicitly account for the multi-modal probability distributions of human movements and generate trajectories that not only proactively avoid potential conflicts but also maintain global optimality. Our previous research [7,8] has shown that resolving conflicts at the planning level is an efficient way to avoid potential human–robot collisions.

To achieve safe motion planning, it is essential to predict how humans will move and how human risk will be during the execution of tasks by both humans and robots. Our purpose is to solve conflicts at the planning level by taking human risk distributions into account while achieving a high task success rate and improving WHT and human collaboration efficiency. To this end, this paper presents a safety-oriented motion planning (SOMP) algorithm that accounts for unknown human motions at the planning level and generates an optimal, safe path. The primary contributions of this paper are outlined as follows:

• We introduce an SOMP framework that accounts for unknown human motions at the planning level and generates an optimal safe path.
• We develop an $\eta$-diversity path generator ($\eta$-DPG), built on a dynamic-quadrant stochastic sampling RRT method that computes multiple diverse paths across the environment according to a diversity metric $\eta$, thereby enhancing the algorithm’s robustness to environmental changes.
• A constraint-fusion mechanism combining hard and soft constraints is proposed for conflict resolution, where hard constraints provide a safety guarantee by forbidding visiting high-risk areas, while soft constraints support optimization.
• We conduct a series of comparative simulations to validate the proposed methods, and the results demonstrate their efficiency in planning safe paths and their robustness to environmental changes.

The rest of this paper is organized as follows. Section 2 presents a review of related works. Section 3 introduces the problem description and formulation. The proposed method is presented in Section 4, followed by the simulation results and discussions in Section 5. Finally, we provide a brief conclusion in Section 6.

2. Related Works

To avoid human–robot collisions, various approaches have already been developed. Korkmaz [9] focused on the human-aware navigation of a robot, where the robot follows a global path and rearranges its path by dynamically detecting obstacles around it. The majority of studies are based on rapidly nexploring random trees (RRT) [10], which explore the environment by sampling. Hyotish et al. [11] developed an asymptotically optimal version of RRT based on incremental sampling that covers the whole space. They used a dynamic replanning scheme in which the robot rectifies or modifies its path when an unknown moving or static snag obstructs it. To build more effective path planning algorithms, many improved RRT algorithms have been proposed in [12,13,14,15]. Herrera-Medina et al. [16] combined YOLO with an RRT algorithm for optimizing path planning and human detection. The RRT algorithm generates collision-free paths, and YOLO is applied for human detection and localization. Aiswarya et al. [17] proposed a human-aware motion planning method with RRT in an Industry 4.0 environment. Rajendran et al. [18] provided a human-aware motion planner, building upon RRT-connect. However, their approach was designed specifically for manipulators and is not well-suited for solving the path-planning problem of mobile robots in environments with stochastically moving humans.

The studies mentioned above demonstrate the effectiveness of RRT and RRT* algorithms in motion planning. They both employ an online scheme that continuously monitors a robot’s surroundings and replans a new path whenever a human obstructs the existing path. It is commonly considered the safest solution for avoiding moving obstacles or humans. However, it focuses too heavily on immediate conflicts and lacks foresight, which prevents it from adopting proactive strategies; instead, it can only respond passively by replanning detours. Consequently, the robot becomes easily trapped in local optima and fails to complete its task within the allocated time budget. Furthermore, the computational cost of continuous replanning is high.

Mi et al. [19] developed a safe A* algorithm to plan a safe local path by considering stochastic human risks, which is a searching-based method. However, their algorithm is limited and highly depends on the reward design. There are also many other famous searching-based algorithms for path planning, such as D*. Choi et al. [20] developed an enhanced D* algorithm for optimizing the path where a robot moves on sidewalks. They analyzed humans’ trajectory data from lidar sensors and identified the average distance and angle of avoidance at which people start to avoid autonomous robots. The searching-based method is more efficient in determining optimal paths in a static environment. For a dynamic environment, it must combine risk constraints or an online replanning scheme to avoid moving obstacles.

Machine learning (ML) has been widely applied in robotics and has shown its effectiveness in decision-making. We consider it a learning-based method, such as reinforcement learning (RL), deep reinforcement learning (DRL), supervised learning, Monte Carlo tree search (MCTS), etc. Mi et al. [21] constructed a double-layer RL framework for determining an optimal path, enabling a robot to work in an environment shared with forklifts. EL-Shamouty et al. [22] developed a framework using DRL to strengthen the intelligence and safety of a robot that collaborates with humans. Chen et al. [23] proposed a safe RL controller incorporating control barrier functions (CBFs), using Gaussian processes to model environmental dynamics and uncertainties. However, Gaussian processes are limited in capturing multi-peaked uncertainty distributions. Zhang et al. [24] developed an online collision detection and identification method for human-collaborative robots based on supervised learning and Bayesian theory. To improve adaptability to environmental changes, Mi et al. [25] introduced an encoded RL approach that integrates formal methods with reinforcement learning. Li et al. [26] proposed a self-learning Monte Carlo tree search algorithm (SL-MCTS) that combines the MCTS algorithm with a two-branch neural network (PV-Network) and has the ability to continuously improve its problem-solving ability in single-player scenarios.

Learning-based approaches are effective at adapting to dynamic environments: RL methods learn an optimal policy via trial-and-error, while MCTS operates without domain knowledge. These methods typically employ reward mechanisms aimed at maximizing a predefined objective function or minimizing its cost. We summarize the above-mentioned methods in

Table 1. Methods in related studies.

Method Type	Algorithm	Advantage	Disadvantage
Sampling-based	RRT, RRT*, TD-RRT*, RRT-connect, etc.	High exploration efficiency and strong spatial exploration capability	No guarantee on optimality
Search-based	A*, safe A*, etc.	Optimality, completeness, and efficiency	High memory consumption and lack of adaptability to dynamic environments
	D*, enhanced D*, etc.	Efficient replanning ability	No guarantee on optimality
Learning-based	MDP, DRL/DQN, etc.	No explicit model required and efficiency	Expensive learning cost and no guarantee on safety or optimality
	MCTS, SL-MCTS, etc.	No/little domain knowledge required and efficiency	Expensive simulation cost and no guarantee on safety or optimality

, classified into three types.

There are also many other methods. Tonola et al. [27] effectively addressed the motion planning of the issue by leveraging a set of offline pre-computed paths to enable efficient online re-planning and trajectory generation. Alvankarian et al. [28] proposed an optimal, local path-planning algorithm with IsoCost-based dynamic programming, which overcomes obstacles of any shape. In industrial human–robot collaboration scenarios, safety and efficiency are two crucial factors for human–robot collaboration. It is challenging to ensure human safety while not sacrificing task efficiency.

To balance safety and productivity, Zhang et al. [24] developed an online collision-detection and identification scheme that can be triggered before further injuries are caused. On the other hand, in order to facilitate collaboration, it is vital to collect the state of humans as well as predict their motions. The most common approach is human aware-based robot motion planning [20,29,30,31]. For example, Abdelshafy et al. [32] developed a safe human–robot interactive algorithm by proposing a real-time human awareness, detection, and avoidance algorithm, enabling the system to respond in real time to human presence and movement. Apart from the human safety, human comfort also needs to be considered. Aéraïz-Bekkis et al. [33] discussed how movement uncertainty of a robot can provide a unified explanation for the modulation of human comfort with robots.

Another approach is a statistical learning method that can make a highly accurate prediction of a person’s long-term motion. For instance, Kamezaki et al. [34] proposed a new concept of a dynamic collaborative workspace based on humans’ predicted trajectory. Similarly, Chen et al. [35] presented a human–robot safety framework based on human hand trajectory prediction. In addition to human prediction, Xiao et al. [36] employed a 3D convolutional neural network for human action recognition.

The literature focuses almost entirely on the motion planning of robotic arms, whose primary objective is to avoid collisions with humans rather than achieve human–robot collaborative operations in the true sense.

Bruckschen et al. [37] presented a novel human-aware navigation approach that relies on the long-term prediction of human movements and is applied to a service robot acting in indoor environments. Literature on human–robot collaboration of this kind is relatively scarce. The main reasons for this are rooted in uncertainty. In dynamic human environments such as warehouses, supermarkets, and workshops, the behavior of human operators is unpredictable and, therefore, difficult to integrate into the robot system’s path-planning analysis. As such, risk constraints are applied in path planning [38,39]; however, risk constraints are hard to estimate.

In this paper, we construct hard and soft human risk constraints by simulating human trajectories and calculating risk distributions, which are efficient and simple to compute. Unlike traditional risk-aware methods, we generate multiple diverse paths distributed over the whole environment and solve potential conflicts with both hard and soft constraints to find an optimal safe path, ensuring the safety of the robot and humans, instead of online replanning solutions.

3. Problem Description and Formulation

A wheeled humanoid robot (hereafter referred to as “robot”) and K humans ($K\in {\mathbb{N}}_{+}$) work in a shared environment. Both the robot and humans have their own tasks. The robot and each human are assigned a goal position that they have to arrive at, where the following are true:

• $S^r$ is the finite state set of the robot, $s_t^r\in S$ defines the state of the robot at discrete time-step t, and $s_0^r$ and $s_g^r$ are the initial state and goal state, respectively,
• $S^h$ is the finite sate set of humans, $s_t^h=\{s_t^1,s_t^2\dots ,s_t^K\}\in S^h$ defines the state of K humans at time-step t, and $s_0^k$ and $s_g^k$ are the initial state and goal state of human k, respectively,
• $A=\{forward,backward,left,right,wait\}$ represents the action set of the robot and humans.

The robot’s movements are controlled by a system controller, whereas human movements are not. Each human moves toward his or her goal independently, without considering the robot. The trajectory of each human is unknown in advance and can only be observed after the human reaches the goal. For the current time $t_0$, we can observe the state of human k, $s_{t_0}^k$; however, $s_{t_0+1}^k$ is unknown.

The robot is required to reach its goal position within the given time budget T while avoiding conflicts with humans. Let $x=s_0^r,s_1^r,\dots ,s_t^r,\dots ,s_T^r$ denote the path of the robot, where $s_T^r=s_g$. A conflict occurs when the robot and a human k either occupy the same position or traverse the same edge. Specifically,

• a vertex conflict occurs if $s_t^r=s_t^k$;
• an edge conflict occurs if $s_t^r=s_{t+1}^k$ and $s_{t+1}^r=s_t^k$.

A task is successfully completed if and only if the robot reaches the goal without collisions and within the time budget T; otherwise, it is regarded as a failure.

The problem is formulated as resolving potential conflicts between the robot and humans and planning a safe path for the robot in the absence of prior knowledge about human motions. Unlike traditional approaches, we aim to solve potential conflicts at the planning level instead of maneuvering around human beings during encounters.

4. Methodology

The architecture of the proposed SOMP is illustrated in Figure 2. It consists of four main modules: the $\eta$-diverse path generator, the human risk creator, the conflict solver, and the safety-first optimizer. Specifically, the $\eta$-diverse path generator generates multiple initial paths to form the initial solution set. The human risk creator predicts the trajectories of randomly moving humans and computes both hard and soft constraints. In the conflict solver, potential high-risk conflicts are solved based on hard constraints. Finally, an optimal path is obtained through safety-first optimization with soft constraints. The functionality of each module is described in detail as follows.

4.1. $\eta$-Diverse Path Generator: Robust Initial Path Set

In this section, we introduce the $\eta$-diverse path generator ($\eta$-DPG) to produce multiple diverse paths distributed across the entire environment. The core part is a dynamic quadrant stochastic sampling RRT method with a diversity metric $\eta$.

Here, a map $G=<V,E>$ with a 2D grid world of $M_x\times M_y$. Figure 3 illustrates an example in which the environment is divided into four quadrants, denoted as $q_1$, $q_2$, $q_3$, and $q_4$. This approach is commonly applied in settings such as warehouses and unmanned supermarkets, where shelving structures exist.

Let $V_i$ define the vertex set of vertices in quadrant $q_i$. Hence, $V=\{V_1,V_2,V_3,V_4\}$ and $V=V_1\cup V_2\cup V_3\cup V_4$. For any $\forall V_i\in V$, we record those vertices already visited in path x as ${\overline{V}}_i$. That is,

$$\forall v\in V,v\in V_i\wedge v\in x\Rightarrow v\in {\overline{V}}_i.$$

(1)

For each quadrant, $q_i$, we define an exploration rate $\delta \left(q_i\right)$ to show its exploring process, which is calculated by

$$\delta \left(q_i\right)={\displaystyle \frac{|{\overline{V}}_i|}{|V_i|}},$$

(2)

where $|·|$ is the size of a set.

At the beginning, the initial paths set $\chi =\varnothing$. For any quadrant $q_i$, ${\overline{V}}_i=\varnothing$; thus, the exploration rate, $\delta \left(q_i\right)=0$, is written as

$$\chi =\varnothing \Rightarrow \forall V_i\subset V,\left|V_i\right|=0,\delta \left(q_i\right)=0.$$

(3)

Once a path is found, $\chi =\{x_1,\dots \}$. At least a quadrant $q_i$ is explored, and its exploration rate $0<\delta \left(V_i\right)$.

$$\left|\chi \right|>0\Rightarrow \exists V_i\subset V,\left|{\overline{V}}_i\right|>0,\delta \left(q_i\right)>0.$$

(4)

If quadrant $q_i$ is fully explored, that is, all vertices of quadrant i have been added to $\overline{V_i}$, ${\overline{V}}_i=V_i$ and the exploration rate $\delta \left(q_i\right)=1$.

$$\left|\chi \right|>0,V_i={\overline{V}}_i\Rightarrow \delta \left(q_i\right)=1.$$

(5)

Let ${\alpha }_i$ define the sampling rate of $q_i$. We propose a dynamic quadrant stochastic sampling policy. For each quadrant $q_i$, the sampling rate ${\alpha }_i$ dynamically changes along the exploration rate $\delta \left(q_i\right)$, where

$${\alpha }_i={\displaystyle \frac{1-\delta \left(q_i\right)}{{\sum }_{j=1}^4(1-\delta \left(q_j\right))}}.$$

(6)

With initialization $\delta \left(q_1\right)=\delta \left(q_2\right)=\delta \left(q_3\right)=\delta \left(q_4\right)=0$, the sampling rate of each quadrant $q_i$ follows the uniform distribution, where ${\alpha }_i={\displaystyle \frac{\beta }{4}}$, where $\beta$ ($0<\beta <1$) is the probability that the robot chooses a dynamic quadrant stochastic sampling policy (DQSSP). The robot takes a probability of $1-\beta$ to sample the goal position.

The diversity metric $\eta$ for scenarios with shelving structures can be calculated by

$$\eta ={\displaystyle \frac{D}{M_x+M_y}},$$

(7)

where D is the length of a shelf. For a map with random obstacles or without obstacles, the $\eta$ is provided by the human. Based on the diversity metric $\eta$, the robot searches multiple diverse paths.

Figure 4 shows the framework of $\eta$-DPG. The basic searching structure is a SQSSP-based RRT. Let $\tau =<V_{\tau },E_{\tau }>$ be the search tree, where $V_{\tau }$ is the node set and $E_{\tau }$ is the edge set. $n_0$ is the root node. $n_0=s_0^r$, and $n_g=s_g^r$ is the goal node.

Based on the SQSSP, a sampling point $n_{rand}$ is obtained. Then, for $\forall n\in V_{\tau }$, we calculate the Manhattan distance from n to $n_{rand}$, written as $Manh(n,n_{rand})$. A nearest-neighbor node $n_m$ with the minimum distance is chosen:

$$n_m\leftarrow argmi{n}_{n\in V_{\tau }}Manh(n,n_{rand}).$$

(8)

Hence, the tree grows in a direction from $n_m$ to $n_{rand}$. It moves from $n_m$ toward $n_{rand}$ and produces a new node $n^{\prime }$. If there are no obstacles between $n_m$ and $n^{\prime }$, we add $n^{\prime }$ as the child of $n_m$; otherwise, we discard it. We repeat sampling and growing the search tree until a path x is found.

We measure the diversity of path x against an existing path $x^{\prime }$ ($x^{\prime }\in \chi )$ using the Jaccard similarity:

$$Div(x,x^{\prime })=1-{\displaystyle \frac{|x\cap x^{\prime }|}{|x\cup x^{\prime }|}},$$

(9)

where $|x\cap x^{\prime }|$ is the size of the intersection set, and $|x\cup x^{\prime }|$ is the union set of two paths. If $Div(x,x^{\prime })\ge \eta$, add x to $\chi$; otherwise, discard x and regenerate a new path until the computational budget runs out or $\left|\chi \right|$ reaches the terminal condition, with a desired path number L. The procedures are shown in Algorithm 1.

Algorithm 1: η-DPG.

4.2. Human Risk Creator: Modeling Uncertain Human Motions

In this paper, we assume that human motions are not affected by the robot. We perform stochastic simulations to model the risks caused by humans, written as $\xi$. The risks are divided into hard constraints ${\xi }^H$ and soft constraints ${\xi }^S$. Hereby, we show how we model uncertain human motions and construct hard and soft constraints.

Figure 5 shows the stochastic motion model of a human k. We know that the human k will travel to his/her goal location. However, their trajectory is unknown. We model human motions as follows. The probability of a$(a\in A)$ is modeled as $p_a\in \{p_f,p_b,p_l,p_r,p_w\}$ with respect to A. Note that $p_a$ is unknown.

All humans follow the following policies:

• ${\pi }_1$: a human tends to favor actions that bring him/her closer to the goal;
• ${\pi }_2$: a human $k_1$ never collides with another human $k_2$, where $s_t^{k_1}=s_t^{k_2}$ and $s_t^{k_1}=s_{t+1}^{k_2}\wedge s_{t+1}^{k_1}=s_t^{k_2}$ never happen.

At state $s_t^k$, a human k takes an action $a_t$, and the state transition is denoted by $(s_t^k,a_t,s_{t+1}^k)$. The set of available actions for human k at $s_t^k$ is a subset of A, denoted as $A\left(s_t^k\right)$. We define $a^{*}$ as the specific action that minimizes the remaining distance to the goal, and

$$a^{*}\leftarrow arg\underset{a\in A\left(s_t^k\right)}{min}Manh(s_{t+1}^k,s_g^k).$$

(10)

Let $O_{pt}\subseteq A\left(h_t^k\right)$ be a set of prioritized actions comprising all possible optimal actions, $a^{*}$. To encourage goal-directed movements, we assign higher conditional probabilities to actions in C. Actions are sampled based on this principle from the following conditional probability distribution:

$$p\left(a_t|s_t^k\right)=\left\{\begin{array}{cc}{\displaystyle \frac{1-\zeta \left(|A\left(s_t^k\right)|-|O_{pt}|\right)}{|O_{pt}|}},\hfill & a_t\in O_{pt}\hfill \\ \theta ,\hfill & \mathrm{otherwise}\hfill \end{array}\right.,$$

(11)

where $|·|$ is the size of the action set, $0\le \theta <0.2$, and ${\sum }_{a_t\in A\left(s_t^k\right)}p\left(a_t\right|s_t^k)=1$.

With the observation $s_{t_0}^k=v$ (vertex v is occupied by human k), we simulate stochastic movements from $s_{t_0}^k$. Then, we calculate the conditional probability of v occupied by human k at $t(0<t\le T)$ by

$$p_{k,t}\left(v|s_{t_0}^k\right)={\displaystyle \frac{N_v}{N_s}},$$

(12)

where $N_s$ and $N_v$ represent the simulation times and visited times of v at t, respectively. The risk value of v occupied by K humans at t is indicated by $R(v,t)$, and 

$$R(v,t)=\sum _{k=1}^K{p}_{k,t}\left(v|s_{t_0}^k\right).$$

(13)

The human risk set $\xi =\{{\xi }_{t_0},{\xi }_{t_0+1},\dots ,{\xi }_{t_0+T}\}$ is obtained. Based on $\xi$, we classify it into hard constraints ${\xi }^H$ and soft constraints ${\xi }^S$.

• $\xi ={\xi }^H\cup {\xi }^S$;
• $\forall {\xi }_t\in \xi$, ${\xi }_t=\{R(v_1,t),R(v_2,t),\dots \}$;
• $\forall R(v,t)\in {\xi }_t$, if $R(v,t)\ge \gamma E\left({\xi }_t\right)$, we have $R(v,t)\in {\xi }_t^H$, where $\gamma$ is a scale parameter, and $E\left({\xi }_t\right)$ is the mean of $R(v,t)$ ($R(v,t)\in {\xi }_t$) and ${\xi }_t^H$ contains those vertices with higher risk than $\gamma E\left({\xi }_t\right)$;
• ${\xi }_t^S$ contains those vertices with lower risk than $\gamma E\left({\xi }_t\right)$;
• ${\xi }_t={\xi }_t^H\cup {\xi }_t^S$.

Algorithm 2 shows the procedures of creating hard and soft constraints.

Algorithm 2: Hard and soft constraint creator.

4.3. Conflict Solver: Hard Constraint-Based Resolving

We solve conflicts by prohibiting the robot from entering high-risk areas, as defined by the hard constraints, ${\xi }^H$. A key advantage of this approach is that it provides a safety guarantee. The framework is shown as Figure 6. We apply hard constraints ${\xi }^H=\{{\xi }_1^H,{\xi }_2^H,\dots ,{\xi }_T^H\}$ to each path x ($x\in \chi$) and check whether potential conflicts exist.

For any path $x=\{s_0^r,\dots ,s_t^r,\dots ,s_g^r\}$, if $s_t^r=v$ and $R(v,t)\in {\xi }^H$, the robot occupies vertex v, where it encounters a high risk of colliding with a human. We consider this condition as a potential conflict. In detail, a potential conflict exists if

• $R(s_t^r,t)\in {\xi }_t^H$ (potential vertex conflict);
• $R(s_t^r,t+1)\in {\xi }_t^H\wedge R(s_{t+1}^r,t)\in {\xi }_t^H$ (potential edge conflict)

We solve these potential conflicts by applying hard constraints to the robot. Hereafter, we show the details.

For each time step t,

• check a potential conflict at state $s_t^r=v$ exists or not;
• if a conflict exists, directly forbid the robot from occupying v at time step t by adding the action $wait$;
• $s_T^r\leftarrow s_{T-1}^r$, …, $s_{t+1}^r\leftarrow s_t^r=v$;
• $s_t^r\leftarrow s_{t-1}^r=v^{\prime }$;
• repeat until no potential conflicts exist, and update x.

Finally, we check for goal satisfaction ($s_T^r=s_g^r$) and prune any updated path x from $\chi$ that fails this condition.

4.4. Safety-First Optimization: Soft Constraint-Based Risk Minimization

Hard constraints ${\xi }^H$ prevent the robot from entering areas with a high risk of conflict. However, due to unknown human motions, not all constraints can be enforced as hard constraints, as this could result in no path being found. Therefore, we model these low-conflict-risk areas as soft constraints ${\xi }^S$, as shown in Figure 7.

During the current stage, each path x of a multiple diverse path set $\chi$ avoids hard constraints. Based on the soft constraints ${\xi }^S$, we define a risk function $f\left(x\right)$ as

$$f\left(x\right)=\sum _{s_t^r\in x}R(s_t^r,t)+\sum _{s_t^r,s_{t+1}^r\in x}R(s_t^r,t+1)\times R(s_{t+1}^r,t).$$

(14)

Based on the risk level associated with each x, we can obtain a path with minimum risk by

$$x^{*}\leftarrow argmi{n}_{x\in \chi }f\left(x\right).$$

(15)

Hence, we can obtain a set of paths ${\chi }^{*}=\{x_1^{*},x_2^{*},\dots \}$ based on the safety-first policy. Finally, following the safety-first and shortest-second policies, we consider the length of the path and generate an optimal path from ${\chi }^{*}$ by

$$x^{*}\leftarrow argmi{n}_{x\in {\chi }^{*}}\left|x\right|,$$

(16)

where $|·|$ computes the length of a path from $s_0^r$ to goal $s_g^r$.

We summarize how an optimal safe path is generated based on the hard and soft constraints in Algorithm 3.

Algorithm 3: Conflict resolve and optimization-based on hard and soft constraints.

5. Results and Discussions

In this paper, the main scope is to verify the effectiveness of the proposed approach in reducing conflicts while maintaining a high task success rate. For a fair comparison, all methods are evaluated under the same motion model. During the current stage, we use a naive motion model, which is simple but enough for algorithm verification. In the future, we can exchange the motion model with a realistic one as the proposed algorithm is compatible with different motion models.

A series of simulations are conducted to verify the proposed method, and a comparative analysis is performed with established conventional methods: A*, RRT, MDP, and DDQN. These algorithms serve as representatives of search-based, sampling-based, and learning-based (RL and DRL) methods, respectively. We verify our algorithm under the following scenarios:

• Scenario 1: investigation on parameter $\gamma$—how the hard and soft constraints affect the algorithm’s performance.
• Scenario 2: verification with different size of maps: $10\times 20$, $20\times 40$, and $40\times 80$ warehouse grid maps [40], as shown in Figure 8.
• Scenario 3: verification with an increasing number if humans K.

The parameters are set as in Table 2. To evaluate the quality of each path, we use a reward scheme with the reward design as follows:

• Goal reward: $1.0$ if the robot reaches the goal.
• Step cost: $-0.01$ cost for a robot moving during each step.
• Conflict penalty: $-0.1$ if the robot collides with a human.

We run 100 simulations for each scenario and evaluate the performance by the average conflict number, task success rate, and path reward. The simulations are performed with a device with an Intel Core i9-10900 CPU running at 2.80 GHz and 32 GB of memory. The simulations are conducted in a custom Python (version 3.12.7)-based software environment.

5.1. Results on Scenario 1: Investigation on $\gamma$—How Hard and Soft Constraints Affect the Algorithm’s Performance

Figure 9 illustrates the restrictiveness of hard and soft constraints with $\gamma$. Figure 10 presents the experimental results of analyzing parameter $\gamma$ under an environment configuration of a $10\times 20$ map with $K=2$ humans. We resolve conflicts by employing both hard and soft constraints, which are defined based on parameter $\gamma$ and the mean risk value. Vertices satisfying $R(v,t)\ge \gamma E\left({\xi }_t\right)$ are assigned to the hard constraint set ${\xi }_t^H$, representing hazardous regions that the robot must avoid. As $\gamma$ decreases, more vertices are incorporated into the hard constraint set, making it increasingly restrictive (as illustrated in Figure 9). In the meantime, the conflict number also decreases, as shown in Figure 10. In addition, it narrows the feasible region. Consequently, an excessively small $\gamma$ may lead to the absence of a valid solution. As shown in Figure 10, no feasible path is found when $\gamma <0.07$.

Table 2. Parameter setting.

Parameter		Value
Simulation times of humans	$N_s$	2000
Probability of DQSSP	$\beta$	0.9
Probability of Goal-bias	$1-\beta$	0.1 *
Scale parameter	$\gamma$	$0.07$ **

* $1-\beta =0.1$: the goal-bias probability is usually set from 0.05 to 0.1, where an excessively large bias may compromise global exploration [41]. ** $\gamma =0.07$: it receives the highest task success rate and the least conflict number (see Figure 10).

Table 2. Parameter setting.

Parameter		Value
Simulation times of humans	$N_s$	2000
Probability of DQSSP	$\beta$	0.9
Probability of Goal-bias	$1-\beta$	0.1 *
Scale parameter	$\gamma$	$0.07$ **

* $1-\beta =0.1$: the goal-bias probability is usually set from 0.05 to 0.1, where an excessively large bias may compromise global exploration [41]. ** $\gamma =0.07$: it receives the highest task success rate and the least conflict number (see Figure 10).

When $\gamma$ increases, the role of soft constraints becomes more significant. Unlike hard constraints, soft constraints do not prohibit the robot from visiting high-risk vertices; instead, they assess the overall risk level of a path and select the path with the minimum risk. A major limitation of soft constraints is that they cannot provide a safety guarantee, and the generated path may still visit vertices with high risk values. This explains why the conflict number increases with $\gamma$, as illustrated in Figure 10. In the meantime, the task success rate decreases. As illustrated in Figure 10, the reward increases as $\gamma$ becomes larger. The reason is that a higher $\gamma$ relaxes the constraints, enabling the robot to take shorter paths, whereas a smaller $\gamma$ forces the robot to detour around hard-constraint vertices, resulting in longer paths. As shown in Figure 10, the hard constraints have almost no effect when $\gamma \ge 0.5$. The reward, task success rate, and conflict number ($\gamma \ge 0.5$) are the same as without using hard constraints, shown as “wo” in Figure 10.

In this paper, we employ both hard and soft constraints: hard constraints provide safety guarantees by prohibiting access to high-risk areas, while soft constraints optimize the overall risk level of the path. By comprehensively considering the task success rate and conflict number, which are shown in Figure 10, $\gamma =0.07$ is a good choice with a higher task success rate and fewer conflicts. Therefore, in the following simulations, we set $\gamma =0.07$.

5.2. Results on Scenario 2: Different Maps, 10 × 20, 20 × 40, and 40 × 80

In scenario 2, three different sizes of warehouse grid maps (Figure 8) are used for the verification of our proposed method. The performance evaluation compares four methods—A*, RRT, MDP, and DDQN—across three key metrics: conflict number, task success rate, and reward.

Table 3. Average results under scenario 1 with human number $K=2$: different sizes of warehouse grid map. The best results on each item are highlighted in bold font.

Map	Methods	Conflict Number	Task Success Rate	Reward
$10\times 20$	A*	0.75 ± 1.13	0.50 ± 0.50	0.68 ± 0.11
	RRT	0.71 ± 0.97	0.50 ± 0.50	0.68 ± 0.10
	MDP	0.55 ± 0.97	0.58 ± 0.49	0.66 ± 0.18
	DDQN	0.58 ± 0.55	0.44 ± 0.50	0.60 ± 0.12
	Proposed	0.01 ± 0.10	0.99 ± 0.10	0.54 ± 0.02
$20\times 40$	A*	0.51 ± 0.96	0.67 ± 0.47	0.47 ± 0.10
	RRT	0.43 ± 0.59	0.62 ± 0.49	0.47 ± 0.06
	MDP	0.41 ± 0.69	0.65 ± 0.47	0.47 ± 0.07
	DDQN	0.51 ± 0.96	0.28 ± 0.45	−0.22 ± 0.60
	Proposed	0.00 ± 0.00	1.00 ± 0.00	0.37 ± 0.04
$40\times 80$	A*	0.30 ± 0.82	0.81 ± 0.39	−0.02 ± 0.08
	RRT	0.24 ± 0.57	0.80 ± 0.57	−0.02 ± 0.06
	MDP	0.17 ± 0.76	0.90 ± 0.30	−0.01 ± 0.08
	DDQN	0.01 ± 0.10	0.00 ± 0.00	−1.50 ± 0.13
	Proposed	0.00 ± 0.00	1.00 ± 0.00	−0.26 ± 0.05

and Figure 11 and Figure 12 summarize the comparison results. The performance of the proposed method is comprehensively evaluated against the other four baseline algorithms.

The simulation results demonstrate that the proposed method outperforms the other four methods across the $10\times 20$, $20\times 40$, and $40\times 80$ warehouse grid maps. As observed, the proposed approach consistently achieves the lowest conflict number and the highest task success rate under all experimental settings. Specifically, in the $10\times 20$ map, the proposed method achieves an average conflict number of $0.01$, which is significantly lower than those of other methods: A* (0.75), RRT (0.71), MDP (0.55), and DDQN (0.58). Meanwhile, the task success rate reaches $0.99\pm 0.10$, indicating nearly perfect completion performance. Similar trends are observed in larger maps ($20\times 40$ and $40\times 80$), where the proposed method maintains a zero-conflict rate and a success rate of $1.00\pm 0.00$, demonstrating superior robustness and scalability.

In contrast, traditional search-based method A* and sampling-based method RRT exhibit frequent conflicts, which reflects their limited adaptability in dynamic environments. The learning-based DDQN shows unstable performance with fluctuating rewards, especially when increasing the map size, due to the sparse reward. The MDP approach performs substantially better; however, its performance still falls far short of the proposed method. When the map size increases, it becomes a sparse environment, and the collision risk between the robot and humans becomes increasingly diluted. This explains why the average conflict number decreases from map $10\times 20$ to $20\times 40$, $40\times 80$.

Note that DDQN achieves the average conflict number of 0.01 in map $40\times 80$; however, it performs the worst among all methods as the robot fails to reach the goal position in all 100 simulations, while MDP achieves a success rate of $0.9$. There are several factors. One is the sparse reward. Another reason is that the growing state space makes it impossible for the DDQN agent to sufficiently explore or approximate the value function over all possible configurations. In comparison, the MDP solver exploits the known transition structure and computes global optimal values via explicit value iteration, without the need for exhaustive sampling. Moreover, DDQN relies on neural function approximation, which introduces estimation bias and instability in high-dimensional sparse-reward settings. When the agent encounters unvisited or rarely sampled states—common in large maps—its Q-value predictions become unreliable, leading to suboptimal or inconsistent policies. In contrast, MDP methods guarantee convergence to the optimal policy given an accurate transition model, ensuring consistent performance. What is more, in dynamic environments where human obstacles move stochastically, the system exhibits non-stationary transitions. Model-free algorithms like DDQN tend to overfit past experiences and fail to adapt to the shifting transition probabilities, whereas MDP frameworks can explicitly encode such stochasticity within the transition model and maintain robustness under varying human behaviors.

Figure 11d shows the conflict numbers at each time step of 100 simulations. We can easily determine that most of the conflicts occur from time step $t=10$ to $t=24$ in the four baseline methods. In the proposed approach, there is no conflict from time step $t=10$ to $t=24$, which proves the efficiency of the proposed SOMP planner in resolving conflicts with humans based on the hard and soft constraints.

Overall, these results validate that the proposed method effectively balances safety and efficiency, significantly outperforming benchmark algorithms across varying map complexities. The findings confirm that integrating risk-aware constraints into the planning process leads to more reliable and globally consistent navigation behavior. This also further confirms our concept that we can solve conflicts at the planning level instead of continuously replanning.

Figure 13 illustrates the process of the proposed SOMP method. Initially, eight diverse paths were generated (top figure). These were then processed by the conflict solver, in which the dangerous areas in the hard constraints set were avoided, and the paths were eliminated to four paths (middle figure). Subsequently, the application of the safety-first optimization criterion selected a single optimal path, as shown in the bottom figure. As shown in Figure 13, the proposed method did not prioritize the shortest path but instead generated a safer trajectory that directed the robot upward, away from regions occupied by humans. The dashed lines depict representative trajectories of human agents.

5.3. Results on Scenario 3: Increasing Human Numbers K = 4, 6, 8, 10, 15

In this scenario, we evaluate the performance of the proposed method on a more complex environment by increasing the number of humans K. The simulation environment is a $40\times 80$ grid world involving 4 ∼ 15 humans, as shown in Figure 8c.

Table 4. Average results under scenario 3 with a different number of humans K: sparse to crowd in a $40\times 80$ grid map. The best results on each item are highlighted in bold font. “–”: poor performance and no single task is achieved.

Human Number K	Methods	Conflict Number	Task Success Rate	Reward
4	A*	0.51 ± 0.80	0.64 ± 0.48	−0.04 ± 0.08
	RRT	0.45 ± 0.75	0.67 ± 0.47	−0.04 ± 0.08
	MDP	0.45 ± 1.40	0.74 ± 0.43	−0.07 ± 0.27
	DDQN	–	–	–
	Proposed	0.00 ± 0.00	1.00 ± 0.00	−0.27 ± 0.05
6	A*	0.55 ± 0.85	0.59 ± 0.49	−0.05 ± 0.09
	RRT	0.64 ± 1.36	0.60 ± 0.49	−0.06 ± 0.14
	MDP	0.48 ± 0.98	0.67 ± 0.47	−0.09 ± 0.24
	DDQN	–	–	–
	Proposed	0.09 ± 0.38	0.93 ± 0.26	−0.32 ± 0.05
8	A*	0.96 ± 1.79	0.51 ± 0.50	−0.09 ± 0.18
	RRT	0.96 ± 1.51	0.51 ± 0.50	−0.09 ± 0.15
	MDP	1.14 ± 2.76	0.55 ± 0.50	−0.22 ± 0.42
	DDQN	–	–	–
	Proposed	0.07 ± 0.29	0.94 ± 0.24	−0.28 ± 0.11
10	A*	0.99 ± 1.33	0.44 ± 0.50	−0.08 ± 0.13
	RRT	1.35 ± 2.36	0.42 ± 0.49	−0.13 ± 0.24
	MDP	0.93 ± 1.32	0.47 ± 0.49	−0.21 ± 0.36
	DDQN	–	–	–
	Proposed	0.07 ± 0.29	0.94 ± 0.24	−0.27 ± 0.12
15	A*	1.80 ± 2.15	0.25 ± 0.43	−0.16 ± 0.21
	RRT	1.71 ± 2.24	0.32 ± 0.47	−0.17 ± 0.23
	MDP	1.40 ± 2.28	0.35 ± 0.48	−0.19 ± 0.34
	DDQN	–	–	–
	Proposed	0.40 ± 0.65	0.67 ± 0.47	−0.05 ± 0.11

and Figure 14 summarize the simulation results. In this scenario, DDQN shows a poor performance and no single task is achieved.

When $K=4$, the proposed approach completely eliminates conflicts ($0.00\pm 0.00$) and achieves a perfect task success rate ($1.00\pm 0.00$), outperforming all baseline methods (A*, RRT, MDP), which exhibit non-zero conflicts and lower success rates. Although the proposed method yields a slightly lower reward in this case, its ability to ensure full task completion without conflicts highlights its effectiveness in sparse scenarios.

As the number of humans increases to $K=6,8$, and 10, the proposed method continues to maintain near-zero conflict counts and high task success rates (ranging from 0.93 to 0.94), while all baselines show a clear decline in performance with higher conflict numbers and reduced success rates. Notably, the DDQN method fails completely in these settings, achieving no successful tasks.

In a more crowded setting with $K=15$, the proposed method still achieves a lower conflict number (0.40 ± 0.65) and a higher task success rate (0.67 ± 0.47) compared to the best-performing baseline. Moreover, it attains a superior reward (−0.05 ± 0.11), indicating better overall performance in dense environments where other methods struggle. The proposed method reduces conflicts by 77.8%, 76.6%, and 71.4% $\left({\displaystyle \frac{1.80-0.40}{1.80}}=77.8\%,{\displaystyle \frac{1.71-0.40}{1.71}}=76.6\%,{\displaystyle \frac{1.40-0.40}{1.40}}=71.4\%\right)$; and improves the task success rate by 168.0%, 109.4%, and 91.4% $\left({\displaystyle \frac{0.67-0.25}{0.25}}=168.0\%,{\displaystyle \frac{0.67-0.32}{0.32}}=109.4\%,{\displaystyle \frac{0.67-0.35}{0.35}}=91.4\%\right)$, compared with A*, RRT, and MDP, respectively. Figure 14 shows the trend of conflict number, task success rate, and reward along human numbers. Figure 15 shows the details of the conflict that occurs during each time step.

In summary, with an increasing number of humans, the environment becomes increasingly crowded and complex. This results in the increment of conflicts (Figure 14a) and the decrement of the task success rate (Figure 14b). The simulation results show that the proposed method exhibits strong scalability and robustness across varying human densities, significantly reducing conflicts and maintaining high task completion rates, especially in sparse to moderately crowded scenarios. Its performance remains competitive even under crowded conditions, where conventional methods show notable degradation.

The simulation results show that the proposed algorithm is more robust to environment changes. The robustness mainly comes from the $\eta$-diverse path generator as it explores diverse paths all over the environment. This provides the robustness to uncertainties; however, it also increases the computational cost. The hard constraint-based conflict solver provides a safety guarantee that the robot never visits dangerous areas, and soft constraint-based safety optimization finds an optimal path.

In this paper, we pay more attention to finding a safe path and do not consider the computational time. We show the computational time in

Table 5. Comparison of the computational time.

Map Size	Number of Humans	Methods	Time (s)
$10\times 20$	2	A*	0.01
		RRT	0.01
		MDP	52.75
		DDQN	341.13
		Proposed	5.04
$20\times 40$	2	A*	0.01
		RRT	0.01
		MDP	103.05
		DDQN	1739.40
		Proposed	5.22
$40\times 80$	2	A*	0.01
		RRT	0.02
		MDP	249.51
		DDQN	6475.19
		Proposed	11.19
$40\times 80$	4	Proposed	13.32
	6		15.46
	8		16.72
	10		18.49
	15		26.57

. The computational cost of the proposed method is higher than that of A* and RRT. As shown in Table 5, the computation time is affected by both the map size and the number of humans in the environment. When the map size increases, the computational time grows, mainly because the algorithm needs more time to generate multiple diverse paths. Similarly, as the number of humans increases, the computational time also rises due to the additional effort required for conflict resolution. Specifically, a larger number of humans increases the likelihood of human–robot conflicts and results in more vertices being treated as hard constraints. Consequently, the algorithm requires more time to resolve more potential conflicts.

5.4. Limitations and Future Studies

During the current stage, our focus is primarily on resolving conflicts at the planning level, while certain aspects are not yet considered. There are several limitations. First, although the proposed method substantially reduces the number of conflicts, it does not provide a theoretical safety guarantee. This is the key barrier to deployment in real-world environments. Second, a more realistic human motion model should be investigated. The current motion model is used for algorithm verification. However, it is not sufficient for real-world applications. Although human–robot collaboration has been widely studied, many companies and logistics centers still restrict close human–robot co-working in warehouses, largely because no existing algorithm can simultaneously provide rigorous safety guarantees and maintain high operational efficiency. In human–robot co-working scenarios in logistics and warehousing, the potential effects of robots on humans’ psychological perceptions and behavioral patterns remain insufficiently understood and warrant further investigation.

In the future, we will optimize the proposed method to enhance path safety and accelerate the searching speed. The proposed algorithm is compatible with different human motion models. We will expand our validation with more realistic human motion models to include more complex environments.

6. Conclusions

In this study, we focused on resolving human–robot conflicts at the planning level for a wheeled humanoid robot in environments shared with stochastically moving humans. A safety-oriented motion planning (SOMP) algorithm was proposed to generate a safe path that is robust to future uncertainties. The algorithm employs a fusion mechanism of hard and soft constraints to improve path safety, where hard constraints provide a safety guarantee that the robot never visits high-risk areas. We verified the proposed algorithm in a warehouse grid environment with different map sizes and human densities. Simulation results have proved the effectiveness of the proposed method in resolving conflicts and achieving a high task success rate in a human-shared environment. In the future, we will verify our method in more complex environments with a realistic human motion model. In addition, we will improve our method to reduce the computational time.