CN115195900B - 一种丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法 - Google Patents
一种丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法 Download PDFInfo
- Publication number
- CN115195900B CN115195900B CN202210710764.4A CN202210710764A CN115195900B CN 115195900 B CN115195900 B CN 115195900B CN 202210710764 A CN202210710764 A CN 202210710764A CN 115195900 B CN115195900 B CN 115195900B
- Authority
- CN
- China
- Prior art keywords
- wheel
- coordinate system
- leg
- vehicle body
- leveling
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 33
- 230000003592 biomimetic effect Effects 0.000 title claims description 10
- 239000011159 matrix material Substances 0.000 claims abstract description 44
- 230000009466 transformation Effects 0.000 claims abstract description 33
- 239000011664 nicotinic acid Substances 0.000 claims abstract description 23
- 238000004088 simulation Methods 0.000 claims abstract description 18
- 239000000725 suspension Substances 0.000 claims abstract description 10
- 238000012795 verification Methods 0.000 claims abstract description 4
- 210000002414 leg Anatomy 0.000 claims description 133
- 238000004422 calculation algorithm Methods 0.000 claims description 19
- 230000002068 genetic effect Effects 0.000 claims description 14
- 238000005457 optimization Methods 0.000 claims description 11
- 238000012360 testing method Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 230000008602 contraction Effects 0.000 claims description 6
- 238000013519 translation Methods 0.000 claims description 6
- 230000009471 action Effects 0.000 claims description 5
- 238000012544 monitoring process Methods 0.000 claims description 5
- 238000000354 decomposition reaction Methods 0.000 claims description 4
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 230000035772 mutation Effects 0.000 claims description 3
- 238000011426 transformation method Methods 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 2
- 210000000689 upper leg Anatomy 0.000 claims description 2
- 238000006243 chemical reaction Methods 0.000 claims 2
- 230000036544 posture Effects 0.000 claims 2
- 230000000694 effects Effects 0.000 abstract description 11
- 238000009825 accumulation Methods 0.000 abstract description 4
- 238000004458 analytical method Methods 0.000 description 2
- 238000013528 artificial neural network Methods 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 238000011217 control strategy Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D57/00—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track
- B62D57/02—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members
- B62D57/028—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members having wheels and mechanical legs
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0891—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for land vehicles
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/12—Computing arrangements based on biological models using genetic models
- G06N3/126—Evolutionary algorithms, e.g. genetic algorithms or genetic programming
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Biophysics (AREA)
- Health & Medical Sciences (AREA)
- Life Sciences & Earth Sciences (AREA)
- Bioinformatics & Computational Biology (AREA)
- Theoretical Computer Science (AREA)
- Evolutionary Biology (AREA)
- General Physics & Mathematics (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Biomedical Technology (AREA)
- Molecular Biology (AREA)
- Remote Sensing (AREA)
- Physiology (AREA)
- Genetics & Genomics (AREA)
- Artificial Intelligence (AREA)
- Radar, Positioning & Navigation (AREA)
- Computational Linguistics (AREA)
- Data Mining & Analysis (AREA)
- Evolutionary Computation (AREA)
- General Health & Medical Sciences (AREA)
- Automation & Control Theory (AREA)
- Computing Systems (AREA)
- General Engineering & Computer Science (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Aviation & Aerospace Engineering (AREA)
- Chemical & Material Sciences (AREA)
- Combustion & Propulsion (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法，包括建立丘陵上帝仿真机械轮腿式无人地盘运动模型；获取位姿变换矩阵；推导空间姿态参数与控制参数间的数学关系；求解目标姿态的控制参数；进行虚腿补偿；质心高度控制；仿真验证，本发明能够快速准确地确定调平所需的控制参数，限制其调平效果的主要因素为轮腿油缸的实际伸缩速度，能够直接输出当前姿态下调平所需的作动量，不需要时间和误差的累积，能够补偿可能出现的轮腿悬空状态，避免轮腿悬空所造成的倾倒风险，保证驱动力充足，控制车身质心高度，提高平稳性。
Description
技术领域
本发明涉及计算机与自动化领域，具体为一种丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法。
背景技术
在丘陵山地等不平坦地表作业时，车体随地形起伏而倾斜，造成作业效率降低、驾驶员舒适性变差、安全性降低的问题，现有的姿态调整方案多基于PID控制器及以其为基础发展出的诸如增量式PID，模糊PID，神经网络PID等控制器，采用姿态测量方法实时检测车体角度，基于位置误差控制或角度误差控制采用PID算法计算并输出信号至控制器以控制调平装置动作，实时调节车体姿态。这种方案的精度和反应速度依赖于PID参数设置，需要进行大量测试不断人为调整参数，或通过模糊逻辑，神经网络来实时产生相应的PID参数，该控制方案需要时间和误差的累积，难以达到最优控制效果。
现有的调平策略未综合考虑调平过程中可能会出现的“虚腿”即轮腿悬空状态可能引起的车体倾倒导致的姿态角突变的情况，调平后车体质心过高影响平稳性等情况，在复杂的丘陵地形中安全性难以保证。
本发明针对在丘陵山地等不平坦地表作业时，车体随地形起伏倾斜而产生的调平需求，追求更快的响应速度，更高的调平精度，针对复杂地形下调平可能遇到的“虚腿”工况和调平后质心过高产生的倾翻风险增加相应的位姿控制策略。
发明内容
本发明针对现有技术存在的上述不足，提供一种丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法，
所述方法包括如下步骤：
根据丘陵山地仿生机械轮腿式无人底盘的结构方案，建立丘陵山地仿生机械轮腿式无人底盘运动学模型；
对所述运动学模型进行分析，获取无人底盘的位姿变换矩阵；
根据所述位姿变换矩阵，推导空间姿态参数侧倾角和俯仰角与进行姿态调平的控制参数轮腿油缸伸缩量间的数学关系；
根据所述空间姿态参数侧倾角和俯仰角与进行姿态调平的控制参数轮腿油缸伸缩量间的数学关系，求解目标姿态的控制参数；
根据压力传感器获得的接地力信息，判定轮腿悬空状态并进行轮腿油缸伸长补偿；
对姿态调平的控制参数轮腿油缸伸缩量进行监控并进行质心高度控制；
通过仿真试验验证。
优选的，所述运动学模型，通过确定轮心坐标系，大臂坐标系，车体坐标系，大地坐标系的位置，确定空间姿态参数为侧倾角，俯仰角和横摆角及进行姿态调平的控制参数为轮腿油缸伸缩量；
建立所述运动学模型，推导出该底盘的轮距，轴距和车身高度与轮腿大臂摆角和轮腿油缸伸缩量间的数学关系。
优选的，对运动学模型进行分析，通过D-H变换法推导从各轮心坐标系到大臂坐标系再到车体坐标系的位姿变换矩阵，由此可得到轮心在车体坐标系下的坐标；
大臂坐标系在车体坐标系基础上经过平移和旋转得到，设大臂坐标系∑O1在车体坐标系∑O中的坐标为(m，n，p)，两坐标系间的角度为α，则大臂坐标系到车体坐标系的变换矩阵为
同理可得等变换矩阵，最终得到轮心坐标系到车体坐标系的变换矩阵T1为
其中px，py，pz为轮心在车体坐标系下的坐标，轮心在车体坐标系下的位姿矩阵w为
由车身的侧倾角，俯仰角和横摆角三个姿态参数R,P,Y得到旋转矩阵R，由车体坐标系原点在大地坐标系下的位置坐标得到位移矩阵T3，最终推导出车体坐标系到大地坐标系的位姿变换矩阵T2；
由此可得到轮心在大地坐标系下的坐标W为
W＝T2·w (6) (7)
优选的，将所述车辆的轮距，轴距和车身高度与轮腿大臂摆角和轮腿油缸伸缩量间的数学关系带入所述变换矩阵，得到轮心在车体坐标系下的坐标与轮腿油缸伸缩量间的数学关系，即轮心相对位置函数f1，以轮腿油缸伸缩量为输入，轮心在车体坐标系中的位置坐标为输出；
wi＝f1(Li)(8)
将轮心在车体坐标系下的坐标与轮腿油缸伸缩量间的数学关系进一步带入车体坐标系到大地坐标系的位姿变换矩阵，得到轮心在大地坐标系下的坐标与轮腿油缸伸缩量间的数学关系，即轮心绝对位置函数f2，以轮腿油缸伸缩量，侧倾角，俯仰角和横摆角，车体坐标系原点在大地坐标系下的位置坐标为输入，轮心在大地坐标系中的位置坐标为输出；
Wi＝f2(L1,L2,L3,L4,R,P,Y,x,y,z)(9)根据轮心在大地坐标系的位置坐标和轮心在车体坐标系中的位置坐标可推导出两坐标系间的变换矩阵，两个相同尺度的世界坐标系可以通过旋转和平移进行转换，计算转换关系需要知道双方N个对应点的坐标，设为A,B，则求解B＝R＊A+T即可。由于N可能比较大，因此此方程通常为超定方程，可使用奇异值分解进行计算，其内部原理是最小二乘法。
[U,S,V]＝SVD(H) (11)
R＝VUT (12)
T＝-RgcentroidA+centroidB (13)
其中centroidA和centroidB是A,B的平均中心。
求得的旋转矩阵则侧倾角roll，俯仰角pitch和横摆角yaw分别为
roll＝arctan(R32/R33) (14)
yaw＝arctan(R21/R11) (16)
将轮心在车体坐标系下的坐标与轮腿油缸伸缩量间的数学关系带入其中可得空间姿态参数俯仰角pitch，侧倾角roll和横摆角yaw与控制参数轮腿油缸伸缩量间的数学关系，即空间姿态函数f3。以轮腿油缸伸缩量，轮心在大地坐标系的位置坐标为输入，空间姿态参数俯仰角pitch，侧倾角roll和横摆角yaw为输出；
[RPY]＝f3(L1,L2,L3,L4,W1,W2,W3,W4) (17)。
优选的，调用所述轮心绝对位置函数f2，以当前车辆位姿信息为输入，轮心在大地坐标系中的位置坐标为输出，
Wi＝f2(L1,L2,L3,L4,R,P,Y,x,y,z) (18)
采用NSGA-II求解目标姿态的控制参数，将当前各轮腿油缸伸缩量，轮心在大地坐标系中的位置作为该算法的输入，算法根据当前各轮腿油缸伸缩量生成目标轮腿油缸伸缩量种群，调用轮心相对位置函数f1，以种群个体轮腿油缸伸缩量为输入，轮心在车体坐标系中的位置坐标为输出，
wi＝f1(Li) (19)
然后调用空间姿态函数f3，以轮心在车体坐标系中的位置坐标和轮心在大地坐标系中的位置坐标为输入，空间姿态参数俯仰角pitch，侧倾角roll和横摆角yaw为输出，
[RPY]＝f3(L1,L2,L3,L4,W1,W2,W3,W4) (20)
以空间姿态参数侧倾角roll和俯仰角pitch的平方和为目标函数f4，
g＝f4(R,P) (21)
经历5代交叉，变异，种群合并，计算目标函数值，非支配排序，计算拥挤距离，选出下一代个体，最终输出最小目标函数值对应的轮腿油缸伸缩量子代个体。
优选的，根据压力传感器获得的接地力信息，当接地力小于100N并持续0.5秒后判定轮腿处于虚腿，即悬空状态，并进行轮腿伸长补偿，判定条件通过仿真试验确定。
优选的，对姿态调平的控制参数轮腿油缸伸缩量进行监控，当四轮腿保持某一超过100mm的伸长量不变且持续0.5秒后，判定车辆处于平稳运行状态，未进行调平动作，四轮腿同时收缩以降低车辆质心高度，判定条件通过仿真试验确定。
优选的，所述方法采用MATLAB/Simulink与Adams联合仿真的形式进行姿态调平控制方法的验证。
优选的，在所述Adams中建立丘陵山地仿生机械轮腿式无人底盘的运动学模型，定义其各组件间的运动连接关系，设置联合仿真变量数据连接接口及输入变量数据驱动函数；
在所述MATLAB中建立轮心相对位置函数f1，轮心绝对位置函数f2，空间姿态函数f3对应的函数文件以待后续调用。
优选的，所述NSGA-II求解目标姿态的控制参数，包括建立对应的遗传算法优化函数，使用MATLAB Fcn模块调用该函数，建立遗传算法优化模块；所述模块只允许有一个输入和输出，在所述模块前后对所需数据进行转换，输入前将轮腿当前调整量，轮心在大地坐标系中的位置坐标遗传算法优化函数所需信息转换为一维数组，输出后将一维的输出信息转换成四个轮腿调平控制量。
通过对比PID控制方案及本发明方案在某丘陵山地仿生机械轮腿式无人底盘上的全向调平位姿控制效果，验证了本方案能够快速准确地确定调平所需的控制参数，限制其调平效果的主要因素为轮腿油缸的实际伸缩速度，能够直接输出当前姿态下调平所需的作动量，不需要时间和误差的累积，能够补偿可能出现的轮腿悬空状态，避免轮腿悬空所造成的倾倒风险，保证驱动力充足，控制车身质心高度，提高平稳性。
附图说明
本发明有如下附图：
图1一种丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法流程图；
图2丘陵山地仿生机械轮腿式无人底盘三维模型；
图3车身低位和高位状态；
图4“虚腿”工况；
图5路面模型；
图6Simulink中的Adams模型；
图7轮心绝对位置计算模块；
图8遗传算法优化模块；
图9“虚腿”补偿模块；
图10“虚腿”补偿量判断逻辑；
图11质心高度控制模块；
图12俯仰角调平效果对比；
图13侧倾角调平效果对比。
图中：1.大臂回转销；2.轮腿伸缩油缸；3.四轮腿油缸伸缩速度；4.车体姿态角；5.四轮接地力；6.车体位置；7.四轮腿油缸伸缩量；8.四轮腿油缸伸缩量，车体姿态角，四轮心位置等信息；9.轮腿调平控制量；10.计时模块；11.“虚腿”补偿量；12.质心高度控制量。
具体实施方式
以下结合附图1-13对本发明作进一步详细说明。
本实施例提供的一种丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法，如图1所示，
步骤1：根据丘陵山地仿生机械轮腿式无人底盘的结构方案，建立丘陵山地仿生机械轮腿式无人底盘运动学模型；
步骤2：对步骤1中运动学模型进行分析，获取无人底盘的位姿变换矩阵；
步骤3：根据步骤2中位姿变换矩阵，推导空间姿态参数侧倾角(roll)和俯仰角(pitch)与进行姿态调平的控制参数轮腿油缸伸缩量间的数学关系；
步骤4：根据步骤3中姿态参数与控制变量间的数学关系，采用NSGA-II(二代非支配排序遗传算法)求解目标姿态的控制参数；
步骤5：根据压力传感器获得的接地力信息，当接地力小于100N并持续0.5秒后判定轮腿处于悬空状态，并进行轮腿油缸伸长补偿；
步骤6：对姿态调平的控制参数轮腿油缸伸缩量进行监控，当四轮腿保持某一超过100mm的固定长度且持续0.5秒后，四轮腿同时收缩以降低车辆质心高度。
步骤7：采用MATLAB/Simulink与Adams联合仿真的形式进行姿态调平控制方法的验证。
以某丘陵山地仿生机械轮腿式无人底盘为例，本专利分析方法如下：
1.建立运动学模型
某丘陵山地仿生机械轮腿式无人底盘三维模型如图2所示，1为大臂回转销，2为轮腿伸缩油缸，该无人底盘四条轮腿分别通过回转销与车身连接，轮腿可绕销轴转动以改变轮距和轴距，通过轮腿上油缸的伸缩运动完成车辆质心高度及车体姿态的调整，调整效果如图3所示，分别为油缸缩短50毫米时车身质心离地高度0.57米的低位状态和油缸伸长250毫米时车身质心离地高度1.70米的高位状态。
根据其结构方案，确定轮心坐标系，大臂坐标系，车体坐标系，大地坐标系的位置，确定其空间姿态参数为侧倾角(roll)，俯仰角(pitch)和横摆角(yaw)及进行姿态调平的控制参数为轮腿油缸伸缩量。
经分析可建立该无人底盘的运动学模型，推导出该底盘的轮距，轴距和车身高度与轮腿大臂摆角和轮腿油缸伸缩量间的数学关系。
2.获取位姿变换矩阵
对步骤1中运动学模型进行分析，通过D-H变换法推导从各轮心坐标系到大臂坐标系再到车体坐标系的位姿变换矩阵，由此可得到轮心在车体坐标系下的坐标。
大臂坐标系可在车体坐标系基础上经过平移和旋转得到，设大臂坐标系∑O1在车体坐标系∑O中的坐标为(m，n，p)，两坐标系间的角度为α，则大臂坐标系到车体坐标系的变换矩阵为
同理可得等变换矩阵，最终得到轮心坐标系到车体坐标系的变换矩阵T1为
其中px，py，pz为轮心在车体坐标系下的坐标，轮心在车体坐标系下的位姿矩阵w为
由车身的侧倾角(roll)，俯仰角(pitch)和横摆角(yaw)三个姿态参数R,P,Y得到旋转矩阵R，由车体坐标系原点在大地坐标系下的位置坐标得到位移矩阵T3，最终推导出车体坐标系到大地坐标系的位姿变换矩阵T2。
由此可得到轮心在大地坐标系下的坐标W为
W＝T2·w (7)
3.推导空间姿态参数与控制参数间的数学关系
将通过步骤1获得的车辆的轮距，轴距和车身高度与轮腿大臂摆角和轮腿油缸伸缩量间的数学关系带入通过步骤2获得的轮心坐标系到车体坐标系的变换矩阵，得到轮心在车体坐标系下的坐标与轮腿油缸伸缩量间的数学关系。即轮心相对位置函数f1，以轮腿油缸伸缩量为输入，轮心在车体坐标系中的位置坐标为输出。
wi＝f1(Li) (8)
将轮心在车体坐标系下的坐标与轮腿油缸伸缩量间的数学关系进一步带入车体坐标系到大地坐标系的位姿变换矩阵，得到轮心在大地坐标系下的坐标与轮腿油缸伸缩量间的数学关系。即轮心绝对位置函数f2，以轮腿油缸伸缩量，侧倾角(roll)，俯仰角(pitch)和横摆角(yaw)，车体坐标系原点在大地坐标系下的位置坐标为输入，轮心在大地坐标系中的位置坐标为输出。
Wi＝f2(L1,L2,L3,L4,R,P,Y,x,y,z) (9)
根据轮心在大地坐标系的位置坐标和轮心在车体坐标系中的位置坐标可推导出两坐标系间的变换矩阵，两个相同尺度的世界坐标系可以通过旋转和平移进行转换，计算转换关系需要知道双方N个对应点的坐标，设为A,B，则求解B＝R＊A+T即可。由于N可能比较大，因此此方程通常为超定方程，可使用奇异值分解(SingularValue Decomposition(SVD))进行计算，其内部原理是最小二乘法。
[U,S,V]＝SVD(H) (11)
R＝VUT (12)
T＝-RgcentroidA+centroidB (13)
其中centroidA和centroidB是A,B的平均中心。
求得的旋转矩阵则侧倾角(roll)，俯仰角(pitch)和横摆角(yaw)分别为
roll＝arctan(R32/R33) (14)
yaw＝arctan(R21/R11) (16)
将轮心在车体坐标系下的坐标与轮腿油缸伸缩量间的数学关系带入其中可得空间姿态参数俯仰角(pitch)，侧倾角(roll)和横摆角(yaw)与控制参数轮腿油缸伸缩量间的数学关系，即空间姿态函数f3。以轮腿油缸伸缩量，轮心在大地坐标系的位置坐标为输入，空间姿态参数俯仰角(pitch)，侧倾角(roll)和横摆角(yaw)为输出。
[RPY]＝f3(L1,L2,L3,L4,W1,W2,W3,W4) (17)
4.求解目标姿态的控制参数
调用步骤3获得的轮心绝对位置函数f2，以当前车辆位姿信息(轮腿油缸伸缩量，侧倾角(roll)，俯仰角(pitch)，横摆角(yaw)，车体坐标系原点在大地坐标系下的位置坐标)为输入，轮心在大地坐标系中的位置坐标为输出。
Wi＝f2(L1,L2,L3,L4,R,P,Y,x,y,z) (18)
采用NSGA-II(二代非支配排序遗传算法)求解目标姿态的控制参数，将当前各轮腿油缸伸缩量，轮心在大地坐标系中的位置作为该算法的输入，算法根据当前各轮腿油缸伸缩量生成目标轮腿油缸伸缩量种群，调用轮心相对位置函数f1，以种群个体轮腿油缸伸缩量为输入，轮心在车体坐标系中的位置坐标为输出。
wi＝f1(Li) (19)
然后调用空间姿态函数f3，以轮心在车体坐标系中的位置坐标和轮心在大地坐标系中的位置坐标为输入，空间姿态参数俯仰角(pitch)，侧倾角(roll)和横摆角(yaw)为输出。
[RPY]＝f3(L1,L2,L3,L4,W1,W2,W3,W4) (20)
以空间姿态参数侧倾角(roll)和俯仰角(pitch)的平方和为目标函数f4
g＝f4(R,P) (21)
经历5代交叉，变异，种群合并，计算目标函数值，非支配排序，计算拥挤距离，选出下一代个体，最终输出最小目标函数值对应的轮腿油缸伸缩量子代个体。
5.进行“虚腿”补偿
若不进行“虚腿”补偿，则可能出现图4所示的轮腿悬空状态，“虚腿”补偿控制是根据压力传感器获得的接地力信息，当接地力小于100N并持续0.5秒后判定轮腿处于“虚腿”即悬空状态，并进行轮腿伸长补偿，判定条件通过仿真试验确定。
6.质心高度控制
对姿态调平的控制参数轮腿油缸伸缩量进行监控，当四轮腿保持某一超过100mm的伸长量不变且持续0.5秒后，判定车辆处于平稳运行状态，未进行调平动作，四轮腿同时收缩以降低车辆质心高度，判定条件通过仿真试验确定。
7.仿真验证
采用MATLAB/Simulink与Adams联合仿真的形式进行姿态调平控制方法的验证。
通过图5所示路面模型来模拟丘陵山地可能遇到的地形，弧状突起路段长10米，高1.3米，第一个梯形路段长10米，高0.3米，坡度15°，第二个梯形路段长9.5米，高0.5米，坡度30°，弧状凹坑路段长7.2米，深0.4米。
在Adams中建立丘陵山地仿生机械轮腿式无人底盘的运动学模型，定义其各组件间的运动连接关系，设置联合仿真变量数据连接接口及输入变量数据驱动函数。轮腿油缸伸缩量的控制形式为速度控制，Simulink中的Adams模型如图6所示，3为四轮腿油缸伸缩速度，4为车体姿态角，5为四轮接地力，6为车身位置，7为四轮腿油缸伸缩量，以3为输入，4，5，6，7为输出。
在MATLAB中建立步骤3中推导的轮心相对位置函数，轮心绝对位置函数，空间姿态函数对应的函数文件以待后续调用。
调用轮心绝对位置函数，建立轮心绝对位置计算模块，如图7所示，7为四轮腿油缸伸缩量，4为车体姿态角，6为车体位置，8为四轮腿油缸伸缩量，车体姿态角，四轮心位置等信息，以7，4，6等信息为输入，8为输出。
根据步骤4中的求解思路使用NSGA-II(二代非支配排序遗传算法)求解目标姿态的控制参数，建立对应的遗传算法优化函数，使用MATLAB Fcn模块调用该函数，建立遗传算法优化模块，如图8所示，8为四轮腿油缸伸缩量，车体姿态角，四轮心位置等信息，9为轮腿调平控制量，以8为输入，9为输出，该模块只允许有一个输入和输出，因此在该模块前后对所需数据进行转换，输入前将轮腿当前调整量，轮心在大地坐标系中的位置坐标等遗传算法优化函数所需信息转换为一维数组，输出后将一维的输出信息转换成四个轮腿调平控制量。
“虚腿”补偿模块如图9所示，5为四轮接地力，9为轮腿调平控制量，10为计时模块，11为“虚腿”补偿量，10对5进行实时监测，当接地力小于100N时开始计时，计时时间超过0.5秒时判定车辆处于“虚腿”状态，施加“虚腿”补偿，增加轮腿伸长量，具体补偿量视遗传算法优化模块输出的9而定，该部分具体逻辑如图10所示，当调平控制量大于0时，补偿量较小，当调平控制量小于0时，补偿量较大。
质心高度控制模块如图11所示，9为轮腿调平控制量，10为计时模块，7为四轮腿油缸伸缩量，12为质心高度控制量，10对9进行实时监测，当轮腿调平控制量为零时开始计时，计时时间超过0.5秒时判定车辆处于平稳运行状态，未进行调平动作，此时若7大于100则施加质心高度控制，增加轮腿收缩量。
最终该控制方案对Adams模型的输入3四轮腿油缸伸缩速度为9轮腿调平控制量，11“虚腿”补偿量，12质心高度控制量三者之和对时间的导数。
控制模型搭建完成后进行仿真，车体俯仰角和侧倾角调平控制效果如图12，13所示，在俯仰方向上PID控制方案与本发明的控制方案均有良好的控制效果，但本方案具备更高的调平精度和调平速度，在侧倾方向上本方案由于添加了“虚腿”补偿和质心高度控制，弥补了侧倾风险，对比PID控制方案有明显优势。本方案测试调平误差最大值为俯仰角1.08°，侧倾角1.19°，俯仰角标准差0.21647，侧倾角标准差0.17622。
通过对比PID控制方案及本发明方案在某丘陵山地仿生机械轮腿式无人底盘上的全向调平位姿控制效果，验证了本方案能够快速准确地确定调平所需的控制参数，限制其调平效果的主要因素为轮腿油缸的实际伸缩速度，能够直接输出当前姿态下调平所需的作动量，不需要时间和误差的累积，能够补偿可能出现的轮腿悬空状态，避免轮腿悬空所造成的倾倒风险，保证驱动力充足，控制车身质心高度，提高平稳性。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。
Claims (10)
1.一种丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法，其特征在于，所述方法包括如下步骤：
根据丘陵山地仿生机械轮腿式无人底盘的结构方案，建立丘陵山地仿生机械轮腿式无人底盘运动学模型；
对所述运动学模型进行分析，获取无人底盘的位姿变换矩阵；
根据所述位姿变换矩阵，推导空间姿态参数侧倾角和俯仰角与进行姿态调平的控制参数轮腿油缸伸缩量间的数学关系；
根据所述空间姿态参数侧倾角和俯仰角与进行姿态调平的控制参数轮腿油缸伸缩量间的数学关系，求解目标姿态的控制参数；
根据压力传感器获得的接地力信息，判定轮腿悬空状态并进行轮腿油缸伸长补偿；
对姿态调平的控制参数轮腿油缸伸缩量进行监控并进行质心高度控制；
通过仿真试验验证；
所述丘陵山地仿生机械轮腿式无人底盘的结构包括大臂回转销，轮腿伸缩油缸，无人底盘四条轮腿分别通过回转销与车身连接，轮腿可绕销轴转动以改变轮距和轴距，通过轮腿上油缸的伸缩运动完成车辆质心高度及车体姿态的调整。
2.如权利要求1所述的丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法，其特征在于，所述运动学模型，通过确定轮心坐标系，大臂坐标系，车体坐标系，大地坐标系的位置，确定空间姿态参数为侧倾角，俯仰角和横摆角及进行姿态调平的控制参数为轮腿油缸伸缩量；
建立所述运动学模型，推导出该底盘的轮距，轴距和车身高度与轮腿大臂摆角和轮腿油缸伸缩量间的数学关系。
3.如权利要求2所述的丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法，其特征在于，对运动学模型进行分析，通过D-H变换法推导从各轮心坐标系到大臂坐标系再到车体坐标系的位姿变换矩阵，得到轮心在车体坐标系下的坐标；
大臂坐标系在车体坐标系基础上经过平移和旋转得到，设大臂坐标系∑O1在车体坐标系∑O中的坐标为{m，n，p}，两坐标系间的角度为α，则大臂坐标系到车体坐标系的变换矩阵为
同理得到变换矩阵，最终得到轮心坐标系到车体坐标系的变换矩阵T1为
其中px，py，pz为轮心在车体坐标系下的坐标，轮心在车体坐标系下的位姿矩阵w为
由车身的侧倾角，俯仰角和横摆角三个姿态参数R,P,Y得到旋转矩阵R，由车体坐标系原点在大地坐标系下的位置坐标得到位移矩阵T3，最终推导出车体坐标系到大地坐标系的位姿变换矩阵T2；
由此得到轮心在大地坐标系下的坐标W为
W＝T2·w (7)。
4.如权利要求3所述的丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法，其特征在于，将车辆的轮距，轴距和车身高度与轮腿大臂摆角和轮腿油缸伸缩量间的数学关系带入所述变换矩阵，得到轮心在车体坐标系下的坐标与轮腿油缸伸缩量间的数学关系，即轮心相对位置函数f1，以轮腿油缸伸缩量为输入，轮心在车体坐标系中的位置坐标为输出；
wi＝f1(Li) (8)
将轮心在车体坐标系下的坐标与轮腿油缸伸缩量间的数学关系进一步带入车体坐标系到大地坐标系的位姿变换矩阵，得到轮心在大地坐标系下的坐标与轮腿油缸伸缩量间的数学关系，即轮心绝对位置函数f2，以轮腿油缸伸缩量，侧倾角，俯仰角和横摆角，车体坐标系原点在大地坐标系下的位置坐标为输入，轮心在大地坐标系中的位置坐标为输出；
Wi＝f2(L1,L2,L3,L4,R,P,Y,x,y,z) (9)
根据轮心在大地坐标系的位置坐标和轮心在车体坐标系中的位置坐标推导出两坐标系间的变换矩阵，两个相同尺度的世界坐标系通过旋转和平移进行转换，计算转换关系需要知道双方N个对应点的坐标，设为A,B，求解B＝R＊A+T,使用奇异值分解进行计算
[U,S,V]＝SVD(H) (11)
R＝VUT (12)
T＝-RgcentroidA+centroidB (13)
其中centroidA和centroidB是A,B的平均中心；
求得的旋转矩阵则侧倾角roll，俯仰角pitch和横摆角yaw分别为
roll＝arctan(R32/R33) (14)
yaw＝arctan(R21/R11) (16)
将轮心在车体坐标系下的坐标与轮腿油缸伸缩量间的数学关系带入其中得到空间姿态参数俯仰角pitch，侧倾角roll和横摆角yaw与控制参数轮腿油缸伸缩量间的数学关系，即空间姿态函数f3,以轮腿油缸伸缩量，轮心在大地坐标系的位置坐标为输入，空间姿态参数俯仰角pitch，侧倾角roll和横摆角yaw为输出；
[R P Y]＝f3(L1,L2,L3,L4,W1,W2,W3,W4) (17)。
5.如权利要求4所述的丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法，其特征在于，调用所述轮心绝对位置函数f2，以当前车辆位姿信息为输入，轮心在大地坐标系中的位置坐标为输出，
Wi＝f2(L1,L2,L3,L4,R,P,Y,x,y,z) (18)
求解目标姿态的控制参数，将当前各轮腿油缸伸缩量，轮心在大地坐标系中的位置作为输入，根据当前各轮腿油缸伸缩量生成目标轮腿油缸伸缩量种群，调用轮心相对位置函数f1，以种群个体轮腿油缸伸缩量为输入，轮心在车体坐标系中的位置坐标为输出，
wi＝f1(Li) (19)
然后调用空间姿态函数f3，以轮心在车体坐标系中的位置坐标和轮心在大地坐标系中的位置坐标为输入，空间姿态参数俯仰角pitch，侧倾角roll和横摆角yaw为输出，
[R P Y]＝f3(L1,L2,L3,L4,W1,W2,W3,W4) (20)
以空间姿态参数侧倾角roll和俯仰角pitch的平方和为目标函数f4，
g＝f4(R,P) (21)
经历5代交叉，变异，种群合并，计算目标函数值，非支配排序，计算拥挤距离，选出下一代个体，最终输出最小目标函数值对应的轮腿油缸伸缩量子代个体。
6.如权利要求5所述的丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法，其特征在于，根据压力传感器获得的接地力信息，当接地力小于100N并持续0.5秒后判定轮腿处于虚腿，即悬空状态，并进行轮腿伸长补偿，判定条件通过仿真试验确定。
7.如权利要求6所述的丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法，其特征在于，对姿态调平的控制参数轮腿油缸伸缩量进行监控，当四轮腿保持某一超过100mm的伸长量不变且持续0.5秒后，判定车辆处于平稳运行状态，未进行调平动作，四轮腿同时收缩以降低车辆质心高度，判定条件通过仿真试验确定。
8.如权利要求1所述的丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法，其特征在于，所述方法采用MATLAB/Simulink与Adams联合仿真的形式进行姿态调平控制方法的验证。
9.如权利要求8所述的丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法，其特征在于，在所述Adams中建立丘陵山地仿生机械轮腿式无人底盘的运动学模型，定义其各组件间的运动连接关系，设置联合仿真变量数据连接接口及输入变量数据驱动函数；
在所述MATLAB中建立轮心相对位置函数f1，轮心绝对位置函数f2，空间姿态函数f3对应的函数文件以待后续调用。
10.如权利要求1所述的丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法，其特征在于，所述求解目标姿态的控制参数为采用NSGA-II，包括建立对应的遗传算法优化函数，使用MATLAB Fcn模块调用该函数，建立遗传算法优化模块；所述模块只允许有一个输入和输出，在所述模块前后对所需数据进行转换，输入前将轮腿当前调整量，轮心在大地坐标系中的位置坐标遗传算法优化函数所需信息转换为一维数组，输出后将一维的输出信息转换成四个轮腿调平控制量。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210710764.4A CN115195900B (zh) | 2022-06-22 | 2022-06-22 | 一种丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210710764.4A CN115195900B (zh) | 2022-06-22 | 2022-06-22 | 一种丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115195900A CN115195900A (zh) | 2022-10-18 |
| CN115195900B true CN115195900B (zh) | 2023-07-25 |
Family
ID=83575399
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210710764.4A Active CN115195900B (zh) | 2022-06-22 | 2022-06-22 | 一种丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115195900B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116923333B (zh) * | 2023-07-10 | 2024-05-07 | 南京航空航天大学 | 一种视觉引导下的车辆对齐与调平方法及装置 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106938597A (zh) * | 2017-02-24 | 2017-07-11 | 吉林大学 | 一种丘陵山地拖拉机车身姿态调整装置及调整方法 |
| CN108508906A (zh) * | 2018-05-23 | 2018-09-07 | 哈尔滨理工大学 | 户外环境下一种新型的多足机器人双边触觉遥操作系统及控制方法 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR3055256B1 (fr) * | 2016-08-31 | 2018-08-17 | Universite Pierre Et Marie Curie | Vehicule autonome compliant et procede de commande associe |
| US20210276642A1 (en) * | 2019-12-18 | 2021-09-09 | Carla R. Gillett | Modular robotic vehicle comprising wheeled leg array |
| CN111857170B (zh) * | 2020-07-29 | 2024-04-09 | 中国人民解放军陆军装甲兵学院 | 一种四足机器人腿部关节负载规律分析方法 |
| CN111891252B (zh) * | 2020-08-06 | 2021-11-05 | 齐鲁工业大学 | 一种四足仿生机器人的身体姿态斜坡自适应控制方法 |
| CN112433535B (zh) * | 2020-11-18 | 2022-03-25 | 北京理工大学 | 轮步复合移动平台及越障模式下多关节协同的自回稳方法 |
| CN112987769B (zh) * | 2021-02-22 | 2022-07-05 | 武汉科技大学 | 四足机器人在变刚度地形稳定过渡的腿部主动调节方法 |
-
2022
- 2022-06-22 CN CN202210710764.4A patent/CN115195900B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106938597A (zh) * | 2017-02-24 | 2017-07-11 | 吉林大学 | 一种丘陵山地拖拉机车身姿态调整装置及调整方法 |
| CN108508906A (zh) * | 2018-05-23 | 2018-09-07 | 哈尔滨理工大学 | 户外环境下一种新型的多足机器人双边触觉遥操作系统及控制方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115195900A (zh) | 2022-10-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| AU2019339956B2 (en) | Vehicle-mounted motion simulation platform based on active suspension, and control method therefor | |
| CN108549237B (zh) | 基于深度增强学习的预观控制仿人机器人步态规划方法 | |
| CN115195900B (zh) | 一种丘陵山地仿生机械轮腿式无人底盘全向调平位姿控制方法 | |
| CN109760761B (zh) | 一种基于生物仿生原理和直觉的四足机器人运动控制方法 | |
| JP7456696B2 (ja) | 車前地形に基づくアクティブサスペンション慣性制御方法及び制御システム | |
| CN112859935A (zh) | 一种多点支撑平台的调平控制方法 | |
| CN105415396A (zh) | 一种关节级联系统的运动参数检测方法及关节级联系统 | |
| Wang et al. | Development of an agricultural vehicle levelling system based on rapid active levelling | |
| CN113485459A (zh) | 一种基于机械变形干扰补偿的车载平台快速调平控制方法 | |
| Fedorov | Humanoid bipedal balancing and locomotion | |
| CN111284497A (zh) | 驾驶状态识别装置 | |
| CN115933722A (zh) | 一种同构并联腿式六足步行机器人主动柔顺控制方法 | |
| CN116859973A (zh) | 基于受力均衡的多足救援机器人运动控制方法及装置 | |
| CN115933723B (zh) | 一种应用于双足机器人快速行走的全身柔顺控制方法 | |
| CN113050409B (zh) | 仿人机器人及其控制方法以及计算机可读存储介质 | |
| Wang et al. | Steady walking of wheel-quadrupled robot over unknown rough terrain based on basic static gait | |
| TR2023006173T2 (tr) | Araç önü arazi̇si̇ne bağli olan akti̇f süspansi̇yon atalet düzenleme yöntemi̇ ve kontrol si̇stemi̇ | |
| Huang et al. | Online Learning Upper Body Posture Control for Humanoid Walking Based on BPNN | |
| CN118061197A (zh) | 一种应用于单轮腿机器人跳跃运动的控制方法及系统 | |
| Cronin et al. | Walking biped robot with distributed hierarchical control system | |
| CN116175553A (zh) | 履带式移动机械臂移动稳定性评估方法、装置和终端设备 | |
| CN117288488A (zh) | 运动平台的标定方法、装置、存储介质及车辆 | |
| CN117193070A (zh) | 一种四足机器人落足与机体协调控制方法 | |
| CN115951696A (zh) | 一种四足机器人路面姿态自适应方法 | |
| JP2000207002A (ja) | ショックアブソ―バの最適制御方法 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |