CN112172800B - 铰接式车辆及其循迹泊车的控制方法和装置 - Google Patents

Abstract

本发明提供一种铰接式车辆及其循迹泊车的控制方法和装置，所述方法包括以下步骤：获取车头的质心参数；根据车头的质心参数获取车头铰接点的参数；根据车头铰接点的参数获取车尾铰接点的参数；根据车尾铰接点的参数获取车尾的质心参数；根据车尾的质心参数和预设的参考点参数计算转向指令；根据转向指令控制车辆的转向，在车辆位置达到停车点时，控制车辆停车。本发明的循迹泊车的控制方法，能够流畅泊车、且控制误差小，实现准确到达停车位置。

Description

铰接式车辆及其循迹泊车的控制方法和装置

技术领域

本发明涉及铰接式车辆技术领域，具体涉及一种铰接式车辆循迹泊车的控制方法、一种铰接式车辆循迹泊车的控制装置和一种铰接式车辆。

背景技术

现有技术涉及辅助铰接式车辆倒车的方法,其包括以下步骤：记录针对指定路径的第一铰接式车辆的预定数目的位置；记录在预定数目的位置处铰接式车辆的每个铰接接头的铰接角度；记录在预定数目的位置处第一铰接式车辆的方位；将指定路径的记录值保存在存储器中；通过使用记录值和铰接式车辆的尺寸信息来计算针对指定路径的第一铰接式车辆的扫掠区域；以及当铰接式车辆沿着指定路径倒车时使用扫掠区域来控制铰接式车辆的转向,使得铰接式车辆在倒车期间不延伸到扫掠区域外。上述技术方案需要事先记录预定数目位置的铰接角度，交接角度不易测量，操作比较繁琐，人工成本比较高，不适合批量应用。

发明内容

本发明为解决上述技术问题，提供了一种铰接式车辆循迹泊车的控制方法，能够流畅泊车、且控制误差小，实现准确到达停车位置。

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

一种铰接式车辆循迹泊车的控制方法，所述方法包括以下步骤：获取车头的质心参数；根据所述车头的质心参数获取车头铰接点的参数；根据所述车头铰接点的参数获取车尾铰接点的参数；根据所述车尾铰接点的参数获取车尾的质心参数；根据所述车尾的质心参数和预设的参考点参数计算转向指令；根据所述转向指令控制所述车辆的转向，在所述车辆位置达到停车点时，控制所述车辆停车。

根据本发明的一个实施例，所述车头的质心参数、所述车头铰接点、所述车尾铰接点和所述车尾的质心参数分别包括：位置、速度、姿态和姿态角速度，其中，所述姿态包括：航向角，所述姿态角速度包括：航向角速度，根据所述车头的质心参数获取车头铰接点的参数，包括：采用杆臂补偿的方式将所述车头的质心位置和质心速度进行转换，以获取所述车头铰接点的位置和速度；将所述车头质心的姿态和姿态角速度作为所述车头铰接点的姿态和姿态角速度。

根据本发明的一个实施例，根据所述车头铰接点的参数获取车尾铰接点的参数，包括：获取所述车辆的铰接角；对所述车辆的铰接角度进行差分均值滤波，以获得所述车辆的铰接角速度；根据所述车头铰接点的航向角、所述车头铰接点的航向角速度、所述车辆的铰接角和所述车辆的铰接角度速度获取所述车尾铰接点的航向角和航向角速度；将所述车头铰接点的位置和速度作为所述车尾铰接点的位置和速度。

根据本发明的一个实施例，根据所述车头铰接点的航向角、所述车头铰接点的航向角速度、所述车辆的铰接角度和所述车辆的铰接角度速度获取所述车尾铰接点的航向角和航向角速度，包括：将所述车头铰接点的航向角与所述车辆的铰接角之和作为所述车尾铰接点的航向角；将所述车头铰接点的航向角速度与所述车辆的铰接角度速度之和作为所述车尾铰接点的航向角速度。

根据本发明的一个实施例，根据所述车尾铰接点的参数获取车尾的质心参数，包括：采用杆臂补偿的方式将所述车尾铰接点的位置和速度进行转换，以获取所述车尾质心的位置和速度；将所述车尾铰接点的航向角和航向角速度作为所述车尾质心的航向角和航向角速度。

