CN109141410B - Agv组合导航的多传感器融合定位方法 - Google Patents

Abstract

本发明公开了一种AGV组合导航的多传感器融合定位方法，所述方法包括：确定坐标：获取AGV机器人的载体坐标系(b)与地理坐标系(n)；数据处理：采用四元数微分方程进行姿态解算；获取数据：通过陀螺仪、角速度计和磁力计分别获取AGV机器人的角速度、运行的加速度和磁场强度；滤波去噪：采用互补滤波算法，通过加速度计和磁力计测量值修正四元数来修正姿态。本发明提供一种AGV组合导航的多传感器融合定位方法，使线性化程度和精度均有所提高，并且降低了全姿态解算算法的计算量和误差。

Description

AGV组合导航的多传感器融合定位方法

技术领域

本发明涉及自动导引机器人技术领域，尤其涉及一种AGV组合导航的多传感器融合定位方法。

背景技术

AGV是(Automated Guided Vehicle)的缩写，即“自动导引运输车”，是指装备有电磁或光学等自动导引装置，它能够沿规定的导引路径行驶，具有安全保护以及各种移载功能的运输车。AGV组合导航技术一般通过两步定位，第一步为粗定位，采用柔性高、精度低的导引方式初步定位，确定AGV的当前位置；第二步为精定位，通过技术成熟、可靠性髙、成本低的导引方式进行误差矫正，提高AGV的定位精度。组合导航作为导航技术发展的重要方向之一，目前在汽车、航海、航空、航天等领域得到广泛运用。

AGV的导航引导技术的核心在于AGV姿态解算，即是确定载体坐标系与地理坐标系之间的方位关系，目前常用的算法分为欧拉角微分方程法、方向余弦微分方程法和四元数微分方程法。其中，欧拉角法虽然较为简单，但三角运算困难，在俯仰角为90度附近时算法将会出现退化失效现象，不能用于全姿态解算；方向余弦法虽可避免欧拉角法产生的方程退化现象，但需解算九个微分方程，计算量大；四元数姿态解算的线性化程度和精度均较高，并可实现全姿态解算。但是，运用四元数姿态解算法，陀螺仪测量角速度会产生累积误差，容易发散，而加速度计和磁力计不易产生累积误差，但实时精度较差。因此，设计一种线性化程度和精度均较高，并可降低计算量的全姿态解算算法是本领域技术人员迫切需要解决的技术问题。

发明内容

本发明的目的在于提供一种AGV组合导航的多传感器融合定位方法，使线性化程度和精度均有所提高，并且降低了全姿态解算算法的计算量和误差。

本发明公开的AGV组合导航的多传感器融合定位方法所采用的技术方案是:

一种AGV组合导航的多传感器融合定位方法，所述方法包括：

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

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

获取数据：通过陀螺仪、角速度计和磁力计分别获取AGV机器人的角速度、运行的加速度和磁场强度；

滤波去噪：采用互补滤波算法，通过加速度计和磁力计测量值修正四元数来修正姿态。

作为优选方案，所述获取数据步骤中，所述陀螺仪采用三个独立的MEMS检测X、Y、Z轴的电容变化，输出与角速度变化相关的电压值。

作为优选方案，所述滤波去噪步骤中，通过陀螺仪、加速度计和磁力计测得的值对俯仰角、翻滚角、航向角和四元数初始化，计算公式如下：

q_{0_init}＝cos(0.5*roll₀)*cos(0.5*pitch₀)*cos(0.5*yaw₀)-

sin(0.5*roll₀)*sin(0.5*pitch₀)*sin(0.5*yaw₀)

q_{1_init}＝cos(0.5*roll₀)*sin(0.5*pitch₀)*cos(0.5*yaw₀)-

sin(0.5*roll₀)*cos(0.5*pitch₀)*sin(0.5*yaw₀)

q_{2_init}＝sin(0.5*roll₀)*cos(0.5*pitch₀)*cos(0.5*yaw₀)+

cos(0.5*roll₀)*sin(0.5*pitch₀)*sin(0.5*yaw₀)

