Autonomous Mobile Robot for Guided Walking Exercise

Project Overview

This project presents an autonomous, human-following mobile robot assistant designed for gait monitoring and walking exercise guidance. Fusing markerless depth body tracking with wireless instrumented insoles, the system estimates a trainee’s dynamic Margin of Stability (MoS) in real-time to assess fall risk and gait performance.

System Architecture

The integrated robotic system consists of two primary hardware components communicating over a localized wireless LAN:

• Mobile Robot Subsystem: A customized differential-drive Pioneer 3-DX mobile base carrying an onboard laptop (Intel i7-9750H, RTX 2060). It features a backward-facing Microsoft Azure Kinect DK depth camera mounted at $0.80\text{ m}$ for real-time gait body tracking, and a forward-facing Kinect v1 for mapping and localization handled by RTAB-Map.
• Wireless Instrumented Footwear (SportSole): A custom pair of shoes equipped with 8 piezoresistive pressure cells per insole (located under the calcaneus, lateral arch, metatarsals, hallux, and toes) and an embedded IMU under the medial arch. A battery-powered logic unit (32-bit ARM Cortex-M4 microcontroller with Wi-Fi) polls sensors at $240\text{ Hz}$ and streams 97-byte UDP packets to the robot.

Core Algorithms

1. Real-Time $O(1)$ Clock Synchronization

Since the insoles and the robot computer operate on independent clocks, we developed a heuristic one-way time synchronization algorithm to translate insole timestamps ($t_0$) to the host computer clock ($t_1$). Rather than standard NTP (which incurs round-trip overhead), we estimate the clock offset $\Delta t_k$ using a sliding window minimum over a time window $T_w$:

$$\Delta t_k \approx -\mathbb{E}\left[\min_{i \in [k-k_w, k]} \tau(i)\right] + \min_{i \in [k-k_w, k]} (t_1(i) - t_0(i))$$

Using a monotonic double-ended queue, the minimum offset search is optimized to $O(1)$ time complexity, neutralizing network jitter and providing accurate time-aligned sensor fusion.

2. Foot Pose Extended Kalman Filter (EKF)

To track precise foot motion, we developed a 6-DOF Foot Pose EKF that fuses local insole IMU readings with spatial Azure Kinect ankle joint tracking. The state vector is defined as:

$$x_{Mi} = \begin{bmatrix} q_{Mi}^T & \omega_{Mi}^T & p_{Mi}^T & v_{Mi}^T & a_{Mi}^T & \omega_{bMi}^T \end{bmatrix}^T$$

where $q_{Mi}$ represents orientation (quaternion), $\omega_{Mi}$ is local angular velocity, $p_{Mi}, v_{Mi}, a_{Mi}$ denote position, velocity, and acceleration in the world frame, and $\omega_{bMi}$ represents gyroscope bias.

• Stance Phase Constraint (ZUPT): By detecting heel-strike and toe-off via the pressure sensors, the filter applies a Zero-Velocity Update (ZUPT) and non-slip constraint during stance phases, completely eliminating integration drift.
• Swing Phase Correction: When the foot is in the air, the filter fuses depth-based body tracking joint positions from the Kinect to correct IMU trajectory estimation.

3. Center of Mass (CoM) & Dynamic Stability (MoS) Estimation

The pelvis joint coordinates are used as a proxy for the human Center of Mass (CoM).

• CoM Kalman Filter: Modeling the walking human as a Linearized Inverted Pendulum:

$$\ddot{r} = \omega_0^2(r - r_P), \quad \omega_0 = \sqrt{g/l}$$

where $r_P$ is the Center of Pressure (CoP) from pressure cells, and $l$ is the effective pendulum length. The filter estimates CoM position $r$ and velocity $\dot{r}$, augmented with turn-compensated estimation of Kinect spatial offset.

• Extrapolated Center of Mass (XCoM): Calculated as:

$$\xi = r + \frac{\dot{r}}{\omega_0}$$

• Base of Support (BoS) & MoS: The boundaries of the active foot segments form a dynamic BoS polygon (derived via Graham’s Scan on 3 to 12 active virtual markers). The Margin of Stability (MoS) is computed as the shortest distance from the XCoM to the boundary of the BoS in both Anterior-Posterior (AP) and Medial-Lateral (ML) directions.

4. Human-Centric Motion Planning & Control

To guide the trainee during walking exercises, a human-following trajectory planner and distance-keeping controller are implemented.

• Human-Robot Formation (Trajectory Planning): To keep the trainee directly behind the robot at a desired tracking distance $\rho$, we formulate the system’s differential kinematics in arc length ($s$) parameterization, decoupling path planning from physical time:

$$\frac{dx_R}{ds} = \cos \theta_H - \sin(\theta_R - \theta_H)\sin\theta_R$$ $$\frac{dy_R}{ds} = \sin \theta_H + \sin(\theta_R - \theta_H)\cos\theta_R$$ $$\frac{d\theta_R}{ds} = \frac{1}{\rho}\sin(\theta_R - \theta_H)$$ $$\frac{d\theta_H}{ds} = \kappa_H$$

where $(x_R, y_R, \theta_R)$ is the robot state, $\theta_H$ is the human heading angle, and $\kappa_H$ is the human trajectory curvature.

• Path-Following & Distance-Keeping Controller: To maintain a target proximity $\rho^*$, a Proportional-Integral (PI) feedback controller generates a target linear velocity $v'_R(t)$ based on the tracking error $\Delta\rho(t) \triangleq \rho^* - \rho(t)$:

$$v'_R(t) = K_P \Delta\rho(t) + K_I \int_0^t \Delta\rho(\tau)d\tau$$

where $K_P$ and $K_I$ are proportional and integral gains tuned for stable distance tracking.

• Curvature-Preserving Command Scaling: To ensure the robot maintains the exact reference path shape (curvature $\kappa_R$), the control command vector $u_R(t) \triangleq [v_R, \omega_R]^T$ is dynamically scaled by the ratio of the target velocity to the nominal velocity, preventing curvature distortion:

