CN114707103A - 一种基于单一感知数据的多拖车行驶轨迹计算方法 - Google Patents

一种基于单一感知数据的多拖车行驶轨迹计算方法 Download PDF

Info

Publication number
CN114707103A
CN114707103A CN202210627450.8A CN202210627450A CN114707103A CN 114707103 A CN114707103 A CN 114707103A CN 202210627450 A CN202210627450 A CN 202210627450A CN 114707103 A CN114707103 A CN 114707103A
Authority
CN
China
Prior art keywords
trailer
time
angle
curve
frequency
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202210627450.8A
Other languages
English (en)
Other versions
CN114707103B (zh
Inventor
杨秀清
陈翼
谌泓宇
张建权
向勇
廖诗来
李貌
马俊勇
王小锐
陈柏宇
李支茂
何威
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Civil Aviation Logistics Technology Co ltd
Original Assignee
Civil Aviation Logistics Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Civil Aviation Logistics Technology Co ltd filed Critical Civil Aviation Logistics Technology Co ltd
Priority to CN202210627450.8A priority Critical patent/CN114707103B/zh
Publication of CN114707103A publication Critical patent/CN114707103A/zh
Application granted granted Critical
Publication of CN114707103B publication Critical patent/CN114707103B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Physics (AREA)
  • Theoretical Computer Science (AREA)
  • Algebra (AREA)
  • Computational Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)

Abstract

本发明公开了一种基于单一感知数据的多拖车行驶轨迹计算方法，涉及拖车行驶轨迹计算技术领域，目的在于实现更精准的拖车轨迹计算，通过传感器获取牵引车的位姿1号拖车的牵引杆角度，从n=1开始依次执行以下步骤直到n=k，以得到所有拖车的位姿点：根据时间值为
Figure 595068DEST_PATH_IMAGE001
时n号拖车的牵引杆角度、n‑1号拖车的位姿点，以及前一时刻n号拖车的位姿点，获取n号拖车运动转弯中心；根据拖车运动转弯中心以及
Figure 964869DEST_PATH_IMAGE001
前一时刻的n号拖车的位姿点获取此时n号拖车的位姿点；本发明具有计算精度和可靠性高的优点。

Description

