Mathematical contest in modeling

\documentclass[12pt]{article}
\usepackage{ctex}                % 支持中文
\usepackage{amsmath, amssymb}    % 数学公式
\usepackage{amsmath}  % 需要加载amsmath宏包
\usepackage{graphicx}            % 插入图片
\usepackage{geometry}            % 页面设置
\usepackage{booktabs}            % 表格横线
\usepackage{caption}             % 图片和表格标题
\usepackage{fancyhdr}            % 页眉页脚
\usepackage{hyperref}            % 超链接
\usepackage{titling}  % 控制标题和页首距离
\usepackage{titlesec}  % 调整标题样式
\usepackage{lipsum}   % 生成示例文本
\usepackage{float}
\geometry{a4paper, scale=0.8}

% 修改章节编号为中文 "一、二、三……"
\renewcommand{\thesection}{\chinese{section}、}

% 保留子节为数字格式 "1.1, 1.2"
\renewcommand{\thesubsection}{\arabic{section}.\arabic{subsection}}

\usepackage{titlesec}

% 设置大标题居中，并减小编号与标题文本之间的距离
\titleformat{\section}
  {\centering\Large\bfseries}  % 标题格式，居中
  {\thesection}                % 编号格式
  {0.01em}                      % 调整编号与标题文本之间的距离
  {}


% 页眉页脚设置
%\pagestyle{fancy}
%\fancyhead[L]{}
%\fancyhead[C]{}
%\fancyhead[R]{}
\fancyfoot[C]{\thepage}

% 设置标题与页首的距离
\setlength{\droptitle}{-2cm}  % 这里的值可以根据需要调整
% 设置标题与摘要之间的距离
%\setlength{\belowtitleskip}{0.5cm}  % 减少标题下方的距离
%\setlength{\abstitleskip}{0.5cm}    % 摘要上方的距离
%\setlength{\absparskip}{0.2cm}      % 摘要和段落的距离


% 调整大标题的字体大小
\titleformat{\title}{\normalfont\Large\bfseries}{\thetitle}{0.5em}{}
% 论文标题、作者和日期
\title{\heiti “板凳龙”等距螺线链式求解模型}
\date{}

\begin{document}

% 封面
\maketitle

\vspace{-3.7cm}  % 这里调整为适当的负值以减少间距

\begin{abstract}
   
本文针对“板凳龙”运动过程，分别建立了\textbf{几何模型}和\textbf{动力学模型}，\textbf{链式求解}舞龙队各节板凳在螺线盘入过程中的空间位置和速度分布。通过\textbf{碰撞检测模型}，确定了舞龙队盘入过程中发生碰撞的时间和位置。随后，基于\textbf{几何约束条件}，优化了螺距和调头路径，并通过\textbf{数值方法}求解了龙头的最大速度，确保各节板凳不超过速度限制。本研究通过精确的数值求解与优化方法，解决了舞龙队在有限空间内盘龙时的调头和速度控制问题，为类似文化活动的模型分析提供了理论依据。

针对问题一，本文建立了\textbf{几何模型}与\textbf{动力学模型}，分析了龙头以恒定速度沿螺线运动时各节板凳的空间位置与速度分布。首先，利用极坐标系构建螺线方程，从龙头的前把手出发，通过\textbf{二分法}探测相邻结点的位置关系，然后通过\textbf{链式求解法}模拟了各节板凳的运动状态，再根据\textbf{运动的关联性}，对速度进行分解，获得了各节板凳在0 - 300s内不同时间的精确位置和速度变化，结果保存在支撑材料的“result1.xlsx”文件中。通过微分几何方法分析，确保了模型的精确性，为后续的运动模拟和优化提供了理论依据。

针对问题二，舞龙队在第一问的基础上沿着螺距为55cm的等距螺线继续运动，根据问题一中给定的\textbf{螺旋线参数}，利用\textbf{极坐标方程}描述舞龙队各节板凳的中心和边缘位置，然后通过\textbf{碰撞测试模型}，研究了舞龙队盘入至板凳之间发生碰撞前的终止时刻，经过推算\textbf{终止时刻$t$为412.42s}。利用\textbf{链式求解法}计算了舞龙队各把手在该时刻的位置和速度，最终得出盘入过程的终止条件，相关数据保存在支撑材料的“result2.xlsx”文件中。

针对问题三，在调头空间为直径 9 米的圆形区域内，首先采用\textbf{数值求解法},用计算机枚举不同的螺距值，并逐步缩小范围。假定龙头的前把手恰好位于调头空间与螺线的交点处，此时采用\textbf{回溯法}，计算出所有把手的位置坐标。再通过\textbf{碰撞测试模型}依次判断板凳是否会发生碰撞，若在该螺距条件下恰好通过碰撞测试，则确定该螺距为舞龙队能够顺利调头所需的最小螺距。经过推算得到\textbf{最小螺距为0.46m}。

针对问题四，研究舞龙队在调头空间内的调头路径优化问题。舞龙队沿螺距为 1.7 m 的螺线盘入，在直径为 9 m 的调头空间内进行调头。通过建立\textbf{几何模型}和\textbf{运动学模型},确定了舞龙队在调头空间内的调头路径。
%调头路径由两段圆弧组成，前一段圆弧的半径为后一段的两倍。%为缩短调头路径，采用优化算法调整了圆弧的半径和位置，使其在保持各部分相切的情况下缩短了调头曲线。
对龙头位于不同曲线中的多种情况进行分类讨论，再通过精确的\textbf{数值计算}，得到了从 -100 秒至 100 秒各时刻舞龙队的具体位置和速度，结果存放在支撑材料的“result4.xlsx” 文件中。
%同时，我们在论文中列出了 -100 秒、-50 秒、0 秒、50 秒和 100 秒时的关键位置和速度。

针对问题五，在问题四所得路径的基础上，进一步研究了龙头行进速度对舞龙队行进速度的影响。为确保舞龙队各把手的速度均不超过 2 m/s，我们利用\textbf{速度微小增量检测模型}对各把手的速度进行了监控，并计算出了龙头的最大行进速度。经过推算得出\textbf{龙头最大行进速度为1.09m/s}。

    \textbf{关键词：} 等距螺线链式求解模型，几何模型，动力学模型，碰撞检测模型
\end{abstract}


\section{问题重述}
\subsection{问题背景}
随着当今社会对文化活动与传统技艺的重视，舞龙作为中华传统文化的重要表现形式之一。板凳龙作为一种独特的舞龙形式，如何优化板凳龙的运动路径、避免队伍间的碰撞、以及提升表演的流畅性与安全性，仍是亟待解决的问题。本题给出舞龙队的组成和结构，模拟板凳沿等距螺线盘入盘出的过程，通过合理的路径规划与速度控制，使其在复杂场景中保持安全、流畅的运动。因此，研究板凳龙的运动模型，并通过优化各节板凳的运动状态，不仅具有文化价值，也具备重要的实际意义。

\subsection{问题提出}
本题研究对象为沿着等距螺旋线行进的板凳龙，通过路径规划和碰撞检测优化各节板凳的运动状态，从而提高舞龙队的安全性和观赏性。

\textbf{问题一} 舞龙队沿着螺距为55cm的等距螺线顺时针盘入，所有把手中心均位于螺线上。在运动过程中， 龙头前把手以1m/s的速度前进，带动后面相连的龙身依次向前运动。初始时龙头前把手位于螺线第16圈的A点。从初始时刻运动至300秒，，要求求得龙头、龙身和龙尾每一秒的位置和速度。

\textbf{问题二} 舞龙队沿问题一设定的螺线盘入，运动到一定时刻，由于螺距、板凳宽度的限制条件，龙头会与龙身发生碰撞，此时运动停止，需确定此时运动的时间，并求出此时龙头、龙身以及龙尾的位置以及速度。

\textbf{问题三} 舞龙队从顺时针盘入到逆时针盘出需要经过一个直径为9米的圆形调头空间，确保在所有板凳满足约束条件的情况下，计算龙头前把手沿螺线盘入至调头空间边界所需的最小螺距，以保证能够顺利进入调头空间。

\textbf{问题四} 本题研究舞龙队在调头空间内的调头路径优化问题。舞龙队在调头空间内完成调头，路径为两段相切的圆弧构成的 S 形曲线，前一段圆弧的半径是后一段的两倍，且与盘入、盘出螺线相切。分析是否可以调整圆弧，仍保持各部分相切，但缩短调头曲线的长度。优化调头曲线并计算舞龙队在调头前后的每秒位置和速度。

\textbf{问题五} 在问题四确定路径的基础上，需要加快龙头行进速度，在整个运动过程中，龙头行进速度仍保持不变，要求求出在运动过程中，舞龙队各把手的速度均不超过2m/s的情况下，龙头的最大行进速度。


\section{问题分析}
\subsection{对问题一的分析}
问题一可以看作是一个由多个相连的刚性体组成的\textbf{链状系统}，每个节点的运动状态都可以由其前一节点的运动决定。选择二维平面直角坐标系，以螺线中心为原点建立坐标系，利用\textbf{螺线极坐标方程}（ $r = a + b\theta$）来描述每个节点（龙头、龙身和龙尾）在盘入过程中位置的变化。问题一的难点在于准确反映舞龙队整体的耦合运动，首先采用\textbf{微元法}，将时间进行微分，每个微元时间内物体运动轨迹近似为圆，求出每个时刻龙头的位置，然后通过\textbf{链式求解法}和\textbf{二分法}，根据板凳长度限制确定每一节板凳的后一节板凳位置，由此确定所有把手坐标，最后根据位置关系和\textbf{运动的关联性}，相连板凳之间的实际速度沿着板凳方向的分速度相等，由此确定所有把手在各个时刻的速度。

\subsection{对问题二的分析}
问题二要求确定舞龙队在问题一设定的螺线盘入过程中，能够继续盘入而不发生板凳之间碰撞的终止时刻。终止时刻确定后，需要记录此时龙头、龙身和龙尾的各个把手位置和速度。问题二的核心难点在于，随着舞龙队在螺线上盘入，板凳之间的相对距离会逐渐缩短。由于龙头长度大于其他板凳，盘入到一定程度后，龙头与其他板凳可能发生碰撞。因此，准确计算此时刻并避免板凳相互碰撞是本问题的关键。根据问题一中给定的\textbf{螺旋线参数}，在笛卡尔坐标系下描述舞龙队各节板凳的中心和边缘位置，在确定龙头位置坐标后，通过\textbf{链式求解法}求出该时刻各把手的位置，根据板凳的几何结构及其在螺旋线上的分布，可以建立\textbf{碰撞测试模型},判定龙头与其他板凳是否发生碰撞，发生碰撞的最早时刻即为所求。

\subsection{对问题三的分析}
问题三要求舞龙队由顺时针盘入切换为逆时针盘出，并需要一定的调头空间。调头空间为以螺线中心为圆心、直径为9米的圆形区域，需要确定最小螺距，使得龙头前把手能够沿着相应的螺线盘入到调头空间的边界。本问题的主要难点在于如何根据螺旋线的几何性质和板凳之间的约束条件确定最小螺距，使得舞龙队能够在有限的调头空间内顺利完成盘入并调头。我们采用\textbf{数值方法}枚举不同的螺距，并逐步缩小测试范围，假定龙头的前把手恰好位于调头空间与螺线的交点处，采用\textbf{回溯法}倒推其他板凳的位置坐标，再利用第二题的\textbf{碰撞测试模型}，检测龙头与其他板凳是否会发生碰撞。

\subsection{对问题四的分析}
问题四要求舞龙队在问题三给出的调头空间内完成调头，路径为两段相切的圆弧构成的 S 形曲线，前一段圆弧的半径是后一段的两倍，且与盘入、盘出螺线相切。我们选取\textbf{临界状态}即龙头的前把手恰好进入调头空间进行分析，确定了舞龙队的调头轨迹。问题四的难点在于整个舞龙队的运动轨迹是由多条复杂的平滑曲线拼接而成，当相邻把手处在不同的曲线上时，速度关联选取的曲线方程不同会增加求解复杂性。舞龙队在求出的路径下进行运动，根据龙头在调头轨迹中的不同位置进行分类讨论，利用\textbf{链式求解模型}，求出各把手的位置和速度。在该调头曲线上求解调头前后100s各个把手的位置和速度。

\subsection{对问题五的分析}
针对问题五，在问题四的基础上，进一步研究了龙头行进速度对舞龙队行进速度的影响。为确保舞龙队各把手的速度均不超过 2 m/s，我们利用\textbf{速度微小增量检测模型}，在原有1m/s行进速度的基础上，逐步增加龙头速度，然后对各把手的速度进行了监控，直到存在把手速度超过限制条件的情况为止，此时龙头行进速度为最大行进速度。


\section{模型假设}
1.所有板凳被视为刚性物体，不考虑板凳在运动或连接过程中的弹性形变。

2.假设所有板凳的长度、宽度和孔的位置均为固定值，不会因外力或其他因素发生变化。

3.把手的连接是理想的，不考虑连接点的松动、磨损或因力矩过大而引起的断裂或偏移。

4.假设在运动过程中，所有板凳保持在同一平面上，并且板凳之间的相对位置关系保持不变。

5.假设在运动过程中，所有把手中心均落在螺旋线上，不考虑复杂的曲线或摆动情况。

6.假设每一节板凳的速度是其前一节板凳速度的线性或非线性函数，保持平稳传导。

7.假设在板凳龙的调头或转弯过程中，速度变化是平滑的，没有突然的加速或减速现象。各节板凳的速度在曲线段之间保持连续可导的变化。


\section{符号说明}
\begin{table}[H]
\centering
%\caption{符号说明}
\begin{tabular}{ccc}
\toprule
符号 & 说明 & 单位 \\
\midrule
$l_{width}$ & 板凳的宽度 & m \\
$l_{head}$ & 龙头的长度 & m \\
$l_{body}$ & 龙身的长度 & m \\
$l_{tail}$ & 龙尾的长度 & m \\
$l_{dist}$ & 前（后）把手到板凳前（后）的距离 & m \\
$l_{fg}$ & 弧长FG & m \\
$d_h$ & 孔径 & m \\
$d_1$ & 碰撞点到把手连线的距离 & m \\
$d_2$ & 碰撞点到把手连线中垂线的距离 & m \\
$r$ & 极径 & m \\
$b$ & 螺距 & m \\
$R$ & 调头空间的半径 & m \\
$r_1$ & 大圆弧的半径 & m \\
$r_2$ & 小圆弧半径 & m \\
$t$ & 运动时间 & s \\
$v_h$ & 龙头的速度 & m/s \\
$v_{max}$ & 龙头的最大速度 & m/s \\
$v_i$ & 每节板凳的最大速度速度 & m/s \\
$\theta$ & 极角 & rad \\
$\alpha$ & A点速度方向与板凳方向的夹角&  rad \\
$\beta$ & B点速度方向与板凳方向的夹角 & rad \\
$\gamma$ &圆心角  & rad \\


\bottomrule
\end{tabular}
\end{table}

\section{模型建立与求解}

\subsection{问题一模型的建立与求解}

\subsubsection{模型的建立}
根据问题，构建链式求解模型。首先，以螺线中心为原点建立坐标系，
\begin{eqnarray}
r & = & \frac{b}{2\pi}\theta  
\end{eqnarray}
其中，$b$为螺距，$r$为极径，$\theta$为极角。

采用\textbf{微元法}对时间微分，在$\Delta$t时间内，龙头的角度改变量为 
\begin{eqnarray}
\Delta \theta & = & \frac{v_h*\Delta t}{r} 
\end{eqnarray}

此时极角和极径分别为
\begin{eqnarray}
\theta_1 & = & \theta -\Delta \theta 
\end{eqnarray}
\begin{eqnarray}
r_1 & = & \frac{b}{2\pi}\theta_1 
\end{eqnarray}

由此确定了龙头的位置坐标，再根据板凳长度限制采用\textbf{二分法}确定每一节板凳后一节的位置,链式解出所有把手任意时刻的位置坐标。
\begin{figure}[h]
    \centering
    \includegraphics[width=1.0\textwidth]{img/3.png}  % 替换为你的图片文件名
    \caption{二分法演示}
    \label{fig:er}
\end{figure}


对任意板凳上的前后两个把手的中心A,B分析，由\textbf{运动的关联性}，A,B实际速度沿着板凳方向的分速度相等，由此\textbf{链式}解出所有把手的任意时刻的速度大小。

A,B两点连线的斜率$k$为
\begin{eqnarray}
k & = & \frac{y_A-y_B}{x_A-x_B} 
\end{eqnarray}

A点处切线斜率$k_a$为
\begin{equation}
\begin{cases}
x_A = x(\theta) \\
y_A = y(\theta) 
\end{cases}
\label{eq:system}
\end{equation}

\begin{eqnarray}
k_a & = & \frac{\mathrm{d} y_A}{\mathrm{d} x_A}
\end{eqnarray}

设A点实际速度的方向与AB连线方向的夹角为$\alpha$，则
\begin{eqnarray}
\tan \alpha = \frac{k-k_a}{1+k*k_a} 
\end{eqnarray}

同理得，B点实际速度的方向与AB连线方向的夹角$\beta$为
\begin{eqnarray}
\tan \beta = \frac{k_b-k}{1+k*k_b} 
\end{eqnarray}

\begin{figure}[H]
    \centering
    \includegraphics[width=0.7\textwidth]{img/2.jpg}  % 插入图片
    \caption{关联速度}
    \label{fig:dragon}
\end{figure}
由A,B沿板凳方向的分速度相等建立等式
\begin{eqnarray}
v_a\cos \alpha =v_b\cos \beta 
\end{eqnarray}

已知龙头的速度始终为1m/s，由运动的关联性链式即可解出所有把手任意时刻的速度大小。

\subsubsection{结果呈现}
将题目所给参数带入模型后，运行结果保存在支撑材料的“result1.xlsx”文件中。0 s、60 s、120 s、180 s、240 s、300 s 时，龙头前把手、龙头后面第 1、51、101、151、201 节龙身前把手和龙尾后把手的位置和速度如下表

% 表 1: 位置结果
\begin{table}[H]
    \centering
    \caption{\quad 位置结果}
    \begin{tabular}{|l|c|c|c|c|c|c|}
        \hline
        & 0 s & 60 s & 120 s & 180 s & 240 s & 300 s \\ \hline
        龙头 x (m)& 8.800000& 5.796937& -4.090647& -2.953271& 2.578989& 4.431352 \\ \hline
        龙头 y (m) &0& -5.773326& -6.300647& -6.300647& 6.099632& 2.298258 \\ \hline
        第1节龙身 x (m) &8.363824 &7.455392 &-1.452245 &-5.229693 &4.810845& 2.480875 \\ \hline
        第1节龙身 y (m)& 2.826544& -3.443277 &-7.404476 &4.368302& -3.575533& 4.389966 \\ \hline
        第51节龙身 x (m) &-9.518732& -8.685393& -5.537882& 2.901004& 5.97061& -6.299132 \\ \hline
        第51节龙身 y (m) &1.341137 &2.543156 &6.382425 &7.244936 &-3.842016 &0.490466 \\ \hline
        第101节龙身 x (m)& 2.913983 &5.684503& 5.356262 &1.887636 &-4.930766 &-6.22425 \\ \hline
        第101节龙身 y (m)& -9.918311& -8.003205& -7.561583& -8.473989& -6.369293 &3.956731 \\ \hline
        第151节龙身 x (m) &10.861726 &6.684755 &2.395454 &1.016482 &2.981406 &7.053565 \\ \hline
        第151节龙身 y (m) &1.828753& 8.132502& 9.725702 &9.42343 &8.393867 &4.371896 \\ \hline
        第201节龙身 x (m) &4.555102 &-6.617082 &-10.626275& -9.292367 &-7.467907& -7.472681 \\ \hline
        第201节龙身 y (m) &10.725118& 9.027433 &1.366694& -4.23626 &-6.167484& -5.243049 \\ \hline
        龙尾（后）x (m) &-5.305444 &7.3621 &10.974821 &7.391946 &3.257137 &1.809236 \\ \hline
        龙尾（后）y (m) &-10.676584& -8.800016 &0.83658& 7.484295& 9.463658& 9.296254 \\ \hline
    \end{tabular}
\end{table}

% 表 2: 速度结果
\begin{table}[H]
    \centering
    \caption{\quad 速度结果}
    \begin{tabular}{|l|c|c|c|c|c|c|}
        \hline
        & 0 s & 60 s & 120 s & 180 s & 240 s & 300 s \\ \hline
        龙头 (m/s) & 1& 1&1 &1 &1 &1 \\ \hline
        第1节龙身 (m/s) &0.999971 &0.999961 &0.999945 &0.999917 &0.999859 & 0.999709\\ \hline
        第51节龙身 (m/s) &0.999742 & 0.999662& 0.999538&0.999331 & 0.998941& 0.998064\\ \hline
        第101节龙身 (m/s) &0.999575 &0.999453 &0.999269 &0.998971 & 0.998435& 0.997301\\ \hline
        第151节龙身 (m/s) & 0.999448&0.999299 & 0.999078&0.998727 &0.998115 & 0.99686\\ \hline
        第201节龙身 (m/s) & 0.999348&0.99918 & 0.998935&0.998551 &0.997893 & 0.996574\\ \hline
        龙尾（后）(m/s) &0.999311 &0.999136 &0.998883 &0.998489 & 0.997816& 0.996477\\ \hline
    \end{tabular}
\end{table}

\subsection{问题二模型的建立与求解}
\subsubsection{模型的建立}
根据问题，构建\textbf{碰撞测试模型}，舞龙队沿问题 1 设定的螺线盘入，根据问题一中给定的\textbf{螺旋线参数}，在笛卡尔坐标系下表示舞龙队各节板凳的中心和边缘位置，进行\textbf{碰撞检测}。
记发生碰撞时龙头前后把手中心分别落在A,B点，该板凳的四个顶点分别记作C,D,E,F（不妨设C,D为距离A更近的顶点）,记与龙头发生碰撞的板凳前后把手中心分别落在P,Q点，过线段PQ中点M作PQ的中垂线MN。

据分析，发生碰撞的板凳可能在C,D,E,F任一点与其他任一板凳碰撞，下面以C点为例建立\textbf{碰撞测试模型},余下的点\textbf{以此类推}。
\begin{figure}[H]
    \centering
    \includegraphics[width=0.7\textwidth]{img/4.png}  % 替换为你的图片文件名
    \caption{碰撞模拟图}
    \label{fig:exameppp}
\end{figure}

首先，在第一问基础上，运用\textbf{微元法}，将时间微分求出龙头前把手的坐标，并根据\textbf{速度关联}求出龙头后把手的坐标。

然后，已知前后把手位置坐标，由把手和板凳的相对位置即可确定顶点C的位置

记直线AB的参数方程为
\begin{eqnarray}
y=k*x+b
\end{eqnarray}

由\textbf{几何关系}，设直线CE和直线CD的参数方程分别为
\begin{eqnarray}
y=k*x+b_1
\end{eqnarray}
\begin{eqnarray}
y=-\frac{1}{k} *x+b_2
\end{eqnarray}
联立直线CE,CD解出C点坐标$C(x(b_1,b_2),y(b_1,b_2))$

将直线AB化为一般方程，由\textbf{两直线间的距离公式}，C点到直线AB的距离为$\frac{1}{2}$$l_{width}$ ，到过A且与AB垂直的直线的距离为$l_{dist}$，由此解出$b_1$,$b_2$，建立了A点到C点的映射。

记直线PQ的参数方程为
\begin{eqnarray}
y=k_1*x+b_1
\end{eqnarray}

记直线MN的参数方程为
\begin{eqnarray}
y=k_2*x+b_2
\end{eqnarray}

下面进行\textbf{碰撞检测},记C点到其他任一板凳两条中心轴线（PQ和MN）的距离分别为$d_1$和$d_2$，则
\begin{align}
d_1=\frac{\left | k_1x_c-y_c+b_1 \right | }{\sqrt{k_1^{2}+1  } }\le \frac{1}{2}d_w  \\
d_2=\frac{\left | k_2x_c-y_c+b_2 \right | }{\sqrt{k_2^{2}+1  } }\le d_l
\end{align}

根据A,B的相对位置关系，存在如下四种碰撞可能性，分析方法如上。
\begin{figure}[H]
    \centering
    \includegraphics[width=0.7\textwidth]{img/5.png}  % 替换为你的图片文件名
    \caption{四种碰撞模拟图}
    \label{fig:examanjanj}
\end{figure}

\subsubsection{结果呈现}
将题目所给参数带入模型后，运行结果保存在支撑材料的“result2.xlsx”文件中。龙头前把手、龙头后面第 1、51、101、151、201 条龙身前把手和龙尾后把手的位置和速度。如下表

% 表 3: 
\begin{table}[H]
    \centering
    %\caption{\quad 速度结果}
    \begin{tabular}{|l|c|c|c|c|c|c|}
        \hline
        & 横坐标(m) & 纵坐标(m) & 速度（m/s）\\ \hline
        龙头  &1.214147 & 1.939922&1 \\ \hline
        第1节龙身  &-1.639979 &1.756715 &0.991547 \\ \hline
        第51节龙身  &1.285945 &4.32508 & 0.976852\\ \hline
        第101节龙身  &-0.541184 & -5.879612& 0.974544\\ \hline
        第151节龙身  &0.963918 &-6.9581 &0.973602 \\ \hline
        第201节龙身  &-7.89387 &-1.225856 & 0.973090\\ \hline
        龙尾（后） &0.961136 &8.322117 &0.972932 \\ \hline
    \end{tabular}
\end{table}

\subsection{问题三模型的建立与求解}
\subsubsection{模型的建立}
根据问题，舞龙队由顺时针盘入切换为逆时针盘出时，需要足够的调头空间。调头空间定义为一个以螺线中心为圆心、直径为9米的圆形区域，求解舞龙队在盘入时的最小螺距，以使得龙头的前把手能够顺利盘入到调头空间的边界。我们采用\textbf{数值方法}进行求解,用计算机枚举不同的螺距值，逐步缩小范围，并利用\textbf{碰撞测试模型}和\textbf{回溯法}，从龙头前把手恰好到达掉头空间倒推该螺距下其他板凳的位置，判断舞龙队是否会发生碰撞，直到找到刚好使得龙头前把手到达调头空间边界的最小螺距。
\begin{figure}[H]
    \centering
    \includegraphics[width=0.5\textwidth]{img/3.jpg}  % 替换为你的图片文件名
    \caption{回溯法演示图}
    \label{fig:exampppp}
\end{figure}

\subsubsection{模型的求解}
采用\textbf{数值方法},首先确定一个固定的螺距$b_i$($b_i$通过计算机枚举产生)，由此可以推出曲线方程为
\begin{eqnarray}
r_i & = & \frac{b_i}{2\pi}\theta  
\end{eqnarray}
其中，$r_i$为极径，$b_i$为螺距,$\theta$为极角

对任意板凳上的前后两个把手的中心A,B分析，由\textbf{运动的关联性}，A,B实际速度沿着板凳方向的分速度相等，由此链式解出所有把手的任意时刻的速度大小。

A,B两点连线的斜率$k$为
\begin{eqnarray}
k & = & \frac{y_A-y_B}{x_A-x_B} 
\end{eqnarray}

A点处切线斜率$k_a$为
\begin{equation}
\begin{cases}
x_A = x(\theta) \\
y_A = y(\theta) 
\end{cases}
\label{eq:system}
\end{equation}

\begin{eqnarray}
k_a & = & \frac{\mathrm{d} y_A}{\mathrm{d} x_A}
\end{eqnarray}

设A点实际速度的方向与AB连线方向的夹角为$\alpha$，则
\begin{eqnarray}
\tan \alpha = \frac{k-k_a}{1+k*k_a} 
\end{eqnarray}

同理得，B点实际速度的方向与AB连线方向的夹角$\beta$为
\begin{eqnarray}
\tan \beta = \frac{k_b-k}{1+k*k_b} 
\end{eqnarray}

由A,B沿板凳方向的分速度相等建立等式
\begin{eqnarray}
v_a\cos \alpha =v_b\cos \beta 
\end{eqnarray}

然后，通过\textbf{链式求解法}推导其他所有把手在该时刻的位置和速度。

在第二问的基础上，对舞龙队进行\textbf{碰撞测试}，检查所有边界条件是否满足要求，如果发生碰撞，则微调螺距重复上述操作，直到找到满足要求的\textbf{最小螺距}为止。

经过计算机\textbf{线性探测},根据推算满足要求的\textbf{最小螺距为0.46m}。

\subsection{问题四模型的建立与求解}
\subsubsection{模型的建立}
问题四中，舞龙队需要在一个更加复杂的环境中运动，涉及多重路径、边界条件等其他约束条件。相比前几问，问题四增加了轨迹多样性，要求我们对已有的运动模型进行进一步扩展。

舞龙队沿螺距为 1.7 m 的螺线盘入，在直径为 9 m 的调头空间内进行调头。调头路径由两段圆弧组成，前一段圆弧的半径为后一段的两倍。我们选取\textbf{临界状态}即龙头的前把手恰好进入调头空间进行分析，在该调头曲线上求解调头前后100s各个把手的位置和速度。

我们可以将该模型分成两大部分。第一部分重点在求舞龙队在调头空间里调头的轨迹。第二部分重点在求龙头、龙身和龙尾的位置以及速度。对于第二部分，我们可以采用分类讨论的思想，将轨迹分为四部分，分别为盘入螺线、第一段圆弧、第二段圆弧和盘出螺线。对于龙头前把手位于四部分的情况进行分类讨论，再通过链式求解法确定后一个把手的位置，结合二分法，最终确定精确的位置关系。

\begin{figure}[H]
    \centering
    \includegraphics[width=0.5\textwidth]{img/6.png}  % 替换为你的图片文件名
    \caption{调头演示图}
    \label{fig:examiiox}
\end{figure}

\subsubsection{模型的求解}
对于第一部分，我们可以通过几何关系，利用笛卡尔坐标系，计算出螺线方程，调头空间的方程，将螺线方程与调头空间的方程进行联立，再将A，B，C，D坐标设出，根据几何关系设出方程进行解答。

已知调头空间是以螺线中心为圆心、直径为 9 m 的圆形区域，我们从\textbf{临界状态}进行分析，考虑龙头恰好进入调头空间（即图中A点）时和龙头恰好离开调头空间（即图中B点）时两种临界状态。

等距螺线的极坐标方程为
\begin{eqnarray}
r & = & \frac{b}{2\pi}\theta 
\end{eqnarray}
其中，b为螺距，r为极径，$\theta$为极角

调头空间的参数方程为
\begin{align}
x^2+y^2=R^2\\
x=r\cos \theta \\
y=r\sin \theta
\end{align}
其中，R为调头空间的半径，r为极径，$\theta$为极角。

联立等距螺线方程和圆的方程可以解出A点坐标，由对称性可以解出B点坐标。

由\textbf{几何关系},圆C与盘入螺线相切于A点，圆D与盘出螺线相切与B点，由对称性，直线AB一定过坐标原点，且直线AC与直线BD平行。取A点和圆心C点的中点为I，依题意，圆C半径$r_1$是圆D半径$r_2$的两倍，由几何关系，D点和I点关于坐标原点中心对称。

根据$r_1$和$r_2$的关系建立方程组。
\begin{align}
r_1 & = \sqrt{(x_A-x_C)^2+(y_A-y_C)^2}  \\
r_2 & = \sqrt{(x_B-x_D)^2+(y_B-y_D)^2}  \\
r_1 & = 2*r_2 
\end{align}

从而解出参数t,即可解出圆心C坐标，进而得到圆C和圆D的参数方程。


对于第二部分，我们可以将轨迹分为四部分，盘入螺线，第一段圆弧，第二段圆弧和盘出螺线，分别命名其为轨迹一、轨迹二、轨迹三和轨迹四。
\begin{figure}[H]
    \centering
    \includegraphics[width=0.5\textwidth]{img/4.jpg}  % 替换为你的图片文件名
    \caption{}
    \label{fig:example}
\end{figure}

如何判断把手的位置仅以下列的图作为简单的实例。
假设F点为其中一个把手，已经进入轨迹二中，判断其后一个节点是否在轨迹二中。我们可以设一个点G位于轨迹二上
使得
\begin{eqnarray}
l_{fg} & = & d_{dist}
\end{eqnarray}

其对应的圆心角大小为$\gamma _1$，再计算弧FA对应的圆心角，大小为$\gamma _2$

若$\gamma _1> \gamma _2$则表明后一节点位于轨迹一上。

若$\gamma _1< \gamma _2$，则表明后一节点位于轨道二上。

依次类推，计算出每一个把手位于的轨迹，再结合问题一的思路，运用\textbf{二分法}，最终确定把手的具体位置。
速度同样可以借鉴问题一的处理方法，计算出实际速度与板凳的夹角，通过对速度进行分解，沿板凳的速度相同，来求解。


\subsubsection{结果呈现}
将题目所给参数带入模型后，运行结果保存在支撑材料的“result4.xlsx”文件中。−100s、−50s、0s、50s、100s 时，龙头前把手、龙头后面第 1、51、101、151、201节龙身前把手和龙尾后把手的位置和速度如下表。
% 表 1: 位置结果
\begin{table}[H]
    \centering
    \caption{\quad 位置结果}
    \begin{tabular}{|l|c|c|c|c|c|c|}
        \hline
        & -100 s & -50 s & 0 s & 50 s & 100 s  \\ \hline
        龙头 x (m)& 7.742197& 6.594403 & -2.711856& 1.288222& -3.232722 \\ \hline
        龙头 y (m) &3.797509& 1.954535& -3.591078& 6.186761& 7.519400 \\ \hline
        第1节龙身 x (m) & 6.149233& 5.331718 &-0.063534 &3.827529 &  -0.427803\\ \hline
        第1节龙身 y (m)&6.172812 & 4.520705 & -4.670888& 4.870889& 8.077995 \\ \hline
        第51节龙身 x (m) &-10.632459 &-3.577388  &2.459962 & -1.713085& 2.024927 \\ \hline
        第51节龙身 y (m) &2.747033 & -8.986629 &-7.778145 &-6.063134 &4.074820  \\ \hline
        第101节龙身 x (m)&-11.891364 &  10.155895& 3.008493&-7.566012 &  -7.260999\\ \hline
        第101节龙身 y (m)&-4.884474 & -5.923519 & 10.108539& 5.210865&2.148392  \\ \hline
        第151节龙身 x (m) &-14.340378 & 12.9919 &-7.002789 & -4.644707&9.428786  \\ \hline
        第151节龙身 y (m) &-2.068227 & -3.755708 & 10.337482&-10.368225 & -3.622596 \\ \hline
        第201节龙身 x (m) &-12.012074 &10.56417  & -6.872842& 0.293194&  8.58586\\ \hline
        第201节龙身 y (m)&10.501995 &-10.768144  &12.382609 & -13.177758& 8.542785 \\ \hline
        龙尾（后）x (m) &-0.923433 &0.132573  &-1.933627 & 5.898380& -11.024927 \\ \hline
        龙尾（后）y (m) &-16.534138 & 15.722160 &-14.713128 &12.59363 & -6.693262 \\ \hline
    \end{tabular}
