CN110489807B

CN110489807B - 一种摇臂悬架结构巡视器的局部精确定位方法

Info

Publication number: CN110489807B
Application number: CN201910668424.8A
Authority: CN
Inventors: 滕宝毅; 邢琰; 刘祥; 贾永; 张晋; 顾朋; 李化云
Original assignee: Beijing Institute of Control Engineering
Current assignee: Beijing Institute of Control Engineering
Priority date: 2019-07-23
Filing date: 2019-07-23
Publication date: 2023-07-18
Anticipated expiration: 2039-07-23
Also published as: CN110489807A

Abstract

一种摇臂悬架结构巡视器的局部精确定位方法，首先计算六轮作用下的巡视器理论瞬时本体速度，然后计算六轮作用下的巡视器理论瞬时转向角速度，最后计算带有滑移补偿的巡视器位置估值信息。与现有技术相比，本发明能够较好地反映地形变化对巡视器结构的影响，并通过间接计算和补偿，有效降低了滑移给整器定位带来的计算误差，可以适用各种复杂地形，具有良好的行走表现，实用性强。

Description

一种摇臂悬架结构巡视器的局部精确定位方法

技术领域

本发明涉及一种摇臂悬架结构巡视器的局部精确定位方法，可以应用于具有摇臂悬架式底盘结构、采用转向轮实施转向控制并装配有角速度测量敏感器的漫游车辆的局部精确定位。

背景技术

巡视器的局部定位也称为相对位置确定，其任务要求为，在以某起始点为原点的导航坐标系中，根据巡视器敏感器测量数据，确定运动过程中的巡视器三轴坐标。

对于低速运动的移动车辆，由于采用加计进行积分获得位置信息的精度较差，一般无法应用惯导系统进行局部定位，较多应用的是基于轮系里程计的定位方法。

轮系里程计定位是对各轮轮速输出进行加权综合，获得巡视器的运动速度，并利用航位推算得到运动里程和位置变化。传统车辆的轮系里程计定位方法仅需考虑平面时的情况即可，即通过各轮与整车中心的平面相对几何位置关系来完成整车速度的估计。对于摇臂悬架式底盘结构的巡视器，其应用环境为非结构化的复杂起伏地形，若只考虑二维平面运动，局部定位会产生较大误差。此外，在松软地形上运动所产生的滑移也会给巡视器的移动带来不确定性，对定位结果产生很大的影响。

发明内容

本发明的目的在于克服现有技术的上述不足，提供一种摇臂悬架结构巡视器的局部精确定位方法，引入摇臂关节测量信息和整器转向角速度测量信息，从而在非平面地形下能够较好地反映地形变化对巡视器结构的影响，并通过间接计算和补偿，有效降低了滑移给整器定位带来的计算误差，可以适用各种复杂地形，具有良好的行走表现，实用性强。

本发明的上述目的主要是通过如下技术方案予以实现的：

一种摇臂悬架结构巡视器的局部精确定位方法，所述巡视器的轮系包括两个前轮、两个后轮和两个中轮共六个车轮，各轮均具备转向能力，六个车轮在同一个平面上或不在同一个平面上，使得六个车轮在任何地形均着地，具体实现步骤如下：

步骤(一)、计算六轮作用下巡视器理论瞬时本体速度，具体方法为：

(1)、建立巡视器车体坐标系OXYZ，其中，巡视器车体前进的方向为X方向，Y方向与Z方向为垂直于X方向的平面内任意两条垂直向量所指的方向，且满足右手定则；

(2)、计算第i个单轮作用下巡视器理论瞬时本体速度

其中：为/>在X方向的分量、/>为/>在Y方向的分量、/>为/>在Z方向的分量；/>为第i个车轮的旋转角速度；J_iθv为第i个车轮的速度雅克比阵，可以描述/>与/>的线性转换关系，J_iθv＝[J_iθvx J_iθvy J_iθvz]^T，其中：J_iθvx为J_iθv在X方向的分量、J_iθvy为J_iθv在Y方向的分量、J_iθvz为J_iθv在Z方向的分量；i＝1、2、3、4、5、6；

(3)、计算六轮作用下巡视器理论瞬时本体速度

其中：为/>在X方向的分量、/>为/>在Y方向的分量、/>为/>在Z方向的分量；

步骤(二)、计算六轮作用下巡视器理论瞬时转向角速度，具体方法为：

(1)、计算第i个车轮轮心在车体坐标系OXYZ下的位置

其中：为/>在X方向的分量、/>为/>在Y方向的分量、/>为/>在Z方向的分量；i＝1、2、3、4、5、6；

(2)、计算第i个车轮轮心在车体坐标系OXYZ下的理论瞬时速度

(3)、计算巡视器理论瞬时转向角速度

步骤(三)、计算带滑移补偿的巡视器位置估值信息，具体方法为：

(1)、计算瞬时滑移补偿系数k_s：

其中：ω_zE为测量得到的巡视器真实转向角速度，k_s0为巡视器默认滑移补偿系数，k_min为滑移补偿下限，k_max为滑移补偿上限，ω_th为计算滑移补偿的转向角速度阈值，k_min和k_max可结合轮壤力学特性的试验或分析结果确定，一般应有k_min<k_max≤1，ω_th用于除零保护，可取为非负小值，LIMIT(A,B,C)表示若A小于B，取B的值，若A大于C，取C的值，否则取A的值；

(2)、计算车体坐标系OXYZ下的巡视器速度估值

(3)、计算导航坐标系下的巡视器速度估值

其中导航坐标系为固连于地形环境的坐标系，区别于车体坐标系，该坐标系原点位置及三轴方向不随巡视器的运动而变化，为车体坐标系到导航坐标系的转换阵。

(4)、积分得到导航坐标系下的巡视器位置估值P_E：

其中P_EL为前一采样周期的巡视器位置估值，Δt为采样周期，即本发明局部精确定位的计算周期。

所述巡视器轮系包括差动机构、两个主摇臂、两个副摇臂、两个前轮、两个后轮和两个中轮，两个主摇臂通过差动机构与巡视器车体连接，差动机构固定连接在巡视器车体上，使得左右两个主摇臂相对于差动机构的旋转角度大小相等、方向相反，每个主摇臂分别连接一个前轮和一个副摇臂，每个副摇臂分别连接一个中轮和一个后轮；

所述步骤(一)的(2)中计算第i个车轮的速度雅克比阵J_iθv的具体方法如下：

对于两个前轮，i＝1、2；有如下公式：

其中：β_i为主摇臂相对差动机构转角，且满足β₁＝-β₂，δ_i为两个前轮的转向角，R_W为两个前轮的车轮半径；

对于两个中轮，i＝3、4；有如下公式：

其中：β_i为主摇臂相对差动机构转角，且满足β₁＝-β₂，ρ₁、ρ₂为副摇臂相对主摇臂的转角，δ_i为两个中轮的转向角，R_W为两个中轮的车轮半径；

对于两个后轮，i＝5、6；有如下公式：

其中：β_i为主摇臂相对差动机构转角，且满足β₁＝-β₂，ρ₁、ρ₂为副摇臂相对主摇臂的转角，δ_i为两个后轮的转向角，R_W为两个后轮的车轮半径。

所述步骤(二)的(1)中计算每一个车轮在车体坐标系OXYZ下的位置的具体方法如下：

对于两个前轮，i＝1、2；有如下公式：

其中：l_m2为前轮轴与主摇臂转轴水平距离，l_x、l_z为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，l_di表示差动机构几何中心到左右两侧车轮轮心的水平距离，d_a1为主摇臂转轴与前轮轴垂直距离，R_W为两个前轮的车轮半径。

对于两个中轮，i＝3、4；有如下公式：

