CN104867172A - 一种实现刚体运动的方法 - Google Patents

Abstract

本发明涉及三维动画设计领域，提供一种实现刚体运动的方法包括：步骤100，采样刚体运动过程N+1个时刻的运动数据；步骤200，通过差商算法对采样的运动数据进行数据处理，得到每个时刻的平移运动的单位切向量和曲率向量，以及旋转运动的单位切向量和曲率向量；步骤300，进行刚体平移运动的构建；步骤400，进行刚体旋转运动的构建；步骤500，根据步骤300中构建的刚体平移运动和步骤400中构建的刚体旋转运动，对刚体整体运动过程进行构建，得到刚体上每点的运动轨迹曲线；步骤600，根据得到的刚体上每点的运动轨迹曲线，使刚体按照运动轨迹曲线进行运动。本发明更接近现实生活中的真实物体运动情景，增强设计的交互性。

Description

一种实现刚体运动的方法

技术领域

本发明涉及三维动画设计领域，尤其涉及一种实现刚体运动的方法。

背景技术

刚体运动原来是物理学中的一个概念。如果一个物体在三维欧氏空间中的运动不改变它的形状及其大小，只改变它在空间中的位置，那么该物体的这种运动称为刚体运动。刚体运动设计在三维动画设计及机器人轨迹规划等相关领域中是一个重要的问题，也是一项基本的任务。

在三维动画设计及机器人轨迹规划过程中，经常会涉及到刚体运动轨迹的设计与实现，如果设计者手工排列刚体运动轨迹上的每一点，显然工作量巨大，某些情况下也并不符合现实生活中物体实际的运动轨迹。在已知某些时刻的运动数据的情况下，如果计算机利用算法能自动生成刚体的整体运动轨迹，完成运动数据的拟合，这将会大大节省设计时间，因而具有重大的现实意义和应用价值。

随着计算机技术的不断发展，计算机辅助几何设计的思想和手段越来越多的融入到了刚体运动设计中。实际上，由于平移运动可以由三维欧氏空间中的参数曲线来表示，因而计算机辅助几何设计中一些经典的样条曲线，例如Bézier样条、B样条以及NURBS曲线，都可以成为有效的平移运动设计工具。另一方面，旋转矩阵R是一个行列式为1的3×3的正交矩阵，其有九个变量，三个自由度。显然，从实际应用的角度出发，旋转矩阵并不适合直接用来描述旋转运动，取而代之的则是与其有1-1对应关系的单位四元素。因此，刚体旋转运动设计可以通过构造单位四元素曲线Q(t)(或者说是三维球面曲线)来实现。然而，球面的非欧结构使得我们无法直接利用计算机辅助几何设计中一些经典的样条曲线。为此，人们一直致力于球面曲线的构造及其性质的研究。球面曲线的构造可以分为两类：一类是将计算机辅助几何设计中一些经典的样条曲线通过映射投影到球面上，这种方法简单直接，但是构造出的球面曲线其速度和加速度波动巨大，可能会引起几何失真，不能满足实际应用的需要。另一类方法是直接在球面上构造曲线，具有代表性的是球面Bézier样条曲线。然而，对于某些初始的旋转运动插值数据，无法构造出球面Bézier插值样条。另外，由于不存在自由参数，所以现有技术利用球面Bézier样条曲线无法进行交互式的旋转运动设计。

发明内容

本发明主要解决现有技术的刚体运动设计可能发生几何失真、对需要拟合的运动数据尤其是旋转运动数据限制过多以及缺乏交互性等技术问题，提出一种实现刚体运动的方法，更接近现实生活中的真实物体运动情景，增强了设计的交互性，因而更具应用价值。

本发明提供了一种实现刚体运动的方法，包括：

步骤100，采样刚体运动过程中N+1个时刻的运动数据。

在刚体实际运动过程中，通过GPS定位设备采样N+1个时刻的运动数据，得到j时刻表示刚***置的三维向量c_j以及表示刚体取向的单位四元素p_j，其中，j＝0，1，...,N。

步骤200，通过差商算法对采样的运动数据进行数据处理，得到每个时刻的平移运动的单位切向量和曲率向量，以及旋转运动的单位切向量和曲率向量。

根据步骤100中的采样的运动数据，通过差商算法近似计算出平移运动j时刻c_j处的单位切向量和曲率向量，以及旋转运动j时刻p_j处的单位切向量和曲率向量

步骤300，进行刚体平移运动的构建。

由于平移运动可以用三维欧氏空间中的一条参数曲线来表示，因而计算机辅助几何设计中一些经典的样条曲线(例如Bézier样条、B样条以及NURBS曲线)都可以成为有效的平移运动设计工具。本发明中，我们采用Bézier样条曲线c(t)对初始给定的平移运动数据进行拟合。