根据本发明的一个实施例，根据所述车尾的质心参数和预设的参考点参数计算转向指令，包括：计算所述车尾的质心位置、速度、航向角和航向角速度分别与所述预设的参考点的位置、速度、航向角和航向角速度之间的误差；根据位置误差、速度误差、航向角误差和航向角速度误差计算所述转向指令。

根据本发明的一个实施例，通过下述公式计算所述位置误差和所述速度误差：

，

其中，

表示所述位置误差，

和

表示所述车尾的质心位置坐标，

和

表示所述预设参考点的位置坐标，

表示航向角，

表示所述预设参考点的 航向角，

表示所述速度误差，

表示所述车尾质心的东向速度，

表示所述预 设参考点的东向速度，

表示所述车尾质心的北向速度，

表示所述预设参考点的北 向速度。

根据本发明的一个实施例，通过下述公式计算所述转向指令：

，

其中，

表示所述转向指令，

、

、

和

表示常数，

表示所 述车尾质心的航向角速度，

表示预设的参考点的航向角速度。

本发明还提出了一种铰接式车辆循迹泊车的控制装置，包括：第一获取模块，用于获取车头的质心参数；第二获取模块，用于根据所述车头的质心参数获取车头铰接点的参数；第三获取模块，用于根据所述车头铰接点的参数获取车尾铰接点的参数；第四获取模块，用于根据所述车尾铰接点的参数获取车尾的质心参数；计算模块，用于根据所述车尾的质心参数和预设的参考点参数计算转向指令；控制模块，用于根据所述转向指令控制所述车辆的转向，在所述车辆位置达到停车点时，控制所述车辆停车。

本发明还提出了一种铰接式车辆，包括上述的铰接式车辆循迹泊车的控制装置。

本发明的有益效果：

本发明能够在现有的铰接式车辆上已经具备的传感器的基础上，通过控制逻辑实现流畅泊车、且控制误差小，实现准确到达停车位置。

附图说明

图1为本发明实施例的铰接式车辆循迹泊车的控制方法的流程图；

图2为本发明一个实施例的车头质心的位置和速度转换到车头铰接点的示意图；

图3为本发明一个实施例的铰接式车辆泊车的示意图；

图4为本发明实施例的铰接式车辆循迹泊车的控制装置的方框示意图；

图5为本发明实施例的车辆的方框示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1为本发明实施例的铰接式车辆循迹泊车的控制方法的流程图。

如图1所示，本发明实施例的铰接式车辆循迹泊车的控制方法可包括以下步骤：

S1，获取车头的质心参数。

在本发明的一个实施例中，车头质心参数可包括：车头质心的位置、速度、姿态和姿态角速度，其中，质心的位置包括经度、纬度和高度，速度包括东、北、天三个方向的速度，姿态可包括航向角、俯仰角和横滚角，姿态角速度包括：航向角速度、俯仰角速度和横滚角速度。由于目前的车上装有惯性导航，且定位传感器安装在车辆的车头位置，因此可以获得车头质心的参数。

S2，根据车头的质心参数获取车头铰接点的参数。其中，车头铰接点的参数包括：车头铰接点的位置、车头铰接点的速度，车头铰接点的姿态和车头铰接点的姿态角速度。

根据本发明的一个实施例，根据车头的质心参数获取车头铰接点的参数，包括：采用杆臂补偿的方式将车头的质心位置和质心速度进行转换，以获取车头铰接点的位置和速度；将车头质心的姿态和姿态角速度作为车头铰接点的姿态和姿态角速度。

具体而言，由于车头质心与车头铰接点在同一个刚体上，因此不用转换姿态以及姿态角速度，只需要将车头质心的位置和速度进行转换，得到车头铰接点的位置和速度。也就是说，车头质心的姿态（航向角、俯仰角、横滚角）和姿态角速度（航向角速度、俯仰角速度、横滚角速度）即为车头铰接点的姿态和姿态角速度。