一种基于单一感知数据的多拖车行驶轨迹计算方法
技术领域
本发明涉及拖车行驶轨迹计算技术领域，更具体的是涉及一种基于单一感知数据的多拖车行驶轨迹计算方法技术领域。
背景技术
目前由于疫情的冲击、机场特种车辆驾驶环境恶劣、驾驶员匮乏以及智慧机场建设的需求等，机场无人驾驶特种车辆逐渐成为智能驾驶的先发落地场景，首先被作为应用目标的就是牵引车。
和普通车辆不同，牵引车常常会在其后方拖挂多辆，通常来说是1到6辆不等的拖车，因此需要合理地对牵引车的路径进行规划，以避免后方拖挂车和其他车辆以及路边设施产生碰撞，发生危险。为此，需要首先对拖车的轨迹进行获取。
目前的拖车轨迹计算主要方法都是通过传感器感知或者图像对拖车轨迹进行获取，有可能存在雨雾天感知不精确，以及视野被所拖挂货物遮挡等问题。有部分通过计算得到拖车轨迹的方法，但或是未考虑地面摩擦、载重等外界实时影响，或是只通过前一节拖车的运动推算后一节拖车的轨迹，未考虑拖车之间运动状态差异的变化。
发明内容
本发明的目的在于：实现更精准的拖车轨迹计算。为了解决上述技术问题，本发明提供一种基于单一感知数据的多拖车行驶轨迹计算方法。
本发明为了实现上述目的具体采用以下技术方案：
一种基于单一感知数据的多拖车行驶轨迹计算方法，牵引车后有多辆拖车通过牵引杆依次首尾相连，按从前往后的顺序拖车的标号n依次记为1到k，将牵引车视为0号拖车；拖车的主要位姿点包括拖车牵引杆前端、前轮轴线中点、后轮轴线中点以及车尾拖挂处；牵引车的主要位姿点包括前轮轴线中点、后轮轴线中点以及车尾拖挂处；
将n号拖车的牵引杆前端、前轮轴线中点、后轮轴线中点以及车尾拖挂处在前一时刻的坐标分别标记为
Figure 835663DEST_PATH_IMAGE001
、
Figure 450315DEST_PATH_IMAGE002
、
Figure 606358DEST_PATH_IMAGE003
和
Figure 495817DEST_PATH_IMAGE004
，当前时刻记为
Figure 233966DEST_PATH_IMAGE005
，其数值代表距离初始时间的单位时间的个数，此时的相应坐标分别标记为
Figure 550678DEST_PATH_IMAGE006
、
Figure 887026DEST_PATH_IMAGE007
、
Figure 580175DEST_PATH_IMAGE008
和
Figure 172831DEST_PATH_IMAGE009
，表示当前时刻
Figure 129285DEST_PATH_IMAGE005
和前一时刻相差一个单位时间；
多拖车行驶轨迹计算方法包括，通过传感器获取时间值为
Figure 259921DEST_PATH_IMAGE005
时牵引车的位姿点和1号拖车的牵引杆角度，从n=1开始依次执行以下步骤直到n=k，以得到所有拖车的位姿点：
根据
Figure 491183DEST_PATH_IMAGE010
时刻n号拖车的牵引杆角度
Figure 938344DEST_PATH_IMAGE011
以及
Figure 596859DEST_PATH_IMAGE005
时刻n-1号拖车的位姿点，获取
Figure 762261DEST_PATH_IMAGE007
；
根据
Figure 531634DEST_PATH_IMAGE007
和
Figure 833302DEST_PATH_IMAGE002
获取n号拖车运动转弯中心
Figure 413450DEST_PATH_IMAGE012
；
根据
Figure 66149DEST_PATH_IMAGE013
以及
Figure 373633DEST_PATH_IMAGE010
前一时刻的n号拖车的位姿点获取时间值为
Figure 795387DEST_PATH_IMAGE014
时的n号拖车的位姿点。
优选地，所述获取
Figure 795704DEST_PATH_IMAGE015
的方法为：
Figure 670119DEST_PATH_IMAGE016
；
Figure 46874DEST_PATH_IMAGE017
;
其中，L为
Figure 41244DEST_PATH_IMAGE018
和
Figure 681304DEST_PATH_IMAGE019
间的距离，代表拖车车身的长度，
Figure 511856DEST_PATH_IMAGE020
为
Figure 177455DEST_PATH_IMAGE018
和
Figure 777064DEST_PATH_IMAGE021
间的距离，代表n号拖车牵引杆的长度。
优选地，n大于1时所述时间值为
Figure 119183DEST_PATH_IMAGE022
时n号拖车的牵引杆角度
Figure 968191DEST_PATH_IMAGE023
的获取方法包括以下步骤：
通过以下方程求解n号拖车的牵引杆角度变化相对1号拖车的时间平移量
Figure 421169DEST_PATH_IMAGE024
：
Figure 672022DEST_PATH_IMAGE025
；
其中
Figure 434310DEST_PATH_IMAGE026
代表牵引车在
Figure 770613DEST_PATH_IMAGE027
时刻的速度，
Figure 27282DEST_PATH_IMAGE028
代表单位时间；
每单位时间记载一次1号拖车的牵引杆角度，得到1号拖车在时间值为0到
Figure 335904DEST_PATH_IMAGE022
时的牵引杆角度的时域变化曲线
Figure 82143DEST_PATH_IMAGE029
，n号拖车在时间值不小于
Figure 577847DEST_PATH_IMAGE024
时的牵引杆角度的时域变化曲线为：
Figure 654518DEST_PATH_IMAGE030
；
通过傅里叶展开将牵引杆角度的时域变化曲线
Figure 614384DEST_PATH_IMAGE031
拆解为时域低频率变化曲线
Figure 734787DEST_PATH_IMAGE032
和时域高频率变化曲线
Figure 983365DEST_PATH_IMAGE033
，大于
Figure 847416DEST_PATH_IMAGE034
的频率视为高频率，其他视为低频率，
Figure 661788DEST_PATH_IMAGE034
是用于划分高频率和低频率的频率阈值；
通过高斯滤波对时域低频变化曲线
Figure 467939DEST_PATH_IMAGE035
进行滤波处理得到
Figure 938235DEST_PATH_IMAGE036
；
将高斯滤波后的低频变化曲线和高频变化曲线相加，得到n号拖车最终的牵引杆角度的变化曲线:
Figure 137135DEST_PATH_IMAGE037
；
所述时间值为
Figure 9276DEST_PATH_IMAGE022
时的n号拖车的牵引杆角度
Figure 268219DEST_PATH_IMAGE023
为：
Figure 225811DEST_PATH_IMAGE038
。
优选地，所述拆解为时域高频率变化曲线和时域低频率变化曲线包括以下步骤：
将牵引杆角度随时间的变化曲线转换为频域曲线， n号拖车频率k时的牵引杆角度为：
Figure 493981DEST_PATH_IMAGE039
；
以所述
Figure 971361DEST_PATH_IMAGE034
为基准进行拆分，分别得到频域高频率变化曲线和频域低频率变化曲线；
将频域高频率变化曲线和频域低频率变化曲线转为时域的高频率变化曲线和低频率变化曲线，转换方法如下：
Figure 401205DEST_PATH_IMAGE040
。
优选地，所述通过高斯滤波对低频变化曲线进行滤波处理的方法为：
Figure 846093DEST_PATH_IMAGE041
；
Figure 652375DEST_PATH_IMAGE042
；
其中，当n=2时，
Figure 233529DEST_PATH_IMAGE043
=1，否则
Figure 99854DEST_PATH_IMAGE044
=3。
优选地，所述获取n号拖车运动在当前时刻的转弯中心
Figure 766459DEST_PATH_IMAGE045
的方法为：
Figure 376432DEST_PATH_IMAGE046
的中垂线与过
Figure 326939DEST_PATH_IMAGE019
的
Figure 98586DEST_PATH_IMAGE047
的垂线的交点为所述转弯中心
Figure 518066DEST_PATH_IMAGE045
；
Figure 869413DEST_PATH_IMAGE046
代表以
Figure 690738DEST_PATH_IMAGE018
和
Figure 587281DEST_PATH_IMAGE015
的连线，
Figure 494057DEST_PATH_IMAGE047
代表
Figure 117937DEST_PATH_IMAGE018
和
Figure 590506DEST_PATH_IMAGE048
的连线。
优选地，所述获取时间值为
Figure 156486DEST_PATH_IMAGE049
时的n号拖车的位姿点的方法为：
Figure 347296DEST_PATH_IMAGE050
；
Figure 774866DEST_PATH_IMAGE051
；
其中，T表示
Figure 367521DEST_PATH_IMAGE022
前一时刻拖车上某一点的坐标，
Figure 855135DEST_PATH_IMAGE052
代表时间值为
Figure 470924DEST_PATH_IMAGE022
时的该点的坐标；直线行驶时，
Figure 764502DEST_PATH_IMAGE053
=0，
Figure 165658DEST_PATH_IMAGE054
为单位矩阵；转弯时，
Figure 761856DEST_PATH_IMAGE055
，
Figure 927258DEST_PATH_IMAGE056
为车辆转向行驶绕所述
Figure 945899DEST_PATH_IMAGE045
驶过的角度。
本发明的有益效果如下：
通过选取牵引杆角度作为单一变量，在计算过程中不仅可以考虑单辆拖车的行驶状态，还可以考虑任意相邻两车的运动关系的变化特点，可以得到牵引车拖车组更可靠的轨迹结果；对于牵引杆角度的处理可以过滤和考虑地面摩擦和载重等外界因素对拖车行驶特性的影响，尤其是对拖车转向特点的影响，相比于纯计算过程可以得到更有参考性的结果；实际的数据采集只针对1号拖车进行，数据的获取简单可靠，且成本低。
附图说明
图1是本发明的流程示意图；
图2是实施例1中对多个拖车位姿点标记的示意图;
图3是实施例2中的拖车轨迹坐标图；
图4是实施例2中时域低频率变化曲线和时域高频率变化曲线的示意图；
图5是实施例2中实测的拖车轨迹图；
图6是实施例2中计算得到的拖车轨迹图。
具体实施方式
为使本发明的实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明的实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。
因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本发明领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本保护的范围。
实施例1
如图1所示，本实施例提供一种基于单一感知数据的多拖车行驶轨迹计算方法，牵引车后有多辆拖车通过牵引杆依次首尾相连，按从前往后的顺序拖车的标号n依次记为1到k，将牵引车视为0号拖车；拖车的主要位姿点包括拖车牵引杆前端、前轮轴线中点、后轮轴线中点以及车尾拖挂处；
将n号拖车的牵引杆前端、前轮轴线中点、后轮轴线中点以及车尾拖挂处在前一时刻的坐标分别标记为
Figure 247567DEST_PATH_IMAGE021
、
Figure 545824DEST_PATH_IMAGE018
、
Figure 136205DEST_PATH_IMAGE019
和
Figure 771586DEST_PATH_IMAGE057
，当前时刻记为
Figure 881756DEST_PATH_IMAGE058
，其数值代表距离初始时间的单位时间的个数，此时的相应坐标分别标记为
Figure 147652DEST_PATH_IMAGE059
、
Figure 22067DEST_PATH_IMAGE015
、
Figure 585773DEST_PATH_IMAGE060
和
Figure 799716DEST_PATH_IMAGE061
，当前时刻
Figure 236514DEST_PATH_IMAGE022
和前一时刻相差一个单位时间；特别说明的是，牵引车的主要位姿点只有前轮轴线中点、后轮轴线中点以及车尾拖挂处；一个具体的标记案例参阅图2。
多拖车行驶轨迹计算方法包括，通过传感器获取时间值为
Figure 598225DEST_PATH_IMAGE022
时牵引车的位姿点和1号拖车的牵引杆角度，从n=1开始依次执行以下步骤直到n=k，以得到所有拖车的位姿点：
根据
Figure 778671DEST_PATH_IMAGE022
时刻n号拖车的牵引杆角度
Figure 875151DEST_PATH_IMAGE023
以及
Figure 482850DEST_PATH_IMAGE022
时刻n-1号拖车的位姿点，获取
Figure 269540DEST_PATH_IMAGE015
；
根据
Figure 50415DEST_PATH_IMAGE015
和
Figure 504530DEST_PATH_IMAGE018
获取n号拖车运动转弯中心
Figure 266818DEST_PATH_IMAGE045
；
根据
Figure 540805DEST_PATH_IMAGE062
以及
Figure 859791DEST_PATH_IMAGE022
前一时刻的n号拖车的位姿点获取时间值为
Figure 60090DEST_PATH_IMAGE014
时的n号拖车的位姿点。
以上方法的核心思想是通过迭代计算获取每一节拖车的位姿点信息，同时在迭代过程中，引入由牵引车和第一节拖车牵引杆角度所推算出来的各拖车的牵引杆角度值，将拖车之间的联系以及摩擦力和载重等会对运动状态造成的影响考虑到迭代计算过程中，得到较好的拖车轨迹计算值。
特别说明的是本实施例默认拖挂的拖车数量和规格已知，即各拖车车身长度已知、牵引杆长度已知。
实施例2
本实施例基于实施例1的技术方案，具体步骤不再赘述,拖车行驶的一个坐标图案例见图3。
在本实施例中，所述获取
Figure 9591DEST_PATH_IMAGE015
的方法为：
Figure 833191DEST_PATH_IMAGE016
；
Figure 893551DEST_PATH_IMAGE017
;
其中，L为
Figure 571526DEST_PATH_IMAGE018
和
Figure 426349DEST_PATH_IMAGE019
间的距离，代表拖车车身的长度，
Figure 471666DEST_PATH_IMAGE020
为
Figure 335716DEST_PATH_IMAGE018
和
Figure 353351DEST_PATH_IMAGE021
间的距离，代表n号拖车牵引杆的长度，由于后一个拖车的牵引杆前端挂接在前一个拖车的车尾拖挂处，所以
Figure 441393DEST_PATH_IMAGE063
的值和
Figure 662421DEST_PATH_IMAGE059
相同。
其中，作为优选方案，由于1号拖车的牵引杆角度可以直接通过传感器获取，所以n大于1时所述时间值为
Figure 126900DEST_PATH_IMAGE022
时n号拖车的牵引杆角度
Figure 999041DEST_PATH_IMAGE023
的获取方法包括以下步骤：
通过对所述牵引车的车速和拖车的车长构建的方程进行求解，计算n号拖车的牵引杆角度变化相对1号拖车的时间平移量
Figure 257984DEST_PATH_IMAGE024
，此处意为视为理想状态下，n号拖车在当前时刻
Figure 215576DEST_PATH_IMAGE022
的牵引杆角度和时间
Figure 483746DEST_PATH_IMAGE024
以前1号拖车的牵引杆角度相同：
Figure 210394DEST_PATH_IMAGE025
；
其中
Figure 640238DEST_PATH_IMAGE026
代表牵引车在
Figure 599973DEST_PATH_IMAGE064
时刻的速度，
Figure 343938DEST_PATH_IMAGE028
代表单位时间；
每单位时间记载一次1号拖车的牵引杆角度，这些离散数值串联起即可得到1号拖车在时间值为0到
Figure 987409DEST_PATH_IMAGE022
时的牵引杆角度的时域变化曲线
Figure 791417DEST_PATH_IMAGE065
， 由于n号拖车的牵引杆角度视为与1号拖车时间
Figure 520338DEST_PATH_IMAGE066
之前的牵引杆角度相同，所以，n号拖车在时间值为0到
Figure 67994DEST_PATH_IMAGE022
时的牵引杆角度的时域变化曲线为：
Figure 565972DEST_PATH_IMAGE030
；
因为牵引杆角度在变化过程中，会同时经历微小高频振动以及由于转向改变造成的较低频变化，其中转向造成的低频变化量对每一节拖车相似，但会展现出内偏的特性；而振动特性造成的影响对每一节拖车有很多相同之处，因此需要通过傅里叶变换获取牵引杆角度不同频率的变化曲线，以实现对高频和低频特征分开处理的目的，进而提升计算的可靠性。
通过傅里叶展开将牵引杆角度的时域变化曲线
Figure 291613DEST_PATH_IMAGE067
拆解为时域低频率变化曲线
Figure 711093DEST_PATH_IMAGE068
和时域高频率变化曲线
Figure 859178DEST_PATH_IMAGE069
，大于
Figure 680504DEST_PATH_IMAGE034
的频率视为高频率，其他视为低频率，
Figure 826314DEST_PATH_IMAGE034
是用于划分高频率和低频率的频率阈值，在本实施例中
Figure 982358DEST_PATH_IMAGE034
可以取8；一个具体的拆分案例参见图4，图4中的实线代表时域低频率变化曲线
Figure 934133DEST_PATH_IMAGE070
，虚线代表时域高频率变化曲线
Figure 344386DEST_PATH_IMAGE069
,其中横坐标代表时间，纵坐标代表牵引杆角度变化。
由于拖车的牵引杆角度受其前后车以及牵引车一段时间内特性的影响，且离目标拖车越近，影响越大，会导致拖车行驶产生角度内偏等现象，为了减小这种影响，通过高斯滤波对时域低频率变化曲线进行处理。通过高斯滤波对时域低频变化曲线
Figure 723415DEST_PATH_IMAGE070
进行滤波处理得到
Figure 117487DEST_PATH_IMAGE071
；
将高斯滤波后的低频变化曲线和高频变化曲线相加，再次获取完整的时域变化曲线，也就是得到n号拖车最终的过滤处理后的牵引杆角度的变化曲线:
Figure 545057DEST_PATH_IMAGE037
；
所述时间值为
Figure 137713DEST_PATH_IMAGE022
时的n号拖车的牵引杆角度
Figure 110479DEST_PATH_IMAGE023
为：
Figure 991847DEST_PATH_IMAGE038
。
进一步地，所述拆解为时域高频率变化曲线和时域低频率变化曲线包括以下步骤：
将牵引杆角度随时间的变化曲线转换为频域曲线， n号拖车频率k时的牵引杆角度为：
Figure 285425DEST_PATH_IMAGE039
；
以所述
Figure 670270DEST_PATH_IMAGE034
为基准进行拆分，也就是将频域曲线划分为
Figure 391102DEST_PATH_IMAGE072
和
Figure 228608DEST_PATH_IMAGE073
两部分，分别得到频域高频率变化曲线和频域低频率变化曲线；
再将频域高频率变化曲线和频域低频率变化曲线分别转换回时域的高频率变化曲线和低频率变化曲线，转换方法如下：
Figure 60297DEST_PATH_IMAGE040
。
作为本实施例的优选方案，在执行高斯滤波时，所述通过高斯滤波对低频变化曲线进行滤波处理的方法为：
Figure 80075DEST_PATH_IMAGE041
；
Figure 909491DEST_PATH_IMAGE042
；
其中，当n=2时，
Figure 296610DEST_PATH_IMAGE043
=1，否则
Figure 869673DEST_PATH_IMAGE044
=3。具体是因为在式中，在计算第二节拖车牵引杆角度时，因为其和第一节拖车临近，影响关系较大，因此取σ=1来增大测量牵引杆角度对结果的影响；当计算其余拖车牵引杆角度时，和第一节拖车较远，取
Figure 760269DEST_PATH_IMAGE044
=3来减小第一节拖车牵引杆角度的影响。
此外，所述获取n号拖车运动在当前时刻的转弯中心
Figure 511319DEST_PATH_IMAGE045
的方法为：
Figure 385734DEST_PATH_IMAGE046
的中垂线与过
Figure 496909DEST_PATH_IMAGE019
的
Figure 773170DEST_PATH_IMAGE047
的垂线的交点为所述转弯中心
Figure 944388DEST_PATH_IMAGE045
；
Figure 509362DEST_PATH_IMAGE046
代表以
Figure 486545DEST_PATH_IMAGE018
和
Figure 804263DEST_PATH_IMAGE015
的连线，
Figure 208699DEST_PATH_IMAGE047
代表
Figure 995390DEST_PATH_IMAGE018
和
Figure 979526DEST_PATH_IMAGE048
的连线。
进一步地，所述获取时间值为
Figure 964800DEST_PATH_IMAGE022
时的n号拖车的位姿点的方法为：
Figure 477821DEST_PATH_IMAGE050
；
Figure 814124DEST_PATH_IMAGE051
；
其中，T表示
Figure 87105DEST_PATH_IMAGE022
前一时刻拖车上某一点的坐标，
Figure 130147DEST_PATH_IMAGE052
代表时间值为
Figure 876386DEST_PATH_IMAGE022
时的该点的坐标；直线行驶时，
Figure 637669DEST_PATH_IMAGE053
=0，
Figure 494766DEST_PATH_IMAGE074
为单位矩阵；转弯时，
Figure 392315DEST_PATH_IMAGE075
，
Figure 575035DEST_PATH_IMAGE076
为车辆转向行驶绕所述
Figure 72881DEST_PATH_IMAGE045
驶过的角度。
在这里，基于已知的转弯中心，此处首先将当前时刻拖车的坐标从原坐标系移动到了以转弯中心为原点的坐标系，通过旋转位移矩阵
Figure 671353DEST_PATH_IMAGE077
反应单位时间内转向角度和行驶距离的相关信息，计算得出单位时间后相对于转弯中心的坐标，最后将坐标移回原坐标参考系。在
Figure 954566DEST_PATH_IMAGE077
中，包含了单位时间内可能的两种行驶状态：固定角度转向和直线行驶。
当车辆在转向时，
Figure 42608DEST_PATH_IMAGE078
，α为车辆转向行驶绕
Figure 512904DEST_PATH_IMAGE045
驶过的角度，可以通过已求解的
Figure 977383DEST_PATH_IMAGE018
、
Figure 600257DEST_PATH_IMAGE015
以及
Figure 859200DEST_PATH_IMAGE045
获得，即可计算固定角度转向后的坐标；当车辆在直线行驶时，
Figure 816791DEST_PATH_IMAGE079
=0，三角函数部分变为单位矩阵，而
Figure 819382DEST_PATH_IMAGE080
表示保持前一时刻的行驶轨迹朝向，单位时间内拖车移动的位移，即可计算出直线行驶后的坐标。
通过以上方法，对拖车依次进行迭代，已知前一时刻的拖车任意一点的坐标，即可计算出单位时间后该点的坐标，通常可以选择将四个描述拖车位姿点的关键点进行计算，也就是拖车前的牵引杆前端、前轮轴线中点、后轮轴线中点以及车尾拖挂处。
根据本实施例对拖车轨迹进行一个计算，并和实测轨迹进行比较，其结果参阅图5和图6。
图5为实测轨迹，图6为本实施例的方案计算获取的轨迹，可见本方案计算的轨迹可靠性高、精准度高。

