CN113641180B - 一种基于变质心的机器人越障控制方法和系统 - Google Patents
一种基于变质心的机器人越障控制方法和系统 Download PDFInfo
- Publication number
- CN113641180B CN113641180B CN202111207023.6A CN202111207023A CN113641180B CN 113641180 B CN113641180 B CN 113641180B CN 202111207023 A CN202111207023 A CN 202111207023A CN 113641180 B CN113641180 B CN 113641180B
- Authority
- CN
- China
- Prior art keywords
- robot
- obstacle
- coordinate system
- mass
- mechanical arm
- 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 63
- 230000009471 action Effects 0.000 claims abstract description 80
- 230000008859 change Effects 0.000 claims abstract description 45
- 230000033001 locomotion Effects 0.000 claims abstract description 30
- 239000011159 matrix material Substances 0.000 claims description 36
- 230000009466 transformation Effects 0.000 claims description 30
- 238000013519 translation Methods 0.000 claims description 14
- 238000012937 correction Methods 0.000 claims description 7
- 230000010391 action planning Effects 0.000 claims description 6
- 210000000707 wrist Anatomy 0.000 claims description 4
- 210000000245 forearm Anatomy 0.000 claims description 2
- 230000008569 process Effects 0.000 description 25
- 238000004458 analytical method Methods 0.000 description 6
- 239000002131 composite material Substances 0.000 description 6
- 238000010586 diagram Methods 0.000 description 6
- 230000004888 barrier function Effects 0.000 description 5
- 230000003542 behavioural effect Effects 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000009194 climbing Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000005538 encapsulation Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 238000007670 refining Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000000547 structure data Methods 0.000 description 1
- 230000000576 supplementary effect Effects 0.000 description 1
Images
Classifications
-
- 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/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
-
- 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/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- 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/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- 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/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Manipulator (AREA)
Abstract
本发明涉及机器人越障控制领域，公开了一种基于变质心的机器人越障控制方法和系统。建立摆臂式履带机器人变结构质心运动学模型，计算障碍估算高度；基于障碍估算高度和动作序列规则生成动作控制序列；基于子动作与变结构质心运动学模型预测质心变化状态；评价质心变化状态是否满足越障需求，若满足，根据动作控制序列控制机器人越障，否则修正子动作，有效提高了机器人越障性能和可靠性，降低非视距条件下操作手操作难度，估算障碍高度，不依赖外部传感器，降低了系统成本和系统复杂度。
Description
技术领域
本发明涉及机器人越障控制领域，具体涉及一种基于变质心的机器人越障控制方法和系统。
背景技术
履带特种机器人作为机器人的重要种类，是为承担反恐防爆、抗震救灾等特殊应急任务需求开发的特种机器人装备，相较于普通机器人，履带特种机器人对越障能力提出了更高的要求，需要具备强大的处置能力、足够的越障能力和便捷的控制手段。为保证系统越障能力，现有技术主要从结构上进行改进，以在结构上留出冗余，增强行进越障能力，出现了履带式、轮式、复合摆臂式、轮腿复合式等多种构型的履带特种机器人，复合摆臂式履带机器人是现行搜救、排爆履带机器人的典型构型。
现行复合摆臂式履带机器人为保证本体功能多样性，通常设置较多的可活动关节数，每个活动关节对应一组操纵摇杆，现有技术中主要采用手动遥控的方式，专业操作员手动遥控各个活动关节对应的操纵摇杆实现越障、抓取等复杂功能，一方面人工遥控时，需要时刻关注机器人姿态防止倾覆，且同时控制多个操作摇杆，操作难度较大；另一方面在执行非视距、极限越障任务时，人工操作难度较高，实现极限越障任务较为困难，便捷性与功能实现可靠性的矛盾极为突出。
目前，部分研究对传统移动机器人越障性能进行模拟分析，为了简化分析过程，现有技术的传统移动机器人的越障性能分析过程通常基于运动过程中整机质心相对位置不变的假设。然而，复合摆臂式履带机器人在行进越障过程中，前摆臂和机械臂动作会发生大幅变化，整机质心位置会发生明显改变，质心相对位置不变的假设会导致越障控制下机器人姿态的倾覆，传统的移动机器人越障分析方法无法适用于复合摆臂式履带机器人越障控制，亟需一种符合摆臂式履带机器人实时变化情况的、面向便捷操作的越障控制方法和系统。
发明内容
本发明的为解决摆臂式履带机器人手动遥控为主以及传统移动机器人越障分析未考虑整机质心位置变化的问题，提供一种基于变质心的机器人越障控制方法，包括如下步骤：
S1：建立摆臂式履带机器人变结构质心运动学模型，获得障碍相对于机器人本体的障碍估算高度；
S2：基于障碍估算高度和动作序列规则生成动作控制序列，所述动作控制序列包括一个或多个子动作；
S3：基于子动作与变结构质心运动学模型预测质心变化状态；
S4：基于质心约束和越障质心需求评价质心变化状态是否满足越障需求，若满足，执行S6，否则执行S5；
S5：基于预测质心变化状态、质心约束和越障质心需求修正子动作，返回步骤S3；
S6：输出子动作至摆臂式履带机器人控制器，根据子动作控制机器人越障；
机械臂末端触碰障碍顶部，获取机器人姿态信息，基于机器人姿态信息计算障碍高度以获得障碍相对于机器人本体的障碍估算高度；预测质心变化状态具体包括根据变结构质心运动学模型计算在子动作和子动作控制量下质心变化后的坐标。
本发明还提供一种基于变质心的机器人越障控制系统，包括：
上位机，用于接收传感器数据、行进速度设定，根据基于变质心的机器人越障控制方法计算动作控制序列，向下位机发送动作控制序列；
下位机，用于根据动作控制序列实现对机器人电机位置控制；
和传感器，用于测量机器人的状态数据。
本发明提供一种基于变质心的机器人越障控制方法和系统，利用机械臂末端触碰障碍顶部，估算障碍高度，不依赖外部传感器，降低了系统成本和系统复杂度；而在动作规划之后，越障控制过程充分考虑了各个子动作导致的质心实时变化对越障成功和平衡性的影响，基于变结构质心运动学模型预测各个子动作下的质心变化状态，监测越障过程各个动作下的质心变化过程以生成既能满足平衡需求又能满足越障需求的动作控制序列，有效提高了机器人越障性能和可靠性，降低非视距条件下操作手操作难度。
附图说明
图1为实施例提供的一种基于变质心的机器人越障控制方法流程图；
图2为实施例提供的竖直障碍机器人越障人工操作序列示意图；
图3为实施例提供的摆臂式履带机器人的机器人初始状态示意图；
图4为实施例提供的摆臂式履带机器人的机器人展开状态示意图；
图5为实施例提供的非视距障碍高度估算示意图；
图6为实施例提供的机械臂载荷运动学模型示意图；
图7为实施例提供的生成动作控制序列过程示意图；
图8为实施例提供的行为-动作-关节封装示意图；
图9为实施例提供的基于规则库的关节控制量推理流程图；
图10为实施例提供的质心约束示意图；
图11为实施例提供的一种基于变质心的机器人越障控制系统结构示意图。
具体实施方式
下面结合附图对本申请实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。本领域普通技术人员可知，随着技术的发展和新场景的出现，本申请实施例提供的技术方案对于类似的技术问题，同样适用。
本申请的说明书和权利要求书及上述附图中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，以便包含一系列单元的过程、方法、系统、产品或设备不必限于那些单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它单元。
实施例一
本发明的第一实施方式涉及一种基于变质心的机器人越障控制方法，包括如下步骤：
S1：建立摆臂式履带机器人变结构质心运动学模型，计算障碍相对于机器人本体的障碍估算高度；
在确定需进行越障的摆臂式履带机器人后，根据机器人的参数信息建立变结构质心运动学模型，以备后续质心变化状态评价使用，相对于普通移动机器人的移动越障控制过程中假定质心不变的分析方法，本发明充分考虑了非视距越障情况下，质心对越障稳定性的影响，建立变结构质心运动学模型，更加准确地预测机器人越障过程中的姿态，提高越障的可靠性。
行进越障任务进程中遇到视线不可见的未知竖直障碍后，操作人员可遥控机械臂末端，触碰障碍顶部，建立接触后，开始计算障碍高度，最终计算障碍相对于机器人本体的障碍估算高度。机械臂末端触碰障碍顶部，获取机器人姿态信息，基于机器人姿态信息计算障碍高度以获得障碍相对于机器人本体的障碍估算高度。非视距是指操作人员的视线无法直接看到障碍的情况，非视距条件下，机器人操作人员对于现场的感知效果受限于机载前置行车摄像头影像的完整性，现有技术中除依赖操作人员经验和加装外置传感器外，尚无针对非视距障碍高度的估算方法。本发明在非视距情况下，不需要额外加装传感器，基于机械臂末端触碰即可估算目标障碍的高度，简化了对硬件设备的要求，降低了系统成本。
S2：基于障碍估算高度和动作序列规则生成动作控制序列，动作控制序列包括一个或多个子动作；
机器人在越障时，基本运动包括主履带平动、前摆臂摆动和机械臂摆动。越障规划的动作序列也是以上三种基本运动的不同组合，寻求稳定高效的动作序列和幅度，分析整机越障性能，提高越障稳定性。对于较高的竖直障碍，尤其是接近越障极限时，如图2所示，在操作人员的遥控下，机器人越障通常需要完成以下动作序列：前摆臂向上摆动；整体前进，接触障碍物；摆臂履带搭上障碍物，整机倾斜；摆臂向下摆动，支起整机；机械臂整体向前摆动，使重心前移，整机翘起；整机前进，整机翻越上岸；机械臂收回，摆臂上摆恢复。获得非视距的障碍高度估算高度后，设定越障前进速度，在整机质心变化实时监控的基础上，参照障碍高度、动作序列规则生成动作控制序列，动作控制序列包括各个关节的动作类型和各个关节的动作顺序，具体来说，动作控制序列包括一个或多个子动作，例如子动作1（整体前进）、子动作2（前摆臂向上摆动）、···。动作序列规则是根据人工操作的经验预先编写的越障动作序列规则，即前摆臂上摆、前进、前摆臂下摆、机械臂前摆、前进、恢复的动作序列；不同于避障路径规划操作，机器人越障运动规划与控制复杂，本发明借鉴操作人员丰富的人工操作经验，事先编写关节运动的离线动作规则库，便于选用、生成完整的越障动作，提高越障控制的可靠性。
动作控制序列包括一个或多个子动作，在步骤S2获得动作控制序列后，计算各个子动作的控制量，以便将各个子动作的控制量输出至机器人控制器实现越障控制，具体来说，读取动作控制序列中的子动作，根据机器人状态和动作序列规则计算子动作的控制量。
S3：基于子动作与变结构质心运动学模型预测质心变化状态；
摆臂式履带机器人的前摆臂和机械臂动作，会导致整机质心位置变化。合理监测并利用该质心变化，可以较大程度的提升机器人越障性能，同时可以保证机器人在越障过程中的平稳性。为了避免基于动作控制序列的越障过程中发生倾覆姿态，在根据控制量驱动机器人之前，根据子动作、子动作的控制量和步骤S1获得的变结构质心运动学模型预测子动作下的质心变化状态。预测质心变化状态具体包括根据变结构质心运动学模型计算在子动作和子动作控制量下质心变化后的坐标。
S4：基于质心约束和越障质心需求评价质心变化状态是否满足越障需求，若满足，执行S6，否则执行S5；
越障要求机器人能够保持平衡的翻越障碍，质心约束是指保持机器人平衡不发生倾覆，质心投影需保持在机器人支撑面内；越障质心需求是指翻越竖直障碍时，质心在竖直和行进方向上“超越”障碍边界。从平衡和越障两个方面评价步骤S3预测得到的质心变化状态，若质心变化状态能够满足越障的需求，则说明基于生成的动作控制序列中的各个子动作能够正常控制摆臂式履带机器人越障，各个子动作保持机器人动作平衡的同时，还能够实现障碍翻越，此时下发指令，控制机器人按照子动作和子动作的控制量行进。若质心变化状态不能够满足越障的需求，则说明生成的动作控制序列中存在子动作不能满足平衡和/或翻越的需求，需要进一步对子动作的控制量进行修正。
S5：基于预测质心变化状态、质心约束和越障质心需求修正子动作，返回步骤S3；
根据预测质心变化状态与质心约束、越障质心需求之间的差距修正子动作的控制量，得到修正后的子动作，再次返回步骤S3判断修正后的子动作是否满足越障的需求，循环对子动作进行微调修正直至由于动作控制序列中各个子动作导致的质心变化状态满足越障需求。
S6：子动作输出至摆臂式履带机器人控制器，根据子动作控制机器人越障。
将满足越障需求的子动作输出至机器人控制器，控制器获得动作类型和控制量，根据子动作和控制量控制机器人越障。
进一步的，步骤S6之后还包括：判断动作控制序列是否存在下一个未输出的子动作，若存在，则获取该下一个未输出的子动作作为当前子动作，返回步骤S3；若不存在，则完成机器人越障控制。
本发明实施例提供的一种基于变质心的机器人越障控制方法，对非视距条件下的竖直障碍进行高度估算，根据障碍高度和动作序列规则生成动作控制序列；基于动作控制序列中的子动作和变结构质心运动学模型预测质心变化状态以修正子动作，获得符合越障需求的子动作，控制机器人越障。相较于现有技术中借助传感器测量非视距障碍高度的方法，本发明利用机械臂末端触碰障碍顶部，估算障碍高度，不依赖外部传感器，降低了系统成本和系统复杂度；而在动作规划之后，相较于传统质心固定的假设，本发明的越障控制过程充分考虑了各个子动作导致的质心实时变化对越障成功和平衡性的影响，基于变结构质心运动学模型预测各个子动作下的质心变化状态，监测越障过程各个动作下的质心变化过程以生成既能满足平衡需求又能满足越障需求的动作控制序列，有效提高了机器人越障性能和可靠性，降低非视距条件下操作手操作难度。
实施例二
进一步的，本发明的第二实施方式涉及一种基于变质心的机器人越障控制方法，包括如下步骤：
S1：建立摆臂式履带机器人变结构质心运动学模型，机械臂末端触碰障碍顶部，计算障碍高度，计算障碍相对于机器人本体的障碍估算高度；
如图3、4所示，摆臂式履带机器人包括机器人初始状态和机器人展开状态，建立摆臂式履带机器人变结构质心运动学模型。其中，摆臂式履带机器人从下至上包括主履带、前摆臂和机械臂（大臂和小臂）和5个关节（关节0-关节4）。其中，
为机器人初始状态下关节3和关节4的张开角度；
分别为机器人展开状态下关节2、关节3、关节4的张开角度，
为摆臂关节1与关节2的连线与机器人坐标系X轴的夹角，用于表征摆臂关节1的张开角度；
为连杆1、2、3与机器人坐标系X轴之间的角度，
。
获得各个关节运动导致的质心变化情况，得到的质量标记矩阵为：
其中，
分别为主履带、前摆臂、机械臂大臂、机械臂小臂和机械臂末端质量。为了便于描述，变换其描述形式，
、
分别为整机、底盘和机械臂质量。
在机器人坐标系下，对于底盘摆臂：
其中，
表示在机器人坐标系下：
为底盘质心在机器人坐标系下的齐次坐标；
分别为底盘、前摆臂质心距离机器人坐标系原点的距离；
为摆臂关节1与关节2的连线与机器人坐标系
轴的夹角。
对于机械臂：
其中，
为机械臂质心在机器人坐标系下的齐次坐标；
表示机械臂大臂、机械臂小臂连杆长度；
分别表示机械臂大臂、机械臂小臂、机械臂末端连杆质心到关节2、3、4的距离；
为机械臂底座中心距机器人坐标系原点
方向和
方向的偏移量。
由
、
可得，机器人在机器人坐标系下质心坐标为：
其中，
为整机质心在机器人坐标系下的坐标。
机器人坐标系与世界坐标系之间变换矩阵包括航向角和平移数据，变换矩阵如下：
其中，
为机器人坐标系与世界坐标系之间的齐次变换矩阵；
为绕固定世界坐标系机器人的滚转、偏航和俯仰角；Rot、Trans分别表示旋转矩阵和平移矩阵；dx,dy,dz为坐标系变换的平动系数，x、y、z分别表示机器人坐标系下的质心坐标。
作为一种可选的实施例，实际竖直越障过程中俯仰角
变化最大，为进一步简化计算量同时保证越障顺利完成，保留俯仰角
，水平平动dx和竖直平动dy参数，简化公式(5)，简化后的坐标变换公式如下：
由公式(6)可知，已知机器人结构数据、各关节转角、整机俯仰角度和质心在机器人坐标系中的XY坐标，即可确定世界坐标系下的整机质心坐标。
基于机器人的展开状态可实时计算机器人的整机质心坐标，在越障行进过程中，预测监督每个子动作实施后的质心位置，为进一步分析机器人越障性能和机器人姿态稳定性提供保障。
进一步的，对于非视距竖直障碍场景，如图5所示，机械臂末端触碰障碍顶部，计算障碍高度，计算障碍相对于机器人本体的障碍估算高度。
计算障碍高度具体采用如下方法：
建立机械臂末端位姿与机器人车体的位姿变换矩阵；
依照改进DH法，建立机械臂运动学模型，如图6所示，建立坐标系，表1列出了机械臂DH参数信息，表中，行为连杆相连两坐标系的转换关系；列分别表示连杆长度、扭角、偏置和关节角。
表1机械臂DH参数表
| i |
 |
 |
