CN111638487A - 一种自动泊车测试设备及方法 - Google Patents

Abstract

本发明公开了一种自动泊车测试设备及方法，车辆自动泊车测试设备技术领域，目的是解决现有的测量方法所存在的诸多缺陷和问题，本发明将车辆、障碍物、周围环境等建立在一个测试坐标系中，通过无线电测距技术、通信技术，实时绘制出各点的坐标位置，进而生成各点的移动轨迹及移动速度、两点相对距离、整车轮廓包络等信息，在任意区域均可使用，整个装置完全依靠装置内元件的工作实现，因此不影响整车正常使用，避免了现有技术所存在的问题。

Description

一种自动泊车测试设备及方法

技术领域

本发明属于车辆自动泊车测试设备技术领域。

背景技术

汽车在进行某些测试或活动(如自动泊车测试、泊车姿态测量、泊车路径演示等)时，需要实时采集汽车上任意点的实时位置进而生成整个移动过程中车辆的轨迹线、轮廓包络、行驶姿态及与障碍物相对距离等参数，目前通常采用GPS/差分基站测量甚至手工测量，由此带来了以下缺点：

a)GPS/差分基站测量：无法在地下停车场、高层建筑附近、遮雨棚等影响GPS信号的区域进行；

b)手工测量：无法实时绘制行驶轨迹、轮廓包络，距离采集速度慢、效率低、误差大。

发明内容

本发明的目的在于提供一种用于自动泊车测试设备及方法，该设备及方法也可用于其他低速移动物体的定位、轨迹绘制、相对位置测量等功能。

本发明采用的技术方案如下：

一种用于自动泊车测试设备，主要包括一个控制主机1，三个或以上配置相同的固定基站2，两个以上配置相同的移动基站3；移动基站3设置在被测试车辆顶部；固定基站2测试场地区域的外围；

其中，固定基站2包括无线电接收器21和无线电发射器22；无线电接收器21及无线电发射器22用于向移动基站/控制主机发送和接收消息。

移动基站3包括无线电接收器31和无线电发射器32；无线电接收器31及无线电发射器32用于向移动基站/控制主机发送和接收消息。

控制主机1由无线电接收器11、无线电发射器12、处理器13和存储器14构成；处理器13分别和无线电接收器11、无线电发射器12和存储器14连接；

无线电接收器11用于实时接收固定基站2和/或移动基站3发送的信息；

无线电发射器12用于向各个固定基站2和移动基站3发送的信息；

处理器13用于根据接收到的固定基站2与移动基站3的距离并计算固定基站2、移动基站3的坐标；

基站A与基站B为任意两个基站，基站A与基站B之间的距离D为：

D＝T×c

式中，c为光速，其值为299792458m/s；

T为测距消息在基站A与基站B之间的飞行时间；

T＝(T_发送1×T_发送2-T_回复1×T_回复2)/(T_发送1+T_回复1+T_发送2+T_回复2)

式中，T_发送1为基站A第一次发送测距消息到第一次接收测距消息的时间间隔；

T_回复1为基站B第一次接收测距消息到第一次发送测距消息的时间间隔；

T_发送2为基站B第一次发送测距消息到第二次接收测距消息的时间间隔；

T_回复2为基站A第一次接收测距消息到第二次发送测距消息的时间间隔。

固定基站2、移动基站3的坐标的计算方法如下：

1)三个固定基站(A、B、C)与一个移动基站(V)构成一个三棱锥，且任意两基站之间距离均已知，即三棱锥棱边长l、m、n及底边长q、p、r已知。由欧拉四面体公式可求得该四面体体积V为：

式中，l为固定基站A与移动基站V的真实距离；

m为固定基站B与移动基站V的真实距离；

n为固定基站C与移动基站V的真实距离；

p为固定基站A与固定基站B的真实距离；

q为固定基站B与固定基站C的真实距离；

r为固定基站A与固定基站C的真实距离；

2)由海伦公式可得由三个固定基站构成的△ABC的面积S为：

式中，p为固定基站A与固定基站B的真实距离；

q为固定基站B与固定基站C的真实距离；

r为固定基站A与固定基站C的真实距离；

k为p、q、r之和的一半，即：

3)此三棱锥的高，即移动基站V到大地平面的距离，可由四面体体积公式得出h为：

