CN103344963A - 一种基于激光雷达的鲁棒航迹推算方法 - Google Patents
一种基于激光雷达的鲁棒航迹推算方法 Download PDFInfo
- Publication number
- CN103344963A CN103344963A CN2013103055124A CN201310305512A CN103344963A CN 103344963 A CN103344963 A CN 103344963A CN 2013103055124 A CN2013103055124 A CN 2013103055124A CN 201310305512 A CN201310305512 A CN 201310305512A CN 103344963 A CN103344963 A CN 103344963A
- Authority
- CN
- China
- Prior art keywords
- theta
- rho
- fault
- alpha
- cos
- 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
Links
Abstract
一种基于激光雷达的鲁棒航迹推算方法,根据移动机器人系统故障模型以及运动学模型,构造不同故障式下的运动学模型;然后构造快速的激光雷达测量模型;最后,在粒子滤波器框架下,整合不同模式运动学模型与测量模型,同时实现故障诊断与航迹推算。
Description
技术领域
本发明属于航迹推算系统测量领域,具体涉及一种基于激光雷达的鲁棒航迹推算方法。
背景技术
自主移动机器人已广泛应用于星球探测、军事搜救、灾难救援、家庭护理以及工业应用等领域。机器人航迹推算根据机器人传感器信息对其位姿进行估计,是移动机器人实现环境建模与自主导航的前提。鲁棒航迹推算是指在机器人出现故障的情况下(如传感器失效、传感器受强噪声干扰及车轮打滑等)仍能利用冗余信息对机器人位姿进行较为准确地估计的技术。
常规地,移动机器人利用部署在其上的内部传感器(如编码器、陀螺仪)和/或外部传感器(如激光雷达、CCD摄像头)进行位姿估计。在系统无故障情况下的航迹推算问题已有诸多成熟技术。然而,当传感器出现故障时,特别是内部传感器和外部传感器可能同时出现故障的情况下,如何利用冗余传感器实信息较为准确地估计移动机器人位姿(即鲁棒航迹推算)问题,目前尚未被系统研究。
实现鲁棒航迹推算的主要困难在于:必须同时对机器人实现故障诊断与位姿估计。因为,内部传感器故障会改变其运动学模型;外部传感器故障则要求建立新的感知模型。
由于故障诊断和位姿估计分别是离散和连续状态估计问题,而粒子滤波器是估计混合系统的一种非参数方法,适合与非线性、非高斯系统估计问题,因此,本技术采用粒子滤波器作为状态估计技术。
综上,本技术针对部分内部传感器故障、激光雷达部分异常等实际的故障情形,通过构造内部传感器故障情况下的运动学模型、激光雷达鲁棒感知模型,并在粒子滤波算法框架下结合上述两类模型,同时进行机器人故障诊断与位姿估计。
现有技术提出了一种利用光纤陀螺仪(Fiber optic gyroscopes,FOGs)以及间接反馈卡尔曼滤波(Kalman filter)方法提高航迹推算系统的精度。该方法首先对光纤陀螺仪进行精确的标定来减少其尺度因子非线性误差以及不同温度引起的误差。然后,利用间接反馈卡尔曼滤波融合光纤陀螺仪和编码器数据。从而提高航迹推算系统精度。
该方法中,光纤陀螺仪因速度以及温度变化导致的误差模型采用文献[2]提出的模型。测量尺度因子误差的基本方法是用已知的输入速度ω旋转陀螺仪,并与陀螺仪的输出测量值ωg比较,两者之差是由于尺度因子非线性性引起的误差,误差可用三次多项式拟合:
测量误差随温度的变化关系用二次多项式拟合:
ε(T)=b0+b1T+b2T2 (2)
其中,ωg为陀螺仪量测值,T为温度,a0,a1,a2,a3,b0,b1以及b2为待定参数。
现有技术一的缺点:
1)对光纤陀螺仪的标定需要专门的高精度仪器,标定过程复杂,不易操作且成本高。且无论如何标定,都难以对陀螺以漂移误差进行精确建模。
2)如果编码器和/或陀螺仪出现故障,此方法则完全失效。
3)采用Kalman滤波器,无法处理非线性系统估计问题。
Verma等利用粒子滤波器(Particle Filter,PF)来解决计算复杂度以及高斯假设问题,对漫游车(rover)进行状态检测和故障诊断。
该技术特点之一:提出了一种变分辨率粒子滤波器(VariableParticle Filter,VRPF),并应用于漫游车故障诊断问题。VRPF通过将具有相似特征的状态抽象为一个状态。一个抽象状态可以表示单个状态或状态集合,当状态表示成抽象状态时,状态空间被缩小了。VRPF动态地改变状态空间的分辨率:在信度强时分辨率高,信度强的抽象状态被分解为多个更细的抽象状态;信度弱时分辨率低,此时将多个相似状态合并为一个抽象状态。
该技术特点之二:针对低发生概率的故障可能存在的漏检问题,提出了风险敏感粒子滤波器(Risk Sensitive Particle Filter,RSPF),通过提高低概率故障的风险水平增加对低概率故障的采用概率,该方法的风险评价是主观的且不能完全避免漏检。
该技术只能诊断机器人内部传感器故障,却不能提供准确的航迹推算信息。
文献提出了一种航迹推算系统软故障诊断与补偿方法。软故障是指元件性能下降但没有完全失效。该技术考虑与航迹推算密切相关的两类软故障:(1)编码器和陀螺仪等内部传感器噪声和尺度因子故障;(2)移动机构异常,包括(a)车轮打滑,(b)车轮爆胎,以及(c)车轮受阻。
该方法首先构造了移动机器人运动学模型,然后根据机器人内部传感器之间的冗余关系构造残差特征。根据残差特征设计机器人内部传感器软故障检测的阈值逻辑。此外,残差特征也作为后续粒子滤波估计的噪声方差调整依据。
该技术没有对航迹推算传感器出现失效故障时加以考虑。
本发明针对航迹推算系统内部传感器可能发生失效故障、激光雷达测量也可能受大噪声干扰的情况,在诊断内部传感器故障的同时,利用激光雷达的有效测量,快速提供较为准确的航迹推算信息。
发明内容
该技术的整体思路为将故障诊断与航迹推算问题作为一个离散与连续状态的联合估计问题。首先,根据移动机器人系统故障模型以及运动学模型,构造不同故障式下的运动学模型;其次,构造快速的激光雷达测量模型;最后,在粒子滤波器框架下,整合不同模式运动学模型与测量模型,同时实现故障诊断与航迹推算。
设Гt-1,Гt分别表示移动机器人t-1,t时刻的坐标系(位姿)。移动机器人航迹推算系统是计算从Гt-1到Гt的齐次变换At。
其中, 表示旋转矩阵,pt=(xt,yt)T表示平移向量。令xt=(xt,yt,θt)T,则航迹推算的实质是估计3维连续状态xt。
适用性说明:本技术适用于平面环境下二轮差动驱动移动机器人,要求该机器人配备有编码器(左、右轮各一)、陀螺仪、激光雷达。
为了实现在传感器故障情况下对移动机器人进行较为快速准确的航迹推算,本技术方案主要包含步骤。
步骤l:建立故障模式及其运动学模型
移动机器人内部传感器故障模式如表1所示,不同的故障模式其运动学模型各不相同,分别表示为公式(6-9)。
表1移动机器人内部传感器故障模式
步骤2:激光雷达感知模型
该步骤通过对激光雷达读数进行分段分析、逆射线跟踪以及高通滤波三个子步骤构造激光雷达快速鲁棒的感知模型。
步骤2.1:通过分段分析方法构建激光雷达有效感知窗口
步骤2.2:利用逆射线跟踪方法确定误差
步骤2.3:利用高通滤波实现鲁棒感知模型
步骤3:利用粒子滤波方法实现故障诊断和鲁棒航迹推算。
1)初始化,设置粒子数目N以及离散故障集合S={1,2,3,4}
2)对于每一时间步t,执行3-15
3)对于前4个粒子(i从1至4)执行4-8
4)
5)
6)
7)
8)
9)i从5至N循环,执行10-12
10)采样使得
11)根据粒子i的离散状态确定的运动学模型,采样连续状态
12)计算权重
13)计算边缘分布
14)故障诊断,
15)航迹推算,
本发明针对航迹推算系统内部传感器可能发生失效故障、激光雷达测量也可能受大噪声干扰的情况,同时诊断内部传感器故障并利用激光雷达的有效测量数据快速提供较为准确的航迹推算信息。
附图说明
下面结合附图,通过非限定性的举例对本发明的优选实施方式作进一步说明,在附图中:图1逆射线跟踪原理图
具体实施方式
以下将对本发明的优选实施例进行详细的描述;应当理解,优选实施例仅为了说明本发明,而不是为了限制本发明的保护范围。
其中,D表示两轮间的轴距,对4种故障模式分别构造运动学模型如下:
模式1(无故障模式)的运动学模型:
模式2(左编码器故障)的运动学模型:
模式3(右编码器故障)的运动学模型:
模式4(陀螺仪故障)的运动学模型:
所述的激光雷达感知模型构建方法为:
(1)通过分段分析方法构建激光雷达有效感知窗口,设激光雷达t时刻的获得的原始数据表示为Rt,
其中,Wt表示Rt的有效感知窗口,γ,λ2,λ1分别表示阈值。特别地,对于LMS291激光雷达,设置γ=5,λ2=81.9m,λ1=.03m;
(2)利用逆射线跟踪方法确定误差
其中,
(3)利用高通滤波实现鲁棒感知模型
为实现上述目标,采用如下方法计算p(Rt|Rt-1,xt),
所述利用粒子滤波方法实现故障诊断和鲁棒航迹推算具体过程:
16)初始化,设置粒子数目N以及离散故障集合S={1,2,3,4)
17)对于每一时间步t,执行3-15
18)对于前4个粒子(i从1至4)执行4-8
19)
20)
21)
22)
23)
24)i从5至N循环,执行10-12
26)根据粒子i的离散状态确定的运动学模型,采样连续状态
27)计算权重
28)计算边缘分布
29)故障诊断,
30)航迹推算,
第3-8步确保每一个离散状态均被采样,同时确定了离散故障模式的初始概率,第10步说明离散故障模式采样概率由步骤8确定,第11步采样连续的状态,第14步进行故障诊断,第15步进行航迹推算。
Claims (1)
1.一种基于激光雷达的鲁棒航迹推算方法,根据移动机器人系统故障模型以及运动学模型,构造不同故障式下的运动学模型;然后构造快速的激光雷达测量模型;最后,在粒子滤波器框架下,整合不同模式运动学模型与测量模型,同时实现故障诊断与航迹推算,所述的不同故障式下的运动学模型的构造方法为:不同的故障模式其运动学模型各不相同,分别表示为公式(6-9):
其中,D表示两轮间的轴距,对4种故障模式分别构造运动学模型如下:
模式1(无故障模式)的运动学模型:
模式2(左编码器故障)的运动学模型:
模式3(右编码器故障)的运动学模型:
模式4(陀螺仪故障)的运动学模型:
所述的激光雷达感知模型构建方法为:
(1)通过分段分析方法构建激光雷达有效感知窗口,设激光雷达t时刻的获得的原始数据表示为Rt,
其中,Wt表示Rt的有效感知窗口,γ,λ2,λ1分别表示阈值。特别地,对于LMS291激光雷达,设置γ=5,λ2=81.9m,λ1=.03m;
(2)利用逆射线跟踪方法确定误差
其中,
(3)利用高通滤波实现鲁棒感知模型
为实现上述目标,采用如下方法计算p(Rt|Rt-l,xt),
所述利用粒子滤波方法实现故障诊断和鲁棒航迹推算具体过程:
1)初始化,设置粒子数目N以及离散故障集合S={1,2,3,4}
2)对于每一时间步t,执行3-15
3)对于前4个粒子(i从1至4)执行4-8
4)
5)
6)
7)
8)
9)i从5至N循环,执行10-12
11)根据粒子i的离散状态确定的运动学模型,采样连续状态
12)计算权重
13)计算边缘分布
14)故障诊断,
15)航迹推算,
第3-8步确保每一个离散状态均被采样,同时确定了离散故障模式的初始概率,第10步说明离散故障模式采样概率由步骤8确定,第11步采样连续的状态,第14步进行故障诊断,第15步进行航迹推算。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310305512.4A CN103344963B (zh) | 2013-07-12 | 2013-07-12 | 一种基于激光雷达的鲁棒航迹推算方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310305512.4A CN103344963B (zh) | 2013-07-12 | 2013-07-12 | 一种基于激光雷达的鲁棒航迹推算方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103344963A true CN103344963A (zh) | 2013-10-09 |
CN103344963B CN103344963B (zh) | 2016-05-18 |
Family
ID=49279776
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201310305512.4A Expired - Fee Related CN103344963B (zh) | 2013-07-12 | 2013-07-12 | 一种基于激光雷达的鲁棒航迹推算方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103344963B (zh) |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105095918A (zh) * | 2015-09-07 | 2015-11-25 | 上海交通大学 | 一种多机器人系统故障诊断方法 |
CN107218939A (zh) * | 2017-06-04 | 2017-09-29 | 吕文君 | 一种基于运动分解的移动机器人航迹推算定位方法 |
CN109669350A (zh) * | 2017-10-13 | 2019-04-23 | 电子科技大学中山学院 | 一种三轮全向移动机器人车轮滑动定量估计方法 |
CN109765569A (zh) * | 2017-11-09 | 2019-05-17 | 电子科技大学中山学院 | 一种基于激光雷达实现虚拟航迹推算传感器的方法 |
CN110333526A (zh) * | 2019-05-22 | 2019-10-15 | 电子科技大学中山学院 | 一种基于gps的仿真航迹推算传感器方法 |
CN113298317A (zh) * | 2021-06-16 | 2021-08-24 | 电子科技大学中山学院 | 一种风力发电机组偏航控制方法 |
CN113375658A (zh) * | 2021-06-15 | 2021-09-10 | 电子科技大学中山学院 | 移动机器人故障下同时fdd和slam的方法及系统 |
CN114856422A (zh) * | 2022-05-07 | 2022-08-05 | 中国矿业大学 | 钻孔机器人用全自主移动底盘控制系统及控制方法 |
-
2013
- 2013-07-12 CN CN201310305512.4A patent/CN103344963B/zh not_active Expired - Fee Related
Non-Patent Citations (2)
Title |
---|
DUAN ZHUO-HUA ET AL.: "Fault Diagnosis and Fault Tolerant Control for Wheeled Mobile Robots under Unknown Environments : A Survey", 《ROBOTICS AND AUTOMATION, 2005. ICRA 2005. PROCEEDINGS OF THE 2005 IEEE INTERNATIONAL CONFERENCE ON》, 22 April 2005 (2005-04-22), pages 3428 - 3433 * |
段琢华: "基于自适应粒子滤波器的移动机器人故障诊断理论与方法研究", 《中国博士学位论文全文数据库(电子期刊)》, no. 1, 31 January 2008 (2008-01-31), pages 1 - 108 * |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105095918A (zh) * | 2015-09-07 | 2015-11-25 | 上海交通大学 | 一种多机器人系统故障诊断方法 |
CN105095918B (zh) * | 2015-09-07 | 2018-06-26 | 上海交通大学 | 一种多机器人系统故障诊断方法 |
CN107218939A (zh) * | 2017-06-04 | 2017-09-29 | 吕文君 | 一种基于运动分解的移动机器人航迹推算定位方法 |
CN109669350A (zh) * | 2017-10-13 | 2019-04-23 | 电子科技大学中山学院 | 一种三轮全向移动机器人车轮滑动定量估计方法 |
CN109669350B (zh) * | 2017-10-13 | 2021-07-20 | 电子科技大学中山学院 | 一种三轮全向移动机器人车轮滑动定量估计方法 |
CN109765569A (zh) * | 2017-11-09 | 2019-05-17 | 电子科技大学中山学院 | 一种基于激光雷达实现虚拟航迹推算传感器的方法 |
CN110333526A (zh) * | 2019-05-22 | 2019-10-15 | 电子科技大学中山学院 | 一种基于gps的仿真航迹推算传感器方法 |
CN110333526B (zh) * | 2019-05-22 | 2021-11-02 | 电子科技大学中山学院 | 一种基于gps的仿真航迹推算传感器方法 |
CN113375658A (zh) * | 2021-06-15 | 2021-09-10 | 电子科技大学中山学院 | 移动机器人故障下同时fdd和slam的方法及系统 |
CN113375658B (zh) * | 2021-06-15 | 2023-05-09 | 电子科技大学中山学院 | 移动机器人故障下同时fdd和slam的方法及系统 |
CN113298317A (zh) * | 2021-06-16 | 2021-08-24 | 电子科技大学中山学院 | 一种风力发电机组偏航控制方法 |
CN114856422A (zh) * | 2022-05-07 | 2022-08-05 | 中国矿业大学 | 钻孔机器人用全自主移动底盘控制系统及控制方法 |
Also Published As
Publication number | Publication date |
---|---|
CN103344963B (zh) | 2016-05-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103344963A (zh) | 一种基于激光雷达的鲁棒航迹推算方法 | |
KR101871441B1 (ko) | 자율주행차량용 고정밀전자지도 제작을 위한 음성인식 현장조사와 도로선형 생성 시스템 | |
CN113945206B (zh) | 一种基于多传感器融合的定位方法及装置 | |
CN101726295B (zh) | 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法 | |
CN102654404B (zh) | 一种提高航姿参考系统解算精度和系统抗干扰能力的方法 | |
US20140316708A1 (en) | Oriented Wireless Structural Health and Seismic Monitoring | |
CN103363993A (zh) | 一种基于无迹卡尔曼滤波的飞机角速率信号重构方法 | |
CN112505737B (zh) | 一种gnss/ins组合导航方法 | |
CN113819914A (zh) | 一种地图构建方法及装置 | |
CN103674021A (zh) | 基于捷联惯导与星敏感器的组合导航系统及方法 | |
Zhang et al. | Lidar-IMU and wheel odometer based autonomous vehicle localization system | |
CN103712598A (zh) | 一种小型无人机姿态确定系统与确定方法 | |
CN103063218A (zh) | 车辆远程监控及轨迹复现系统及其轨迹复现方法 | |
Kong et al. | Performance improvement of visual-inertial navigation system by using polarized light compass | |
CN114323033A (zh) | 基于车道线和特征点的定位方法、设备及自动驾驶车辆 | |
CN105203130A (zh) | 一种基于信息融合的船舶组合导航系统故障诊断方法 | |
Barrau et al. | Invariant filtering for pose ekf-slam aided by an imu | |
Zheng et al. | Mobile robot integrated navigation algorithm based on template matching VO/IMU/UWB | |
CN114088091A (zh) | 一种基于多传感器的井工矿位姿融合方法和系统 | |
ES2651369T3 (es) | Sistema y proceso para medición y evaluación de datos aéreos e inerciales | |
CN110926483B (zh) | 一种用于自动驾驶的低成本传感器组合定位系统及方法 | |
CN104501817A (zh) | 一种基于误差消除的车载导航系统 | |
Emran et al. | A cascaded approach for quadrotor's attitude estimation | |
Oonk et al. | Extended kalman filter for improved navigation with fault awareness | |
CN112629521A (zh) | 一种旋翼飞行器双冗余组合的导航系统建模方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20160518 Termination date: 20200712 |