步骤301，利用n次Bézier曲线对相邻两个时刻的平移运动数据进行拟合，得到插值平移运动数据的五次Bézier曲线。

步骤302，将步骤301中的五次Bézier曲线组合成Bézier样条曲线，从而完成平移运动的构建。

步骤301和步骤302的具体过程为：根据j时刻和j+1时刻的平移运动数据求出一段Bézier曲线的六个控制顶点，进而得到由这六个控制顶点确定的Bézier曲线，该曲线插值j时刻和j+1时刻的平移运动数据；再将这N段曲线组合成Bézier样条曲线c(t)，该样条曲线插值所有N+1个时刻的平移运动数据，从而完成平移运动的构建。

步骤400，进行刚体旋转运动的构建，包括以下过程：

步骤401，建立标准型n次广义有理Bézier曲线。

对于整数n≥1，用b₀，b₁，…，b_n表示m维单位球面S^m上n+1个互不相同的点的坐标，w₀，w₁，…，w_n是n+1个正数。对于i＝0，1，...,n-1，令

θ_i：＝∠(b_i，b_i+1)，    (1)

$l_i^0\left(t\right):=\left\{\begin{array}{cc}\frac{{\mathrm{tw}}_{i+1}}{(1-t)w_i},& t\∈\[0,1),\\ \mathrm{\∞},& t=1,\end{array}\right.---\left(2\right)$

$b_i^1\left(t\right):=\frac{\sin \frac{{\mathrm{\θ}}_i}{1+l_i^0\left(t\right)}}{{\mathrm{sin\θ}}_i}{b}_i+\frac{\sin \frac{l_i^0\left(t\right){\mathrm{\θ}}_i}{1+l_i^0\left(t\right)}}{{\mathrm{sin\θ}}_i}{b}_{i+1},t\∈\[0,1\],---\left(3\right)$

$w_i^1\left(t\right):=(1-t)w_i+{\mathrm{tw}}_{i+1},t\∈\[0,1\],---\left(4\right)$

其中，∠(·，·)表示两个向量之间的夹角，表示球面S^m上连接b_i和b_i+1两点的大圆劣弧。

类似地，对于k＝2，3，...，n，i＝0，1，...，n-k，令

${\mathrm{\θ}}_i^{k-1}\left(t\right):=\∠(b_i^{k-1}\left(t\right),b_{i+1}^{k-1}\left(t\right)),t\∈\[0,1\],---\left(5\right)$

$l_i^{k-1}\left(t\right):=\left\{\begin{array}{cc}\frac{{\mathrm{tw}}_{i+1}^{k-1}\left(t\right)}{(1-t)w_i^{k-1}\left(t\right)},& t\∈\[0,1),\\ \mathrm{\∞},& t=1,\end{array}\right.---\left(6\right)$

$b_i^k\left(t\right):=\frac{\sin \frac{{\mathrm{\θ}}_i^{k-1}\left(t\right)}{1+l_i^{k-1}\left(t\right)}}{{\mathrm{sin\θ}}_i^{k-1}\left(t\right)}{b}_i^{k-1}\left(t\right)+\frac{\sin \frac{l_i^{k-1}\left(t\right){\mathrm{\θ}}_i^{k-1}\left(t\right)}{1+l_i^{k-1}\left(t\right)}}{{\mathrm{sin\θ}}_i^{k-1}\left(t\right)}{b}_{i+1}^{k-1}\left(t\right),t\∈\[0,1\],---\left(7\right)$

$w_i^k\left(t\right):=(1-t)w_i^{k-1}\left(t\right)+{\mathrm{tw}}_{i+1}^{k-1}\left(t\right),t\∈\[0,1\],---\left(8\right)$

公式(1)-(8)定义的算法称为广义有理德卡斯特里奥算法。图1为广义有理德卡斯特里奥算法操作过程的示意图。参照图1，由该算法定义的曲线r：[0，1]→S^m:

$r\left(t\right):=b_0^n\left(t\right)$

称为以b₀，b₁，…，b_n为控制顶点，以w₀，w₁，…，w_n为相应权因子的n次广义有理Bézier曲线。通常情况下，令w₀＝w_n＝1，相应的曲线称为标准型n次广义有理Bézier曲线。

步骤402，利用建立的标准型n次广义有理Bézier曲线对相邻两个时刻的旋转运动数据进行拟合，得到插值旋转运动数据的标准型五次广义有理Bézier曲线。

在本发明中，对于标准型n次广义有理Bézier曲线，使n取5。设j＝0，1，...，N-1，单位四元素p_j和p_j+1分别表示刚体在j时刻和j+1时刻的取向，即刚体在两个相邻时刻的取向，和分别表示p_j处的单位切向量和曲率向量，和分别表示p_j+1处的单位切向量和曲率向量。通过以下公式计算标准型五次广义有理Bézier曲线r_j：[0，1]→S³的控制顶点：

b_6j＝p_j，b_6j+5＝p_j+1,    (9)

$b_{6j+1}={\mathrm{cos\θ}}_{6j}{b}_{6j}+{\mathrm{sin\θ}}_{6j}{v}_j^{\left(1\right)},---\left(10\right)$

$b_{6j+4}={\mathrm{cos\θ}}_{6j+4}{b}_{6j+5}-{\mathrm{sin\θ}}_{6j+4}{v}_{j+1}^{\left(1\right)},---\left(11\right)$

$b_{6j+2}={\mathrm{cos\θ}}_{6j+1}{b}_{6j+1}+\frac{w_{6j+1}{\mathrm{sin\θ}}_{6j+1}}{w_{6j+2}{\mathrm{\θ}}_{6j+1}}{\stackrel{\·}{b}}_{6j+1}^1\left(0\right),---\left(12\right)$

$b_{6j+3}={\mathrm{cos\θ}}_{6j+3}{b}_{6j+4}-\frac{w_{6j+4}{\mathrm{sin\θ}}_{6j+3}}{w_{6j+3}{\mathrm{\θ}}_{6j+3}}{\stackrel{\·}{b}}_{6j+3}^1\left(1\right),---\left(13\right)$

其中,

${\mathrm{\θ}}_{6j}=\frac{{\mathrm{\β}}_j^{\left(1\right)}}{5w_{6j+1}},{\mathrm{\θ}}_{6j+4}=\frac{{\mathrm{\β}}_{j+1}^{\left(1\right)}}{5w_{6j+4}},---\left(14\right)$

${\mathrm{\θ}}_{6j+1}=\frac{w_{6j+1}||{\stackrel{\·}{b}}_{6j+1}^1\left(0\right)||}{w_{6j+2}},{\mathrm{\θ}}_{6j+3}=\frac{w_{6j+4}||{\stackrel{\·}{b}}_{6j+3}^1\left(1\right)||}{w_{6j+3}},---\left(15\right)$

$\begin{array}{c}{\stackrel{\&CenterDot;}{b}}_{6j+1}^1\left(0\right)=\frac{{\mathrm{sin\&theta;}}_{6j}}{n(n-1)w_{6j+1}{\mathrm{\&theta;}}_{6j}}{c}_j-\frac{2w_{6j+1}-2n{\left(w_{6j+1}\right)}^2+\left(n-1\right)w_{6j+2}}{n-1}{\stackrel{\&CenterDot;}{b}}_{6j}^1\left(1\right)\\ -\frac{<c_j,b_{6j+1}>}{n\left(n-1\right){\mathrm{\&theta;}}_{6j}}(\frac{1}{{\left(w_1\right)}^2{\mathrm{\&theta;}}_{6j}}{\stackrel{\&CenterDot;}{b}}_{6j}^1\left(0\right)-\frac{1}{{\mathrm{sin\&theta;}}_{6j}}{\stackrel{\&CenterDot;}{b}}_{6j}^1\left(1\right)),\end{array}---\left(16\right)$

$\begin{array}{c}{\stackrel{\&CenterDot;}{b}}_{6j+3}^1\left(1\right)=-\frac{{\mathrm{sin\&theta;}}_{6j+4}}{n(n-1)w_{6j+4}{\mathrm{\&theta;}}_{6j+4}}{c}_{j+1}\\ -\frac{2w_{6j+4}-2n{\left(w_{6j+4}\right)}^2+\left(n-1\right)w_{6j+3}}{n-1}{\stackrel{\&CenterDot;}{b}}_{6j+4}^1\left(0\right)\\ -\frac{<c_{j+1},b_{6j+4}>}{n\left(n-1\right){\mathrm{\&theta;}}_{6j+4}}\left(\frac{{\stackrel{\&CenterDot;}{b}}_{6j+4}^1\left(1\right)}{{\left(w_{6j+4}\right)}^2{\mathrm{\&theta;}}_{n-1}}-\frac{{\stackrel{\&CenterDot;}{b}}_{6j+4}^1\left(0\right)}{{\mathrm{sin\&theta;}}_{6j+4}}\right),\end{array}---\left(17\right)$

$c_j={\left({\mathrm{\&beta;}}_j^{\left(1\right)}\right)}^2{p}_j+{\mathrm{\&beta;}}_j^{\left(2\right)}{v}_j^{\left(1\right)}+{\left({\mathrm{\&beta;}}_j^{\left(1\right)}\right)}^2{v}_j^{\left(2\right)},---\left(18\right)$

$c_{j+1}={\left({\mathrm{\&beta;}}_{j+1}^{\left(1\right)}\right)}^2{p}_{j+1}+{\mathrm{\&beta;}}_{j+1}^{\left(2\right)}{v}_{j+1}^{\left(1\right)}+{\left({\mathrm{\&beta;}}_{j+1}^{\left(1\right)}\right)}^2{v}_{j+1}^{\left(2\right)},---\left(19\right)$

其中，表示形状参数。由此构造出的曲线r_j(t)按照如下方式插值旋转运动数据：

r(0)＝p_j，

r(1)＝p_j+1，

$\stackrel{\&CenterDot;}{r}\left(0\right)={\mathrm{\&beta;}}_j^{\left(1\right)}{v}_j^{\left(1\right)},$

$\stackrel{\&CenterDot;}{r}\left(1\right)={\mathrm{\&beta;}}_{j+1}^{\left(1\right)}{v}_{j+1}^{\left(1\right)},$

$\stackrel{\&CenterDot;\&CenterDot;}{r}\left(0\right)={\mathrm{\&beta;}}_j^{\left(2\right)}{v}_j^{\left(1\right)}+{\left({\mathrm{\&beta;}}_j^{\left(1\right)}\right)}^2{v}_j^{\left(2\right)},$

$\stackrel{\&CenterDot;\&CenterDot;}{r}\left(1\right)={\mathrm{\&beta;}}_{j+1}^{\left(2\right)}{v}_{j+1}^{\left(1\right)}+{\left({\mathrm{\&beta;}}_{j+1}^{\left(1\right)}\right)}^2{v}_{j+1}^{\left(2\right)}.$

步骤403，将步骤402中的标准型五次广义有理Bézier曲线组合成广义有理Bézier样条曲线，从而完成旋转运动的构建。

设r_j(t)(j＝0，1，...，N-1)是步骤402中得到的标准型五次广义有理Bézier曲线，通过以下公式构造广义有理Bézier样条曲线

$\tilde{r}\left(t\right):=r_j(t-j),t\&Element;\&lsqb;j,j+1\&rsqb;,$

则样条曲线插值初始给定的旋转运动数据，从而完成旋转运动的构建。

在上述过程中，我们采用步骤401中建立的由控制顶点坐标确定的标准型n次广义有理Bézier曲线对初始给定的N+1个时刻的旋转运动数据进行拟合。具体的，首先在步骤402中根据j时刻和j+1时刻的旋转运动数据，求出一段标准型五次广义有理Bézier曲线的六个控制顶点；然后使n取5，并利用步骤401中的定义方法得到由这六个控制顶点确定的标准型五次广义有理Bézier曲线r_j(t)(j＝0，1，...，N-1)，该曲线插值j时刻和j+1时刻的旋转运动数据；最后在步骤403中将这N段曲线组合成广义有理Bézier样条曲线该样条曲线插值所有N+1个时刻的旋转运动数据，从而完成旋转运动的构建。

步骤500：根据步骤300中构建出的刚体平移运动和步骤400中构建的刚体旋转运动，对刚体整体运动过程进行构建，得到刚体上每点的运动轨迹曲线。

对于任意时刻t，首先根据公式(20)将四元素转化为旋转矩阵R(t)。具体对应关系如下：

$R=\frac{1}{q_0^2+q_1^2+q_2^2+q_3^2}\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_1{q}_3-q_0{q}_2\right)\\ 2\left(q_1{q}_2+q_0{q}_3\right)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3+q_0{q}_2\right)& 2\left(q_2{q}_3+q_0{q}_1\right)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right).---\left(20\right)$