采用杆臂补偿的方式将车头质心的位置和速度转换到车头铰接点的位置和速度，具体的转换过程如下所述：

如图2所示，假设车头铰接点相对于地心O的矢量为R，车头质心相对于地心O的矢 量为r，车头铰接点相对于车头质心的矢量为

，三者之间的矢量关系满足：

。

根据图1中的相对位置关系，结合相对求导公式，可以得到车头铰接点与车头质心位置关系：

式中，

，

，

，

，

。

其中，

表示车头铰接点的速度矢量，

表示车头质心的速度矢量，

表示在车体系（b系）相对于导航坐标系（n系）的姿态矩阵，

分别表示航向 角、俯仰角和横滚角，

表示

，

表示

，

表示

，

表 示

，

表示

，

表示

，

表示向量

对应的叉乘矩 阵，

为b系相对于n系的角速度，可近似为IMU（Inertial Measurement Unit，惯性测量 仪）的陀螺仪输出，

表示车头铰接点相对于车头质心的矢量，

表示车头铰接点的 位置矢量，

表示车头质心的位置矢量，

表示参数矩阵，

表示纬度L的正 割函数，

为高度h位置子午圈主曲率半径，

为高度h位置卯酉圈主曲率半径，

为子午圈主曲率半径，

为卯酉圈主曲率半径，B表示经度，a表示地球长轴半径，b 表示地球短轴半径，e表示地球椭圆面的离心率。

S3，根据车头铰接点的参数获取车尾铰接点的参数。

根据本发明的一个实施例，根据车头铰接点的参数获取车尾铰接点的参数，包括：获取车辆的铰接角；对车辆的铰接角度进行差分均值滤波，以获得车辆的铰接角速度；根据车头铰接点的航向角、车头铰接点的航向角速度、车辆的铰接角和车辆的铰接角度速度获取车尾铰接点的航向角和航向角速度；将车头铰接点的位置和速度作为车尾铰接点的位置和速度。

进一步地，根据本发明的一个实施例，根据车头铰接点的航向角、车头铰接点的航向角速度、车辆的铰接角度和车辆的铰接角度速度获取车尾铰接点的航向角和航向角速度，包括：将车头铰接点的航向角与车辆的铰接角之和作为车尾铰接点的航向角；将车头铰接点的航向角速度与车辆的铰接角度速度之和作为车尾铰接点的航向角速度。

具体而言，由于车头铰接点和车尾铰接点是同一个点，所以车头交接点的位置和速度即为车尾铰接点的位置和速度，但是，车头交接点固定在车头部分，而车尾铰接点固定在车尾部分，所以车头铰接点与车尾铰接点的航向角和航向角速度不同。

通过铰接角度传感器获取铰接角度，如图3中的α角，对获取的铰接角度进行差分均值滤波，可以得到铰接角速度ω′，假设，获取一些列铰接角度值α₁、α₂、α₃、α₄、α₅、α₆、α₇、α₈、α₉、α₁₀、α₁₁，计算周期为T，那么可以对应获取铰接角速度的值ω₁′=（α₂-α₁）/T，…，ω₁₀′=（α₁₁-α₁₀）/T，设置滤波长度为10，可以得到铰接角速度ω′=（ω₁′+ω₂′+…+ω₁₀′）/10。可以理解的是，如果滤波长度不够10的话，那么得到几个数据就按照几个数据求平均值，使用滤波的目的是为了消除差分的噪音。

假设车头铰接点的航向角为

（与车头质心的航向角相同）、航向角速度为

（与车头质心的航向角速度相同），车尾铰接点的航向角为

、航向角速度为

，那么通 过以下公式即可计算得到车尾铰接点的航向角为

、航向角速度为

：

，

。

S4，根据车尾铰接点的参数获取车尾的质心参数。

根据本发明的一个实施例，根据所述车尾铰接点的参数获取车尾的质心参数，包括：采用杆臂补偿的方式将车尾铰接点的位置和速度进行转换，以获取车尾质心的位置和速度；将车尾铰接点的航向角和航向角速度作为车尾质心的航向角和航向角速度。