式中，V为1)求得的三棱锥体积；

S为2)求得的三棱锥底面积；

4)将移动基站V正交投影在大地平面，得到正交投影点G(以下简称投影点)，此时在大地平面下共有4个点，1个为移动基站投影点G，3个为固定基站点A、B、C，见图5。△AGV、△BGV、△CGV均为直角三角形，由勾股定理可得到点G分别与三个固定基站A、B、C的连线AG、BG、CG的长度u、v、w。大地平面内4个点距离两两已知。

5)将其中一个固定基站A作为坐标系零点(0,0)，其与另外任一个固定基站B的连线作为x轴正方向，构成一笛卡尔坐标系(以下简称测试坐标系)。在△ABC中，已知三角形面积S及三边长p、q、r，由三角形面积公式，可求得C点坐标(x_C,y_C)分别为：

式中，p为固定基站A与固定基站B的真实距离；

S为2)求得的三棱锥底面积；

式中，r为固定基站A与固定基站C的真实距离；

y_C为5)求得的固定基站C纵坐标；

6)在△ABG中，参照5)先由求得G点坐标(x_G,y_G)分别为：

式中，p为固定基站A与固定基站B的真实距离；

u为固定基站A与正交投影点G的真实距离；

v为固定基站B与正交投影点G的真实距离；

j为p、u、v之和的一半，即：

式中，u为固定基站A与正交投影点G的真实距离；

y_G为6)求得的正交投影点G纵坐标；

7)值得注意的是，由6)求得的x_G,y_G的值为G点坐标的绝对值，其值正负共构成4种可能性，即分别对应测试坐标系四个象限中的位置，需通过C点坐标求其值的正负号，其应满足：

w²＝(x_C-x_G)²+(y_C-y_G)²

通过以上步骤即可确定移动基站V正交投影点G的坐标(x_G,y_G)。

基于以上计算的各个固定基站、移动基站的坐标，

根据两个移动基站3正交投影点坐标输出车辆运动速度、运动轨迹，方法如下：

测量车辆轮廓上选取的特征点与两移动基站间水平横向距离、水平纵向距离，将测得的特征点距移动基站的横向距离、纵向距离，代入移动基站坐标，得到各个特征点在测试坐标系下的坐标值。对坐标系中构成每一个特征点之间进行线性插值，将轮廓点数量插值。再进行均值平滑滤波，保证车辆轮廓的平滑。

基于以上得出的各个特征点的初始坐标，求得任意时刻车辆特征点的坐标，将测试车辆上靠车尾一侧安装的移动基站的投影点作为本车坐标系的原点，与另外一个移动基站的投影点的连线作为车辆方向，则可由车辆坐标系与测试坐标系之间的坐标转化矩阵，实时得出车辆移动过程中特征点在测试坐标系下的坐标：

式中，x₁～x_n为车辆轮廓所有特征点在测试坐标系下的初始横坐标；

y₁～y_n为车辆轮廓所有特征点在测试坐标系下的初始纵坐标；

x₁’～x_n’为车辆轮廓所有特征点在测试坐标系下某一时刻的动态横坐标；

y₁’～y_n’为车辆轮廓所有特征点在测试坐标系下某一时刻的动态纵坐标；

Δx为安装在车头、车尾两移动基站的中点在某一时刻的横向位移，即：

式中，Δx_f为安装在车头一侧的移动基站横坐标变化量；

Δx_r为安装在车尾一侧的移动基站横坐标变化量；

Δy为安装在车头、车尾两移动基站的中点在某一时刻的纵向位移，即：

式中，Δy_f为安装在车头一侧的移动基站纵坐标变化量；

Δy_r为安装在车尾一侧的移动基站纵坐标变化量；

θ为车辆在某一时刻的航向角，即：

式中，Δx_f为安装在车头一侧的移动基站横坐标变化量；

Δy_f为安装在车头一侧的移动基站纵坐标变化量；

Δx_r为安装在车尾一侧的移动基站横坐标变化量；

Δy_r为安装在车尾一侧的移动基站纵坐标变化量；

z为安装在车头、车尾两移动基站的在测试坐标系中的距离；