其中：l_r2为中轮轴与副摇臂转轴水平距离，l_m1为主副摇臂转轴水平距离，l_x、l_z为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，l_d(i-2)表示差动机构几何中心到左右两侧车轮轮心的水平距离，d_a2为副摇臂转轴与中轮轴垂直距离，d_r为主副摇臂转轴垂直距离，R_W为两个中轮的车轮半径。

对于两个后轮，i＝5、6；有如下公式：

其中：l_r1为后轮轴与副摇臂转轴水平距离，l_m1为主副摇臂转轴水平距离，l_x、l_z为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，l_d(i-4)表示差动机构几何中心到左右两侧车轮轮心的距离，d_a3为副摇臂转轴与后轮轴垂直距离，d_r为主副摇臂转轴垂直距离，R_W为两个后轮的车轮半径。

本发明与现有技术相比具有如下有益效果：

(1)、本发明在传统车辆基于轮系里程计的定位方法设计思路基础上，通过引入摇臂关节转角测量信息，从而在非平面地形下，仍能够最大限度地降低地形起伏给局部定位带来的负面影响。

(2)、本发明利用转向角速度测量信息，并通过间接计算和补偿，有效降低了滑移给整器定位带来的计算误差。

(3)、本发明局部定位方法同样适用于具有类似可变形底盘结构的漫游车辆或轮式移动机器人的局部定位，具有较广的应用范围，且适用各种复杂地形，具有良好的行走表现，实用性强。

附图说明

图1为常见的平面六轮转向车辆转角分配示意图；

图2为本发明巡视器轮系结构示意图；

图3为本发明巡视器轮系几何结构参数示意图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细的描述：

巡视器具有六轮转向、六轮驱动的摇臂悬架式底盘结构，可以被动适应自然起伏地形，因此属于具有非完整约束的轮式移动机器人。本发明巡视器轮系包括六个车轮：两个前轮、两个后轮和两个中轮，各轮均具有转向能力，且六个车轮在同一个平面上或不在同一个平面上。

若巡视器运动过程中并不能保证各轮共面，若各轮不在同一平面时，必须考虑三维空间关系对轮系里程计定位方法进行调整，将由于摇臂悬架式底盘结构被动适应地形时产生的各轮相对于整器的位置变化代入算法。

理想情况下，如图1所示，通过合理分配各转向轮转角，使其处于协调运动状态时，各轮在地形上的期望运动速度应与实际速度基本一致，但由于轮壤接触力学关系复杂，运动过程中往往会产生纵向的滑移，对巡视器的局部定位带来不利影响，而运动滑移一般属于不可测量，无法准确测得。

本发明摇臂悬架结构巡视器的局部精确定位方法具体包括如下步骤：

(2)、计算第i个单轮作用下巡视器理论瞬时本体速度

(3)、计算六轮作用下巡视器理论瞬时本体速度

(1)、计算第i个车轮轮心在车体坐标系OXYZ下的位置

(2)、计算第i个车轮轮心在车体坐标系OXYZ下的理论瞬时速度

(3)、计算巡视器理论瞬时转向角速度

(1)、计算瞬时滑移补偿系数k_s：

(2)、计算车体坐标系OXYZ下的巡视器速度估值

(3)、计算导航坐标系下的巡视器速度估值

其中导航坐标系为固连于地形环境的坐标系，三轴方向分别指向北、东、地方向，为车体坐标系到导航坐标系的转换阵，其计算公式为：

其中为采用3-2-1转序来描述的车体坐标系相对于导航坐标系的欧拉角，分别为滚动姿态、俯仰姿态和偏航姿态。

(4)、积分得到导航坐标系下的巡视器位置估值P_E：

其中P_EL为前一采样周期的巡视器位置估值，Δt为采样周期。

