CN102721409B - 一种基于车身控制点的移动车辆三维运动轨迹的测定方法 - Google Patents

Abstract

本发明公开了一种基于车身控制点的移动车辆三维运动轨迹的测定方法，该测定方法包括以下步骤：10.建立车身控制点数据库；20．获得移动车辆的车身控制点的三维坐标；30.测定不同时刻的移动车辆的车身三维坐标系与图像空间坐标系的转换关系；40.测定移动车辆的三维运动轨迹和姿态变化。该测定方法利用固定摄影摄像设备，基于移动车辆上的车身控制点，对移动车辆进行近景摄影测量，能够方便实现车辆在行驶过程中车速和姿态的准确测定。

Description

一种基于车身控制点的移动车辆三维运动轨迹的测定方法

技术领域

本发明涉及一种移动车辆三维运动轨迹的测定方法，具体来说，涉及一种基于车身控制点的移动车辆三维运动轨迹的测定方法。

背景技术

交通系统在车辆管理方面需要对车辆速度进行监测。传统测量车速的方法有：第一种方法是配合两个地感线圈进行测量；第二种方法是应用多普勒效应进行雷达测量；第三种方法是基于视频图像对车速测量。第一种方法受到线圈必须固定的限制，第二种方法的缺点是由于雷达本身的不足导致车速可能与车辆不匹配。基于视频图像的车速测量方法一般可以分为单目视频图像测量和双目视频图像测量，其中基于双目视频图像的测量方法受到摄影摄像设备布设位置要求较高的限制，同时需要在测速位置布设一定数量的空间三维控制点，实际很少采用。基于单目视频图像的车速测量根据不同时刻车辆在图像上的位置变化、利用数字图像处理方法、由车辆移动的像素值的变化转化为空间位置的变化，该方法受图像成像的精度限制，该方法测速的精度较低。

发明内容

技术问题：本发明要解决的技术问题是：提供一种基于车身控制点的移动车辆三维运动轨迹的测定方法，该测定方法利用固定摄影摄像设备，基于移动车辆上的车身控制点，对移动车辆进行近景摄影测量，能够方便实现车辆在行驶过程中车速和姿态的准确测定。

技术方案：为解决上述技术问题，本发明采用一种基于车身控制点的移动车辆三维运动轨迹的测定方法，该测定方法包括以下步骤：

10.建立车身控制点数据库：

101.选择车身控制点：对每种型号的车辆，在车身外表面选择n个车身特征点，作为车身控制点，n个车身特征点不处于同一平面内，其中，n为整数，且n≥6，

102.建立车身三维坐标系：车身三维坐标系以一个车身控制点为原点，以车辆前进方向为Z轴，垂直车辆前进方向的左侧为X轴，垂直车辆前进方向的指向天顶方向为Y轴，X、Y、Z轴构成右手坐标系；

103.测量车身控制点的三维坐标：用几何测量的方法，测量每个车身控制点的三维坐标(X_i、Y_i、Z_i)，其中，1≤i≤n；

104.建立车身控制点数据库：对每种型号的车辆分别按照步骤101-103进行测量，然后将测量结果输入数据库，构成车身控制点数据库；

20.获得移动车辆的车身控制点的三维坐标：

201.建立图像空间坐标系：图像空间坐标系以固定摄影设备物镜中心S点为原点，摄影主光轴的反方向为Z轴，平行于图像平面水平方向为X轴的右手坐标系；

202.利用固定摄影摄像设备，连续拍摄移动车辆的视频或像片；

203.在步骤202拍摄的像片中提取t₁时刻的像片P₁和t₂时刻的像片P₂，根据该移动车辆型号从步骤104建立的车身控制点数据库中读取该移动车辆的车身控制点的三维坐标(X_i、Y_i、Z_i)，其中，t₁不等于t₂，1≤i≤n；

