875 lines
35 KiB
TeX
875 lines
35 KiB
TeX
\documentclass[12pt, a4paper]{article}
|
||
|
||
% ========== 基础包 ==========
|
||
\usepackage[UTF8]{ctex} % 中文支持
|
||
\usepackage[margin=2.5cm]{geometry} % 页边距
|
||
\usepackage{titlesec} % 章节标题格式
|
||
\usepackage{titletoc} % 目录格式
|
||
\usepackage{fancyhdr} % 页眉页脚
|
||
\usepackage{listings} % 代码块
|
||
\usepackage{xcolor} % 颜色
|
||
\usepackage{graphicx} % 图片
|
||
\usepackage{amsmath} % 数学公式
|
||
\usepackage{amssymb} % 数学符号
|
||
\usepackage{booktabs} % 表格
|
||
\usepackage{enumitem} % 列表格式
|
||
\usepackage{tcolorbox} % 彩色盒子（用于Q&A）
|
||
\usepackage{fontawesome5} % 图标
|
||
\usepackage{setspace} % 行距
|
||
\usepackage{hyperref} % 超链接（放最后避免与其他包冲突）
|
||
\lstdefinestyle{bashstyle}{
|
||
backgroundcolor=\color{codebg},
|
||
basicstyle=\ttfamily\footnotesize,
|
||
breaklines=true,
|
||
frame=single,
|
||
rulecolor=\color{codeframe},
|
||
language=bash,
|
||
commentstyle=\color{codegreen},
|
||
keywordstyle=\color{blue},
|
||
}
|
||
% ========== 页面设置 ==========
|
||
\onehalfspacing % 1.5倍行距
|
||
|
||
% ========== 页眉页脚 ==========
|
||
\pagestyle{fancy}
|
||
\fancyhf{}
|
||
\fancyhead[L]{\small 机器人动作重定向文档}
|
||
\fancyhead[R]{\small \leftmark}
|
||
\fancyfoot[C]{\thepage}
|
||
\renewcommand{\headrulewidth}{0.4pt}
|
||
|
||
% ========== 超链接设置 ==========
|
||
\hypersetup{
|
||
colorlinks=true,
|
||
linkcolor=blue!70!black,
|
||
urlcolor=blue!70!black,
|
||
citecolor=green!60!black,
|
||
bookmarks=true,
|
||
bookmarksnumbered=true
|
||
}
|
||
|
||
% ========== 代码块设置 ==========
|
||
\definecolor{codebg}{RGB}{245, 245, 245}
|
||
\definecolor{codeframe}{RGB}{200, 200, 200}
|
||
\definecolor{codegreen}{rgb}{0,0.6,0}
|
||
\definecolor{codegray}{rgb}{0.5,0.5,0.5}
|
||
\definecolor{codepurple}{rgb}{0.58,0,0.82}
|
||
|
||
\lstdefinestyle{pythonstyle}{
|
||
backgroundcolor=\color{codebg},
|
||
commentstyle=\color{codegreen},
|
||
keywordstyle=\color{blue}\bfseries,
|
||
numberstyle=\tiny\color{codegray},
|
||
stringstyle=\color{codepurple},
|
||
basicstyle=\ttfamily\footnotesize,
|
||
breakatwhitespace=false,
|
||
breaklines=true,
|
||
captionpos=b,
|
||
keepspaces=true,
|
||
numbers=left,
|
||
numbersep=5pt,
|
||
showspaces=false,
|
||
showstringspaces=false,
|
||
showtabs=false,
|
||
tabsize=4,
|
||
frame=single,
|
||
rulecolor=\color{codeframe},
|
||
language=Python
|
||
}
|
||
|
||
\lstset{style=pythonstyle}
|
||
|
||
% ========== Q&A 盒子样式 ==========
|
||
\tcbuselibrary{skins, breakable}
|
||
|
||
\newtcolorbox{qabox}[2][]{
|
||
enhanced,
|
||
breakable,
|
||
colback=blue!5!white,
|
||
colframe=blue!60!black,
|
||
fonttitle=\bfseries,
|
||
title={Q: #2},
|
||
#1
|
||
}
|
||
|
||
\newtcolorbox{answerbox}[1][]{
|
||
enhanced,
|
||
breakable,
|
||
colback=green!5!white,
|
||
colframe=green!60!black,
|
||
leftrule=4pt,
|
||
#1
|
||
}
|
||
|
||
% ========== 文件说明盒子 ==========
|
||
\newtcolorbox{filebox}[2][]{
|
||
enhanced,
|
||
breakable,
|
||
colback=gray!10!white,
|
||
colframe=gray!60!black,
|
||
fonttitle=\bfseries\ttfamily,
|
||
title={\faFile\ #2},
|
||
#1
|
||
}
|
||
|
||
% ========== 章节标题格式 ==========
|
||
\titleformat{\section}
|
||
{\Large\bfseries\color{blue!70!black}}
|
||
{\thesection}{1em}{}
|
||
[\titlerule]
|
||
|
||
\titleformat{\subsection}
|
||
{\large\bfseries\color{blue!50!black}}
|
||
{\thesubsection}{1em}{}
|
||
|
||
\titleformat{\subsubsection}
|
||
{\normalsize\bfseries}
|
||
{\thesubsubsection}{1em}{}
|
||
|
||
% ========== 文档开始 ==========
|
||
\begin{document}
|
||
|
||
% ========== 封面 ==========
|
||
\begin{titlepage}
|
||
\centering
|
||
\vspace*{3cm}
|
||
|
||
{\Huge\bfseries 机器人动作重定向\\[0.5em]
|
||
\Large 技术文档}
|
||
|
||
\vspace{2cm}
|
||
\rule{\linewidth}{0.5mm}
|
||
\vspace{1cm}
|
||
|
||
{\large
|
||
\begin{tabular}{ll}
|
||
\textbf{项目名称：} & 人形机器人动作重定向系统 \\[0.5em]
|
||
\textbf{作者：} & 沈玉祥 \\[0.5em]
|
||
\textbf{日期：} & \today \\[0.5em]
|
||
\textbf{版本：} & v1.0 \\
|
||
\end{tabular}
|
||
}
|
||
|
||
\vspace{1cm}
|
||
\rule{\linewidth}{0.5mm}
|
||
|
||
\vfill
|
||
{\small 本文档记录了基于PINK差分IK的人形机器人动作重定向方法及实现细节}
|
||
\end{titlepage}
|
||
|
||
% ========== 目录 ==========
|
||
\tableofcontents
|
||
\newpage
|
||
|
||
% ============================================================
|
||
% 第一章：程序文件说明
|
||
% ============================================================
|
||
\section{程序文件说明}
|
||
|
||
\subsection{概述}
|
||
主要包括：人体动作到机器人动作的重定向方法、动作之间的平滑插值、视频提取到机器人动作的方法等内容。
|
||
% 在这里写一段整体项目结构的介绍
|
||
|
||
\subsection{核心文件}
|
||
|
||
% ---- 文件1 ----
|
||
\begin{filebox}{\texttt{ik\_redirection\_npy.py}}
|
||
\textbf{功能：} 将人体动作信息转化为机器人各关节角度(rad)信息。\\[0.5em]
|
||
\textbf{主要类/函数：}
|
||
\begin{itemize}
|
||
\item \texttt{H1Config} —— 机器人配置信息
|
||
\item \texttt{H1PinkSolver} —— 核心求解器
|
||
\item \texttt{process\_motion()} —— 主处理函数
|
||
\item \texttt{apply\_bone\_retargeting\_single()} —— 单帧骨骼重定向
|
||
\item \texttt{load\_data\_all\_npy()} —— 输入格式转换：$[N, 22, 3, T]$ -> 第 i 个 $[T, 22, 3]$
|
||
\end{itemize}
|
||
|
||
\vspace{0.5em}
|
||
\textbf{输入：} $[N, 22, 3]$ 的numpy数组，N为帧数，22为SMPL主要关节数，3为XYZ坐标
|
||
|
||
\textbf{输出：} 字典，主要为dof（关节角度序列，弧度，shape为 $[N, 28]$）
|
||
|
||
\textbf{运行效率：}0.08s/196帧 $\approx$ 0.4ms/帧（单线程，CPU）
|
||
\end{filebox}
|
||
|
||
\vspace{1em}
|
||
|
||
% ---- 文件2 ----
|
||
\begin{filebox}{\texttt{easy\_MotionInterpolator.py}}
|
||
\textbf{功能：} 实现机器人动作（关节角度dof）的插值、帧率重定向及平滑处理；可处理过渡的自碰撞问题。
|
||
|
||
\vspace{0.5em}
|
||
\textbf{主要配置项：}
|
||
\begin{itemize}
|
||
\item \texttt{MotionInterpolator} —— 核心插值类
|
||
\item \texttt{interpolate()} —— 对dof序列进行三次样条插值
|
||
\item \texttt{smooth()} —— 加权滑动滤波消除插值引入的抖动
|
||
\item \texttt{process()} —— 依次执行插值与平滑，返回处理后的dof序列
|
||
\end{itemize}
|
||
\vspace{0.5em}
|
||
\textbf{输入：} 低帧率的关节角度序列，shape为 $[N, 28]$
|
||
|
||
\textbf{输出：} 高帧率平滑后的关节角度序列，shape为 $[N', 28]$
|
||
|
||
\textbf{运行效率：}单次拼接（含碰撞检测）$\approx$
|
||
0.8ms（单线程，CPU）
|
||
\end{filebox}
|
||
|
||
\vspace{1em}
|
||
|
||
\begin{filebox}{\texttt{vis.py}}
|
||
\textbf{功能：} 逐帧播放并可视化。
|
||
|
||
\vspace{0.5em}
|
||
\textbf{主要配置项：}
|
||
\begin{itemize}
|
||
\item \texttt{load\_motion\_data()} —— 加载动作数据
|
||
\item \texttt{build\_obstacle\_geoms()} —— 根据障碍物数据构建红色线框盒几何体
|
||
\item \texttt{main()} —— 逐帧绘制机器人关节角度与根节点状态
|
||
\end{itemize}
|
||
\vspace{0.5em}
|
||
\textbf{输入：} 包含dof序列、根节点位姿、可选的h1\_joint\_pos和smpl\_joints\_target
|
||
|
||
\textbf{输出：} Isaac Gym实时可视化窗口
|
||
|
||
\textbf{依赖：}Isaac Gym、PyTorch（支持GPU加速渲染）
|
||
\end{filebox}
|
||
|
||
% ---- 添加更多文件 ----
|
||
% \begin{filebox}{\texttt{文件名.py}}
|
||
% ...
|
||
% \end{filebox}
|
||
|
||
\subsection{服务器端文件}
|
||
\textbf{位置：} 服务器下的GVHMR目录下；conda activate gvhmr
|
||
|
||
\begin{filebox}{\texttt{GVHMR/run.py}}
|
||
\textbf{功能：} 把人体动作视频(mp4)转换成机器人各关节电机角度(rad)。
|
||
|
||
\vspace{0.5em}
|
||
\textbf{主要配置项：}
|
||
\begin{itemize}
|
||
\item \texttt{GVHMRSystem} —— 核心系统类
|
||
\item \texttt{\_\_init\_\_()} —— 冷启动初始化，加载GVHMR主模型、Tracker、VitPose、特征提取器、SMPL模型及H1PinkSolver
|
||
\item \texttt{process\_video()} —— 热运行入口，处理单个视频，依次执行预处理、GVHMR推理、H1 IK优化，返回关节角度结果
|
||
\item \texttt{\_run\_preprocess\_efficient()} —— SLAM后台线程并行，Tracker、VitPose、特征提取串行GPU推理
|
||
\item \texttt{\_run\_h1\_optimization()} —— 从GVHMR输出的SMPL参数提取关节坐标，调用H1PinkSolver完成重定向
|
||
\end{itemize}
|
||
\vspace{0.5em}
|
||
\textbf{输入：} mp4视频文件
|
||
|
||
\textbf{输出：} 包含dof关节角度序列（rad，shape为$[N,28]$）的字典
|
||
|
||
\textbf{使用方法：}
|
||
\begin{lstlisting}[style=bashstyle]
|
||
conda activate gvhmr
|
||
cd GVHMR/
|
||
python run.py --input (视频路径)
|
||
# 结果存储到 outputs 目录的 result.pkl 文件
|
||
\end{lstlisting}
|
||
|
||
\textbf{运行效率：}4.5s/8s（4090GPU）
|
||
\end{filebox}
|
||
|
||
\vspace{1em}
|
||
|
||
\begin{filebox}{\texttt{GVHMR/new-server.py}}
|
||
\textbf{功能：} 支持大模型工具调用的服务端。
|
||
|
||
\vspace{0.5em}
|
||
\textbf{主要配置项：}
|
||
\begin{itemize}
|
||
\item \texttt{\_run\_gvhmr()} —— 核心推理函数
|
||
\item \texttt{\_frames\_to\_tmp\_video()} —— 将帧列表写入临时视频文件
|
||
\item \texttt{\_process\_frames\_async()} —— 异步推理包装
|
||
\end{itemize}
|
||
\vspace{0.5em}
|
||
|
||
\textbf{主要接口：}
|
||
\begin{itemize}
|
||
\item \lstinline{GET /tools} —— 查询当前可用工具，大模型调用前先请求：
|
||
\begin{lstlisting}[style=bashstyle]
|
||
curl http://localhost:8003/tools\end{lstlisting}
|
||
\item \lstinline{POST /tool/call} —— 大模型调用工具的入口，传工具名和参数：
|
||
\begin{lstlisting}[style=bashstyle]
|
||
curl -X POST http://localhost:8003/tool/call \
|
||
-d '{"name": "process_video_to_robot_motion",
|
||
"parameters": {"video_path": "/data/demo.mp4"}}'\end{lstlisting}
|
||
\item \lstinline{POST /generate} —— 直接输入视频文件路径处理：
|
||
\begin{lstlisting}[style=bashstyle]
|
||
curl -X POST http://localhost:8003/generate \
|
||
-d '{"video_path": "/data/demo.mp4"}'\end{lstlisting}
|
||
\item \lstinline{WS /ws/stream} —— 接入实时相机推流，配合 camera\_client.py 使用：
|
||
\begin{lstlisting}[style=bashstyle]
|
||
python camera_client.py --camera 0\end{lstlisting}
|
||
\end{itemize}
|
||
\vspace{0.5em}
|
||
\textbf{输入：} 视频文件绝对路径，或通过 WebSocket 推送的 JPEG 编码帧字节流
|
||
|
||
\textbf{输出：} 字典，主要为 dof（关节角度序列，弧度，shape 为 $[N,28]$）、fps、duration\_sec
|
||
\end{filebox}
|
||
|
||
\vspace{1em}
|
||
|
||
\begin{filebox}{\texttt{GVHMR/camera\_client.py}}
|
||
\textbf{功能：} 获取本地摄像头，将视频帧实时推送到 new-server.py 进行推理，并接收返回的关节角度。\\[0.5em]
|
||
\textbf{主要函数：}
|
||
\begin{itemize}
|
||
\item \texttt{run\_camera\_stream()} —— 打开摄像头、推帧、接收结果
|
||
\item \texttt{\_handle\_result()} —— 处理每次推理返回的结果
|
||
\end{itemize}
|
||
|
||
\vspace{0.5em}
|
||
\textbf{使用方法：}
|
||
\begin{lstlisting}[style=bashstyle]
|
||
# 默认摄像头，默认参数
|
||
python camera_client.py
|
||
|
||
# 指定摄像头编号
|
||
python camera_client.py --camera 0 --fps 30 --chunk_sec 4
|
||
|
||
# 服务端不在同一台机器时指定地址
|
||
python camera_client.py --server 192.168.1.100:8003
|
||
|
||
# 固定机位相机
|
||
python camera_client.py --static_cam
|
||
\end{lstlisting}
|
||
|
||
\textbf{输入：} 本地摄像头实时画面（通过 OpenCV 读取）\\[0.5em]
|
||
\textbf{输出：} 每隔 \texttt{chunk\_sec} 秒打印一次推理结果，格式如下：
|
||
\begin{lstlisting}[style=bashstyle]
|
||
[推理结果] chunk_id=0 | 帧数=120 | 时长=4.0s | fps=90 | DOF shape=[360, 28]
|
||
\end{lstlisting}
|
||
|
||
\textbf{备注：} \texttt{\_handle\_result()} 函数内的注释处（第~117~行）可对接机器人控制器，将 DOF 数据发给 ROS 节点。
|
||
\end{filebox}
|
||
|
||
\vspace{1em}
|
||
|
||
\begin{filebox}{\texttt{GVHMR/test\_stream.py}}
|
||
\textbf{功能：} 用本地 mp4 视频文件模拟摄像头推流，测试 new-server.py 的视频流接口是否正常工作。\\[0.5em]
|
||
\textbf{主要函数：}
|
||
\begin{itemize}
|
||
\item \texttt{test\_stream()} —— 读取视频文件逐帧推送到服务端，并接收推理结果
|
||
\item \texttt{\_print\_result()} —— 打印每个 chunk 的推理结果
|
||
\end{itemize}
|
||
|
||
\vspace{0.5em}
|
||
\textbf{使用方法：}
|
||
\begin{lstlisting}[style=bashstyle]
|
||
# 基本用法（必须指定视频文件）
|
||
python test_stream.py --video /data/demo.mp4
|
||
|
||
# 指定参数
|
||
python test_stream.py --video /data/demo.mp4 --fps 30 --chunk_sec 4
|
||
|
||
# 服务端不在同一台机器时指定地址
|
||
python test_stream.py --video /data/demo.mp4 --server 192.168.1.100:8003
|
||
\end{lstlisting}
|
||
|
||
\textbf{输入：} 本地视频文件路径\\[0.5em]
|
||
\textbf{输出：} 每积累 \texttt{chunk\_sec} 秒的帧触发一次推理，打印结果：
|
||
\begin{lstlisting}[style=bashstyle]
|
||
[chunk 0] dof_shape=[360, 28], fps=90, duration=4.0s
|
||
[done] 共处理 2 个 chunk
|
||
\end{lstlisting}
|
||
\end{filebox}
|
||
|
||
\subsection{其他说明}
|
||
|
||
\begin{itemize}
|
||
\item \textbf{根节点固定：} 本模块的目标是从人体动作生成机器人电机角度序列，机器人waist点在世界坐标系中保持固定，相当于将机器人腰部悬挂于空间中。根节点的全局位移不参与重定向计算。
|
||
|
||
\item \textbf{输出崩溃排查：} 若输出的电机角度出现突变或崩溃，大概率是输入动作幅度过大，超出机器人关节限位范围，导致IK无法在单帧内追上目标。解决方法是适当降低大模型生成的动作幅度，或延长动作时长使运动更平缓。
|
||
|
||
\item \textbf{体型通用性：} 重定向模块对输入人体的臂长、身高、体型无要求，骨骼重定向阶段会自动将SMPL骨骼方向映射到机器人实际骨骼长度，不受输入人体比例影响。
|
||
\end{itemize}
|
||
% 在这里描述其他辅助脚本
|
||
|
||
|
||
\newpage
|
||
|
||
% ============================================================
|
||
% 第二章：重定向方法与原理
|
||
% ============================================================
|
||
\section{重定向方法与原理}
|
||
\subsection{常见的三种重定向方法}
|
||
常见的机器人动作重定向方法主要分为三类：梯度下降方法、神经网络模型+IK微调、纯IK逆解方法三种。
|
||
|
||
\begin{table}[h]
|
||
\centering
|
||
\caption{三种重定向方法对比}
|
||
\begin{tabular}{p{2.5cm}p{4cm}p{4cm}p{3cm}}
|
||
\toprule
|
||
\textbf{方法} & \textbf{优点} & \textbf{缺点} & \textbf{适用场合} \\
|
||
\midrule
|
||
梯度下降 &
|
||
动作质量高；批量处理性价比好 &
|
||
实时性差，处理数秒视频需2s以上；需预先标定缩放比例 &
|
||
离线批量处理 \\
|
||
\midrule
|
||
神经网络 + IK微调 &
|
||
推理速度快；兼顾数据驱动与几何约束 &
|
||
依赖大量高质量标注数据，准备成本高；对体型变化鲁棒性弱 &
|
||
数据充足的生产环境 \\
|
||
\midrule
|
||
纯IK逆解（本文） &
|
||
无需训练数据；实时求解；体型适应性好 &
|
||
需合理设计任务约束与权重 &
|
||
实时重定向 \\
|
||
\bottomrule
|
||
\end{tabular}
|
||
\end{table}
|
||
|
||
\subsection{整体流程}
|
||
|
||
本系统的重定向流程如下：
|
||
|
||
\begin{center}
|
||
\begin{tcolorbox}[width=0.85\textwidth, colback=blue!5, colframe=blue!40,
|
||
boxrule=0.5pt, arc=3pt]
|
||
\centering
|
||
\textbf{输入} SMPL关节坐标序列 $[N, J, 3]$ \\[4pt]
|
||
$\downarrow$ \\[2pt]
|
||
坐标系变换（quat旋转，对齐机器人坐标系）\\[4pt]
|
||
$\downarrow$ \\[2pt]
|
||
根节点归一化 $\rightarrow$ 比例缩放 $\rightarrow$ 平移至waist高度 \\[4pt]
|
||
$\downarrow$ \\[2pt]
|
||
Yaw朝向对齐（固定根节点朝向）\\[4pt]
|
||
$\downarrow$ \\[2pt]
|
||
Savitzky-Golay 平滑滤波 \\[4pt]
|
||
$\downarrow$ \\[2pt]
|
||
设定IK追踪任务与权重 \\[4pt]
|
||
$\downarrow$ \\[2pt]
|
||
逐帧骨骼重定向（动态Anchor + 骨骼长度替换）\\[4pt]
|
||
$\downarrow$ \\[2pt]
|
||
QP差分IK求解 \\[4pt]
|
||
$\downarrow$ \\[2pt]
|
||
加权滑动窗口平滑 $\rightarrow$ 脚踝pitch几何覆写 \\[4pt]
|
||
$\downarrow$ \\[2pt]
|
||
三次样条插值升采样至90fps \\[4pt]
|
||
$\downarrow$ \\[2pt]
|
||
\textbf{输出} 关节角度序列（rad，$[N, 28]$）
|
||
\end{tcolorbox}
|
||
\end{center}
|
||
|
||
% 描述整体 pipeline，可以用 itemize 列出步骤
|
||
|
||
\subsection{坐标系处理}
|
||
|
||
\subsubsection{SMPL坐标系与机器人坐标系的对齐}
|
||
|
||
SMPL以骨盆（pelvis）为根节点，机器人以waist为躯干中心，两者物理意义最接近，因此将pelvis对齐到机器人waist位置。具体分两步：以pelvis为原点中心化后乘以缩放系数，再平移至机器人waist高度：
|
||
|
||
\begin{lstlisting}[style=bashstyle]
|
||
# 中心化 + 缩放
|
||
smpl_joints = (smpl_raw_seq - smpl_raw_seq[:, 0:1, :]) * self.SMPL_SCALE
|
||
# 平移至机器人waist高度
|
||
waist_pos = self.configuration.get_transform_frame_to_world("waist").translation
|
||
smpl_joints += waist_pos.reshape(1, 1, 3)
|
||
\end{lstlisting}
|
||
|
||
% 描述 quat = [0.5, 0.5, 0.5, 0.5] 的作用
|
||
|
||
\subsubsection{朝向对齐预处理}
|
||
|
||
大模型输出的动作可能包含根节点移动（如走圈），直接重定向会导致机器人跟随转向。因此对每帧计算髋部左右向量的水平偏转角，绕Z轴旋转补偿，使全程朝向与第0帧保持一致：
|
||
|
||
\begin{lstlisting}[style=bashstyle]
|
||
# 计算每帧髋部向量的yaw角
|
||
hip_vecs = smpl_joints[:, idx_r] - smpl_joints[:, idx_l]
|
||
hip_vecs[:, 2] = 0 # 投影到水平面
|
||
yaw_ref = np.arctan2(hip_vecs[0, 1], hip_vecs[0, 0])
|
||
yaw_all = np.arctan2(hip_vecs[:, 1], hip_vecs[:, 0])
|
||
|
||
# 绕Z轴旋转补偿，固定朝向
|
||
rot_mats = sRot.from_euler('z', yaw_ref - yaw_all).as_matrix()
|
||
centered = smpl_joints - smpl_joints[:, 0:1, :]
|
||
smpl_joints = np.einsum('nij,nkj->nki', rot_mats, centered) + smpl_joints[:, 0:1, :]
|
||
\end{lstlisting}
|
||
|
||
% 描述固定根节点朝向的处理逻辑
|
||
|
||
\subsection{骨骼重定向}
|
||
\begin{figure}[h]
|
||
\centering
|
||
\begin{minipage}{0.45\textwidth}
|
||
\centering
|
||
\includegraphics[width=\textwidth]{before.png}
|
||
\caption{重定向前}
|
||
\end{minipage}
|
||
\hfill
|
||
\begin{minipage}{0.45\textwidth}
|
||
\centering
|
||
\includegraphics[width=\textwidth]{after.png}
|
||
\caption{重定向后}
|
||
\end{minipage}
|
||
\caption{重定向前后对比：绿色为H1机器人关节点，蓝色为SMPL目标关节点}
|
||
\label{fig:retarget_compare}
|
||
\end{figure}
|
||
\subsubsection{动态Anchor设计}
|
||
|
||
SMPL人体与机器人结构不同，肩部和髋部的关节位置无法直接对应。若直接以SMPL的肩膀点作为anchor，会有两个问题：一是两者骨骼比例不同导致位置偏差；二是人体上肢挥动时SMPL肩膀点存在上下位移，而机器人肩部三个电机的结构无法复现这种位移。
|
||
|
||
因此每帧从机器人当前正运动学结果中读取肩部、髋部、颈部的真实世界坐标作为anchor，再以SMPL骨骼的方向信息驱动末端位置：
|
||
|
||
\begin{lstlisting}[style=bashstyle]
|
||
# 每帧从机器人正运动学读取anchor点
|
||
anchors = {
|
||
'l_shoulder': self.configuration.get_transform_frame_to_world(
|
||
"left_upper_arm").translation.copy(),
|
||
'r_shoulder': self.configuration.get_transform_frame_to_world(
|
||
"right_upper_arm").translation.copy(),
|
||
'l_thigh': self.configuration.get_transform_frame_to_world(
|
||
"left_thigh").translation.copy(),
|
||
'r_thigh': self.configuration.get_transform_frame_to_world(
|
||
"right_thigh").translation.copy(),
|
||
'neck': self.configuration.get_transform_frame_to_world(
|
||
"neck_linkage").translation.copy(),
|
||
}
|
||
\end{lstlisting}
|
||
|
||
同时，利用SMPL的spine1、left\_collar、right\_collar三点构造正交基，得到躯干朝向，用于约束机器人胸部旋转，间接驱动上肢姿态跟随躯干运动：
|
||
|
||
\begin{lstlisting}[style=bashstyle]
|
||
# 三点构造胸部正交基
|
||
up = _normalize((p_l_collar + p_r_collar) / 2.0 - p_spine3)
|
||
right = _normalize(p_l_collar - p_r_collar)
|
||
forward = _normalize(np.cross(right, up))
|
||
right = _normalize(np.cross(up, forward))
|
||
chest_rot = np.column_stack([forward, right, up])
|
||
\end{lstlisting}
|
||
|
||
% 描述为什么要用动态anchor，以及如何实现
|
||
|
||
\subsubsection{链式骨骼长度替换}
|
||
|
||
SMPL与机器人的骨骼长度不同，直接使用SMPL关节坐标会导致末端位置偏差。解决方案是保留SMPL各骨骼段的方向向量，将长度替换为机器人实际物理尺寸，从anchor点出发链式计算各关节位置，使末端（手、脚）能够正确对齐：
|
||
|
||
\begin{lstlisting}[style=bashstyle]
|
||
# 机器人实际骨骼长度（米）
|
||
ROBOT_DIMS = {
|
||
'thigh': 0.0832, # 髋 -> 膝
|
||
'calf': 0.1105, # 膝 -> 踝
|
||
'upper_arm': 0.07579, # 肩 -> 肘
|
||
'forearm': 0.04739, # 肘 -> 腕
|
||
'neck2head': 0.022487329, # 颈部 -> 头
|
||
}
|
||
|
||
# 以anchor为父节点，沿SMPL方向链式扩展（以左腿为例）
|
||
nj[:, b['left_hip']] = anchors['l_thigh']
|
||
nj[:, b['left_knee']] = anchors['l_thigh'] \
|
||
+ _normalize(j[:, b['left_knee']] - j[:, b['left_hip']]) * d['thigh']
|
||
nj[:, b['left_ankle']] = nj[:, b['left_knee']] \
|
||
+ _normalize(j[:, b['left_ankle']] - j[:, b['left_knee']]) * d['calf']
|
||
\end{lstlisting}
|
||
|
||
左右臂同理，从肩部anchor出发，依次计算肘、腕位置。经过链式替换后，末端关节坐标既保留了SMPL动作的姿态方向，又符合机器人的实际骨骼比例。
|
||
|
||
% 描述 apply_bone_retargeting_single 的逻辑
|
||
|
||
\subsection{差分IK求解}
|
||
|
||
\subsubsection{PINK框架介绍}
|
||
|
||
PINK（Python Inverse kiNematics）是基于Pinocchio的差分IK框架。Pinocchio负责机器人运动学/动力学计算，PINK在其基础上将IK问题构造为二次规划（QP）问题，通过定义多个带权重的\textbf{任务}（Task）和\textbf{约束}（Limit）来描述求解目标。
|
||
|
||
每个任务对应一个末端执行器的位置或旋转目标，权重决定各任务的优先级。求解器在满足关节限位约束的前提下，每帧求解一个关节速度增量，再积分更新机器人配置，从而逐帧追踪目标轨迹。本系统使用quadprog作为底层QP求解器，阻尼系数设为$10^{-3}$防止奇异。
|
||
|
||
% 简要介绍 PINK 和 Pinocchio
|
||
|
||
\subsubsection{任务设计与权重}
|
||
|
||
系统共定义以下IK任务，各任务的位置权重与旋转权重如表所示：
|
||
|
||
\begin{table}[h]
|
||
\centering
|
||
\caption{IK任务权重配置}
|
||
\begin{tabular}{llcc}
|
||
\toprule
|
||
\textbf{机器人Link} & \textbf{对应SMPL关节} & \textbf{位置权重} & \textbf{旋转权重} \\
|
||
\midrule
|
||
chest & — & 0.0 & 50.0 \\
|
||
left\_hand / right\_hand & left\_wrist / right\_wrist & 10.0 & 0.0 \\
|
||
left\_foot / right\_foot & left\_ankle / right\_ankle & 10.0 & 1.0 \\
|
||
left\_force\_arm / right\_force\_arm & left\_elbow / right\_elbow & 5.0 & 0.0 \\
|
||
left\_calf / right\_calf & left\_knee / right\_knee & 5.0 & 0.0 \\
|
||
head & head & 1.0 & 5.0 \\
|
||
\bottomrule
|
||
\end{tabular}
|
||
\end{table}
|
||
|
||
chest任务不追位置，只追旋转，用于约束躯干朝向防止后仰；手和脚作为末端执行器位置权重最高；肘和膝作为中间关节权重较低，主要用于引导肢体方向；头部以旋转约束为主。此外还加入了一个低权重（0.1）的PostureTask作为软约束，防止未被追踪的关节乱飘：
|
||
|
||
\begin{lstlisting}[style=bashstyle]
|
||
self.posture_task = pink.tasks.PostureTask(self.model)
|
||
self.posture_task.set_target(self.q_ref)
|
||
self.posture_task.cost = 0.1
|
||
\end{lstlisting}
|
||
|
||
% 描述各个 FrameTask 的权重设计思路
|
||
|
||
\subsubsection{躯干朝向约束}
|
||
|
||
IK求解时若不对躯干朝向加以约束，机器人上半身容易出现后仰或侧倾。为此单独设置一个chest的FrameTask，只追旋转不追位置（位置权重为0，旋转权重为50），每帧从SMPL的spine1、left\_collar、right\_collar三点构造正交基作为目标旋转矩阵（见2.4.1节），强制机器人胸部朝向与SMPL躯干保持一致。
|
||
|
||
由于位置不强制追踪，chest的世界坐标始终取机器人当前真实位置，避免与其他任务产生冲突：
|
||
|
||
\begin{lstlisting}[style=bashstyle]
|
||
chest_pos = self.configuration.get_transform_frame_to_world("chest").translation.copy()
|
||
self.chest_task.set_target(pin.SE3(chest_rot, chest_pos))
|
||
\end{lstlisting}
|
||
|
||
% 描述用 spine3/left_collar/right_collar 三点确定胸部朝向的方法
|
||
|
||
\subsubsection{头部朝向处理}
|
||
|
||
头部朝向通过两种方式处理。若上游提供了\lstinline{head_rot_mats}（局部旋转矩阵），则将其与当前帧的躯干旋转矩阵相乘得到头部世界朝向；若未提供则只追位置，旋转使用单位矩阵：
|
||
|
||
\begin{lstlisting}[style=bashstyle]
|
||
if smpl_name == "head" and head_rot_mats is not None:
|
||
# 世界朝向 = 躯干朝向 x 头部局部旋转
|
||
task.set_target(pin.SE3(chest_rot @ head_rot_mats[i], limited_targets[smpl_name]))
|
||
else:
|
||
task.set_target(pin.SE3(np.eye(3), limited_targets[smpl_name]))
|
||
\end{lstlisting}
|
||
|
||
由于\lstinline{head_rot_mats}是相对于父关节的局部旋转，对于包含大幅根节点移动的动作（如走圈），预处理阶段会自动检测并清除旋转向量的Yaw分量，避免头部跟随身体转向产生突变。
|
||
|
||
% 描述头部朝向的处理方法
|
||
|
||
\subsection{数据处理与平滑}
|
||
|
||
\subsubsection{插值扩帧}
|
||
|
||
IK求解在20fps上运行，完成后对关节角度序列做三次样条插值升采样至90fps。在关节空间插值比在笛卡尔空间插值更物理合理，且计算开销小：
|
||
|
||
\begin{lstlisting}[style=bashstyle]
|
||
interp = interp1d(t_orig, sim_dof, axis=0, kind='cubic')
|
||
sim_dof = interp(t_new)
|
||
\end{lstlisting}
|
||
|
||
% 描述从20fps插值到90fps的方法
|
||
|
||
\subsubsection{加权滑动滤波}
|
||
|
||
对IK输出的关节角度序列做线性加权滑动均值滤波（窗口7帧），越近的帧权重越高，有效抑制IK求解引入的高频抖动。该滤波方法在机器人动作重定向领域被广泛采用：
|
||
|
||
\begin{lstlisting}[style=bashstyle]
|
||
w = np.arange(1, window_size + 1, dtype=float)
|
||
w /= w.sum() # 线性权重归一化，越新的帧权重越大
|
||
\end{lstlisting}
|
||
|
||
% 描述 WeightedMovingFilter
|
||
|
||
\subsubsection{关节限位约束}
|
||
|
||
在IK求解时加入ConfigurationLimit约束，将28个关节的角度限制在物理允许范围内，防止求解结果超出机器人实际关节行程：
|
||
|
||
\begin{lstlisting}[style=bashstyle]
|
||
# 将限位写入Pinocchio模型
|
||
self.model.lowerPositionLimit[q_idx] = H1Config.JOINT_LIMITS[idx, 0]
|
||
self.model.upperPositionLimit[q_idx] = H1Config.JOINT_LIMITS[idx, 1]
|
||
self.config_limit = ConfigurationLimit(self.model)
|
||
|
||
# IK求解时传入约束
|
||
velocity = pink.solve_ik(
|
||
self.configuration, self.tasks, dt,
|
||
solver="quadprog", damping=1e-3,
|
||
limits=[self.config_limit],
|
||
)
|
||
\end{lstlisting}
|
||
|
||
% 描述 ConfigurationLimit 的使用
|
||
|
||
\subsubsection{脚踝角度计算}
|
||
|
||
机器人没有脚掌关节，脚踝pitch角度无法通过IK直接求解。因此从SMPL几何信息中提取每帧小腿方向向量与脚掌方向向量的夹角，以第0帧（站立姿态）为基准计算相对变化量，直接覆写到脚踝pitch电机：
|
||
|
||
\begin{lstlisting}[style=bashstyle]
|
||
# 计算小腿与脚掌方向向量的夹角
|
||
shin = _normalize(ankle - knee)
|
||
toes = _normalize(foot - ankle)
|
||
angle = np.arccos(np.clip((shin * toes).sum(axis=-1), -1.0, 1.0))
|
||
|
||
# 以第0帧为中性角基准，限制在关节限位内
|
||
pitch = angle - angle[0]
|
||
pitch = np.clip(pitch, lo, hi)
|
||
\end{lstlisting}
|
||
|
||
% 描述从小腿/脚掌方向向量计算脚踝pitch角的方法
|
||
|
||
\subsection{其他}
|
||
|
||
\subsubsection{遥操作与实时重定向的区别}
|
||
|
||
目前业内已有端到端的实时遥操作方案（如GR-1、AnyTeleop等），直接从摄像头到机器人控制信号，延迟极低。本系统与其定位不同，对比如下：
|
||
|
||
\begin{table}[h]
|
||
\centering
|
||
\caption{实时遥操作方案与本系统对比}
|
||
\begin{tabular}{p{3cm}p{5.5cm}p{5.5cm}}
|
||
\toprule
|
||
& \textbf{实时遥操作（如GMR）} & \textbf{本系统（GVHMR + IK）} \\
|
||
\midrule
|
||
\textbf{输入来源} & 实时摄像头 & 视频文件或大模型npy输出 \\
|
||
\textbf{实时性} & 极低延迟，在线处理 & 离线为主，单帧$\approx$0.4ms \\
|
||
\textbf{精度} & 人体捕捉精度较低，但足够还原动作 & GVHMR精度较高 \\
|
||
\textbf{机器人泛化性} & 换机器人需重新训练模型 & 只需修改URDF和骨骼参数，无需重训 \\
|
||
\textbf{适用场景} & 实时控制、遥操作 & 动作生成、离线批量处理 \\
|
||
\textbf{主要优势} & 延迟低、部署简单 & 通用性强，兼容多种输入源 \\
|
||
\bottomrule
|
||
\end{tabular}
|
||
\end{table}
|
||
|
||
本系统优先接入大模型生成的npy动作数据，以通用IK重定向为核心，机器人结构发生变化时只需更新URDF和骨骼长度参数即可适配，无需重新训练任何模型，维护成本低。
|
||
|
||
\subsubsection{局部四肢避障方法}
|
||
|
||
|
||
在动作模仿过程中，若摄像头检测到障碍物与机器人四肢存在碰撞风险，需要对目标关节点位进行实时修正。
|
||
|
||
\textbf{基本思路}：不修改IK框架，而是在骨骼重定向阶段对SMPL关节点位进行几何修正，将避障后的新点位作为IK追踪目标传入。
|
||
|
||
\textbf{几何避障原理}：以手臂为例，肩膀、肘部、手腕三点构成一个两边长度固定（大臂长、小臂长）的运动链。当大臂或小臂检测到与障碍物碰撞时，在保持骨骼长度不变的约束下，迭代计算绕障后的新关节位置，使整个手臂绕过障碍物的同时保持人体物理结构不变形。腿部同理，由髋、膝、踝三点构成固定长度运动链做同样处理。
|
||
|
||
\textbf{局限性}：该方法基于纯几何计算，时效性好，适用于简单的局部避障场景。对于需要腰部扭转、重心转移等全身协调配合的复杂避障情况，单纯修正四肢点位无法保证整体姿态合理性，需要引入更完整的全身运动规划方案。
|
||
|
||
\begin{figure}[h]
|
||
\centering
|
||
\includegraphics[width=\textwidth]{arm_avoidance.png}
|
||
\caption{局部四肢避障几何方法示意图}
|
||
\label{fig:arm_avoidance}
|
||
\end{figure}
|
||
% 在这里写你对整个重定向方法的理解，遇到的问题和解决思路
|
||
|
||
\newpage
|
||
|
||
% ============================================================
|
||
% 第三章：问答记录
|
||
% ============================================================
|
||
\section{常见踩坑}
|
||
|
||
\begin{qabox}{quat旋转的作用是什么？}
|
||
\end{qabox}
|
||
|
||
\begin{answerbox}
|
||
大模型输出的SMPL关节坐标以Y轴朝上为标准坐标系，而本系统机器人重定向以Z轴朝上为基准。若不做坐标系变换，输入的SMPL人体在机器人坐标系下呈躺倒状态，无法正常重定向。
|
||
|
||
通过四元数 $q=[0.5, 0.5, 0.5, 0.5]$ 对应的旋转矩阵，将Y轴朝上变换为Z轴朝上：
|
||
|
||
\begin{lstlisting}[style=bashstyle]
|
||
rot = sRot.from_quat([0.5, 0.5, 0.5, 0.5]).as_matrix()
|
||
smpl_raw_seq = target_motion @ rot.T # [T, J, 3]
|
||
\end{lstlisting}
|
||
|
||
对于GVHMR视频提取的SMPL数据，输出已是Z轴朝上，无需做Y轴到Z轴的变换，只需绕Z轴旋转$-90°$调整朝向，对应quat参数为$[0, 0, -0.707, 0.707]$ （参考服务器端GVHMR目录下的逆解文件）。
|
||
\end{answerbox}
|
||
\vspace{1em}
|
||
|
||
% ---- 继续添加 Q&A ----
|
||
|
||
\subsection{骨骼重定向相关}
|
||
|
||
\begin{qabox}{若不将躯干与两臂任务解耦会有什么后果？}
|
||
\end{qabox}
|
||
|
||
\begin{answerbox}
|
||
若直接以SMPL肩膀点作为两臂IK追踪的anchor，由于机器人与SMPL骨骼比例不同，肩膀点无法完全对齐，导致以肩膀为起点的手肘、手腕等末端追踪点整体偏移，IK始终存在残差。
|
||
|
||
更严重的问题是，PINK本质上求解的是QP问题，当多个任务存在冲突时，求解器会在各任务间做权重妥协。若躯干与手臂任务耦合，某一侧手臂恰好能追上目标而另一侧追不上时，QP求解器为了整体最优会将误差集中到某个关节，导致该关节的轴向电机出现突变甚至疯转。
|
||
|
||
通过动态Anchor设计，每帧以机器人自身正运动学的肩膀位置为起点，将躯干运动与手臂末端追踪解耦，从根本上避免了上述问题。
|
||
\end{answerbox}
|
||
% Q&A
|
||
|
||
\subsection{IK求解相关}
|
||
|
||
\begin{qabox}{为什么不先插值到90fps再进行IK求解？}
|
||
\end{qabox}
|
||
|
||
\begin{answerbox}
|
||
IK求解的耗时与帧数成正比，若先插值到90fps再求解，计算量是当前方案的4.5倍。而先在20fps上完成IK求解，再对关节角度序列做三次样条插值升采样至90fps，精度损失极小，因为相邻帧间的关节角度变化平滑，插值能够很好地还原中间帧。
|
||
|
||
因此选择先IK后插值的方案，在几乎不影响动作质量的前提下将IK阶段耗时降低至原来的$\frac{1}{4.5}$，是性价比最高的做法。
|
||
\end{answerbox}
|
||
|
||
\begin{qabox}{为什么不在IK求解过程中加入速度控制？}
|
||
\end{qabox}
|
||
|
||
\begin{answerbox}
|
||
主要有三个原因：
|
||
|
||
第一，职责解耦。重定向模块只负责将人体动作映射为机器人关节角度，速度控制属于运动控制层的职责，两者混合会导致模块边界模糊，出现问题时难以定位。
|
||
|
||
第二，仿真与现实存在差距。PINK求解的是仿真环境下的机器人状态，加入速度控制后轨迹会产生延迟跟踪效果，而这个延迟在真实机器人上的表现与仿真并不一致，反而引入额外误差。
|
||
|
||
第三，需求已满足。本系统的目标是离线生成某段动作对应的机器人关节角度序列，并不需要在线实时跟踪，因此静态的角度序列输出已经完全满足需求，无需引入速度控制增加复杂度。
|
||
\end{answerbox}
|
||
|
||
|
||
% Q&A
|
||
|
||
% ============================================================
|
||
% 附录
|
||
% ============================================================
|
||
\newpage
|
||
\section{后续规划}
|
||
\subsection{目标}
|
||
|
||
\begin{itemize}
|
||
\item \textbf{训练机器人行走：} 利用重定向模块生成的高质量行走动作序列作为参考轨迹，通过强化学习训练机器人在仿真环境中实现稳定行走。
|
||
|
||
\item \textbf{去掉固定根节点限制：} 当前重定向模块输出的电机角度序列以固定waist为前提，无法直接在真实环境中执行带位移的动作。目标是结合强化学习，训练机器人在真实动力学约束下完成动作模仿，使其能够在现实环境中真正执行出对应动作。
|
||
\end{itemize}
|
||
|
||
\subsection{计划路线}
|
||
当前调研的技术路线如下：
|
||
|
||
\begin{center}
|
||
\begin{tcolorbox}[width=0.85\textwidth, colback=blue!5, colframe=blue!40,
|
||
boxrule=0.5pt, arc=3pt]
|
||
\centering
|
||
\textbf{输入} 大模型生成 / 视频提取 \\[4pt]
|
||
$\downarrow$ \\[2pt]
|
||
转换为SMPL关节坐标序列（.npy） \\[4pt]
|
||
$\downarrow$ \\[2pt]
|
||
动作重定向模块 $\rightarrow$ 输出电机角度序列（dof） \\[4pt]
|
||
$\downarrow$ \\[2pt]
|
||
接入强化学习模块 \\[2pt]
|
||
（结合动力学信息对角度序列进行调整与优化） \\[4pt]
|
||
$\downarrow$ \\[2pt]
|
||
\textbf{输出} 可在真实环境执行的机器人动作
|
||
\end{tcolorbox}
|
||
\end{center}
|
||
|
||
\subsection{参考方法}
|
||
|
||
\begin{itemize}
|
||
\item \textbf{humanoid-gym}：基于PPO的H1行走训练，用于验证仿真环境与机器人配置。
|
||
\item \textbf{ExBody2}：基于AMP框架的动作模仿，以重定向模块输出的dof序列作为参考轨迹，对重定向误差容忍度较高。
|
||
\item \textbf{OmniH2O}：全身精确动作模仿备选方案，关节角度直接跟踪，还原度更高但对重定向精度要求更高。
|
||
\end{itemize}
|
||
|
||
\newpage
|
||
\appendix
|
||
|
||
\section{关键参数说明}
|
||
|
||
\begin{table}[h]
|
||
\centering
|
||
\caption{重定向关键参数}
|
||
\begin{tabular}{lll}
|
||
\toprule
|
||
\textbf{参数名} & \textbf{值} & \textbf{说明} \\
|
||
\midrule
|
||
\texttt{scale} & 0.2466 & SMPL到机器人的缩放比例 \\
|
||
\texttt{aim\_fps} & 90 & 目标帧率 \\
|
||
\texttt{substeps} & 2 & IK每帧子步数 \\
|
||
\texttt{max\_step} & 0.01 & 目标点每步最大移动距离(m) \\
|
||
\texttt{damping} & 1e-3 & IK阻尼系数 \\
|
||
\bottomrule
|
||
\end{tabular}
|
||
\end{table}
|
||
|
||
\end{document} |