然后通过公式(21)对刚体整体运动过程进行构建：

$p\left(t\right)=c\left(t\right)+R\left(t\right)\hat{p},---\left(21\right)$

其中，在刚体上建立一个坐标系，称为移动坐标系，表示刚体上任意一点在移动坐标系下的坐标，在刚体运动所在三维欧氏空间中建立一个坐标系，称为世界坐标系，p(t)表示运动过程中t时刻点在世界坐标系下的坐标，平移运动c(t)表示任意时刻刚体在三维欧氏空间中的位置，旋转运动R(t)则表示刚体在每个位置上的取向。根据公式(21)计算出刚体上任意一点在世界坐标系下的坐标p(t)。由此，完成了刚体整体运动过程的构建，得到刚体上每点的运动轨迹曲线。

步骤600，根据得到的刚体上每点的运动轨迹曲线，使刚体按照运动轨迹曲线进行运动。

本发明提供的一种实现刚体运动的方法，在已知某些时刻刚体平移运动数据和旋转运动数据的基础上，利用Bézier样条曲线和本发明中的广义有理Bézier样条曲线，可以分别拟合出刚体平移运动轨迹和旋转运动轨迹，进而计算机可以自动拟合出刚体整体运动轨迹，这样就避免了设计者手工排列刚体运动轨迹上的每一点的巨大工作量。本发明中的广义有理Bézier曲线是直接在球面上构造的，因此可以避免投影映射引起的几何失真的情况发生，使得拟合结果更加符合现实生活中物体实际的运动轨迹。广义有理Bézier曲线的权因子和拟合过程中引入的形状参数使得本发明适用于任意需要拟合的旋转运动数据，取消了对运动数据的限制，应用范围更广。另外设计者还可以根据设计需求，通过修改权因子和形状参数的取值对拟合出的刚体运动轨迹做进一步的调整，增强了设计的交互性，更具实际应用价值。

