BVIP Guiding System with Adaptability to Individual Differences

Before collecting the data, we mapped the experimental environment and pre-set several paths. We invited 10 volunteers to hold the leash which was attached to the guide robot, who guided them from the starting point to the target. All volunteers were asked to wear blindfolds, earmuffs and our positioning vest. When collecting data, we used a four wheeled robot as its movement is smoother than quadruped robots, resulting in less oscillation and higher data quality.

Our sampling strategy was as follows: (a). The experiment assistant remotely controlled the robot to follow the planned path from the starting point to the target, and adjusted the speed to maintain a certain distance from different human agents. (b). The tension of the leash motor was randomly changed every 10 seconds, ranging from 2N to 20N. (c). The sampling frequency was set to be 50 Hz. (d). The collected data included the position of the robot $\mathbf{x}^{r}$, the position of the detected AprilTags, the magnitude of the tension $F$, and the direction of the leash $\mathbf{e}_{l}$.

After collecting the data, we further processed them. We obtained the relative distance of the human agent and the robot $l$ by averaging the positions of the detected AprilTags. Human agent’s positions were calculated by adding the relative positions to the robot’s positions:

$\displaystyle\mathbf{x}^{h}=\mathbf{x}^{r}+l\mathbf{e}_{l}$

(1)

The sampled tension magnitudes was decomposed into orthogonal components:

$\displaystyle\mathbf{F}^{h}=-F\mathbf{e}_{l}={[F^{h}_{x},F^{h}_{y},F^{h}_{z}]}^{\intercal}$

(2)

By taking the derivatives of human agent’s positions, we obtained human agent’s velocities $\mathbf{v}^{h}$. (d). The data was smoothed and filtered to acquire better quality. (e). The data was segmented into the form:

$\displaystyle(\mathbf{v}_{k-W^{h}+1:k}^{h},\mathbf{F}_{k-W^{h}+1:k}^{h}|\mathbf{v}_{k+1}^{h})$

(3)

, which means human agent’s velocities and the received tensions in past $W^{h}$ timesteps were set to be the input of HMP, and the velocity of next timestep were set to be the label.

We use 3 well-known architecture varients for the sequence predicting task: CNN, LSTM and TCN.

In order to regress the predicted velocity to the real velocity we collected, we designed a loss function:

$\displaystyle\widetilde{\mathbf{v}}^{h}_{k}=f_{\textbf{HMP}}(\mathbf{v}_{k-W^{h}:k-1}^{h},\mathbf{F}_{k-W^{h}:k-1}^{h})$

(4)

$\displaystyle\widetilde{\mathbf{x}}^{h}_{k+1}=\widetilde{\mathbf{x}}^{h}_{k}+\widetilde{\mathbf{v}}^{h}_{k}T$

(5)

$\displaystyle loss=L_{2}(\widetilde{\mathbf{x}}^{h},\mathbf{x}^{h})$

(6)

Moreover, we use the optimizer called Adam[18] to optimize the model parameters.

Our sampling strategy was as follows: (a). Set the quadruped robot to walk at random constant speeds. (b). Applying forces of varying magnitudes, durations and directions to the quadruped robot using our force controlling device. (c). The sampling frequency was set to be 50 Hz. (d). The collected data includes the velocity of the robot $\mathbf{v}^{r}$, the control input of the Quadruped Robot Controller $\mathbf{u}^{r}$, the magnitude of the tension $F$, and the direction of the leash $\mathbf{e}_{l}$.

After the collection, we decomposed the tension into orthogonal components:

$\displaystyle\mathbf{F}^{r}=F\mathbf{e}_{l}={[F^{r}_{x},F^{r}_{y},F^{r}_{z}]}^{\intercal}$

(7)

We also filtered the data, and segment the data into the form:

$\displaystyle(\mathbf{v}_{k-W^{h}+1:k}^{r},\mathbf{u}_{k-W^{h}+1:k}^{r},\mathbf{F}_{k-W^{h}+1:k}^{r}|\mathbf{v}_{k+1}^{r})$

(8)

Our Robot Dynamics Model shares the 3 well-known architecture variants with our Human Motion Predictor, while the dimension of the input is revised.

The function of Human Motion Planner is to use optimization algorithms to solve for the best robot position and leash tension, in order to maximize the probability of human agents reaching the expected position planned by the Waypoints Selector. MPC algorithm consists of three parts: a predictive model, feedback correction, and optimization in range of a temporal window.