q_{3_init}＝cos(0.5*roll₀)*cos(0.5*pitch₀)*sin(0.5*yaw₀)+

sin(0.5*roll₀)*sin(0.5*pitch₀)*cos(0.5*yaw₀)

其中，ax，ay，az为加速度计测得的轴加速度值，mx，my，mz为陀螺仪测得的三轴磁感应强度值。

作为优选方案，所述数据处理步骤中，采用数值积分中的四阶龙格库塔算法。微分方程如下：

其中Q(t)为姿态四元数，

为b系相对n系的角速度，四元数解为：

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

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

Pitch＝arcsinC₃₂,Pitch∈(-90°，90°)

Roll∈(-180°，180°)

Yaw∈(-180°，180°)。

作为优选方案，所述方法还包括双轮差速驱动AGV运动模型算法，包括：

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

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

运动模型建立。

作为优选方案，所述运动学运算步骤中，计算公式为：

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

x_P＝x_M+lcos(θ+β)

y_P＝y_M+lsin(θ+β)

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

作为优选方案，所述运动模型建立步骤中，其运动模型公式：

作为优选方案，所述方法还包括辅助定位方法，包括：

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

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

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

作为优选方案，所述铺设的校正标识为二维码。

作为优选方案，所述更新对应标识数据，获取距离偏差e并清除步骤中，AGV的距离偏差e为：

其中:x为每个像素对应的实际距离。当e>o时，车辆在导航线右侧，当e<o时，车辆在导航线左侧，从而校正AGV的位置和航向角。

本发明公开的AGV组合导航的多传感器融合定位方法的有益效果是：通过四元数法进行姿态解算，利用数值积分中的四阶龙格库塔算法，简化全姿态解算算法的计算量；与此同时，采用互补滤波算法通过加速度计和磁力计测量值修正四元数来修正姿态，使线性化程度和精度均有所提高，并且降低了全姿态解算算法误差。

附图说明

图1是本发明AGV组合导航的多传感器融合定位方法的流程图。

图2是本发明AGV机器人信息流转关系图。

图3是本发明AGV机器人无线通讯模块ESP8266。

图4是本发明AGV机器人工业控制器和传感器MPU9255。

图5是本发明AGV组合导航的多传感器融合定位方法互补滤波姿态修正算法流程图。

图6是本发明AGV组合导航的多传感器融合定位方法位姿监测测试结果。

图7是本发明双轮差速驱动AGV运动模型算法。

图8是本发明AGV机器人的运动模型。

图9是本发明辅助定位方法。

图10是本发明二维码图像与参数定义示意图。

具体实施方式

下面结合具体实施例和说明书附图对本发明做进一步阐述和说明:

请参考图1，一种AGV组合导航的多传感器融合定位方法，所述方法包括：

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

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

步骤S103：获取数据：通过陀螺仪、角速度计和磁力计分别获取AGV机器人的角速度、运行的加速度和磁场强度；

步骤S104：滤波去噪：采用互补滤波算法，通过加速度计和磁力计测量值修正四元数来修正姿态。

请参考图2，具体的，本方法主要应用于AGV机器人领域。本实施例中的AGV机器人的通信模块都是通过串口RS232与外界进行通信的，AGV车载***与上位机调度***之间采用无线局域网通信。

由于上位机和下位机之间传输数据量较少，为减少***资源占用，本实施例选择OSI(Open System Interconnection)参考模型中的UDP(User Datagram Protocol)协议进行无线模块传输数据。本实施例硬件选的是目前技术比较成熟的ESP8266串口wifi无线模块，如图3所示。现阶段主要用来测试接收AGV发送的数掘的稳定性，包括AGV的IP、电压、速度、运行状态、位置等相关信息。UDP通讯模块编程包括服务器和客户端两部分组成，主要通过UDP控制块和相关巧用接口函数实现数据传输功能。

为提高姿态测量精度，陀螺仪需要结合加速度计或磁力计进行姿态检测。本实施例采用基于LPC2388设计的工业控制器和IrwenSense公司生产的9轴姿态检测数字传感器MPU925实现AGV捷联惯导技术，如图4所示。