附图说明

图1为广义有理德卡斯特里奥算法操作过程的示意图；

图2为本发明实施例提供的实现刚体运动的方法的实现流程图；

图3为本发明实施例提供的刚体旋转运动构建方法的实现流程图。

具体实施方式

为使本发明解决的技术问题、采用的技术方案和达到的技术效果更加清楚，下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部内容。

图2为本发明实施例提供的实现刚体运动的方法的实现流程图。本实施例中以飞行器为例对实现刚体运动的方法进行说明。如图2所示，本发明实施例提供的实现刚体运动的方法包括：

步骤100，采样飞行器飞行过程中N+1个时刻的运动数据。

在飞行器实际飞行过程中，通过GPS定位设备采样N+1个时刻的运动数据，得到j时刻表示飞行器位置的三维向量c_j以及表示飞行器取向的单位四元素p_j，其中，j＝0，1，...,N。

步骤200，对采样的运动数据进行数据处理。

根据步骤100中的采样的运动数据，通过差商算法近似计算出平移运动j时刻c_j处的单位切向量和曲率向量，以及旋转运动j时刻p_j处的单位切向量和曲率向量

步骤300，进行飞行器平移运动的构建。

由于平移运动可以用三维欧氏空间中的一条参数曲线来表示，因而计算机辅助几何设计中一些经典的样条曲线(例如Bézier样条、B样条以及NURBS曲线)都可以成为有效的平移运动设计工具。本发明中，我们采用Bézier样条曲线c(t)对初始给定的平移运动数据进行拟合。具体的，根据j时刻和j+1时刻的平移运动数据求出一段Bézier曲线的六个控制顶点，进而得到由这六个控制顶点确定的Bézier曲线，该曲线插值j时刻和j+1时刻的平移运动数据；再将这N段曲线组合成Bézier样条曲线c(t)，该样条曲线插值所有N+1个时刻的平移运动数据，从而完成平移运动的构建。