30.测定不同时刻的移动车辆的车身三维坐标系与图像空间坐标系的转换关系：利用移动车辆在t₁时刻和t₂时刻的车身控制点，测定固定摄影设备物镜中心S在t₁时刻和t₂时刻车身坐标系中的坐标，以及t₁时刻和t₂时刻的车身三维坐标系与图像空间坐标系之间的姿态角，利用姿态角构成旋转矩阵；

40.测定移动车辆的三维运动轨迹和姿态变化：

401.测定移动车辆的三维运动轨迹：t₂时刻的车身三维坐标系的原点相对于t₁时刻的车身三维坐标系中坐标，就是移动车辆的三维运动轨迹，即

$\left[\begin{array}{c}\mathrm{\ΔX}\\ \mathrm{\ΔY}\\ \mathrm{\ΔZ}\end{array}\right]=-R_1{R}_2^T\left[\begin{array}{c}{X}_{S2}\\ Y_{S2}\\ Z_{S2}\end{array}\right]+\left[\begin{array}{c}{X}_{S1}\\ Y_{S1}\\ Z_{S1}\end{array}\right]$ 式(1)

在式(1)中，ΔX、ΔY、ΔZ表示移动车辆的三维运动轨迹，其中ΔX表示车辆的左右位移量，ΔY表示车辆的上下位移量，ΔZ表示车辆的前进或后退的位移量；移动车辆的三维绝对位移

402.测定移动车辆的姿态变化：

第一张像片P₁与第二张像片P₂在车身三维坐标系之间的旋转矩阵可得R中的A1、A2、A3、B1、B2、B3、C1、C2、C3参数，即式(2)

$R=R_2{R}_1^T=\left[\begin{array}{ccc}{A}_1& A_2& A_3\\ B_1& B_2& B_3\\ C_1& C_2& C_3\end{array}\right]$ 式(2)

根据式(3)可得移动车辆的姿态变化量(Φ、Ω、K)，

$\left.\begin{array}{c}\tan \mathrm{\Φ}=-\frac{A_3}{C_3}\\ \sin \mathrm{\Ω}=-B_3\\ \tan K=\frac{B_1}{B_2}\end{array}\right\}$ 式(3)

在式(3)中，Φ表示车辆的左右转向角、Ω表示上下俯仰角、K表示车辆的左右倾角。

有益效果：与现有技术相比，本发明具有以下有益效果：

1.对移动车辆的三维运动轨迹可以实现准确测量。本发明的测定方法以车身特征部位为控制点，在利用固定摄影摄像设备在固定位置对移动车辆连续拍摄，根据提取一定时间间隔的两张像片，测算出该时间间隔的车辆的三维移动轨迹。该方法基于数字摄影测量的基本原理，利用严密的直接线性变换模型，由三维车身控制点精确计算不同时刻的车辆位置，可以同时计算车辆的移动速度、移动方位和空间姿态。这样更加全面准确的测量出移动车辆的三维运动轨迹。

2.实施便利，适用性强。该方法的实施现场只需一台摄影摄像设备，根据编制的专业软件和车身控制点数据库，就可以实现对移动车辆三维轨迹的精确计算。

附图说明

图1是本发明的流程图。

具体实施方式

下面结合附图，对本发明的技术方案进行详细的说明。

如图1所示，本发明的一种基于车身控制点的移动车辆三维运动轨迹的测定方法，该测定方法包括以下步骤：

10.建立车身控制点数据库：

101.选择车身控制点：对每种型号的车辆，在车身外表面选择n个车身特征点，作为车身控制点，n个车身特征点不处于同一平面内，其中，n为整数，且n≥6。

在步骤101中，n个车身特征点位于车身外表面，且以位于车身周边为佳。

102.建立车身三维坐标系：车身三维坐标系以一个车身控制点为原点，以车辆前进方向为Z轴，垂直车辆前进方向的左侧为X轴，垂直车辆前进方向的指向天顶方向为Y轴，X、Y、Z轴构成右手坐标系。

