CN107490373A - 基于编码器和惯性元器件融合的设备位姿估计方法及系统 - Google Patents
基于编码器和惯性元器件融合的设备位姿估计方法及系统 Download PDFInfo
- Publication number
- CN107490373A CN107490373A CN201710740505.5A CN201710740505A CN107490373A CN 107490373 A CN107490373 A CN 107490373A CN 201710740505 A CN201710740505 A CN 201710740505A CN 107490373 A CN107490373 A CN 107490373A
- Authority
- CN
- China
- Prior art keywords
- encoder
- microcontroller
- robot
- inertia component
- covariance
- 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.)
- Pending
Links
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种基于编码器和惯性元器件融合的设备位姿估计方法及其系统。所述位姿估计方法包括：定义坐标系与初始状态量；编码器与惯性元器件数据采集；根据对机器人的输入控制量，通过状态方程预测状态向量；预测协方差；根据所述预测的协方差，计算卡尔曼滤波增益；其中，所述编码器的测量方差根据两个编码器的脉冲计数之间的差值进行调整；根据测量向量以及卡尔曼滤波增益，更新所述机器人的状态向量以及协方差。该方法在进行位姿估计时，根据机器人的运动情况不断调整计算模型中的观测噪声方差，从而使计算结果能够更加接近真实值。
Description
技术领域
本发明涉及一种机器人或移动设备位姿估计方法，具体涉及一种基于编码器和惯性元器件融合的设备位姿估计方法及系统。
背景技术
位姿是位置和姿态的统称。为了实现机器人或移动设备的移动导航或者自动化控制等，需要对轮式机器人或移动设备的位置和姿态进行估计，为下一步的机器人或移动设备控制提供参考依据。
现有的估计方法通常是在主动轮上加装编码器读取转速信息，然后再根据测程法计算历程信息，从而得到机器人或移动设备的位置与姿态。
当然，还有一些技术方案中，在机器人中使用惯性导航单元对机器人的位置和姿态进行估计。
对于第一种方案，由于编码器的精度问题、轮子与地面的细小打滑等因素的影响，使用基于编码器获得的历程信息会逐渐累积误差，导致位姿估计结果的偏差越来越大，最终获得的姿态估计不准确。
对于第二种方案，由于惯性元件存在零漂和温漂，长时间积分后会产生累积误差，尤其是对位置的估计偏差较大。因此，实际可实用的都是价格昂贵的高精度惯性导航单元，成本较高。
因此，现有技术还有待发展。
发明内容
鉴于上述现有技术的不足之处，本发明的目的在于提供一种机器人或移动设备位姿估计方法及系统，旨在解决现有技术中位姿估计方法实现成本和精度之间存在矛盾，无法同时兼顾的问题。
为了达到上述目的，本发明采取了以下技术方案：
一种基于编码器和惯性元器件融合的机器人或移动设备位姿估计系统，其中，系统组成包括：微控制器、编码器、惯性元器件以及晶振时钟单元；
所述微控制器设置有两个编码器接口以及至少一个通讯接口；所述编码器接口与编码器连接，接收编码器的脉冲计数；所述通讯接口(如USB接口)用于输出位姿估计的运算结果。
所述编码器为正交编码器，两编码器分别加装在轮式机器人或移动设备的两车轮上，并通过编码器接口分别与微控制器连接，用于测量车轮位置与速度。
所述惯性元器件通过通讯接口(如SPI接口)与所述微控制器连接并进行数据传输；
所述晶振时钟单元用于向所述微控制器提供时钟信号；
所述的系统，其中，所述微控制器通过差分接收所述编码器的脉冲计数。
所述的系统，其中，所述微控制器通过所述通讯接口(如SPI接口)采集所述惯性元器件采集的数据，并记录编码器的脉冲计数；
所述微控制器根据所述时钟信号为所述惯性元器件采集的数据以及对应的脉冲计数设置时间戳。
所述的系统，其中，所述晶振时钟单元包括：稳步晶振以及有源激励电路；
所述有源激励电路输出时钟信号至所述微控制器。
一种应用如上所述的系统的位姿估计方法，其中，包括：
定义坐标系与初始状态量；
编码器与惯性元器件数据采集；
状态向量预测；
协方差预测；
计算卡尔曼滤波增益；
状态向量及协方差更新。
根据所述预测的协方差，计算卡尔曼滤波增益；其中，所述编码器的测量方差根据两个编码器的脉冲计数之间的差值进行调整；
根据测量向量以及卡尔曼滤波增益，更新所述机器人的状态向量以及协方差。
所述的方法，其中，所述预定的参数包括：机器人的结构参数、编码器的特性参数、惯性元器件的采样频率、计算模型以及一对编码器之间的脉冲计数差值的第一、第二以及第三阈值。
所述的方法，其中，所述编码器的测量方差根据两个编码器的脉冲计数之间的差值进行调整，具体包括：令|ηL-ηR|表示编码器之间的脉冲计数差值；Md1、Md2、Md3……Mdn(n＝1、2、3……)分别为第一、第二、第三……第n(n＝1、2、3……)阈值；
当|ηL-ηR|∈Md1时，所述编码器的测量方差为第一预设值；
当|ηL-ηR|∈Md2时，所述编码器的测量方差为第二预设值；
当|ηL-ηR|∈Md3时，所述编码器的测量方差为第三预设值；
……
当|ηL-ηR|∈Mdn时，所述编码器的测量方差为第n预设值。
有益效果：本发明提供的一种基于编码器和惯性元器件融合的机器人或移动设备的位姿估计方法。在进行位姿估计时，根据机器人的运动情况不断调整计算模型中的观测噪声方差，从而使计算结果能够更加接近真实值。另外，通过设置晶振时钟单元，令惯性元件与编码器的数据实现时间同步，使其更加接近真实值，实现成本低。
附图说明
图1为本发明具体实施例的机器人的位姿估计系统的结构框图。
图2为本发明具体实施例的机器人的位姿估计方法的参数计算流程图。
图3为本发明具体实施例的机器人的位姿估计方法的方法流程图。
具体实施方式
本发明提供一种机器人位姿估计系统及其位姿估计方法及其系统。为使本发明的目的、技术方案及效果更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
如图1所示，为本发明具体实施例的一种机器人位姿估计系统。所述机器人位姿估计系统可以包括：微控制器101、编码器(包括两个编码器1021、1022)、惯性元器件103、晶振时钟单元104以及接口(包括两路编码器接口1051、1052以及通讯接口1053)。
所述惯性元器件103与所述微控制器101连接，所述晶振时钟单元用于向所述微控制器101提供时钟信号。
如图1所示，所述微控制器101设置有两个编码器接口1051和1052以及至少一个数据输出接口1053。所述编码器接口1051和1052分别与机器人的两个车轮上的编码器连接，接收编码器的脉冲计数。所述通讯接口1053则用于输出位姿估计的运算结果。
具体的，所述微控制器101具体可以采用32位高性能浮点运算处理器，通过微控制器进行数据采集并通过定时器输入捕捉功能对编码器进行计数。
在本实施例中，所述微控制器101通过所述通讯接口(如SPI接口)采集所述惯性元器件采集的数据，并记录编码器的脉冲计数；所述微控制器根据所述时钟信号为所述惯性元器件采集的数据以及对应的脉冲计数设置时间戳。亦即，分别将惯性元器件采集的数据和脉冲计数均打上时间戳，根据所述时间戳实现两个传感器数据的时间同步。
在一些实施例中，所述惯性元器件102具体通过通讯接口(如SPI接口)与所述微控制器101连接。所述微控制器101以差分接收的方式接收所述编码器的脉冲计数。
在另一些实施中，所述晶振时钟单元具体可以为稳步晶振以及有源激励电路组成的晶振时钟单元，通过有源激励电路输出时钟信号至所述微控制器，为微控制器提供稳定的系统时钟。
以下以轮式机器人为例，详细阐述应用如上所述的系统的位姿估计方法。图2为本发明实施例提供的位姿估计方法的参数计算流程图。图3为本发明实施例提供的位姿估计方法的方法示意图。
如图2-3所示，所述方法包括如下步骤，以完成相应的计算流程：
301、定义坐标系及确定状态向量的初始值。
所述状态向量为机器人在三轴惯性坐标系中的位移、速度以及姿态角。其中，定义状态向量X＝[x,y,z,vx,vy,zz,α,β,γ]；其中[x,y,z]为机器人在惯性坐标系X轴、Y轴、Z轴上的位移，[vx,vy,zz]为机器人在惯性坐标系X轴、Y轴、Z轴上的速度，[α,β,γ]为惯性坐标系下的姿态角。初始状态向量为X0。
302、编码器及惯性元器件数据采集。
在实际运动过程中，假设小车行走在平地上，可以通过编码器测量计算车轮速度为vk。Zk为机器人或移动设备在坐标系下位移测量向量，对于机身坐标下沿y轴，z轴运动的速度vy,vz等于零，沿x轴方向的速度vx即为编码器测量的速度Zk＝[vx,vy,vz]T＝[vk,0,0]T。惯性单元获得的三轴角速度w和加速度a作为输入控制量μk。
303、根据对机器人的输入控制量，通过状态方程预测状态向量。亦即，可以根据上一次状态向量Xk-1和当前控制输入量μk计算得到状态方程在第k次采样时的预估值Xk|k-1。
304、预测协方差。
亦即，通过Pk-1(上次状态向量Xk-1的协方差)和Qk(惯性测量单元加速度和角速度噪声方差)计算当前协方差预估值Pk|k-1。
305、根据所述预测的协方差，计算卡尔曼滤波增益。其中，所述编码器的测量方差根据两个编码器的脉冲计数之间的差值进行调整。
具体的，设Rk为观测噪声方差(编码器测量噪声方差)。由上次得到的协方差预估值Pk|k-1结合观测噪声方差Rk计算增益Kk。然后，通过编码器脉冲计数之差|ηL-ηR|与所设定阀度值Mn(n＝1,2,3……)比较来调整噪声方差Rk，进而调节增益Kk。
具体的调整方式为：令|ηL-ηR|表示编码器之间的脉冲计数差值；所选阈值数目根据实验效果确定，假设取n＝3，Md1、Md2、Md3分别为第一、第二以及第三阈值；
当|ηL-ηR|∈Md1时，所述编码器的测量方差为第一预设值；当|ηL-ηR|∈Md2时，所述编码器的测量方差为第二预设值；当|ηL-ηR|∈Md3时，所述编码器的测量方差为第三预设值。
当然，所述第一预设值、第二预设值以及第三预设值、第一阈值、第二阈值以及第三阈值均可以由技术人员根据实验数据计算确定。
306、根据测量向量以及卡尔曼滤波增益，更新所述机器人的状态向量以及协方差。
亦即，分别通过滤波增益Kk以及状态向量预估值Xk|k-1计算状态向量Xk；通过滤波增益Kk以及协方差预估值Pk|k-1计算协方差Pk。
可以理解的是，对本领域普通技术人员来说，可以根据本发明的技术方案及本发明构思加以等同替换或改变，而所有这些改变或替换都应属于本发明所附的权利要求的保护范围。
Claims (7)
1.一种基于编码器和惯性元器件融合的设备位姿估计系统，其特征在于，包括：微控制器、编码器、惯性元器件及晶振时钟单元；
所述微控制器设置有两个编码器接口以及至少一个通讯接口(如USB2.0)；所述编码器接口与编码器连接，接收编码器的脉冲计数；所述通讯接口(如USB2.0)用于输出位姿估计的运算结果；
所述编码器为正交编码器，加装在车轮上，测量车轮位置与速度；
所述惯性元器件通过通讯接口与所述微控制器单元连接；
所述晶振时钟单元用于向所述微控制器提供时钟信号。
2.根据权利要求1所述的系统，其特征在于，所述惯性元器件通过通讯接口(如SPI接口)与所述微控制器连接；所述微控制器通过差分接收所述编码器的脉冲计数。
3.根据权利要求2所述的系统，其特征在于，所述微控制器通过所述通讯接口(如SPI接口)采集所述惯性元器件采集的数据，并记录编码器的脉冲计数；
所述微控制器根据所述时钟信号为所述惯性元器件采集的数据以及对应的脉冲计数设置时间戳。
4.根据权利要求1所述的系统，其特征在于，所述晶振时钟单元包括：稳步晶振以及有源激励电路；
所述有源激励电路输出时钟信号至所述微控制器。
5.一种应用如权利要求1-4任一所述的系统的位姿估计方法，其特征在于，包括：
定义坐标系与初始状态量；
编码器与惯性元器件数据采集；
根据对机器人的输入控制量，通过状态方程预测状态向量；
预测协方差；
根据所述预测的协方差，计算卡尔曼滤波增益；其中，所述编码器的测量方差根据两个编码器的脉冲计数之间的差值进行调整；
根据测量向量以及卡尔曼滤波增益，更新所述机器人的状态向量以及协方差。
6.根据权利要求5所述的方法，其特征在于，所述预定的参数包括：机器人的结构参数、编码器的特性参数、惯性元件的采样频率以及一对编码器之间的脉冲计数差值的第一、第二以及第三阈值。
7.根据权利要求6所述的方法，其特征在于，所述编码器的测量方差根据两个编码器的脉冲计数之间的差值进行调整，具体包括：令|ηL-ηR|表示编码器之间的脉冲计数差值；Md1、Md2、Md3……Mdn(n＝1、2、3……)分别为第一、第二、第三……第n(n＝1、2、3……)阈值；
当|ηL-ηR|∈Md1时，所述编码器的测量方差为第一预设值；
当|ηL-ηR|∈Md2时，所述编码器的测量方差为第二预设值；
当|ηL-ηR|∈Md3时，所述编码器的测量方差为第三预设值；
……
当|ηL-ηR|∈Mdn时，所述编码器的测量方差为第n预设值。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710740505.5A CN107490373A (zh) | 2017-08-25 | 2017-08-25 | 基于编码器和惯性元器件融合的设备位姿估计方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710740505.5A CN107490373A (zh) | 2017-08-25 | 2017-08-25 | 基于编码器和惯性元器件融合的设备位姿估计方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107490373A true CN107490373A (zh) | 2017-12-19 |
Family
ID=60645890
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710740505.5A Pending CN107490373A (zh) | 2017-08-25 | 2017-08-25 | 基于编码器和惯性元器件融合的设备位姿估计方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107490373A (zh) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108731664A (zh) * | 2018-05-18 | 2018-11-02 | 深圳清创新科技有限公司 | 机器人状态估计方法、装置、计算机设备和存储介质 |
| CN110986988A (zh) * | 2019-12-20 | 2020-04-10 | 上海有个机器人有限公司 | 融合多传感器数据的轨迹推算方法、介质、终端和装置 |
| CN112697153A (zh) * | 2020-12-31 | 2021-04-23 | 广东美的白色家电技术创新中心有限公司 | 自主移动设备的定位方法、电子设备及存储介质 |
| CN113942008A (zh) * | 2020-07-17 | 2022-01-18 | 精工爱普生株式会社 | 过冲量检测方法及机器人系统 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101590323A (zh) * | 2009-07-08 | 2009-12-02 | 北京工业大学 | 一种独轮机器人系统及其控制方法 |
| CN104235618A (zh) * | 2014-09-04 | 2014-12-24 | 哈尔滨工程大学 | 一种基于mems惯性测量单元的管道测绘及缺陷定位装置及其管道测绘及缺陷定位方法 |
| CN105173940A (zh) * | 2015-07-16 | 2015-12-23 | 中南大学 | 一种基于组合导航技术的超深矿井罐笼位姿测量系统及方法 |
| CN105547288A (zh) * | 2015-12-08 | 2016-05-04 | 华中科技大学 | 一种煤矿井下移动设备自主定位的方法及系统 |
-
2017
- 2017-08-25 CN CN201710740505.5A patent/CN107490373A/zh active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101590323A (zh) * | 2009-07-08 | 2009-12-02 | 北京工业大学 | 一种独轮机器人系统及其控制方法 |
| CN104235618A (zh) * | 2014-09-04 | 2014-12-24 | 哈尔滨工程大学 | 一种基于mems惯性测量单元的管道测绘及缺陷定位装置及其管道测绘及缺陷定位方法 |
| CN105173940A (zh) * | 2015-07-16 | 2015-12-23 | 中南大学 | 一种基于组合导航技术的超深矿井罐笼位姿测量系统及方法 |
| CN105547288A (zh) * | 2015-12-08 | 2016-05-04 | 华中科技大学 | 一种煤矿井下移动设备自主定位的方法及系统 |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108731664A (zh) * | 2018-05-18 | 2018-11-02 | 深圳清创新科技有限公司 | 机器人状态估计方法、装置、计算机设备和存储介质 |
| CN108731664B (zh) * | 2018-05-18 | 2020-08-11 | 深圳一清创新科技有限公司 | 机器人状态估计方法、装置、计算机设备和存储介质 |
| CN110986988A (zh) * | 2019-12-20 | 2020-04-10 | 上海有个机器人有限公司 | 融合多传感器数据的轨迹推算方法、介质、终端和装置 |
| CN110986988B (zh) * | 2019-12-20 | 2023-12-08 | 上海有个机器人有限公司 | 融合多传感器数据的轨迹推算方法、介质、终端和装置 |
| CN113942008A (zh) * | 2020-07-17 | 2022-01-18 | 精工爱普生株式会社 | 过冲量检测方法及机器人系统 |
| CN113942008B (zh) * | 2020-07-17 | 2023-12-22 | 精工爱普生株式会社 | 过冲量检测方法及机器人系统 |
| CN112697153A (zh) * | 2020-12-31 | 2021-04-23 | 广东美的白色家电技术创新中心有限公司 | 自主移动设备的定位方法、电子设备及存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110706279B (zh) | 基于全局地图与多传感器信息融合的全程位姿估计方法 | |
| CN107490373A (zh) | 基于编码器和惯性元器件融合的设备位姿估计方法及系统 | |
| CN105043385B (zh) | 一种行人自主导航定位的自适应卡尔曼滤波方法 | |
| CN102508278B (zh) | 一种基于观测噪声方差阵估计的自适应滤波方法 | |
| CN111721289A (zh) | 车辆定位方法、装置、设备、存储介质及车辆 | |
| CN105865461B (zh) | 一种基于多传感器融合算法的汽车定位系统及方法 | |
| CN105953796A (zh) | 智能手机单目和imu融合的稳定运动跟踪方法和装置 | |
| CN106168485B (zh) | 步行航迹数据推算方法及装置 | |
| CN105136145A (zh) | 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法 | |
| WO2006104552A1 (en) | Method and apparatus for high accuracy relative motion determinatation using inertial sensors | |
| CN104964685A (zh) | 一种手机运动姿态的判定方法 | |
| CN104613965B (zh) | 一种基于双向滤波平滑技术的步进式行人导航方法 | |
| CN103900574A (zh) | 一种基于迭代容积卡尔曼滤波姿态估计方法 | |
| CN103604430A (zh) | 一种基于边缘化ckf重力辅助导航的方法 | |
| US10488432B2 (en) | Systems and methods for compensating for the absence of a sensor measurement in a heading reference system | |
| CN111522034A (zh) | 基于惯性导航的定位方法、设备及装置 | |
| CN108592907A (zh) | 一种基于双向滤波平滑技术的准实时步进式行人导航方法 | |
| Si et al. | High-precision estimation of steering angle of agricultural tractors using GPS and low-accuracy MEMS | |
| CN104597460A (zh) | 一种基于北斗卫星导航接收机的载波跟踪环路晶体振荡器加速度敏感系数标定方法 | |
| CN111190207B (zh) | 基于pstcsdref算法的无人机ins bds组合导航方法 | |
| CN116929335A (zh) | 基于多源信息感知的辐射场地图构建方法 | |
| CN114915913A (zh) | 一种基于滑窗因子图的uwb-imu组合室内定位方法 | |
| Zhang et al. | Mag-ODO: Motion speed estimation for indoor robots based on dual magnetometers | |
| Seyr et al. | Proprioceptive navigation, slip estimation and slip control for autonomous wheeled mobile robots | |
| CN117388830A (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 | ||
| WD01 | Invention patent application deemed withdrawn after publication | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20171219 |