一种摇臂悬架结构巡视器的运动学定位方法
阅读说明：本技术 一种摇臂悬架结构巡视器的运动学定位方法 (Kinematic positioning method for swinging arm suspension frame structure inspection device ) 是由 滕宝毅 孙赫婕 邢琰 魏春岭 贾永 毛晓艳 李志平 陈建新 于 2021-09-18 设计创作，主要内容包括：本发明一种摇臂悬架结构巡视器的运动学定位方法,所述巡视器轮系包括两个前轮、两个后轮和两个中轮共六个车轮,各轮均具备转向能力,所述六个车轮在同一个平面上或不在同一个平面上,步骤如下：步骤(一)、计算得到六轮作用下巡视器理论瞬时本体速度；步骤(二)、计算得到六轮作用下巡视器理论瞬时转向角速度；步骤(三)、获取巡视器三轴姿态；步骤(四)、计算得到带滑移补偿的巡视器位置估值信息。本发明有效降低了滑移给整器定位带来的计算误差,同时通过运动学预估配合加速度计和恒星敏感器实现运动过程中连续的三轴姿态确定,可以适用各种复杂地形,具有良好的行走表现,实用性强。(The invention relates to a kinematic positioning method of a swinging arm suspension structure patroller, wherein a patroller wheel train comprises six wheels including two front wheels, two rear wheels and two middle wheels, each wheel has steering capacity, and the six wheels are on the same plane or not, and the method comprises the following steps: calculating to obtain theoretical instantaneous body speed of the inspection device under six rounds of actions; calculating to obtain theoretical instantaneous steering angular speed of the inspection device under the action of six wheels; acquiring a three-axis posture of the inspection device; and step four, calculating to obtain the position estimation information of the patrol device with slip compensation. The invention effectively reduces the calculation error brought by the slippage to the positioning of the whole device, realizes the continuous three-axis attitude determination in the movement process by the kinematic prediction and the matching of the accelerometer and the star sensor, can be suitable for various complex terrains, has good walking performance and strong practicability.)
技术领域
本发明涉及一种摇臂悬架结构巡视器的运动学定位方法，可以应用于具有摇臂悬架式底盘结构、采用转向轮实施转向控制并装配有加速度计和恒星敏感器的漫游车辆的行进间连续定位。
背景技术
巡视器的局部定位也称为相对位置确定，其任务要求为，在以某起始点为原点的导航坐标系中，根据巡视器敏感器测量数据，确定运动过程中的巡视器三轴坐标。
对于低速运动的移动车辆，由于采用加计进行积分获得位置信息的精度较差，一般无法应用惯导系统进行局部定位，较多应用的是基于轮系里程计的定位方法。
轮系里程计定位是对各轮轮速输出进行加权综合，获得巡视器的运动速度，并利用航位推算得到运动里程和位置变化。传统车辆的轮系里程计定位方法仅需考虑平面时的情况即可，即通过各轮与整车中心的平面相对几何位置关系来完成整车速度的估计。对于摇臂悬架式底盘结构的巡视器，其应用环境为非结构化的复杂起伏地形，若只考虑二维平面运动，局部定位会产生较大误差。此外，在松软地形上运动所产生的滑移也会给巡视器的移动带来不确定性，对定位结果产生很大的影响。
考虑到采用太阳敏感器或星敏感器完成偏航姿态的确定往往收到星历、光照、环境等因素的制约，巡视器一般会配备陀螺以在运动过程中通过预估算法精确确定三轴姿态，以保证姿态确定的连续性，但该位姿确定方案对陀螺有较为严重的依赖，一旦陀螺发生故障，便无法实现运动中的连续定姿定位。
发明内容
本发明解决的技术问题是：克服现有技术的上述不足，提供一种摇臂悬架结构巡视器的运动学定位方法，引入摇臂关节测量信息和整器转向角速度测量信息，从而在非平面地形下能够较好地反映地形变化对巡视器结构的影响，并通过间接计算和补偿，有效降低了滑移给整器定位带来的计算误差，同时通过运动学预估配合加速度计和恒星敏感器实现运动过程中连续的三轴姿态确定，可以适用各种复杂地形，具有良好的行走表现，实用性强。
本发明的技术方案是：一种摇臂悬架结构巡视器的运动学定位方法，所述巡视器轮系包括两个前轮、两个后轮和两个中轮共六个车轮，各轮均具备转向能力，所述六个车轮在同一个平面上或不在同一个平面上，步骤如下：
步骤(一)、计算得到六轮作用下巡视器理论瞬时本体速度；
步骤(二)、计算得到六轮作用下巡视器理论瞬时转向角速度；
步骤(三)、获取巡视器三轴姿态；
步骤(四)、计算得到带滑移补偿的巡视器位置估值信息。
所述步骤(一)的具体过程为：
(11)、建立巡视器车体坐标系OXYZ，其中，巡视器车体前进的方向为X方向，Y方向与Z方向为垂直于X方向的平面内任意两条垂直向量所指的方向，且满足右手定则；
(12)、计算得到第i个单轮作用下巡视器理论瞬时本体速度
其中：为在X方向的分量、为在Y方向的分量、为在Z方向的分量；为第i个车轮的旋转角速度；Jiθv为第i个车轮的速度雅克比阵，可以描述与的线性转换关系，Jiθv＝[Jiθvx Jiθvy Jiθvz]T，其中：Jiθvx为Jiθv在X方向的分量、Jiθvy为Jiθv在Y方向的分量、Jiθvz为Jiθv在Z方向的分量；i＝1、2、3、4、5、6；
(13)、计算得到六轮作用下巡视器理论瞬时本体速度
其中：为在X方向的分量、为在Y方向的分量、为在Z方向的分量、ks为瞬时滑移补偿系数，初值为ks0。
所述步骤(12)中计算第i个车轮的速度雅克比阵Jiθv的具体方法如下：
对于两个前轮，i＝1、2；有如下公式：
其中：βi为主摇臂相对差动机构转角，且满足β1＝-β2，δi为两个前轮的转向角，RW为两个前轮的车轮半径；
对于两个中轮，i＝3、4；有如下公式：
其中：βi为主摇臂相对差动机构转角，且满足β1＝-β2，ρ1、ρ2为副摇臂相对主摇臂的转角，δi为两个中轮的转向角，RW为两个中轮的车轮半径；
对于两个后轮，i＝5、6；有如下公式：
其中：βi为主摇臂相对差动机构转角，且满足β1＝-β2，ρ1、ρ2为副摇臂相对主摇臂的转角，δi为两个后轮的转向角，RW为两个后轮的车轮半径。
所述步骤(二)的具体过程为：
(21)、计算得到第i个车轮轮心在车体坐标系OXYZ下的位置pCi：
其中：为在X方向的分量、为在Y方向的分量、为在Z方向的分量；i＝1、2、3、4、5、6；
(22)、计算得到第i个车轮轮心在车体坐标系OXYZ下的理论瞬时速度
其中：为在X方向的分量、为在Y方向的分量、为在Z方向的分量；i＝1、2、3、4、5、6；
(23)、计算得到巡视器理论瞬时转向角速度
所述步骤(21)中计算每一个车轮在车体坐标系OXYZ下的位置的具体方法如下：
对于两个前轮，i＝1、2；有如下公式：
其中：lm2为前轮轴与主摇臂转轴水平距离，lx、lz为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，ldi表示差动机构几何中心到左右两侧车轮轮心的水平距离，da1为主摇臂转轴与前轮轴垂直距离，RW为两个前轮的车轮半径。
对于两个中轮，i＝3、4；有如下公式：
其中：lr2为中轮轴与副摇臂转轴水平距离，lm1为主副摇臂转轴水平距离，lx、lz为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，ld(i-2)表示差动机构几何中心到左右两侧车轮轮心的水平距离，da2为副摇臂转轴与中轮轴垂直距离，dr为主副摇臂转轴垂直距离，RW为两个中轮的车轮半径。
对于两个后轮，i＝5、6；有如下公式：
其中：lr1为后轮轴与副摇臂转轴水平距离，lm1为主副摇臂转轴水平距离，lx、lz为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，ld(i-4)表示差动机构几何中心到左右两侧车轮轮心的距离，da3为副摇臂转轴与后轮轴垂直距离，dr为主副摇臂转轴垂直距离，RW为两个后轮的车轮半径。
所述步骤(三)中，若恒星敏感器可用，利用加速度计和恒星敏感器可确定此时巡视器车体坐标系相对于导航坐标系的三轴姿态并通过差分得到巡视器车体坐标系下的转向角速度ωzE.
所述步骤(三)中，若恒星敏感器不可用，利用加速度计可确定此时巡视器车体坐标系相对于导航坐标系的滚动姿态和俯仰姿态并结合通过积分得到三轴姿态
所述步骤(四)的具体过程为：
(41)、若恒星敏感器不可用，则不更新瞬时滑移补偿系数ks，若恒星敏感器可用，更新瞬时滑移补偿系数ks：
其中：ωzE为差分得到的巡视器转向角速度，kmin为滑移补偿下限，kmax为滑移补偿上限；ωth为计算滑移补偿的转向角速度阈值，ωth用于除零保护，取非负小值，LIMIT(A,B,C)表示若A小于B，取B的值，若A大于C，取C的值，否则取A的值；
(42)、计算得到导航坐标系下的巡视器速度估值
其中导航坐标系为固连于地形环境的坐标系，区别于车体坐标系，该坐标系原点位置及三轴方向不随巡视器的运动而变化，为车体坐标系到导航坐标系的转换阵；
(43)、积分得到导航坐标系下的巡视器位置估值PE：
其中PEL为前一采样周期的巡视器位置估值，Δt为采样周期，即本发明运动学定位的计算周期。
所述且kmin<kmax≤1。
所述巡视器轮系包括差动机构、两个主摇臂、两个副摇臂、两个前轮、两个后轮和两个中轮，两个主摇臂通过差动机构与巡视器车体连接，差动机构固定连接在巡视器车体上，使得左右两个主摇臂相对于差动机构的旋转角度大小相等、方向相反，每个主摇臂分别连接一个前轮和一个副摇臂，每个副摇臂分别连接一个中轮和一个后轮。
本发明与现有技术相比具有如下有益效果：
(1)、本发明在传统车辆基于轮系里程计的定位方法设计思路基础上，通过引入摇臂关节转角测量信息，从而在非平面地形下，仍能够最大限度地降低地形起伏给局部定位带来的负面影响。
(2)、本发明利用转向角速度测量信息，并通过间接计算和补偿，有效降低了滑移给整器定位带来的计算误差。
(3)、本发明可以在无法利用陀螺预估的情况下，依据有限配置获取连续姿态信息，从而扩展了巡视器的控制能力。
(4)、本发明局部定位方法同样适用于具有类似可变形底盘结构的漫游车辆或轮式移动机器人的局部定位，具有较广的应用范围，且适用各种复杂地形，具有良好的行走表现，实用性强。
附图说明
图1是常见的平面六轮转向车辆转角分配示意图。
图2是本发明巡视器轮系结构示意图。
图3是本发明巡视器轮系几何结构参数示意图。
具体实施方式
巡视器具有六轮转向、六轮驱动的摇臂悬架式底盘结构，可以被动适应自然起伏地形，因此属于具有非完整约束的轮式移动机器人。本发明巡视器轮系包括六个车轮：两个前轮、两个后轮和两个中轮，各轮均具有转向能力，且六个车轮在同一个平面上或不在同一个平面上。
若巡视器运动过程中并不能保证各轮共面，若各轮不在同一平面时，必须考虑三维空间关系对轮系里程计定位方法进行调整，将由于摇臂悬架式底盘结构被动适应地形时产生的各轮相对于整器的位置变化代入算法。
理想情况下，如图1所示，通过合理分配各转向轮转角，使其处于协调运动状态时，各轮在地形上的期望运动速度应与实际速度基本一致，但由于轮壤接触力学关系复杂，运动过程中往往会产生纵向的滑移，对巡视器的局部定位带来不利影响，而运动滑移一般属于不可测量，无法准确测得。
本发明摇臂悬架结构巡视器的局部精确定位方法具体包括如下步骤：
步骤(一)、计算六轮作用下巡视器理论瞬时本体速度，具体方法为：
(1)、建立巡视器车体坐标系OXYZ，其中，巡视器车体前进的方向为X方向，Y方向与Z方向为垂直于X方向的平面内任意两条垂直向量所指的方向，且满足右手定则；
(2)、计算第i个单轮作用下巡视器理论瞬时本体速度
其中：为在X方向的分量、为在Y方向的分量、为在Z方向的分量；为第i个车轮的旋转角速度；Jiθv为第i个车轮的速度雅克比阵，可以描述与的线性转换关系，Jiθv＝[Jiθvx Jiθvy Jiθvz]T，其中：Jiθvx为Jiθv在X方向的分量、Jiθvy为Jiθv在Y方向的分量、Jiθvz为Jiθv在Z方向的分量；i＝1、2、3、4、5、6；
(3)、计算六轮作用下巡视器理论瞬时本体速度
其中：为在X方向的分量、为在Y方向的分量、为在Z方向的分量、ks为瞬时滑移补偿系数，初值为ks0；
步骤(二)、计算六轮作用下巡视器理论瞬时转向角速度，具体方法为：
(1)、计算第i个车轮轮心在车体坐标系OXYZ下的位置
其中：为在X方向的分量、为在Y方向的分量、为在Z方向的分量；i＝1、2、3、4、5、6；
(2)、计算第i个车轮轮心在车体坐标系OXYZ下的理论瞬时速度
其中：为在X方向的分量、为在Y方向的分量、为在Z方向的分量；i＝1、2、3、4、5、6；
(3)、计算巡视器理论瞬时转向角速度
步骤(三)、获取巡视器三轴姿态，具体方法为：
(1)、若太阳敏感器可用，利用加速度计和太阳敏感器可确定此时巡视器车体坐标系相对于导航坐标系的三轴姿态并通过差分得到巡视器车体坐标系下的转向角速度ωzE。
(2)、若太阳敏感器不可用，利用加速度计可确定此时巡视器车体坐标系相对于导航坐标系的滚动姿态和俯仰姿态并结合通过积分得到三轴姿态
步骤(四)、计算带滑移补偿的巡视器位置估值信息，具体方法为：
(1)、若太阳敏感器可用，更新瞬时滑移补偿系数ks：
其中：ωzE为差分得到的巡视器转向角速度，kmin为滑移补偿下限，kmax为滑移补偿上限，ωth为计算滑移补偿的转向角速度阈值，kmin和kmax可结合轮壤力学特性的试验或分析结果确定，一般应有kmin<kmax≤1，ωth用于除零保护，可取为非负小值，LIMIT(A,B,C)表示若A小于B，取B的值，若A大于C，取C的值，否则取A的值；
(2)、计算导航坐标系下的巡视器速度估值
其中导航坐标系为固连于地形环境的坐标系，三轴方向分别指向北、东、地方向，为车体坐标系到导航坐标系的转换阵，其计算公式为：
其中为采用3-2-1转序来描述的车体坐标系相对于导航坐标系的欧拉角，分别为滚动姿态、俯仰姿态和偏航姿态。
(3)、积分得到导航坐标系下的巡视器位置估值PE：
其中PEL为前一采样周期的巡视器位置估值，Δt为采样周期。
如图2所示为本发明巡视器轮系结构示意图，本发明提供一种巡视器轮系结构，包括差动机构、两个主摇臂和两个副摇臂，其中两个主摇臂通过差动机构与巡视器车体连接，差动机构固连于车体，其作用为使得左右两个主摇臂相对于差动机构的旋转角度大小相等、方向相反，每个主摇臂分别连接一个前轮和一个副摇臂，每个副摇臂分别连接一个中轮和一个后轮。图3中给出了巡视器轮系一侧(分为左右两侧)的图示，包括一个主摇臂、一个副摇臂、一个前轮、一个中轮和一个后轮的结构，另一侧结构完全对称，相关的几何结构参数定义如图3所示。
建立巡视器车体坐标系OXYZ，其中巡视器车体的底板中心为圆心O，底板平面上指向巡视器车体前进的方向为X方向，底板平面上垂直于X方向指向巡视器右侧的方向为Y方向，Z方向垂直于底板平面且与X、Y方向满足右手定则，转向轮转角定义右转为正，左转为负。
对于两个前轮，i＝1、2；有如下公式：
其中：βi即β1、β2，为主摇臂相对差动机构转角，且满足β1＝-β2；δi为转向角，RW为车轮半径；da1为主摇臂转轴与前轮轴垂直距离，lx、lz为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，lm2为前轮轴与主摇臂转轴水平距离，ldi即ld1、ld2，表示差动机构几何中心到左右两侧车轮轮心的距离。
对于两个中轮，i＝3、4；有如下公式：
其中：即分别为ρ1、ρ2为副摇臂相对主摇臂的转角；da2为副摇臂转轴与中轮轴垂直距离，lm1为主副摇臂转轴水平距离，lr2为中轮轴与副摇臂转轴水平距离，dr为主副摇臂转轴垂直距离，ld(i-2)即ld1、ld2，表示差动机构几何中心到左右两侧车轮轮心的距离。
对于两个后轮，i＝5、6；有如下公式：
其中：即分别为da3为副摇臂转轴与后轮轴垂直距离，lr1为后轮轴与副摇臂转轴水平距离，ld(i-4)即ld1、ld2，表示差动机构几何中心到左右两侧车轮轮心的距离。
本实施例中在计算滑移补偿系数时，取ks0为0.9，kmin为0.8，kmax为1.0，ωth为0.005rad/s，即：
滑移补偿系数初值取为0.9，当太敏可用且差分转向角速度绝对值大于0.005rad/s时，根据计算结果更新滑移补偿系数在0.8～1.0之间取值。
本发明方法同样适用于具有类似可变形底盘结构的漫游车辆或轮式移动机器人的轮系协调控制。
本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：智能化行走设备及其爬楼梯方法