CN108717298B - 多体机械系统的动态定位方法 - Google Patents
多体机械系统的动态定位方法 Download PDFInfo
- Publication number
- CN108717298B CN108717298B CN201810376805.4A CN201810376805A CN108717298B CN 108717298 B CN108717298 B CN 108717298B CN 201810376805 A CN201810376805 A CN 201810376805A CN 108717298 B CN108717298 B CN 108717298B
- Authority
- CN
- China
- Prior art keywords
- mechanical system
- inertial navigation
- body mechanical
- model
- kalman filter
- 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
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/10—Simultaneous control of position or course in three dimensions
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种多体机械系统的动态定位方法,所述方法假定多体机械系统兴趣点在X轴、Y轴和Z轴三个方向上是相互独立的,构建每个方向上解耦独立运行的卡尔曼滤波器,利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的位置估计。本发明实现了多体机械系统点的连续三维定位,且定位精度不受短时间遮挡的影响,这为刚体的质心位置测量和姿态角测量以及模型验证提供了基础。
Description
技术领域
本发明涉及一种动态定位方法,尤其是多体机械系统的动态定位方法,属于多体机械系统动力学领域。
背景技术
多体机械系统都可看成包含不同数量的体和用于连接各体的不同类型的关节组成的系统,各体表面兴趣点的动态定位对刚体质心位置及姿态角的求解以及对机械系统的模型验证非常必要。
目前,多体机械系统兴趣点动态三维位置的测量手段有双目视觉定位和惯导系统定位,双目图像测量具有测量精度高、非接触测量、同时实现多点测量等优点,但运动的机械系统容易导致标记点遮挡及测量视场小等缺点,这些情况出现时则双目定位方法失效,而惯性导航系统(INS)体积小安装方便,通过测量点的加速度和角速度来定位,定位不受遮挡影响,但存在惯性传感器零偏引起的随时间增长的加速度漂移,这种漂移一般随时间增长而增大因而不宜长时间依赖其定位。
发明内容
本发明的目的是为了解决上述现有技术的缺陷,提供了一种多体机械系统的动态定位方法,该方法实现了多体机械系统点的连续三维定位,且定位精度不受短时间遮挡的影响,这为刚体的质心位置测量和姿态角测量以及模型验证提供了基础。
本发明的目的可以通过采取如下技术方案达到:
多体机械系统的动态定位方法,所述方法假定多体机械系统兴趣点在X轴、Y轴和Z轴三个方向上是相互独立的,构建每个方向上解耦独立运行的卡尔曼滤波器,利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的位置估计。
进一步的,所述构建每个方向上解耦独立运行的卡尔曼滤波器,具体包括:
构建第一卡尔曼滤波器的过程模型和测量模型,所述测量模型为图像传感器与惯性导航传感器的输出结果之差,第一卡尔曼滤波器实时估计惯性导航传感器加速度零偏;
构建第二卡尔曼滤波器的过程模型和测量模型,所述测量模型包括图像传感器模型和惯性导航传感器模型,在图像传感器有效时,第二卡尔曼滤波器按照图像传感器模型的滤波结果输出,否则,第二卡尔曼滤波器按照惯性导航传感器模型的滤波结果输出。
进一步的,所述第一卡尔曼滤波器的过程模型由0阶的状态变量b的多项式表示;其中,b表示惯性导航传感器加速度零偏。
进一步的,所述第二卡尔曼滤波器的过程模型由2阶的三个状态变量的多项式表示,如下:
进一步的,所述图像传感器模型和惯性导航传感器模型,分别如下式:
ZIMG=x+VIMG
ZINS=x+0.5bt2
其中,ZIMG为图像传感器的测量结果;ZINS为惯性导航传感器的测量结果;VIMG为图像测量的噪音,均值为0,协方差矩阵为R;b为惯性导航传感器加速度零偏;t为时间。
进一步的,所述图像传感器有效是指图像传感器所采集的兴趣点未被遮挡。
进一步的,所述利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的位置估计,具体包括:
根据惯性导航传感器测量原理和惯性导航传感器模型,求解惯导定位测量模型的误差协方差矩阵;
根据误差协方差矩阵,判断多体机械系统兴趣点在三个方向位置变量之间的相关性;
当多体机械系统兴趣点在三个方向位置变量之间不相关时,采用每个方向上构建的解耦独立运行的卡尔曼滤波器,利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的位置估计;
当多体机械系统兴趣点在三个方向位置变量之间相关时,建立两个同时运行的多状态变量的卡尔曼滤波器,根据耦合运行的兴趣点位置卡尔曼滤波算法,利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的最佳位置估计。
进一步的,所述相关性用变量间的相关系数r来衡量,若r=0,则变量不相关;若0<r≦1,则变量间相关,变量间的相关系数r可表示如下:
其中,cov(X,Y)为变量X与Y的协方差,Var[X]为变量X的方差,Var[Y]为变量Y的方差。
进一步的,所述多状态变量为包括X轴、Y轴和Z轴三个方向上的位移、速度、加速度共九个状态变量的列向量,以及三个方向上的加速度计与陀螺仪的零偏共六个状态变量的列向量。
进一步的,所述根据耦合运行的兴趣点位置卡尔曼滤波算法,利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的最佳位置估计,具体为:
确定参数Gk、Hk、Qk、Rk以及初值X0、P0;然后确定卡尔曼滤波器的循环体,利用Riccati方程求解Mk、Kk、Pk,其中Mk是k时刻的误差协方差矩阵,Kk是k时刻的增益,Pk是k时刻的估计误差协方差矩阵;求取理论输出值Xk与理论测量值Zk,读取图像传感器和惯性导航传感器融合的输入值Z,求取余值RES=Z-Zk,最后求解卡尔曼滤波器输出的最佳估计值X与误差协方差矩阵R。
本发明相对于现有技术具有如下的有益效果:
1、本发明通过卡尔曼滤波器对图像传感器和惯性导航传感器(包括三个加速度计和三个陀螺仪)的信息融合,实现了多体机械系统兴趣点的连续三维定位,在高速相机定位失效三秒时,该套测量系统仍保证定位精度1~2cm。
2、本发明可实现多体机械系统中五点以上的连续三维定位,利用这些点可以求解任意体的质心位置及姿态角,为模型验证及系统优化设计提供了必要的手段。
3、本发明避免了利用高速相机单独定位时,双目图像中兴趣点匹配易受视场限制而被迫增加高速相机数量的情况发生,大大的降低了成本,处理也更加方便。
附图说明
图1为本发明实施例1的动态定位方法流程图。
图2为本发明实施例1的卡尔曼滤波算法基本流程图。
图3为本发明实施例2的水田激光平地机结构示意图。
图4为本发明实施例2的对水田激光平地机平地铲上兴趣点的定位布置图。
其中,1-激光接收器,2-安装台,3-高程油缸,4-调平油缸,5-平地铲,6-牵引车,7-上平行连杆,8-下平行连杆,9-高速相机,10-惯性导航传感器,11-第一兴趣点,12-第二兴趣点。
具体实施方式
下面结合实施例及附图对本发明作进一步详细的描述,但本发明的实施方式不限于此。
实施例1:
如图1所示,本实施例提供了一种多体机械系统的动态定位方法,该方法包括以下步骤:
S1、假定多体机械系统兴趣点在X轴、Y轴和Z轴三个方向上是相互独立的,构建每个方向上解耦独立运行的卡尔曼滤波器。
构建每个方向上解耦独立运行的卡尔曼(Kalman)滤波器,具体包括:
S11、构建第一卡尔曼滤波器的过程模型和测量模型,所述测量模型为图像传感器与惯性导航传感器的输出结果之差,第一卡尔曼滤波器实时估计惯性导航传感器加速度零偏。
在步骤S11中,第一卡尔曼滤波器的过程模型由0阶的状态变量b的多项式表示;其中,b表示惯性导航传感器加速度零偏。
S12、构建第二卡尔曼滤波器的过程模型和测量模型,所述测量模型包括图像传感器模型和惯性导航传感器模型,在图像传感器有效时,第二卡尔曼滤波器按照图像传感器模型的滤波结果输出,否则,第二卡尔曼滤波器按照惯性导航传感器模型的滤波结果输出。
在步骤S12中,第二卡尔曼滤波器的过程模型由2阶的三个状态变量的多项式表示,如下:
在步骤S12中,图像传感器有效是指图像传感器所采集的兴趣点未被遮挡。
在步骤S12中,所述图像传感器模型和惯性导航传感器模型,分别如下式:
ZIMG=x+VIMG
ZINS=x+0.5bt2
其中,ZIMG为图像传感器的测量结果;ZINS为惯性导航传感器的测量结果;VIMG为图像测量的噪音,均值为0,协方差矩阵为R;b为惯性导航传感器加速度零偏;t为时间。
S2、根据惯性导航传感器测量原理和惯性导航传感器模型,求解惯导定位测量模型的误差协方差矩阵。协方差矩阵是对称矩阵,而且对角线是各个方向位置变量的方差,我们用变量X、Y、Z来表示三个方向的位置变量,则协方差矩阵可表示如下:
其中,R误差协方差矩阵,cov是matlab中用于求解变量间的协方差函数。
S3、根据误差协方差矩阵,判断多体机械系统兴趣点在三个方向位置变量之间的相关性;相关性用变量间的相关系数r来衡量,若r=0,则变量不相关,进入步骤S4;若0<r≦1,则变量间相关,进入步骤S5,变量间的相关系数r可表示如下:
其中,cov(X,Y)为变量X与Y的协方差,Var[X]为变量X的方差,Var[Y]为变量Y的方差。
S4、采用每个方向上构建的解耦独立运行的卡尔曼滤波器,利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的位置估计。
S5、建立两个同时运行的多状态变量的卡尔曼滤波器,根据耦合运行的兴趣点位置卡尔曼滤波算法,利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的最佳位置估计。
在步骤S5中,多状态变量为包括X轴、Y轴和Z轴三个方向上的位移、速度、加速度共九个状态变量的列向量,以及三个方向上的加速度计与陀螺仪的零偏共六个状态变量的列向量。
耦合运行的兴趣点位置卡尔曼滤波算法通过编写得到,在步骤S5中,根据耦合运行的兴趣点位置卡尔曼滤波算法,利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的最佳位置估计,具体包括:
根据卡尔曼滤波算法的方程Dif f(x)=F*X+G*u+W以及Z=H*X+v,推导得到下式:
Zk=Hk*Wk+vk
S32、确定卡尔曼滤波器的循环体,利用Riccati(里卡蒂)方程求解Mk、Kk、Pk,其中Mk是k时刻的误差协方差矩阵,Kk是k时刻的增益,Pk是k时刻的估计误差协方差矩阵;求取理论输出值Xk与理论测量值Zk,读取图像传感器和惯性导航传感器融合的输入值Z,求取余值RES=Z-Zk,最后求解卡尔曼滤波器输出的最佳估计值X与误差协方差矩阵R。
实施例2:
如图3所示,本实施例的多体机械系统为水田激光平地机,水田激光平地机包括三根平行连杆、激光接收器1、安装台2、高程油缸3、调平油缸4、平地铲5以及牵引车6,激光接收器1安装在安装台2上,三根平行连杆包括一根上平行连杆7和两根下平行连杆8,三根平行连杆的一端与牵引车6通过旋转副连接,另一端与安装台2通过旋转副连接,高程油缸3的一端与牵引车6铰接,另一端与上平行连杆7铰接,平地铲5与安装台2通过旋转副连接,调平油缸4的一端与安装台铰接,另一端与平地铲5铰接,其中通过高程油缸3的伸缩运动来控制平地铲5的升降,这里采用了机械原理中的平行四连杆机构,另外调平油缸4的伸缩运动控制平地铲5的左右倾斜,由于在水田激光平地机在调平过程中平行连杆的变形量较大,所以平地铲5的运动较为复杂。
如图4所示,利用高速相机9与捷联惯性导航传感器10的融合实现平地铲调平过程中平地铲上兴趣点的连续三维定位,其中在所测量的第一兴趣点11和第二兴趣点12处粘贴宝马标志点,用于高速相机9对兴趣点的实时捕捉;将两台高速相机9(构成双目相机)及两台新闻灯摆放到平地铲5的后面,调整高速相机及新闻灯的位置,使两台高速相机9能同时清晰的拍摄到两个兴趣点的画面,高速相机9在拍摄前经过了专用标定棒的标定,实现了两台高速相机9对平地铲上两个兴趣点在三个方向上的位移测量;将惯性测量单元1和2安装到两个兴趣点处,通过对两个惯性导航传感器(三个加速度计和三轴陀螺仪)10输出的加速度和角速度进行二次积分及坐标转换,实现两个兴趣点在三个方向上的位置测量,通过启动牵引车6为调平油缸4不断的提供油压,使平地铲5绕安装台左右转动,实现平地铲5的调平动作。
将上述两种传感器(高速相机和惯性导航传感器)得到的兴趣点在三个方向的测量值作为第二卡尔曼滤波器中测量模型的ZIMG和ZINS,三个方向的测量值ZIMG和ZINS之差作为卡尔曼滤波器2中测量模型的零偏b,第一卡尔曼滤波器对零偏b进行实时估计,将b的估计值实时提供给第二卡尔曼滤波器,第二卡尔曼滤波器实现两个兴趣点在三个方向上位置的实时估计,它选择性地输出位移的估计值,即当高速相机9无遮挡时,按照高速相机9的测量值输出,当高速相机9短时间遮挡时,利用上一时刻零偏b的估计值以及惯性导航传感器10此刻的测量值ZINS实现兴趣点的位置估计。
在这里,进行了两次测量,第一次测量未对高速相机进行遮挡操作,所测得的两个兴趣点的连续三维位置是按照高速相机9的测量值输出的;第二次测量在某一时刻对高速相机9进行了短时间遮挡,所测量的两个兴趣点的连续三维位置是按照两种传感器(高速相机和惯性导航传感器)选择性输出的。
综上所述,本发明实现了多体机械系统点的连续三维定位,且定位精度不受短时间遮挡的影响,这为刚体的质心位置测量和姿态角测量以及模型验证提供了基础。
以上所述,仅为本发明专利较佳的实施例,但本发明专利的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明专利所公开的范围内,根据本发明专利的技术方案及其发明构思加以等同替换或改变,都属于本发明专利的保护范围。
Claims (8)
1.多体机械系统的动态定位方法,其特征在于:所述方法假定多体机械系统兴趣点在X轴、Y轴和Z轴三个方向上是相互独立的,构建每个方向上解耦独立运行的卡尔曼滤波器,利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的位置估计;
所述构建每个方向上解耦独立运行的卡尔曼滤波器,具体包括:
构建第一卡尔曼滤波器的过程模型和测量模型,所述第一卡尔曼滤波器的测量模型为图像传感器与惯性导航传感器的输出结果之差,第一卡尔曼滤波器实时估计惯性导航传感器加速度零偏;
构建第二卡尔曼滤波器的过程模型和测量模型,所述第二卡尔曼滤波器的测量模型包括图像传感器模型和惯性导航传感器模型,在图像传感器有效时,第二卡尔曼滤波器按照图像传感器模型的滤波结果输出,否则,第二卡尔曼滤波器按照惯性导航传感器模型的滤波结果输出。
2.根据权利要求1所述的多体机械系统的动态定位方法,其特征在于:所述第一卡尔曼滤波器的过程模型由0阶的状态变量b的多项式表示;其中,b表示惯性导航传感器加速度零偏。
4.根据权利要求1所述的多体机械系统的动态定位方法,其特征在于:所述图像传感器模型和惯性导航传感器模型,分别如下式:
ZIMG=x+VIMG
ZINS=x+0.5bt2
其中,x表示位移,ZIMG为图像传感器的测量结果;ZINS为惯性导航传感器的测量结果;VIMG为图像测量的噪音,均值为0,协方差矩阵为R;b为惯性导航传感器加速度零偏;t为时间。
5.根据权利要求1所述的多体机械系统的动态定位方法,其特征在于:所述图像传感器有效是指图像传感器所采集的兴趣点未被遮挡。
6.根据权利要求1所述的多体机械系统的动态定位方法,其特征在于:所述利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的位置估计,具体包括:
根据惯性导航传感器测量原理和惯性导航传感器模型,求解惯性导航传感器模型的误差协方差矩阵;
根据误差协方差矩阵,判断多体机械系统兴趣点在三个方向位置变量之间的相关性;
当多体机械系统兴趣点在三个方向位置变量之间不相关时,采用每个方向上构建的解耦独立运行的卡尔曼滤波器,利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的位置估计;
当多体机械系统兴趣点在三个方向位置变量之间相关时,建立两个同时运行的多状态变量的卡尔曼滤波器,根据耦合运行的卡尔曼滤波算法,利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的最佳位置估计。
8.根据权利要求6所述的多体机械系统的动态定位方法,其特征在于:所述多状态变量为包括X轴、Y轴和Z轴三个方向上的位移、速度、加速度共九个状态变量的列向量,以及三个方向上的加速度计与陀螺仪的零偏共六个状态变量的列向量。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810376805.4A CN108717298B (zh) | 2018-04-25 | 2018-04-25 | 多体机械系统的动态定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810376805.4A CN108717298B (zh) | 2018-04-25 | 2018-04-25 | 多体机械系统的动态定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108717298A CN108717298A (zh) | 2018-10-30 |
CN108717298B true CN108717298B (zh) | 2020-10-20 |
Family
ID=63899125
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810376805.4A Active CN108717298B (zh) | 2018-04-25 | 2018-04-25 | 多体机械系统的动态定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108717298B (zh) |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102538781A (zh) * | 2011-12-14 | 2012-07-04 | 浙江大学 | 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 |
-
2018
- 2018-04-25 CN CN201810376805.4A patent/CN108717298B/zh active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102538781A (zh) * | 2011-12-14 | 2012-07-04 | 浙江大学 | 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 |
Non-Patent Citations (3)
Title |
---|
基于多项式卡尔曼滤波器的车辆定位试验;赵汝祺等;《农业工程学报》;20120930;第28卷(第18期);全文 * |
多传感器融合的车辆自主定位试验;张胜宾等;《 Mechanical & Electrical Engineering Technology》;20160831;第45卷(第8期);全文 * |
融合视觉与惯性导航的机器人自主定位;路丹晖;《中国优秀硕士学位论文全文数据库(电子期刊)信息科技辑》;20120715(第7期);摘要 * |
Also Published As
Publication number | Publication date |
---|---|
CN108717298A (zh) | 2018-10-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
WO2020087846A1 (zh) | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 | |
CN109991636A (zh) | 基于gps、imu以及双目视觉的地图构建方法及系统 | |
CN110487267B (zh) | 一种基于vio&uwb松组合的无人机导航系统及方法 | |
CN107607113B (zh) | 一种两轴姿态倾角测量方法 | |
CN109269512B (zh) | 行星着陆图像与测距融合的相对导航方法 | |
CN111156998A (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
CN109141433A (zh) | 一种机器人室内定位系统及定位方法 | |
CN108225258A (zh) | 基于惯性单元和激光跟踪仪动态位姿测量装置和方法 | |
CN109520476B (zh) | 基于惯性测量单元的后方交会动态位姿测量系统及方法 | |
CN105606127A (zh) | 一种双目立体相机与惯性测量单元相对姿态标定方法 | |
US9470507B2 (en) | Vehicle wheel alignment method and system based on gyroscopic sensors or angular rate sensors or MEMS angular rate sensors | |
CN110411457B (zh) | 基于行程感知与视觉融合的定位方法、系统、终端和存储介质 | |
KR101080078B1 (ko) | 통합센서 기반의 모션 캡쳐 시스템 | |
CN107621266B (zh) | 基于特征点跟踪的空间非合作目标相对导航方法 | |
CN109631876B (zh) | 一种基于单相机导航影像的巡视探测器定位方法 | |
CN208751577U (zh) | 一种机器人室内定位系统 | |
KR100898169B1 (ko) | 관성항법시스템의 초기정렬 방법 | |
CN114529576A (zh) | 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法 | |
CN111025330B (zh) | 一种基于深度图的目标倾斜角检测方法及设备 | |
CN104546391B (zh) | 一种用于盲杖的陀螺稳定装置及其互补滤波方法 | |
CN106197376B (zh) | 基于单轴mems惯性传感器的车身倾角测量方法 | |
CN108444468A (zh) | 一种融合下视视觉与惯导信息的定向罗盘 | |
CN112862818B (zh) | 惯性传感器和多鱼眼相机联合的地下停车场车辆定位方法 | |
CN108717298B (zh) | 多体机械系统的动态定位方法 | |
CN112113564B (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 |