多体机械系统的动态定位方法技术方案

本发明专利技术公开了一种多体机械系统的动态定位方法，所述方法假定多体机械系统兴趣点在X轴、Y轴和Z轴三个方向上是相互独立的，构建每个方向上解耦独立运行的卡尔曼滤波器，利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的位置估计。本发明专利技术实现了多体机械系统点的连续三维定位，且定位精度不受短时间遮挡的影响，这为刚体的质心位置测量和姿态角测量以及模型验证提供了基础。

Dynamic positioning method for multibody mechanical systems

The invention discloses a dynamic positioning method for a multi-body mechanical system. The method assumes that the points of interest of the multi-body mechanical system are independent in three directions of X axis, Y axis and Z axis, constructs a Kalman filter which operates independently in each direction, and realizes multi-body by fusion of image sensor and inertial navigation sensor. Location estimation of interest points in mechanical systems. The method realizes the continuous three-dimensional positioning of points in a multi-body mechanical system, and the positioning accuracy is not affected by short time occlusion, which provides a basis for the position measurement of the center of mass, the attitude angle measurement and the model verification of a rigid body.

【技术实现步骤摘要】
多体机械系统的动态定位方法
本专利技术涉及一种动态定位方法，尤其是多体机械系统的动态定位方法，属于多体机械系统动力学领域。
技术介绍
多体机械系统都可看成包含不同数量的体和用于连接各体的不同类型的关节组成的系统，各体表面兴趣点的动态定位对刚体质心位置及姿态角的求解以及对机械系统的模型验证非常必要。目前，多体机械系统兴趣点动态三维位置的测量手段有双目视觉定位和惯导系统定位，双目图像测量具有测量精度高、非接触测量、同时实现多点测量等优点，但运动的机械系统容易导致标记点遮挡及测量视场小等缺点，这些情况出现时则双目定位方法失效，而惯性导航系统(INS)体积小安装方便，通过测量点的加速度和角速度来定位，定位不受遮挡影响，但存在惯性传感器零偏引起的随时间增长的加速度漂移，这种漂移一般随时间增长而增大因而不宜长时间依赖其定位。
技术实现思路
本专利技术的目的是为了解决上述现有技术的缺陷，提供了一种多体机械系统的动态定位方法，该方法实现了多体机械系统点的连续三维定位，且定位精度不受短时间遮挡的影响，这为刚体的质心位置测量和姿态角测量以及模型验证提供了基础。本专利技术的目的可以通过采取如下技术方案达到：多体机械系统的动态定位方法，所述方法假定多体机械系统兴趣点在X轴、Y轴和Z轴三个方向上是相互独立的，构建每个方向上解耦独立运行的卡尔曼滤波器，利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的位置估计。进一步的，所述构建每个方向上解耦独立运行的卡尔曼滤波器，具体包括：构建第一卡尔曼滤波器的过程模型和测量模型，所述测量模型为图像传感器与惯性导航传感器的输出结果之差，第一卡尔曼滤波器实时估计惯性导航传感器加速度零偏；构建第二卡尔曼滤波器的过程模型和测量模型，所述测量模型包括图像传感器模型和惯性导航传感器模型，在图像传感器有效时，第二卡尔曼滤波器按照图像传感器模型的滤波结果输出，否则，第二卡尔曼滤波器按照惯性导航传感器模型的滤波结果输出。进一步的，所述第一卡尔曼滤波器的过程模型由0阶的状态变量b的多项式表示；其中，b表示惯性导航传感器加速度零偏。进一步的，所述第二卡尔曼滤波器的过程模型由2阶的三个状态变量的多项式表示，如下：其中，x表示位移，表示速度，表示加速度；us表示过程模型不准确度的过程噪音，其均值为0，均方差为Φs。进一步的，所述图像传感器模型和惯性导航传感器模型，分别如下式：ZIMG＝x+VIMGZINS＝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阶的三个状态变量的多项式表示，如下：其中，x表示位移，表示速度，表示加速度；us表示过程模型不准确度的过程噪音，其均值为0，均方差为Φs。在步骤S12中，图像传感器有效是指图像传感器所采集的兴趣点未被遮挡。在步骤S12中，所述图像传感器模型和惯性导航传感器模型，分别如下式：ZIMG＝x+VIMGZINS＝x+0.5bt2其中，ZIMG为图像传感器的测量本文档来自技高网...

【技术保护点】
1.多体机械系统的动态定位方法，其特征在于：所述方法假定多体机械系统兴趣点在X轴、Y轴和Z轴三个方向上是相互独立的，构建每个方向上解耦独立运行的卡尔曼滤波器，利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的位置估计。

【技术特征摘要】
1.多体机械系统的动态定位方法，其特征在于：所述方法假定多体机械系统兴趣点在X轴、Y轴和Z轴三个方向上是相互独立的，构建每个方向上解耦独立运行的卡尔曼滤波器，利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的位置估计。2.根据权利要求1所述的多体机械系统的动态定位方法，其特征在于：所述构建每个方向上解耦独立运行的卡尔曼滤波器，具体包括：构建第一卡尔曼滤波器的过程模型和测量模型，所述测量模型为图像传感器与惯性导航传感器的输出结果之差，第一卡尔曼滤波器实时估计惯性导航传感器加速度零偏；构建第二卡尔曼滤波器的过程模型和测量模型，所述测量模型包括图像传感器模型和惯性导航传感器模型，在图像传感器有效时，第二卡尔曼滤波器按照图像传感器模型的滤波结果输出，否则，第二卡尔曼滤波器按照惯性导航传感器模型的滤波结果输出。3.根据权利要求2所述的多体机械系统的动态定位方法，其特征在于：所述第一卡尔曼滤波器的过程模型由0阶的状态变量b的多项式表示；其中，b表示惯性导航传感器加速度零偏。4.根据权利要求2所述的多体机械系统的动态定位方法，其特征在于：所述第二卡尔曼滤波器的过程模型由2阶的三个状态变量的多项式表示，如下：其中，x表示位移，表示速度，表示加速度；us表示过程模型不准确度的过程噪音，其均值为0，均方差为Φs。5.根据权利要求2所述的多体机械系统的动态定位方法，其特征在于：所述图像传感器模型和惯性导航传感器模型，分别如下式：ZIMG＝x+VIMGZINS＝x+0.5bt2其中，ZIMG为图像传感器的测量结果；ZINS为惯性导航传感器的测量结果；VIMG为图像测量的噪音，均值为0，协方差矩阵为R；b为惯性导航传感器加速度零偏；t为时间。6.根据权利要求2所述的多体机械系统的动态定位方法，其特征在于：所述图像传感器有效是指图像传感器所采集的兴趣点未被遮挡。7.根据权利要求1所述的多体机械系统的动态定位方法，其特征在于：所述利用图像传感器和惯性导航传感器的...

【专利技术属性】
技术研发人员：赵祚喜，冯荣，可欣荣，宋俊文，张丁山，
申请(专利权)人：华南农业大学，
类型：发明
国别省市：广东,44