The model is already explained in section III.(B). We use deep neural network to predict the relationship of human agent’s motion and the received tension according to formula 4. While predicting human agent’s velocity in next $M^{h}$ timesteps, the prediction of the tension $\widetilde{\mathbf{F}}$ was set to be the input when $k>t$ rather than the feedback $\mathbf{F}$.

Then, human agent’s position can be predicted according to formula 5, and robot’s target position can be predicted according to:

$\displaystyle\widetilde{\mathbf{x}}^{r*}=\mathbf{\widetilde{x}}^{h}+l\mathbf{e}_{\theta}$

(11)

Next, to acquire the optimal control variables, we minimized the cost function:

	$\displaystyle J^{h}(\widetilde{\mathbf{F}}^{h},\widetilde{l},\widetilde{\theta})=\sum_{k=0}^{M^{h}-1}$	$\displaystyle(\|\widetilde{\mathbf{x}}^{h}_{k}-\mathbf{x}^{h*}_{k}\|_{\omega_{1}}$		(12a)
	$\displaystyle+\|\widetilde{\mathbf{F}}_{k+1}-\mathbf{\widetilde{F}}_{k}\|_{\omega_{2}}+\omega_{3}$	$\displaystyle(\|\mathbf{\widetilde{F}}_{k+1}\|-\|\mathbf{\widetilde{F}}_{k}\|)^{2}$		(12b)
	$\displaystyle+\omega_{4}(1-\cos(\widetilde{\theta}_{k+1}-\widetilde{\theta}_{k}$	$\displaystyle))+\omega_{5}(\widetilde{l}_{k+1}-\widetilde{l}_{k})^{2})$		(12c)
	$$F_{min}<\|\widetilde{\mathbf{F}}_{k}\|<F_{max}$$			(12d)
	$$l_{min}<\widetilde{l}_{k}<l_{max}$$			(12e)
	$$\left\langle\widetilde{\mathbf{F}}_{k},\widetilde{\mathbf{F}}_{k+1}\right\rangle\leq\varphi_{\mathbf{F}}$$			(12f)
	$$\left\langle\mathbf{e}_{\widetilde{\mathbf{F}}_{k}},\mathbf{e}_{\widetilde{\theta}_{k}}\right\rangle\leq\varphi_{\theta}$$			(12g)
	$$\|\widetilde{\mathbf{x}}_{k}^{h}-\mathbf{x}_{j}^{obs}\|{\geq}r^{obs}$$			(12h)
	$$\|\widetilde{\mathbf{x}}^{r*}_{k}-\mathbf{x}^{obs}_{j}\|{\geq}r^{obs}$$			(12i)

where $\|{\mathbf{x}}\|_{\omega}=\frac{1}{2}\omega\mathbf{x}^{\intercal}\mathbf{x}$. $\omega_{1},\omega_{2}\in\mathbb{R}^{3\times 3},\omega_{3},\omega_{4},\omega_{5}\in\mathbb{R}$ are weight coefficients, $F_{min},F_{max},l_{min},l_{max},\varphi_{\mathbf{F}},\varphi_{\theta},r^{obs}$ are the the thresholds.

As for the cost function, minimizing $\sum_{k=0}^{M^{h}-1}{\|\mathbf{\widetilde{x}}^{h}_{k}-\mathbf{x}^{h*}_{k}\|}$ promises the predicted trajectory $\mathbf{\widetilde{x}}^{h}$ to approach the planned waypoints $\mathbf{x}^{h*}$; minimizing $\sum_{k=0}^{M^{h}-1}{\|\mathbf{\widetilde{F}}_{k+1}-\mathbf{\widetilde{F}}_{k}\|}$ and $\sum_{k=0}^{M^{h}-1}(\|\mathbf{\widetilde{F}}_{k+1}\|-\|\mathbf{\widetilde{F}}_{k}\|)$ promises the direction and magnitude of the force to change smoothly; and minimizing $\sum_{k=0}^{M^{h}-1}{(1-\cos(\widetilde{\theta}_{k+1}-\widetilde{\theta}_{k}))}$ and $\sum_{k=0}^{M^{h}-1}{(\widetilde{l}_{k+1}-\widetilde{l}_{k})^{2})}$ promises the direction and magnitude of the leash to change smoothly.