如图2所示为本发明巡视器轮系结构示意图，本发明提供一种巡视器轮系结构，包括差动机构、两个主摇臂和两个副摇臂，其中两个主摇臂通过差动机构与巡视器车体连接，差动机构固连于车体，其作用为使得左右两个主摇臂相对于差动机构的旋转角度大小相等、方向相反，每个主摇臂分别连接一个前轮和一个副摇臂，每个副摇臂分别连接一个中轮和一个后轮。图3中给出了巡视器轮系一侧(分为左右两侧)的图示，包括一个主摇臂、一个副摇臂、一个前轮、一个中轮和一个后轮的结构，另一侧结构完全对称，相关的几何结构参数定义如图3所示。

建立巡视器车体坐标系OXYZ，其中巡视器车体的底板中心为圆心O，底板平面上指向巡视器车体前进的方向为X方向，底板平面上垂直于X方向指向巡视器右侧的方向为Y方向，Z方向垂直于底板平面且与X、Y方向满足右手定则，转向轮转角定义右转为正，左转为负。

对于两个前轮，i＝1、2；有如下公式：

其中：β_i即β₁、β₂，为主摇臂相对差动机构转角，且满足β₁＝-β₂；δ_i为转向角，R_W为车轮半径；d_a1为主摇臂转轴与前轮轴垂直距离，l_x、l_z为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，l_m2为前轮轴与主摇臂转轴水平距离，l_di即l_d1、l_d2，表示差动机构几何中心到左右两侧车轮轮心的距离。

对于两个中轮，i＝3、4；有如下公式：

其中：即/>分别为/>ρ₁、ρ₂为副摇臂相对主摇臂的转角；d_a2为副摇臂转轴与中轮轴垂直距离，l_m1为主副摇臂转轴水平距离，l_r2为中轮轴与副摇臂转轴水平距离，d_r为主副摇臂转轴垂直距离，l_d(i-2)即l_d1、l_d2，表示差动机构几何中心到左右两侧车轮轮心的距离。

对于两个后轮，i＝5、6；有如下公式：

其中：即/>分别为/>d_a3为副摇臂转轴与后轮轴垂直距离，l_r1为后轮轴与副摇臂转轴水平距离，l_d(i-4)即l_d1、l_d2，表示差动机构几何中心到左右两侧车轮轮心的距离。

本实施例中在计算滑移补偿系数时，取k_s0为0.9，k_min为0.8，k_max为1.0，ω_th为0.005rad/s，即：

当理论转向角速度绝对值小于0.005rad/s时，滑移补偿系数取为0.9，否则根据计算结果将滑移补偿系数在0.8～1.0之间取值。

本发明方法同样适用于具有类似可变形底盘结构的漫游车辆或轮式移动机器人的轮系协调控制。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。

Claims

1.一种摇臂悬架结构巡视器的局部精确定位方法，其特征在于具体实现步骤如下：

步骤(一)、计算六轮作用下巡视器理论瞬时本体速度，其中，巡视器的轮系包括两个前轮、两个后轮和两个中轮共六个车轮，各轮均具备转向能力，六个车轮在同一个平面上或不在同一个平面上，使得六个车轮在任何地形均着地；

步骤(二)、计算六轮作用下巡视器理论瞬时转向角速度；

(21)、计算第i个车轮轮心在车体坐标系OXYZ下的位置

(22)、计算第i个车轮轮心在车体坐标系OXYZ下的理论瞬时速度

其中：为/>在X方向的分量、/>为/>在Y方向的分量、/>为/>在Z方向的分量；i＝1、2、3、4、5、6；/>为第i个单轮作用下巡视器理论瞬时本体速度，/>为六轮作用下巡视器理论瞬时本体速度；

(23)、计算巡视器理论瞬时转向角速度

步骤(三)、计算带滑移补偿的巡视器位置估值信息:

(31)、计算瞬时滑移补偿系数k_s：

其中：ω_zE为测量得到的巡视器真实转向角速度，k_s0为巡视器默认滑移补偿系数，k_min为滑移补偿下限，k_max为滑移补偿上限，ω_th为计算滑移补偿的转向角速度阈值，k_min和k_max可结合轮壤力学特性的试验或分析结果确定，k_min＜k_max≤1，ω_th用于除零保护，LIMIT(A,B,C)表示若A小于B，取B的值，若A大于C，取C的值，否则取A的值；