在步骤102中，车身三维坐标系的原点以位于车身前部中间位置为佳，例如，位于车身前部的换气格栅处。

103.测量车身控制点的三维坐标：用几何测量的方法，测量每个车身控制点的三维坐标(X_i、Y_i、Z_i)，其中，1≤i≤n。也就是测量每个车身控制点在自身的车身三维坐标系中的坐标。

104.建立车身控制点数据库：对每种型号的车辆分别按照步骤101-103进行测量，然后将测量结果输入数据库，构成车身控制点数据库。车身控制点数据库中包含每种型号车辆的车身控制点的三维坐标。

20.获得移动车辆的车身控制点的三维坐标：

201.建立图像空间坐标系：图像空间坐标系以固定摄影设备物镜中心S点为原点，摄影主光轴的反方向为Z轴，平行于图像平面水平方向为X轴的右手坐标系。

固定摄影设备是指该摄影设备固定在某个地方，其物镜也不能移动。

202.利用固定摄影摄像设备，连续拍摄移动车辆的视频或像片。

203.在步骤202拍摄的像片中提取t₁时刻的像片P₁和t₂时刻的像片P₂，根据该移动车辆型号从步骤104建立的车身控制点数据库中读取该移动车辆的车身控制点的三维坐标(X_i、Y_i、Z_i)，其中，t₁不等于t₂，1≤i≤n。

30.测定不同时刻的移动车辆的车身三维坐标系与图像空间坐标系的转换关系：利用移动车辆在t₁时刻和t₂时刻的车身控制点，测定固定摄影设备物镜中心S在t₁时刻和t₂时刻车身坐标系中的坐标，以及t₁时刻和t₂时刻的车身三维坐标系与图像空间坐标系之间的姿态角，利用姿态角构成旋转矩阵。

所述的步骤30包括以下步骤：

301.获取移动车辆的车身控制点在图像空间坐标系中的坐标(x_i，y_i)；

302.利用式(4)测得三维直接线性变换参数和像片的畸变系数k₁；

$\left.\begin{array}{c}{x}_i+{\mathrm{\Δx}}_i+\frac{l_1{X}_i+l_2{Y}_i+l_3{Z}_i+l_4}{l_9{X}_i+l_{10}{Y}_i+l_{11}{Z}_i+1}=0\\ y_i+{\mathrm{\Δy}}_i+\frac{l_5{X}_i+l_6{Y}_i+l_7{Z}_i+l_8}{l_9{X}_i+l_{10}{Y}_i+l_{11}{Z}_i+1}=0\end{array}\right\}$ 式(4)

