CN110555801B - 一种航迹推演的校正方法、终端和存储介质 - Google Patents
一种航迹推演的校正方法、终端和存储介质 Download PDFInfo
- Publication number
- CN110555801B CN110555801B CN201910680100.6A CN201910680100A CN110555801B CN 110555801 B CN110555801 B CN 110555801B CN 201910680100 A CN201910680100 A CN 201910680100A CN 110555801 B CN110555801 B CN 110555801B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- track deduction
- track
- deduction
- map
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 48
- 238000012937 correction Methods 0.000 title claims abstract description 46
- 238000003860 storage Methods 0.000 title claims abstract description 14
- 230000009466 transformation Effects 0.000 claims abstract description 28
- 239000011159 matrix material Substances 0.000 claims abstract description 26
- 230000008569 process Effects 0.000 claims description 14
- 241000791900 Selene vomer Species 0.000 claims description 11
- 238000006243 chemical reaction Methods 0.000 claims description 10
- 238000001514 detection method Methods 0.000 claims description 9
- 230000000007 visual effect Effects 0.000 claims description 9
- 230000000694 effects Effects 0.000 claims description 5
- 238000005259 measurement Methods 0.000 claims description 5
- 230000008447 perception Effects 0.000 claims description 5
- 230000005540 biological transmission Effects 0.000 claims description 3
- 238000004590 computer program Methods 0.000 claims description 3
- 230000004927 fusion Effects 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 230000016776 visual perception Effects 0.000 claims description 3
- 238000013441 quality evaluation Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 238000009826 distribution Methods 0.000 description 4
- 238000012986 modification Methods 0.000 description 4
- 230000004048 modification Effects 0.000 description 4
- 238000012544 monitoring process Methods 0.000 description 4
- 230000006399 behavior Effects 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 239000000725 suspension Substances 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
- B60W30/06—Automatic manoeuvring for parking
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T3/00—Geometric image transformations in the plane of the image
- G06T3/40—Scaling of whole images or parts thereof, e.g. expanding or contracting
- G06T3/4038—Image mosaicing, e.g. composing plane images from plane sub-images
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T5/00—Image enhancement or restoration
- G06T5/50—Image enhancement or restoration using two or more images, e.g. averaging or subtraction
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20212—Image combination
- G06T2207/20221—Image fusion; Image merging
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Theoretical Computer Science (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明提供一种航迹推演的校正方法、终端和存储介质，包括以下步骤：S01：根据车辆起始点和目标点位置获得车辆航迹推演结果；S02：将包括但不限于车辆实时感知数据、目标点位置表述、历史航迹推演结果输入航迹推演纠偏系统；S03：航迹推演纠偏系统先识别目标点位置的追踪目标，再依据追踪目标位置度量里程计偏差，获得里程计和地图之间的坐标转换矩阵，纠正航迹推演偏差。本发明引入图像信号来实时观测目标车位，并且将观测的结果与DR推测出目标车位的位置进行比对，给出DR的误差。控制系统可以调整相关执行参数或重新规划泊车入位的路径。
Description
技术领域
本发明涉及汽车电子技术领域，特别是涉及一种航迹推演的校正方法、终端和存储介质。
背景技术
在现有技术中，“自动代客泊车”(Auto Valet Parking)成为自动驾驶领域的热门技术之一，同样也将是自动驾驶量产道路上的一个重要里程碑。作为一套完整的自动无人驾驶汽车系统，AVP系统以低速驾驶汽车或将汽车停在有限的区域内，如停车场或周围道路。此外，作为泊车辅助的一种功能扩展，也会是最早商业化的全自动驾驶功能之一。
由于在泊车过程中，车辆进行路径规划和定位时依赖的信号是航迹推演(DR)，由于各种不可控的因素，DR信号存在较为明显的累积误差，无法满足控制系统的精度要求，最终导致泊车结束后，无法让车辆到达预定的位置并满足期望的姿态。
现有技术中，如公开号DE102015116220A1的用于检测机动车辆错误的机动车辆的至少半自动操纵的方法，计算装置，驾驶员辅助系统和机动车辆的方法，该文献涉及一种采用接触式测量误差的方案，即发动机扭矩信号来检测障碍物的距离来发现里程计误差的方法，这里的障碍物一般指路沿或限位器，车轮装上去，并且花比较大力气开不过去，因此扭矩增大。通过检测到这样的障碍物，来确定目前车辆的接触位置已经和地图上的障碍物有接触(有偏差的话，就会提前或者滞后接触)，从而发现误差。
如公开号DE102016106978A1的用于操作机动车辆的驾驶员辅助系统的方法，计算装置，驾驶员辅助系统和机动车辆，该文献主要公开了一项以图像为基础的非接触式测量里程计误差的方案，但是该方案中图像需要识别出明显的目标物作为参考。例如，需要在图像中找出车辆，车牌，墙面标识，地面标识等，可以被认为是目标级的参考物。但是这种依靠目标级参考物来识别里程计误差的方式，再车辆实时感知无法感知到目标级参考物或者实际场景中没有目标级参考物的情况下，即功能无法实现。
如公开号DE102015112313A1的用于用于具有位置校正，驾驶员辅助系统和机动车辆的机动车辆的至少半自动操纵的方法，该文献主要公开一项以超声波传感器探测数据为基础的里程计误差感知方法，需要车辆在遇到立体物时才能给出偏差信号。且超声波传感器探测物体精度差，仅仅输出距离和速度信号，对于障碍物的位姿和形状无法掌握。作为里程计误差的参照物不是最好的选择。
发明内容
为了解决上述的以及其他潜在的技术问题，本发明提供了一种航迹推演的校正方法、终端和存储介质，需要引入图像信号来实时观测目标车位，并且将观测的结果与DR推测出目标车位的位置进行比对，给出DR的误差。控制系统可以调整相关执行参数或重新规划泊车入位的路径。
一种航迹推演的校正方法，包括以下步骤：
S01：根据车辆起始点和目标点位置获得车辆航迹推演结果；
S02：将包括但不限于车辆实时感知数据、目标点位置表述、历史航迹推演结果输入航迹推演纠偏系统；
S03：航迹推演纠偏系统先识别目标点位置的追踪目标，再依据追踪目标位置度量里程计偏差，获得里程计和地图之间的坐标转换矩阵，纠正航迹推演偏差。
进一步地，所述步骤S01中的车辆航迹推演可用于定位导航和方向控制，除了对车辆姿进行估计，还可以从航迹推演中获得移动机器人前进速度和转向角速度。
进一步地，所述步骤S02中所述的车辆实时感知数据包括视觉感知数据、超声波感知数据和毫米波感知数据。
进一步地，所述步骤S02中所述的车辆实时感知数据仅包含视觉感知数据，所述视觉感知数据为包含车辆的俯视图像。
进一步地，所述步骤S02中所述的俯视图像，高位俯览环视图像，即TopView环视图：
由于车位线是喷涂于地面的一种人工标识，AVM图像以俯视的方式将车身周围地面景象进行拼接，较好的保留了地面标识的几何表现。
进一步地，所述俯览环视图像的输入形式为：ros message，环视图像参数定义如下：
Header header
uint64index
ImgRect blindArea
ImgRect carPos
int32phyHeight
int32phyFront
sensor_msgs/Image avm
相关成员说明：
header：主要包含时间戳信息
index：帧序号
blindArea：中心盲区在图像中占据的区域
avm：VYUY格式的彩色图片
进一步地，所述俯览环视图像的获取方式：订阅zdada系统的TOPIC_TRACK_AVM_SRC。
进一步地，所述俯览环视图像的输入要求包括下列条件中一种或几种：
(1)观测范围：车头前方3m，车位后方3m，车身侧面的可是范围可以自动推算出。依据现有zadas系统提供的环视图片清晰度，能够在车辆后轴中心距离车位3m的范围内给出校正信号。
(2)图像拼接角度：45°。在此参数设置之下，生成的环视图可以在拼接处有较好的融合效果，不容易出现摄像头之间特别明显的错位感；因此在泊车过车中可以较好地满足车辆与车位之间呈现不同观测角度时观测的需求。
(3)图片尺寸：高600像素，宽480像素，宽高尺寸比例为1。按照此前设定的观测范围，假设车辆长度5m。每一个像素代表的坐标轴向距离为2.16cm。如果检测精度在3个像素以内，就可以保证输出结果的分辨精度在5cm以内。
(4)发送频率：2～3Hz。根据车辆控制系统的要求，每秒钟得到2～3次校正即可满足要求，所有的处理流程在CPU上的耗时不超过250ms。
进一步地，所述步骤S02中目标点位置表述用于自俯览环视图像中分割与本次泊车过程相关的信息，即缩小目标点位置的检测范围。
进一步地，所述步骤S02中目标点位置的输入形式为ros message，其数据包表达定义如下：
Header header
SlotVertex slot
SlotVertex boundry
int32type
int32valid_type
int32valid
int32id
相关成员说明：
header：主要包含时间戳信息
slot：包含停车位4个顶点在zadas坐标系下的物理坐标
type：车位类型。
进一步地，所述步骤S02中目标点位置的获取方式：订阅zdada系统的TOPIC_APA_TARGET。
进一步地，所述步骤S02中目标点位置的输入要求：在某一确定的坐标系下，发送构成一个车位的4个顶点的物理坐标位置，该信号只需要在泊车开始阶段，车辆静止时，发送一次即可。
进一步地，所述步骤S02中输入航迹推演纠偏系统的还包括泊车状态信号，泊车状态信号不是系统基础服务，是根据需求启动的，在行车过程中并不需求提供此模块的输出，避免不必要的系统负担。例如，在泊车过程中，如果车辆俯览环视图中在车辆路径规划范围内突然出现障碍物或者根据其他感知设备例如超声波雷达或者毫米波雷达感知到障碍物而必须立即停止时，将此信号输入至航迹推演模块。
进一步地，所述步骤S02中的泊车状态信号的输入形式：ros message，泊车状态信号的数据包表达定义如下
int32apaStage
相关信号含义说明：
APA_STATUS_PI_GUIDANCE：开始泊入
APA_STATUS_PO_GUIDANCE：开始泊出
APA_STATUS_SUSPEND：功能挂起
APA_STATUS_PI_SLOT_SEARCHING：正在搜寻车位
APA_STATUS_PI_SLOT_CONFIRMED：目标车位已确认
APA_STATUS_STANDBY：等待开始泊车
进一步地，所述步骤S02中的泊车状态信号的获取方式：订阅ZADAS系统的TOPIC_APA_STAGE。
进一步地，所述步骤S02中的泊车状态信号的输入要求：无特殊要求，监听即可。
进一步地，所述步骤S02中输入航迹推演纠偏系统的还包括车辆档位信号，因为在整个泊车过程中并不是所有的行驶阶段都是泊入行为，因此需要在倒车模式下车辆档位信号应当给航迹推演纠偏系统提供校正输出。
进一步地，所述步骤S02中输入航迹推演纠偏系统的还包括车辆档位信号的输入形式：car_live_info，所述车辆档位信号数据包的定义如下：GEAR_STATUS_R
进一步地，所述车辆档位信号获取方式：调用get_car_gear()。
进一步地，所述步骤S03中航迹推演纠偏系统先识别目标点位置的追踪目标，其追踪目标为车位线。
进一步地，所述步骤S03中里程计和地图之间的坐标转换矩阵，地图坐标代表了客观现实中存在的一些标识物的位置，里程计航迹推演坐标表达了依据车辆行驶轨迹推测的车辆位置，而车辆实时视觉感知数据中的俯览环视图像中观测到的信息来源于数据库链接baselink，存在于地图中，图像得到的观测，相当于是从地图产生的；从局部的角度来看，数据库链接baselink的观测是与地图一致的，里程计和地图之间的坐标转换矩阵就是从航迹推演变换回map，因此也代表了需要纠正的偏差，如果衡量图像检测到的目标点位置信息与航迹推演推测的车位信息间存在的误差，就可以校正航迹推演已形成的误差。
进一步地，所述里程计到地图之间的坐标转换矩阵的输出方式：ZADAS系统的广播TOPIC_TF_MAP_ODOM_CONF。
进一步地，所述步骤S03还包括对程计到地图之间的坐标转换矩阵校正的信号评价机制，为了能够让使用者了解信号质量，以此来决定是否接受本次校正信号，在发出转换矩阵的同时给出信号质量评价指标。
进一步地，所述步骤S03还包括对程计到地图之间的坐标转换矩阵校正的信号质量评价指标包括至少三类，即包含平行度，直线证据累积数量，直线证据分布方差。
(1)平行度-parallelism
当两条直线都检测出的情况下，给出一个0～1的浮点数，越接近与1，平行度越好，检测的可信赖度越高
(2)直线证据累积数量-leftDispersion，rightDispersion
表明了在搜集了多少数据后，给出了这条直线信息，左右两条校准线各有一个此项指标(3)直线证据分布方差-leftEvidence，rightEvidence
表明了搜集到的直线证据的离散程度。
进一步地，所述步骤S03还包括对程计到地图之间的坐标转换矩阵校正的信号质量评价指标的情况下，所述里程计到地图之间的坐标转换矩阵的数据包表达定义如下：
Header header
float64leftDispersion
float64rightDispersion
float64parallelism
int32leftEvidence
int32rightEvidence
geometry_msgs/TransformStamped trans
成员说明：
header：主要包含时间戳信息
leftDispersion：左侧校正线累积证据方差
rightDispersion：右侧校正线累积证据方差
parallelism：两条校正线平行度
leftEvidence：左侧校正线累积证据度量
rightEvidence：右侧校正线累积证据度量。
进一步地，所述步骤S03中的航迹推演纠偏系统先识别目标点位置的追踪目标，再依据追踪目标位置度量里程计偏差，获得里程计和地图之间的坐标转换矩阵，纠正航迹推演偏差，并输出航迹推演位置变换信号，在泊车过程中，使用航迹推演位置变换信号可以实时更新目标车位的位置，并实时了解目标车位在图像以及物理坐标系中的位置，以便根据实时位置制定相应的路径规划策略。
进一步地，所述步骤S03中航迹推演位置变换信号的输入形式：ros message，航迹推演位置变换信号数据包表达定义如下：tf2_msgs::TFMessage message
相关成员说明：
message：3×3的位姿转换矩阵。
进一步地，所述步骤S03中航迹推演位置变换信号的获取方式：订阅zdada系统的TOPIC_TF_ODOM_BASELINK。
进一步地，所述步骤S03中航迹推演位置变换信号的输入要求：无特殊要求，监听即可。
如上所述，本发明的具有以下有益效果：
需要引入图像信号来实时观测目标车位，并且将观测的结果与DR推测出目标车位的位置进行比对，给出DR的误差。控制系统可以调整相关执行参数或重新规划泊车入位的路径。
本发明的特点是：
(1)非接触式测量方法(使用图像)；
(2)这套系统中可以不依赖于地图，仅依赖于已经识别出的车位框，而无需地图来全面覆盖场景中的内容(柱子，道路)；在有地图的环境也可以使用，自主泊车中没有地图，代客泊车中有地图。
在自主泊车中目标车位来源于已经识别到的车位。代客泊车中可以来源于地图上已经确定的目标车位(需要先让车辆开到车位附近，这套系统才起作用)。
(3)参考的特征是地面标识线(车位框的边线)，并不要求特别明确的目标物；
(4)给出的偏差可以非常准确的描述车辆的横向偏差和纵向偏差。横向偏差可以理解为，车有没有停歪，有没有太靠近某一侧边线。纵向偏差可以理解为，车停的太靠车位里面还是车头出来点。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1显示为本发明的流程框图。
图2显示为本发明另一实施例的流程框图。
图3显示为本发明另一实施例的流程框图。
图4显示为本发明另一实施例的流程框图。
图5显示为本发明另一实施例的流程框图。
图6显示为本发明另一实施例的流程框图。
具体实施方式
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
须知，本说明书所附图式所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明可实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容得能涵盖的范围内。同时，本说明书中所引用的如“上”、“下”、“左”、“右”、“中间”及“一”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。
参见图1～图6，
一种航迹推演的校正方法，包括以下步骤：
S01：根据车辆起始点和目标点位置获得车辆航迹推演结果；
S02：将包括但不限于车辆实时感知数据、目标点位置表述、历史航迹推演结果输入航迹推演纠偏系统；
S03：航迹推演纠偏系统先识别目标点位置的追踪目标，再依据追踪目标位置度量里程计偏差，获得里程计和地图之间的坐标转换矩阵，纠正航迹推演偏差。
进一步地，所述步骤S01中的车辆航迹推演可用于定位导航和方向控制，除了对车辆姿进行估计，还可以从航迹推演中获得移动机器人前进速度和转向角速度。
进一步地，所述步骤S02中所述的车辆实时感知数据包括视觉感知数据、超声波感知数据和毫米波感知数据。
进一步地，所述步骤S02中所述的车辆实时感知数据仅包含视觉感知数据，所述视觉感知数据为包含车辆的俯视图像。
进一步地，所述步骤S02中所述的俯视图像，高位俯览环视图像，即TopView环视图：
由于车位线是喷涂于地面的一种人工标识，AVM图像以俯视的方式将车身周围地面景象进行拼接，较好的保留了地面标识的几何表现。
进一步地，所述俯览环视图像的输入形式为：ros message，环视图像参数定义如下：
Header header
uint64index
ImgRect blindArea
ImgRect carPos
int32phyHeight
int32phyFront
sensor_msgs/Image avm
相关成员说明：
header：主要包含时间戳信息
index：帧序号
blindArea：中心盲区在图像中占据的区域
avm：VYUY格式的彩色图片
进一步地，所述俯览环视图像的获取方式：订阅zdada系统的TOPIC_TRACK_AVM_SRC。
进一步地，所述俯览环视图像的输入要求包括下列条件中一种或几种：
(1)观测范围：车头前方3m，车位后方3m，车身侧面的可是范围可以自动推算出。依据现有zadas系统提供的环视图片清晰度，能够在车辆后轴中心距离车位3m的范围内给出校正信号。
(2)图像拼接角度：45°。在此参数设置之下，生成的环视图可以在拼接处有较好的融合效果，不容易出现摄像头之间特别明显的错位感；因此在泊车过车中可以较好地满足车辆与车位之间呈现不同观测角度时观测的需求。
(3)图片尺寸：高600像素，宽480像素，宽高尺寸比例为1。按照此前设定的观测范围，假设车辆长度5m。每一个像素代表的坐标轴向距离为2.16cm。如果检测精度在3个像素以内，就可以保证输出结果的分辨精度在5cm以内。
(4)发送频率：2～3Hz。根据车辆控制系统的要求，每秒钟得到2～3次校正即可满足要求，所有的处理流程在CPU上的耗时不超过250ms。
进一步地，所述步骤S02中目标点位置表述用于自俯览环视图像中分割与本次泊车过程相关的信息，即缩小目标点位置的检测范围。
进一步地，所述步骤S02中目标点位置的输入形式为ros message，其数据包表达定义如下：
Header header
SlotVertex slot
SlotVertex boundry
int32type
int32valid_type
int32valid
int32id
相关成员说明：
header：主要包含时间戳信息
slot：包含停车位4个顶点在zadas坐标系下的物理坐标
type：车位类型。
进一步地，所述步骤S02中目标点位置的获取方式：订阅zdada系统的TOPIC_APA_TARGET。
进一步地，所述步骤S02中目标点位置的输入要求：在某一确定的坐标系下，发送构成一个车位的4个顶点的物理坐标位置，该信号只需要在泊车开始阶段，车辆静止时，发送一次即可。
进一步地，所述步骤S02中输入航迹推演纠偏系统的还包括泊车状态信号，泊车状态信号不是系统基础服务，是根据需求启动的，在行车过程中并不需求提供此模块的输出，避免不必要的系统负担。例如，在泊车过程中，如果车辆俯览环视图中在车辆路径规划范围内突然出现障碍物或者根据其他感知设备例如超声波雷达或者毫米波雷达感知到障碍物而必须立即停止时，将此信号输入至航迹推演模块。
进一步地，所述步骤S02中的泊车状态信号的输入形式：ros message，泊车状态信号的数据包表达定义如下
int32apaStage
相关信号含义说明：
APA_STATUS_PI_GUIDANCE：开始泊入
APA_STATUS_PO_GUIDANCE：开始泊出
APA_STATUS_SUSPEND：功能挂起
APA_STATUS_PI_SLOT_SEARCHING：正在搜寻车位
APA_STATUS_PI_SLOT_CONFIRMED：目标车位已确认
APA_STATUS_STANDBY：等待开始泊车
进一步地，所述步骤S02中的泊车状态信号的获取方式：订阅ZADAS系统的TOPIC_APA_STAGE。
进一步地，所述步骤S02中的泊车状态信号的输入要求：无特殊要求，监听即可。
进一步地，所述步骤S02中输入航迹推演纠偏系统的还包括车辆档位信号，因为在整个泊车过程中并不是所有的行驶阶段都是泊入行为，因此需要在倒车模式下车辆档位信号应当给航迹推演纠偏系统提供校正输出。
进一步地，所述步骤S02中输入航迹推演纠偏系统的还包括车辆档位信号的输入形式：car_live_info，所述车辆档位信号数据包的定义如下：GEAR_STATUS_R
进一步地，所述车辆档位信号获取方式：调用get_car_gear()。
进一步地，所述步骤S03中航迹推演纠偏系统先识别目标点位置的追踪目标，其追踪目标为车位线。
进一步地，所述步骤S03中里程计和地图之间的坐标转换矩阵，地图坐标代表了客观现实中存在的一些标识物的位置，里程计航迹推演坐标表达了依据车辆行驶轨迹推测的车辆位置，而车辆实时视觉感知数据中的俯览环视图像中观测到的信息来源于数据库链接baselink，存在于地图中，图像得到的观测，相当于是从地图产生的；从局部的角度来看，数据库链接baselink的观测是与地图一致的，里程计和地图之间的坐标转换矩阵就是从航迹推演变换回map，因此也代表了需要纠正的偏差，如果衡量图像检测到的目标点位置信息与航迹推演推测的车位信息间存在的误差，就可以校正航迹推演已形成的误差。
进一步地，所述里程计到地图之间的坐标转换矩阵的输出方式：ZADAS系统的广播TOPIC_TF_MAP_ODOM_CONF。
进一步地，所述步骤S03还包括对程计到地图之间的坐标转换矩阵校正的信号评价机制，为了能够让使用者了解信号质量，以此来决定是否接受本次校正信号，在发出转换矩阵的同时给出信号质量评价指标。
进一步地，所述步骤S03还包括对程计到地图之间的坐标转换矩阵校正的信号质量评价指标包括至少三类，即包含平行度，直线证据累积数量，直线证据分布方差。
(1)平行度-parallelism
当两条直线都检测出的情况下，给出一个0～1的浮点数，越接近与1，平行度越好，检测的可信赖度越高
(2)直线证据累积数量-leftDispersion，rightDispersion
表明了在搜集了多少数据后，给出了这条直线信息，左右两条校准线各有一个此项指标(3)直线证据分布方差-leftEvidence，rightEvidence
表明了搜集到的直线证据的离散程度。
进一步地，所述步骤S03还包括对程计到地图之间的坐标转换矩阵校正的信号质量评价指标的情况下，所述里程计到地图之间的坐标转换矩阵的数据包表达定义如下：
Header header
float64leftDispersion
float64rightDispersion
float64parallelism
int32leftEvidence
int32rightEvidence
geometry_msgs/TransformStamped trans
成员说明：
header：主要包含时间戳信息
leftDispersion：左侧校正线累积证据方差
rightDispersion：右侧校正线累积证据方差
parallelism：两条校正线平行度
leftEvidence：左侧校正线累积证据度量
rightEvidence：右侧校正线累积证据度量。
进一步地，所述步骤S03中的航迹推演纠偏系统先识别目标点位置的追踪目标，再依据追踪目标位置度量里程计偏差，获得里程计和地图之间的坐标转换矩阵，纠正航迹推演偏差，并输出航迹推演位置变换信号，在泊车过程中，使用航迹推演位置变换信号可以实时更新目标车位的位置，并实时了解目标车位在图像以及物理坐标系中的位置，以便根据实时位置制定相应的路径规划策略。
进一步地，所述步骤S03中航迹推演位置变换信号的输入形式：ros message，航迹推演位置变换信号数据包表达定义如下：tf2_msgs::TFMessage message
相关成员说明：
message：3×3的位姿转换矩阵。
进一步地，所述步骤S03中航迹推演位置变换信号的获取方式：订阅zdada系统的TOPIC_TF_ODOM_BASELINK。
进一步地，所述步骤S03中航迹推演位置变换信号的输入要求：无特殊要求，监听即可。
一种移动终端，其可以是车载终端或手机移动终端，其执行上述航迹推演的校正方法或自车载终端航迹推演的校正方法获得航迹推演结果以更新车辆定位的手机移动终端。
一种计算机存储介质，其是依照上述航迹推演的校正方法所编写的计算机程序。
作为优选实施例，本实施例还提供一种终端设备，如可以执行程序的智能手机、平板电脑、笔记本电脑、台式计算机、机架式云端、刀片式云端、塔式云端或机柜式云端(包括独立的云端，或者多个云端所组成的云端集群)等。本实施例的终端设备至少包括但不限于：可通过系统总线相互通信连接的存储器、处理器。需要指出的是，具有组件存储器、处理器的终端设备，但是应理解的是，并不要求实施所有示出的组件，可以替代的航迹推演的校正方法实施更多或者更少的组件。
作为优选实施例，存储器(即可读存储介质)包括闪存、硬盘、多媒体卡、卡型存储器(例如，SD或DX存储器等)、随机访问存储器(RAM)、静态随机访问存储器(SRAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、可编程只读存储器(PROM)、磁性存储器、磁盘、光盘等。在一些实施例中，存储器可以是计算机设备的内部存储单元，例如该计算机设备的硬盘或内存。在另一些实施例中，存储器也可以是计算机设备的外部存储设备，例如该计算机设备上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(SecureDigital,SD)卡，闪存卡(Flash Card)等。当然，存储器还可以既包括计算机设备的内部存储单元也包括其外部存储设备。本实施例中，存储器通常用于存储安装于计算机设备的操作系统和各类应用软件，例如实施例中的航迹推演的校正程序代码等。此外，存储器还可以用于暂时地存储已经输出或者将要输出的各类数据。
本实施例还提供一种计算机可读存储介质，如闪存、硬盘、多媒体卡、卡型存储器(例如，SD或DX存储器等)、随机访问存储器(RAM)、静态随机访问存储器(SRAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、可编程只读存储器(PROM)、磁性存储器、磁盘、光盘、云端、App应用商城等等，其上存储有计算机程序，程序被处理器执行时实现相应功能。本实施例的计算机可读存储介质用于航迹推演的校正程序，被处理器执行时实现航迹推演的校正程序实施例中的航迹推演的校正方法。
上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中包括通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。
Claims (11)
1.一种航迹推演的校正方法，其特征在于，包括以下步骤：
S01：根据车辆起始点和目标点位置获得车辆航迹推演结果；
S02：将包括但不限于车辆实时感知数据、目标点位置表述、历史航迹推演结果输入航迹推演纠偏系统；所述车辆实时感知数据中的俯览环视图像中观测到的信息来源于数据库链接baselink，存在于地图中；
S03：航迹推演纠偏系统先识别目标点位置的追踪目标，再依据追踪目标位置度量里程计偏差，获得里程计和地图之间的坐标转换矩阵，通过衡量所述俯览环视图像检测到的目标点位置信息与航迹推演推测的车位信息间存在的误差，实时纠正航迹推演偏差；
其中，所述里程计和地图之间的坐标转换矩阵，地图坐标代表了客观现实中存在的一些标识物的位置，不会因为观测或自车状态的不同产生变化，里程计航迹推演坐标表达了依据车辆行驶轨迹推测的车辆位置，而车辆实时视觉感知数据中的俯览环视图像中观测到的信息来源于当前时刻观测到的地面标识，存在于地图中，图像得到的观测，相当于是从地图产生的；从局部的角度来看，当前时刻观测到的地面标识是与地图一致的，里程计和地图之间的坐标转换矩阵就是从航迹推演变换回map，因此也代表了需要纠正的偏差，如果衡量图像检测到的目标点位置信息与航迹推演推测的车位信息间存在的误差，就可以校正航迹推演已形成的误差。
2.根据权利要求1所述的航迹推演的校正方法，其特征在于，所述步骤S01中的车辆航迹推演可用于定位导航和方向控制，除了对车辆姿进行估计，还可以从航迹推演中获得移动机器人前进速度和转向角速度。
3.根据权利要求2所述的航迹推演的校正方法，其特征在于，所述步骤S02中所述的车辆实时感知数据包括视觉感知数据、超声波感知数据和毫米波雷达感知数据。
4.根据权利要求3所述的航迹推演的校正方法，其特征在于，所述步骤S02中所述的车辆实时感知数据仅包含视觉感知数据，所述视觉感知数据为包含车辆的俯视图像。
5.根据权利要求4所述的航迹推演的校正方法，其特征在于，所述俯览环视图像的输入要求包括下列条件中一种或几种：
(1)观测范围：车头前方3m，车位后方3m，车身侧面的可视范围可以自动推算出，依据现有环视拼接系统提供的环视图片清晰度，能够在车辆后轴中心距离车位3m的范围内给出校正信号；
(2)图像拼接角度：45°，在此参数设置之下，生成的环视图可以在拼接处有较好的融合效果，不容易出现摄像头之间特别明显的错位感；因此在泊车过车中可以较好地满足车辆与车位之间呈现不同观测角度时观测的需求；
(3)图片尺寸：高600像素，宽480像素，宽高尺寸比例为1，按照此前设定的观测范围，假设车辆长度5m，每一个像素代表的坐标轴向距离为2 .16cm，如果检测精度在3个像素以内，就可以保证输出结果的分辨精度在5cm以内；
(4)发送频率：2～3Hz，根据车辆控制系统的要求，每秒钟得到2～3次校正即可满足要求，所有的处理流程在移动端上的耗时不超过250ms。
6.根据权利要求5所述的航迹推演的校正方法，其特征在于，所述步骤S02中目标点位置表述用于自俯览环视图像中分割与本次泊车过程相关的信息，即缩小目标点位置的检测范围，目标点位置表述中包含停车位4个顶点在车载系统坐标系下的物理坐标；在某一确定的坐标系下，发送构成一个车位的4个顶点的物理坐标位置，该信号只需要在泊车开始阶段，车辆静止时，发送一次即可。
7.根据权利要求6所述的航迹推演的校正方法，其特征在于，所述步骤S02中输入航迹推演纠偏系统的还包括泊车状态信号，泊车状态信号不是系统基础服务，是根据需求启动的，在行车过程中并不需求提供此模块的输出，避免不必要的系统负担。
8.根据权利要求6所述的航迹推演的校正方法，其特征在于，所述步骤S02中输入航迹推演纠偏系统的还包括车辆档位信号，因为在整个泊车过程中并不是所有的行驶阶段都是泊入行为，因此需要在泊车模式下车辆档位信号应当给航迹推演纠偏系统提供校正输出。
9.根据权利要求1所述的航迹推演的校正方法，其特征在于，所述步骤S03中航迹推演纠偏系统先识别目标点位置的追踪目标，其追踪目标为车位线，仅依赖于已经识别出的车位线可以实现里程计纠偏，而无需预先建立的地图来全面覆盖场景中的内容，但在有地图的环境也可以使用。
10.一种移动终端，其特征在于：其可以是车载终端或手机移动终端，其执行上述权利要求1-9所述航迹推演的校正方法或自上述权利要求1-9所述的车载终端航迹推演的校正方法获得航迹推演结果以更新车辆定位的手机移动终端。
11.一种计算机存储介质，其是依照如权利要求1-9所述的航迹推演的校正方法所编写的计算机程序。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910680100.6A CN110555801B (zh) | 2019-07-26 | 2019-07-26 | 一种航迹推演的校正方法、终端和存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910680100.6A CN110555801B (zh) | 2019-07-26 | 2019-07-26 | 一种航迹推演的校正方法、终端和存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110555801A CN110555801A (zh) | 2019-12-10 |
| CN110555801B true CN110555801B (zh) | 2024-08-16 |
Family
ID=68735698
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910680100.6A Active CN110555801B (zh) | 2019-07-26 | 2019-07-26 | 一种航迹推演的校正方法、终端和存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110555801B (zh) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113340313B (zh) * | 2020-02-18 | 2024-04-16 | 北京四维图新科技股份有限公司 | 导航地图参数确定方法及装置 |
| CN112987029A (zh) * | 2021-02-09 | 2021-06-18 | 上海振华重工(集团)股份有限公司 | 一种适用于驾驶设备的定位方法、系统、设备及介质 |
| CN114248761B (zh) * | 2021-12-10 | 2024-06-04 | 重庆长安汽车股份有限公司 | 目标轨迹离散化方法、泊车路径跟踪误差优化方法及系统 |
| CN114274948A (zh) * | 2021-12-15 | 2022-04-05 | 武汉光庭信息技术股份有限公司 | 一种基于360度全景的自动泊车方法及装置 |
| CN114419943B (zh) * | 2021-12-29 | 2024-05-10 | 宜昌测试技术研究所 | 一种多auv半离线战术推演系统 |
| CN115953328B (zh) * | 2023-03-13 | 2023-05-30 | 天津所托瑞安汽车科技有限公司 | 一种目标矫正方法、系统及电子设备 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109131317A (zh) * | 2018-07-23 | 2019-01-04 | 同济大学 | 基于多段式规划与机器学习的自动垂直泊车系统及方法 |
| CN109710724A (zh) * | 2019-03-27 | 2019-05-03 | 深兰人工智能芯片研究院（江苏）有限公司 | 一种构建点云地图的方法和设备 |
Family Cites Families (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102147262A (zh) * | 2011-01-11 | 2011-08-10 | 北京理工大学 | 用于纠正导航偏差的方法、及采用该方法的导航显示器 |
| CN104360366B (zh) * | 2014-11-05 | 2017-02-08 | 中国科学院嘉兴微电子与系统工程中心 | 航位推算和全球定位系统的组合定位方法 |
| CN106767894B (zh) * | 2015-11-20 | 2019-11-15 | 北方信息控制集团有限公司 | 一种用于捷联惯导的北斗/里程计组合标定方法 |
| DE102016106978A1 (de) * | 2016-04-15 | 2017-10-19 | Valeo Schalter Und Sensoren Gmbh | Verfahren zum Betreiben eines Fahrerassistenzsystems eines Kraftfahrzeugs, Recheneinrichtung, Fahrerassistenzsystem sowie Kraftfahrzeug |
| CN106412826B (zh) * | 2016-09-07 | 2019-09-17 | 清华大学 | 基于多源信息融合的室内定位方法及定位装置 |
| CN107389088B (zh) * | 2017-05-27 | 2020-11-17 | 纵目科技（上海）股份有限公司 | 车载惯性导航的误差修正方法、装置、介质及设备 |
| KR102044026B1 (ko) * | 2017-11-20 | 2019-11-12 | 현대오트론 주식회사 | 라이다 센서를 이용한 자율 주차 시스템 및 그것의 실내 측위 방법 |
| CN108492619A (zh) * | 2018-04-17 | 2018-09-04 | 济南浪潮高新科技投资发展有限公司 | 一种智能停车系统及方法 |
| CN109029417B (zh) * | 2018-05-21 | 2021-08-10 | 南京航空航天大学 | 基于混合视觉里程计和多尺度地图的无人机slam方法 |
| CN108871336B (zh) * | 2018-06-20 | 2019-05-07 | 湘潭大学 | 一种车辆位置估算系统及方法 |
| CN109631896B (zh) * | 2018-07-23 | 2020-07-28 | 同济大学 | 一种基于车辆视觉和运动信息的停车场自主泊车定位方法 |
| CN109085840B (zh) * | 2018-09-21 | 2022-05-27 | 大连维德集成电路有限公司 | 一种基于双目视觉的车辆导航控制系统及控制方法 |
| KR101941654B1 (ko) * | 2018-09-28 | 2019-01-24 | (주)신한항업 | 수치지도 내 위치정보와 링크정보의 정밀 검색을 위한 수치지도 합성용 gps좌표의 변형처리 시스템 |
| CN109556617A (zh) * | 2018-11-09 | 2019-04-02 | 同济大学 | 一种自动建图机器人的地图要素提取方法 |
| CN109945858B (zh) * | 2019-03-20 | 2021-04-13 | 浙江零跑科技有限公司 | 用于低速泊车驾驶场景的多传感融合定位方法 |
| CN110017850B (zh) * | 2019-04-19 | 2021-04-20 | 小狗电器互联网科技（北京）股份有限公司 | 一种陀螺仪漂移估计方法、装置及定位系统 |
-
2019
- 2019-07-26 CN CN201910680100.6A patent/CN110555801B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109131317A (zh) * | 2018-07-23 | 2019-01-04 | 同济大学 | 基于多段式规划与机器学习的自动垂直泊车系统及方法 |
| CN109710724A (zh) * | 2019-03-27 | 2019-05-03 | 深兰人工智能芯片研究院（江苏）有限公司 | 一种构建点云地图的方法和设备 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110555801A (zh) | 2019-12-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110555801B (zh) | 一种航迹推演的校正方法、终端和存储介质 | |
| JP6930600B2 (ja) | 車両位置推定装置および車両制御装置 | |
| CN107646114B (zh) | 用于估计车道的方法 | |
| EP3644294B1 (en) | Vehicle information storage method, vehicle travel control method, and vehicle information storage device | |
| CN105539586B (zh) | 用于自主驾驶的车辆躲避移动障碍物的统一的运动规划 | |
| US20170016740A1 (en) | Method and apparatus for determining a vehicle ego-position | |
| CN110865374A (zh) | 定位系统 | |
| US20190064826A1 (en) | Autonomous driving device | |
| JP7156924B2 (ja) | 車線境界設定装置、車線境界設定方法 | |
| CN115235500B (zh) | 基于车道线约束的位姿校正方法及装置、全工况静态环境建模方法及装置 | |
| US11161516B2 (en) | Vehicle control device | |
| CN110515376B (zh) | 一种航迹推演校正的评价方法、终端和存储介质 | |
| JP6756101B2 (ja) | 物体認識装置 | |
| US20220108473A1 (en) | Traffic camera calibration | |
| CN112824997A (zh) | 用于局部行进车道感知的方法和系统 | |
| CN111881245B (zh) | 能见度动态地图的产生方法、装置、设备及存储介质 | |
| CN111376902A (zh) | 一种自动驾驶的车道保持方法及系统 | |
| CN112215214A (zh) | 调整智能车载终端的摄像头偏移的方法及系统 | |
| US20230098314A1 (en) | Localizing and updating a map using interpolated lane edge data | |
| CN110893845A (zh) | 用于对角车道检测的方法与装置 | |
| WO2020258222A1 (en) | Method and system for identifying object | |
| CN113763693A (zh) | 一种车辆数据处理方法、装置、介质和设备 | |
| US20230227035A1 (en) | Method used for deriving a control variable for lateral guidance of a motor vehicle | |
| US20230192116A1 (en) | Vehicle control with incomplete calibration | |
| US11887272B2 (en) | Method and system for determining a spatial transformation employing partial dimension iterative closest point |
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 |