Claims (7)

1.一种基于单一感知数据的多拖车行驶轨迹计算方法，其特征在于，牵引车后有多辆拖车通过牵引杆依次首尾相连，按从前往后的顺序拖车的标号n依次记为1到k，将牵引车视为0号拖车；拖车的主要位姿点包括拖车牵引杆前端、前轮轴线中点、后轮轴线中点以及车尾拖挂处；牵引车的主要位姿点包括前轮轴线中点、后轮轴线中点以及车尾拖挂处；
将n号拖车的牵引杆前端、前轮轴线中点、后轮轴线中点以及车尾拖挂处在前一时刻的坐标分别标记为
Figure 201153DEST_PATH_IMAGE001
、
Figure 383873DEST_PATH_IMAGE002
、
Figure 616140DEST_PATH_IMAGE003
和
Figure 276928DEST_PATH_IMAGE004
，当前时刻记为
Figure 294563DEST_PATH_IMAGE005
，其数值代表距离初始时间的单位时间的个数，此时的相应坐标分别标记为
Figure 382604DEST_PATH_IMAGE006
、
Figure 852900DEST_PATH_IMAGE007
、
Figure 317379DEST_PATH_IMAGE008
和
Figure 189521DEST_PATH_IMAGE009
，表示当前时刻
Figure 448464DEST_PATH_IMAGE010
和前一时刻相差一个单位时间；
多拖车行驶轨迹计算方法包括，通过传感器获取时间值为
Figure 156788DEST_PATH_IMAGE010
时牵引车的位姿点和1号拖车的牵引杆角度，从n=1开始依次执行以下步骤直到n=k，以得到所有拖车的位姿点：
根据
Figure 362641DEST_PATH_IMAGE005
时刻n号拖车的牵引杆角度
Figure 151606DEST_PATH_IMAGE011
以及
Figure 784712DEST_PATH_IMAGE010
时刻n-1号拖车的位姿点，获取
Figure 291917DEST_PATH_IMAGE007
；
根据
Figure 770303DEST_PATH_IMAGE007
和
Figure 679353DEST_PATH_IMAGE002
获取n号拖车运动转弯中心
Figure 467049DEST_PATH_IMAGE012
；
根据
Figure 461550DEST_PATH_IMAGE013
以及
Figure 743627DEST_PATH_IMAGE005
前一时刻的n号拖车的位姿点获取时间值为
Figure 507183DEST_PATH_IMAGE014
时的n号拖车的位姿点。
2.根据权利要求1所述的一种基于单一感知数据的多拖车行驶轨迹计算方法，其特征在于，所述获取
Figure 747672DEST_PATH_IMAGE015
的方法为：
Figure 543983DEST_PATH_IMAGE016
；
Figure 613439DEST_PATH_IMAGE017
;
其中，L为
Figure 638027DEST_PATH_IMAGE018
和
Figure 65728DEST_PATH_IMAGE019
间的距离，代表拖车车身的长度，
Figure 706925DEST_PATH_IMAGE020
为
Figure 861963DEST_PATH_IMAGE018
和
Figure 537795DEST_PATH_IMAGE021
间的距离，代表n号拖车牵引杆的长度，
Figure 916824DEST_PATH_IMAGE022
为时间值为
Figure 294584DEST_PATH_IMAGE023
时刻n号拖车的牵引杆角度。
3.根据权利要求2所述的一种基于单一感知数据的多拖车行驶轨迹计算方法，其特征在于， n大于1时，所述时间值为
Figure 784472DEST_PATH_IMAGE023
时刻n号拖车的牵引杆角度
Figure 49231DEST_PATH_IMAGE024
的获取方法包括以下步骤：
通过以下方程求解n号拖车的牵引杆角度变化相对1号拖车的时间平移量
Figure 599161DEST_PATH_IMAGE025
：
Figure 214950DEST_PATH_IMAGE026
；
其中
Figure 508528DEST_PATH_IMAGE027
代表牵引车在
Figure 893373DEST_PATH_IMAGE028
时刻的速度，
Figure 348625DEST_PATH_IMAGE029
代表单位时间；
每单位时间记载一次1号拖车的牵引杆角度，得到1号拖车在时间值为0到
Figure 468022DEST_PATH_IMAGE023
时的牵引杆角度的时域变化曲线
Figure 237395DEST_PATH_IMAGE030
，n号拖车在时间值不小于
Figure 804642DEST_PATH_IMAGE025
时的牵引杆角度的时域变化曲线为：
Figure 368479DEST_PATH_IMAGE031
；
通过傅里叶展开将牵引杆角度的时域变化曲线
Figure 21177DEST_PATH_IMAGE032
拆解为时域低频率变化曲线
Figure 594241DEST_PATH_IMAGE033
和时域高频率变化曲线
Figure 750416DEST_PATH_IMAGE034
，大于
Figure 608836DEST_PATH_IMAGE035
的频率视为高频率，其他视为低频率，
Figure 47908DEST_PATH_IMAGE035
是用于划分高频率和低频率的频率阈值；
通过高斯滤波对时域低频变化曲线
Figure 261851DEST_PATH_IMAGE036
进行滤波处理得到
Figure 495387DEST_PATH_IMAGE037
；
将高斯滤波后的低频变化曲线和高频变化曲线相加，得到n号拖车最终的牵引杆角度的变化曲线:
Figure 794781DEST_PATH_IMAGE038
；
所述时间值为
Figure 771964DEST_PATH_IMAGE023
时的n号拖车的牵引杆角度
Figure 840414DEST_PATH_IMAGE024
为：
Figure 244851DEST_PATH_IMAGE039
。
4.根据权利要求3所述的一种基于单一感知数据的多拖车行驶轨迹计算方法，其特征在于，所述拆解为时域低频率变化曲线
Figure 782274DEST_PATH_IMAGE036
和时域高频率变化曲线
Figure 297569DEST_PATH_IMAGE040
包括以下步骤：
将牵引杆角度随时间的变化曲线转换为频域曲线， n号拖车频率k时的牵引杆角度为：
Figure 486105DEST_PATH_IMAGE041
；
以所述
Figure 61442DEST_PATH_IMAGE035
为基准进行拆分，分别得到频域高频率变化曲线和频域低频率变化曲线；
将频域低频率变化曲线和频域高频率变化曲线分别转为时域低频率变化曲线
Figure 335429DEST_PATH_IMAGE042
和时域高频率变化曲线
Figure 857677DEST_PATH_IMAGE043
，转换方法如下：
Figure 963036DEST_PATH_IMAGE044
。
5.根据权利要求3所述的一种基于单一感知数据的多拖车行驶轨迹计算方法，其特征在于，所述通过高斯滤波对时域低频变化曲线
Figure 896226DEST_PATH_IMAGE045
进行滤波处理的方法为：
Figure 454247DEST_PATH_IMAGE046
；
Figure 514606DEST_PATH_IMAGE047
；
其中，当n=2时，
Figure 474472DEST_PATH_IMAGE048
=1，否则
Figure 329296DEST_PATH_IMAGE048
=3。
6.根据权利要求1所述的一种基于单一感知数据的多拖车行驶轨迹计算方法，其特征在于，所述获取n号拖车运动转弯中心
Figure 640191DEST_PATH_IMAGE049
的方法为：
Figure 238663DEST_PATH_IMAGE050
的中垂线与过
Figure 260890DEST_PATH_IMAGE019
的
Figure 552194DEST_PATH_IMAGE051
的垂线的交点为所述转弯中心
Figure 84807DEST_PATH_IMAGE049
；
Figure 486969DEST_PATH_IMAGE050
代表以
Figure 155848DEST_PATH_IMAGE018
和
Figure 618054DEST_PATH_IMAGE015
的连线，
Figure 637962DEST_PATH_IMAGE051
代表
Figure 827504DEST_PATH_IMAGE018
和
Figure 616468DEST_PATH_IMAGE052
的连线。
7.根据权利要求1所述的一种基于单一感知数据的多拖车行驶轨迹计算方法，其特征在于，所述获取时间值为
Figure 249575DEST_PATH_IMAGE023
时的n号拖车的位姿点的方法为：
Figure 694463DEST_PATH_IMAGE053
；
Figure 500745DEST_PATH_IMAGE054
；
其中，T表示
Figure 347478DEST_PATH_IMAGE023
前一时刻拖车上某一点的坐标，
Figure 948224DEST_PATH_IMAGE055
代表时间值为
Figure 631140DEST_PATH_IMAGE023
时的该点的坐标；直线行驶时，
Figure 975534DEST_PATH_IMAGE056
=0，
Figure 676773DEST_PATH_IMAGE057
为单位矩阵；转弯时，
Figure 448420DEST_PATH_IMAGE058
，
Figure 867900DEST_PATH_IMAGE059
为车辆转向行驶绕所述
Figure 15985DEST_PATH_IMAGE049
驶过的角度。
CN202210627450.8A 2022-06-06 2022-06-06 一种基于单一感知数据的多拖车行驶轨迹计算方法 Active CN114707103B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210627450.8A CN114707103B (zh) 2022-06-06 2022-06-06 一种基于单一感知数据的多拖车行驶轨迹计算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210627450.8A CN114707103B (zh) 2022-06-06 2022-06-06 一种基于单一感知数据的多拖车行驶轨迹计算方法