式(4)为三维直接线性变换公式，其中， $\left\{\begin{array}{c}{\mathrm{\Δx}}_i=k_1{r}_i^2(x_i-x_0)\\ {\mathrm{\Δy}}_i=k_1{r}_i^2(y_i-y_0)\end{array}\right.$ 为畸变，k₁畸变参数，(x₀，y₀)为固定摄影摄像设备的内方位元素；l₁、l₂、l₃、l₄、l₂、l₆、l₇、l₈、l₉、l₁₀、l₁₁为三维直接线性变换参数；

303.根据步骤302得到的三维直接线性变换参数测定固定摄影摄像设备的内方位元素：

先根据式(5)测得过渡参数γ₃，

${\mathrm{\γ}}_3=\sqrt{\frac{1}{l_9^2+l_{10}^2+l_{11}^2}}$ 式(5)

其次根据式(6)和式(7)测定固定摄影摄像设备的内方位元素(x₀，y₀)：

$x_0=-(l_1{l}_9+l_2{l}_{10}+l_3{l}_{11})/(l_9^2+l_{10}^2+l_{11}^2)$ 式(6)

$y_0=-(l_5{l}_9+l_6{l}_{10}+l_7{l}_{11})/(l_9^2+l_{10}^2+l_{11}^2)$ 式(7)

304.根据式(8)测定像片P₁在图像空间坐标系中的x轴和y轴比例尺不一致参数ds，根据式(9)计算像片P₁在图像空间坐标系中的x轴和y轴不垂直参数dβ：

$\mathrm{ds}=\sqrt{\frac{r_3^2(l_1^2+l_2^2+l_3^2)-x_0^2}{r_3^2(l_5^2+l_6^2+l_7^2)-y_0^2}}-1$ 式(8)

$\mathrm{d\β}=\arcsin \left\{\frac{\frac{(l_1{l}_5+l_2{l}_6+l_3{l}_7)}{(l_9^2+l_{10}^2+l_{11}^2)}-x_0{y}_0}{[r_3^2(l_1^2+l_2^2+l_3^2)-x_0^2][r_3^2(l_5^2+l_6^2+l_7^2)-y_0^2]}\right\}$ 式(9)

305.根据式(10)测定摄影时的相机物镜焦距f_x：

$f_x=\cos \mathrm{d\β}\sqrt{\frac{l_1^2+l_2^2+l_3^2}{l_9^2+l_{10}^2+l_{11}^2}-x_0^2}$ 式(10)

306.根据式(11)测定X_S1、Y_S1、Z_S1：

$\left.\begin{array}{c}{l}_1{X}_{s1}+l_2{Y}_{s1}+l_3{Z}_{s1}=-l_4\\ l_5{X}_{s1}+l_6{Y}_{s1}+l_7{Z}_{s1}=-l_8\\ l_9{X}_{s1}+l_{10}{Y}_{s1}+l_{11}{Z}_{s1}=-1\end{array}\right\}$ 式(11)

307.测定姿态角：利用式(12)测定过渡参数：a₃、b₃、c₃、b₂、b₁；

$\left\{\begin{array}{c}{a}_3=l_9{\mathrm{\γ}}_3\\ b_3=l_{10}{\mathrm{\γ}}_3\\ c_3=l_{11}{\mathrm{\γ}}_3\\ b_2=\frac{(l_6{\mathrm{\γ}}_3+b_3{y}_0)(1+\mathrm{ds}\cos \mathrm{d\β})}{f_x}\\ b_1=\frac{(l_2{\mathrm{\γ}}_3-b_2{f}_x\tan \mathrm{d\β}+b_3{x}_0)}{f_x}\end{array}\right.$ 式(12)

利用式(13)测定姿态角ω₁、κ₁，

式(13)

式(13)中，表示像片P₁相对于车身三维坐标系的转向角、ω₁表示像片P₁相对于车身三维坐标系的俯仰角，κ₁表示像片P₁相对于车身三维坐标系的侧向倾角。

姿态角ω₁、κ₁构成旋转矩阵R₁，即

$R_1=\left[\begin{array}{ccc}{a}_1& a_2& a_3\\ b_1& b_2& b_3\\ c_1& c_2& c_3\end{array}\right]$ 式(14)

式(14)中，

在步骤30中，利用上述步骤301至307，先测定t₁时刻的移动车辆的车身三维坐标系与图像空间坐标系的转换关系：测定t₁时刻固定摄影设备物镜中心S在t₁时刻的车身三维坐标系的坐标为：X_S1、Y_S1、Z_S1，然后测定图像空间坐标系与t₁时刻的车身三维坐标系的姿态角为ω₁、κ₁，姿态角ω₁、κ₁构成旋转矩阵为R₁；然后测定t₂时刻的移动车辆的车身三维坐标系与图像空间坐标系的转换关系：测定t₂时刻固定摄影设备物镜中心S在t₂时刻的车身三维坐标系的坐标为：X_S2、Y_S2、Z_S2，然后测定图像空间坐标系与t₂时刻的车身三维坐标系的姿态角为ω₂、κ₂，姿态角ω₂、κ₂构成旋转矩阵为R₂。

40.测定移动车辆的三维运动轨迹和姿态变化：

401.测定移动车辆的三维运动轨迹：t₂时刻的车身三维坐标系的原点相对于t₁时刻的车身三维坐标系中坐标，就是移动车辆的三维运动轨迹，即

$\left[\begin{array}{c}\mathrm{\ΔX}\\ \mathrm{\ΔY}\\ \mathrm{\ΔZ}\end{array}\right]=-R_1{R}_2^T\left[\begin{array}{c}{X}_{S2}\\ Y_{S2}\\ Z_{S2}\end{array}\right]+\left[\begin{array}{c}{X}_{S1}\\ Y_{S1}\\ Z_{S1}\end{array}\right]$ 式(1)

在式(1)中，ΔX、ΔY、ΔZ表示移动车辆的三维运动轨迹，其中ΔX表示车辆的左右位移量，ΔY表示车辆的上下位移量，ΔZ表示车辆的前进或后退的位移量；移动车辆的三维绝对位移量

402.测定移动车辆的姿态变化：

第一张像片P₁与第二张像片P₂在车身三维坐标系之间的旋转矩阵可得R中的A1、A2、A3、B1、B2、B3、C1、C2、C3参数，即式(2)

$R=R_2{R}_1^T=\left[\begin{array}{ccc}{A}_1& A_2& A_3\\ B_1& B_2& B_3\\ C_1& C_2& C_3\end{array}\right]$ 式(2)

根据式(3)可得移动车辆的姿态变化量(Φ、Ω、K)，

$\left.\begin{array}{c}\tan \mathrm{\Φ}=-\frac{A_3}{C_3}\\ \sin \mathrm{\Ω}=-B_3\\ \tan K=\frac{B_1}{B_2}\end{array}\right\}$ 式(3)

