CN113734317A - 一种摇臂悬架结构巡视器的运动学定位方法 - Google Patents

一种摇臂悬架结构巡视器的运动学定位方法 Download PDF

Info

Publication number
CN113734317A
CN113734317A CN202111112079.3A CN202111112079A CN113734317A CN 113734317 A CN113734317 A CN 113734317A CN 202111112079 A CN202111112079 A CN 202111112079A CN 113734317 A CN113734317 A CN 113734317A
Authority
CN
China
Prior art keywords
wheel
wheels
rocker arm
coordinate system
component
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202111112079.3A
Other languages
English (en)
Inventor
滕宝毅
孙赫婕
邢琰
魏春岭
贾永
毛晓艳
李志平
陈建新
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Control Engineering
Original Assignee
Beijing Institute of Control Engineering
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Control Engineering filed Critical Beijing Institute of Control Engineering
Priority to CN202111112079.3A priority Critical patent/CN113734317A/zh
Publication of CN113734317A publication Critical patent/CN113734317A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D61/00—Motor vehicles or trailers, characterised by the arrangement or number of wheels, not otherwise provided for, e.g. four wheels in diamond pattern
    • B62D61/10—Motor vehicles or trailers, characterised by the arrangement or number of wheels, not otherwise provided for, e.g. four wheels in diamond pattern with more than four wheels
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D7/00—Steering linkage; Stub axles or their mountings
    • B62D7/06—Steering linkage; Stub axles or their mountings for individually-pivoted wheels, e.g. on king-pins
    • B62D7/14—Steering linkage; Stub axles or their mountings for individually-pivoted wheels, e.g. on king-pins the pivotal axes being situated in more than one plane transverse to the longitudinal centre line of the vehicle, e.g. all-wheel steering
    • B62D7/142—Steering linkage; Stub axles or their mountings for individually-pivoted wheels, e.g. on king-pins the pivotal axes being situated in more than one plane transverse to the longitudinal centre line of the vehicle, e.g. all-wheel steering specially adapted for particular vehicles, e.g. tractors, carts, earth-moving vehicles, trucks
    • B62D7/144—Steering linkage; Stub axles or their mountings for individually-pivoted wheels, e.g. on king-pins the pivotal axes being situated in more than one plane transverse to the longitudinal centre line of the vehicle, e.g. all-wheel steering specially adapted for particular vehicles, e.g. tractors, carts, earth-moving vehicles, trucks for vehicles with more than two axles

Landscapes

  • Engineering & Computer Science (AREA)
  • Chemical & Material Sciences (AREA)
  • Combustion & Propulsion (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)

Abstract

本发明一种摇臂悬架结构巡视器的运动学定位方法，所述巡视器轮系包括两个前轮、两个后轮和两个中轮共六个车轮，各轮均具备转向能力，所述六个车轮在同一个平面上或不在同一个平面上，步骤如下：步骤(一)、计算得到六轮作用下巡视器理论瞬时本体速度；步骤(二)、计算得到六轮作用下巡视器理论瞬时转向角速度；步骤(三)、获取巡视器三轴姿态；步骤(四)、计算得到带滑移补偿的巡视器位置估值信息。本发明有效降低了滑移给整器定位带来的计算误差，同时通过运动学预估配合加速度计和恒星敏感器实现运动过程中连续的三轴姿态确定，可以适用各种复杂地形，具有良好的行走表现，实用性强。

Description