与上述实施例中根据车头的质心参数获取车头的铰接点参数相同的道理，利用杆臂补偿的方式，根据车尾铰接点的参数获取车尾的质心参数。其中，车尾铰接点与车尾质心在同一个刚体上，因此不需要转换姿态和姿态角速度，只需要将车尾铰接点的位置和速度进行转换。具体地转换方式参照上述的实施例，这里不再详述。

S5，根据车尾的质心参数和预设的参考点参数计算转向指令。

其中，预设的参考点的获取方式为：图3中有一系列目标轨迹点（每个参考点信息有：位置、东北天三个方向的速度、航向角、航向角速度）。将车尾质心的位置坐标转换到与参考点相同的坐标系中，根据车尾的质心位置查找与其最近的参考点，作为预设的参考点。

根据本发明的一个实施例，根据车尾的质心参数和预设的参考点参数计算转向指令，包括：计算车尾的质心位置、速度、航向角和航向角速度分别与预设的参考点的位置、速度、航向角和航向角速度之间的误差；根据位置误差、速度误差、航向角误差和航向角速度误差计算转向指令。

进一步地，在本发明的一个实施例中，通过下述公式计算所述位置误差和所述速度误差：

，

其中，

表示位置误差，

和

表示车尾的质心位置坐标，

和

表 示预设参考点的位置坐标，

表示航向角，

表示所述预设参考点的航向角，

表示速度误差，

表示车尾质心的东向速度，

表示预设参考点的东向速度，

表示 车尾质心的北向速度，

表示预设参考点的北向速度。

通过下述公式计算转向指令：

，

其中，

表示转向指令，

、

、

和

表示常数，

表示车尾质心 的航向角速度，

表示预设的参考点的航向角速度。

具体而言，假设预设的参考点的参数包括：位置坐标（

，

），东向速度

，北向速度

，航向角

、航向角速度

，车尾的质心的参数为：质心位置 坐标（

，

），东向速度

，北向速度

，航向角

、航向角速度

，那么，车辆的横 向误差和横向速度误差如下所示：

式中，两公式里的

、

涉及到把预设的参考点到车尾的质心的位 置矢量、速度矢量转换到预设的参考点坐标系（坐标原点为预设的参考点，纵坐标为预设的 参考点的切线方向，如图3中预设的参考点的坐标系）中，横坐标即为横向位置误差、横向速 度误差。需要说明的是，航向北向为0度，北偏西为正，北偏东为负。

根据实时计算的横向位置误差

、横向速度误差

、航向角误差

、航向角速度误差

。采用PD控制算法，通过以下公式计算转向指令：

。其中，

具有正负的区分，正负号表示控制车辆转向的反向，例如，当

为负值时，控制车 辆向左转，当

为正值时，控制车辆向右转。

可以理解的是，为了减小上述几次转换过程中的噪声，还可对获取的转向指令进行低通滤波。

S6，根据转向指令控制车辆的转向，在车辆位置达到停车点时，控制车辆停车。

也就是说，实时判断车辆位置与泊车停车点的位置之间的关系，根据转向指令控制车辆进行转向，当车辆达到泊车停车点时，控制车辆停车，从而无需额外增加定位传感器，且不需要预先记录预定数目位置的铰接角度，利用现有的车头部分设置的定位传感器，进行车头的质心参数和车尾的质心参数的转换，实现流畅泊车，且控制误差小，能够准确的到达停车点停车。

综上所述，本发明的控制方法，根据车头的质心参数获取车头铰接点的参数；根据车头铰接点的参数获取车尾铰接点的参数；根据车尾铰接点的参数获取车尾的质心参数；根据车尾的质心参数和预设的参考点参数计算转向指令；根据转向指令控制车辆的转向，在车辆位置达到停车点时，控制车辆停车。从而实现流畅泊车、且控制误差小，实现准确到达停车位置。

对应上述实施例，本发明还提出了一种铰接式车辆循迹泊车的控制装置。

图4为本发明实施例的铰接式车辆循迹泊车的控制装置的方框示意图。

如图4所示，本发明实施例的铰接式车辆循迹泊车的控制装置可包括：第一获取模块10、第二获取模块20、第三获取模块30、第四获取模块40、计算模块50和控制模块60。