根据测试前已知的周围环境任意一点坐标及障碍物轮廓，输出车辆轮廓与周围环境任意一点距离或与障碍物轮廓的最近距离。

存储器14用于保存测试前输入的各项参数，包括：车辆外轮廓特征点与两移动基站间水平横向距离和水平纵向距离、障碍物轮廓坐标，将测试设置保存以便后续调用，将处理器13计算的各项数据、评价结果加以存储，将测试前、测试中、测试后所有被需求的数据存储。

本发明设备的测试方法，步骤具体如下：

a)测试前，在测试空间内布置三个(或以上)固定基站使之位置固定，固定基站应尽量位于测试区域外围，当测试区域超出固定基站覆盖范围时，可适当增加固定基站数量。

b)测试前，将两移动基站安装于车辆上；测量车辆的轮廓特征点与两移动基站间水平横向距离、水平纵向距离；测量障碍物的轮廓特征点与两移动基站间水平横向距离、水平纵向距离(当障碍物是移动物体时，可在障碍物上增设两个移动基站，测量方式同测试车辆)；另可根据实际需要，测量空间任意点到两移动基站的水平横向距离、水平纵向距离(此点在移动基站移动时可被实时输出与移动基站的距离)。

c)控制主机初始化，获得固定基站的各个相对位置。控制主机通过三个(或以上)固定基站相对位置确立测试坐标系，三个(或以上)固定基站在测试坐标系中分别具有一个固定的坐标。

d)移动基站与固定基站间通过飞行测距法测得两者之间距离，该距离由固定基站发送至控制主机，控制主机获得各移动基站与固定基站的距离，得出移动基站在测试坐标系中的坐标，并基于3.2已测得的车辆轮廓数据、障碍物轮廓数据，控制主机可在测试坐标系中确定车辆轮廓、障碍物轮廓在测试坐标系中的位置。

e)测试过程中，移动基站、固定基站始终保持高频测距并实时发送给控制主机，控制主机将移动基站各时刻坐标点的连线即为车辆轨迹线、车辆轮廓各时刻的连线即为车辆轮廓包络线，车辆轮廓的坐标与障碍物的坐标经过坐标距离计算即为两者实际距离，此外，针对某一固定坐标(可以是事先设置的障碍物坐标等)、某一移动坐标(可以是事先设置的移动障碍车辆等)，控制主机也可以通过测试坐标系实时计算相对距离等各种参数，并将所有测得的参数、计算的结果等信息全部存储于存储器中；增设显示器时，控制主机可描绘出俯视平面图以实时显示测试车辆、障碍物等的运动情况、相对距离等信息。

本发明的有益效果：

本发明将车辆、障碍物、周围环境等建立在一个测试坐标系中，通过无线电测距技术、通信技术，实时绘制出各点的坐标位置，进而生成各点的移动轨迹及移动速度、两点相对距离、整车轮廓包络等信息，在任意区域均可使用，整个装置完全依靠装置内元件的工作实现，因此不影响整车正常使用，避免了现有技术所存在的问题。

附图说明

图1本发明装置连接示意图；

图2本发明测试场地布置图；

图3为车辆轮廓包络线示意图；

图4为两基站之间测距原理示意图；

图5固定基站与移动基站组成的三棱锥测量移动基站正交投影坐标点原理；

图6车辆轮廓特征点和计算得到的车辆轮廓；

图7测试车辆、障碍物相对距离示意图。

具体实施方式

下面参照附图对本发明所采用的技术方案做进一步解释和说明。

参照图1，本发明基本配置主要包括一个控制主机1，三个(或以上)配置相同的固定基站2，两个(或以上)配置相同的移动基站3。当活动范围扩大时，可增设固定基站2；当移动目标增加时，可增设移动基站3；当需要实时查看数据、车辆轮廓线等的绘制时，可增设与控制主机相连的显示器。

控制主机1由无线电接收器11、无线电发射器12、处理器13、存储器14构成。无线电接收器11的目的在于实时接收固定基站/移动基站发送的信息。无线电发射器12的目的在于向各基站发送信息等。处理器13的目的在于根据接收到的固定基站与移动基站的距离并计算固定基站、移动基站的坐标，根据两个移动基站输出车辆运动速度、运动轨迹；根据测试前输入的车辆轮廓坐标，输出车辆轮廓包络；根据测试前已知的周围环境任意一点坐标及障碍物轮廓，输出车辆轮廓与周围环境任意一点距离或与障碍物轮廓的最近距离。存储器14的目的在于将测试前输入的各项(如车辆、障碍物轮廓等)参数保存，将测试设置保存以便后续调用，将处理器13计算的各项数据、评价结果加以存储，将测试前、测试中、测试后所有被需求的数据存储。

固定基站2由无线电接收器21、无线电发射器22等构成。无线电接收器21及无线电发射器22的目的在于对移动基站/控制主机接收、发送消息。

移动基站3由无线电接收器31、无线电发射器32等构成。无线电接收器31及无线电发射器32的目的在于对固定基站/控制主机接收、发送消息。

基于以上结构配置，本发明具体实施方式如下，参照图2、图3：

a)测试前，在测试空间内布置三个(或以上)固定基站使之位置固定，固定基站应尽量位于测试区域外围，当测试区域超出固定基站覆盖范围时，可适当增加固定基站数量。

b)测试前，将两移动基站安装于车辆上；测量车辆的轮廓特征点与两移动基站间水平横向距离、水平纵向距离；测量障碍物的轮廓特征点与两移动基站间水平横向距离、水平纵向距离(当障碍物是移动物体时，可在障碍物上增设两个移动基站，测量方式同测试车辆)；另可根据实际需要，测量空间任意点到两移动基站的水平横向距离、水平纵向距离(此点在移动基站移动时可被实时输出与移动基站的距离)。

c)控制主机初始化，获得固定基站的各个相对位置。控制主机通过三个(或以上)固定基站相对位置确立测试坐标系，三个(或以上)固定基站在测试坐标系中分别具有一个固定的坐标。

d)移动基站与固定基站间通过飞行测距法测得两者之间距离，该距离由固定基站发送至控制主机，控制主机获得各移动基站与固定基站的距离，得出移动基站在测试坐标系中的坐标，并基于3.2已测得的车辆轮廓数据、障碍物轮廓数据，控制主机可在测试坐标系中确定车辆轮廓、障碍物轮廓在测试坐标系中的位置。

e)测试过程中，移动基站、固定基站始终保持高频测距并实时发送给控制主机，控制主机将移动基站各时刻坐标点的连线即为车辆轨迹线、车辆轮廓各时刻的连线即为车辆轮廓移动包络线，车辆轮廓的坐标与障碍物的坐标经过坐标距离计算即为两者实际距离，此外，针对某一固定坐标(可以是事先设置的障碍物坐标等)、某一移动坐标(可以是事先设置的移动障碍车辆等)，控制主机也可以通过测试坐标系实时计算相对距离等各种参数，并将所有测得的参数、计算的结果等信息全部存储于存储器中；增设显示器时，控制主机可描绘出俯视平面图以实时显示测试车辆、障碍物等的运动情况、相对距离等信息，如图3。

本发明的以上实施方式是基于以下原理实现的：

首先确定任意两基站之间的距离使用飞行测距法测量，即基站A主动发起第一次测距消息，基站B响应，得到4个时间戳；然后过了一段时间，基站B主动发起测距，基站A响应，同样得到4个不同的时间戳。最终可以得到如下四个时间差：T_发送1、T_回复1、T_发送2、T_回复2；测距消息在基站A与基站B之间的飞行时间T为：

T＝(T_发送1×T_发送2-T_回复1×T_回复2)/(T_发送1+T_回复1+T_发送2+T_回复2)

式中，T_发送1为基站A第一次发送测距消息到第一次接收测距消息的时间间隔；

T_回复1为基站B第一次接收测距消息到第一次发送测距消息的时间间隔；

T_发送2为基站B第一次发送测距消息到第二次接收测距消息的时间间隔；

T_回复2为基站A第一次接收测距消息到第二次发送测距消息的时间间隔；

进而得出基站A与基站B之间的距离D为：

D＝T×c

式中，c为光速，其值为299792458m/s；

通过以上方式可实时获得任意两个基站之间的距离，三个固定基站构成一个平面，移动基站通常布置于车顶位置，其高度随车型的不同而不同，与固定基站可视为不在同一平面内，因此，需要计算出移动基站在三个固定基站确定的平面下(以下简称大地平面)的正交投影坐标。计算方式如下：

1)三个固定基站(图5中的点A、B、C)与一个移动基站(图5中的点V)构成一个三棱锥，且任意两基站之间距离均已知，即三棱锥棱边长l、m、n及底边长q、p、r已知。由欧拉四面体公式可求得该四面体体积V为：

式中，l为固定基站A与移动基站V的真实距离；

m为固定基站B与移动基站V的真实距离；

n为固定基站C与移动基站V的真实距离；

p为固定基站A与固定基站B的真实距离；

q为固定基站B与固定基站C的真实距离；

r为固定基站A与固定基站C的真实距离；

2)由海伦公式可得由三个固定基站构成的△ABC的面积S为：

式中，p为固定基站A与固定基站B的真实距离；

q为固定基站B与固定基站C的真实距离；

r为固定基站A与固定基站C的真实距离；

k为p、q、r之和的一半，即：

3)此三棱锥的高，即移动基站V到大地平面的距离，可由四面体体积公式得出h为：

式中，V为1)求得的三棱锥体积；

S为2)求得的三棱锥底面积；

4)将移动基站V正交投影在大地平面，得到正交投影点G(以下简称投影点)，此时在大地平面下共有4个点，1个为移动基站投影点G，3个为固定基站点A、B、C，见图5。△AGV、△BGV、△CGV均为直角三角形，由勾股定理可得到点G分别与三个固定基站A、B、C的连线AG、BG、CG的长度u、v、w。大地平面内4个点距离两两已知。

5)将其中一个固定基站A作为坐标系零点(0,0)，其与另外任一个固定基站B的连线作为x轴正方向，构成一笛卡尔坐标系(以下简称测试坐标系)。在△ABC中，已知三角形面积S及三边长p、q、r，由三角形面积公式，可求得C点坐标(x_C,y_C)分别为：

式中，p为固定基站A与固定基站B的真实距离；

S为2)求得的三棱锥底面积；

式中，r为固定基站A与固定基站C的真实距离；

y_C为5)求得的固定基站C纵坐标；

6)在△ABG中，参照5)先由求得G点坐标(x_G,y_G)分别为：

式中，p为固定基站A与固定基站B的真实距离；

u为固定基站A与正交投影点G的真实距离；

v为固定基站B与正交投影点G的真实距离；

j为p、u、v之和的一半，即：

式中，u为固定基站A与正交投影点G的真实距离；

y_G为6)求得的正交投影点G纵坐标；

7)值得注意的是，由6)求得的x_G,y_G的值为G点坐标的绝对值，其值正负共构成4种可能性，即分别对应测试坐标系四个象限中的位置，需通过C点坐标求其值的正负号，其应满足：

w²＝(x_C-x_G)²+(y_C-y_G)²

通过以上步骤即可确定正交投影点G的坐标(x_G,y_G)。

基于以上计算的各个固定基站、移动基站坐标，由步骤b)测得的特征点距移动基站的横向距离、纵向距离(见图6)，代入移动基站坐标，可得到各个特征点在测试坐标系下的坐标值。值得注意的是，为了使在特征点测量较少的情况下，依旧能使轮廓计算值更加精确，则对坐标系中构成每一个物体的特征点之间进行线性插值，将轮廓点数量插值到原来的20倍左右。再进行窗口为10的均值平滑滤波，保证物体边缘的平滑。障碍物轮廓特征点、周围环境任意一点坐标代入与测试车辆轮廓坐标代入原理相同。

基于以上得出的各轮廓点的初始坐标，可求得任意时刻车辆轮廓点的坐标，将测试车辆上靠车尾一侧安装的移动基站的投影点作为本车坐标系的原点，与另外一个移动基站的投影点的连线作为车辆方向，则可由车辆坐标系与测试坐标系之间的坐标转化矩阵，实时得出车辆移动过程中轮廓点在测试坐标系下的坐标：

式中，x₁～x_n为车辆轮廓所有点在测试坐标系下的初始横坐标；

y₁～y_n为车辆轮廓所有点在测试坐标系下的初始纵坐标；

x₁’～x_n’为车辆轮廓所有点在测试坐标系下某一时刻的动态横坐标；

y₁’～y_n’为车辆轮廓所有点在测试坐标系下某一时刻的动态纵坐标；

Δx为安装在车头、车尾两移动基站的中点在某一时刻的横向位移，即：

式中，Δx_f为安装在车头一侧的移动基站横坐标变化量；

Δx_r为安装在车尾一侧的移动基站横坐标变化量；

Δy为安装在车头、车尾两移动基站的中点在某一时刻的纵向位移，即：

式中，Δy_f为安装在车头一侧的移动基站纵坐标变化量；

Δy_r为安装在车尾一侧的移动基站纵坐标变化量；

θ为车辆在某一时刻的航向角，即：

式中，Δx_f为安装在车头一侧的移动基站横坐标变化量；

Δy_f为安装在车头一侧的移动基站纵坐标变化量；

Δx_r为安装在车尾一侧的移动基站横坐标变化量；

Δy_r为安装在车尾一侧的移动基站纵坐标变化量；

z为安装在车头、车尾两移动基站的在测试坐标系中的距离；

基于以上测试车辆、障碍物、周围环境任意一点的任意时刻的坐标，可求得以下结果：

i)车辆轮廓点与障碍物轮廓点坐标均已知，则坐标系下任意车辆轮廓点与障碍物轮廓点之间距离则可由欧式距离计算得出。

ii)将障碍物轮廓点与车辆轮廓点实时进行循环计算欧式距离，找到距离最小的点即为车辆与障碍物的最近距离，如图7。

iii)将环境中任意一点坐标与车辆轮廓点坐标进行实时轮询计算欧式距离，距离最小点即为车辆轮廓与周围环境中任意一点的最近距离。

Claims (10)

1.一种用于自动泊车测试设备，其特征在于，该设备主要包括：一个控制主机(1)，三个或以上配置相同的固定基站(2)，两个以上配置相同的移动基站(3)；移动基站(3)设置在被测试车辆顶部；固定基站(2)测试场地区域的外围的地面上；

其中，固定基站(2)包括无线电接收器(21)和无线电发射器(22)；无线电接收器(21)及无线电发射器(22)用于向移动基站/控制主机发送和接收消息。

移动基站(3)包括无线电接收器(31)和无线电发射器(32)；无线电接收器(31)及无线电发射器(32)用于向移动基站/控制主机发送和接收消息。

控制主机(1)由无线电接收器(11)、无线电发射器(12)、处理器(13)和存储器(14)构成；处理器(13)分别和无线电接收器(11)、无线电发射器(12)和存储器(14)连接；

无线电接收器(11)用于实时接收固定基站(2)和/或移动基站(3)发送的信息；

无线电发射器(12)用于向各个固定基站(2)和移动基站(3)发送的信息；

处理器(13)用于根据接收到的固定基站与移动基站的距离并计算固定基站、移动基站的坐标，根据两个移动基站输出车辆运动速度、运动轨迹；根据测试前输入的车辆轮廓坐标，输出车辆轮廓包络；根据测试前已知的周围环境任意一点坐标及障碍物轮廓，输出车辆轮廓与周围环境任意一点距离或与障碍物轮廓的最近距离；

存储器(14)用于保存测试前输入的各项参数，包括：车辆外轮廓特征点与两移动基站间水平横向距离和水平纵向距离、障碍物轮廓坐标，将测试设置保存以便后续调用，将处理器(13)计算的各项数据、评价结果加以存储，将测试前、测试中、测试后所有被需求的数据存储。

2.根据权利要求1所述的用于自动泊车测试设备，其特征在于，固定基站与固定基站中任意两个基站的距离根据以下方法获得：

基站A与基站B为任意两个基站，基站A与基站B之间的距离D为：

D＝T×c

式中，c为光速，其值为299792458m/s；

T为测距消息在基站A与基站B之间的飞行时间；

T＝(T_发送1×T_发送2-T_回复1×T_回复2)/(T_发送1+T_回复1+T_发送2+T_回复2)

式中，T_发送1为基站A第一次发送测距消息到第一次接收测距消息的时间间隔；

T_回复1为基站B第一次接收测距消息到第一次发送测距消息的时间间隔；

T_发送2为基站B第一次发送测距消息到第二次接收测距消息的时间间隔；

