\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}              % 彩色盒子
\usepackage{fontawesome5}           % 图标
\usepackage{setspace}               % 行距
\usepackage{hyperref}               % 超链接

% ========== 页面设置 ==========
\onehalfspacing

% ========== 可修改的文档信息 ==========
\newcommand{\doctitle}{停车机器人}
\newcommand{\docsubtitle}{坐标变换问题排查与后续计划}
\newcommand{\projectname}{停车机器人项目}
\newcommand{\docauthor}{沈玉祥}
\newcommand{\docversion}{v1.0}
\newcommand{\docdescription}{本文档用于说明XXXX项目的设计、实现、测试及维护方法。}

% ========== 页眉页脚 ==========
\pagestyle{fancy}
\fancyhf{}
\fancyhead[L]{\small \doctitle}
\fancyhead[R]{\small \leftmark}
\fancyfoot[C]{\thepage}
\renewcommand{\headrulewidth}{0.4pt}

% ========== 颜色设置 ==========
\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}

% ========== 超链接设置 ==========
\hypersetup{
    colorlinks=true,
    linkcolor=blue!70!black,
    urlcolor=blue!70!black,
    citecolor=green!60!black,
    bookmarks=true,
    bookmarksnumbered=true,
    pdftitle={\doctitle},
    pdfauthor={\docauthor}
}

% ========== 代码块设置 ==========
\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
}

\lstdefinestyle{bashstyle}{
    backgroundcolor=\color{codebg},
    basicstyle=\ttfamily\footnotesize,
    breaklines=true,
    frame=single,
    rulecolor=\color{codeframe},
    language=bash,
    commentstyle=\color{codegreen},
    keywordstyle=\color{blue}
}

\lstset{style=pythonstyle}

% ========== 彩色盒子样式 ==========
\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 \doctitle\\[0.5em]
    \Large \docsubtitle}

    \vspace{2cm}
    \rule{\linewidth}{0.5mm}
    \vspace{1cm}

    {\large
    \begin{tabular}{ll}
        \textbf{项目名称：} & \projectname \\[0.5em]
        \textbf{作者：}     & \docauthor \\[0.5em]
        \textbf{日期：}     & \today \\[0.5em]
        \textbf{版本：}     & \docversion \\
    \end{tabular}
    }

    \vspace{1cm}
    \rule{\linewidth}{0.5mm}

    \vfill
    {\small \docdescription}
\end{titlepage}

% ========== 目录 ==========
\tableofcontents
\newpage

% ============================================================
% 使用时可直接修改下面各章节，也可以复制或删除任意示例
% ============================================================
\section{文档概述}

\subsection{编写目的}
本文档主要针对停车机器人坐标变换的问题排查以及后续规划安排的说明。

\subsection{项目位置}
XXXX：git链接以及分支

\subsection{已知问题}

\begin{enumerate}
    \item 坐标系变换问题bug
    \item ±120°物理限制无反馈方式
    \item Simple设置初次设置任务需要等待较长时间
\end{enumerate}

% \subsection{术语说明}
% \begin{table}[htbp]
%     \centering
%     \caption{术语说明示例}
%     \begin{tabular}{ll}
%         \toprule
%         \textbf{术语} & \textbf{说明} \\
%         \midrule
%         XXXX & 填写术语的完整名称及含义 \\
%         XXXX & 填写缩写、单位或专有概念 \\
%         \bottomrule
%     \end{tabular}
% \end{table}

\section{蟹行反向}

\subsection{问题描述}
蟹行模式发送正速度理论来说应往车体左侧运动，实际向车体右侧运动，这个不一致的问题

\subsection{问题原因}
SetChassisDirection(90) 的含义容易误解，它设置的“车身坐标系相对运动坐标系的偏置” +90°，不是让舵轮转到 +90°。
\begin{lstlisting}[caption={蟹行反向原因梳理}]
  右摇杆Y > 0
  → speed > 0
  → SetChassisDirection(+90°)
  → 舵轮零方向 ZeroDirection = +90°
  → SendMotion 的直行轴角 axisTh = 0°
  → 实际舵角目标 = axisTh - ZeroDirection
  → 0° - 90° = -90°
  → 正速度沿 -90° 方向运动
  → 整车向右蟹行