$$u_R(t) \mapsto \frac{v'_R(t)}{v_R(t)} u_R(t)$$

Cross-System Infrared Interference Elimination

During validation in labs equipped with OptiTrack motion capture systems, infrared pulses from OptiTrack cameras interfered with the Azure Kinect DK’s time-of-flight depth camera. To resolve this, we designed a hardware-level synchronization trigger scheme:

• OptiTrack was configured to run at $210\text{ Hz}$ (with $250\ \mu\text{s}$ exposure).
• Azure Kinect (operating in wide-FOV 2x2 binned mode at $30\text{ Hz}$) was triggered in subordinate wired sync mode via its physical “Sync in” port at exactly $\frac{1}{7}$ of the OptiTrack frequency.
• By interleaving the Kinect’s 9 infrared pulses ($125\ \mu\text{s}$ width) within the idle periods of the OptiTrack trigger cycle, we successfully eliminated infrared crosstalk, enabling clean depth and motion capture data collection.

Experimental Field Testing

• Young Adult Validation (Motion Capture Lab): Tested on 10 healthy subjects (aged 24-29) performing guided walking on a custom track, validated against an OptiTrack gold-standard reference system. The Foot Pose EKF and MoS estimation achieved high tracking accuracy and stability assessment.
• Older Adult Clinical Study (Sunnyside Community Services, NY): Deployed in a community center with 33 older adult participants to study the effects of plantar vibrotactile feedback during guided walking on an oval track. Dynamic path-following maintained distance between 1.5 and 2.5 meters. Gait parameter estimations were validated against a Zeno Instrumented Walkway, confirming clinical viability.

---

项目概览

该项目研究并开发了一款具备人体跟随功能的自主移动助行机器人系统，用于步行运动导引和步态监控。该系统融合了无标记深度身体跟踪技术和无线智能压力鞋垫，可实时估计受训者的动态稳定裕度 (MoS)，以评估跌倒风险和步态表现。

系统架构

该集成机器人系统包含两个通过本地无线局域网 (LAN) 通信的核心硬件组件：

• 移动机器人子系统： 一台定制的差速驱动 Pioneer 3-DX 移动机器人底盘，搭载一台车载笔记本电脑（Intel i7-9750H，RTX 2060）。其配备了一台安装在 $0.80\text{ m}$ 高度、面向后方的 Microsoft Azure Kinect DK 深度相机，用于实时步态身体跟踪；以及一台面向前方的 Kinect v1，由 RTAB-Map 进行建图和定位。
• 无线智能鞋垫子系统 (SportSole)： 定制鞋垫每侧包含 8 个压阻式压力传感器（分布于足跟、外侧足弓、第一、三、五跖骨头、拇趾及趾尖）以及嵌入在内侧足弓下方的 IMU (Yost Labs)。由锂电池供电的逻辑单元（搭载 32 位 ARM Cortex-M4 微控制器与 Wi-Fi 模块）以 $240\text{ Hz}$ 采样率采集数据，并通过 UDP 协议向机器人发送 97 字节的数据包。

核心算法

1. 实时 $O(1)$ 时间同步算法

由于智能鞋垫和机器人车载电脑运行在独立的时钟源上，系统实现了一种启发式单向时间同步算法，用以将鞋垫时钟戳 ($t_0$) 转换为车载电脑时钟戳 ($t_1$)。算法在滑动时间窗口 $T_w$ 内通过估计最小单向延迟来计算时钟偏差 $\Delta t_k$：

$$\Delta t_k \approx -\mathbb{E}\left[\min_{i \in [k-k_w, k]} \tau(i)\right] + \min_{i \in [k-k_w, k]} (t_1(i) - t_0(i))$$

通过引入单调双端队列，将滑动窗口内最小值搜索的时间复杂度优化至 $O(1)$，彻底消除了无线网络延迟抖动带来的同步误差，确保了多模态数据的高精度融合。

2. 足部姿态扩展卡尔曼滤波器 (Foot Pose EKF)

为获取高精度的足部轨迹，开发了六自由度 (6-DOF) 足部姿态 EKF，用于融合局部鞋垫 IMU 测量值与 Azure Kinect 关节跟踪数据。其状态向量定义为：

$$x_{Mi} = \begin{bmatrix} q_{Mi}^T & \omega_{Mi}^T & p_{Mi}^T & v_{Mi}^T & a_{Mi}^T & \omega_{bMi}^T \end{bmatrix}^T$$

其中 $q_{Mi}$ 表示旋转四元数， $\omega_{Mi}$ 为局部角速度， $p_{Mi}, v_{Mi}, a_{Mi}$ 分别表示世界坐标系下的位置、速度和加速度， $\omega_{bMi}$ 为陀螺仪偏差。

• 支撑相约束 (ZUPT)： 通过压力传感器识别足跟触地 (Heel-Strike) 和足尖离地 (Toe-Off) 事件，在脚部支撑相期间施加零速度更新 (ZUPT) 和无滑动约束，从根本上消除了积分漂移。
• 摆动相修正： 在脚部悬空的摆动相期间，滤波器融合 Azure Kinect 深度相机提供的踝关节空间坐标，对 IMU 轨迹估计进行实时修正。

3. 质心 (CoM) 及动态稳定性 (MoS) 估计

系统将静态或动态下的人体骨盆关节坐标作为人体质心 (CoM) 的替代观测值。

• 质心卡尔曼滤波器： 基于线性倒立摆模型 (Linearized Inverted Pendulum) 进行动态平衡建模：

$$\ddot{r} = \omega_0^2(r - r_P), \quad \omega_0 = \sqrt{g/l}$$

其中 $r_P$ 为鞋垫测得的压力中心 (CoP)， $l$ 为倒立摆的等效长度。滤波器实时估计质心位置 $r$ 及速度 $\dot{r}$，并加入了针对转弯运动的深度相机空间偏差补偿。

• 外推质心 (XCoM)： 定义并计算为：

$$\xi = r + \frac{\dot{r}}{\omega_0}$$

• 支撑多边形 (BoS) 与动态稳定裕度 (MoS)： 依据运动相，基于 3 到 12 个当前接触地面的虚拟足部标记点，利用 Graham 算法 (Graham’s Scan) 提取凸包构建动态支撑多边形 (BoS)。动态稳定裕度 (MoS) 定义为外推质心 (XCoM) 到 BoS 边界的最短距离，并在前后 (AP) 和内外 (ML) 方向上进行精确解算。

4. 以人为中心的运动规划与控制

为在步行训练中导引受训者，系统设计并实现了一种人体跟随轨迹规划器和距离保持控制器。

• 人机编队设计（轨迹规划）： 为使机器人保持在受训者正前方特定距离 $\rho$ 处引导行走，建立了基于弧长 ($s$) 参数化的微分运动学方程，从而将路径几何形状的规划与物理时间解耦：

$$\frac{dx_R}{ds} = \cos \theta_H - \sin(\theta_R - \theta_H)\sin\theta_R$$ $$\frac{dy_R}{ds} = \sin \theta_H + \sin(\theta_R - \theta_H)\cos\theta_R$$ $$\frac{d\theta_R}{ds} = \frac{1}{\rho}\sin(\theta_R - \theta_H)$$ $$\frac{d\theta_H}{ds} = \kappa_H$$

其中 $(x_R, y_R, \theta_R)$ 为机器人状态， $\theta_H$ 为人体行走朝向角， $\kappa_H$ 为人体运动轨迹曲率。

• 路径跟随与距离控制： 为维持目标间距 $\rho^*$，采用比例-积分 (PI) 反馈控制器依据间距误差 $\Delta\rho(t) \triangleq \rho^* - \rho(t)$ 计算目标线速度指令 $v'_R(t)$：

$$v'_R(t) = K_P \Delta\rho(t) + K_I \int_0^t \Delta\rho(\tau)d\tau$$

其中 $K_P$ 和 $K_I$ 分别为比例和积分增益，用以确保稳定的距离跟踪性能。

• 保曲率速度缩放： 为确保机器人在调整车速时严格遵循参考路径形状（曲率 $\kappa_R$ 不发生畸变），系统对控制输入向量 $u_R(t) \triangleq [v_R, \omega_R]^T$进行动态等比例缩放：

$$u_R(t) \mapsto \frac{v'_R(t)}{v_R(t)} u_R(t)$$

跨系统红外信号干涉硬件消除

在配备 OptiTrack 动作捕捉系统的实验室环境中进行性能验证时，由于 OptiTrack 相机发出的高频红外脉冲会严重干扰 Azure Kinect DK 的飞行时间法 (ToF) 深度相机，设计了硬件级同步触发方案：

• 将 OptiTrack 帧率配置为 $210\text{ Hz}$（曝光时间设为 $250\ \mu\text{s}$）。
• 机器人后向 Azure Kinect（工作在 Wide-FOV 2x2-binned 模式，帧率为 $30\text{ Hz}$）通过其物理 “Sync in” 接口配置为从动有线同步模式，触发频率精准设定为 OptiTrack 触发频率的 $\frac{1}{7}$ (即 $30\text{ Hz}$)。
• 借由将 Azure Kinect 的 9 个红外曝光脉冲（脉宽 $125\ \mu\text{s}$）精准排插在 OptiTrack 触发周期之间的 $1450\ \mu\text{s}$ 空闲间隔内，实现了两种红外光源在时域上的完美交错，彻底消除了红外串扰，确保了深度相机点云与动捕参考数据的纯净与精确。

临床与实地实验验证

• 青年受试者实验室验证： 招募 10 名健康受试者（24-29岁）在动捕实验室中沿定制轨迹进行导引步行测试。以黄金标准 OptiTrack 动捕系统为对照，验证了足部姿态 EKF 与 MoS 估计的高精度与鲁棒性。
• 社区中心老年人临床研究 (Sunnyside Community Services, NY)： 在纽约 Sunnyside 社区中心对 33 名老年受试者 进行了实地部署，研究足底振动触觉反馈对受训者在机器人导引步行（椭圆轨迹）期间步态的影响。机器人运动规划与测距控制器成功将人机距离稳定保持在 1.5 至 2.5 米之间。步态参数评估以 Zeno 步态分析步道 (Zeno Instrumented Walkway) 为基准进行校准，证明了该系统在临床监控环境中的实用价值。