一种摇臂悬架结构巡视器的运动学定位方法
技术领域
本发明涉及一种摇臂悬架结构巡视器的运动学定位方法，可以应用于具有摇臂悬架式底盘结构、采用转向轮实施转向控制并装配有加速度计和恒星敏感器的漫游车辆的行进间连续定位。
背景技术
巡视器的局部定位也称为相对位置确定，其任务要求为，在以某起始点为原点的导航坐标系中，根据巡视器敏感器测量数据，确定运动过程中的巡视器三轴坐标。
对于低速运动的移动车辆，由于采用加计进行积分获得位置信息的精度较差，一般无法应用惯导系统进行局部定位，较多应用的是基于轮系里程计的定位方法。
轮系里程计定位是对各轮轮速输出进行加权综合，获得巡视器的运动速度，并利用航位推算得到运动里程和位置变化。传统车辆的轮系里程计定位方法仅需考虑平面时的情况即可，即通过各轮与整车中心的平面相对几何位置关系来完成整车速度的估计。对于摇臂悬架式底盘结构的巡视器，其应用环境为非结构化的复杂起伏地形，若只考虑二维平面运动，局部定位会产生较大误差。此外，在松软地形上运动所产生的滑移也会给巡视器的移动带来不确定性，对定位结果产生很大的影响。
考虑到采用太阳敏感器或星敏感器完成偏航姿态的确定往往收到星历、光照、环境等因素的制约，巡视器一般会配备陀螺以在运动过程中通过预估算法精确确定三轴姿态，以保证姿态确定的连续性，但该位姿确定方案对陀螺有较为严重的依赖，一旦陀螺发生故障，便无法实现运动中的连续定姿定位。
发明内容
本发明解决的技术问题是：克服现有技术的上述不足，提供一种摇臂悬架结构巡视器的运动学定位方法，引入摇臂关节测量信息和整器转向角速度测量信息，从而在非平面地形下能够较好地反映地形变化对巡视器结构的影响，并通过间接计算和补偿，有效降低了滑移给整器定位带来的计算误差，同时通过运动学预估配合加速度计和恒星敏感器实现运动过程中连续的三轴姿态确定，可以适用各种复杂地形，具有良好的行走表现，实用性强。
本发明的技术方案是：一种摇臂悬架结构巡视器的运动学定位方法，所述巡视器轮系包括两个前轮、两个后轮和两个中轮共六个车轮，各轮均具备转向能力，所述六个车轮在同一个平面上或不在同一个平面上，步骤如下：
步骤(一)、计算得到六轮作用下巡视器理论瞬时本体速度；
步骤(二)、计算得到六轮作用下巡视器理论瞬时转向角速度；
步骤(三)、获取巡视器三轴姿态；
步骤(四)、计算得到带滑移补偿的巡视器位置估值信息。
所述步骤(一)的具体过程为：
(11)、建立巡视器车体坐标系OXYZ，其中，巡视器车体前进的方向为X方向，Y方向与Z方向为垂直于X方向的平面内任意两条垂直向量所指的方向，且满足右手定则；
(12)、计算得到第i个单轮作用下巡视器理论瞬时本体速度
Figure BDA0003270414090000021
Figure BDA0003270414090000022
其中：
Figure BDA0003270414090000023
为
Figure BDA0003270414090000024
在X方向的分量、
Figure BDA0003270414090000025
为
Figure BDA0003270414090000026
在Y方向的分量、
Figure BDA0003270414090000027
为
Figure BDA0003270414090000028
在Z方向的分量；
Figure BDA0003270414090000029
为第i个车轮的旋转角速度；Jiθv为第i个车轮的速度雅克比阵，可以描述
Figure BDA00032704140900000210
与
Figure BDA00032704140900000211
的线性转换关系，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)、计算得到六轮作用下巡视器理论瞬时本体速度
Figure BDA00032704140900000212
Figure BDA0003270414090000031
其中：
Figure BDA0003270414090000032
为
Figure BDA0003270414090000033
在X方向的分量、
Figure BDA0003270414090000034
为
Figure BDA0003270414090000035
在Y方向的分量、
Figure BDA0003270414090000036
为
Figure BDA0003270414090000037
在Z方向的分量、ks为瞬时滑移补偿系数，初值为ks0。
所述步骤(12)中计算第i个车轮的速度雅克比阵Jiθv的具体方法如下：
对于两个前轮，i＝1、2；有如下公式：
Figure BDA0003270414090000038
其中：βi为主摇臂相对差动机构转角，且满足β1＝-β2，δi为两个前轮的转向角，RW为两个前轮的车轮半径；
对于两个中轮，i＝3、4；有如下公式：
Figure BDA0003270414090000039
其中：
Figure BDA00032704140900000310
βi为主摇臂相对差动机构转角，且满足β1＝-β2，ρ1、ρ2为副摇臂相对主摇臂的转角，δi为两个中轮的转向角，RW为两个中轮的车轮半径；
对于两个后轮，i＝5、6；有如下公式：
Figure BDA00032704140900000311
其中：
Figure BDA00032704140900000312
βi为主摇臂相对差动机构转角，且满足β1＝-β2，ρ1、ρ2为副摇臂相对主摇臂的转角，δi为两个后轮的转向角，RW为两个后轮的车轮半径。
所述步骤(二)的具体过程为：
(21)、计算得到第i个车轮轮心在车体坐标系OXYZ下的位置pCi：
Figure BDA0003270414090000041
其中：
Figure BDA0003270414090000042
为
Figure BDA0003270414090000043
在X方向的分量、
Figure BDA0003270414090000044
为
Figure BDA0003270414090000045
在Y方向的分量、
Figure BDA0003270414090000046
为
Figure BDA0003270414090000047
在Z方向的分量；i＝1、2、3、4、5、6；
(22)、计算得到第i个车轮轮心在车体坐标系OXYZ下的理论瞬时速度
Figure BDA0003270414090000048
Figure BDA0003270414090000049
其中：
Figure BDA00032704140900000410
为
Figure BDA00032704140900000411
在X方向的分量、
Figure BDA00032704140900000412
为
Figure BDA00032704140900000413
在Y方向的分量、
Figure BDA00032704140900000414
为
Figure BDA00032704140900000415
在Z方向的分量；i＝1、2、3、4、5、6；
(23)、计算得到巡视器理论瞬时转向角速度
Figure BDA00032704140900000416
Figure BDA00032704140900000417
所述步骤(21)中计算每一个车轮在车体坐标系OXYZ下的位置
Figure BDA00032704140900000418
的具体方法如下：
对于两个前轮，i＝1、2；有如下公式：
Figure BDA00032704140900000419
其中：lm2为前轮轴与主摇臂转轴水平距离，lx、lz为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，ldi表示差动机构几何中心到左右两侧车轮轮心的水平距离，da1为主摇臂转轴与前轮轴垂直距离，RW为两个前轮的车轮半径。
对于两个中轮，i＝3、4；有如下公式：
Figure BDA00032704140900000420
其中：lr2为中轮轴与副摇臂转轴水平距离，lm1为主副摇臂转轴水平距离，lx、lz为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，ld(i-2)表示差动机构几何中心到左右两侧车轮轮心的水平距离，da2为副摇臂转轴与中轮轴垂直距离，dr为主副摇臂转轴垂直距离，RW为两个中轮的车轮半径。
对于两个后轮，i＝5、6；有如下公式：
Figure BDA0003270414090000051
其中：lr1为后轮轴与副摇臂转轴水平距离，lm1为主副摇臂转轴水平距离，lx、lz为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，ld(i-4)表示差动机构几何中心到左右两侧车轮轮心的距离，da3为副摇臂转轴与后轮轴垂直距离，dr为主副摇臂转轴垂直距离，RW为两个后轮的车轮半径。
所述步骤(三)中，若恒星敏感器可用，利用加速度计和恒星敏感器可确定此时巡视器车体坐标系相对于导航坐标系的三轴姿态
Figure BDA0003270414090000052
并通过差分得到巡视器车体坐标系下的转向角速度ωzE.
所述步骤(三)中，若恒星敏感器不可用，利用加速度计可确定此时巡视器车体坐标系相对于导航坐标系的滚动姿态和俯仰姿态
Figure BDA0003270414090000053
并结合
Figure BDA0003270414090000054
通过积分得到三轴姿态
Figure BDA0003270414090000055
所述步骤(四)的具体过程为：
(41)、若恒星敏感器不可用，则不更新瞬时滑移补偿系数ks，若恒星敏感器可用，更新瞬时滑移补偿系数ks：
Figure BDA0003270414090000056
其中：ωzE为差分得到的巡视器转向角速度，kmin为滑移补偿下限，kmax为滑移补偿上限；ωth为计算滑移补偿的转向角速度阈值，ωth用于除零保护，取非负小值，LIMIT(A,B,C)表示若A小于B，取B的值，若A大于C，取C的值，否则取A的值；
(42)、计算得到导航坐标系下的巡视器速度估值
Figure BDA0003270414090000061
Figure BDA0003270414090000062
其中导航坐标系为固连于地形环境的坐标系，区别于车体坐标系，该坐标系原点位置及三轴方向不随巡视器的运动而变化，
Figure BDA0003270414090000063
为车体坐标系到导航坐标系的转换阵；
(43)、积分得到导航坐标系下的巡视器位置估值PE：
Figure BDA0003270414090000064
其中PEL为前一采样周期的巡视器位置估值，Δt为采样周期，即本发明运动学定位的计算周期。
所述且kmin<kmax≤1。
所述巡视器轮系包括差动机构、两个主摇臂、两个副摇臂、两个前轮、两个后轮和两个中轮，两个主摇臂通过差动机构与巡视器车体连接，差动机构固定连接在巡视器车体上，使得左右两个主摇臂相对于差动机构的旋转角度大小相等、方向相反，每个主摇臂分别连接一个前轮和一个副摇臂，每个副摇臂分别连接一个中轮和一个后轮。
本发明与现有技术相比具有如下有益效果：
(1)、本发明在传统车辆基于轮系里程计的定位方法设计思路基础上，通过引入摇臂关节转角测量信息，从而在非平面地形下，仍能够最大限度地降低地形起伏给局部定位带来的负面影响。
(2)、本发明利用转向角速度测量信息，并通过间接计算和补偿，有效降低了滑移给整器定位带来的计算误差。
(3)、本发明可以在无法利用陀螺预估的情况下，依据有限配置获取连续姿态信息，从而扩展了巡视器的控制能力。
(4)、本发明局部定位方法同样适用于具有类似可变形底盘结构的漫游车辆或轮式移动机器人的局部定位，具有较广的应用范围，且适用各种复杂地形，具有良好的行走表现，实用性强。
附图说明
图1是常见的平面六轮转向车辆转角分配示意图。
图2是本发明巡视器轮系结构示意图。
图3是本发明巡视器轮系几何结构参数示意图。
具体实施方式
巡视器具有六轮转向、六轮驱动的摇臂悬架式底盘结构，可以被动适应自然起伏地形，因此属于具有非完整约束的轮式移动机器人。本发明巡视器轮系包括六个车轮：两个前轮、两个后轮和两个中轮，各轮均具有转向能力，且六个车轮在同一个平面上或不在同一个平面上。
若巡视器运动过程中并不能保证各轮共面，若各轮不在同一平面时，必须考虑三维空间关系对轮系里程计定位方法进行调整，将由于摇臂悬架式底盘结构被动适应地形时产生的各轮相对于整器的位置变化代入算法。
理想情况下，如图1所示，通过合理分配各转向轮转角，使其处于协调运动状态时，各轮在地形上的期望运动速度应与实际速度基本一致，但由于轮壤接触力学关系复杂，运动过程中往往会产生纵向的滑移，对巡视器的局部定位带来不利影响，而运动滑移一般属于不可测量，无法准确测得。
本发明摇臂悬架结构巡视器的局部精确定位方法具体包括如下步骤：
步骤(一)、计算六轮作用下巡视器理论瞬时本体速度，具体方法为：
(1)、建立巡视器车体坐标系OXYZ，其中，巡视器车体前进的方向为X方向，Y方向与Z方向为垂直于X方向的平面内任意两条垂直向量所指的方向，且满足右手定则；
(2)、计算第i个单轮作用下巡视器理论瞬时本体速度
Figure BDA0003270414090000071
Figure BDA0003270414090000072
其中：
Figure BDA0003270414090000073
为
Figure BDA0003270414090000074
在X方向的分量、
Figure BDA0003270414090000075
为
Figure BDA0003270414090000076
在Y方向的分量、
Figure BDA0003270414090000077
为
Figure BDA0003270414090000078
在Z方向的分量；
Figure BDA0003270414090000081
为第i个车轮的旋转角速度；Jiθv为第i个车轮的速度雅克比阵，可以描述
Figure BDA0003270414090000082
与
Figure BDA0003270414090000083
的线性转换关系，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)、计算六轮作用下巡视器理论瞬时本体速度
Figure BDA0003270414090000084
Figure BDA0003270414090000085
其中：
Figure BDA0003270414090000086
为
Figure BDA0003270414090000087
在X方向的分量、
Figure BDA0003270414090000088
为
Figure BDA0003270414090000089
在Y方向的分量、
Figure BDA00032704140900000810
为
Figure BDA00032704140900000811
在Z方向的分量、ks为瞬时滑移补偿系数，初值为ks0；
步骤(二)、计算六轮作用下巡视器理论瞬时转向角速度，具体方法为：
(1)、计算第i个车轮轮心在车体坐标系OXYZ下的位置
Figure BDA00032704140900000812
Figure BDA00032704140900000813
其中：
Figure BDA00032704140900000814
为
Figure BDA00032704140900000815
在X方向的分量、
Figure BDA00032704140900000816
为
Figure BDA00032704140900000817
在Y方向的分量、
Figure BDA00032704140900000818
为
Figure BDA00032704140900000819
在Z方向的分量；i＝1、2、3、4、5、6；
(2)、计算第i个车轮轮心在车体坐标系OXYZ下的理论瞬时速度
Figure BDA00032704140900000820
Figure BDA00032704140900000821
其中：
Figure BDA00032704140900000822
为
Figure BDA00032704140900000823
在X方向的分量、
Figure BDA00032704140900000824
为
Figure BDA00032704140900000825
在Y方向的分量、
Figure BDA00032704140900000826
为
Figure BDA00032704140900000827
在Z方向的分量；i＝1、2、3、4、5、6；
(3)、计算巡视器理论瞬时转向角速度
Figure BDA00032704140900000828
Figure BDA00032704140900000829
步骤(三)、获取巡视器三轴姿态，具体方法为：
(1)、若太阳敏感器可用，利用加速度计和太阳敏感器可确定此时巡视器车体坐标系相对于导航坐标系的三轴姿态
Figure BDA00032704140900000830
并通过差分得到巡视器车体坐标系下的转向角速度ωzE。
(2)、若太阳敏感器不可用，利用加速度计可确定此时巡视器车体坐标系相对于导航坐标系的滚动姿态和俯仰姿态
Figure BDA00032704140900000831
并结合
Figure BDA00032704140900000832
通过积分得到三轴姿态
Figure BDA0003270414090000091
步骤(四)、计算带滑移补偿的巡视器位置估值信息，具体方法为：
(1)、若太阳敏感器可用，更新瞬时滑移补偿系数ks：
Figure BDA0003270414090000092
其中：ωzE为差分得到的巡视器转向角速度，kmin为滑移补偿下限，kmax为滑移补偿上限，ωth为计算滑移补偿的转向角速度阈值，kmin和kmax可结合轮壤力学特性的试验或分析结果确定，一般应有kmin<kmax≤1，ωth用于除零保护，可取为非负小值，LIMIT(A,B,C)表示若A小于B，取B的值，若A大于C，取C的值，否则取A的值；
(2)、计算导航坐标系下的巡视器速度估值
Figure BDA0003270414090000093
Figure BDA0003270414090000094
其中导航坐标系为固连于地形环境的坐标系，三轴方向分别指向北、东、地方向，
Figure BDA0003270414090000095
为车体坐标系到导航坐标系的转换阵，其计算公式为：
Figure BDA0003270414090000096
其中
Figure BDA0003270414090000097
为采用3-2-1转序来描述的车体坐标系相对于导航坐标系的欧拉角，分别为滚动姿态、俯仰姿态和偏航姿态。
(3)、积分得到导航坐标系下的巡视器位置估值PE：
Figure BDA0003270414090000098
其中PEL为前一采样周期的巡视器位置估值，Δt为采样周期。
如图2所示为本发明巡视器轮系结构示意图，本发明提供一种巡视器轮系结构，包括差动机构、两个主摇臂和两个副摇臂，其中两个主摇臂通过差动机构与巡视器车体连接，差动机构固连于车体，其作用为使得左右两个主摇臂相对于差动机构的旋转角度大小相等、方向相反，每个主摇臂分别连接一个前轮和一个副摇臂，每个副摇臂分别连接一个中轮和一个后轮。图3中给出了巡视器轮系一侧(分为左右两侧)的图示，包括一个主摇臂、一个副摇臂、一个前轮、一个中轮和一个后轮的结构，另一侧结构完全对称，相关的几何结构参数定义如图3所示。
建立巡视器车体坐标系OXYZ，其中巡视器车体的底板中心为圆心O，底板平面上指向巡视器车体前进的方向为X方向，底板平面上垂直于X方向指向巡视器右侧的方向为Y方向，Z方向垂直于底板平面且与X、Y方向满足右手定则，转向轮转角定义右转为正，左转为负。
对于两个前轮，i＝1、2；有如下公式：
Figure BDA0003270414090000101
Figure BDA0003270414090000102
其中：βi即β1、β2，为主摇臂相对差动机构转角，且满足β1＝-β2；δi为转向角，RW为车轮半径；da1为主摇臂转轴与前轮轴垂直距离，lx、lz为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，lm2为前轮轴与主摇臂转轴水平距离，ldi即ld1、ld2，表示差动机构几何中心到左右两侧车轮轮心的距离。
对于两个中轮，i＝3、4；有如下公式：
Figure BDA0003270414090000103
Figure BDA0003270414090000104
其中：
Figure BDA0003270414090000105
即
Figure BDA0003270414090000106
分别为
Figure BDA0003270414090000107
ρ1、ρ2为副摇臂相对主摇臂的转角；da2为副摇臂转轴与中轮轴垂直距离，lm1为主副摇臂转轴水平距离，lr2为中轮轴与副摇臂转轴水平距离，dr为主副摇臂转轴垂直距离，ld(i-2)即ld1、ld2，表示差动机构几何中心到左右两侧车轮轮心的距离。
对于两个后轮，i＝5、6；有如下公式：
Figure BDA0003270414090000111
Figure BDA0003270414090000112
其中：
Figure BDA0003270414090000113
即
Figure BDA0003270414090000114
分别为
Figure BDA0003270414090000115
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之间取值。
本发明方法同样适用于具有类似可变形底盘结构的漫游车辆或轮式移动机器人的轮系协调控制。
本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (10)

1.一种摇臂悬架结构巡视器的运动学定位方法，所述巡视器轮系包括两个前轮、两个后轮和两个中轮共六个车轮，各轮均具备转向能力，所述六个车轮在同一个平面上或不在同一个平面上，其特征在于步骤如下：
步骤(一)、计算得到六轮作用下巡视器理论瞬时本体速度；
步骤(二)、计算得到六轮作用下巡视器理论瞬时转向角速度；
步骤(三)、获取巡视器三轴姿态；
步骤(四)、计算得到带滑移补偿的巡视器位置估值信息。
2.根据权利要求1所述的一种摇臂悬架结构巡视器的运动学定位方法，其特征在于：所述步骤(一)的具体过程为：
(11)、建立巡视器车体坐标系OXYZ，其中，巡视器车体前进的方向为X方向，Y方向与Z方向为垂直于X方向的平面内任意两条垂直向量所指的方向，且满足右手定则；
(12)、计算得到第i个单轮作用下巡视器理论瞬时本体速度
Figure FDA0003270414080000011
Figure FDA0003270414080000012
其中：
Figure FDA0003270414080000013
为
Figure FDA0003270414080000014
在X方向的分量、
Figure FDA0003270414080000015
为
Figure FDA0003270414080000016
在Y方向的分量、
Figure FDA0003270414080000017
为
Figure FDA0003270414080000018
在Z方向的分量；
Figure FDA0003270414080000019
为第i个车轮的旋转角速度；Jiθv为第i个车轮的速度雅克比阵，可以描述
Figure FDA00032704140800000110
与
Figure FDA00032704140800000111
的线性转换关系，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)、计算得到六轮作用下巡视器理论瞬时本体速度
Figure FDA00032704140800000112
Figure FDA00032704140800000113
其中：
Figure FDA00032704140800000114
为
Figure FDA00032704140800000115
在X方向的分量、
Figure FDA00032704140800000116
为
Figure FDA00032704140800000117
在Y方向的分量、
Figure FDA00032704140800000118
为
Figure FDA00032704140800000119
在Z方向的分量、ks为瞬时滑移补偿系数，初值为ks0。
3.根据权利要求2所述的一种摇臂悬架结构巡视器的运动学定位方法，其特征在于：所述步骤(12)中计算第i个车轮的速度雅克比阵Jiθv的具体方法如下：
对于两个前轮，i＝1、2；有如下公式：
Figure FDA0003270414080000021
其中：βi为主摇臂相对差动机构转角，且满足β1＝-β2，δi为两个前轮的转向角，RW为两个前轮的车轮半径；
对于两个中轮，i＝3、4；有如下公式：
Figure FDA0003270414080000022
其中：
Figure FDA0003270414080000023
βi为主摇臂相对差动机构转角，且满足β1＝-β2，ρ1、ρ2为副摇臂相对主摇臂的转角，δi为两个中轮的转向角，RW为两个中轮的车轮半径；
对于两个后轮，i＝5、6；有如下公式：
Figure FDA0003270414080000024
其中：
Figure FDA0003270414080000025
βi为主摇臂相对差动机构转角，且满足β1＝-β2，ρ1、ρ2为副摇臂相对主摇臂的转角，δi为两个后轮的转向角，RW为两个后轮的车轮半径。
4.根据权利要求2所述的一种摇臂悬架结构巡视器的运动学定位方法，其特征在于：所述步骤(二)的具体过程为：
(21)、计算得到第i个车轮轮心在车体坐标系OXYZ下的位置
Figure FDA0003270414080000026
Figure FDA0003270414080000031
其中：
Figure FDA0003270414080000032
为
Figure FDA0003270414080000033
在X方向的分量、
Figure FDA0003270414080000034
为
Figure FDA0003270414080000035
在Y方向的分量、
Figure FDA0003270414080000036
为
Figure FDA0003270414080000037
在Z方向的分量；i＝1、2、3、4、5、6；
(22)、计算得到第i个车轮轮心在车体坐标系OXYZ下的理论瞬时速度
Figure FDA0003270414080000038
Figure FDA0003270414080000039
其中：
Figure FDA00032704140800000310
为
Figure FDA00032704140800000311
在X方向的分量、
Figure FDA00032704140800000312
为
Figure FDA00032704140800000313
在Y方向的分量、
Figure FDA00032704140800000314
为
Figure FDA00032704140800000315
在Z方向的分量；i＝1、2、3、4、5、6；
(23)、计算得到巡视器理论瞬时转向角速度
Figure FDA00032704140800000316
Figure FDA00032704140800000317
5.根据权利要求4所述的一种摇臂悬架结构巡视器的运动学定位方法，其特征在于：所述步骤(21)中计算每一个车轮在车体坐标系OXYZ下的位置
Figure FDA00032704140800000318
的具体方法如下：
对于两个前轮，i＝1、2；有如下公式：
Figure FDA00032704140800000319
其中：lm2为前轮轴与主摇臂转轴水平距离，lx、lz为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，ldi表示差动机构几何中心到左右两侧车轮轮心的水平距离，da1为主摇臂转轴与前轮轴垂直距离，RW为两个前轮的车轮半径。
对于两个中轮，i＝3、4；有如下公式：
Figure FDA00032704140800000320
其中：lr2为中轮轴与副摇臂转轴水平距离，lm1为主副摇臂转轴水平距离，lx、lz为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，ld(i-2)表示差动机构几何中心到左右两侧车轮轮心的水平距离，da2为副摇臂转轴与中轮轴垂直距离，dr为主副摇臂转轴垂直距离，RW为两个中轮的车轮半径。
对于两个后轮，i＝5、6；有如下公式：
Figure FDA0003270414080000041
其中：lr1为后轮轴与副摇臂转轴水平距离，lm1为主副摇臂转轴水平距离，lx、lz为差动机构中心在巡视器车体坐标系下的X方向和Z方向坐标，ld(i-4)表示差动机构几何中心到左右两侧车轮轮心的距离，da3为副摇臂转轴与后轮轴垂直距离，dr为主副摇臂转轴垂直距离，RW为两个后轮的车轮半径。
6.根据权利要求5所述的一种摇臂悬架结构巡视器的运动学定位方法，其特征在于：所述步骤(三)中，若恒星敏感器可用，利用加速度计和恒星敏感器可确定此时巡视器车体坐标系相对于导航坐标系的三轴姿态
Figure FDA0003270414080000042
并通过差分得到巡视器车体坐标系下的转向角速度ωzE。
7.根据权利要求5所述的一种摇臂悬架结构巡视器的运动学定位方法，其特征在于：所述步骤(三)中，若恒星敏感器不可用，利用加速度计可确定此时巡视器车体坐标系相对于导航坐标系的滚动姿态和俯仰姿态
Figure FDA0003270414080000043
并结合
Figure FDA0003270414080000044
通过积分得到三轴姿态
Figure FDA0003270414080000045
8.根据权利要求5所述的一种摇臂悬架结构巡视器的运动学定位方法，其特征在于：所述步骤(四)的具体过程为：
(41)、若恒星敏感器不可用，则不更新瞬时滑移补偿系数ks，若恒星敏感器可用，更新瞬时滑移补偿系数ks：
Figure FDA0003270414080000051
其中：ωzE为差分得到的巡视器转向角速度，kmin为滑移补偿下限，kmax为滑移补偿上限；ωth为计算滑移补偿的转向角速度阈值，ωth用于除零保护，取非负小值，LIMIT(A,B,C)表示若A小于B，取B的值，若A大于C，取C的值，否则取A的值；
(42)、计算得到导航坐标系下的巡视器速度估值
Figure FDA0003270414080000052
Figure FDA0003270414080000053
其中导航坐标系为固连于地形环境的坐标系，区别于车体坐标系，该坐标系原点位置及三轴方向不随巡视器的运动而变化，
Figure FDA0003270414080000054
为车体坐标系到导航坐标系的转换阵；
(43)、积分得到导航坐标系下的巡视器位置估值PE：
Figure FDA0003270414080000055
其中PEL为前一采样周期的巡视器位置估值，Δt为采样周期，即本发明运动学定位的计算周期。
9.根据权利要求8所述的一种摇臂悬架结构巡视器的运动学定位方法，其特征在于：所述且kmin<kmax≤1。
10.根据权利要求1-9任一所述的一种摇臂悬架结构巡视器的运动学定位方法，其特征在于：所述巡视器轮系包括差动机构、两个主摇臂、两个副摇臂、两个前轮、两个后轮和两个中轮，两个主摇臂通过差动机构与巡视器车体连接，差动机构固定连接在巡视器车体上，使得左右两个主摇臂相对于差动机构的旋转角度大小相等、方向相反，每个主摇臂分别连接一个前轮和一个副摇臂，每个副摇臂分别连接一个中轮和一个后轮。
CN202111112079.3A 2021-09-18 2021-09-18 一种摇臂悬架结构巡视器的运动学定位方法 Pending CN113734317A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111112079.3A CN113734317A (zh) 2021-09-18 2021-09-18 一种摇臂悬架结构巡视器的运动学定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111112079.3A CN113734317A (zh) 2021-09-18 2021-09-18 一种摇臂悬架结构巡视器的运动学定位方法

