CN111044053B - 单舵轮无人车的导航方法、装置和单舵轮无人车 - Google Patents
单舵轮无人车的导航方法、装置和单舵轮无人车 Download PDFInfo
- Publication number
- CN111044053B CN111044053B CN201911418490.6A CN201911418490A CN111044053B CN 111044053 B CN111044053 B CN 111044053B CN 201911418490 A CN201911418490 A CN 201911418490A CN 111044053 B CN111044053 B CN 111044053B
- Authority
- CN
- China
- Prior art keywords
- wheel
- speed
- period
- estimated
- current
- 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
Images
Classifications
-
- 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
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D63/00—Motor vehicles or trailers not otherwise provided for
- B62D63/02—Motor vehicles
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Electromagnetism (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Chemical & Material Sciences (AREA)
- Combustion & Propulsion (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明涉及单舵轮无人车的导航方法、装置和单舵轮无人车,该导航方法通过获取当前预估周期的舵角测量值和轮向速度测量值并根据当前预估周期的舵角测量值和轮向速度测量值计算单舵轮无人车在下一预估周期所对应的下一预估速度,获取单舵轮无人车在下一观测周期所对应的位姿测量数据,位姿测量数据包括对应的位置测量数据,根据当前预估速度和下一预估速度对位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果,根据位置滤波结果、位置测量数据和预设比例系数计算得到轮向速度偏差,根据轮向速度偏差对下一预估周期对应的轮向速度测量值进行修正以得到轮向速度输出值,提高了导航数据输出频率。
Description
技术领域
本发明涉及控制领域,尤其涉及一种单舵轮无人车的导航方法、装置和单舵轮无人车。
背景技术
单舵轮无人车上设置的激光雷达可测量出单舵轮无人车的准确位置信息,然而激光雷达的数据输出频率往往过低,而且往往容易受换将影响。
发明内容
鉴于此,提供一种单舵轮无人车的导航方法、装置和单舵轮无人车,能够融合单舵轮无人车的轮速数据和激光雷达数据,将快速更新的预估周期的轮速参数引入卡尔曼滤波器,进而利用激光雷达的位姿测量数据中的位置测量数据进行卡尔曼滤波,得到对应的位置滤波结果,进一步根据位置滤波结果、位置测量数据和预设比例系数计算得到轮向速度偏差,最后根据轮向速度偏差对下一预估周期对应的轮向速度测量值进行修正以得到轮向速度输出值作为导航数据进行输出,两者相互融合弥补各自的缺点,达到了增加导航数据输出频率的目的,同时也保证导航数据输出的稳定性,为单舵轮无人车的导航提供了准确的导航基础。
一种单舵轮无人车的导航方法,单舵轮无人车上设置轮速传感器和激光雷达,导航方法包括:
通过轮速传感器获取当前预估周期的舵角测量值和轮向速度测量值并根据当前预估周期的舵角测量值和轮向速度测量值计算单舵轮无人车在下一预估周期所对应的下一预估速度;
通过激光雷达获取单舵轮无人车在下一观测周期所对应的位姿测量数据,位姿测量数据包括对应的位置测量数据;其中,每个观测周期包括至少两个对应的预估周期;
根据当前预估速度和下一预估速度对位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果;
根据位置滤波结果、位置测量数据和预设比例系数计算得到轮向速度偏差,预设比例系数根据轮速传感器的输出噪声确定;
根据轮向速度偏差对下一预估周期对应的轮向速度测量值进行修正以得到轮向速度输出值作为导航数据进行输出。
在一个实施例中,根据当前预估速度和下一预估速度对位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果的步骤包括:
根据当前预估速度和下一预估速度确定当前观测周期所对应的速度增加率;
将速度增加率作为卡尔曼滤波的输入控制量以对位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果。
在一个实施例中,根据当前预估速度和下一预估速度确定当前观测周期所对应的速度增加率的步骤包括:
根据下一预估速度与当前预估速度之间的速度差值计算得到当前预估周期对应的速度增加量;
根据当前预估周期对应的速度增加量和当前观测周期计算确定当前观测周期所对应的速度增加率。
在一个实施例中,卡尔曼滤波为四阶卡尔曼滤波,将速度增加率作为卡尔曼滤波的输入控制量以对位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果中对应的计算公式为:
其中,Φ表示状态转移矩阵,b表示输入转换矩阵,x(k)表示在当前预估周期k时刻的位置测量数据所对应的后验状态估计值,x(k+1|k)表示在下一预估周期k+1时刻的位置测量数据所对应的先验状态估计值,x(k+1)表示在下一预估周期k+1时刻的位置滤波结果,u表示速度增加率,P(k)表示x(k)的后验估计协方差,P(k+1|k)表示x(k+1|k)的先验估计协方差,P(k+1)表示x(k+1)后验估计协方差,Q表示过程激励噪声协方差,H表示状态变量到测量的转换矩阵,τ表示噪声驱动矩阵,K表示卡尔曼滤波增益,R表示测量噪声协方差,Z表示位置测量数据;
在一个实施例中,过程激励噪声协方差Q的取值为0.8。
在一个实施例中,根据当前预估周期的舵角测量值和轮向速度测量值计算单舵轮无人车在下一预估周期所对应的下一预估速度所对应的计算公式采用以下公式:
其中,Vw表示当前预估周期内的轮向速度测量值,vx表示下一预估速度的水平分量,vy表示下一预估速度的竖直分量,β表示当前预估周期内的舵角测量值,θk+1表示下一预估航向角,θk表示当前预估航向角,ω表示当前航向角速度,L为单舵轮无人车的车身长度,tk表示当前观测时刻,tk+1表示下一观测时刻,dt表示单个预估周期。
在一个实施例中,位姿测量数据还包括对应的航向角测量数据,导航方法还包括:
根据当前预估周期的舵角测量值和轮向速度测量值计算单舵轮无人车在下一预估周期所对应的下一预估航向角;
对下一预估航向角进行低通滤波处理,并根据航向角测量数对低通滤波处理后的下一预估航向角进行修正以得到航向角输出值。
此外,还提供一种单舵轮无人车的导航装置,单舵轮无人车上设置轮速传感器和激光雷达,导航装置包括:
预估速度生成装置,用于通过轮速传感器获取当前预估周期的舵角测量值和轮向速度测量值并根据当前预估周期的舵角测量值和轮向速度测量值计算单舵轮无人车在下一预估周期所对应的下一预估速度;
测量数据获取装置,用于通过激光雷达获取单舵轮无人车在下一观测周期所对应的位姿测量数据,位姿测量数据包括对应的位置测量数据;其中,每个观测周期包括至少两个对应的预估周期;
滤波处理装置,用于根据当前预估速度和下一预估速度对位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果;
偏差计算装置,用于根据位置滤波结果、位置测量数据和预设比例系数计算得到轮向速度偏差,预设比例系数根据轮速传感器的输出噪声确定;
数据输出装置,用于根据轮向速度偏差对下一预估周期对应的轮向速度测量值进行修正以得到轮向速度输出值作为导航数据进行输出。
此外,还提供一种单舵轮无人车,设置轮速传感器和激光雷达,单舵轮无人车还包括存储器以及处理器,存储器用于存储计算机程序,处理器运行计算机程序以使单舵轮无人车执行上述的导航方法。
一种可读存储介质,可读存储介质存储有计算机程序,计算机程序在被处理器运行时执行上述导航方法。
上述单舵轮无人车的导航方法,单舵轮无人车上设置轮速传感器和激光雷达,通过轮速传感器获取当前预估周期的舵角测量值和轮向速度测量值并根据当前预估周期的舵角测量值和轮向速度测量值计算单舵轮无人车在下一预估周期所对应的下一预估速度,通过激光雷达获取单舵轮无人车在下一观测周期所对应的位姿测量数据,位姿测量数据包括对应的位置测量数据;其中,每个观测周期包括至少两个对应的预估周期,根据当前预估速度和下一预估速度对位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果,根据位置滤波结果、位置测量数据和预设比例系数计算得到轮向速度偏差,预设比例系数根据轮速传感器的输出噪声确定,根据轮向速度偏差对下一预估周期对应的轮向速度测量值进行修正以得到轮向速度输出值以作为导航数据输出,能够融合单舵轮无人车的轮速数据和激光雷达数据,将快速更新的预估周期的轮速参数引入卡尔曼滤波器,进而利用激光雷达的位姿测量数据中的位置测量数据进行卡尔曼滤波,得到对应的位置滤波结果,进一步根据位置滤波结果、位置测量数据和预设比例系数计算得到轮向速度偏差,最后根据轮向速度偏差对下一预估周期对应的轮向速度测量值进行修正以得到轮向速度输出值,其中,轮速作为预估输入量,短周期内可信度较高,而激光雷达数据中的位置测量数据作为卡尔曼滤波器的观测量,无误差的叠加效应,两者相互融合弥补各自的缺点,既达到了增加导航数据输出频率的目的,同时也保证导航数据输出的稳定平滑和不发散,为单舵轮无人车的导航提供了准确的导航基础。
附图说明
为了更清楚地说明本发明的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,应当理解,以下附图仅示出了本发明的某些实施例,因此不应被看作是对本发明保护范围的限定。在各个附图中,类似的构成部分采用类似的编号。
图1为一个实施例中提供的一种单舵轮无人车的导航方法的流程示意图;
图2为一个实施例中提供的一种当前观测周期与预估周期之间对应关系的界面示意图;
图3为一个实施例中提供的一种得到下一观测周期所对应的位置滤波结果的方法流程示意图;
图4为一个实施例中提供的计算确定当前观测周期所对应的速度增加率的方法流程示意图;
图5为一个实施例中提供的单舵轮无人车的俯视界面示意图;
图6为一个实施例中提供的一种单舵轮无人车的导航装置的结构框图。
具体实施方式
下面将结合本发明实施例中附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。
通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此,以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围,而是仅仅表示本发明的选定实施例。基于本发明的实施例,本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例,都属于本发明保护的范围。
在下文中,将更全面地描述本公开的各种实施例。本公开可具有各种实施例,并且可在其中做出调整和改变。然而,应理解:不存在将本公开的各种实施例限于在此公开的特定实施例的意图,而是应将本公开理解为涵盖落入本公开的各种实施例的精神和范围内的所有调整、等同物和/或可选方案。
在下文中,可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合,并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。
此外,术语“第一”、“第二”、“第三”等仅用于区分描述,而不能理解为指示或暗示相对重要性。
除非另有限定,否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义,除非在本发明的各种实施例中被清楚地限定。
图1为一个实施例中提供的一种单舵轮无人车的导航方法,单舵轮无人车上设置轮速传感器和激光雷达,导航方法包括:
步骤S110,通过轮速传感器获取当前预估周期的舵角测量值和轮向速度测量值并根据当前预估周期的舵角测量值和轮向速度测量值计算单舵轮无人车在下一预估周期所对应的下一预估速度。
其中,单舵轮无人的车身上设置的轮速传感器可获取无人车在当前预估周期的舵角测量值和轮向速度测量值,然后进一步根据上述舵角测量值和轮向速度测量值计算出单舵轮无人车在下一预估周期所对应的下一预估速度。
其中,单舵轮无人车的轮速测量周期通常称之为预估周期。
在一个实施例中,上述单舵轮无人车的轮速预估周期为20HZ。
步骤S120,通过激光雷达获取单舵轮无人车在下一观测周期所对应的位姿测量数据,位姿测量数据包括对应的位置测量数据;其中,每个观测周期包括至少两个对应的预估周期。
其中,单舵轮无人车上设置的激光雷达数据输出的频率通常为低频频率,在单舵轮无人车的下一预估周期开始后,通过激光雷达可获取舵轮无人车在下一观测周期所对应的位姿测量数据,位姿测量数据包括对应的位置测量数据,其中,每个观测周期包括至少两个对应的预估周期。
在一个实施例中,如图2所示,当前观测周期对应于上一预估周期和当前预估周期,在下一预估周期开始后的某一时刻,激光雷达的新的观测数据输出,此时开始下一观测周期。
在一个实施例中,上述激光雷达的观测频率为8HZ。
步骤S130,根据当前预估速度和下一预估速度对位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果。
其中,将当前预估速度和下一预估速度等轮速数据得到的预估数据参数引入对位置测量数据进行卡尔曼滤波处理的过程当中,从而为后续的融合过程奠定基础。
其中,根据当前预估速度和下一预估速度对位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果。
步骤S140,根据位置滤波结果、位置测量数据和预设比例系数计算得到轮向速度偏差,预设比例系数根据轮速传感器的输出噪声确定。
其中,在得到上述位置滤波结果之后,进一步结合激光雷达的位置测量数据,以及预设比例系数,可进一步得到轮向速度偏差,输出噪声越大,对应的预设比例系数越大。
其中,上述预设比例系数根据轮速传感器的输出噪声确定。
步骤S150,根据轮向速度偏差对下一预估周期对应的轮向速度测量值进行修正以得到轮向速度输出值作为导航数据进行输出。
其中,当下一预估周期的轮向速度测量值到来时,可根据轮向速度偏差对下一预估周期对应的轮向速度测量值进行修正,以生成轮向速度输出值作为导航数据进行输出。
上述单舵轮无人车的导航方法,能够融合单舵轮无人车的轮速数据和激光雷达数据,将快速更新的预估周期的轮速参数引入卡尔曼滤波器,进而利用激光雷达的位姿测量数据中的位置测量数据进行卡尔曼滤波,得到对应的位置滤波结果,进一步根据位置滤波结果、位置测量数据和预设比例系数计算得到轮向速度偏差,最后根据轮向速度偏差对下一预估周期对应的轮向速度测量值进行修正以得到轮向速度输出值,其中,轮速作为预估输入量,短周期内可信度较高,而激光雷达数据中的位置测量数据作为卡尔曼滤波器的观测量,无误差的叠加效应,两者相互融合弥补各自的缺点,既达到了增加导航数据输出频率的目的,同时也保证导航数据输出的稳定平滑和不发散,为单舵轮无人车的导航提供了准确的导航基础。
在一个实施例中,如图3所示,步骤S130包括:
步骤S132,根据当前预估速度和下一预估速度确定当前观测周期所对应的速度增加率。
步骤S134,将速度增加率作为卡尔曼滤波的输入控制量以对位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果。
其中,上述过程利用当前预估速度和下一预估速度确定当前观测周期所对应的速度增加率,进而将速度增加率作为卡尔曼滤波的输入控制量以对位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果,将轮速传感器数据和卡尔曼滤波有机的融合在一起,两者相互融合弥补了各自的缺点。
在一个实施例中,如图4所示,步骤S132包括:
步骤S132a,根据下一预估速度与当前预估速度之间的速度差值计算得到当前预估周期对应的速度增加量。
其中,通过计算下一预估速度与当前预估速度之间的速度差值方可得到当前预估周期对应的速度增加量。
步骤S132b,根据当前预估周期对应的速度增加量和当前观测周期计算确定当前观测周期所对应的速度增加率。
进一步地,得到当前预估周期对应的速度增加量之后进一步可根据当前观测周期所对应的速度增加量,进而计算出当前观测周期所对应的速度增加率。
在一个实施例中,卡尔曼滤波为四阶卡尔曼滤波,步骤S134中得到下一观测周期所对应的位置滤波结果中对应的计算公式为:
其中,Φ表示状态转移矩阵,b表示输入转换矩阵,x(k)表示在当前预估周期k时刻的位置测量数据所对应的后验状态估计值,x(k+1|k)表示在下一预估周期k+1时刻的位置测量数据所对应的先验状态估计值,x(k+1)表示在下一预估周期k+1时刻的位置滤波结果,u表示速度增加率,P(k)表示x(k)的后验估计协方差,P(k+1|k)表示x(k+1|k)的先验估计协方差,P(k+1)表示x(k+1)后验估计协方差,Q表示过程激励噪声协方差,H表示状态变量到测量的转换矩阵,τ表示噪声驱动矩阵,K表示卡尔曼滤波增益,R表示测量噪声协方差,Z表示位置测量数据;
在一个实施例中,过程激励噪声协方差Q的取值为0.8。
在一个实施例中,根据当前预估周期的舵角测量值和轮向速度测量值计算单舵轮无人车在下一预估周期所对应的下一预估速度所对应的计算公式采用以下公式:
其中,Vw表示当前预估周期内的轮向速度测量值,vx表示下一预估速度的水平分量,vy表示下一预估速度的竖直分量,β表示当前预估周期内的舵角测量值,θk+1表示下一预估航向角,θk表示当前预估航向角,ω表示当前航向角速度,L为单舵轮无人车的车身长度,tk表示当前观测时刻,tk+1表示下一观测时刻,dt表示单个预估周期。
在一个实施例中,图5为提供的单舵轮无人车的俯视界面示意图,上述计算公式中单舵轮无人车的L、θk、vx、vy、β和Vw之间的关系界面示意图如图5所示。
其中,这里以单舵轮无人车的车身两后轮中心连线的中点B作为参考点,并规定从参考点向前轮看逆时针为正,顺时针为负,且全局坐标系下单舵轮无人车的姿态可用[x,y,θ]表示,车身轴线与X轴正方向的夹角为航向角。
在一个实施例中,位姿测量数据还包括对应的航向角测量数据,导航方法还包括:
根据当前预估周期的舵角测量值和轮向速度测量值计算单舵轮无人车在下一预估周期所对应的下一预估航向角。
其中,在上述根据当前预估周期的舵角测量值和轮向速度测量值计算单舵轮无人车在下一预估周期所对应的下一预估速度的过程中可一并计算出单舵轮无人车在下一预估周期所对应的下一预估航向角。
进一步地,对下一预估航向角进行低通滤波处理,并根据航向角测量数对低通滤波处理后的下一预估航向角进行修正以得到航向角输出值作为导航数据进行输出。
其中,在得到上述下一预估航向角之后,还可以对下一预估航向角进行低通滤波处理,并根据航向角测量数对低通滤波处理后的下一预估航向角进行修正以得到航向角输出值作为导航数据进行输出,进一步提高了航向角数据输出的准确性。
此外,如图6所示,还提供一种单舵轮无人车的导航装置200,单舵轮无人车上设置轮速传感器和激光雷达,导航装置包括:
预估速度生成装置210,用于通过轮速传感器获取当前预估周期的舵角测量值和轮向速度测量值并根据当前预估周期的舵角测量值和轮向速度测量值计算单舵轮无人车在下一预估周期所对应的下一预估速度;
测量数据获取装置220,用于通过激光雷达获取单舵轮无人车在下一观测周期所对应的位姿测量数据,位姿测量数据包括对应的位置测量数据;其中,每个观测周期包括至少两个对应的预估周期;
滤波处理装置230,用于根据当前预估速度和下一预估速度对位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果;
偏差计算装置240,用于根据位置滤波结果、位置测量数据和预设比例系数计算得到轮向速度偏差,预设比例系数根据轮速传感器的输出噪声确定;
数据输出装置250,用于根据轮向速度偏差对下一预估周期对应的轮向速度测量值进行修正以得到轮向速度输出值作为导航数据进行输出。
此外,还提供一种单舵轮无人车,设置轮速传感器和激光雷达,单舵轮无人车还包括存储器以及处理器,存储器用于存储计算机程序,处理器运行计算机程序以使单舵轮无人车执行上述的导航方法。
一种可读存储介质,可读存储介质存储有计算机程序,计算机程序在被处理器运行时执行上述导航方法。
在本申请所提供的几个实施例中,应该理解到,所揭露的装置和方法,也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的,例如,附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上,流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分,所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意,在作为替换的实现方式中,方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如,两个连续的方框实际上可以基本并行地执行,它们有时也可以按相反的顺序执行,这依所涉及的功能而定。也要注意的是,结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合,可以用执行规定的功能或动作的专用的基于硬件的系统来实现,或者可以用专用硬件与计算机指令的组合来实现。
另外,在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分,也可以是各个模块单独存在,也可以两个或更多个模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-Only Memory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述,仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到变化或替换,都应涵盖在本发明的保护范围之内。
Claims (10)
1.一种单舵轮无人车的导航方法,其特征在于,所述单舵轮无人车上设置轮速传感器和激光雷达,所述导航方法包括:
通过所述轮速传感器获取当前预估周期的舵角测量值和轮向速度测量值并根据所述当前预估周期的舵角测量值和轮向速度测量值计算所述单舵轮无人车在下一预估周期所对应的下一预估速度;
通过所述激光雷达获取所述单舵轮无人车在下一观测周期所对应的位姿测量数据,所述位姿测量数据包括对应的位置测量数据;其中,每个观测周期包括至少两个对应的预估周期,在下一预估周期开始后的某一时刻,激光雷达的新的观测数据输出,此时开始下一观测周期;
根据当前预估速度和所述下一预估速度对所述位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果;
根据所述位置滤波结果、所述位置测量数据和预设比例系数计算得到轮向速度偏差,所述预设比例系数根据所述轮速传感器的输出噪声确定;
根据轮向速度偏差对下一预估周期对应的轮向速度测量值进行修正以得到轮向速度输出值作为导航数据进行输出。
2.根据权利要求1所述的导航方法,其特征在于,所述根据当前预估速度和所述下一预估速度对所述位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果的步骤包括:
根据当前预估速度和所述下一预估速度确定当前观测周期所对应的速度增加率;
将所述速度增加率作为卡尔曼滤波的输入控制量以对所述位置测量数据进行所述卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果。
3.根据权利要求2所述的导航方法,其特征在于,所述根据所述当前预估速度和所述下一预估速度确定当前观测周期所对应的速度增加率的步骤包括:
根据所述下一预估速度与所述当前预估速度之间的速度差值计算得到所述当前预估周期对应的速度增加量;
根据所述当前预估周期对应的速度增加量和当前观测周期计算确定当前观测周期所对应的速度增加率。
4.根据权利要求2所述的导航方法,其特征在于,所述卡尔曼滤波为四阶卡尔曼滤波,所述将所述速度增加率作为卡尔曼滤波的输入控制量以对所述位置测量数据进行所述卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果中对应的计算公式为:
其中,Φ表示状态转移矩阵,b表示输入转换矩阵,x(k)表示在所述当前预估周期k时刻的位置测量数据所对应的后验状态估计值,x(k+1|k)表示在所述下一预估周期k+1时刻的位置测量数据所对应的先验状态估计值,x(k+1)表示在所述下一预估周期k+1时刻的所述位置滤波结果,u表示所述速度增加率,P(k)表示所述x(k)的后验估计协方差,P(k+1|k)表示所述x(k+1|k)的先验估计协方差,P(k+1)表示所述x(k+1)的后验估计协方差,Q表示过程激励噪声协方差,H表示状态变量到测量的转换矩阵,τ表示噪声驱动矩阵,K表示卡尔曼滤波增益,R表示测量噪声协方差,Z表示所述位置测量数据;
其中,P初始值为单位矩阵I,
Q∈(0,1),
5.根据权利要求4所述的导航方法,其特征在于,所述过程激励噪声协方差Q的取值为0.8。
7.根据权利要求1所述的导航方法,其特征在于,所述位姿测量数据还包括对应的航向角测量数据,所述导航方法还包括:
根据当前预估周期的舵角测量值和轮向速度测量值计算所述单舵轮无人车在下一预估周期所对应的下一预估航向角;
对所述下一预估航向角进行低通滤波处理,并根据所述航向角测量数对低通滤波处理后的下一预估航向角进行修正以得到航向角输出值作为导航数据进行输出。
8.一种单舵轮无人车的导航装置,其特征在于,所述单舵轮无人车上设置轮速传感器和激光雷达,所述导航装置包括:
预估速度生成装置,用于通过所述轮速传感器获取当前预估周期的舵角测量值和轮向速度测量值并根据所述当前预估周期的舵角测量值和轮向速度测量值计算所述单舵轮无人车在下一预估周期所对应的下一预估速度;
测量数据获取装置,用于通过所述激光雷达获取所述单舵轮无人车在下一观测周期所对应的位姿测量数据,所述位姿测量数据包括对应的位置测量数据;其中,每个观测周期包括至少两个对应的预估周期,在下一预估周期开始后的某一时刻,激光雷达的新的观测数据输出,此时开始下一观测周期;
滤波处理装置,用于根据当前预估速度和所述下一预估速度对所述位置测量数据进行卡尔曼滤波处理,得到下一观测周期所对应的位置滤波结果;
偏差计算装置,用于根据所述位置滤波结果、所述位置测量数据和预设比例系数计算得到轮向速度偏差,所述预设比例系数根据所述轮速传感器的输出噪声确定;
数据输出装置,用于根据轮向速度偏差对下一预估周期对应的轮向速度测量值进行修正以得到轮向速度输出值作为导航数据进行输出。
9.一种单舵轮无人车,其特征在于,设置轮速传感器和激光雷达,所述单舵轮无人车还包括存储器以及处理器,所述存储器用于存储计算机程序,所述处理器运行所述计算机程序以使所述单舵轮无人车执行权利要求1至7中任一项所述的导航方法。
10.一种可读存储介质,其特征在于,所述可读存储介质存储有计算机程序,所述计算机程序在被处理器运行时执行权利要求1至7中任一项所述的导航方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911418490.6A CN111044053B (zh) | 2019-12-31 | 2019-12-31 | 单舵轮无人车的导航方法、装置和单舵轮无人车 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911418490.6A CN111044053B (zh) | 2019-12-31 | 2019-12-31 | 单舵轮无人车的导航方法、装置和单舵轮无人车 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111044053A CN111044053A (zh) | 2020-04-21 |
CN111044053B true CN111044053B (zh) | 2022-04-01 |
Family
ID=70243239
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911418490.6A Active CN111044053B (zh) | 2019-12-31 | 2019-12-31 | 单舵轮无人车的导航方法、装置和单舵轮无人车 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111044053B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112025706B (zh) * | 2020-08-26 | 2022-01-04 | 北京市商汤科技开发有限公司 | 机器人的状态确定方法及装置、机器人及存储介质 |
CN113927157B (zh) * | 2021-12-17 | 2022-03-08 | 武汉锐科光纤激光技术股份有限公司 | 激光设备输出功率的控制方法、装置、计算机设备及介质 |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110262517A (zh) * | 2019-07-18 | 2019-09-20 | 石家庄辰宙智能装备有限公司 | Agv系统的轨迹跟踪控制方法 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9816820B2 (en) * | 2016-01-07 | 2017-11-14 | Caterpillar Inc. | Positioning system having smoothed kalman filter update |
CN109883423B (zh) * | 2019-02-25 | 2021-02-09 | 广州市香港科大霍英东研究院 | 基于卡尔曼滤波的定位方法、系统、设备及存储介质 |
CN110307836B (zh) * | 2019-07-10 | 2021-05-07 | 北京智行者科技有限公司 | 一种用于无人清扫车辆贴边清扫的精确定位方法 |
-
2019
- 2019-12-31 CN CN201911418490.6A patent/CN111044053B/zh active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110262517A (zh) * | 2019-07-18 | 2019-09-20 | 石家庄辰宙智能装备有限公司 | Agv系统的轨迹跟踪控制方法 |
Non-Patent Citations (2)
Title |
---|
单舵轮激光导引AGV控制器的设计与实现;张微;《中国优秀硕士学位论文全文数据库信息科技辑》;20180415(第04期);第12-43页 * |
基于CAN总线激光导引AGV车载控制系统研究;赵国栋;《中国优秀硕士学位论文全文数据库信息科技辑》;20160815(第08期);第10-21、35-53页 * |
Also Published As
Publication number | Publication date |
---|---|
CN111044053A (zh) | 2020-04-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11120562B2 (en) | Posture estimation method, posture estimation apparatus and computer readable storage medium | |
CN111044053B (zh) | 单舵轮无人车的导航方法、装置和单舵轮无人车 | |
CN108508471A (zh) | 一种无人驾驶车辆定位方法及装置 | |
RU2762143C2 (ru) | Система определения курса и углового пространственного положения, выполненная с возможностью функционирования в полярной области | |
CN104075713A (zh) | 一种惯性/天文组合导航方法 | |
EP4345421A2 (en) | Method for calibrating sensor parameters based on autonomous driving, apparatus, storage medium, and vehicle | |
CN113984044A (zh) | 一种基于车载多感知融合的车辆位姿获取方法及装置 | |
CN110637209B (zh) | 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质 | |
CN111751857A (zh) | 一种车辆位姿的估算方法、装置、存储介质及系统 | |
CN111623779A (zh) | 一种适用于噪声特性未知的时变系统自适应级联滤波方法 | |
CN113551666A (zh) | 自动驾驶多传感器融合定位方法和装置、设备及介质 | |
CN113720349A (zh) | 一种基于卡尔曼滤波的里程计信息平滑方法 | |
CN112046491B (zh) | 车轮侧偏刚度的估算方法、装置、车辆及可读存储介质 | |
CN110832274A (zh) | 地面坡度计算方法、装置、设备及存储介质 | |
CN116182905A (zh) | 激光雷达和组合惯导的时空外参标定方法、装置及系统 | |
CN114019954B (zh) | 航向安装角标定方法、装置、计算机设备和存储介质 | |
US11269081B2 (en) | Method and apparatus for determining the absolute position of a motor vehicle, localization system, motor vehicle | |
CN112577512A (zh) | 一种基于轮速融合的状态量误差确定方法及车载终端 | |
CN115727871A (zh) | 一种轨迹质量检测方法、装置、电子设备和存储介质 | |
US20230078005A1 (en) | Navigation assistance method for a mobile carrier | |
CN113959433A (zh) | 一种组合导航方法及装置 | |
CN110940345B (zh) | 车身定位、装置、计算机设备和存储介质 | |
JP2007155365A (ja) | 方位センサの補正係数演算装置及び演算プログラム | |
CN111141279B (zh) | 行车轨迹的处理方法及装置 | |
CN114323007A (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 |