\end{lstlisting}

\subsection{解决思路}
增加类似ROS的TF变换的简化版本，暂时不对commonusage上层进行改动，针对停车机器人本项目增加适配层，明确语义，方便后续多车编队坐标系问题的排查。
\begin{lstlisting}[caption={目前解决思路梳理}]
MyParking
├─ Shared
│  ├─ FrameTransform2D.cs          坐标变换
│  ├─ ChassisCommand.cs            Pose2D、Twist2D等数据定义
│  ├─ FleetKinematics.cs           车队到各单车的运动学分解
│  └─ MultiWheelChassisAdapter.cs  包装原Chassis
├─ MedullaAdapter
└─ ClumsyPilot
\end{lstlisting}


\section{CommonUsage问题}
\subsection{问题整理}

\subsubsection{\texttt{SendXYThSpeed()}}

使用 $F$ 坐标系下的 \texttt{sw.Position} 计算角度，却直接作为 $B$ 坐标系舵角下发，未减去 \texttt{sw.ZeroDirection}。因此非零偏置下存在坐标系混用风险。

\subsubsection{\texttt{GetCarSpeed()}}

使用 $F$ 坐标系下的轮子位置，与 $B$ 坐标系下的舵角共同计算速度。非零偏置下，得到的 $V_x/V_y$ 坐标系不明确。

\subsubsection{差速舵左右轮位置计算}

\texttt{AccumulateSpeed()} 使用 $F$ 坐标系下的 \texttt{dsw.Position}，但轮子方向 \texttt{\_sendAngle[i]} 属于 $B$ 坐标系。应转换为：

\[
\texttt{wheelDirInFleet}
=
\texttt{\_sendAngle[i]}
+
\texttt{dsw.ZeroDirection}
\]

\subsubsection{\texttt{SendRotateMotion()}}

目标舵角使用 \texttt{sw.Position} 计算正确，但旋转半径使用了相对单车中心的 \texttt{CenterDistance()}。当编队原点存在位置偏置时，应改为：

\[
\texttt{sw.Position.Length()}
\]

\subsubsection{\texttt{CommonMath} 角度归一化}

当前使用以下方式进行角度归一化：

\begin{lstlisting}[language={[Sharp]C}]
angle - Math.Round(angle / 360f) * 360f
\end{lstlisting}

受 \texttt{Math.Round()} 银行家舍入影响，在 $\pm180^\circ$、$\pm540^\circ$ 等边界处可能同时出现 $180^\circ$ 和 $-180^\circ$ 两种表达。虽然物理含义相同，但容易影响范围判断，建议统一归一化到：

\[
[-180^\circ,\,180^\circ)
\]

\subsection{总体结论}

CommonUsage 的核心坐标变换公式基本正确，主要问题是接口没有明确规定输入输出属于车体坐标系 $B$、编队坐标系 $F$ 还是世界坐标系 $W$。

例如，\texttt{CommonMath.Transform2D(src, t)} 无法从方法名判断输入输出的坐标系、变换方向及数据类型。当前系统也缺少
\texttt{BodyFrame}、\texttt{FleetFrame}、\texttt{WorldFrame} 等坐标系定义，
\texttt{Pose2D}、\texttt{Twist2D} 等明确类型，以及
\texttt{BodyToFleet()}、\texttt{FleetToBody()} 等方向性接口。

因此，CommonUsage 具备坐标变换的数学工具，但缺少坐标变换的语义管理层。当 \texttt{originBias} 不为零时，\texttt{SendXYThSpeed()}、\texttt{GetCarSpeed()}、差速舵轮速计算及 \texttt{SendRotateMotion()} 存在坐标系混用或旋转半径不明确的问题。