Publications (2)

Publication Number Publication Date
CN114707103A true CN114707103A (zh) 2022-07-05
CN114707103B CN114707103B (zh) 2022-10-18

Family

ID=82177756

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210627450.8A Active CN114707103B (zh) 2022-06-06 2022-06-06 一种基于单一感知数据的多拖车行驶轨迹计算方法

Country Status (1)

Country Link
CN (1) CN114707103B (zh)

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101974876A (zh) * 2010-10-29 2011-02-16 天津市市政工程设计研究院 考虑重型拖车车轮轨迹偏差和甩尾的弯道加宽设计方法
CN105583824A (zh) * 2016-01-26 2016-05-18 清华大学 力控牵引和摆位的多自由度机械臂控制装置及其控制方法
DE102015206689A1 (de) * 2015-04-14 2016-10-20 Horsch Leeb Application Systems Gmbh Verfahren zur Lenkung eines landwirtschaftlichen Anhängers und landwirtschaftlicher Zugverbund
CN108871338A (zh) * 2018-06-25 2018-11-23 驭势科技（北京）有限公司 一种拖车系统位姿预测方法、装置及存储介质
CN109407517A (zh) * 2018-12-19 2019-03-01 驭势科技（北京）有限公司 一种拖车系统路径跟踪方法、装置、系统及存储介质
CN109823129A (zh) * 2019-02-19 2019-05-31 成都新捷物流有限公司 一种卡车用牵引车和挂车的连接结构
CN110286683A (zh) * 2019-07-15 2019-09-27 北京科技大学 一种履带式移动机器人的自主行驶路径跟踪控制方法
CN110335305A (zh) * 2018-11-01 2019-10-15 驭势科技（北京）有限公司 一种拖车位姿的确定方法、装置、车载设备及存储介质
CN111284502A (zh) * 2020-03-10 2020-06-16 仓擎智能科技(上海)有限公司 一种牵引车组位姿检测方法及系统
CN111487976A (zh) * 2020-05-03 2020-08-04 哈尔滨工程大学 一种倒车轨迹跟踪方法
CN113696970A (zh) * 2021-08-31 2021-11-26 驭势科技（北京）有限公司 半挂汽车列车、倒车控制方法、装置、设备和介质
CN113821026A (zh) * 2021-08-27 2021-12-21 中国人民解放军军事科学院战争研究院 一种基于rhc的有杆拖挂式无人系统在线轨迹跟踪控制方法
CN114104105A (zh) * 2020-08-25 2022-03-01 郑州宇通客车股份有限公司 转向角测量装置及拖挂车

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101974876A (zh) * 2010-10-29 2011-02-16 天津市市政工程设计研究院 考虑重型拖车车轮轨迹偏差和甩尾的弯道加宽设计方法
DE102015206689A1 (de) * 2015-04-14 2016-10-20 Horsch Leeb Application Systems Gmbh Verfahren zur Lenkung eines landwirtschaftlichen Anhängers und landwirtschaftlicher Zugverbund
CN105583824A (zh) * 2016-01-26 2016-05-18 清华大学 力控牵引和摆位的多自由度机械臂控制装置及其控制方法
CN108871338A (zh) * 2018-06-25 2018-11-23 驭势科技（北京）有限公司 一种拖车系统位姿预测方法、装置及存储介质
CN110335305A (zh) * 2018-11-01 2019-10-15 驭势科技（北京）有限公司 一种拖车位姿的确定方法、装置、车载设备及存储介质
CN109407517A (zh) * 2018-12-19 2019-03-01 驭势科技（北京）有限公司 一种拖车系统路径跟踪方法、装置、系统及存储介质
CN109823129A (zh) * 2019-02-19 2019-05-31 成都新捷物流有限公司 一种卡车用牵引车和挂车的连接结构
CN110286683A (zh) * 2019-07-15 2019-09-27 北京科技大学 一种履带式移动机器人的自主行驶路径跟踪控制方法
CN111284502A (zh) * 2020-03-10 2020-06-16 仓擎智能科技(上海)有限公司 一种牵引车组位姿检测方法及系统
CN111487976A (zh) * 2020-05-03 2020-08-04 哈尔滨工程大学 一种倒车轨迹跟踪方法
CN114104105A (zh) * 2020-08-25 2022-03-01 郑州宇通客车股份有限公司 转向角测量装置及拖挂车
CN113821026A (zh) * 2021-08-27 2021-12-21 中国人民解放军军事科学院战争研究院 一种基于rhc的有杆拖挂式无人系统在线轨迹跟踪控制方法
CN113696970A (zh) * 2021-08-31 2021-11-26 驭势科技（北京）有限公司 半挂汽车列车、倒车控制方法、装置、设备和介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
任工昌等: "带单节拖车的机器人控制系统的设计与实现", 《电子技术应用》 *
宋毅: "半挂汽车列车被动转向系统的设计及仿真研究", 《中国优秀硕士学位论文全文数据库工程科技II辑》 *