Publications (1)

Publication Number Publication Date
CN113734317A true CN113734317A (zh) 2021-12-03

Family

ID=78740400

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111112079.3A Pending CN113734317A (zh) 2021-09-18 2021-09-18 一种摇臂悬架结构巡视器的运动学定位方法

Country Status (1)

Country Link
CN (1) CN113734317A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115257922A (zh) * 2022-08-17 2022-11-01 航天科工智能机器人有限责任公司 一种原地转向控制方法、装置及控制系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101173858A (zh) * 2007-07-03 2008-05-07 北京控制工程研究所 一种月面巡视探测器的三维定姿与局部定位方法
CN103921955A (zh) * 2014-03-28 2014-07-16 北京空间飞行器总体设计部 一种用于探月工程的巡视器刨坑调姿方法
CN107901917A (zh) * 2017-11-16 2018-04-13 中国科学院合肥物质科学研究院 一种基于滑转滑移耦合估计的无人驾驶车辆路径跟踪控制方法
CN110489807A (zh) * 2019-07-23 2019-11-22 北京控制工程研究所 一种摇臂悬架结构巡视器的局部精确定位方法
CN112440647A (zh) * 2020-12-04 2021-03-05 应急管理部天津消防研究所 一种含有悬架结构的移动机器人底盘

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101173858A (zh) * 2007-07-03 2008-05-07 北京控制工程研究所 一种月面巡视探测器的三维定姿与局部定位方法
CN103921955A (zh) * 2014-03-28 2014-07-16 北京空间飞行器总体设计部 一种用于探月工程的巡视器刨坑调姿方法
CN107901917A (zh) * 2017-11-16 2018-04-13 中国科学院合肥物质科学研究院 一种基于滑转滑移耦合估计的无人驾驶车辆路径跟踪控制方法
CN110489807A (zh) * 2019-07-23 2019-11-22 北京控制工程研究所 一种摇臂悬架结构巡视器的局部精确定位方法
CN112440647A (zh) * 2020-12-04 2021-03-05 应急管理部天津消防研究所 一种含有悬架结构的移动机器人底盘

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
黄晗等: "滑转条件下星球车坡面通过性评估试验", 《农业工程学报》, no. 16, 23 August 2016 (2016-08-23), pages 40 - 43 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115257922A (zh) * 2022-08-17 2022-11-01 航天科工智能机器人有限责任公司 一种原地转向控制方法、装置及控制系统