步骤400，进行飞行器旋转运动的构建。

图3为本发明实施例提供的刚体旋转运动构建方法的实现流程图，如图3所示，飞行器旋转运动的构建具体包括以下过程：

步骤401，建立标准型n次广义有理Bézier曲线。

步骤402，利用建立的标准型n广义有理Bézier曲线对相邻两个时刻的旋转运动数据进行拟合，得到插值旋转运动数据的标准型五次广义有理Bézier曲线。

步骤403，将步骤402中的标准型五次广义有理Bézier曲线组合成广义有理Bézier样条曲线，从而完成旋转运动的构建。

其中，步骤402和步骤403的具体过程为：

1、令j＝0；

2、根据公式(9)计算出五次广义有理Bézier曲线r_j：[0，1]→S³的控制顶点b_6j，b_6j+5；

3、采用自适应的方法，选取权因子w_6j+1,w_6j+4和形状参数根据公式(14)计算出θ_6j,θ_6j+4；根据公式(10)-(11)计算出曲线r_j(t)的控制顶点b_6j+1，b_6j+4；

4、采用自适应的方法，选取权因子w_6j+2,w_6j+3和形状参数根据公式(15)-(19)计算出θ_6j+1,θ_6j+3；根据公式(12)-(13)计算出曲线r_j(t)的控制顶点b_6j+2，b_6j+3；

5、利用上面求出的b_6j，b_6j+5、b_6j+1，b_6j+4和b_6j+2，b_6j+3，根据广义有理德卡斯特里奥算法计算曲线r_j(t)；

6、如果旋转运动数据全部拟合完毕，则按照步骤7进行操作；否则，将j的取值加1,并按照步骤2进行操作；

7、设r_j(t)(j＝0，1，...，N-1)是步骤5中得到的标准型五次广义有理Bézier曲线，通过以下公式构造广义有理Bézier样条曲线

$\tilde{r}\left(t\right):=r_j(t-j),t\&Element;\&lsqb;j,j+1\&rsqb;,$

则样条拟合初始给定的旋转运动数据，从而完成旋转运动的构建。