Also Published As

Publication number Publication date
CN114707103B (zh) 2022-10-18

Similar Documents

Publication Publication Date Title
DE102016106298B4 (de) Fusionsverfahren für eine Querverkehr-Applikation unter Verwenden von Radargeräten und einer Kamera
EP2561419B1 (de) Verfahren zur bestimmung des fahrbahnverlaufes für ein kraftfahrzeug
DE102014118414A1 (de) Geschwindigkeitsbegrenzungsschwelleerfassungsvorrichtung und Navigationsdatenaktualisierungsvorrichtung und Verfahren, welches diese verwendet
DE112014002019B4 (de) Kurvenformmodelliereinrichtung, Fahrzeuginformationsverarbeitungssystem, Kurvenformmodellierverfahren und Kurvenformmodellierprogramm
DE102017131125A1 (de) Fahrzeuginterne Alarmvorrichtung
DE102015109832A1 (de) Objektklassifizierung für Fahrzeugradarsysteme
DE102020100685A1 (de) Vorhersage zeitlicher informationen in autonomenmaschinenanwendungen
CN110726993B (zh) 一种利用单线激光雷达和毫米波雷达的障碍物检测方法
Peng et al. Driving maneuver early detection via sequence learning from vehicle signals and video images
DE102017214032A1 (de) Verfahren zum Bestimmen eines Reibwerts für einen Kontakt zwischen einem Reifen eines Fahrzeugs und einer Fahrbahn und Verfahren zum Steuern einer Fahrzeugfunktion eines Fahrzeugs
CN112964262A (zh) 无人驾驶车载传感器数据采集与处理系统与方法
WO2019211168A1 (de) Verfahren und vorrichtung zum betreiben von ultraschallsensoren eines fahrzeugs
DE102017108248A1 (de) Strassenmerkmalserkennung mit einem fahrzeugkamerasystem
CN106327433B (zh) 一种基于单俯视相机与后轴转向的车辆路径跟随方法
CN106295651B (zh) 一种基于双俯视相机与后轴转向的车辆路径跟随方法
DE102017108255A1 (de) Parallele detektion von primitiven in einer szene unter verwendung eines rundum-kamerasystems
DE102021111544A1 (de) System und verfahren zur schätzung des kupplungswinkels
CN114707103B (zh) 一种基于单一感知数据的多拖车行驶轨迹计算方法
Ding et al. A lane detection method based on semantic segmentation
DE112020002753T5 (de) Fahrzeugsteuerverfahren, fahrzeugsteuervorrichtung und fahrzeugsteuersystem diese enthaltend
DE112020000325T5 (de) Schätzung von Anhängereigenschaften mit Fahrzeugsensoren
DE112021005777T5 (de) Systeme und Verfahren zum Entfernungsraten-Dealiasing unter Verwendung einer Positionskonsistenz
DE112022002791T5 (de) Systeme und verfahren zur partikelfilterverfolgung
DE102018216999A1 (de) Verfahren, Computerprogramm und Messsystem für die Auswertung von Bewegungsinformationen
CN111857469B (zh) 一种道路环境信息的重构方法、装置及设备

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant