多体机械系统的动态定位方法与流程

本发明涉及一种动态定位方法，尤其是多体机械系统的动态定位方法，属于多体机械系统动力学领域。

背景技术：

多体机械系统都可看成包含不同数量的体和用于连接各体的不同类型的关节组成的系统，各体表面兴趣点的动态定位对刚体质心位置及姿态角的求解以及对机械系统的模型验证非常必要。

目前，多体机械系统兴趣点动态三维位置的测量手段有双目视觉定位和惯导系统定位，双目图像测量具有测量精度高、非接触测量、同时实现多点测量等优点，但运动的机械系统容易导致标记点遮挡及测量视场小等缺点，这些情况出现时则双目定位方法失效，而惯性导航系统(ins)体积小安装方便，通过测量点的加速度和角速度来定位，定位不受遮挡影响，但存在惯性传感器零偏引起的随时间增长的加速度漂移，这种漂移一般随时间增长而增大因而不宜长时间依赖其定位。

技术实现要素：

本发明的目的是为了解决上述现有技术的缺陷，提供了一种多体机械系统的动态定位方法，该方法实现了多体机械系统点的连续三维定位，且定位精度不受短时间遮挡的影响，这为刚体的质心位置测量和姿态角测量以及模型验证提供了基础。

本发明的目的可以通过采取如下技术方案达到：

多体机械系统的动态定位方法，所述方法假定多体机械系统兴趣点在x轴、y轴和z轴三个方向上是相互独立的，构建每个方向上解耦独立运行的卡尔曼滤波器，利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的位置估计。

进一步的，所述构建每个方向上解耦独立运行的卡尔曼滤波器，具体包括：

构建第一卡尔曼滤波器的过程模型和测量模型，所述测量模型为图像传感器与惯性导航传感器的输出结果之差，第一卡尔曼滤波器实时估计惯性导航传感器加速度零偏；

构建第二卡尔曼滤波器的过程模型和测量模型，所述测量模型包括图像传感器模型和惯性导航传感器模型，在图像传感器有效时，第二卡尔曼滤波器按照图像传感器模型的滤波结果输出，否则，第二卡尔曼滤波器按照惯性导航传感器模型的滤波结果输出。

进一步的，所述第一卡尔曼滤波器的过程模型由0阶的状态变量b的多项式表示；其中，b表示惯性导航传感器加速度零偏。

进一步的，所述第二卡尔曼滤波器的过程模型由2阶的三个状态变量的多项式表示，如下：

其中，x表示位移，表示速度，表示加速度；us表示过程模型不准确度的过程噪音，其均值为0，均方差为φs。

进一步的，所述图像传感器模型和惯性导航传感器模型，分别如下式：

zimg＝x+vimg

zins＝x+0.5bt²

其中，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+vimg

zins＝x+0.5bt²

其中，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中，根据耦合运行的兴趣点位置卡尔曼滤波算法，利用图像传感器和惯性导航传感器的融合实现多体机械系统兴趣点的最佳位置估计，具体包括：

s31、确定参数gk、hk、qk、rk以及初值x0、p0。

根据卡尔曼滤波算法的方程diff(x)＝f*x+g*u+w以及z＝h*x+v，推导得到下式：

zk＝hk*wk+vk

上述两式中，xk是k时刻的系统输出值，uk是k时刻对系统的控制量，zk是k时刻的测量值，gk和hk是设定好的系统矩阵，qk是离散的过程噪声矩阵，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进行了短时间遮挡，所测量的两个兴趣点的连续三维位置是按照两种传感器(高速相机和惯性导航传感器)选择性输出的。

综上所述，本发明实现了多体机械系统点的连续三维定位，且定位精度不受短时间遮挡的影响，这为刚体的质心位置测量和姿态角测量以及模型验证提供了基础。

以上所述，仅为本发明专利较佳的实施例，但本发明专利的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明专利所公开的范围内，根据本发明专利的技术方案及其发明构思加以等同替换或改变，都属于本发明专利的保护范围。