步骤500：根据步骤300中构建出的飞行器平移运动和步骤400中构建出的飞行器旋转运动，对飞行器整体飞行过程进行构建，得到飞行器上每点的运动轨迹曲线。

对于任意时刻t，首先根据公式(20)将四元素转化为旋转矩阵R(t)；再根据公式(21)对飞行器整体飞行过程进行构建，得到飞行器上每点的运动轨迹曲线。

步骤600，根据得到的飞行器上每点的运动轨迹曲线，使飞行器按照运动轨迹曲线进行运动。

本实施例提供的实现刚体运动的方法，是将刚体运动分为旋转运动和平移运动两部分来进行构建的。其中旋转运动的构建，首先是通过将有理德卡斯特里奥算法中的直线段用大圆劣弧代替而得到广义有理德卡斯特里奥算法，并根据该算法建立了标准型广义有理Bézier曲线；再依据标准型广义有理Bézier曲线端点处的性质，规定了标准型五次广义有理Bézier曲线控制顶点的选取方式，使得曲线拟合初始给定的旋转运动数据，其中的权因子和形状参数的选取，采用自适应的方式。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (7)

1.一种实现刚体运动的方法，其特征在于，所述实现刚体运动的方法包括：

步骤100，采样刚体运动过程中N+1个时刻的运动数据；

步骤200，通过差商算法对采样的运动数据进行数据处理，得到每个时刻的平移运动的单位切向量和曲率向量，以及旋转运动的单位切向量和曲率向量；

步骤300，进行刚体平移运动的构建；

步骤400，进行刚体旋转运动的构建，包括以下过程：

步骤401，建立标准型n次广义有理Bézier曲线；

步骤402，利用建立的标准型n次广义有理Bézier曲线对相邻两个时刻的旋转运动数据进行拟合，得到插值旋转运动数据的标准型五次广义有理Bézier曲线；

步骤403，将步骤402中的标准型五次广义有理Bézier曲线组合成广义有理Bézier样条曲线，从而完成旋转运动的构建；

步骤500，根据步骤300中构建的刚体平移运动和步骤400中构建的刚体旋转运动，对刚体整体运动过程进行构建，得到刚体上每点的运动轨迹曲线；

步骤600，根据得到的刚体上每点的运动轨迹曲线，使刚体按照运动轨迹曲线进行运动。

2.根据权利要求1所述的实现刚体运动的方法，其特征在于，所述采样刚体运动过程中N+1个时刻的运动数据，包括：

在刚体实际运动过程中，通过GPS定位设备采样刚体运动过程中N+1个时刻的运动数据，得到表示刚***置的三维向量序列以及表示刚体取向的单位四元素序列。

3.根据权利要求1所述的实现刚体运动的方法，其特征在于，所述进行刚体平移运动的构建，包括以下子步骤：

步骤301，利用n次Bézier曲线对相邻两个时刻的平移运动数据进行拟合，得到插值平移运动数据的五次Bézier曲线；

步骤302，将步骤301中的五次Bézier曲线组合成Bézier样条曲线，从而完成平移运动的构建。

4.根据权利要求1所述的实现刚体运动的方法，其特征在于，所述建立标准型n次广义有理Bézier曲线，包括：

对于整数n≥1，用b₀，b₁，…，b_n表示m维单位球面S^m上n+1个互不相同的点的坐标，w₀，w₁，…，w_n是n+1个正数，对于i＝0，1，…,n-1，令

θ_i：＝∠(b_i，b_i+1)，   (1)

$l_i^0\left(t\right):=\left\{\begin{array}{cc}\frac{{\mathrm{tw}}_{i+1}}{(1-t)w_i},& t\&Element;\&lsqb;0,1),\\ \mathrm{\&infin;},& t=1,\end{array}\right.---\left(2\right)$

$b_i^1\left(t\right):=\frac{\sin \frac{{\mathrm{\&theta;}}_i}{1+l_i^0\left(t\right)}}{{\mathrm{sin\&theta;}}_i}{b}_i+\frac{\sin \frac{l_i^0\left(t\right){\mathrm{\&theta;}}_i}{1+l_i^0\left(t\right)}}{{\mathrm{sin\&theta;}}_i}{b}_{i+1},t\&Element;\&lsqb;0,1\&rsqb;,---\left(3\right)$

$w_i^1\left(t\right):=(1-t)w_i+{\mathrm{tw}}_{i+1},t\&Element;\&lsqb;0,1\&rsqb;,---\left(4\right)$ 其中，∠(·，·)表示两个向量之间的夹角，表示球面S^m上连接b_i和b_i+1两点的大圆劣弧；

对于k＝2，3，…，n，i＝0，1，…，n-k，令