Finally, through the optimization in range of a temporal window, we obtained the optimal rope motor force $\mathbf{F}^{*}$, optimal relative distance $l^{*}$, and optimal relative yaw angle $\theta^{*}$. Robot’s target positions can be calculated according to:

$\displaystyle\mathbf{x}^{r*}=\mathbf{x}^{h*}+l^{*}\mathbf{e}_{\theta^{*}}$

(13)

The function of Robot Motion Planner is to plan the best control input for the Quadruped Robot Controller, so that the robot can reach the expected position planned by the Human Motion Planner.

The model is already explained in section III.(C). The model described robot’s velocity after being towed:

$\displaystyle\widetilde{\mathbf{v}}^{r}_{k}=f_{\textbf{RDM}}(\mathbf{v}_{k-W^{h}:k-1}^{r},\mathbf{u}_{k-W^{h}:k-1}^{r},\mathbf{F}_{k-W^{h}:k-1}^{r})$

(14)

While predicting robot’s velocity in next $M^{r}$ timesteps, the prediction of the control input $\widetilde{\mathbf{u}}^{r}$ was set to be the input when $k>t$ rather than the historic value $\mathbf{u}^{r}$. Robot’s position can be predicted according to:

$\displaystyle\widetilde{\mathbf{x}}^{r}_{k+1}=\widetilde{\mathbf{x}}^{r}_{k}+\mathbf{v}^{r}_{k}T$

(15)

Next, to acquire the optimal control variables, we minimized the cost function:

	$$J^{r}(\widetilde{\mathbf{u}}^{r})=\sum_{k=0}^{M^{r}-1}{(\|\widetilde{\mathbf{x}}^{r}_{k}-\mathbf{x}^{r*}_{k}\|_{\omega_{6}}+\|\widetilde{\mathbf{u}}^{r}_{k}\|_{\omega_{7}})}$$		(16a)
	$$\|\widetilde{\mathbf{u}}^{r}_{x}\|_{\infty}{\leq}u^{r}_{xmax}$$		(16b)
	$$\|\widetilde{\mathbf{u}}^{r}_{y}\|_{\infty}{\leq}u^{r}_{ymax}$$		(16c)
	$$\|\widetilde{\mathbf{u}}^{r}_{\omega}\|_{\infty}{\leq}u^{r}_{{\omega}max}$$		(16d)

where $\omega_{6},\omega_{7}\in\mathbb{R}^{3\times 3}$ are weight coefficients, $u^{r}_{xmax},u^{r}_{ymax},u^{r}_{{\omega}max}$ are the the thresholds.

As for the cost function, minimizing $\sum_{k=0}^{M^{r}-1}{\|\widetilde{\mathbf{x}}^{r}_{k}-\mathbf{x}^{r*}_{k}\|}$ promises robot’s predicted positions $\widetilde{\mathbf{x}}^{r}$ to approach the planned target positions $\mathbf{x}^{r*}$; and minimizing $\sum_{k=0}^{M^{r}-1}{\|\widetilde{\mathbf{u}}^{r}_{k}\|}$ promises the robot to walk efficiently.

Finally, through the optimization in range of a temporal window, we obtained the optimal input $\mathbf{u}^{r*}$ for the Quadruped Robot Controller.

To test the performance of our Human Motion Predictor, we compared our HMP model with the linear model proposed by Chen et al.[7] and the GeoC model proposed by Nanavati et al.[6].

As shown in TABLE II, we compare the performance of different models on our dataset using K-fold Cross-Validation. We calculate the error and standard deviation between the predicted human velocity and the ground truth velocity for each model. The result shows that our models has smaller error and standard deviation, indicating that our models is closer to the ground truth velocity and have a more stable performance. Thanks to its more advanced architecture, our HMP-TCN model performed the best among the three variants. It can be concluded that our model achieved a significant performance improvement.

To test the performance of our Robot Dynamics Model, we compared our model with the Velocity Discount Coefficient Matrix(VDCM) model proposed by Chen et al.[7]

As shown in TABLE III, we also use K-fold Cross-Validation to compare the performance of different models on the dataset. We calculate the error and standard deviation between the predicted robot velocity and the ground truth velocity for different models. Same as the results of NHMP, our models achieve good performance in both accuracy and stability, indicating that our models is effective in predicting robot’s velocity.