Similar Documents

Publication Publication Date Title
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN109916431B (zh) 一种针对四轮移动机器人的车轮编码器标定算法
CN112083725B (zh) 一种面向自动驾驶车辆的结构共用多传感器融合定位系统
CN101173858B (zh) 一种月面巡视探测器的三维定姿与局部定位方法
CN112083726B (zh) 一种面向园区自动驾驶的双滤波器融合定位系统
CN111426318A (zh) 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
JP7036080B2 (ja) 慣性航法装置
CN107063241A (zh) 基于双gnss天线及单轴mems陀螺的前轮测角系统
Kang et al. Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator
CN104713555A (zh) 应用全天域中性点辅助定向的车辆自主导航方法
CN105172793A (zh) 自动驾驶汽车的位姿估算方法
WO2020191977A1 (zh) 一种自动泊车定位系统的航迹推算方法
CN113819905A (zh) 一种基于多传感器融合的里程计方法及装置
CN110929402A (zh) 一种基于不确定分析的概率地形估计方法
Yu et al. GPS/INS/Odometer/DR integrated navigation system aided with vehicular dynamic characteristics for autonomous vehicle application
CN110489807B (zh) 一种摇臂悬架结构巡视器的局部精确定位方法
Balaram Kinematic state estimation for a Mars rover
CN206540555U (zh) 基于双gnss天线及单轴mems陀螺的前轮测角系统
CN114234967B (zh) 一种基于多传感器融合的六足机器人定位方法
CN113734317A (zh) 一种摇臂悬架结构巡视器的运动学定位方法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN109033017B (zh) 一种丢包环境下的车辆侧倾角与俯仰角估计方法
Zhang et al. Learning end-to-end inertial-wheel odometry for vehicle ego-motion estimation
CN116625394A (zh) 一种未知路况下机器人环境感知与路径寻优系统
CN116338719A (zh) 基于b样条函数的激光雷达-惯性-车辆融合定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20211203