其中，第一获取模块10用于获取车头的质心参数。第二获取模块20用于根据车头的质心参数获取车头铰接点的参数。第三获取模块30用于根据车头铰接点的参数获取车尾铰接点的参数。第四获取模块40用于根据车尾铰接点的参数获取车尾的质心参数。计算模块50用于根据车尾的质心参数和预设的参考点参数计算转向指令。控制模块60用于根据转向指令控制车辆的转向，在车辆位置达到停车点时，控制车辆停车。

根据本发明的一个实施例，车头的质心参数、车头铰接点、车尾铰接点和车尾的质心参数分别包括：位置、速度、姿态和姿态角速度，其中，姿态包括：航向角，姿态角速度包括：航向角速度，第二获取模块20根据车头的质心参数获取车头铰接点的参数，具体用于，采用杆臂补偿的方式将车头的质心位置和质心速度进行转换，以获取车头铰接点的位置和速度；将车头质心的姿态和姿态角速度作为车头铰接点的姿态和姿态角速度。

根据本发明的一个实施例，第三获取模块30根据车头铰接点的参数获取车尾铰接点的参数，具体用于，获取车辆的铰接角；对车辆的铰接角度进行差分均值滤波，以获得车辆的铰接角速度；根据车头铰接点的航向角、车头铰接点的航向角速度、车辆的铰接角和车辆的铰接角度速度获取车尾铰接点的航向角和航向角速度；将车头铰接点的位置和速度作为车尾铰接点的位置和速度。

根据本发明的一个实施例，第三获取模块30根据车头铰接点的航向角、车头铰接点的航向角速度、车辆的铰接角度和车辆的铰接角度速度获取车尾铰接点的航向角和航向角速度，具体用于，将车头铰接点的航向角与车辆的铰接角之和作为车尾铰接点的航向角；将车头铰接点的航向角速度与车辆的铰接角度速度之和作为车尾铰接点的航向角速度。

根据本发明的一个实施例，第四获取模块40根据车尾铰接点的参数获取车尾的质心参数，具体用于，采用杆臂补偿的方式将车尾铰接点的位置和速度进行转换，以获取车尾质心的位置和速度；将车尾铰接点的航向角和航向角速度作为车尾质心的航向角和航向角速度。

根据本发明的一个实施例，计算模块50根据车尾的质心参数和预设的参考点参数计算转向指令，包括：计算车尾的质心位置、速度、航向角和航向角速度分别与预设的参考点的位置、速度、航向角和航向角速度之间的误差；根据位置误差、速度误差、航向角误差和航向角速度误差计算转向指令。

根据本发明的一个实施例，计算模块50通过下述公式计算位置误差和速度误差：

，

其中，

表示位置误差，

和

表示车尾的质心位置坐标，

和

表 示预设参考点的位置坐标，

表示航向角，

表示所述预设参考点的航向角，

表示速度误差，

表示车尾质心的东向速度，

表示预设参考点的东向速度，

表示 车尾质心的北向速度，

表示预设参考点的北向速度。

根据本发明的一个实施例，计算模块50通过下述公式计算转向指令：

，

其中，

表示转向指令，

、

、

和

表示常数，

表示车尾质心 的航向角速度，

表示预设的参考点的航向角速度。

需要说明的是，本发明实施例的铰接式车辆循迹泊车的控制装置中未披露的细节，请参照本发明实施例的铰接式车辆循迹泊车的控制方法中所披露的细节，具体这里不再赘述。

根据本发明实施例的铰接式车辆循迹泊车的控制装置，第一获取模块获取车头的质心参数，第二获取模块根据车头的质心参数获取车头铰接点的参数，第三获取模块根据车头铰接点的参数获取车尾铰接点的参数，第四获取模块根据车尾铰接点的参数获取车尾的质心参数，计算模块根据车尾的质心参数和预设的参考点参数计算转向指令，控制模块根据转向指令控制车辆的转向，在车辆位置达到停车点时，控制车辆停车。由此，能够流畅泊车、且控制误差小，实现准确到达停车位置。

对应上述实施例，本发明还提出了一种铰接式车辆。