\end{table}

% 表 2: 速度结果
\begin{table}[H]
    \centering
    \caption{\quad 速度结果}
    \begin{tabular}{|l|c|c|c|c|c|c|}
        \hline
        & -100 s & -50 s & 0 s & 50 s & 100 s  \\ \hline
        龙头 (m/s) &1 &1 & 1& 1&1  \\ \hline
        第1节龙身 (m/s)  &0.999904 & 0.999762&0.998687 &1.000362 & 1.000124 \\ \hline
        第51节龙身 (m/s) &0.999347 &0.998643 &0.995134 &0.960396 & 1.003955 \\ \hline
        第101节龙身 (m/s)  &0.999092 &0.998249 & 0.994448&0.958926 & 1.100962 \\ \hline
        第151节龙身 (m/s) &0.998945 & 0.998049& 0.994156&0.958477 &1.100000  \\ \hline
        第201节龙身 (m/s) &0.99885 &0.997927 &0.993994 &0.95826 & 1.099626 \\ \hline
        龙尾（后）(m/s) &0.998818 &0.997887 & 0.993944& 0.958196&  1.099525\\ \hline
    \end{tabular}
\end{table}

\subsection{问题五模型的建立与求解}
问题五要求在问题四确定的路径上，通过加快龙头初始行进速度，但要求龙头在行进过程中仍保持速度不变，在其他各把手的速度不大于2m/s的情况下，求出龙头的最快速度。该问题的关键在于考虑舞龙队在不同路径中的运 动，特别是龙头加速后，各节板凳的速度变化情况。

问题五其实是在问题四的基础上对速度进行的一个更深入的研究。在问题四所建立的模型上，进一步进行调整和补充完善。

首先，建立\textbf{几何模型}。问题四中，舞龙队沿螺线盘入，进入调头区域后，通过两段圆弧相切的 S 形曲线实现调头。曲线的前段半径为后段的两倍，并与盘入、盘出螺线相切。可以通过参数化方程描述 S 形曲线
\begin{align}
x_1(t) & = r_1 \cos(\theta(t))\\
y_1(t) & = r_1 \sin(\theta(t))
\end{align}
其中，$r_1$为前段圆弧的半径，t为运动时间。

\begin{align}
x_2(t) & = r_2 \cos(\theta(t))\\
y_2(t) & = r_2 \sin(\theta(t))
\end{align}
其中，$r_2$为后段圆弧的半径，t为运动时间。

然后，建立\textbf{速度传播模型}。舞龙队中各节板凳的位置和速度是依赖龙头的运动来确定的,为了确保舞龙队各节板凳的速度不超过 2 m/s，我们需要控制龙头的速度。假设龙头的最大速度为 $v_{max}$，采用\textbf{数值求解法},用计算机枚举不同的速度，逐步缩小范围，利用\textbf{链式求解法}，根据速度关联性求解出每节板凳的最大速度 $v_i$，确保每节板凳的最大速度不超过2m/s。
                                                                                                
将题目所给参数带入模型后，可以求得，\textbf{龙头的最大速度应为1.09m/s}。


\section{模型的分析与检验}
\subsection{模型与思想分析}
1.\textbf{微元法}：通过将时间进行微分，在微小的时间段内，舞龙队的运动轨迹可以近似为圆。这种方法的优点在于其能够有效简化运动轨迹的计算，适合复杂路径的近似。

2.\textbf{链式求解模型}：龙头的轨迹可以通过极坐标方程进行描述，龙身和龙尾则依赖于龙头的运动，通过位置的关系和速度的关联性，利用链式求解法确定各节板凳的位置。这一方法有效地将系统复杂性分解为多个较小的子问题，每节板凳的运动都取决于上一节板凳的状态。

3.\textbf{数值法}：为了找到最小的螺距，该问题通过逐步枚举不同的螺距值，并利用碰撞测试模型来判断是否满足调头空间的条件。这种方法相对简单有效，适用于逐步缩小搜索范围，找到最优解。

\subsection{模型与结果检验}
1.微元法的精度取决于时间步长的大小，较小的时间步长可以提供更精确的结果，但会增加计算量。我们通过实验调整时间步长，大大提高了结果的精度。

2.链式求解的过程会受前一节板凳位置和速度的影响，可能导致误差的累积。为减小误差，我们多次修正前后板凳的相对位置，采用了迭代优化技术。

3.碰撞检测模型通过检测相邻板凳之间的距离，判断它们是否发生碰撞。该模型依赖于几何距离阈值的设定，考虑了板凳的长度和形状。

4.复杂度分析：随着路径复杂性和板凳数量的增加，碰撞检测和回溯法的\textbf{时间复杂度}也会大幅度提升。可能需要对算法进行进一步的优化，例如引入快速碰撞检测算法或采用更智能的搜索策略。

5.模型结合了微元法、链式求解、碰撞检测、回溯法等多种数值计算技术，保证了对复杂动态系统的高效求解。

\section{模型的评价}
\subsection{模型优点}
1. 原创性高，本文中所给出模型均为自主创建；

2. 在问题求解中，对问题进行了合理地简化，避免了复杂因素的讨论。

3. \textbf{链式求解法}将问题分解为多个微小的步骤，逐节计算板凳的位置。通过结合速度关联，能够保证每个板凳的实际运动是连续且符合力学规律的。这种逐步推进的求解方式在描述复杂物体中各部分的相互影响时非常有效。

4.\textbf{碰撞测试模型}可以根据不同的几何形状和布局调整碰撞检测的算法，适应不同的舞龙队形态。通过调整阈值，也可以适应不同的物理环境，甚至是不同的物体类型。

\subsection{模型缺点和改进}
1.模型忽略了物体的形变、运动的连贯性等带来的影响，因此和实际生活存在差异。

2.由于\textbf{链式求解法}要求逐一计算每节板凳的位置，这种方法随着问题规模的扩大，计算的复杂度会显著提升。

3.\textbf{碰撞测试模型}主要针对板凳之间的相对位置和距离进行检测，无法直接反映板凳之间的其他物理性质（如相互作用的力、速度等），因此可能忽略了某些复杂的物理碰撞行为。

4.\textbf{回溯法}虽然能够倒推计算位置，但如果遇到复杂的约束条件或者初始条件设置不当，可能会陷入局部最优解，无法找到全局最优的螺距。

5.在问题四中，对于调头曲线的路径优化问题还有所不足，目前所建的模型很难求出在调头空间中的最优解。

6.在问题五中，介于对每一个把手所求的速度均为整秒时的速度，与2m/s的上限所比较的也是整秒时的速度，还未考量未在整秒时的速度与上限的比较。

可能的改进方法：

1.加入其他客观因素的影响，使其更好地拟合真实情况

2.选择更优化的积分算法，或者提高所取微元的精度，以求更准确的解。


\subsection{模型推广}
链式求解法和速度关联模型可以推广到其他类似的分段系统，如火车节车厢的运动、航行中的蛇形机器人或者具有多个关节的机械臂等场景。任何涉及到多个互相连接的物体并且需要保持速度和力传递连续性的系统，都可以受益于这种链式求解法。



\begin{thebibliography}{99}
    \bibitem{ref1} 刘崇军.等距螺旋的原理与计算[J].数学的实践与认识,2018,48(11):165-174.
    \bibitem{ref2} 马天艺,郭晓日,马君.Matlab图形求解方法在资产评估中的应用——兼论插值法与图像法的比较分析[J].中国资产评估,2023,(05):26-31.
\bibitem{ref3} 陈文苑.文化展演与仪式重构：徽州右龙板凳龙仪式实践的民族志考察——以“互动仪式”理论为视角[J].河池学院学报,2024,44(03):31-39.

\end{thebibliography}

\appendix

\section{附录: 支撑材料说明}
\begin{verbatim}
solution1.py 问题一解答源码

solution2.py 问题二解答源码

solution3.py 问题三解答源码

solution4.py 问题四解答源码

solution5.py 问题五解答源码

caculateangle.py 计算角度源码

result1.xlsx 问题一结果  

result2.xlsx 问题二结果  

result4.xlsx 问题四结果  

answer2.txt 问题二解答

answer3.txt 问题三解答

answer5.txt 问题五解答
\end{verbatim}

\appendix

\section{Appendix A: Detailed Derivations}
\lipsum[4]
solution1.py 问题一解答源码

solution2.py 问题二解答源码

solution3.py 问题三解答源码

solution4.py 问题四解答源码

solution5.py 问题五解答源码

caculateangle.py 计算角度源码

result1.xlsx 问题一结果  

result2.xlsx 问题二结果  

result4.xlsx 问题四结果  

answer2.txt 问题二解答

answer3.txt 问题三解答

answer5.txt 问题五解答
\section{Appendix B: Code Example}

\section{附录: solution1.py}
\begin{verbatim}
import numpy as np
import pandas as pd
import openpyxl

workbook = openpyxl.load_workbook('result1.xlsx')
worksheet1 = workbook['位置']
worksheet2 = workbook['速度']

# 定义常量
v_head = 1.0  # 龙头的速度 (m/s)
b = 0.55  # 螺距 (m)
theta_initial = 16 * 2 * np.pi  # 初始角度，对应第16圈
time_steps = 300  # 总时间 300 秒
dragon_sections = 223  # 总板凳数

# 每个板凳的长度（米），注意龙头的长度不同
lengths = [2.86] + [1.65] * 222


# 初始位置
theta = theta_initial
r = b * theta / (2 * np.pi)