本实施例中，陀螺仪利用科里奥利效应，通过三个独立MEMS检测X、Y、Z轴的电容变化，输出与角速率变化相关的电压；加速度计利用惯性原理，通过测量三轴上电容来检测轴的偏差度；磁力计利用霍尔效应，检测地磁场在X、Y、Z轴的电磁强度。

在步骤S102中，采用数值积分中的四阶龙格库塔算法。微分方程如下：

其中Q(t)为姿态四元数，

为b系相对n系的角速度，四元数解为：

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

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

Pitch＝arcsinC₃₂,Pitch∈(-90°，90°)

Roll∈(-180°，180°)

Yaw∈(-180°，180°)。

由于陀螺仪测量角速度会产生累积误差，容易发散；而加速度计和磁力计不易产生累积误差，但实时精度较差，二者适合进行互补滤波。本实施例中进行姿态修正的原理是：对于确定的向量n，当向量n经过存在误差的旋转矩阵变换后，在另一个坐标系中和理论值产生偏差，通过这个偏差来修正旋转矩阵，而旋转矩阵由四元数组成，因此可以修正四元数，从而修正姿态。

由四元数构成的方向余弦矩阵nCb表示从b系到n的转换矩阵，bCn表示从n系到b的转换矩阵，二者互为转置。加速度计和地磁计是测量工具和载体，本实施例通过这两个器件表征旋转矩阵的误差存在，然后通过互补滤波算法修正误差，即修正四元数，互补滤波修正姿态的具体求解流程如图5所示。

其中，n系中重力的理论输出转换到机体坐标系中，该矢量与测量的值存在误差，通过该差值来修正陀螺；同理磁力计将将测量的磁强用上一次的四元数得到的bCn转换到地理坐标系，xy平面的分量合成到x轴上后作为精确输出再经过nCb转换到机体坐标系中，该矢量与测量的值存在误差，通过该差值来修正磁力计。

在步骤S104中，通过陀螺仪、加速度计和磁力计测得的值对俯仰角、翻滚角、航向角和四元数初始化，计算公式如下：

q_{0_init}＝cos(0.5*roll₀)*cos(0.5*pitch₀)*cos(0.5*yaw₀)-

sin(0.5*roll₀)*sin(0.5*pitch₀)*sin(0.5*yaw₀)

q_{1_init}＝cos(0.5*roll₀)*sin(0.5*pitch₀)*cos(0.5*yaw₀)-

sin(0.5*roll₀)*cos(0.5*pitch₀)*sin(0.5*yaw₀)

q_{2_init}＝sin(0.5*roll₀)*cos(0.5*pitch₀)*cos(0.5*yaw₀)+

cos(0.5*roll₀)*sin(0.5*pitch₀)*sin(0.5*yaw₀)

q_{3_init}＝cos(0.5*roll₀)*cos(0.5*pitch₀)*sin(0.5*yaw₀)+

sin(0.5*roll₀)*sin(0.5*pitch₀)*cos(0.5*yaw₀)

其中，ax，ay，az为加速度计测得的轴加速度值，mx，my，mz为陀螺仪测得的三轴磁感应强度值。

采用MPU9255分别测试水平左右移动和前后俯仰倾斜两种情况，采样频率为125Hz，通过互补滤波融合修正陀螺仪、加速度计和电子罗盘测得的数据，输出航向角(Yaw)、俯仰角巧(Pitch)、翻滚角(Roll)随采样次数的变化，如图6所示。

水平测试时，航向角、俯仰角变化速度较快，翻滚角基本稳定；前后俯仰倾斜测试时，翻滚角、俯仰角变化速度较快，航向角基本稳定。测试结果表明，两种情况下传感器均能较快的跟随位姿变化，姿态稳定时波形上下偏差在1度以内，互补滤波算法的效果显著，能够满足实时监测AGV位姿的要求。

请参考图7，本技术方案还包括双轮差速驱动AGV运动模型算法，包括：

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