在式(3)中，Φ表示车辆的左右转向角、Ω表示上下俯仰角、K表示车辆的左右倾角。

Claims (2)

1.一种基于车身控制点的移动车辆三维运动轨迹的测定方法，其特征在于，该测定方法包括以下步骤：

10.建立车身控制点数据库：

101.选择车身控制点：对每种型号的车辆，在车身外表面选择n个车身特征点，作为车身控制点，n个车身特征点不处于同一平面内，其中，n为整数，且n≥6，

102.建立车身三维坐标系：车身三维坐标系以一个车身控制点为原点，以车辆前进方向为Z轴，垂直车辆前进方向的左侧为X轴，垂直车辆前进方向的指向天顶方向为Y轴，X、Y、Z轴构成右手坐标系；

103.测量车身控制点的三维坐标：用几何测量的方法，测量每个车身控制点的三维坐标(X_i、Y_i、Z_i),其中，1≤i≤n；

104.建立车身控制点数据库：对每种型号的车辆分别按照步骤101—103进行测量，然后将测量结果输入数据库，构成车身控制点数据库；

20.获得移动车辆的车身控制点的三维坐标：

201.建立图像空间坐标系：图像空间坐标系以固定摄影设备物镜中心S点为原点，摄影主光轴的反方向为Z轴，平行于图像平面水平方向为X轴的右手坐标系；

202.利用固定摄影设备，连续拍摄移动车辆的像片；

203.在步骤202拍摄的像片中提取t₁时刻的像片P₁和t₂时刻的像片P₂，根据该移动车辆型号从步骤104建立的车身控制点数据库中读取该移动车辆的车身控制点的三维坐标(X_i、Y_i、Z_i),其中，t₁不等于t₂,1≤i≤n；

30.测定不同时刻的移动车辆的车身三维坐标系与图像空间坐标系的转换关系：利用移动车辆在t₁时刻和t₂时刻的车身控制点，测定固定摄影设备物镜中心S在t₁时刻和t₂时刻车身坐标系中的坐标，以及t₁时刻和t₂时刻的车身三维坐标系与图像空间坐标系之间的姿态角，利用姿态角构成旋转矩阵；

所述的步骤30包括以下步骤：