(32)、计算车体坐标系OXYZ下的巡视器速度估值

(33)、计算导航坐标系下的巡视器速度估值

其中，导航坐标系为固连于地形环境的坐标系，坐标系原点位置及三轴方向不随巡视器的运动而变化，为车体坐标系到导航坐标系的转换阵；

(34)、积分得到导航坐标系下的巡视器位置估值P_E：

其中，P_EL为前一采样周期的巡视器位置估值，Δt为局部精确定位的计算周期对应的采样周期。

2.根据权利要求1所述的一种摇臂悬架结构巡视器的局部精确定位方法，其特征在于：所述的步骤(一)中计算六轮作用下巡视器理论瞬时本体速度的具体方法为：

(2)、计算第i个单轮作用下巡视器理论瞬时本体速度

其中：为/>在X方向的分量、/>为/>在Y方向的分量、/>为/>在Z方向的分量；/>为第i个车轮的旋转角速度；J_iθv为第i个车轮的描述/>与/>的线性转换关系的速度雅克比阵，J_iθv＝[J_iθvx J_iθvy J_iθvz]^T，其中：J_iθvx为J_iθv在X方向的分量、J_iθvy为J_iθv在Y方向的分量、J_iθvz为J_iθv在Z方向的分量；i＝1、2、3、4、5、6；

(3)、计算六轮作用下巡视器理论瞬时本体速度

其中：为/>在X方向的分量、/>为/>在Y方向的分量、/>为/>在Z方向的分量。

3.根据权利要求1所述的一种摇臂悬架结构巡视器的局部精确定位方法，其特征在于：所述的巡视器轮系包括差动机构、两个主摇臂、两个副摇臂、两个前轮、两个后轮和两个中轮，两个主摇臂通过差动机构与巡视器车体连接，差动机构固定连接在巡视器车体上，使得左右两个主摇臂相对于差动机构的旋转角度大小相等、方向相反，每个主摇臂分别连接一个前轮和一个副摇臂，每个副摇臂分别连接一个中轮和一个后轮。

4.根据权利要求2所述的一种摇臂悬架结构巡视器的局部精确定位方法，其特征在于：所述步骤(一)中的步骤(2)中计算第i个车轮的速度雅克比阵J_iθv的具体方法如下：

对于两个前轮，i＝1、2，得到

对于两个中轮，i＝3、4，得到

对于两个后轮，i＝5、6，得到

5.根据权利要求1所述的一种摇臂悬架结构巡视器的局部精确定位方法，其特征在于：所述步骤(二)中的步骤(21)中计算每一个车轮在车体坐标系OXYZ下的位置的具体方法如下：

对于两个前轮，i＝1、2，得到

其中：l_m2为前轮轴与主摇臂转轴水平距离，l_x、l_z为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，l_di表示差动机构几何中心到左右两侧第i个车轮轮心的水平距离，d_a1为主摇臂转轴与前轮轴垂直距离，R_W为两个前轮的车轮半径；

对于两个中轮，i＝3、4，得到

其中：l_r2为中轮轴与副摇臂转轴水平距离，l_m1为主副摇臂转轴水平距离，l_x、l_z为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，l_d(i-2)表示差动机构几何中心到左右两侧第i个车轮轮心的水平距离，d_a2为副摇臂转轴与中轮轴垂直距离，d_r为主副摇臂转轴垂直距离，R_W为两个中轮的车轮半径；

对于两个后轮，i＝5、6；有如下公式：

其中：l_r1为后轮轴与副摇臂转轴水平距离，l_m1为主副摇臂转轴水平距离，l_x、l_z为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，l_d(i-4)表示差动机构几何中心到左右两侧第i个车轮轮心的距离，d_a3为副摇臂转轴与后轮轴垂直距离，d_r为主副摇臂转轴垂直距离，R_W为两个后轮的车轮半径。