d<sub>i</sub> |
 |
| 1 |
 |
0 | 0 |
 |
| 2 |
 |
0 | d<sub>1</sub> |
 |
| 3 |
 |
180 | d<sub>2</sub> |
 |
| 4 |
 |
0 | 0 |
 |
根据表1中列出的DH参数表，得到机械臂末端位姿与机器人车体的位姿变换矩阵：
其中，
为机械臂末端相对底座的平移矩阵；
为机械臂末端相对底座的旋转矩阵；
为底座坐标系0到末端坐标系4的齐次变换矩阵，
为底座坐标系0到大臂坐标系1的齐次变换矩阵，
为大臂坐标系1到小臂坐标系2的齐次变换矩阵，
为小臂坐标系2到腕部坐标系3的齐次变换矩阵，
为腕部坐标系3到末端坐标系4的齐次变换矩阵。
确定机器人姿态，基于机器人姿态计算障碍与地面的相对高度。
具体来说，若摆臂式履带机器人姿态为水平，障碍与机器人移动平台的相对高度为：
；若摆臂式履带机器人姿态为倾斜，障碍与机器人移动平台的相对高度为：
，其中，
为固定世界坐标系机器人的俯仰角，用于表示摆臂式履带机器人整机倾角，
为机械臂末端相对底座的平移矩阵中Z方向的数据，h为机械臂底座中心距机器人坐标系原点y方向的偏移量，
为底盘质心距离机器人坐标系原点的距离。
机器人机械臂末端与障碍建立接触后，经机械臂载荷运动学模型，可得到机械臂末端位姿与机器人车体的位姿变换矩阵；假设机器人的移动平台未被架空，与地面保持水平接触，机器人姿态为水平状态，则障碍高度可从齐次矩阵平移向量中直接得到；若机器人姿态为整机倾斜，可由移动平台倾角仪和相关结构参数修正，最终得到非视距障碍与移动平台的相对高度。
作为一种可选的实施例，摆臂式履带机器人各关节的转动数据参与上述解算，也可用于机器人状态的远程3D显示，在操作人员操作端生成操作孪生模型，辅助操作人员远程了解机器人状态，降低操作难度。
S2：基于障碍估算高度和动作序列规则生成动作控制序列，动作控制序列包括一个或多个子动作；
如图7所示，基于障碍估算高度和动作序列规则生成动作控制序列具体包括：行为规划层根据障碍估算高度和摆臂式履带机器人位姿信息产生行为命令；动作规划层根据行为规划层的行为命令和封装后的规则动作，确定与行为命令对应的规则动作，根据动作序列规则产生与规则动作对应的关节控制命令。
如图8所示，行为命令通常包括上竖直障碍和下竖直障碍，上竖直障碍的行为封装对应有规则动作：地面行走、上坡抬头、建立支撑、机械臂摆动和地面行走；下竖直障碍的行为封装对应有规则动作：地面行走、下坡探底、机械臂摆动、建立支撑、机械臂摆动和地面行走。根据行为命令找到对应封装的规则动作之后，进一步的，根据动作序列规则产生与规则动作中各个规则动作相对应的关节控制命令。
相较于现有技术中普通的避障路径规划操作，机器人越障运动规划与控制较为复杂，本发明逐层封装行为与动作，并将动作与行为之间建立对应关系，充分利用熟练操作员经验的同时，逐层向上封装越障行为选项，形成可靠、便捷、与实际情况相对应的越障控制命令，简化规则生成。
步骤S2之前，还包括建立动作序列规则，可根据经验事先针对不同动作建立动作序列规则，动作序列规则如表2所示：
表2动作序列规则
表中，
为固定世界坐标系机器人的俯仰角、极限倾角；v为机器人前进速度；
为机器人质心竖直方向加速度。动作序列规则以动作规则库的形式存储，里面存储多条规则语句，其中，每条规则语句以IF-Then的形式存储，IF之后记录了规则条件语句，Then之后记录了规则操作语句。
读取动作控制序列中的子动作，根据机器人状态和动作序列规则计算子动作的控制量，如图9所示，计算子动作的控制量具体包括：
S201：提取第ｉ个动作和动作规则库的第ｊ条规则；
S202：根据机器人状态匹配第ｊ条规则的规则条件语句；
S203：若二者匹配，则根据第ｊ条规则的规则操作语句和第ｉ个动作生成第ｉ个动作的控制量；若二者不匹配，则j=j+1，返回步骤S202；
S204：判断动作是否完成，即是否存在未生成控制量的子动作，若是，则i=i+1，返回S201；否则，结束控制量生成方法。
S3：基于子动作与变结构质心运动学模型预测质心变化状态；
为了避免基于动作控制序列的越障过程中发生倾覆姿态，在根据控制量驱动机器人之前，根据子动作、子动作的控制量和步骤S1获得的变结构质心运动学模型预测子动作下的质心变化状态。预测质心变化状态具体包括：计算子动作的控制量，子动作的控制量包括整机倾角、前摆臂摆角、机械臂摆角的控制量，将子动作的控制量输入变结构质心运动学模型预测质心坐标。
S4：基于质心约束和越障质心需求评价质心变化状态是否满足越障需求，若满足，执行S6，否则执行S5；
将预测的质心坐标与前摆臂支撑点、主履带支撑点在世界坐标系下的坐标进行比较，结合整机倾角，基于质心约束和越障质心需求评价质心变化状态是否满足越障需求，若满足，执行S6，否则执行S5；
质心约束是指保持机器人平衡不发生倾覆，质心投影需保持在机器人支撑面内。机器人翻越竖直障碍时，如图10所示，为了防止机器人整机向后倾覆，质心投影需保持在支撑面内，即整机质心P水平方向不越过主履带后支撑点C。可等效为：点C至P的向量
与水平单位向量
的夹角需小于90°：
其中，
为由点C向左的水平单位向量；
为点C指向点P的向量；
为机器人整机在世界坐标系下的质心坐标；
为底盘质心距离机器人坐标系原点的距离；r为主履带轮半径；
为固定世界坐标系机器人的俯仰角。
改写公式(9)：
可计算得到防倾覆约束下理论爬坡角度极限
，并获得机器人稳定爬坡角度范围0-
。
越障质心需求是指翻越竖直障碍时，质心在竖直和行进方向上“超越”障碍边界。
S5：基于预测质心变化状态、质心约束和越障质心需求修正子动作，返回步骤S3；
根据预测质心变化状态与质心约束、越障质心需求之间的差距修正子动作，具体来说，进一步细化补充动作序列规则，重新计算子动作的控制量，得到修正后的子动作，再次返回步骤S3判断修正后的子动作是否满足越障的需求，循环对子动作进行微调修正直至由于动作控制序列中各个子动作导致的质心变化状态满足越障需求。
S6：子动作输出至摆臂式履带机器人控制器，根据子动作控制机器人越障。
将满足越障需求的子动作输出至机器人控制器，控制器获得动作类型和控制量，根据子动作和控制量控制机器人越障。
本发明实施例提供的一种基于变质心的机器人越障控制方法，对各个关节的运动进行分析，建立准确的变结构质心运动学模型，从而对越障运动过程中准确预测机器人的质心变化，便于平稳越障控制，相较于传统质心固定的假设，本发明的越障控制过程充分考虑了质心实时变化对越障成功和平衡性的影响，基于变结构质心运动学模型预测质心变化状态，监测越障过程各个动作下的质心变化过程以生成越障控制动作序列，有效提高了机器人越障性能和可靠性，降低非视距条件下操作手操作难度。本发明不依赖外部传感器，利用机械臂末端触碰障碍顶部，基于机械臂末端位姿与机器人车体的位姿变换矩阵和机器人的姿态估算障碍高度，降低了系统成本和系统复杂度，基于几何关系估算障碍高度简化了估算过程。
上面各种方法的步骤划分，只是为了描述清楚，实现时可以合并为一个步骤或者对某些步骤进行拆分，分解为多个步骤，只要包括相同的逻辑关系，都在本专利的保护范围内；对算法中或者流程中添加无关紧要的修改或者引入无关紧要的设计，但不改变其算法和流程的核心设计都在该专利的保护范围内。
实施例三
本发明的第三实施方式涉及一种基于变质心的机器人越障控制系统，如图11所示，机器人越障控制系统包括上位机、下位机和传感器。上位机接收传感器数据、行进速度设定，根据实施例一或二涉及的基于变质心的机器人越障控制方法计算动作控制序列，向下位机发送动作控制序列；下位机由DSP实现控制，根据动作控制序列实现对机器人电机位置控制，机器人电机包括摆臂电机、行进电机和关节电机1-3。传感器用于测量机器人的状态数据，机器人摆臂电机、机械臂关节电机和部分传动机构上均安装电位计，用于测量电机转向角数据，同时安装有倾角仪，用于测量角度数据，以及惯性导航仪等传感器。上位机接收行进速度设定，动作控制量解算均由嵌入式计算机完成。
值得一提的是，本实施方式中所涉及到的各模块均为逻辑模块，在实际应用中，一个逻辑单元可以是一个物理单元，也可以是一个物理单元的一部分，还可以以多个物理单元的组合实现。此外，为了突出本发明的创新部分，本实施方式中并没有将与解决本发明所提出的技术问题关系不太密切的单元引入，但这并不表明本实施方式中不存在其它的单元。
本领域的普通技术人员可以理解，上述各实施方式是实现本发明的具体实施例，而在实际应用中，可以在形式上和细节上对其作各种改变，而不偏离本发明的精神和范围。
上述实例仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉此技术领域的技术人员均可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。
Claims (10)
1.一种基于变质心的机器人越障控制方法，其特征在于，包括如下步骤：
S1：建立摆臂式履带机器人变结构质心运动学模型，计算障碍相对于机器人本体的障碍估算高度，机械臂末端触碰障碍顶部，获取机器人姿态信息，基于机器人姿态信息计算障碍高度以获得障碍相对于机器人本体的障碍估算高度；
机器人在机器人坐标系下质心坐标为：
其中，
为整机质心在机器人坐标系下的坐标，
、
分别为整机和机械臂质量，
分别为主履带、前摆臂、机械臂大臂、机械臂小臂和机械臂末端质量，
分别为底盘、前摆臂质心距离机器人坐标系原点的距离，
分别表示机械臂大臂、机械臂小臂、机械臂末端连杆质心到起始关节的距离，
表示机械臂大臂、机械臂小臂连杆长度，
为前摆臂与机器人坐标系
轴的夹角，
为各连杆与机器人坐标系X轴之间的角度，
为机械臂底座中心距机器人坐标系原点
方向和
方向的偏移量；
S2：基于障碍估算高度和动作序列规则生成动作控制序列，所述动作控制序列包括一个或多个子动作；
S3：基于子动作与变结构质心运动学模型预测质心变化状态，根据变结构质心运动学模型计算在子动作和子动作控制量下质心变化后的坐标；
S4：基于质心约束和越障质心需求评价质心变化状态是否满足越障需求，若满足，执行S6，否则执行S5；
S5：基于预测质心变化状态、质心约束和越障质心需求修正子动作，返回步骤S3；
S6：输出子动作至摆臂式履带机器人控制器，根据子动作控制机器人越障。
2.根据权利要求1所述的一种基于变质心的机器人越障控制方法，其特征在于，所述S1具体包括：
机器人质心在机器人坐标系中的坐标与在世界坐标系的坐标转换公式为：
其中：
表示机器人质心坐标在机器人坐标系与世界坐标系的坐标转换，
为机器人坐标系与世界坐标系之间的齐次变换矩阵，
为整机质心在机器人坐标系下的坐标，
为中间参数，
为绕固定世界坐标系机器人的俯仰角，
为坐标系变换的平动系数，
3.根据权利要求1所述的一种基于变质心的机器人越障控制方法，其特征在于，所述机械臂末端触碰障碍顶部，获取机器人姿态信息，基于机器人姿态信息计算障碍高度以获得障碍相对于机器人本体的障碍估算高度，具体包括：
建立机械臂载荷运动学模型；
建立机械臂末端位姿与机器人车体的位姿变换矩阵；
确定机器人姿态，基于机器人姿态计算障碍与地面的相对高度。
4.根据权利要求3所述的一种基于变质心的机器人越障控制方法，其特征在于，所述机械臂末端位姿与机器人车体的位姿变换矩阵为：
其中，
为机械臂末端相对底座的平移矩阵；
为机械臂末端相对底座的旋转矩阵；
为底座坐标系0到末端坐标系4的齐次变换矩阵，
为底座坐标系0到大臂坐标系1的齐次变换矩阵，
为大臂坐标系1到小臂坐标系2的齐次变换矩阵，
为小臂坐标系2到腕部坐标系3的齐次变换矩阵，
为腕部坐标系3到末端坐标系4的齐次变换矩阵。
5.根据权利要求3所述的一种基于变质心的机器人越障控制方法，其特征在于，若摆臂式履带机器人姿态为水平，障碍与机器人移动平台的相对高度为：
，其中，
为机械臂末端相对底座的平移矩阵中Z方向的数据，h为机械臂底座中心距机器人坐标系原点y方向的偏移量。
6.根据权利要求3所述的一种基于变质心的机器人越障控制方法，其特征在于，若摆臂式履带机器人姿态为倾斜，障碍与机器人移动平台的相对高度为：
，其中，
为固定世界坐标系机器人的俯仰角，
为机械臂末端相对底座的平移矩阵中Z方向的数据，h为机械臂底座中心距机器人坐标系原点y方向的偏移量，
为底盘质心距离机器人坐标系原点的距离。
7.根据权利要求1所述的一种基于变质心的机器人越障控制方法，其特征在于，所述基于障碍估算高度和动作序列规则生成动作控制序列，具体包括：
行为规划层根据障碍估算高度和摆臂式履带机器人位姿信息产生行为命令；
动作规划层根据行为规划层的行为命令和封装后的规则动作，确定与行为命令对应的规则动作；
根据动作序列规则产生与规则动作对应的关节控制命令。
8.根据权利要求1所述的一种基于变质心的机器人越障控制方法，其特征在于，
子动作的控制量包括整机倾角、前摆臂摆角、机械臂摆角的控制量，将子动作的控制量输入变结构质心运动学模型预测质心坐标；
将预测的质心坐标与前摆臂支撑点、主履带支撑点在世界坐标系下的坐标进行比较，结合整机倾角，基于质心约束和越障质心需求评价质心变化状态是否满足越障需求。
9.根据权利要求8所述的一种基于变质心的机器人越障控制方法，其特征在于，所述质心约束具体包括：
其中，
为由主履带后支撑点向左的水平单位向量；
为主履带后支撑点指向整机质心的向量；
为机器人整机在世界坐标系下的质心坐标；
为底盘质心距离机器人坐标系原点的距离；r为主履带轮半径；
为固定世界坐标系机器人的俯仰角。
10.一种基于变质心的机器人越障控制系统，包括：
上位机，用于接收传感器数据、行进速度设定，根据权利要求1-9中任一项的基于变质心的机器人越障控制方法计算动作控制序列，向下位机发送动作控制序列；
下位机，用于根据动作控制序列实现对机器人电机位置控制；
和传感器，用于测量机器人的状态数据。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111207023.6A CN113641180B (zh) | 2021-10-18 | 2021-10-18 | 一种基于变质心的机器人越障控制方法和系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111207023.6A CN113641180B (zh) | 2021-10-18 | 2021-10-18 | 一种基于变质心的机器人越障控制方法和系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113641180A CN113641180A (zh) | 2021-11-12 |
| CN113641180B true CN113641180B (zh) | 2022-01-11 |
Family
ID=78427288
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111207023.6A Active CN113641180B (zh) | 2021-10-18 | 2021-10-18 | 一种基于变质心的机器人越障控制方法和系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113641180B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114115295B (zh) * | 2021-12-28 | 2023-08-18 | 深圳市优必选科技股份有限公司 | 机器人运动规划方法及装置、机器人控制设备和存储介质 |
| CN114967722B (zh) * | 2022-06-10 | 2023-04-25 | 北京理工大学 | 一种摇臂式机动平台自主越台阶障碍方法 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2004021895A (ja) * | 2002-06-20 | 2004-01-22 | Fujitsu Ltd | 移動ロボット |
| CN104881027A (zh) * | 2015-05-04 | 2015-09-02 | 国家电网公司 | 轮履复合式变电站巡检机器人自主越障系统及控制方法 |
| CN108153309A (zh) * | 2017-12-22 | 2018-06-12 | 安徽农业大学 | 用于履带机器人的控制方法以及履带机器人 |
| CN113467478A (zh) * | 2021-08-05 | 2021-10-01 | 盐城工学院 | 一种变质心无人搬运车轨迹跟踪控制方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101571313B1 (ko) * | 2014-06-20 | 2015-11-26 | 한국과학기술연구원 | 무게중심 제어를 통한 로봇의 자세 제어 시스템 |
-
2021
- 2021-10-18 CN CN202111207023.6A patent/CN113641180B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2004021895A (ja) * | 2002-06-20 | 2004-01-22 | Fujitsu Ltd | 移動ロボット |
| CN104881027A (zh) * | 2015-05-04 | 2015-09-02 | 国家电网公司 | 轮履复合式变电站巡检机器人自主越障系统及控制方法 |
| CN108153309A (zh) * | 2017-12-22 | 2018-06-12 | 安徽农业大学 | 用于履带机器人的控制方法以及履带机器人 |
| CN113467478A (zh) * | 2021-08-05 | 2021-10-01 | 盐城工学院 | 一种变质心无人搬运车轨迹跟踪控制方法 |
Non-Patent Citations (3)
| Title |
|---|
| The research on obstacle-surmounting capability of six-track robot with four swing arms;Shaorong Xie 等;《2013 IEEE International Conference on Robotics and Biomimetics (ROBIO)》;20131231;全文 * |
| 四履带双摆臂机器人越障机理及越障能力;李允旺 等;《机器人》;20100331;第32卷(第2期);全文 * |
| 履带式城市排爆机器人运动底盘越障性能研究;邵永贵;《中国优秀硕士学位论文全文数据库信息科技辑》;20190915(第09期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113641180A (zh) | 2021-11-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113641180B (zh) | 一种基于变质心的机器人越障控制方法和系统 | |
| JP5945419B2 (ja) | 脚式移動ロボットの脚体運動軌道生成装置。 | |
| CN105867379B (zh) | 一种机器人的运动控制方法及控制系统 | |
| Wang et al. | A hybrid visual servo controller for robust grasping by wheeled mobile robots | |
| KR100981698B1 (ko) | 다리식 이동 로봇 | |
| CN112378408A (zh) | 一种实现轮式移动机器人实时避障的路径规划方法 | |
| CN109240091B (zh) | 一种基于强化学习的水下机器人控制方法及其进行跟踪的控制方法 | |
| CN109333534B (zh) | 预规划的实时步态控制算法 | |
| CN111216125B (zh) | 面向狭窄通道的移动机械臂装置的避障方法及系统 | |
| KR102692977B1 (ko) | 발자국 접촉 검출 | |
| CN109778939A (zh) | 一种可自主规划轨迹的挖掘机臂智能控制系统及方法 | |
| CN106607910A (zh) | 一种机器人实时模仿方法 | |
| CN111290272A (zh) | 基于多足机器人的姿态平稳性调整方法 | |
| US20070267994A1 (en) | Legged Robot, Method for Controlling Legged Robot, and Device and Method for Generating Gait Data | |
| US20230321830A1 (en) | Construction constrained motion primitives from robot maps | |
| JP2018185747A (ja) | 非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラム | |
| Corsini et al. | Nonlinear model predictive control for human-robot handover with application to the aerial case | |
| Feng et al. | Learning-based end-to-end navigation for planetary rovers considering non-geometric hazards | |
| CN112650234B (zh) | 一种双足机器人的路径规划方法 | |
| Kumagai et al. | Efficient locomotion planning for a humanoid robot with whole-body collision avoidance guided by footsteps and centroidal sway motion | |
| US20120203420A1 (en) | Method and system for path planning and controlling | |
| US20230418305A1 (en) | Integrated navigation callbacks for a robot | |
| CN114137962B (zh) | 一种基于四足机器人的爬梯方法及系统 | |
| JP7221833B2 (ja) | 非線形モデル予測制御装置 | |
| CN111367291B (zh) | 一种自越障机器人及控制方法 |
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 |