301.获取移动车辆的车身控制点在图像空间坐标系中的坐标(x_i,y_i)；

302.利用式(4)测得三维直接线性变换参数和像片的畸变系数k₁；

$\left.\begin{array}{c}{x}_i+\mathrm{\Δ}{x}_i+\frac{l_1{X}_i+l_2{Y}_i+l_3{Z}_i+l_4}{l_9{X}_i+l_{10}{Y}_i+l_{11}{Z}_i+1}=0\\ y_i+\mathrm{\Δ}{y}_i+\frac{l_5{X}_i+l_6{Y}_i+l_7{Z}_i+l_8}{l_9{X}_i+l_{10}{Y}_i+l_{11}{Z}_i+1}=0\end{array}\right\}$       式(4)

式(4)为三维直接线性变换公式，其中， $\left\{\begin{array}{c}\mathrm{\Δ}{x}_i=k_1{r_i}^2(x_i-x_0)\\ \mathrm{\Δ}{y}_i=k_1{r_i}^2(y_i-y_0)\end{array}\right.$ 为畸变，k₁畸变参数，(x₀,y₀)为固定摄影设备的内方位元素；l₁、l₂、l₃、l₄、l₅、l₆、l₇、l₈、l₉、l₁₀、l₁₁为三维直接线性变换参数；X_i、Y_i、Z_i为移动车辆的车身控制点的三维坐标；

303.根据步骤302得到的三维直接线性变换参数测定固定摄影设备的内方位元素：

先根据式(5)测得过渡参数γ₃，

${\mathrm{\γ}}_3=\sqrt{\frac{1}{l_9^2+l_{10}^2+l_{11}^2}}$     式(5)

其次根据式(6)和式(7)测定固定摄影设备的内方位元素(x₀,y₀)：

$x_0=-(l_1{l}_9+l_2{l}_{10}+l_3{l}_{11})/(l_9^2+l_{10}^2+l_{11}^2)$      式(6)

$y_0=-(l_5{l}_9+l_6{l}_{10}+l_7{l}_{11})/(l_9^2+l_{10}^2+l_{11}^2)$      式(7)

304.根据式(8)测定像片P₁在图像空间坐标系中的x轴和y轴比例尺不一致参数ds，根据式(9)计算像片P₁在图像空间坐标系中的x轴和y轴不垂直参数dβ：

$\mathrm{ds}=\sqrt{\frac{{\mathrm{\γ}}_3^2(l_1^2+l_2^2+l_3^2)-x_0^2}{{\mathrm{\γ}}_3^2(l_5^2+l_6^2+l_7^2)-y_0^2}}-1$      式(8)

$\mathrm{d\β}=\arcsin \left\{\frac{\frac{(l_1{l}_5+l_2{l}_6+l_3{l}_7)}{(l_9^2+l_{10}^2+l_{11}^2)}-x_0{y}_0}{[{\mathrm{\γ}}_3^2(l_1^2+l_2^2+l_3^2)-x_0^2][{\mathrm{\γ}}_3^2(l_5^2+l_6^2+l_7^2)-y_0^2]}\right\}$     式(9)

305.根据式(10)测定摄影时的相机物镜焦距f_x：

$f_x=\cos \mathrm{d\β}\sqrt{\frac{l_1^2+l_2^2+l_3^2}{l_9^2+l_{10}^2+l_{11}^2}-x_0^2}$      式(10)

306.根据式(11)测定t₁时刻固定摄影设备物镜中心S在t₁时刻的车身三维坐标系的坐标为：X_S1、Y_S1、Z_S1：

$\left.\begin{array}{c}{l}_1{X}_{s1}+l_2{Y}_{s1}+l_3{Z}_{s1}=-l_4\\ l_5{X}_{s1}+l_6{Y}_{s1}+l_7{Z}_{s1}=-l_8\\ l_9{X}_{s1}+l_{10}{Y}_{s1}+l_{11}{Z}_{s1}=-1\end{array}\right\}$       式(11)