更准确地说：核心变换思路正确，但接口契约不清晰，部分方法仅在
\texttt{originBias = 0} 时能够明确保证坐标一致。



\section{测试实验}

\subsection{当前进度}

目前已完成停车机器人M层和C层代码的整理、编译及部署，并通过实车验证。车辆现已能够正常完成前进、后退、蟹行和原地旋转等基本动作，C层、M层到底层电机的控制链路已经打通。

\subsection{后续计划}

下一阶段将针对直线、圆弧和贝塞尔曲线开展重复性实车测试。从底盘读取实际速度反馈，从Detour读取车辆实际运动轨迹，并使用Python脚本量化现有方法的轨迹跟踪精度和速度响应，绘制参考值与实际值的对比曲线。

在此基础上建立车辆运动学模型，设计基于LQR的Track轨迹跟踪方法，并研究舵轮$\pm120^\circ$物理约束下的运动控制和指令分解。首先完成单车轨迹跟踪测试与优化，待单车控制稳定后，再开展双车及多车协调控制研究。简要后续规划整理如下：

\begin{lstlisting}[caption={后续计划简要梳理}]
一、定义实验需要获取的物理量：车的速度反馈、位置信息
二、速度信息从M层可获取、位置信息计划从Detour中获得
三、后续主要先测多种的直线、圆弧和贝塞尔曲线、多次实验
四、获取下发速度、实际速度、理论轨迹、实际轨迹后用python脚本画出曲线；
  观察跟踪精度、横向偏差、纵向偏差、速度响应等量化指标
五、测量当前stanley控制器的量化跟踪的性能、编写LQR控制器
六、单车跑通跑稳之后增加±120°极限位置的打印/停车调整的防御策略，设置极限位置进行测试
七、单车调整过程中排查一些已知的bug，假如新写的适配层功能不足，可能会考虑修改commonusage的Chassis的部分代码
八、当单车调稳之后、编写双车之间的通信和调整方法，仿真验证坐标系转换是否正常再实车试验
\end{lstlisting}

% \begin{enumerate}
%     \item XXXX：填写流程的第一步。
%     \item XXXX：填写流程的第二步。
%     \item XXXX：填写流程的第三步。
% \end{enumerate}

\section{库版本不一致问题}
如图~\ref{fig:example}~所示，停车机器人仓库自带的ref的commonusage.dll会比仓库里commonusage的项目多一些方法，应该是之前排除问题的时候也对上层的commonusage进行了修改，有版本不一致的问题。
\begin{figure}[htbp]
    \centering
    \includegraphics[width=\linewidth]{pic.png}
    \caption{问题描述}
    \label{fig:example}
\end{figure}

\section{后续规划}

\subsection{个人想法}
\subsubsection{项目引用和版本问题}
目前的仓库项目的.csproj文件大多对dll的引用是通过绝对路径，建议使用相对路径自带一个ref文件夹来包含项目需要的依赖，同时需要确认依赖的版本一致。
\subsubsection{readme问题}
项目增加readme说明，固定格式，可通过简单的skill执行；同时一些实际执行遇到的问题可以记录成文件放在项目里面，方便学习和后续使用。
\subsubsection{技术文档}
技术文档的固定模板，有时候语言表达问题不全面，需要文档进行记录或者整理。
\subsection{版本记录}
\begin{table}[htbp]
    \centering
    \caption{版本记录}
    \begin{tabular}{llll}
        \toprule
        \textbf{版本} & \textbf{日期} & \textbf{作者} & \textbf{修改内容} \\
        \midrule
        v1.0 & \today & 沈玉祥 & 创建基础文档 \\
        \bottomrule
    \end{tabular}
\end{table}

\newpage

\section{时间计划节点}
\begin{figure}[htbp]
    \centering
    \includegraphics[width=\linewidth]{parking.png}
    \caption{停车机器人节点计划}
    \label{fig:parking}
\end{figure}


\end{document}