${\mathrm{\&theta;}}_i^{k-1}\left(t\right):=\&angle;\left(b_i^{k-1}\right(t),b_{i+1}^{k-1}\left(t\right)),t\&Element;\&lsqb;0,1\&rsqb;,---\left(5\right)$

$l_i^{k-1}\left(t\right):=\left\{\begin{array}{cc}\frac{{\mathrm{tw}}_{i+1}^{k-1}\left(t\right)}{(1-t)w_i^{k-1}\left(t\right)},& t\&Element;\&lsqb;0,1),\\ \mathrm{\&infin;},& t=1,\end{array}\right.---\left(6\right)$

$b_i^k\left(t\right):=\frac{\sin \frac{{\mathrm{\&theta;}}_i^{k-1}\left(t\right)}{1+l_i^{k-1}\left(t\right)}}{{\mathrm{sin\&theta;}}_i^{k-1}\left(t\right)}{b}_i^{k-1}\left(t\right)+\frac{\sin \frac{l_i^{k-1}\left(t\right){\mathrm{\&theta;}}_i^{k-1}\left(t\right)}{1+l_i^{k-1}\left(t\right)}}{{\mathrm{sin\&theta;}}_i^{k-1}\left(t\right)}{b}_{i+1}^{k-1}\left(t\right),t\&Element;\&lsqb;0,1\&rsqb;,---\left(7\right)$

$w_i^k\left(t\right):=(1-t)w_i^{k-1}\left(t\right)+{\mathrm{tw}}_{i+1}^{k-1}\left(t\right),t\&Element;\&lsqb;0,1\&rsqb;,---\left(8\right)$ 由公式(1)-(8)定义的曲线r：[0，1]→S^m:

$r\left(t\right):=b_0^n\left(t\right)$

称为以b₀，b₁，…，b_n为控制顶点，以w₀，w₁，…，w_n为相应权因子的n次广义有理Bézier曲线；当w₀＝w_n＝1时，为标准型n次广义有理Bézier曲线。

5.根据权利要求1所述的实现刚体运动的方法，其特征在于，利用建立的标准型n次广义有理Bézier曲线对相邻两个时刻的旋转运动数据进行拟合，得到插值旋转运动数据的标准型五次广义有理Bézier曲线，包括以下过程：

通过以下公式计算五次广义有理Bézier曲线r_j：[0，1]→S³的控制顶点：

b_6j＝p_j，b_6j+5＝p_j+1,    (9)

$b_{6j+1}={\mathrm{cos\&theta;}}_{6j}{b}_{6j}+{\mathrm{sin\&theta;}}_{6j}{v}_j^{\left(1\right)},---\left(10\right)$

$b_{6j+4}={\mathrm{cos\&theta;}}_{6j+4}{b}_{6j+5}-{\mathrm{sin\&theta;}}_{6j+4}{v}_{j+1}^{\left(1\right)},---\left(11\right)$

$b_{6j+2}={\mathrm{cos\&theta;}}_{6j+1}{b}_{6j+1}+\frac{w_{6j+1}{\mathrm{sin\&theta;}}_{6j+1}}{w_{6j+2}{\mathrm{\&theta;}}_{6j+1}}{\stackrel{\&CenterDot;}{b}}_{6j+1}^1\left(0\right),---\left(12\right)$

$b_{6j+3}={\mathrm{cos\&theta;}}_{6j+3}{b}_{6j+4}-\frac{W_{6j+4}{\mathrm{sin\&theta;}}_{6j+3}}{W_{6j+3}{\mathrm{\&theta;}}_{6j+3}}{\stackrel{\&CenterDot;}{b}}_{6j+3}^1\left(1\right),---\left(13\right)$ 其中，j＝0，1，…,N-1，单位四元素p_j和p_j+1分别表示刚体在j时刻和j+1时刻的取向，和分别表示p_j处的单位切向量和曲率向量，和分别表示p_j+1处的单位切向量和曲率向量；

${\mathrm{\&theta;}}_{6j}=\frac{{\mathrm{\&beta;}}_j^{\left(1\right)}}{5w_{6j+1}},{\mathrm{\&theta;}}_{6j+4}=\frac{{\mathrm{\&beta;}}_{j+1}^{\left(1\right)}}{5w_{6j+4}},---\left(14\right)$

${\mathrm{\&theta;}}_{6j+1}=\frac{w_{6j+1}\left|\right|{\stackrel{\&CenterDot;}{b}}_{6j+1}^1\left(0\right)\left|\right|}{w_{6j+2}},{\mathrm{\&theta;}}_{6j+3}=\frac{w_{6j+4}\left|\right|{\stackrel{\&CenterDot;}{b}}_{6j+3}^1\left(1\right)\left|\right|}{w_{6j+3}},---\left(15\right)$