307.测定姿态角：利用式(12)测定过渡参数：a₃、b₃、c₃、b₂、b₁；

$\left\{\begin{array}{c}{a}_3=l_9{\mathrm{\γ}}_3\\ b_3=l_{10}{\mathrm{\γ}}_3\\ c_3=l_{11}{\mathrm{\γ}}_3\\ b_2=\frac{(l_6{\mathrm{\γ}}_3+b_3{y}_0)(1+\mathrm{ds}\cos \mathrm{d\β})}{f_x}\\ b_1=\frac{(l_2{\mathrm{\γ}}_3-b_2{f}_x\tan \mathrm{d\β}+b_3{x}_0)}{f_x}\end{array}\right.$      式(12)

利用式(13)测定姿态角ω₁、κ₁，

     式(13)

式(13)中，表示像片P₁相对于车身三维坐标系的转向角、ω₁表示像片P₁相对于车身三维坐标系的俯仰角，κ₁表示像片P₁相对于车身三维坐标系的侧向倾角；

姿态角ω₁、κ₁构成旋转矩阵R₁，即

$R_1=\left[\begin{array}{ccc}{a}_1& a_2& a_3\\ b_1& b_2& b_3\\ c_1& c_2& c_3\end{array}\right]$      式(14)

式(14)中，

然后测定t₂时刻的移动车辆的车身三维坐标系与图像空间坐标系的转换关系：测定t₂时刻固定摄影设备物镜中心S在t₂时刻的车身三维坐标系的坐标为：X_S2、Y_S2、Z_S2，然后测定图像空间坐标系与t₂时刻的车身三维坐标系的姿态角为ω₂、κ₂，姿态角ω₂、κ₂构成旋转矩阵为R₂；

40.测定移动车辆的三维运动轨迹和姿态变化：

401.测定移动车辆的三维运动轨迹：t₂时刻的车身三维坐标系的原点相对于t₁时刻的车身三维坐标系中坐标，就是移动车辆的三维运动轨迹，即

$\left[\begin{array}{c}\mathrm{\ΔX}\\ \mathrm{\ΔY}\\ \mathrm{\ΔZ}\end{array}\right]=-R_1{R}_2^T\left[\begin{array}{c}{X}_{S2}\\ Y_{S2}\\ Z_{S2}\end{array}\right]+\left[\begin{array}{c}{X}_{S1}\\ Y_{S1}\\ Z_{S1}\end{array}\right]$      式(1)

在式(1)中，ΔX、ΔY、ΔZ表示移动车辆的三维运动轨迹，其中ΔX表示车辆的左右位移量，ΔY表示车辆的上下位移量，ΔZ表示车辆的前进或后退的位移量；移动车辆的三维绝对位移

402.测定移动车辆的姿态变化：

第一张像片P₁与第二张像片P₂在车身三维坐标系之间的旋转矩阵可得R中的A1、A2、A3、B1、B2、B3、C1、C2、C3参数，即式(2)

$R=R_2{R}_1^T=\left[\begin{array}{ccc}{A}_1& A_2& A_3\\ B_1& B_2& B_3\\ C_1& C_2& C_3\end{array}\right]$      式(2)

根据式(3)可得移动车辆的姿态变化量(Φ、Ω、Κ)，

$\left.\begin{array}{c}\tan \mathrm{\Φ}=-\frac{A_3}{C_3}\\ \sin \mathrm{\Ω}=-B_3\\ \tan K=\frac{B_1}{B_2}\end{array}\right\}$      式(3)

在式(3)中，Φ表示车辆的左右转向角、Ω表示上下俯仰角、Κ表示车辆的左右倾角。

2.按照权利要求1所述的基于车身控制点的移动车辆三维运动轨迹的测定方法，其特征在于，步骤102中，车身三维坐标系的原点位于车身前部中间位置。