具体的，AGV运动模型，P为AGV形心位置，θ为平台航向角，d为两驱动轮轴间间距，r为驱动轮半径，M为轴间连线中必点，坐标为(x_M,y_M)P与M距离为l，直线PM与AGV中轴线夹角为β，两轮旋转角速度为w_L和w_R。为分析AGV运动状态。本实施例中，预先设定：①车体为刚体；②水平面内运动；③左右车轮受力相同，且不变形；④轮面与接触面为点接触；⑤驱动轮尺寸相同。

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

请参考图8，具体的，图中M点速度和角速度可由左右两轮角速度和车轮半径推出，计算公式为：

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

x_P＝x_M+lcos(θ+β)

y_P＝y_M+lsin(θ+β)

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

步骤S203：运动模型建立。

具体的，由以上公式联立得运动模型公式：

由运动学模型公式可知，P与M距离为l和直线PM，AGV中轴线夹角为β和车轮半径(r)已知的情况下，调度***对AGV实时速度(x_P,y_P)与角速度(θ)的控制可通过控制两个驱动轮的角速度(w_L,w_R)实现。

请参考图9，本技术方案还包括辅助定位方法，包括：

步骤S301：向地理坐标系(n)铺设校正标识；

步骤S302：扫描经过的每一个校正标识；

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

具体的，惯性导航单独使用时，随时间增长会产生累积误差，因此对于需要长时间运行并且不便停机校正的AGV***，需要采用其他辅助定位方式修正AGV位置。本实施例引入二维码技术，将包含有位置信息的二维码粘贴到对应地面坐标点，通过对车载工业相机拍摄到二维码图像识别处理进行定位导航。，利用己知二维码的绝对位置便可计算出AGV的绝对位置，从而校正AGV的位置和航向角。

工业相机采集到二维码图像后，通过对二维码识别读取位置信息完成定位，然后根据二维码特征点在图像中的坐标确定偏差参数，得到ACV位姿。二维码图像与参数定义示意图，如图10所示。

图10中白色方框区域为二维码的背景区域，黑色点为车体中心，α为ACV中心线与二维码中心线的方向偏差，e为两者的距离偏差。假设图2中A,B,C,D四个顶点在图像坐标系OUV中的坐标分别为(a_x，a_y)、(b_x，b_y)、(c_x，c_y)、(d_x，d_y)，车体中心点坐标为(o_x，o_y)右下角像素即最大像素坐标为(u，v)，则：

当α>0时，车辆方向左偏，α<0时，车辆方向右偏。以左下角为原点，向上为Y轴正方向，向右为X轴正方向建立坐标系，则A、B、C、D在OXY坐标系中的坐标(x，y)为：

根据顶点新坐标建立二维码中心线方程，由上可知中心线斜率为cotα，且P

为线上一点，根据点斜式方程，可得中心线方程为：

将

带入公式

得中心线与

的交点Q的x坐标为：

则AGV的距离偏差e为：

其中:x为每个像素对应的实际距离。当e>o时，车辆在导航线右侧，当e<o时，车辆在导航线左侧，从而校正AGV的位置和航向角。

本发明提供一种AGV组合导航的多传感器融合定位方法，通过四元数法进行姿态解算，利用数值积分中的四阶龙格库塔算法，简化全姿态解算算法的计算量；与此同时，采用互补滤波算法通过加速度计和磁力计测量值修正四元数来修正姿态，使线性化程度和精度均有所提高，并且降低了全姿态解算算法误差。

最后应当说明的是，以上实施例仅用以说明本发明的技术方案，而非对本发明保护范围的限制，尽管参照较佳实施例对本发明作了详细地说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的实质和范围。

Claims (8)

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点的速度，计算公式为：

=

+ l

sin(θ+β)

=

+ l

cos(θ+β)。

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

=

。

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

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

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

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

7.如权利要求6所述的AGV组合导航的多传感器融合定位方法，其特征在于，所述铺设的校正标识为二维码。

8.如权利要求6所述的AGV组合导航的多传感器融合定位方法，其特征在于，所述更新对应标识数据，获取距离偏差e并清除步骤中，AGV的距离偏差e为：

e = (

)*

*x

其中:x为每个像素对应的实际距离；当e>0时，车辆在导航线右侧，当e<0时，车辆在导航线左侧，从而校正AGV的位置和航向角。

Images