$\begin{array}{c}{\stackrel{\&CenterDot;}{b}}_{6j+1}^1\left(0\right)=\frac{{\mathrm{sin\&theta;}}_{6j}}{n\left(n-1\right)w_{6j+1}{\mathrm{\&theta;}}_{6j}}{c}_j-\frac{2w_{6j+1}-2n{\left(w_{6j+1}\right)}^2+\left(n-1\right)w_{6j+2}}{n-1}{\stackrel{\&CenterDot;}{b}}_{6j}^1\left(1\right)\\ -\frac{<c_j,b_{6j+1}>}{n\left(n-1\right){\mathrm{\&theta;}}_{6j}}\left(\frac{1}{{\left(w_1\right)}^2{\mathrm{\&theta;}}_{6j}}{\stackrel{\&CenterDot;}{b}}_{6j}^1\left(0\right)-\frac{1}{{\mathrm{sin\&theta;}}_{6j}}{\stackrel{\&CenterDot;}{b}}_{6j}^1\left(1\right)\right),\end{array}---\left(16\right)$

$\begin{array}{c}{\stackrel{\&CenterDot;}{b}}_{6j+3}^1\left(1\right)=-\frac{{\mathrm{sin\&theta;}}_{6j+4}}{n\left(n-1\right)w_{6j+4}{\mathrm{\&theta;}}_{6j+4}}{c}_{j+1}\\ -\frac{2w_{6j+4}-2n{\left(w_{6j+4}\right)}^2+\left(n-1\right)w_{6j+3}}{n-1}{\stackrel{\&CenterDot;}{b}}_{6j+4}^1\left(0\right)\\ -\frac{<c_{j+1},b_{6j+4}>}{n\left(n-1\right){\mathrm{\&theta;}}_{6j+4}}\left(\frac{{\stackrel{\&CenterDot;}{b}}_{6j+4}^1\left(1\right)}{{\left(w_{6j+4}\right)}^2{\mathrm{\&theta;}}_{n-1}}-\frac{{\stackrel{\&CenterDot;}{b}}_{6j+4}^1\left(0\right)}{{\mathrm{sin\&theta;}}_{6j+4}}\right),\end{array}---\left(17\right)$

$c_j={\left({\mathrm{\&beta;}}_j^{\left(1\right)}\right)}^2{p}_j+{\mathrm{\&beta;}}_j^{\left(2\right)}{v}_j^{\left(1\right)}+{\left({\mathrm{\&beta;}}_j^{\left(1\right)}\right)}^2{v}_j^{\left(2\right)},---\left(18\right)$

$c_{j+1}={\left({\mathrm{\&beta;}}_{j+1}^{\left(1\right)}\right)}^2{p}_{j+1}+{\mathrm{\&beta;}}_{j+1}^{\left(2\right)}{v}_{j+1}^{\left(1\right)}+{\left({\mathrm{\&beta;}}_{j+1}^{\left(1\right)}\right)}^2{v}_{j+1}^{\left(2\right)},---\left(19\right)$

其中， ${\mathrm{\&beta;}}_j^{\left(1\right)},{\mathrm{\&beta;}}_j^{\left(2\right)},{\mathrm{\&beta;}}_{j+1}^{\left(1\right)},{\mathrm{\&beta;}}_{j+1}^{\left(2\right)}$ 表示形状参数。

6.根据权利要求1所述的实现刚体运动的方法，其特征在于，将步骤402中的标准型五次广义有理Bézier曲线组合成广义有理Bézier样条曲线，包括：

通过以下公式构造广义有理Bézier样条曲线

$\tilde{r}\left(t\right):=r_j(t-j),t\&Element;\&lsqb;j,j+1\&rsqb;,$

其中，j＝0，1，…,N-1，则样条曲线插值初始给定的旋转运动数据，从而完成旋转运动的构建。

7.根据权利要求1所述的实现刚体运动的方法，其特征在于，根据步骤300中构建出的刚体平移运动和步骤400中构建出的刚体旋转运动，对刚体整体运动过程进行构建，得到刚体上每点的运动轨迹曲线，包括：

对于任意时刻t，首先根据公式(20)将样条转化为旋转矩阵R(t),具体对应关系如下：

$R=\frac{1}{q_0^2+q_1^2+q_2^2+q_3^2}\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right).---\left(20\right)$

通过公式(21)对刚体整体运动过程进行构建：

$p\left(t\right)=c\left(t\right)+R\left(t\right)\hat{p},---\left(21\right)$ 其中，表示刚体上任意一点在移动坐标系下的坐标，p(t)表示运动过程中t时刻点在世界坐标系下的坐标，平移运动c(t)表示任意时刻刚体在三维欧氏空间中的位置，旋转运动R(t)则表示刚体在每个位置上的取向，根据公式(21)计算出刚体上任意一点在世界坐标系下的坐标p(t)。