T_回复2为基站A第一次接收测距消息到第二次发送测距消息的时间间隔。

3.根据权利要求1所述的用于自动泊车测试设备，其特征在于，固定基站(2)和移动基站(3)中，移动基站(3)坐标的计算方法如下：

1)三个固定基站(A、B和C)与一个移动基站(V)构成一个三棱锥，且任意两基站之间距离均已知，即三棱锥棱边长l、m、n及底边长q、p、r已知。由欧拉四面体公式可求得该四面体体积V为：

式中，l为固定基站A与移动基站V的真实距离；

m为固定基站B与移动基站V的真实距离；

n为固定基站C与移动基站V的真实距离；

p为固定基站A与固定基站B的真实距离；

q为固定基站B与固定基站C的真实距离；

r为固定基站A与固定基站C的真实距离；

2)由海伦公式可得由三个固定基站构成的△ABC的面积S为：

式中，p为固定基站A与固定基站B的真实距离；

q为固定基站B与固定基站C的真实距离；

r为固定基站A与固定基站C的真实距离；

k为p、q、r之和的一半，即：

3)此三棱锥的高，即移动基站V到大地平面的距离，可由四面体体积公式得出h为：

式中，V为1)求得的三棱锥体积；

S为2)求得的三棱锥底面积；

4)将移动基站V正交投影在大地平面，得到正交投影点G(以下简称投影点)，此时在大地平面下共有4个点，1个为移动基站投影点G，3个为固定基站点A、B、C，见图5。△AGV、△BGV、△CGV均为直角三角形，由勾股定理可得到点G分别与三个固定基站A、B、C的连线AG、BG、CG的长度u、v、w；大地平面内4个点距离两两已知；

5)将其中一个固定基站A作为坐标系零点(0,0)，其与另外任一个固定基站B的连线作为x轴正方向，构成一笛卡尔坐标系(以下简称测试坐标系)。在△ABC中，已知三角形面积S及三边长p、q、r，由三角形面积公式，可求得C点坐标(x_C,y_C)分别为：

式中，p为固定基站A与固定基站B的真实距离；

S为2)求得的三棱锥底面积；

式中，r为固定基站A与固定基站C的真实距离；

y_C为5)求得的固定基站C纵坐标；

6)在△ABG中，参照5)先由求得G点坐标(x_G,y_G)分别为：

式中，p为固定基站A与固定基站B的真实距离；

u为固定基站A与正交投影点G的真实距离；

v为固定基站B与正交投影点G的真实距离；

j为p、u、v之和的一半，即：

式中，u为固定基站A与正交投影点G的真实距离；

y_G为6)求得的正交投影点G纵坐标；

7)由6)求得的x_G,y_G的值为G点坐标的绝对值，其值正负共构成4种可能性，即分别对应测试坐标系四个象限中的位置，需通过C点坐标求其值的正负号，其应满足：

w²＝(x_C-x_G)²+(y_C-y_H)²

通过以上步骤即可确定移动基站V正交投影点G的坐标(x_G,y_G)。

4.根据权利要求1所述的用于自动泊车测试设备，其特征在于，根据两个移动基站(3)正交投影点坐标输出车辆运动速度、运动轨迹，方法如下：

测量车辆轮廓上选取的特征点与两移动基站间水平横向距离、水平纵向距离，将测得的特征点距移动基站的横向距离、纵向距离，代入移动基站坐标，得到各个特征点在测试坐标系下的坐标值；对坐标系中构成每一个特征点之间进行线性插值，将轮廓点数量插值；再进行均值平滑滤波，保证车辆轮廓的平滑；

基于以上得出的各个特征点的初始坐标，求得任意时刻车辆特征点的坐标，将测试车辆上靠车尾一侧安装的移动基站的投影点作为本车坐标系的原点，与另外一个移动基站的投影点的连线作为车辆方向，则可由车辆坐标系与测试坐标系之间的坐标转化矩阵，实时得出车辆移动过程中特征点在测试坐标系下的坐标：

式中，x₁～x_n为车辆轮廓所有特征点在测试坐标系下的初始横坐标；

y₁～y_n为车辆轮廓所有特征点在测试坐标系下的初始纵坐标；

x₁’～x_n’为车辆轮廓所有特征点在测试坐标系下某一时刻的动态横坐标；