图5为本发明实施例的车辆的方框示意图。

如图5所示，本发明实施例的车辆100可包括：上述的铰接式车辆循迹泊车的控制装置110。

本发明实施例的车辆，通过上述的铰接式车辆循迹泊车的控制装置，能够流畅泊车、且控制误差小，实现准确到达停车位置。

对应上述实施例，本发明还提出一种计算机设备。

本发明实施例的计算机设备包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行该计算机程序时，可实现根据本发明上述实施例所述的铰接式车辆循迹泊车的控制方法。

根据本发明实施例的计算机设备，处理器执行存储在存储器上的计算机程序时，首先获取车头的质心参数，然后根据车头的质心参数获取车头铰接点的参数，接着根据车头铰接点的参数获取车尾铰接点的参数，根据车尾铰接点的参数获取车尾的质心参数，然后根据车尾的质心参数和预设的参考点参数计算转向指令，最后根据转向指令控制车辆的转向，在车辆位置达到停车点时，控制车辆停车，从而能够流畅泊车、且控制误差小，实现准确到达停车位置。

对应上述实施例，本发明还提出一种非临时性计算机可读存储介质。

本发明实施例的非临时性计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时可实现根据本发明上述实施例所述的铰接式车辆循迹泊车的控制方法。

根据本发明实施例的非临时性计算机可读存储介质，处理器执行存储在其上的计算机程序时，首先获取车头的质心参数，然后根据车头的质心参数获取车头铰接点的参数，接着根据车头铰接点的参数获取车尾铰接点的参数，根据车尾铰接点的参数获取车尾的质心参数，然后根据车尾的质心参数和预设的参考点参数计算转向指令，最后根据转向指令控制车辆的转向，在车辆位置达到停车点时，控制车辆停车，从而能够流畅泊车、且控制误差小，实现准确到达停车位置。

对应上述实施例，本发明还提出一种计算机程序产品。

当本发明实施例的计算机程序产品中的指令由处理器执行时，可执行根据本发明上述实施例所述的铰接式车辆循迹泊车的控制方法。

根据本发明实施例的计算机程序产品，处理器执行其中的指令时，首先获取车头的质心参数，然后根据车头的质心参数获取车头铰接点的参数，接着根据车头铰接点的参数获取车尾铰接点的参数，根据车尾铰接点的参数获取车尾的质心参数，然后根据车尾的质心参数和预设的参考点参数计算转向指令，最后根据转向指令控制车辆的转向，在车辆位置达到停车点时，控制车辆停车，从而能够流畅泊车、且控制误差小，实现准确到达停车位置。

在本发明的描述中，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。“多个”的含义是两个或两个以上，除非另有明确具体的限定。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必针对相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。

在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行***、装置或设备（如基于计算机的***、包括处理器的***或其他可以从指令执行***、装置或设备取指令并执行指令的***）使用，或结合这些指令执行***、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行***、装置或设备或结合这些指令执行***、装置或设备而使用的装置。计算机可读介质的更具体的示例（非穷尽性列表）包括以下：具有一个或多个布线的电连接部（电子装置），便携式计算机盘盒（磁装置），随机存取存储器（RAM），只读存储器（ROM），可擦除可编辑只读存储器（EPROM或闪速存储器），光纤装置，以及便携式光盘只读存储器（CDROM）。另外，计算机可读介质甚至可以是可在其上打印程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得程序，然后将其存储在计算机存储器中。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行***执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列（PGA），现场可编程门阵列（FPGA）等。

本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。

此外，在本发明各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。

上述提到的存储介质可以是只读存储器，磁盘或光盘等。尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (10)

1.一种铰接式车辆循迹泊车的控制方法，其特征在于，所述方法包括以下步骤：

获取车头的质心参数；

根据所述车头的质心参数获取车头铰接点的参数；

根据所述车头铰接点的参数获取车尾铰接点的参数；

根据所述车尾铰接点的参数获取车尾的质心参数；

根据所述车尾的质心参数和预设的参考点参数计算转向指令；

根据所述转向指令控制所述车辆的转向，在所述车辆位置达到停车点时，控制所述车辆停车。

