[发明专利]AGV组合导航的多传感器融合定位方法

权利要求书

1.一种AGV组合导航的多传感器融合定位方法，其特征在于，所述方法包括：

确定坐标：获取AGV机器人的载体坐标系b与地理坐标系n；

数据处理：采用四元数微分方程进行姿态解算；

获取数据：通过陀螺仪、加速度计和磁力计分别获取AGV机器人的角速度、运行的加速度和磁场强度，所述陀螺仪采用三个独立的MEMS检测X、Y、Z轴的电容变化，输出与角速度变化相关的电压值；

滤波去噪：采用互补滤波算法，通过加速度计和磁力计测量值修正四元数来修正姿态，即通过陀螺仪、加速度计和磁力计测得的值对俯仰角、翻滚角、航向角和四元数初始化，计算公式如下：

=arctan

=arcsin

=arctan

=cos(0.5*)*cos(0.5*)*cos(0.5*)-

sin(0.5*) * sin(0.5*) * sin(0.5*)

= cos(0.5*)* sin(0.5*)*cos(0.5*)-

sin(0.5*) * cos(0.5*) * sin(0.5*)

= sin(0.5*)*cos(0.5*)*cos(0.5*)+

cos(0.5*)* sin(0.5*) * sin(0.5*)

=cos(0.5*)*cos(0.5*)* sin(0.5*)+

sin(0.5*) * sin(0.5*) * cos(0.5*)

其中，Roll₀为翻滚角、Pitch₀为俯仰角、Yaw₀为航向角，ax，ay，az为加速度计测得的轴加速度值，mx，my，mz为陀螺仪测得的三轴磁感应强度值。

2.如权利要求1所述的AGV组合导航的多传感器融合定位方法，其特征在于，所述数据处理步骤中，采用数值积分中的四阶龙格库塔算法；微分方程如下：

其中Q(t)为姿态四元数，为载体坐标系b相对地理坐标系n的角速度，T是采样周期，t是某一时刻，四元数解为：

=[]

=()

=()

=()

由求解的四元数表示地理坐标系n到载体坐标系b之间的姿态矩阵为：

C=

根据以 上求解的四元数数值和姿态矩阵计算载体的姿态角为：

Pitch=arcsin , Pitch∈(-)

Roll=arctan , Roll∈(-)

Yaw= arctan , Yaw∈(-)。

3.如权利要求1或2所述的AGV组合导航的多传感器融合定位方法，其特征在于，所述方法还包括双轮差速驱动AGV运动模型算法，包括：

获取AGV形心位置P、航向角Ɵ、两驱动轮轴间间距d，驱动轮半径r，轴间连线中点M，坐标为(,)， P与M距离l，直线PM与AGV中轴线夹角β，两轮旋转角速度为和；

运动学运算：通过左右两轮角速度和车轮半径推出M点速度和角速度；

运动模型建立。

4.如权利要求3所述的AGV组合导航的多传感器融合定位方法，其特征在于，所述运动学运算步骤中，计算公式为：

=(r+ r)cosθ

=(r+ r)sinθ

=

P点的位置坐标可由其与点位置关系推出，计算公式为：

=+lcos(θ+β)

=+lsin(θ+β)

P点的位置方程对时间求导得到P点的速度，计算公式为：

=+ lsin(θ+β)

=+ lcos(θ+β)。

5.如权利要求4所述的AGV组合导航的多传感器融合定位方法，其特征在于，所述运动模型建立步骤中，其运动模型公式：

=。

6.如权利要求1或2任一项所述的AGV组合导航的多传感器融合定位方法，其特征在于，所述方法还包括辅助定位方法，包括：

向地理坐标系n铺设校正标识；

扫描经过的每一个校正标识；

更新对应标识数据，获取距离偏差e并清除。