y₁’～y_n’为车辆轮廓所有特征点在测试坐标系下某一时刻的动态纵坐标；

Δx为安装在车头、车尾两移动基站的中点在某一时刻的横向位移，即：

式中，Δx_f为安装在车头一侧的移动基站横坐标变化量；

Δx_r为安装在车尾一侧的移动基站横坐标变化量；

Δy为安装在车头、车尾两移动基站的中点在某一时刻的纵向位移，即：

式中，Δy_f为安装在车头一侧的移动基站纵坐标变化量；

Δy_r为安装在车尾一侧的移动基站纵坐标变化量；

θ为车辆在某一时刻的航向角，即：

式中，Δx_f为安装在车头一侧的移动基站横坐标变化量；

Δy_f为安装在车头一侧的移动基站纵坐标变化量；

Δx_r为安装在车尾一侧的移动基站横坐标变化量；

Δy_r为安装在车尾一侧的移动基站纵坐标变化量；

z为安装在车头、车尾两移动基站的在测试坐标系中的距离；

将各个时刻的车辆特征点连接得到车辆运动运动轨迹。

5.根据权利要求1所述的用于自动泊车测试设备，其特征在于，根据测试前已知的周围环境任意一点坐标及障碍物轮廓，输出车辆轮廓与周围环境任意一点距离或与障碍物轮廓的最近距离，具体方法如下：

i)车辆轮廓点与障碍物轮廓点坐标均已知，则坐标系下任意车辆轮廓点与障碍物轮廓点之间距离则可由欧式距离计算得出。

ii)将障碍物轮廓点与车辆轮廓点实时进行循环计算欧式距离，找到距离最小的点即为车辆与障碍物的最近距离。

iii)将环境中任意一点坐标与车辆轮廓点坐标进行实时轮询计算欧式距离，距离最小点即为车辆轮廓与周围环境中任意一点的最近距离。

6.根据权利要求1所述的用于自动泊车测试设备，其特征在于，该设备还包含一个与处理器(13)连接的显示器,用于绘出俯视平面图以实时显示测试车辆、障碍物的运动情况和相对距离。

7.如权利要求1所述的用于自动泊车测试设备的测试方法，步骤具体如下：

a)测试前，在测试空间内布置三个固定基站使之位置固定，固定基站应尽量位于测试区域外围，当测试区域超出固定基站覆盖范围时，可适当增加固定基站数量。

b)测试前，将两移动基站安装于车辆上；测量车辆的轮廓特征点与两移动基站间水平横向距离、水平纵向距离；测量障碍物的轮廓特征点与两移动基站间水平横向距离、水平纵向距离；

c)控制主机初始化，获得固定基站的各个相对位置。控制主机通过三个固定基站相对位置确立测试坐标系，三个固定基站在测试坐标系中分别具有一个固定的坐标。

d)移动基站与固定基站间通过飞行测距法测得两者之间距离，该距离由固定基站发送至控制主机，控制主机获得各移动基站与固定基站的距离，得出移动基站在测试坐标系中的坐标，并基于3.2已测得的车辆轮廓数据、障碍物轮廓数据，控制主机可在测试坐标系中确定车辆轮廓、障碍物轮廓在测试坐标系中的位置。

e)测试过程中，移动基站、固定基站始终保持高频测距并实时发送给控制主机，控制主机将移动基站各时刻坐标点的连线即为车辆轨迹线、车辆轮廓各时刻的连线即为车辆轮廓包络线，车辆轮廓的坐标与障碍物的坐标经过坐标距离计算即为两者实际距离，

8.根据权利要求7所述的用于自动泊车测试设备的测试方法，其特征在于，当障碍物是移动物体时，可在障碍物上增设两个移动基站，测量方式同测试车辆。

9.根据权利要求7所述的用于自动泊车测试设备的测试方法，其特征在于，可根据实际需要，测量空间任意点到两移动基站的水平横向距离、水平纵向距离，此点在移动基站移动时可被实时输出与移动基站的距离。

10.根据权利要求7所述的用于自动泊车测试设备的测试方法，其特征在于，该设备增设显示器时，控制主机可描绘出俯视平面图以实时显示测试车辆、障碍物的运动情况和相对距离信息。

Images