2.根据权利要求1所述的铰接式车辆循迹泊车的控制方法，其特征在于，所述车头的质心参数、所述车头铰接点、所述车尾铰接点和所述车尾的质心参数分别包括：位置、速度、姿态和姿态角速度，其中，所述姿态包括：航向角，所述姿态角速度包括：航向角速度，根据所述车头的质心参数获取车头铰接点的参数，包括：

采用杆臂补偿的方式将所述车头的质心位置和质心速度进行转换，以获取所述车头铰接点的位置和速度；

将所述车头质心的姿态和姿态角速度作为所述车头铰接点的姿态和姿态角速度。

3.根据权利要求2所述的铰接式车辆循迹泊车的控制方法，其特征在于，根据所述车头铰接点的参数获取车尾铰接点的参数，包括：

获取所述车辆的铰接角；

对所述车辆的铰接角度进行差分均值滤波，以获得所述车辆的铰接角速度；

根据所述车头铰接点的航向角、所述车头铰接点的航向角速度、所述车辆的铰接角和所述车辆的铰接角度速度获取所述车尾铰接点的航向角和航向角速度；

将所述车头铰接点的位置和速度作为所述车尾铰接点的位置和速度。

4.根据权利要求3所述的铰接式车辆循迹泊车的控制方法，其特征在于，根据所述车头铰接点的航向角、所述车头铰接点的航向角速度、所述车辆的铰接角度和所述车辆的铰接角度速度获取所述车尾铰接点的航向角和航向角速度，包括：

将所述车头铰接点的航向角与所述车辆的铰接角之和作为所述车尾铰接点的航向角；

将所述车头铰接点的航向角速度与所述车辆的铰接角度速度之和作为所述车尾铰接点的航向角速度。

5.根据权利要求4所述的铰接式车辆循迹泊车的控制方法，其特征在于，根据所述车尾铰接点的参数获取车尾的质心参数，包括：

采用杆臂补偿的方式将所述车尾铰接点的位置和速度进行转换，以获取所述车尾质心的位置和速度；

将所述车尾铰接点的航向角和航向角速度作为所述车尾质心的航向角和航向角速度。

6.根据权利要求5所述的铰接式车辆循迹泊车的控制方法，其特征在于，根据所述车尾的质心参数和预设的参考点参数计算转向指令，包括：

计算所述车尾的质心位置、速度、航向角和航向角速度分别与所述预设的参考点的位置、速度、航向角和航向角速度之间的误差；

根据位置误差、速度误差、航向角误差和航向角速度误差计算所述转向指令。

7.根据权利要求6所述的铰接式车辆循迹泊车的控制方法，其特征在于，通过下述公式计算所述位置误差和所述速度误差：

，

其中，

表示所述位置误差，

和

表示所述车尾的质心位置坐标，

和

表示所述预设的参考点的位置坐标，

表示车尾质心的航向角，

表示所述预设 的参考点的航向角，

表示所述速度误差，

表示所述车尾质心的东向速度，

表示所述预设的参考点的东向速度，

表示所述车尾质心的北向速度，

表示所述预 设的参考点的北向速度。

8.根据权利要求7所述的铰接式车辆循迹泊车的控制方法，其特征在于，通过下述公式计算所述转向指令：

，

其中，

表示所述转向指令，

、

、

和

表示常数，

表示所述车 尾质心的航向角速度，

表示预设的参考点的航向角速度。

9.一种铰接式车辆循迹泊车的控制装置，其特征在于，包括：

第一获取模块，用于获取车头的质心参数；

第二获取模块，用于根据所述车头的质心参数获取车头铰接点的参数；

第三获取模块，用于根据所述车头铰接点的参数获取车尾铰接点的参数；

第四获取模块，用于根据所述车尾铰接点的参数获取车尾的质心参数；

计算模块，用于根据所述车尾的质心参数和预设的参考点参数计算转向指令；

控制模块，用于根据所述转向指令控制所述车辆的转向，在所述车辆位置达到停车点时，控制所述车辆停车。

10.一种铰接式车辆，其特征在于，包括根据权利要求9所述的铰接式车辆循迹泊车的控制装置。

Images