# 记录各节位置
sections = np.zeros((dragon_sections+1, 2))  # 223节的x, y坐标初始化
sections[0] = [r * np.cos(theta), r * np.sin(theta)]  # 龙头初始位置
# 记录各节速度
sections_v = np.zeros(dragon_sections+1)
sections_v[0] = 1.0
# 每秒记录数据
for t in range(time_steps + 1):
    # 更新各节的位置
    cur_theta = theta
    for i in range(1, dragon_sections+1):
        dist = lengths[i - 1]
        prev_x, prev_y = sections[i - 1]
        prev_v = sections_v[i-1]
        prev_theta = cur_theta
        # 计算出当前节点位置
        min_theta = cur_theta
        max_theta = cur_theta + np.pi
        cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos((max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin((max_theta + min_theta) / 2)
        dx = prev_x - cur_x
        dy = prev_y - cur_y
        cur_dist = np.hypot(dx, dy)
        while abs(cur_dist-dist) > np.exp(-20):
            if cur_dist > dist:
                max_theta = (max_theta + min_theta) / 2
            else:
                min_theta = (max_theta + min_theta) / 2
            cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos((max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin((max_theta + min_theta) / 2)
            dx = prev_x - cur_x
            dy = prev_y - cur_y
            cur_dist = np.hypot(dx, dy)
        sections[i] = [cur_x, cur_y]
        cur_theta = (max_theta + min_theta) / 2

        # 计算速度
        k_tangl1 = (np.sin(prev_theta) + prev_theta * np.cos(prev_theta)) / (np.cos(prev_theta) - prev_theta * np.sin(prev_theta))
        k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
        k_sec = (cur_y - prev_y) / (cur_x - prev_x)
        tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
        tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
        angle_A = np.arctan(tangentA)
        angle_B = np.arctan(tangentB)
        cur_v = prev_v * np.cos(angle_A) / np.cos(angle_B)
        sections_v[i] = cur_v

    # 存入表格
    for i in range(dragon_sections+1):
        worksheet1.cell(row=2*i+2, column=t+2, value=round(sections[i][0], 6))
        worksheet1.cell(row=2 * i + 3, column=t + 2, value=round(sections[i][1], 6))
        worksheet2.cell(row=i+2, column=t + 2, value=round(sections_v[i], 6))

    # 将时间微分至0.0001提高精度
    for i in range(10000):
        dtheta = v_head / 10000 / r
        theta -= dtheta
        r = b * theta / (2 * np.pi)
    sections[0] = [r * np.cos(theta), r * np.sin(theta)]
    print("时间 %d 完成" % t)

workbook.save('result1.xlsx')

\end{verbatim}

\section{附录: solution2.py}
\begin{verbatim}
import numpy as np
import pandas as pd
import openpyxl

workbook = openpyxl.load_workbook('result2.xlsx')
worksheet = workbook['Sheet1']


# 定义常量
v_head = 1.0  # 龙头的速度 (m/s)
b = 0.55  # 螺距 (m)
theta_initial = 16 * 2 * np.pi  # 初始角度，对应第16圈
time_steps = 50000  # 总时间 500 秒，每秒分成100小段以提高精度
dragon_sections = 223  # 总板凳数
# 每个板凳的长度（米），注意龙头的长度不同
lengths = [2.86] + [1.65] * 222


# 初始位置
theta = theta_initial
r = b * theta / (2 * np.pi)

# 记录各节位置
sections = np.zeros((dragon_sections+1, 2))  # 223节的x, y坐标初始化
sections[0] = [r * np.cos(theta), r * np.sin(theta)]  # 龙头初始位置
# 记录各节速度
sections_v = np.zeros(dragon_sections+1)
sections_v[0] = 1.0
# 每秒记录数据
for t in range(time_steps+1):
    # 更新各节的位置和速度
    cur_theta = theta
    for i in range(1, dragon_sections+1):
        # 读取本段距离和上一个把手位置
        dist = lengths[i - 1]
        prev_x, prev_y = sections[i - 1]
        prev_v = sections_v[i-1]
        prev_theta = cur_theta
        # 计算出当前节点位置
        min_theta = cur_theta
        max_theta = cur_theta + np.pi
        cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos((max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin((max_theta + min_theta) / 2)
        dx = prev_x - cur_x
        dy = prev_y - cur_y
        cur_dist = np.hypot(dx, dy)
        while abs(cur_dist-dist) > np.power(10.0, -10):
            if cur_dist > dist:
                max_theta = (max_theta + min_theta) / 2
            else:
                min_theta = (max_theta + min_theta) / 2
            cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos((max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin((max_theta + min_theta) / 2)
            dx = prev_x - cur_x
            dy = prev_y - cur_y
            cur_dist = np.hypot(dx, dy)
        sections[i] = [cur_x, cur_y]
        cur_theta = (max_theta + min_theta) / 2

        # 计算速度
        k_tangl1 = (np.sin(prev_theta) + prev_theta * np.cos(prev_theta)) / (np.cos(prev_theta) - prev_theta * np.sin(prev_theta))
        k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
        k_sec = (cur_y - prev_y) / (cur_x - prev_x)
        tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
        tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
        angle_A = np.arctan(tangentA)
        angle_B = np.arctan(tangentB)
        cur_v = prev_v * np.cos(angle_A) / np.cos(angle_B)
        sections_v[i] = cur_v

    # 判断是否发生碰撞
    x1, y1 = sections[0]
    x2, y2 = sections[1]
    k1 = (y1 - y2) / (x1 - x2)
    b1 = y1 - k1 * x1
    k2 = -1 / k1
    b2 = y1 - k2 * x1
    b3 = b1 + 0.15 * np.sqrt(1+k1*k1)
    b4 = b1 - 0.15 * np.sqrt(1+k1*k1)
    b5 = b2 + 0.275 * np.sqrt(1+k2*k2)
    b6 = b2 - 0.275 * np.sqrt(1+k2*k2)
    c_x = 0.0
    c_y = 0.0
    if x1 > x2 and y1 > y2:
        c_x = (b5 - b3) / (k1 - k2)
        c_y = k1 * c_x + b3
    elif x1 > x2 and y1 < y2:
        c_x = (b6 - b3) / (k1 - k2)
        c_y = k1 * c_x + b3
    elif x1 < x2 and y1 > y2:
        c_x = (b5 - b4) / (k1 - k2)
        c_y = k1 * c_x + b4
    elif x1 < x2 and y1 < y2:
        c_x = (b6 - b4) / (k1 - k2)
        c_y = k1 * c_x + b4
    elif x1 == x2 and y1 > y2:
        c_x = x1 - 0.15
        c_y = y1 + 0.275
    elif x1 == x2 and y1 < y2:
        c_x = x1 + 0.15
        c_y = y1 - 0.275
    elif x1 > x2 and y1 == y2:
        c_x = x1 + 0.275
        c_y = y1 + 0.15
    elif x1 < x2 and y1 == y2:
        c_x = x1 - 0.275
        c_y = y1 - 0.15
    for i in range(2, dragon_sections + 1, 1):
        xn, yn = sections[i-1]
        xn1, yn1 = sections[i]
        xm = (xn + xn1) / 2
        ym = (yn + yn1) / 2
        kn = (yn1 - yn) / (xn1 - xn)
        bn = yn - kn * xn
        kn1 = -1 / kn
        bn1 = ym - kn1 * xm
        d1 = abs(kn*c_x+bn-c_y) / np.sqrt(1+kn*kn)
        d2 = abs(kn1*c_x+bn1-c_y) / np.sqrt(1+kn1*kn1)
        if d1 <= 0.15 and d2 <= 1.1:
            for j in range(dragon_sections+1):
                worksheet.cell(row=j+2, column=2,value=round(sections[j][0],6))
                worksheet.cell(row=j + 2, column=3, value=round(sections[j][1],6))
                worksheet.cell(row=j + 2, column=4, value=round(sections_v[j],6))
            print("在时刻 %f秒 时发生碰撞" % (t/100))
            workbook.save('result2.xlsx')
            exit()

    # 将时间微分至0.00001s提高精度
    for i in range(1000):
        dtheta = v_head / 100000 / r
        theta -= dtheta
        r = b * theta / (2 * np.pi)
    sections[0] = [r * np.cos(theta), r * np.sin(theta)]

\end{verbatim}

\section{附录: solution3.py}
\begin{verbatim}
import numpy as np

# 定义最小螺距为0.1m，可以很容易得出这个螺距是不行的:
# 由于螺距小于板凳宽，一旦板凳龙绕了一圈后必定相碰，在半径4.5m处就会绕一圈相碰。
b = 0.1

dragon_sections = 223  # 总板凳数
lengths = [2.86] + [1.65] * 222  # 每两个把手的长度（米），注意龙头的长度不同

# 由第二问可得到0.55m螺距下碰撞位置在半径4.5m的圆内，0.55满足要求，现求0.1-0.55范围内恰好满足要求的点，增量设置为0.01m
for db in range(46):
    b += db * 0.01
    # 找到该螺线与半径为4.5m的圆的交点
    inter_theta = 9 * np.pi / b
    sites = np.zeros((dragon_sections + 1, 2))  # 记录每个节点的位置
    head_x = 4.5 * np.cos(inter_theta)
    head_y = 4.5 * np.sin(inter_theta)
    sites[0] = [head_x, head_y]
    cur_theta = inter_theta
    # 计算各把手位置
    for i in range(1, dragon_sections + 1):
        # 读取本段距离和上一个把手位置
        dist = lengths[i - 1]
        prev_x, prev_y = sites[i - 1]
        prev_theta = cur_theta
        # 计算出当前节点位置
        min_theta = cur_theta
        max_theta = cur_theta + np.pi
        cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos((max_theta + min_theta) / 2), b * (
                max_theta + min_theta) / 2 / (2 * np.pi) * np.sin((max_theta + min_theta) / 2)
        dx = prev_x - cur_x
        dy = prev_y - cur_y
        cur_dist = np.hypot(dx, dy)
        while abs(cur_dist - dist) > np.power(10.0, -10):
            if cur_dist > dist:
                max_theta = (max_theta + min_theta) / 2
            else:
                min_theta = (max_theta + min_theta) / 2
            cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                (max_theta + min_theta) / 2)
            dx = prev_x - cur_x
            dy = prev_y - cur_y
            cur_dist = np.hypot(dx, dy)
        sites[i] = [cur_x, cur_y]
        cur_theta = (max_theta + min_theta) / 2

    # 判断是否发生碰撞
    is_collision = 0
    x1, y1 = sites[0]
    x2, y2 = sites[1]
    k1 = (y1 - y2) / (x1 - x2)
    b1 = y1 - k1 * x1
    k2 = -1 / k1
    b2 = y1 - k2 * x1
    b3 = b1 + 0.15 * np.sqrt(1 + k1 * k1)
    b4 = b1 - 0.15 * np.sqrt(1 + k1 * k1)
    b5 = b2 + 0.275 * np.sqrt(1 + k2 * k2)
    b6 = b2 - 0.275 * np.sqrt(1 + k2 * k2)
    c_x = 0.0
    c_y = 0.0
    if x1 > x2 and y1 > y2:
        c_x = (b5 - b3) / (k1 - k2)
        c_y = k1 * c_x + b3
    elif x1 > x2 and y1 < y2:
        c_x = (b6 - b3) / (k1 - k2)
        c_y = k1 * c_x + b3
    elif x1 < x2 and y1 > y2:
        c_x = (b5 - b4) / (k1 - k2)
        c_y = k1 * c_x + b4
    elif x1 < x2 and y1 < y2:
        c_x = (b6 - b4) / (k1 - k2)
        c_y = k1 * c_x + b4
    for i in range(2, dragon_sections + 1, 1):
        xn, yn = sites[i - 1]
        xn1, yn1 = sites[i]
        xm = (xn + xn1) / 2
        ym = (yn + yn1) / 2
        kn = (yn1 - yn) / (xn1 - xn)
        bn = yn - kn * xn
        kn1 = -1 / kn
        bn1 = ym - kn1 * xm
        d1 = abs(kn * c_x + bn - c_y) / np.sqrt(1 + kn * kn)
        d2 = abs(kn1 * c_x + bn1 - c_y) / np.sqrt(1 + kn1 * kn1)
        if d1 <= 0.15 and d2 <= 1.1:
            is_collision = 1
            break
    if is_collision == 0:
        print("最小螺距为%fm" % b)
        exit()

\end{verbatim}

\section{附录: solution4.py}
\begin{verbatim}
import numpy as np
import openpyxl
from caculate_angle import angle_head
from caculate_angle import angle_body
workbook = openpyxl.load_workbook('result4.xlsx')
worksheet1 = workbook['位置']
worksheet2 = workbook['速度']
b = 1.7
dragon_sections = 223
time_steps = 100
lengths = [2.86] + [1.65] * 222
theta_initial_in = 90 * np.pi / 17
theta_initial_out = 73 * np.pi / 17
azi_o1_in, azi_o1_out, azi_o2_in, azi_o2_out = (0, 0, 0, 0)

# 求当前切入切出位置下的S形圆弧位置
theta_in = theta_initial_in
theta_out = theta_initial_out
# 入切点和出切点的坐标
in_x = b / 2 / np.pi * theta_in * np.cos(theta_in)
in_y = b / 2 / np.pi * theta_in * np.sin(theta_in)
out_x = b / 2 / np.pi * theta_out * np.cos(theta_out) + b / 2 * np.cos(theta_out)
out_y = b / 2 / np.pi * theta_out * np.sin(theta_out) + b / 2 * np.sin(theta_out)
# 出-入切点处切线
k_tangl_in = (np.sin(theta_in) + theta_in * np.cos(theta_in)) / (np.cos(theta_in) - theta_in * np.sin(theta_in))
b_tangl_in = in_y - k_tangl_in * in_x
k_tangl_out = (np.sin(theta_out) + (theta_out + np.pi) * np.cos(theta_out)) / (np.cos(theta_out) - (theta_out + np.pi) * np.sin(theta_out))
b_tangl_out = out_y - k_tangl_out * out_x
# 出-入切点处切线垂线
k_perpl_in = -1 / k_tangl_in
b_perpl_in = in_y - k_perpl_in * in_x
k_perpl_out = -1 / k_tangl_out
b_perpl_out = out_y - k_perpl_out * out_x

# 找到对应盘出轨迹
for d_r1 in range(4010000):
    r1 = d_r1 / 1000000
    r2 = r1 / 2
    # 求两圆心的坐标
    o1_x, o1_y, o2_x, o2_y = (0.0, 0.0, 0.0, 0.0)
    dx_o1 = r1 / np.sqrt(1 + np.power(k_perpl_in, 2))
    dy_o1 = abs(k_perpl_in * r1) / np.sqrt(1 + np.power(k_perpl_in, 2))
    dx_o2 = r2 / np.sqrt(1 + np.power(k_perpl_out, 2))
    dy_o2 = abs(k_perpl_out * r2) / np.sqrt(1 + np.power(k_perpl_out, 2))
    if k_tangl_in > 0 and b_tangl_in > 0:
        o1_x = in_x + dx_o1
        o1_y = in_y - dy_o1
    elif k_tangl_in > 0 and b_tangl_in < 0:
        o1_x = in_x - dx_o1
        o1_y = in_y + dy_o1
    elif k_tangl_in < 0 and b_tangl_in > 0:
        o1_x = in_x - dx_o1
        o1_y = in_y - dy_o1
    elif k_tangl_in < 0 and b_tangl_in < 0:
        o1_x = in_x + dx_o1
        o1_y = in_y + dy_o1
    if k_tangl_out > 0 and b_tangl_out > 0:
        o2_x = out_x + dx_o2
        o2_y = out_y - dy_o2
    elif k_tangl_out > 0 and b_tangl_out < 0:
        o2_x = out_x - dx_o2
        o2_y = out_y + dy_o2
    elif k_tangl_out < 0 and b_tangl_out > 0:
        o2_x = out_x - dx_o2
        o2_y = out_y - dy_o2
    elif k_tangl_out < 0 and b_tangl_out < 0:
        o2_x = out_x + dx_o2
        o2_y = out_y + dy_o2

    # 验证两圆心距离是否满足要求
    d_o1_o2 = np.hypot(o1_x-o2_x, o1_y-o2_y)
    if abs(d_o1_o2 - r1 - r2) <= np.power(10.0, -6):
        # 求两圆切点
        dot_tang_x = (o1_x + 2 * o2_x) / 3
        dot_tang_y = (o1_y + 2 * o2_y) / 3
        print("大圆半径: %lfm\n"
              "小圆半径: %lfm\n"
              "入切点位置: %lf x=%lf y=%lf\n"
              "大圆圆心: %lf %lf\n"
              "两圆切点: %lf %lf\n"
              "小圆圆心: %lf %lf\n"
              "出切点位置: %lf x=%lf y=%lf"
              % (r1,
                 r2,
                 theta_in, in_x, in_y,
                 o1_x, o1_y,
                 dot_tang_x, dot_tang_y,
                 o2_x, o2_y,
                 theta_out, out_x, out_y)
              )
        k_o1_in = (o1_y-in_y) / (o1_x-in_x)
        k_o1_tang = (dot_tang_y-o1_y) / (dot_tang_x-o1_x)
        k_o2_tang = (o2_y-dot_tang_y) / (o2_x-dot_tang_x)
        k_o2_out = (out_y-o2_y) / (out_x-o2_x)
        central_angle_o1 = np.arctan(k_o1_in) + np.pi - np.arctan(k_o1_tang)  # 大圆中的圆心角
        central_angle_o2 = np.arctan(k_o2_out) + np.pi - np.arctan(k_o2_tang)  # 小圆中的圆心角
        azi_o1_in = np.pi + np.arctan(k_o1_in)  # 大圆中进入半径对应方位角
        azi_o1_out = np.arctan(k_o1_tang)  # 大圆中离开半径对应方位角
        azi_o2_in = 2 * np.pi - central_angle_o2 + np.arctan(k_o2_out)  # 小圆中进入半径对应方位角
        azi_o2_out = np.arctan(k_o2_out)  # 小圆中离开半径对应方位角
        break


# 求位置速度相关信息
site_and_v = np.zeros((dragon_sections+1, 3))
site_and_v[0] = (in_x, in_y, 1.0)
head_theta = theta_in
r = b * head_theta / 2 / np.pi
# 针对-100~0的数据计算
for t in range(time_steps + 1):
    # 更新各节的位置
    cur_theta = head_theta
    for i in range(1, dragon_sections+1):
        dist = lengths[i - 1]
        prev_x, prev_y, prev_v= site_and_v[i - 1]
        prev_theta = cur_theta
        # 计算出当前节点位置
        min_theta = cur_theta
        max_theta = cur_theta + np.pi
        cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos((max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin((max_theta + min_theta) / 2)
        dx = prev_x - cur_x
        dy = prev_y - cur_y
        cur_dist = np.hypot(dx, dy)
        while abs(cur_dist-dist) > np.power(10.0, -10):
            if cur_dist > dist:
                max_theta = (max_theta + min_theta) / 2
            else:
                min_theta = (max_theta + min_theta) / 2
            cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos((max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin((max_theta + min_theta) / 2)
            dx = prev_x - cur_x
            dy = prev_y - cur_y
            cur_dist = np.hypot(dx, dy)
        site_and_v[i][0] = cur_x
        site_and_v[i][1] = cur_y
        cur_theta = (max_theta + min_theta) / 2

        # 计算速度
        k_tangl1 = (np.sin(prev_theta) + prev_theta * np.cos(prev_theta)) / (np.cos(prev_theta) - prev_theta * np.sin(prev_theta))
        k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
        k_sec = (cur_y - prev_y) / (cur_x - prev_x)
        tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
        tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
        angle_A = np.arctan(tangentA)
        angle_B = np.arctan(tangentB)
        cur_v = prev_v * np.cos(angle_A) / np.cos(angle_B)
        site_and_v[i][2] = cur_v

    # 存入表格
    for i in range(dragon_sections+1):
        worksheet1.cell(row=2*i+2, column=102-t, value=np.round(site_and_v[i][0], 6))
        worksheet1.cell(row=2 * i + 3, column=102 - t, value=np.round(site_and_v[i][1], 6))
        worksheet2.cell(row=i+2, column=102 - t, value=np.round(site_and_v[i][2], 6))

    # 将时间微分至0.01提高精度
    for i in range(10000):
        dtheta = 1.0 / 10000 / r
        head_theta += dtheta
        r = b * head_theta / (2 * np.pi)
    site_and_v[0] = [r * np.cos(head_theta), r * np.sin(head_theta), 1.0]

# 针对0~100的数据计算
site_and_v = np.zeros((dragon_sections+1, 3))
site_and_v[0] = (in_x, in_y, 1.0)
head_theta = theta_initial_out  # 龙头到达盘出螺线对应极角
r = b * (head_theta + np.pi) / 2 / np.pi
time_used = 0  # 经过时间
cur_azi_theta = azi_o1_in  # 当前方位角
all_t_o1 = central_angle_o1 * r1  # 在o1中的运动时间
all_t_o2 = central_angle_o2 * r2  # 在o2中的运动时间
d_azi1_o1 = 2 * np.arcsin(1.43 / r1)  # 第一个板凳在o1所需夹角
d_azi2_o1 = 2 * np.arcsin(0.825 / r1)  # 后续板凳在o1所需夹角
d_azi1_o2 = 2 * np.arcsin(1.43 / r2)  # 第一个板凳在o2所需夹角
d_azi2_o2 = 2 * np.arcsin(0.825 / r2)  # 后续板凳在o2所需夹角
theta_out_head = angle_head()  # 第一个板凳在出螺旋线所需龙头前把手极角
theta_out_body = angle_body()  # 后续板凳在出螺旋线所需前一节点极角
print("在o1中时间: %fs\n在o2中时间: %fs" % (all_t_o1, all_t_o2))
for t in range(1, time_steps+1):
    # 龙头进入第一段圆弧
    if t <= all_t_o1:
        print("龙头进入第一段圆弧 %d" % t)
        cur_azi_theta = azi_o1_in - t / r1
        site_and_v[0][0] = o1_x + r1 * np.cos(cur_azi_theta)
        site_and_v[0][1] = o1_y + r1 * np.sin(cur_azi_theta)
        site_and_v[0][2] = 1
        is_intrace = 1
        dist = lengths[0]
        pre_dazi = azi_o1_in - cur_azi_theta
        # 第一节点在第一段圆弧，第二节点在第一段圆弧 v1
        if pre_dazi > d_azi1_o1:
            is_intrace = 1
            cur_azi_theta += d_azi1_o1
            site_and_v[1][0] = o1_x + r1 * np.cos(cur_azi_theta)
            site_and_v[1][1] = o1_y + r1 * np.sin(cur_azi_theta)
            site_and_v[1][2] = 1
        # 第一节点在第一段圆弧，第二节点在盘入螺旋线 v2
        else:
            is_intrace = 0
            prev_azi_theta = cur_azi_theta
            prev_x, prev_y, prev_v = site_and_v[0]
            # 计算出当前节点位置
            min_theta = theta_initial_in
            max_theta = theta_initial_in + np.pi
            cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos((max_theta + min_theta) / 2), b * (
                    max_theta + min_theta) / 2 / (2 * np.pi) * np.sin((max_theta + min_theta) / 2)
            dx = prev_x - cur_x
            dy = prev_y - cur_y
            cur_dist = np.hypot(dx, dy)
            while abs(cur_dist - dist) > np.power(10.0, -10):
                if cur_dist > dist:
                    max_theta = (max_theta + min_theta) / 2
                else:
                    min_theta = (max_theta + min_theta) / 2
                cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                    (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                    (max_theta + min_theta) / 2)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
            site_and_v[1][0] = cur_x
            site_and_v[1][1] = cur_y
            cur_theta = (max_theta + min_theta) / 2

            # 计算速度
            k_tangl1 = -1 / np.tan(prev_azi_theta)
            k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (
                        np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
            k_sec = (cur_y - prev_y) / (cur_x - prev_x)
            tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
            tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
            angle_A = np.arctan(tangentA)
            angle_B = np.arctan(tangentB)
            cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
            site_and_v[1][2] = cur_v

        for i in range(2, dragon_sections+1):
            prev_x, prev_y, prev_v = site_and_v[i-1]
            dist = lengths[i-1]
            # 上一节点在第一段圆弧中
            if is_intrace:
                pre_dazi = azi_o1_in - cur_azi_theta
                # 上一节点在第一段圆弧，当前节点在第一段圆弧 v3
                if pre_dazi > d_azi2_o1:
                    is_intrace = 1
                    cur_azi_theta += d_azi2_o1
                    site_and_v[i][0] = o1_x + r1 * np.cos(cur_azi_theta)
                    site_and_v[i][1] = o1_y + r1 * np.sin(cur_azi_theta)
                    site_and_v[i][2] = site_and_v[i-1][2]
                # 上一节点在第一段圆弧，当前节点在盘入螺旋线 v4
                else:
                    is_intrace = 0
                    prev_azi_theta = cur_azi_theta
                    prev_x, prev_y, prev_v = site_and_v[i-1]
                    # 计算出当前节点位置
                    min_theta = theta_initial_in
                    max_theta = theta_initial_in + np.pi
                    cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                        (max_theta + min_theta) / 2), b * (
                                           max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                        (max_theta + min_theta) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                    while abs(cur_dist - dist) > np.power(10.0, -10):
                        if cur_dist > dist:
                            max_theta = (max_theta + min_theta) / 2
                        else:
                            min_theta = (max_theta + min_theta) / 2
                        cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                            (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                            (max_theta + min_theta) / 2)
                        dx = prev_x - cur_x
                        dy = prev_y - cur_y
                        cur_dist = np.hypot(dx, dy)
                    site_and_v[i][0] = cur_x
                    site_and_v[i][1] = cur_y
                    cur_theta = (max_theta + min_theta) / 2

                    # 计算速度
                    k_tangl1 = -1 / np.tan(prev_azi_theta)
                    k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (
                            np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
                    k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                    tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                    tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                    angle_A = np.arctan(tangentA)
                    angle_B = np.arctan(tangentB)
                    cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                    site_and_v[i][2] = cur_v

            # 上一节点在盘入螺旋线，下一节点在盘入螺旋线 v5
            else:
                prev_theta = cur_theta
                prev_x, prev_y, prev_v = site_and_v[i - 1]
                # 计算出当前节点位置
                min_theta = cur_theta
                max_theta = min_theta + np.pi
                cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                    (max_theta + min_theta) / 2), b * (
                                       max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                    (max_theta + min_theta) / 2)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
                while abs(cur_dist - dist) > np.power(10.0, -10):
                    if cur_dist > dist:
                        max_theta = (max_theta + min_theta) / 2
                    else:
                        min_theta = (max_theta + min_theta) / 2
                    cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                        (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                        (max_theta + min_theta) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                site_and_v[i][0] = cur_x
                site_and_v[i][1] = cur_y
                cur_theta = (max_theta + min_theta) / 2

                # 计算速度
                k_tangl1 = (np.sin(prev_theta) + prev_theta * np.cos(prev_theta)) / (
                            np.cos(prev_theta) - prev_theta * np.sin(prev_theta))
                k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (
                            np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
                k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                angle_A = np.arctan(tangentA)
                angle_B = np.arctan(tangentB)
                cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                site_and_v[i][2] = cur_v
        # 将数据填入表格
        for i in range(dragon_sections+1):
            worksheet1.cell(row=2*i+2, column=102 + t, value=np.round(site_and_v[i][0], 6))
            worksheet1.cell(row=2*i+3, column=102 + t, value=np.round(site_and_v[i][1], 6))
            worksheet2.cell(row=i+2, column=102 + t, value=np.round(site_and_v[i][2], 6))

    # 龙头进入第二段圆弧
    elif t <= all_t_o1 + all_t_o2:
        print("龙头进入第二段圆弧 %d" % t)
        cur_azi_theta = azi_o2_in + (t-all_t_o1) / r2
        site_and_v[0][0] = o2_x + r2 * np.cos(cur_azi_theta)
        site_and_v[0][1] = o2_y + r2 * np.sin(cur_azi_theta)
        site_and_v[0][2] = 1
        is_intrace2 = 1
        is_intrace1 = 0
        dist = lengths[0]
        pre_dazi2 = cur_azi_theta - azi_o2_in
        # 第一节点在第二段圆弧，第二节点在第二段圆弧 v6
        if pre_dazi2 > d_azi1_o2:
            is_intrace2 = 1
            is_intrace1 = 0
            cur_azi_theta -= d_azi1_o2
            site_and_v[1][0] = o2_x + r2 * np.cos(cur_azi_theta)
            site_and_v[1][1] = o2_y + r2 * np.sin(cur_azi_theta)
            site_and_v[1][2] = 1
        # 第一节点在第二段圆弧，第二节点在第一段圆弧 v7
        else:
            is_intrace2 = 0
            is_intrace1 = 1
            prev_azi_theta = cur_azi_theta
            prev_x, prev_y, prev_v = site_and_v[0]
            # 计算出当前节点位置
            min_azi = azi_o1_out
            max_azi = azi_o1_in
            cur_x, cur_y = o1_x + r1 * np.cos((min_azi+max_azi)/2), o1_y + r1 * np.sin((min_azi+max_azi)/2)
            dx = prev_x - cur_x
            dy = prev_y - cur_y
            cur_dist = np.hypot(dx, dy)
            while abs(cur_dist - dist) > np.power(10.0, -10):
                if cur_dist > dist:
                    max_azi = (max_azi + min_azi) / 2
                else:
                    min_azi = (max_azi + min_azi) / 2
                cur_x, cur_y = o1_x + r1 * np.cos((min_azi+max_azi)/2), o1_y + r1 * np.sin((min_azi+max_azi)/2)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
            site_and_v[1][0] = cur_x
            site_and_v[1][1] = cur_y
            cur_azi_theta = (max_azi + min_azi) / 2

            # 计算速度
            k_tangl1 = -1 / np.tan(cur_azi_theta)
            k_tangl2 = -1 / np.tan(prev_azi_theta)
            k_sec = (cur_y - prev_y) / (cur_x - prev_x)
            tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
            tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
            angle_A = np.arctan(tangentA)
            angle_B = np.arctan(tangentB)
            cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
            site_and_v[1][2] = cur_v

        for i in range(2, dragon_sections+1):
            dist = lengths[i-1]
            prev_x, prev_y, prev_v = site_and_v[i-1]
            # 上一节点在第二段圆弧
            if is_intrace2:
                pre_dazi2 = cur_azi_theta - azi_o2_in
                # 上一节点在第二段圆弧，当前节点在第二段圆弧 v8
                if pre_dazi2 > d_azi2_o2:
                    is_intrace2 = 1
                    is_intrace1 = 0
                    cur_azi_theta -= d_azi2_o2
                    site_and_v[i][0] = o2_x + r2 * np.cos(cur_azi_theta)
                    site_and_v[i][1] = o2_y + r2 * np.sin(cur_azi_theta)
                    site_and_v[i][2] = site_and_v[i-1][2]
                # 上一节点在第二段圆弧，当前节点在第一段圆弧 v9
                else:
                    is_intrace2 = 0
                    is_intrace1 = 1
                    prev_azi_theta = cur_azi_theta
                    prev_x, prev_y, prev_v = site_and_v[i-1]
                    # 计算出当前节点位置
                    min_azi = azi_o1_out
                    max_azi = azi_o1_in
                    cur_x, cur_y = o1_x + r1 * np.cos((min_azi + max_azi) / 2), o1_y + r1 * np.sin(
                        (min_azi + max_azi) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                    while abs(cur_dist - dist) > np.power(10.0, -10):
                        if cur_dist > dist:
                            max_azi = (max_azi + min_azi) / 2
                        else:
                            min_azi = (max_azi + min_azi) / 2
                        cur_x, cur_y = o1_x + r1 * np.cos((min_azi + max_azi) / 2), o1_y + r1 * np.sin(
                            (min_azi + max_azi) / 2)
                        dx = prev_x - cur_x
                        dy = prev_y - cur_y
                        cur_dist = np.hypot(dx, dy)
                    site_and_v[i][0] = cur_x
                    site_and_v[i][1] = cur_y
                    cur_azi_theta = (max_azi + min_azi) / 2

                    # 计算速度
                    k_tangl1 = -1 / np.tan(prev_azi_theta)
                    k_tangl2 = -1 / np.tan(cur_azi_theta)
                    k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                    tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                    tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                    angle_A = np.arctan(tangentA)
                    angle_B = np.arctan(tangentB)
                    cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                    site_and_v[i][2] = cur_v

            # 上一节点在第一段圆弧
            elif is_intrace1:
                prev_azi_theta = cur_azi_theta
                pre_dazi1 = azi_o1_in - cur_azi_theta
                # 上一节点在第一段圆弧，当前节点在第一段圆弧 v10
                if pre_dazi1 > d_azi2_o1:
                    is_intrace1 = 1
                    is_intrace2 = 0
                    cur_azi_theta += d_azi2_o1
                    site_and_v[i][0] = o1_x + r1 * np.cos(cur_azi_theta)
                    site_and_v[i][1] = o1_y + r1 * np.sin(cur_azi_theta)
                    site_and_v[i][2] = site_and_v[i-1][2]
                # 上一节点在第一段圆弧，当前节点在盘入螺旋线 v11
                else:
                    is_intrace1 = 0
                    is_intrace2 = 0
                    prev_azi_theta = cur_azi_theta
                    prev_x, prev_y, prev_v = site_and_v[i - 1]
                    # 计算出当前节点位置
                    min_theta = theta_initial_in
                    max_theta = theta_initial_in + np.pi
                    cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                        (max_theta + min_theta) / 2), b * (
                                           max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                        (max_theta + min_theta) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                    while abs(cur_dist - dist) > np.power(10.0, -10):
                        if cur_dist > dist:
                            max_theta = (max_theta + min_theta) / 2
                        else:
                            min_theta = (max_theta + min_theta) / 2
                        cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                            (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                            (max_theta + min_theta) / 2)
                        dx = prev_x - cur_x
                        dy = prev_y - cur_y
                        cur_dist = np.hypot(dx, dy)
                    site_and_v[i][0] = cur_x
                    site_and_v[i][1] = cur_y
                    cur_theta = (max_theta + min_theta) / 2

                    # 计算速度
                    k_tangl1 = -1 / np.tan(prev_azi_theta)
                    k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (
                            np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
                    k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                    tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                    tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                    angle_A = np.arctan(tangentA)
                    angle_B = np.arctan(tangentB)
                    cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                    site_and_v[i][2] = cur_v

            # 上一节点在盘入螺旋线，当前节点在盘入螺旋线 v12
            else:
                is_intrace1 = 0
                is_intrace2 = 0
                prev_theta = cur_theta
                prev_x, prev_y, prev_v = site_and_v[i - 1]
                # 计算出当前节点位置
                min_theta = cur_theta
                max_theta = min_theta + np.pi
                cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                    (max_theta + min_theta) / 2), b * (
                                       max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                    (max_theta + min_theta) / 2)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
                while abs(cur_dist - dist) > np.power(10.0, -10):
                    if cur_dist > dist:
                        max_theta = (max_theta + min_theta) / 2
                    else:
                        min_theta = (max_theta + min_theta) / 2
                    cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                        (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                        (max_theta + min_theta) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                site_and_v[i][0] = cur_x
                site_and_v[i][1] = cur_y
                cur_theta = (max_theta + min_theta) / 2

                # 计算速度
                k_tangl1 = (np.sin(prev_theta) + prev_theta * np.cos(prev_theta)) / (
                        np.cos(prev_theta) - prev_theta * np.sin(prev_theta))
                k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (
                        np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
                k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                angle_A = np.arctan(tangentA)
                angle_B = np.arctan(tangentB)
                cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                site_and_v[i][2] = cur_v

            for i in range(dragon_sections+1):
                worksheet1.cell(row=2*i+2, column=102 + t, value=np.round(site_and_v[i][0], 6))
                worksheet1.cell(row=2*i+3, column=102 + t, value=np.round(site_and_v[i][1], 6))
                worksheet2.cell(row=i+2, column=102 + t, value=np.round(site_and_v[i][2], 6))

    # 龙头进入盘出螺旋线
    else:
        print("龙头进入盘出螺旋线 %d" % t)
        for i in range(100000):
            dtheta = 1 / 100000 * (t - all_t_o1 - all_t_o2) / r
            head_theta += dtheta
            r = b * (head_theta + np.pi) / 2 / np.pi
        time_used = t
        break
for t in range(time_used, time_steps+1):
    print("龙头进入盘出螺旋线 %d" % t)
    site_and_v[0][0] = r * np.cos(head_theta)
    site_and_v[0][1] = r * np.sin(head_theta)
    site_and_v[0][2] = 1
    dist = lengths[0]
    is_intrace3 = 1
    is_intrace2 = 0
    is_intrace1 = 0
    prev_x, prev_y, prev_v = site_and_v[0]
    theta_out = head_theta
    # 第一节点在盘出螺旋线，第二节点在盘出螺旋线 v13
    if theta_out > theta_out_head:
        print("第一节点在盘出螺旋线，第二节点在盘出螺旋线 %d" % t)
        prev_theta = head_theta
        is_intrace3 = 1
        is_intrace2 = 0
        is_intrace1 = 0
        max_theta = head_theta
        min_theta = max_theta - np.pi
        cur_x, cur_y = b * ((max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.cos((max_theta + min_theta) / 2), b * (
                    (max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.sin((max_theta + min_theta) / 2)
        dx = prev_x - cur_x
        dy = prev_y - cur_y
        cur_dist = np.hypot(dx, dy)
        while abs(cur_dist - dist) > np.power(10.0, -10):
            if cur_dist > dist:
                min_theta = (max_theta + min_theta) / 2
            else:
                max_theta = (max_theta + min_theta) / 2
            cur_x, cur_y = b * ((max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.cos((max_theta + min_theta) / 2), b * (
                    (max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.sin((max_theta + min_theta) / 2)
            dx = prev_x - cur_x
            dy = prev_y - cur_y
            cur_dist = np.hypot(dx, dy)
        site_and_v[1][0] = cur_x
        site_and_v[1][1] = cur_y
        cur_theta = (min_theta + max_theta) / 2

        # 计算速度
        k_tangl1 = (np.sin(prev_theta) + (prev_theta + np.pi) * np.cos(prev_theta)) / (
                    np.cos(prev_theta) - (prev_theta+np.pi) * np.sin(prev_theta))
        k_tangl2 = (np.sin(cur_theta) + (cur_theta+np.pi) * np.cos(cur_theta)) / (
                    np.cos(cur_theta) - (cur_theta+np.pi) * np.sin(cur_theta))
        k_sec = (cur_y - prev_y) / (cur_x - prev_x)
        tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
        tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
        angle_A = np.arctan(tangentA)
        angle_B = np.arctan(tangentB)
        cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
        site_and_v[1][2] = cur_v
    # 第一节点在盘出螺旋线，第二节点在第二段圆弧 v14
    else:
        print("第一节点在盘出螺旋线，第二节点在第二段圆弧 %d" % t)
        prev_theta = head_theta
        prev_x,prev_y,prev_v = site_and_v[0]
        is_intrace3 = 0
        is_intrace2 = 1
        is_intrace1 = 0
        min_azi = azi_o2_in
        max_azi = 2 * np.pi + azi_o2_out
        cur_azi_theta = (min_azi + max_azi) / 2
        cur_x, cur_y = o2_x + r2 * np.cos(cur_azi_theta), o2_y + r2 * np.sin(
            cur_azi_theta)
        dx = prev_x - cur_x
        dy = prev_y - cur_y
        cur_dist = np.hypot(dx, dy)
        while abs(cur_dist - dist) > np.power(10.0, -10):
            if cur_dist > dist:
                min_azi = (max_azi + min_azi) / 2
            else:
                max_azi = (max_azi + min_azi) / 2
            cur_azi_theta = (min_azi + max_azi) / 2
            cur_x, cur_y = o2_x + r2 * np.cos(cur_azi_theta), o2_y + r2 * np.sin(
                cur_azi_theta)
            dx = prev_x - cur_x
            dy = prev_y - cur_y
            cur_dist = np.hypot(dx, dy)
        site_and_v[1][0] = cur_x
        site_and_v[1][1] = cur_y

        # 计算速度
        k_tangl1 = (np.sin(prev_theta) + (prev_theta + np.pi) * np.cos(prev_theta)) / (
                np.cos(prev_theta) - (prev_theta + np.pi) * np.sin(prev_theta))
        k_tangl2 = -1 / np.tan(cur_azi_theta)
        k_sec = (cur_y - prev_y) / (cur_x - prev_x)
        tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
        tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
        angle_A = np.arctan(tangentA)
        angle_B = np.arctan(tangentB)
        cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
        site_and_v[1][2] = cur_v

    for i in range(2, dragon_sections + 1):
        dist = lengths[i - 1]
        prev_x, prev_y, prev_v = site_and_v[i-1]
        # 上一节点在盘出螺旋线
        if is_intrace3:
            theta_out = cur_theta
            prev_theta = cur_theta
            # 上一节点在盘出螺旋线，当前节点在盘出螺旋线 v15
            if theta_out > theta_out_body:
                print("上一节点 %d 在盘出螺旋线，当前节点 %d 在盘出螺旋线 %d" % (i-1, i, t))
                is_intrace3 = 1
                is_intrace2 = 0
                is_intrace1 = 0
                max_theta = cur_theta
                min_theta = max_theta - np.pi
                cur_x, cur_y = b * ((max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.cos(
                    (max_theta + min_theta) / 2), b * ((max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.sin(
                    (max_theta + min_theta) / 2)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
                while abs(cur_dist - dist) > np.power(10.0, -10):
                    if cur_dist > dist:
                        min_theta = (max_theta + min_theta) / 2
                    else:
                        max_theta = (max_theta + min_theta) / 2
                    cur_x, cur_y = b * ((max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.cos(
                        (max_theta + min_theta) / 2), b * (
                                           (max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.sin(
                        (max_theta + min_theta) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                site_and_v[i][0] = cur_x
                site_and_v[i][1] = cur_y
                cur_theta = (max_theta + min_theta) / 2

                # 计算速度
                k_tangl1 = (np.sin(prev_theta) + (prev_theta + np.pi) * np.cos(prev_theta)) / (
                        np.cos(prev_theta) - (prev_theta + np.pi) * np.sin(prev_theta))
                k_tangl2 = (np.sin(cur_theta) + (cur_theta + np.pi) * np.cos(cur_theta)) / (
                        np.cos(cur_theta) - (cur_theta + np.pi) * np.sin(cur_theta))
                k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                angle_A = np.arctan(tangentA)
                angle_B = np.arctan(tangentB)
                cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                site_and_v[i][2] = cur_v
            # 上一节点在盘出螺旋线，当前节点在第二段圆弧 v16
            else:
                print("上一节点 %d 在盘出螺旋线，当前节点 %d 在第二段圆弧 %d" % (i-1, i, t))
                is_intrace3 = 0
                is_intrace2 = 1
                is_intrace1 = 0
                prev_theta = cur_theta
                min_azi = azi_o2_in
                max_azi = 2 * np.pi + azi_o2_out
                cur_azi_theta = (min_azi + max_azi) / 2
                cur_x, cur_y = o2_x + r2 * np.cos(cur_azi_theta), o2_y + r2 * np.sin(
                    cur_azi_theta)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
                while abs(cur_dist - dist) > np.power(10.0, -10):
                    if cur_dist > dist:
                        min_azi = (max_azi + min_azi) / 2
                    else:
                        max_azi = (max_azi + min_azi) / 2
                    cur_azi_theta = (min_azi + max_azi) / 2
                    cur_x, cur_y = o2_x + r2 * np.cos(cur_azi_theta), o2_y + r2 * np.sin(
                        cur_azi_theta)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                site_and_v[i][0] = cur_x
                site_and_v[i][1] = cur_y

                # 计算速度
                k_tangl1 = (np.sin(prev_theta) + (prev_theta + np.pi) * np.cos(prev_theta)) / (
                        np.cos(prev_theta) - (prev_theta + np.pi) * np.sin(prev_theta))
                k_tangl2 = -1 / np.tan(cur_azi_theta)
                k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                angle_A = np.arctan(tangentA)
                angle_B = np.arctan(tangentB)
                cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                site_and_v[i][2] = cur_v

        # 上一节点在第二段圆弧
        elif is_intrace2:
            pre_dazi2 = cur_azi_theta - azi_o2_in
            # 上一节点在第二段圆弧，当前节点在第二段圆弧 v17
            if pre_dazi2 > d_azi2_o2:
                print("上一节点 %d 在第二段圆弧，当前节点 %d 在第二段圆弧 %d" % (i-1, i, t))
                is_intrace3 = 0
                is_intrace2 = 1
                is_intrace1 = 0
                cur_azi_theta -= d_azi2_o2
                site_and_v[i][0] = o2_x + r2 * np.cos(cur_azi_theta)
                site_and_v[i][1] = o2_y + r2 * np.sin(cur_azi_theta)
                site_and_v[i][2] = site_and_v[i-1][2]
            # 上一节点在第二段圆弧，当前节点在第一段圆弧 v18
            else:
                print("上一节点 %d 在第二段圆弧，当前节点 %d 在第一段圆弧 %d" % (i-1, i, t))
                is_intrace3 = 0
                is_intrace2 = 0
                is_intrace1 = 1
                prev_azi_theta = cur_azi_theta
                prev_x, prev_y, prev_v = site_and_v[i - 1]
                # 计算出当前节点位置
                min_azi = azi_o1_out
                max_azi = azi_o1_in
                cur_x, cur_y = o1_x + r1 * np.cos((min_azi + max_azi) / 2), o1_y + r1 * np.sin(
                    (min_azi + max_azi) / 2)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
                times = 0
                while abs(cur_dist - dist) > np.power(10.0, -5):
                    if cur_dist > dist:
                        max_azi = (max_azi + min_azi) / 2
                    else:
                        min_azi = (max_azi + min_azi) / 2
                    cur_x, cur_y = o1_x + r1 * np.cos((min_azi + max_azi) / 2), o1_y + r1 * np.sin(
                        (min_azi + max_azi) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                    times += 1
                    if times <= 100 and t == 17:
                        print(cur_dist, min_azi, max_azi, sep=' ')
                site_and_v[i][0] = cur_x
                site_and_v[i][1] = cur_y
                cur_azi_theta = (max_azi + min_azi) / 2

                # 计算速度
                k_tangl1 = -1 / np.tan(prev_azi_theta)
                k_tangl2 = -1 / np.tan(cur_azi_theta)
                k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                angle_A = np.arctan(tangentA)
                angle_B = np.arctan(tangentB)
                cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                site_and_v[i][2] = cur_v

        # 上一节点在第一段圆弧
        elif is_intrace1:
            pre_dazi1 = azi_o1_in - cur_azi_theta
            # 上一节点在第一段圆弧，当前节点在第一段圆弧 v19
            if pre_dazi1 > d_azi2_o1:
                print("上一节点 %d 在第一段圆弧，当前节点 %d 在第一段圆弧 %d" % (i-1, i, t))
                is_intrace3 = 0
                is_intrace1 = 1
                is_intrace2 = 0
                cur_azi_theta += d_azi2_o1
                site_and_v[i][0] = o1_x + r1 * np.cos(cur_azi_theta)
                site_and_v[i][1] = o1_y + r1 * np.sin(cur_azi_theta)
                site_and_v[i][2] = site_and_v[i-1][2]
            # 上一节点在第一段圆弧，当前节点在盘入螺旋线 v20
            else:
                print("上一节点 %d 在第一段圆弧，当前节点 %d 在盘入螺旋线 %d" % (i-1, i, t))
                is_intrace1 = 0
                is_intrace2 = 0
                is_intrace3 = 0
                prev_azi_theta = cur_azi_theta
                prev_x, prev_y, prev_v = site_and_v[i - 1]
                # 计算出当前节点位置
                min_theta = theta_initial_in
                max_theta = theta_initial_in + np.pi
                cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                    (max_theta + min_theta) / 2), b * (
                                       max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                    (max_theta + min_theta) / 2)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
                while abs(cur_dist - dist) > np.power(10.0, -10):
                    if cur_dist > dist:
                        max_theta = (max_theta + min_theta) / 2
                    else:
                        min_theta = (max_theta + min_theta) / 2
                    cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                        (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                        (max_theta + min_theta) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                site_and_v[i][0] = cur_x
                site_and_v[i][1] = cur_y
                cur_theta = (max_theta + min_theta) / 2

                # 计算速度
                k_tangl1 = -1 / np.tan(prev_azi_theta)
                k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
                k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                angle_A = np.arctan(tangentA)
                angle_B = np.arctan(tangentB)
                cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                site_and_v[i][2] = cur_v
        # 上一节点在盘入螺旋线，当前节点在盘入螺旋线 v21
        else:
            print("上一节点 %d 在盘入螺旋线，当前节点 %d 在盘入螺旋线 %d" % (i-1, i, t))
            is_intrace1 = 0
            is_intrace2 = 0
            is_intrace3 = 0
            prev_theta = cur_theta
            prev_x, prev_y, prev_v = site_and_v[i - 1]
            # 计算出当前节点位置
            min_theta = cur_theta
            max_theta = min_theta + np.pi
            cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                (max_theta + min_theta) / 2), b * (
                                   max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                (max_theta + min_theta) / 2)
            dx = prev_x - cur_x
            dy = prev_y - cur_y
            cur_dist = np.hypot(dx, dy)
            while abs(cur_dist - dist) > np.power(10.0, -10):
                if cur_dist > dist:
                    max_theta = (max_theta + min_theta) / 2
                else:
                    min_theta = (max_theta + min_theta) / 2
                cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                    (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                    (max_theta + min_theta) / 2)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
            site_and_v[i][0] = cur_x
            site_and_v[i][1] = cur_y
            cur_theta = (max_theta + min_theta) / 2

            # 计算速度
            k_tangl1 = (np.sin(prev_theta) + prev_theta * np.cos(prev_theta)) / (
                        np.cos(prev_theta) - prev_theta * np.sin(prev_theta))
            k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (
                        np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
            k_sec = (cur_y - prev_y) / (cur_x - prev_x)
            tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
            tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
            angle_A = np.arctan(tangentA)
            angle_B = np.arctan(tangentB)
            cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
            site_and_v[i][2] = cur_v

    for i in range(dragon_sections + 1):
        worksheet1.cell(row=2 * i + 2, column=102 + t, value=np.round(site_and_v[i][0], 6))
        worksheet1.cell(row=2 * i + 3, column=102 + t, value=np.round(site_and_v[i][1], 6))
        worksheet2.cell(row=i + 2, column=102 + t, value=np.round(site_and_v[i][2], 6))

    for i in range(10000):
        dtheta = 1 / 10000 / r
        head_theta += dtheta
        r = b * (head_theta + np.pi) / 2 / np.pi

workbook.save('result4.xlsx')
\end{verbatim}

\section{附录: solution5.py}
\begin{verbatim}
import numpy as np
from caculate_angle import angle_head
from caculate_angle import angle_body

b = 1.7
v_head = 1.0
dragon_sections = 223
time_steps = 100
lengths = [2.86] + [1.65] * 222
theta_initial_in = 90 * np.pi / 17
theta_initial_out = 73 * np.pi / 17
azi_o1_in, azi_o1_out, azi_o2_in, azi_o2_out = (0, 0, 0, 0)

# 求当前切入切出位置下的S形圆弧位置
theta_in = theta_initial_in
theta_out = theta_initial_out
# 入切点和出切点的坐标
in_x = b / 2 / np.pi * theta_in * np.cos(theta_in)
in_y = b / 2 / np.pi * theta_in * np.sin(theta_in)
out_x = b / 2 / np.pi * theta_out * np.cos(theta_out) + b / 2 * np.cos(theta_out)
out_y = b / 2 / np.pi * theta_out * np.sin(theta_out) + b / 2 * np.sin(theta_out)
# 出-入切点处切线
k_tangl_in = (np.sin(theta_in) + theta_in * np.cos(theta_in)) / (np.cos(theta_in) - theta_in * np.sin(theta_in))
b_tangl_in = in_y - k_tangl_in * in_x
k_tangl_out = (np.sin(theta_out) + (theta_out + np.pi) * np.cos(theta_out)) / (np.cos(theta_out) - (theta_out + np.pi) * np.sin(theta_out))
b_tangl_out = out_y - k_tangl_out * out_x
# 出-入切点处切线垂线
k_perpl_in = -1 / k_tangl_in
b_perpl_in = in_y - k_perpl_in * in_x
k_perpl_out = -1 / k_tangl_out
b_perpl_out = out_y - k_perpl_out * out_x

# 找到对应盘出轨迹
for d_r1 in range(4010000):
    r1 = d_r1 / 1000000
    r2 = r1 / 2
    # 求两圆心的坐标
    o1_x, o1_y, o2_x, o2_y = (0.0, 0.0, 0.0, 0.0)
    dx_o1 = r1 / np.sqrt(1 + np.power(k_perpl_in, 2))
    dy_o1 = abs(k_perpl_in * r1) / np.sqrt(1 + np.power(k_perpl_in, 2))
    dx_o2 = r2 / np.sqrt(1 + np.power(k_perpl_out, 2))
    dy_o2 = abs(k_perpl_out * r2) / np.sqrt(1 + np.power(k_perpl_out, 2))
    if k_tangl_in > 0 and b_tangl_in > 0:
        o1_x = in_x + dx_o1
        o1_y = in_y - dy_o1
    elif k_tangl_in > 0 and b_tangl_in < 0:
        o1_x = in_x - dx_o1
        o1_y = in_y + dy_o1
    elif k_tangl_in < 0 and b_tangl_in > 0:
        o1_x = in_x - dx_o1
        o1_y = in_y - dy_o1
    elif k_tangl_in < 0 and b_tangl_in < 0:
        o1_x = in_x + dx_o1
        o1_y = in_y + dy_o1
    if k_tangl_out > 0 and b_tangl_out > 0:
        o2_x = out_x + dx_o2
        o2_y = out_y - dy_o2
    elif k_tangl_out > 0 and b_tangl_out < 0:
        o2_x = out_x - dx_o2
        o2_y = out_y + dy_o2
    elif k_tangl_out < 0 and b_tangl_out > 0:
        o2_x = out_x - dx_o2
        o2_y = out_y - dy_o2
    elif k_tangl_out < 0 and b_tangl_out < 0:
        o2_x = out_x + dx_o2
        o2_y = out_y + dy_o2

    # 验证两圆心距离是否满足要求
    d_o1_o2 = np.hypot(o1_x-o2_x, o1_y-o2_y)
    if abs(d_o1_o2 - r1 - r2) <= np.power(10.0, -6):
        # 求两圆切点
        dot_tang_x = (o1_x + 2 * o2_x) / 3
        dot_tang_y = (o1_y + 2 * o2_y) / 3
        print("大圆半径: %lfm\n"
              "小圆半径: %lfm\n"
              "入切点位置: %lf x=%lf y=%lf\n"
              "大圆圆心: %lf %lf\n"
              "两圆切点: %lf %lf\n"
              "小圆圆心: %lf %lf\n"
              "出切点位置: %lf x=%lf y=%lf"
              % (r1,
                 r2,
                 theta_in, in_x, in_y,
                 o1_x, o1_y,
                 dot_tang_x, dot_tang_y,
                 o2_x, o2_y,
                 theta_out, out_x, out_y)
              )
        k_o1_in = (o1_y-in_y) / (o1_x-in_x)
        k_o1_tang = (dot_tang_y-o1_y) / (dot_tang_x-o1_x)
        k_o2_tang = (o2_y-dot_tang_y) / (o2_x-dot_tang_x)
        k_o2_out = (out_y-o2_y) / (out_x-o2_x)
        central_angle_o1 = np.arctan(k_o1_in) + np.pi - np.arctan(k_o1_tang)  # 大圆中的圆心角
        central_angle_o2 = np.arctan(k_o2_out) + np.pi - np.arctan(k_o2_tang)  # 小圆中的圆心角
        azi_o1_in = np.pi + np.arctan(k_o1_in)  # 大圆中进入半径对应方位角
        azi_o1_out = np.arctan(k_o1_tang)  # 大圆中离开半径对应方位角
        azi_o2_in = 2 * np.pi - central_angle_o2 + np.arctan(k_o2_out)  # 小圆中进入半径对应方位角
        azi_o2_out = np.arctan(k_o2_out)  # 小圆中离开半径对应方位角
        break

for dv in range(1, 101):
    v_head += dv / 100
    print(v_head)
    # 求位置速度相关信息
    site_and_v = np.zeros((dragon_sections+1, 3))
    site_and_v[0] = (in_x, in_y, v_head)
    head_theta = theta_initial_in
    r = b * head_theta / 2 / np.pi
    # 针对-100~0的数据计算
    for t in range(time_steps + 1):
        # 更新各节的位置
        cur_theta = head_theta
        for i in range(1, dragon_sections+1):
            dist = lengths[i - 1]
            prev_x, prev_y, prev_v = site_and_v[i - 1]
            prev_theta = cur_theta
            # 计算出当前节点位置
            min_theta = cur_theta
            max_theta = cur_theta + np.pi
            cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos((max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin((max_theta + min_theta) / 2)
            dx = prev_x - cur_x
            dy = prev_y - cur_y
            cur_dist = np.hypot(dx, dy)
            while abs(cur_dist-dist) > np.power(10.0, -10):
                if cur_dist > dist:
                    max_theta = (max_theta + min_theta) / 2
                else:
                    min_theta = (max_theta + min_theta) / 2
                cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos((max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin((max_theta + min_theta) / 2)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
            site_and_v[i][0] = cur_x
            site_and_v[i][1] = cur_y
            cur_theta = (max_theta + min_theta) / 2

            # 计算速度
            k_tangl1 = (np.sin(prev_theta) + prev_theta * np.cos(prev_theta)) / (np.cos(prev_theta) - prev_theta * np.sin(prev_theta))
            k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
            k_sec = (cur_y - prev_y) / (cur_x - prev_x)
            tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
            tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
            angle_A = np.arctan(tangentA)
            angle_B = np.arctan(tangentB)
            cur_v = prev_v * np.cos(angle_A) / np.cos(angle_B)
            site_and_v[i][2] = cur_v
            if cur_v > 2.0:
                print("时间:%d" % t)
                print("最大速度为%fm/s" % (v_head-0.01))
                exit()
        # 将时间微分至0.01提高精度
        for i in range(10000):
            dtheta = v_head / 10000 / r
            head_theta += dtheta
            r = b * head_theta / (2 * np.pi)
        site_and_v[0] = [r * np.cos(head_theta), r * np.sin(head_theta), v_head]

    # 针对0~100的数据计算
    site_and_v = np.zeros((dragon_sections+1, 3))
    site_and_v[0] = (in_x, in_y, v_head)
    head_theta = theta_initial_out  # 龙头到达盘出螺线对应极角
    r = b * (head_theta + np.pi) / 2 / np.pi
    time_used = 0  # 经过时间
    cur_azi_theta = azi_o1_in  # 当前方位角
    all_t_o1 = central_angle_o1 * r1 / v_head  # 在o1中的运动时间
    all_t_o2 = central_angle_o2 * r2 / v_head  # 在o2中的运动时间
    d_azi1_o1 = 2 * np.arcsin(1.43 / r1)  # 第一个板凳在o1所需夹角
    d_azi2_o1 = 2 * np.arcsin(0.825 / r1)  # 后续板凳在o1所需夹角
    d_azi1_o2 = 2 * np.arcsin(1.43 / r2)  # 第一个板凳在o2所需夹角
    d_azi2_o2 = 2 * np.arcsin(0.825 / r2)  # 后续板凳在o2所需夹角
    theta_out_head = angle_head()  # 第一个板凳在出螺旋线所需龙头前把手极角
    theta_out_body = angle_body()  # 后续板凳在出螺旋线所需前一节点极角
    print("在o1中时间: %fs\n在o2中时间: %fs" % (all_t_o1, all_t_o2))
    for t in range(1, time_steps+1):
        # 龙头进入第一段圆弧
        if t <= all_t_o1:
            print("龙头进入第一段圆弧 %d" % t)
            cur_azi_theta = azi_o1_in - v_head * t / r1
            site_and_v[0][0] = o1_x + r1 * np.cos(cur_azi_theta)
            site_and_v[0][1] = o1_y + r1 * np.sin(cur_azi_theta)
            site_and_v[0][2] = v_head
            is_intrace = 1
            dist = lengths[0]
            pre_dazi = azi_o1_in - cur_azi_theta
            # 第一节点在第一段圆弧，第二节点在第一段圆弧 v1
            if pre_dazi > d_azi1_o1:
                is_intrace = 1
                cur_azi_theta += d_azi1_o1
                site_and_v[1][0] = o1_x + r1 * np.cos(cur_azi_theta)
                site_and_v[1][1] = o1_y + r1 * np.sin(cur_azi_theta)
                site_and_v[1][2] = v_head
                if v_head > 2.0:
                    print("时间:%d" % t)
                    print("最大速度为%fm/s" % (v_head-0.01))
                    exit()
            # 第一节点在第一段圆弧，第二节点在盘入螺旋线 v2
            else:
                is_intrace = 0
                prev_azi_theta = cur_azi_theta
                prev_x, prev_y, prev_v = site_and_v[0]
                # 计算出当前节点位置
                min_theta = theta_initial_in
                max_theta = theta_initial_in + np.pi
                cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos((max_theta + min_theta) / 2), b * (
                        max_theta + min_theta) / 2 / (2 * np.pi) * np.sin((max_theta + min_theta) / 2)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
                while abs(cur_dist - dist) > np.power(10.0, -10):
                    if cur_dist > dist:
                        max_theta = (max_theta + min_theta) / 2
                    else:
                        min_theta = (max_theta + min_theta) / 2
                    cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                        (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                        (max_theta + min_theta) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                site_and_v[1][0] = cur_x
                site_and_v[1][1] = cur_y
                cur_theta = (max_theta + min_theta) / 2

                # 计算速度
                k_tangl1 = -1 / np.tan(prev_azi_theta)
                k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (
                            np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
                k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                angle_A = np.arctan(tangentA)
                angle_B = np.arctan(tangentB)
                cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                site_and_v[1][2] = cur_v

                if cur_v > 2.0:
                    print("时间:%d" % t)
                    print("最大速度为%fm/s" % (v_head - 0.01))
                    exit()

            for i in range(2, dragon_sections+1):
                prev_x, prev_y, prev_v = site_and_v[i-1]
                dist = lengths[i-1]
                # 上一节点在第一段圆弧中
                if is_intrace:
                    pre_dazi = azi_o1_in - cur_azi_theta
                    # 上一节点在第一段圆弧，当前节点在第一段圆弧 v3
                    if pre_dazi > d_azi2_o1:
                        is_intrace = 1
                        cur_azi_theta += d_azi2_o1
                        site_and_v[i][0] = o1_x + r1 * np.cos(cur_azi_theta)
                        site_and_v[i][1] = o1_y + r1 * np.sin(cur_azi_theta)
                        site_and_v[i][2] = site_and_v[i-1][2]
                        if site_and_v[i][2] > 2.0:
                            print("时间:%d" % t)
                            print("最大速度为%fm/s" % (v_head - 0.01))
                            exit()
                    # 上一节点在第一段圆弧，当前节点在盘入螺旋线 v4
                    else:
                        is_intrace = 0
                        prev_azi_theta = cur_azi_theta
                        prev_x, prev_y, prev_v = site_and_v[i-1]
                        # 计算出当前节点位置
                        min_theta = theta_initial_in
                        max_theta = theta_initial_in + np.pi
                        cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                            (max_theta + min_theta) / 2), b * (
                                               max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                            (max_theta + min_theta) / 2)
                        dx = prev_x - cur_x
                        dy = prev_y - cur_y
                        cur_dist = np.hypot(dx, dy)
                        while abs(cur_dist - dist) > np.power(10.0, -10):
                            if cur_dist > dist:
                                max_theta = (max_theta + min_theta) / 2
                            else:
                                min_theta = (max_theta + min_theta) / 2
                            cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                                (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                                (max_theta + min_theta) / 2)
                            dx = prev_x - cur_x
                            dy = prev_y - cur_y
                            cur_dist = np.hypot(dx, dy)
                        site_and_v[i][0] = cur_x
                        site_and_v[i][1] = cur_y
                        cur_theta = (max_theta + min_theta) / 2

                        # 计算速度
                        k_tangl1 = -1 / np.tan(prev_azi_theta)
                        k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (
                                np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
                        k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                        tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                        tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                        angle_A = np.arctan(tangentA)
                        angle_B = np.arctan(tangentB)
                        cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                        site_and_v[i][2] = cur_v

                        if cur_v > 2.0:
                            print("时间:%d" % t)
                            print("最大速度为%fm/s" % (v_head - 0.01))
                            exit()

                # 上一节点在盘入螺旋线，下一节点在盘入螺旋线 v5
                else:
                    prev_theta = cur_theta
                    prev_x, prev_y, prev_v = site_and_v[i - 1]
                    # 计算出当前节点位置
                    min_theta = cur_theta
                    max_theta = min_theta + np.pi
                    cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                        (max_theta + min_theta) / 2), b * (
                                           max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                        (max_theta + min_theta) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                    while abs(cur_dist - dist) > np.power(10.0, -10):
                        if cur_dist > dist:
                            max_theta = (max_theta + min_theta) / 2
                        else:
                            min_theta = (max_theta + min_theta) / 2
                        cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                            (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                            (max_theta + min_theta) / 2)
                        dx = prev_x - cur_x
                        dy = prev_y - cur_y
                        cur_dist = np.hypot(dx, dy)
                    site_and_v[i][0] = cur_x
                    site_and_v[i][1] = cur_y
                    cur_theta = (max_theta + min_theta) / 2

                    # 计算速度
                    k_tangl1 = (np.sin(prev_theta) + prev_theta * np.cos(prev_theta)) / (
                                np.cos(prev_theta) - prev_theta * np.sin(prev_theta))
                    k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (
                                np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
                    k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                    tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                    tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                    angle_A = np.arctan(tangentA)
                    angle_B = np.arctan(tangentB)
                    cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                    site_and_v[i][2] = cur_v
                    if cur_v > 2.0:
                        print("时间:%d" % t)
                        print("最大速度为%fm/s" % (v_head - 0.01))
                        exit()
        # 龙头进入第二段圆弧
        elif t <= all_t_o1 + all_t_o2:
            print("龙头进入第二段圆弧 %d" % t)
            cur_azi_theta = azi_o2_in + v_head * (t-all_t_o1) / r2
            site_and_v[0][0] = o2_x + r2 * np.cos(cur_azi_theta)
            site_and_v[0][1] = o2_y + r2 * np.sin(cur_azi_theta)
            site_and_v[0][2] = v_head
            is_intrace2 = 1
            is_intrace1 = 0
            dist = lengths[0]
            pre_dazi2 = cur_azi_theta - azi_o2_in
            # 第一节点在第二段圆弧，第二节点在第二段圆弧 v6
            if pre_dazi2 > d_azi1_o2:
                is_intrace2 = 1
                is_intrace1 = 0
                cur_azi_theta -= d_azi1_o2
                site_and_v[1][0] = o2_x + r2 * np.cos(cur_azi_theta)
                site_and_v[1][1] = o2_y + r2 * np.sin(cur_azi_theta)
                site_and_v[1][2] = v_head
                if v_head > 2.0:
                    print("时间:%d" % t)
                    print("最大速度为%fm/s" % (v_head - 0.01))
                    exit()
            # 第一节点在第二段圆弧，第二节点在第一段圆弧 v7
            else:
                is_intrace2 = 0
                is_intrace1 = 1
                prev_azi_theta = cur_azi_theta
                prev_x, prev_y, prev_v = site_and_v[0]
                # 计算出当前节点位置
                min_azi = azi_o1_out
                max_azi = azi_o1_in
                cur_x, cur_y = o1_x + r1 * np.cos((min_azi+max_azi)/2), o1_y + r1 * np.sin((min_azi+max_azi)/2)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
                while abs(cur_dist - dist) > np.power(10.0, -10):
                    if cur_dist > dist:
                        max_azi = (max_azi + min_azi) / 2
                    else:
                        min_azi = (max_azi + min_azi) / 2
                    cur_x, cur_y = o1_x + r1 * np.cos((min_azi+max_azi)/2), o1_y + r1 * np.sin((min_azi+max_azi)/2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                site_and_v[1][0] = cur_x
                site_and_v[1][1] = cur_y
                cur_azi_theta = (max_azi + min_azi) / 2

                # 计算速度
                k_tangl1 = -1 / np.tan(cur_azi_theta)
                k_tangl2 = -1 / np.tan(prev_azi_theta)
                k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                angle_A = np.arctan(tangentA)
                angle_B = np.arctan(tangentB)
                cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                site_and_v[1][2] = cur_v
                if cur_v > 2.0:
                    print("时间:%d" % t)
                    print("最大速度为%fm/s" % (v_head - 0.01))
                    exit()

            for i in range(2, dragon_sections+1):
                dist = lengths[i-1]
                prev_x, prev_y, prev_v = site_and_v[i-1]
                # 上一节点在第二段圆弧
                if is_intrace2:
                    pre_dazi2 = cur_azi_theta - azi_o2_in
                    # 上一节点在第二段圆弧，当前节点在第二段圆弧 v8
                    if pre_dazi2 > d_azi2_o2:
                        is_intrace2 = 1
                        is_intrace1 = 0
                        cur_azi_theta -= d_azi2_o2
                        site_and_v[i][0] = o2_x + r2 * np.cos(cur_azi_theta)
                        site_and_v[i][1] = o2_y + r2 * np.sin(cur_azi_theta)
                        site_and_v[i][2] = site_and_v[i-1][2]
                        if site_and_v[i][2] > 2.0:
                            print("时间:%d" % t)
                            print("最大速度为%fm/s" % (v_head - 0.01))
                            exit()
                    # 上一节点在第二段圆弧，当前节点在第一段圆弧 v9
                    else:
                        is_intrace2 = 0
                        is_intrace1 = 1
                        prev_azi_theta = cur_azi_theta
                        prev_x, prev_y, prev_v = site_and_v[i-1]
                        # 计算出当前节点位置
                        min_azi = azi_o1_out
                        max_azi = azi_o1_in
                        cur_x, cur_y = o1_x + r1 * np.cos((min_azi + max_azi) / 2), o1_y + r1 * np.sin(
                            (min_azi + max_azi) / 2)
                        dx = prev_x - cur_x
                        dy = prev_y - cur_y
                        cur_dist = np.hypot(dx, dy)
                        while abs(cur_dist - dist) > np.power(10.0, -10):
                            if cur_dist > dist:
                                max_azi = (max_azi + min_azi) / 2
                            else:
                                min_azi = (max_azi + min_azi) / 2
                            cur_x, cur_y = o1_x + r1 * np.cos((min_azi + max_azi) / 2), o1_y + r1 * np.sin(
                                (min_azi + max_azi) / 2)
                            dx = prev_x - cur_x
                            dy = prev_y - cur_y
                            cur_dist = np.hypot(dx, dy)
                        site_and_v[i][0] = cur_x
                        site_and_v[i][1] = cur_y
                        cur_azi_theta = (max_azi + min_azi) / 2

                        # 计算速度
                        k_tangl1 = -1 / np.tan(prev_azi_theta)
                        k_tangl2 = -1 / np.tan(cur_azi_theta)
                        k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                        tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                        tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                        angle_A = np.arctan(tangentA)
                        angle_B = np.arctan(tangentB)
                        cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                        site_and_v[i][2] = cur_v
                        if cur_v > 2.0:
                            print("时间:%d" % t)
                            print("最大速度为%fm/s" % (v_head - 0.01))
                            exit()

                # 上一节点在第一段圆弧
                elif is_intrace1:
                    prev_azi_theta = cur_azi_theta
                    pre_dazi1 = azi_o1_in - cur_azi_theta
                    # 上一节点在第一段圆弧，当前节点在第一段圆弧 v10
                    if pre_dazi1 > d_azi2_o1:
                        is_intrace1 = 1
                        is_intrace2 = 0
                        cur_azi_theta += d_azi2_o1
                        site_and_v[i][0] = o1_x + r1 * np.cos(cur_azi_theta)
                        site_and_v[i][1] = o1_y + r1 * np.sin(cur_azi_theta)
                        site_and_v[i][2] = site_and_v[i-1][2]
                        if  site_and_v[i-1][2] > 2.0:
                            print("时间:%d" % t)
                            print("最大速度为%fm/s" % (v_head - 0.01))
                            exit()
                    # 上一节点在第一段圆弧，当前节点在盘入螺旋线 v11
                    else:
                        is_intrace1 = 0
                        is_intrace2 = 0
                        prev_azi_theta = cur_azi_theta
                        prev_x, prev_y, prev_v = site_and_v[i - 1]
                        # 计算出当前节点位置
                        min_theta = theta_initial_in
                        max_theta = theta_initial_in + np.pi
                        cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                            (max_theta + min_theta) / 2), b * (
                                               max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                            (max_theta + min_theta) / 2)
                        dx = prev_x - cur_x
                        dy = prev_y - cur_y
                        cur_dist = np.hypot(dx, dy)
                        while abs(cur_dist - dist) > np.power(10.0, -10):
                            if cur_dist > dist:
                                max_theta = (max_theta + min_theta) / 2
                            else:
                                min_theta = (max_theta + min_theta) / 2
                            cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                                (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                                (max_theta + min_theta) / 2)
                            dx = prev_x - cur_x
                            dy = prev_y - cur_y
                            cur_dist = np.hypot(dx, dy)
                        site_and_v[i][0] = cur_x
                        site_and_v[i][1] = cur_y
                        cur_theta = (max_theta + min_theta) / 2

                        # 计算速度
                        k_tangl1 = -1 / np.tan(prev_azi_theta)
                        k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (
                                np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
                        k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                        tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                        tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                        angle_A = np.arctan(tangentA)
                        angle_B = np.arctan(tangentB)
                        cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                        site_and_v[i][2] = cur_v
                        if cur_v > 2.0:
                            print("时间:%d" % t)
                            print("最大速度为%fm/s" % (v_head - 0.01))
                            exit()

                # 上一节点在盘入螺旋线，当前节点在盘入螺旋线 v12
                else:
                    is_intrace1 = 0
                    is_intrace2 = 0
                    prev_theta = cur_theta
                    prev_x, prev_y, prev_v = site_and_v[i - 1]
                    # 计算出当前节点位置
                    min_theta = cur_theta
                    max_theta = min_theta + np.pi
                    cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                        (max_theta + min_theta) / 2), b * (
                                           max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                        (max_theta + min_theta) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                    while abs(cur_dist - dist) > np.power(10.0, -10):
                        if cur_dist > dist:
                            max_theta = (max_theta + min_theta) / 2
                        else:
                            min_theta = (max_theta + min_theta) / 2
                        cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                            (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                            (max_theta + min_theta) / 2)
                        dx = prev_x - cur_x
                        dy = prev_y - cur_y
                        cur_dist = np.hypot(dx, dy)
                    site_and_v[i][0] = cur_x
                    site_and_v[i][1] = cur_y
                    cur_theta = (max_theta + min_theta) / 2

                    # 计算速度
                    k_tangl1 = (np.sin(prev_theta) + prev_theta * np.cos(prev_theta)) / (
                            np.cos(prev_theta) - prev_theta * np.sin(prev_theta))
                    k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (
                            np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
                    k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                    tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                    tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                    angle_A = np.arctan(tangentA)
                    angle_B = np.arctan(tangentB)
                    cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                    site_and_v[i][2] = cur_v
                    if cur_v > 2.0:
                        print("时间:%d" % t)
                        print("最大速度为%fm/s" % (v_head - 0.01))
                        exit()

        # 龙头进入盘出螺旋线
        else:
            print("龙头进入盘出螺旋线 %d" % t)
            for i in range(100000):
                dtheta = v_head / 100000 * (t - all_t_o1 - all_t_o2) / r
                head_theta += dtheta
                r = b * (head_theta + np.pi) / 2 / np.pi
            time_used = t
            break
    for t in range(time_used, time_steps+1):
        print("龙头进入盘出螺旋线 %d" % t)
        site_and_v[0][0] = r * np.cos(head_theta)
        site_and_v[0][1] = r * np.sin(head_theta)
        site_and_v[0][2] = v_head
        dist = lengths[0]
        is_intrace3 = 1
        is_intrace2 = 0
        is_intrace1 = 0
        prev_x, prev_y, prev_v = site_and_v[0]
        theta_out = head_theta
        # 第一节点在盘出螺旋线，第二节点在盘出螺旋线 v13
        if theta_out > theta_out_head:
            print("第一节点在盘出螺旋线，第二节点在盘出螺旋线 %d" % t)
            prev_theta = head_theta
            is_intrace3 = 1
            is_intrace2 = 0
            is_intrace1 = 0
            max_theta = head_theta
            min_theta = max_theta - np.pi
            cur_x, cur_y = b * ((max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.cos((max_theta + min_theta) / 2), b * (
                        (max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.sin((max_theta + min_theta) / 2)
            dx = prev_x - cur_x
            dy = prev_y - cur_y
            cur_dist = np.hypot(dx, dy)
            while abs(cur_dist - dist) > np.power(10.0, -10):
                if cur_dist > dist:
                    min_theta = (max_theta + min_theta) / 2
                else:
                    max_theta = (max_theta + min_theta) / 2
                cur_x, cur_y = b * ((max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.cos((max_theta + min_theta) / 2), b * (
                        (max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.sin((max_theta + min_theta) / 2)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
            site_and_v[1][0] = cur_x
            site_and_v[1][1] = cur_y
            cur_theta = (min_theta + max_theta) / 2

            # 计算速度
            k_tangl1 = (np.sin(prev_theta) + (prev_theta + np.pi) * np.cos(prev_theta)) / (
                        np.cos(prev_theta) - (prev_theta+np.pi) * np.sin(prev_theta))
            k_tangl2 = (np.sin(cur_theta) + (cur_theta+np.pi) * np.cos(cur_theta)) / (
                        np.cos(cur_theta) - (cur_theta+np.pi) * np.sin(cur_theta))
            k_sec = (cur_y - prev_y) / (cur_x - prev_x)
            tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
            tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
            angle_A = np.arctan(tangentA)
            angle_B = np.arctan(tangentB)
            cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
            site_and_v[1][2] = cur_v
            if cur_v > 2.0:
                print("时间:%d" % t)
                print("最大速度为%fm/s" % (v_head - 0.01))
                exit()
        # 第一节点在盘出螺旋线，第二节点在第二段圆弧 v14
        else:
            print("第一节点在盘出螺旋线，第二节点在第二段圆弧 %d" % t)
            prev_theta = head_theta
            prev_x,prev_y,prev_v = site_and_v[0]
            is_intrace3 = 0
            is_intrace2 = 1
            is_intrace1 = 0
            min_azi = azi_o2_in
            max_azi = 2 * np.pi + azi_o2_out
            cur_azi_theta = (min_azi + max_azi) / 2
            cur_x, cur_y = o2_x + r2 * np.cos(cur_azi_theta), o2_y + r2 * np.sin(
                cur_azi_theta)
            dx = prev_x - cur_x
            dy = prev_y - cur_y
            cur_dist = np.hypot(dx, dy)
            while abs(cur_dist - dist) > np.power(10.0, -10):
                if cur_dist > dist:
                    min_azi = (max_azi + min_azi) / 2
                else:
                    max_azi = (max_azi + min_azi) / 2
                cur_azi_theta = (min_azi + max_azi) / 2
                cur_x, cur_y = o2_x + r2 * np.cos(cur_azi_theta), o2_y + r2 * np.sin(
                    cur_azi_theta)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
            site_and_v[1][0] = cur_x
            site_and_v[1][1] = cur_y

            # 计算速度
            k_tangl1 = (np.sin(prev_theta) + (prev_theta + np.pi) * np.cos(prev_theta)) / (
                    np.cos(prev_theta) - (prev_theta + np.pi) * np.sin(prev_theta))
            k_tangl2 = -1 / np.tan(cur_azi_theta)
            k_sec = (cur_y - prev_y) / (cur_x - prev_x)
            tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
            tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
            angle_A = np.arctan(tangentA)
            angle_B = np.arctan(tangentB)
            cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
            site_and_v[1][2] = cur_v
            if cur_v > 2.0:
                print("时间:%d" % t)
                print("最大速度为%fm/s" % (v_head - 0.01))
                exit()
        for i in range(2, dragon_sections + 1):
            dist = lengths[i - 1]
            prev_x, prev_y, prev_v = site_and_v[i-1]
            # 上一节点在盘出螺旋线
            if is_intrace3:
                theta_out = cur_theta
                prev_theta = cur_theta
                # 上一节点在盘出螺旋线，当前节点在盘出螺旋线 v15
                if theta_out > theta_out_body:
                    print("上一节点 %d 在盘出螺旋线，当前节点 %d 在盘出螺旋线 %d" % (i-1, i, t))
                    is_intrace3 = 1
                    is_intrace2 = 0
                    is_intrace1 = 0
                    max_theta = cur_theta
                    min_theta = max_theta - np.pi
                    cur_x, cur_y = b * ((max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.cos(
                        (max_theta + min_theta) / 2), b * ((max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.sin(
                        (max_theta + min_theta) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                    while abs(cur_dist - dist) > np.power(10.0, -10):
                        if cur_dist > dist:
                            min_theta = (max_theta + min_theta) / 2
                        else:
                            max_theta = (max_theta + min_theta) / 2
                        cur_x, cur_y = b * ((max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.cos(
                            (max_theta + min_theta) / 2), b * (
                                               (max_theta + min_theta) / 2 + np.pi) / 2 / np.pi * np.sin(
                            (max_theta + min_theta) / 2)
                        dx = prev_x - cur_x
                        dy = prev_y - cur_y
                        cur_dist = np.hypot(dx, dy)
                    site_and_v[i][0] = cur_x
                    site_and_v[i][1] = cur_y
                    cur_theta = (max_theta + min_theta) / 2

                    # 计算速度
                    k_tangl1 = (np.sin(prev_theta) + (prev_theta + np.pi) * np.cos(prev_theta)) / (
                            np.cos(prev_theta) - (prev_theta + np.pi) * np.sin(prev_theta))
                    k_tangl2 = (np.sin(cur_theta) + (cur_theta + np.pi) * np.cos(cur_theta)) / (
                            np.cos(cur_theta) - (cur_theta + np.pi) * np.sin(cur_theta))
                    k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                    tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                    tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                    angle_A = np.arctan(tangentA)
                    angle_B = np.arctan(tangentB)
                    cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                    site_and_v[i][2] = cur_v
                    if cur_v > 2.0:
                        print("时间:%d" % t)
                        print("最大速度为%fm/s" % (v_head - 0.01))
                        exit()
                # 上一节点在盘出螺旋线，当前节点在第二段圆弧 v16
                else:
                    print("上一节点 %d 在盘出螺旋线，当前节点 %d 在第二段圆弧 %d" % (i-1, i, t))
                    is_intrace3 = 0
                    is_intrace2 = 1
                    is_intrace1 = 0
                    prev_theta = cur_theta
                    min_azi = azi_o2_in
                    max_azi = 2 * np.pi + azi_o2_out
                    cur_azi_theta = (min_azi + max_azi) / 2
                    cur_x, cur_y = o2_x + r2 * np.cos(cur_azi_theta), o2_y + r2 * np.sin(
                        cur_azi_theta)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                    while abs(cur_dist - dist) > np.power(10.0, -10):
                        if cur_dist > dist:
                            min_azi = (max_azi + min_azi) / 2
                        else:
                            max_azi = (max_azi + min_azi) / 2
                        cur_azi_theta = (min_azi + max_azi) / 2
                        cur_x, cur_y = o2_x + r2 * np.cos(cur_azi_theta), o2_y + r2 * np.sin(
                            cur_azi_theta)
                        dx = prev_x - cur_x
                        dy = prev_y - cur_y
                        cur_dist = np.hypot(dx, dy)
                    site_and_v[i][0] = cur_x
                    site_and_v[i][1] = cur_y

                    # 计算速度
                    k_tangl1 = (np.sin(prev_theta) + (prev_theta + np.pi) * np.cos(prev_theta)) / (
                            np.cos(prev_theta) - (prev_theta + np.pi) * np.sin(prev_theta))
                    k_tangl2 = -1 / np.tan(cur_azi_theta)
                    k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                    tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                    tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                    angle_A = np.arctan(tangentA)
                    angle_B = np.arctan(tangentB)
                    cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                    site_and_v[i][2] = cur_v
                    if cur_v > 2.0:
                        print("时间:%d" % t)
                        print("最大速度为%fm/s" % (v_head - 0.01))
                        exit()

            # 上一节点在第二段圆弧
            elif is_intrace2:
                pre_dazi2 = cur_azi_theta - azi_o2_in
                # 上一节点在第二段圆弧，当前节点在第二段圆弧 v17
                if pre_dazi2 > d_azi2_o2:
                    print("上一节点 %d 在第二段圆弧，当前节点 %d 在第二段圆弧 %d" % (i-1, i, t))
                    is_intrace3 = 0
                    is_intrace2 = 1
                    is_intrace1 = 0
                    cur_azi_theta -= d_azi2_o2
                    site_and_v[i][0] = o2_x + r2 * np.cos(cur_azi_theta)
                    site_and_v[i][1] = o2_y + r2 * np.sin(cur_azi_theta)
                    site_and_v[i][2] = site_and_v[i-1][2]
                    if site_and_v[i-1][2] > 2.0:
                        print("时间:%d" % t)
                        print("最大速度为%fm/s" % (v_head - 0.01))
                        exit()
                # 上一节点在第二段圆弧，当前节点在第一段圆弧 v18
                else:
                    print("上一节点 %d 在第二段圆弧，当前节点 %d 在第一段圆弧 %d" % (i-1, i, t))
                    is_intrace3 = 0
                    is_intrace2 = 0
                    is_intrace1 = 1
                    prev_azi_theta = cur_azi_theta
                    prev_x, prev_y, prev_v = site_and_v[i - 1]
                    # 计算出当前节点位置
                    min_azi = azi_o1_out
                    max_azi = azi_o1_in
                    cur_x, cur_y = o1_x + r1 * np.cos((min_azi + max_azi) / 2), o1_y + r1 * np.sin(
                        (min_azi + max_azi) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                    times = 0
                    while abs(cur_dist - dist) > np.power(10.0, -5):
                        if cur_dist > dist:
                            max_azi = (max_azi + min_azi) / 2
                        else:
                            min_azi = (max_azi + min_azi) / 2
                        cur_x, cur_y = o1_x + r1 * np.cos((min_azi + max_azi) / 2), o1_y + r1 * np.sin(
                            (min_azi + max_azi) / 2)
                        dx = prev_x - cur_x
                        dy = prev_y - cur_y
                        cur_dist = np.hypot(dx, dy)
                        times += 1
                        if times <= 100 and t == 17:
                            print(cur_dist, min_azi, max_azi, sep=' ')
                    site_and_v[i][0] = cur_x
                    site_and_v[i][1] = cur_y
                    cur_azi_theta = (max_azi + min_azi) / 2

                    # 计算速度
                    k_tangl1 = -1 / np.tan(prev_azi_theta)
                    k_tangl2 = -1 / np.tan(cur_azi_theta)
                    k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                    tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                    tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                    angle_A = np.arctan(tangentA)
                    angle_B = np.arctan(tangentB)
                    cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                    site_and_v[i][2] = cur_v
                    if cur_v > 2.0:
                        print("时间:%d" % t)
                        print("最大速度为%fm/s" % (v_head - 0.01))
                        exit()

            # 上一节点在第一段圆弧
            elif is_intrace1:
                pre_dazi1 = azi_o1_in - cur_azi_theta
                # 上一节点在第一段圆弧，当前节点在第一段圆弧 v19
                if pre_dazi1 > d_azi2_o1:
                    print("上一节点 %d 在第一段圆弧，当前节点 %d 在第一段圆弧 %d" % (i-1, i, t))
                    is_intrace3 = 0
                    is_intrace1 = 1
                    is_intrace2 = 0
                    cur_azi_theta += d_azi2_o1
                    site_and_v[i][0] = o1_x + r1 * np.cos(cur_azi_theta)
                    site_and_v[i][1] = o1_y + r1 * np.sin(cur_azi_theta)
                    site_and_v[i][2] = site_and_v[i-1][2]
                    if site_and_v[i-1][2] > 2.0:
                        print("时间:%d" % t)
                        print("最大速度为%fm/s" % (v_head - 0.01))
                        exit()
                # 上一节点在第一段圆弧，当前节点在盘入螺旋线 v20
                else:
                    print("上一节点 %d 在第一段圆弧，当前节点 %d 在盘入螺旋线 %d" % (i-1, i, t))
                    is_intrace1 = 0
                    is_intrace2 = 0
                    is_intrace3 = 0
                    prev_azi_theta = cur_azi_theta
                    prev_x, prev_y, prev_v = site_and_v[i - 1]
                    # 计算出当前节点位置
                    min_theta = theta_initial_in
                    max_theta = theta_initial_in + np.pi
                    cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                        (max_theta + min_theta) / 2), b * (
                                           max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                        (max_theta + min_theta) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                    while abs(cur_dist - dist) > np.power(10.0, -10):
                        if cur_dist > dist:
                            max_theta = (max_theta + min_theta) / 2
                        else:
                            min_theta = (max_theta + min_theta) / 2
                        cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                            (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                            (max_theta + min_theta) / 2)
                        dx = prev_x - cur_x
                        dy = prev_y - cur_y
                        cur_dist = np.hypot(dx, dy)
                    site_and_v[i][0] = cur_x
                    site_and_v[i][1] = cur_y
                    cur_theta = (max_theta + min_theta) / 2

                    # 计算速度
                    k_tangl1 = -1 / np.tan(prev_azi_theta)
                    k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
                    k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                    tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                    tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                    angle_A = np.arctan(tangentA)
                    angle_B = np.arctan(tangentB)
                    cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                    site_and_v[i][2] = cur_v
                    if cur_v > 2.0:
                        print("时间:%d" % t)
                        print("最大速度为%fm/s" % (v_head - 0.01))
                        exit()
            # 上一节点在盘入螺旋线，当前节点在盘入螺旋线 v21
            else:
                print("上一节点 %d 在盘入螺旋线，当前节点 %d 在盘入螺旋线 %d" % (i-1, i, t))
                is_intrace1 = 0
                is_intrace2 = 0
                is_intrace3 = 0
                prev_theta = cur_theta
                prev_x, prev_y, prev_v = site_and_v[i - 1]
                # 计算出当前节点位置
                min_theta = cur_theta
                max_theta = min_theta + np.pi
                cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                    (max_theta + min_theta) / 2), b * (
                                       max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                    (max_theta + min_theta) / 2)
                dx = prev_x - cur_x
                dy = prev_y - cur_y
                cur_dist = np.hypot(dx, dy)
                while abs(cur_dist - dist) > np.power(10.0, -10):
                    if cur_dist > dist:
                        max_theta = (max_theta + min_theta) / 2
                    else:
                        min_theta = (max_theta + min_theta) / 2
                    cur_x, cur_y = b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.cos(
                        (max_theta + min_theta) / 2), b * (max_theta + min_theta) / 2 / (2 * np.pi) * np.sin(
                        (max_theta + min_theta) / 2)
                    dx = prev_x - cur_x
                    dy = prev_y - cur_y
                    cur_dist = np.hypot(dx, dy)
                site_and_v[i][0] = cur_x
                site_and_v[i][1] = cur_y
                cur_theta = (max_theta + min_theta) / 2

                # 计算速度
                k_tangl1 = (np.sin(prev_theta) + prev_theta * np.cos(prev_theta)) / (
                            np.cos(prev_theta) - prev_theta * np.sin(prev_theta))
                k_tangl2 = (np.sin(cur_theta) + cur_theta * np.cos(cur_theta)) / (
                            np.cos(cur_theta) - cur_theta * np.sin(cur_theta))
                k_sec = (cur_y - prev_y) / (cur_x - prev_x)
                tangentA = (k_sec - k_tangl1) / (1.0 + k_sec * k_tangl1)
                tangentB = (k_tangl2 - k_sec) / (1.0 + k_tangl2 * k_sec)
                angle_A = np.arctan(tangentA)
                angle_B = np.arctan(tangentB)
                cur_v = abs(prev_v * np.cos(angle_A) / np.cos(angle_B))
                site_and_v[i][2] = cur_v
                if cur_v > 2.0:
                    print("时间:%d" % t)
                    print("最大速度为%fm/s" % (v_head - 0.01))
                    exit()

        for i in range(10000):
            dtheta = v_head / 10000 / r
            head_theta += dtheta
            r = b * (head_theta + np.pi) / 2 / np.pi


\end{verbatim}



\section{附录:caculateangle.py}
你也可以在这里放置代码，例如：
\begin{verbatim}
import numpy as np


def angle_head():
    b = 1.7
    dist = 2.86
    theta = 73 * np.pi / 17
    prev_x, prev_y = b * (theta + np.pi) / 2 / np.pi * np.cos(theta), b * (theta + np.pi) / 2 / np.pi * np.sin(theta)
    min_theta = theta
    max_theta = min_theta + np.pi
    cur_theta = (min_theta + max_theta) / 2
    cur_x, cur_y = b * (cur_theta + np.pi) / 2 / np.pi * np.cos(cur_theta), b * (cur_theta + np.pi) / 2 / np.pi * np.sin(cur_theta)
    dx = prev_x - cur_x
    dy = prev_y - cur_y
    cur_dist = np.hypot(dx, dy)
    while abs(cur_dist-dist) > np.power(10.0, -10):
        if cur_dist > dist:
            max_theta = (min_theta + max_theta) / 2
        else:
            min_theta = (min_theta + max_theta) / 2
        cur_theta = (min_theta + max_theta) / 2
        cur_x, cur_y = b * (cur_theta + np.pi) / 2 / np.pi * np.cos(cur_theta), b * (
                    cur_theta + np.pi) / 2 / np.pi * np.sin(cur_theta)
        dx = prev_x - cur_x
        dy = prev_y - cur_y
        cur_dist = np.hypot(dx, dy)
    return cur_theta


def angle_body():
    b = 1.7
    dist = 1.65
    theta = 73 * np.pi / 17
    prev_x, prev_y = b * (theta + np.pi) / 2 / np.pi * np.cos(theta), b * (theta + np.pi) / 2 / np.pi * np.sin(theta)
    min_theta = theta
    max_theta = min_theta + np.pi
    cur_theta = (min_theta + max_theta) / 2
    cur_x, cur_y = b * (cur_theta + np.pi) / 2 / np.pi * np.cos(cur_theta), b * (
                cur_theta + np.pi) / 2 / np.pi * np.sin(cur_theta)
    dx = prev_x - cur_x
    dy = prev_y - cur_y
    cur_dist = np.hypot(dx, dy)
    while abs(cur_dist - dist) > np.power(10.0, -10):
        if cur_dist > dist:
            max_theta = (min_theta + max_theta) / 2
        else:
            min_theta = (min_theta + max_theta) / 2
        cur_theta = (min_theta + max_theta) / 2
        cur_x, cur_y = b * (cur_theta + np.pi) / 2 / np.pi * np.cos(cur_theta), b * (
                cur_theta + np.pi) / 2 / np.pi * np.sin(cur_theta)
        dx = prev_x - cur_x
        dy = prev_y - cur_y
        cur_dist = np.hypot(dx, dy)
    return cur_theta

\end{verbatim}

\end{document}
try something new

CATALOG

FEATURED TAGS
