CN114043479A - 一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置 - Google Patents
一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置 Download PDFInfo
- Publication number
- CN114043479A CN114043479A CN202111406310.XA CN202111406310A CN114043479A CN 114043479 A CN114043479 A CN 114043479A CN 202111406310 A CN202111406310 A CN 202111406310A CN 114043479 A CN114043479 A CN 114043479A
- Authority
- CN
- China
- Prior art keywords
- robot
- deviation
- track
- polishing
- pose
- 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
Links
- 238000005498 polishing Methods 0.000 title claims abstract description 83
- 238000000034 method Methods 0.000 title claims abstract description 59
- 230000033001 locomotion Effects 0.000 claims abstract description 62
- 238000012937 correction Methods 0.000 claims abstract description 47
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 44
- 239000002245 particle Substances 0.000 claims description 32
- 230000008569 process Effects 0.000 claims description 21
- 238000006073 displacement reaction Methods 0.000 claims description 12
- 238000009826 distribution Methods 0.000 claims description 9
- 238000002474 experimental method Methods 0.000 description 29
- 230000009191 jumping Effects 0.000 description 18
- QBWCMBCROVPCKQ-UHFFFAOYSA-N chlorous acid Chemical compound OCl=O QBWCMBCROVPCKQ-UHFFFAOYSA-N 0.000 description 8
- 230000000694 effects Effects 0.000 description 8
- 230000003287 optical effect Effects 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 5
- 230000003044 adaptive effect Effects 0.000 description 4
- 238000012360 testing method Methods 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 3
- 230000035772 mutation Effects 0.000 description 3
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 2
- 238000012952 Resampling Methods 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 239000004593 Epoxy Substances 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 239000002969 artificial stone Substances 0.000 description 1
- 230000002567 autonomic effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000004567 concrete Substances 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005315 distribution function Methods 0.000 description 1
- 239000010438 granite Substances 0.000 description 1
- 238000011065 in-situ storage Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 238000007517 polishing process Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000006641 stabilisation Effects 0.000 description 1
- 238000011105 stabilization Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J11/00—Manipulators not otherwise provided for
- B25J11/005—Manipulators for mechanical processing tasks
- B25J11/0065—Polishing or grinding
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
本发明提出一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置，涉及机器人控制技术领域。本发明实施例通过自适应蒙特卡罗算法实时获取机器人位态，根据当前位姿与轨迹超前点求取偏差量，然后将偏差量解算为机器人运动控制指令，最后定位算法获取修正后的位姿，重新计算误差和控制量，形成闭环控制。实验结果表明，基于该控制系统的地坪磨抛机器人能够应对较大的轨迹偏离情况，在处理[0,15]度范围内的角度偏差和[0,22]厘米范围内的横向偏离的情况下，能够快速修正偏移量，保证导航精度。与现有方法相比，本发明实施例提出的轨迹纠偏方法能够应对较大程度的偏移量，有效解决了地坪磨抛机器人的轨迹偏离问题。
Description
技术领域
本发明涉及机器人控制技术领域，特别涉及一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置。
背景技术
地坪磨抛机器人是一种用于地面打磨和抛光的设备，主要应用于混凝土、人造石、花岗岩、环氧地面的处理，通过高速转动的磨盘打磨地面使地面平整光滑，是地坪工程处理的主流设备。因为打磨地面平整度不一致导致打磨过程中机器人导航精度降低；地面凹陷、突起、水渍等摩擦力不一致的因素导致机器人产生跳动，主要表现为航向角偏离和横向侧滑，对机器人高精度路径导航、自主偏差轨迹纠正提出了新的挑战，研究其轨迹矫正自校正算法是提升自主导航地坪磨抛机器人导航精度的关键之一。
由于地面的情况复杂，外界干扰种类多，机器人实际的轨迹偏差复杂多样，因此很难确定精确的机器人数学模型，利用传统的控制理论难以应对实际机器人移动过程中的轨迹纠偏。因此，前人提出多种解决方案用于解决机器人移动过程中的轨迹偏离问题。潘世举等人提出一种基于线性时变模型预测控制的控制策略，与普通的MPC算法对比，通过仿真和实车实验证明该算法的轨迹跟踪误差在理想范围内，缺点是没有充分考虑机器人实时速度的变化，与实际运动情况存在偏差。黄刚提出一种实时修正机器人偏差的方法，并结合粒子群轨迹规划算法应用到实际的麦科勒姆轮机器人上，实现机器人快速，有效的轨迹纠偏，能够处理较小的偏差情况，美中不足的是该实验所处理的轨迹偏差值较小。S.Mata、Li S E等人将模型预测控制用于实际场合，考虑实际车辆运行中的情况设置控制器，具有较好的控制效果，缺点是由于车辆的运动学模型差异每次都需要重新设计控制模型。
发明内容
本发明要解决的技术问题，在于提供一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置，实现快速修正偏移量，保证导航精度。
第一方面，本发明提供了一种误差矫正的地坪磨抛机器人轨迹纠偏方法，包括：
步骤10、获取机器人覆盖的理想轨迹，将轨迹上机器人正前方距离为L的点设置为机器人的轨迹超前点；建立地坪磨抛机器人的运动控制模型；
步骤20、在机器人沿着路径运动过程中，实时获取机器人的运动状态，然后通过自适应蒙特卡洛算法更新机器人的位姿；
步骤30、计算机器人当前位姿与轨迹超前点之间的偏差量，然后根据所述运动控制模型将偏差量转化为机器人的控制指令，控制机器人运动得到新的位姿，然后返回步骤20。
进一步地，所述步骤10中，当应用于两轮差速结构底盘的地坪磨抛机器人时，运动控制模型具体为：
其中，w表示机器人的转动角速度，v表示由驱动轮产生的前进方向速度分量，θ表示磨抛机器人在全局坐标下的方向角度，α表示地坪磨抛机器人受力方向与运动方向的夹角，F表示磨盘受到的来自地面的力，J表示地坪磨抛机器人的转动惯量，m表示地坪磨抛机器人的质量，L为超前距离。
进一步地，所述步骤30中，计算机器人当前位姿与轨迹超前点之间的偏差量，具体为：
ex＝xt-xn
ey＝yt-yn
其中，ex代表X方向距离偏差，ey代表Y方向距离偏差，eθ代表角度偏差，Pn(xn,yn,θn)为机器人当前位姿，Pt(xt,yt,θt)为轨迹超前点。
进一步地，所述步骤30中，将偏差量转化为机器人的控制指令，具体为：
根据PID算法，将角度偏移量eθ以及X轴和Y轴方向的距离偏移量ex、ey分别带入公式，得到两组PID输出；
其中，u(t)为偏差量修正量，kP为比例系数，kI为积分系数，kD为微分系数，当输入角度偏差eθ时输出为角度控制量，当输入为距离偏移量时输出为速度控制量。
进一步地，所述步骤20具体为：在机器人进行正常位移时，自适应蒙特卡洛算法在原有分布的基础上叠加运动学模型更新粒子位姿；在机器人发生跳机的情况下，自适应蒙特卡洛算法重新分布粒子，开始全新一轮的迭代计算，最终得到准确表示机器人真实坐标的结果。
第二方面，本发明提供了一种误差矫正的地坪磨抛机器人轨迹纠偏装置，包括：初始化模块、位姿更新模块以及运动控制模块；
所述初始化模块，用于获取机器人覆盖的理想轨迹，将轨迹上机器人正前方距离为L的点设置为机器人的轨迹超前点；建立地坪磨抛机器人的运动控制模型；
所述位姿更新模块，用于在机器人沿着路径运动过程中，实时获取机器人的运动状态，然后通过自适应蒙特卡洛算法更新机器人的位姿；
所述运动控制模块，用于计算机器人当前位姿与轨迹超前点之间的偏差量，然后根据所述运动控制模型将偏差量转化为机器人的控制指令，控制机器人运动得到新的位姿，然后返回位姿更新模块。
进一步地，所述初始化模块中，当应用于两轮差速结构底盘的地坪磨抛机器人时，运动控制模型具体为：
其中，w表示机器人的转动角速度，v表示由驱动轮产生的前进方向速度分量，θ表示磨抛机器人在全局坐标下的方向角度，α表示地坪磨抛机器人受力方向与运动方向的夹角，F表示磨盘受到的来自地面的力，J表示地坪磨抛机器人的转动惯量，m表示地坪磨抛机器人的质量，L为超前距离。
进一步地，所述运动控制模块中，计算机器人当前位姿与轨迹超前点之间的偏差量，具体为：
ex＝xt-xn
ey＝yt-yn
其中，ex代表X方向距离偏差，ey代表Y方向距离偏差，eθ代表角度偏差，Pn(xn,yn,θn)为机器人当前位姿，Pt(xt,yt,θt)为轨迹超前点。
进一步地，所述运动控制模块中，将偏差量转化为机器人的控制指令，具体为：
根据PID算法，将角度偏移量eθ以及X轴和Y轴方向的距离偏移量ex、ey分别带入公式，得到两组PID输出；
其中，u(t)为偏差量修正量，kP为比例系数，kI为积分系数，kD为微分系数，当输入角度偏差eθ时输出为角度控制量，当输入为距离偏移量时输出为速度控制量。
进一步地，所述位姿更新模块具体为：在机器人进行正常位移时，自适应蒙特卡洛算法在原有分布的基础上叠加运动学模型更新粒子位姿；在机器人发生跳机的情况下，自适应蒙特卡洛算法重新分布粒子，开始全新一轮的迭代计算，最终得到准确表示机器人真实坐标的结果。
本发明实施例中提供的技术方案具有如下技术效果或优点：
本发明实施例采用超前点控制的方法能够保证机器人的轨迹偏差为正，机器人一边进行矫正一边沿着轨迹运行，接着通过自适应蒙特卡罗算法实时获取机器人位态，根据当前位姿与轨迹超前点求取偏差量，然后将偏差量解算为机器人运动控制指令，最后定位算法获取修正后的位姿，重新计算误差和控制量，形成闭环控制。本发明实施例提出的轨迹纠偏方法能够应对较大程度的偏移量，有效解决了地坪磨抛机器人的轨迹偏离问题。
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。
附图说明
下面参照附图结合实施例对本发明作进一步的说明。
图1为本发明实施例一中方法的流程图；
图2为本发明实施例一中地坪磨抛机器人控制系统流程图；
图3为本发明实施例一中磨抛机器人运动学示意图；
图4为本发明实施例一中偏差量计算示意图；
图5为本发明实施例一中两次无扰动实验轨迹示意图；
图6为本发明实施例一中扰动实验轨迹示意图；
图7为本发明实施例一中相对实验偏差示意图；
图8为本发明实施例一中两次干扰实验绝对偏差示意图；
图9为本发明实施例二中装置的结构示意图。
具体实施方式
本申请实施例通过提供一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置，实现快速修正偏移量，保证导航精度。
本发明实施例中的技术方案，总体思路如下：
本发明针对地坪磨抛机器人自主导航过程中抖动和跳机产生轨迹偏差的问题，提出一种基于误差量的轨迹矫正方法，解决由于地面不均匀导致机器人抖动，跳动出现大程度偏离轨迹的问题，进而实现地坪磨抛机器人的自主导航精确控制。
首先规划地坪磨抛机器人覆盖的理想轨迹并设定轨迹超前点。然后，根据机器人当前位姿选择理想轨迹点作为跟踪目标点，通过自适应蒙特卡洛算法(AdaptiveDeterministic Monte Carlo,AMCL)实时获取机器人位置和姿态，从而计算目标点与机器人当前位置存在的偏差，偏差量经过距离、角度两个PID控制器解算得到电机的控制指令，控制机器人运动得到新的位姿。不断循环控制机器人循迹纠偏，实现定位到矫正的闭环系统。
为了精准的控制机器人，纠正机器人跳机、抖动时导致轨迹偏移轨迹，本发明实施例提出一种基于误差量的轨迹纠偏方法，轨迹纠偏方法主要包括机器人定位、超前点误差计算方法、偏差量与控制指令的解算算法。
实施例一
本实施例提供一种误差矫正的地坪磨抛机器人轨迹纠偏方法，如图1所示，包括：
步骤10、获取机器人覆盖的理想轨迹，将轨迹上机器人正前方距离为L的点设置为机器人的轨迹超前点；建立地坪磨抛机器人的运动控制模型；
步骤20、在机器人沿着路径运动过程中，实时获取机器人的运动状态，然后通过自适应蒙特卡洛算法更新机器人的位姿；
步骤30、计算机器人当前位姿与轨迹超前点之间的偏差量，然后根据所述运动控制模型将偏差量转化为机器人的控制指令，控制机器人运动得到新的位姿，然后返回步骤20。
通过超前点控制的方法能够保证机器人的轨迹偏差为正，机器人一边进行矫正一边沿着轨迹运行，接着通过自适应蒙特卡罗算法实时获取机器人位态，根据当前位姿与轨迹超前点求取偏差量，然后将偏差量解算为机器人运动控制指令，最后定位算法获取修正后的位姿，重新计算误差和控制量，形成闭环控制。
本发明的方法可以适用于所有轮式结构的地坪磨抛机器人，图2是地坪磨抛机器人的控制系统流程图。首先规划地坪磨抛机器人覆盖的理想轨迹。然后，根据机器人当前位姿选择理想轨迹点作为跟踪目标点，通过自适应蒙特卡洛算法(Adaptive DeterministicMonte Carlo,AMCL)实时获取机器人位置和姿态，从而计算目标点与机器人当前位置存在的偏差，偏差量经过距离、角度两个PID控制器解算得到电机的控制指令，控制机器人运动得到新的位姿。不断循环控制机器人循迹纠偏，实现定位到矫正的闭环系统。
作为本发明的一种具体实现方式，以采用两轮差速结构底盘的地坪磨抛机器人为对象，两轮差速结构底盘包含公转的大磨盘和自转的小磨盘。其中，通过自转磨盘的相对转动平均来自地面的摩擦受力，而大磨盘的公转可以使受力出现周期性变化，减小受力不均匀的影响。
1、地坪磨抛机器人运动控制模型
为了分析磨抛机器人在磨抛地面时的受力情况，搭建了机器人在磨抛运动过程中的运动控制模型。磨抛机器人某一时刻，以机器人两轮中心为原点建立机器人坐标系xc-yc，绝对坐标系X-Y。如图3所示，其中θ为Y和yc的夹角，机器人在全局坐标系中的姿态可表示为(x,y,θ)。对机器人受力状态展开分析，运动过程中的受力包括磨盘受力和两个驱动轮的动力。F表示磨盘受到的来自地面的力，F的大小和方向随着磨盘的转动不断发生变化，Fx表示F垂直机器人运动方向的分量，Fy表示F沿着机器人运动方向的分量。
Fx作用点与机器人的转向中心距离为L，根据刚体转动定律，受力造成机器人发生刚体转动，如公式2所示。平行于机器人运动方向的受力应用牛顿第二定律公式推导，Fy产生机器人沿着机器人方向的位移动作，如公式3所示。两个力的作用效果相互叠加出现机器人发生角度改变和沿着角度方向的跳动位移。
Jω′＝Fsinα*L (1)
其中，w表示机器人的转动角速度，vy表示机器人前进方向的瞬时速度，α表示地坪磨抛机器人受力方向与运动方向的夹角，J表示地坪磨抛机器人的转动惯量，m表示地坪磨抛机器人的质量。
磨抛机器人的运动控制模型由磨盘动力学模型叠加差速机器人运动学模型组成。差速机器人运动学模型分析需假定机器人在运动过程中不发生侧滑，此时轮子运动为纯滚动无滑动的情况，机器人运动学模型可以用速度和角速度表示，如公式3。
其中，θ表示磨抛机器人在全局坐标下的方向角度，v表示由驱动轮产生的前进方向速度分量(单位：m/s)，w表示由两驱动轮产生的角速度度(单位：rad/s)。
综上分析，通过两轮差速模型的动力学模型公式(1)(2)叠加磨盘受力的运动学模型公式(3)，得到地坪磨抛机器人的运动控制模型公式(4)。
其中，w表示机器人的转动角速度，v表示由驱动轮产生的前进方向速度分量，θ表示磨抛机器人在全局坐标下的方向角度，α表示地坪磨抛机器人受力方向与运动方向的夹角，F表示磨盘受到的来自地面的力，J表示地坪磨抛机器人的转动惯量，m表示地坪磨抛机器人的质量，L为超前距离。
2、轨迹纠偏算法
为了精准的控制机器人，纠正机器人跳机、抖动时导致轨迹偏移轨迹，本实施例提出一种基于误差量的轨迹纠偏方法，轨迹纠偏方法主要包括机器人定位、超前点误差计算方法、偏差量与控制指令的解算算法。
2.1自适应蒙特卡洛定位
在沿着路径运动过程中，由于地面均匀度不同，机器人自身抖动、跳动等因素，对机器人定位提出了新的挑战。传统的绝对定位方案在长距离下表现了较差的鲁棒性，如里程计时常出现信号飘移、IMU产生累积误差，因此采用自适应蒙特卡洛方法(AdaptiveMonte Carlo Localization,AMCL)。AMCL是应用广泛的激光雷达相对定位方法，分为预测阶段、测量阶段和重采样阶段，通过迭代更新粒子的方式得到机器人的准确位姿。
预测阶段：设置粒子参数N，参数大小根据环境地图面积设定；随机散布粒子，根据机器人运动学模型和先验概率分布，随机生成N个粒子；初始时刻每一个粒子的权重如下：
所述测量阶段为：根据上一时刻粒子姿态信息和运动学模型推算产生下一时刻粒子的位姿，遍历每个粒子计算下一时刻的粒子位姿；更新粒子权重，根据建议分布函数与贝叶斯概率分布计算的后验概率密度函数的关系，利用测量数据计算粒子的权重如公式(5)。其中，p(xk 0:t|z1:t,u1:t)表示后验概率，q(xk 0:t|z1:t,u1:t)表示建议分布。
其中，xt k表示N个粒子的集合，上标k表示集合中的第k个粒子，下标t指时刻；p(zt|xt)是观测模型，表示当移动机器人姿态信息xt已知时，观测到zt的概率；p(xt|xt-1,ut)是运动模型，表示移动机器人在控制输入信息的作用下不同状态之间的转移概率，xt-1是机器人上一时刻的位姿，ut表示当前的控制向量，在贝叶斯理论中将这一部分描述为后验概率；分母中的q(xt|xt-1,ut)是p(xt|xt-1,ut)的理论值。
所述重采样阶段为：根据wt k的大小来选择保留或者舍弃该粒子，丢弃权重小的粒子，保留权重高的粒子，并且加入随机粒子来规避粒子绑架问题，提高算法鲁棒性。经过迭代，最终粒子集中在一个区域，这个区域即机器人此时的坐标Pr(xr,yr,θr)。
自适应蒙特卡洛算法是一种相对定位的方法，利用激光雷达数据，迭代更新位姿有效避免了磨抛机器人抖动、跳机导致的定位误差累加、定位失灵的情况。正常情况下，机器人产生位移时，算法在原有分布的基础上叠加运动学模型更新粒子位姿。在发生跳机的情况下，算法重新分布粒子，开始全新一轮的迭代计算，最终准确表示机器人真实坐标，因此之前的定位结果不会对跳机之后的定位产生影响，不会造成误差累加。
2.2偏差量获取
在沿着路径运动过程中，由于机器人打磨地面造成的抖动、跳动等问题，出现实际运行轨迹偏离理想轨迹的情况。不加矫正会造成后续轨迹完全偏离，导致出现更加严重的生产事故。机器人出现跳动的情况比较复杂，由于其受力方向和大小的不确定性，导致其可能发生的跳动情况比较多，可能出现前向跳机，后向跳机，侧向跳机等多种情况。为了保证在出现跳机的同时能够继续控制机器人继续沿着轨迹运行，不出现原地矫正或倒车的情况，保证控制的连续性和路径的平滑性，使用超前点控制的方法。超前点是指超前于机器人一个固定距离的路径对应点。将轨迹上机器人正前方距离为L的点设置为机器人的跟踪点，从而保证机器人跟踪点与定位点之间的误差值始终为正，能够控制机器人矫正过程中继续朝着超前点移动。
机器人跳机的情况复杂，其中前向跳动和后向跳动的误差较为简单，最为复杂的是发生侧向跳机的情况，如图4所示。发生侧向跳机时，机器人的坐标由Pr(xr,yr,θr)突变为Pn(xn,yn,θn)。此时，传统的矫正方法机器人会发生原地转向调整，造成了轨迹的断续。采用超前点控制的方法能够保证机器人的轨迹偏差为正，机器人一边进行矫正一边沿着轨迹运行。此时机器人与超前点的偏差由AMCL计算定位Pn(xn,yn,θn)和机器人轨迹超前点Pt(xt,yt,θt)。用(ex,ey,eθ)表示机器人位置与超前点的偏差。其中：包括X方向距离偏差ex、Y方向距离偏差ey，以及角度偏差eθ。
ex＝xt-xn (7)
ey＝yt-yn (8)
使用当前位置与超前点的偏差计算方式，不论是出现哪种跳机工况的都能够保证偏差值为正，此时既保证了机器人运行的连续性，又保证了控制的鲁棒性。
2.3偏移量修正
偏移量的修正实际是将偏差量转化为机器人的控制指令，本实施例结合经典PID算法设计距离、角度控制器，如公式(10)。角度PID控制器进行机器人航向角的控制，距离PID控制器输出机器人速度指令。该控制方法将速度与角度分别控制，在误差出现时可以快速矫正，能够更加灵活的应对不同情况下的偏差。
根据PID算法，将角度偏移量eθ以及X轴和Y轴方向的距离偏移量ex、ey分别带入公式(10)，得到两组PID输出；
其中，u(t)为偏差量修正量，kP为比例系数，kI为积分系数，kD为微分系数，当输入角度偏差eθ时输出为角度控制量，当输入为距离偏移量时输出为速度控制量。
超前点的误差计算方法有助于保证运行轨迹的平滑性，PID算法快速跟踪误差量，两种算法保证了机器人在轨迹矫正的过程中，保证轨迹跟踪的平滑性、和误差跟踪的快速性。
3、实验与结果分析
轨迹矫正实验是在安装Vicon光学轨迹捕捉系统的空间下进行，实现并记录机器人的移动轨迹。实验设备包含Vicon光学轨迹捕捉系统和地坪磨抛机器人平台。其中，使用7个Vicon光学捕捉镜头能够实时捕捉、记录空间中光学标记物的轨迹。光学反光球用于标记机器人，Vicon镜头捕捉光学反光球并建立对应机器人刚体，通过刚体反应机器人姿态。7个捕捉摄像机捕捉到的位置数据通过Vicon自身的交换机汇总到Tracker中，Tracker会对每个摄像机的数据处理然后可视化，并通过局域网共享数据，最终经过Matlab、Numpy进行分析处理。
轨迹自校正实验的基本参数设置如表1所示，其中Vicon光学镜头分辨率130万，频率120Hz，能够捕捉高速移动目标的移动轨迹。地坪磨抛机器伺服电机额定转速3000r/min，最大线速度为每秒1m/s，最大角速度每秒1rad/s，磨盘转速每分钟2700rad/s。使用北阳激光雷达UTM-30LX，有效扫描范围0.1-30m，距离分辨率0.001m，有效扫描角度270°，角度分辨率0.25°。
表1机器人和Vicon基本参数设置
3.1无扰动真值轨迹实验
进行两组无扰动误差实验，并以第一次无扰动实验作为轨迹真值，后续实验对照均以第一组无扰动实验为基础。实验中参数设置如表2所示，设置机器人最大速度0.3m/s，机器人超前点距离为0.3m，机器人最大角速度0.25rad/s，以及相关PID参数。
表2机器人轨迹跟踪参数设置
分别在相同实验环境下进行两次正常工况下的地坪磨抛机器人轨迹实验，测量实验设备的误差。其中，U字形轨迹横向位移2m、纵向位移1m，记录实验轨迹如图5，机器人出发点(-1.472，-1.213)，终点(-1.618，-0.201)。实验结果可以看出，在没有扰动的情况下，两次实验的轨迹基本重合，在拐角处出现较大偏离，原因是磨抛机器人体积庞大，且转弯中心和磨盘受力点不在同一点上，属于正常现象。此时两条轨迹的偏差平均值作为对照值。
3.2扰动实验
地坪磨抛机器人在生产过程中，产生的跳动和角度突变处于一个较小的范围内，大约在(10，-10)的角度范围内，横向跳机小于10cm。为了测试极端情况下的轨迹矫正效果，通过人为模拟机器人受力进行两组大程度的扰动实验，第一次测试机器人在出现横向跳动的时候的矫正效果，实验中在出发0.25m处横向推动机器人，产生一个横向作用力，使机器人产生横向跳动位移，经过计算位移为22cm。第二次测试机器人在产生角度偏离时的轨迹矫正效果，在机器人出发0.5m处施加一个外力使机器人产生刚体转动，经过计算该角度偏差15度，测试其轨迹自校正能力，实验结果如图6所示。
通过实验分析可以得到，两次扰动最终机器人都能完成轨迹的矫正，表现出较好的轨迹自校正能力，能够纠正大角度偏差和横向跳动的情况。机器人在经过直线矫正和直角转弯后基本矫正完成，此时轨迹的误差较小，机器人轨迹与无扰动轨迹贴近。在第二个拐角(0.237，-0.05104)附近由于机器人航向角未完成矫正，此时偏差增大的趋势。转弯完成后偏差逐渐减小，再经过1m的直线矫正，机器人回归轨迹，到达(-1.557，-0.163)处矫正完成，此时误差趋近于为零，后续轨迹与无扰动实验轨迹重合。
3.3评价指标
轨迹跟踪评估指标用机器人轨迹偏差表示。轨迹偏差指的是两条轨迹的偏离度，计算轨迹之间的偏离程度，以第一次无扰动轨迹作为轨迹真值，第二条轨迹与真值之间的欧式距离公式(11)表示。
将第一次无扰动实验作轨迹真值，求出后续三组实验对其轨迹的偏差如图7所示，实线是添加横向22cm跳动时的误差情况，点划线表示添加15度偏差时的轨迹误差情况，虚线表示第二次无扰动实验的误差情况。
实验结果可见，无扰动实验的偏差小于0.025m。在添加扰动的一段时间内，轨迹误差持续增大，此时由于误差较大，AMCL定位算法处于迭代阶段，尚未准确识别机器人位姿。后误差增加的速度快速降低，此时开始矫正轨迹，误差最大时达到0.34m，之后误差快速降低，此时机器人已经准确定位且快速矫正。当添加横向移动偏差时，机器人快速定位偏差并矫正轨迹，误差不断较小。机器人经过扰动后快速进行误差矫正，最终误差在一个较小的范围内调整，在第二个拐角后快速较小误差，最终轨轨迹矫正完成。过程中机器人经过1.5m到达第一次拐角处时误差已经小于0.05m，后续出现波动。从误差开始到矫正结束总共经过2.5m的路程，最终误差处于约等于无扰动实验误差。取轨迹稳定后的500个误差数据，计算器矫正后稳态误差数据及整个过程中最大误差如表3给出。
表3实验误差
| 组别 | 稳态误差均值 | 稳态最小误差 | 稳态误差方差 | 实验最大误差 |
| 角度偏航(m) | 0.0288 | 0.0003 | 0.0182 | 0.3464 |
| 横向跳机(m) | 0.0110 | 0.0072 | 0.0044 | 0.1788 |
| 无扰动(m) | 0.0096 | 0.0056 | 0.0012 | 0.0361 |
机器人无扰动试验下其稳态误差均值为0.0096m，该误差来源于设备的不稳定性。加入横向跳机扰动后，机器人最大偏差0.1788m经过2.5m的矫正后，机器人稳态误差约0.0110m，加入角度偏航后机器人最大偏差约0.3464m，机器人经过2.5m的矫正后，稳态误差约0.0288m，矫正效果明显。
实际工作中，在地坪磨抛机器人产生的跳动和角度突变一般处于一个较小的范围内，极少出现实验中设置的极端条件。图8为长距离下(42米)地坪磨抛机器人工作轨迹，当产生轨迹偏差时，机器人可以即时矫正位姿，整体轨迹效果较好、精度较高。因此，本实施例提出的轨迹自校正算法能够很好的解决自主导航地坪磨抛机器人实际工作中的问题。
本实施例针对地坪磨抛机器人自主导航过程中抖动和跳机产生轨迹偏差的问题，提出一种基于误差量的轨迹矫正方法。解决由于地面不均匀导致机器人抖动，跳动出现大程度偏离轨迹的问题，进而实现地坪磨抛机器人的自主导航精确控制。为了验证算法，本实施例借助Vicon光学捕捉系统进行实验，分别对机器人22cm的横向跳动，15度大角度偏差进行实验，最终经过2.5m路程的矫正后轨迹偏差小于0.03m，属于正常范围。在长距离的实际场景中，机器人也能较好地应对轨迹偏离问题。因此，本实施例提供的方法能够矫正地坪磨抛机器人自主导航过程中的轨迹偏差，具有较高导航精度。
基于同一发明构思，本申请还提供了与实施例一中的方法对应的装置，详见实施例二。
实施例二
在本实施例中提供了一种误差矫正的地坪磨抛机器人轨迹纠偏装置，如图9所示，包括：初始化模块、位姿更新模块以及运动控制模块；
所述初始化模块，用于获取机器人覆盖的理想轨迹，将轨迹上机器人正前方距离为L的点设置为机器人的轨迹超前点；建立地坪磨抛机器人的运动控制模型；
所述位姿更新模块，用于在机器人沿着路径运动过程中，实时获取机器人的运动状态，然后通过自适应蒙特卡洛算法更新机器人的位姿；
所述运动控制模块，用于计算机器人当前位姿与轨迹超前点之间的偏差量，然后根据所述运动控制模型将偏差量转化为机器人的控制指令，控制机器人运动得到新的位姿，然后返回位姿更新模块。
本实施例的一种具体实现方式：
所述初始化模块中，当应用于两轮差速结构底盘的地坪磨抛机器人时，运动控制模型具体为：
其中，w表示机器人的转动角速度，v表示由驱动轮产生的前进方向速度分量，θ表示磨抛机器人在全局坐标下的方向角度，α表示地坪磨抛机器人受力方向与运动方向的夹角，F表示磨盘受到的来自地面的力，J表示地坪磨抛机器人的转动惯量，m表示地坪磨抛机器人的质量，L为超前距离。
所述运动控制模块中，计算机器人当前位姿与轨迹超前点之间的偏差量，具体为：
ex＝xt-xn
ey＝yt-yn
其中，ex代表X方向距离偏差，ey代表Y方向距离偏差，eθ代表角度偏差，Pn(xn,yn,θn)为机器人当前位姿，Pt(xt,yt,θt)为轨迹超前点。
所述运动控制模块中，将偏差量转化为机器人的控制指令，具体为：
根据PID算法，将角度偏移量eθ以及X轴和Y轴方向的距离偏移量ex、ey分别带入公式，得到两组PID输出；
其中，u(t)为偏差量修正量，kP为比例系数，kI为积分系数，kD为微分系数，当输入角度偏差eθ时输出为角度控制量，当输入为距离偏移量时输出为速度控制量。
所述位姿更新模块具体为：在机器人进行正常位移时，自适应蒙特卡洛算法在原有分布的基础上叠加运动学模型更新粒子位姿；在机器人发生跳机的情况下，自适应蒙特卡洛算法重新分布粒子，开始全新一轮的迭代计算，最终得到准确表示机器人真实坐标的结果。
由于本发明实施例二所介绍的装置，为实施本发明实施例一的方法所采用的装置，故而基于本发明实施例一所介绍的方法，本领域所属人员能够了解该装置的具体结构及变形，故而在此不再赘述。凡是本发明实施例一的方法所采用的装置都属于本发明所欲保护的范围。
虽然以上描述了本发明的具体实施方式，但是熟悉本技术领域的技术人员应当理解，我们所描述的具体的实施例只是说明性的，而不是用于对本发明的范围的限定，熟悉本领域的技术人员在依照本发明的精神所作的等效的修饰以及变化，都应当涵盖在本发明的权利要求所保护的范围内。
Claims (10)
1.一种误差矫正的地坪磨抛机器人轨迹纠偏方法，其特征在，包括：
步骤10、获取机器人覆盖的理想轨迹，将轨迹上机器人正前方距离为L的点设置为机器人的轨迹超前点；建立地坪磨抛机器人的运动控制模型；
步骤20、在机器人沿着路径运动过程中，实时获取机器人的运动状态，然后通过自适应蒙特卡洛算法更新机器人的位姿；
步骤30、计算机器人当前位姿与轨迹超前点之间的偏差量，然后根据所述运动控制模型将偏差量转化为机器人的控制指令，控制机器人运动得到新的位姿，然后返回步骤20。
2.根据权利要求1所述的方法，其特征在于：所述步骤10中，当应用于两轮差速结构底盘的地坪磨抛机器人时，运动控制模型具体为：
其中，w表示机器人的转动角速度，v表示由驱动轮产生的前进方向速度分量，θ表示磨抛机器人在全局坐标下的方向角度，α表示地坪磨抛机器人受力方向与运动方向的夹角，F表示磨盘受到的来自地面的力，J表示地坪磨抛机器人的转动惯量，m表示地坪磨抛机器人的质量，L为超前距离。
3.根据权利要求2所述的方法，其特征在于：所述步骤30中，计算机器人当前位姿与轨迹超前点之间的偏差量，具体为：
ex＝xt-xn
ey＝yt-yn
其中，ex代表X方向距离偏差，ey代表Y方向距离偏差，eθ代表角度偏差，Pn(xn,yn,θn)为机器人当前位姿，Pt(xt,yt,θt)为轨迹超前点。
4.根据权利要求2或3所述的方法，其特征在于：所述步骤30中，将偏差量转化为机器人的控制指令，具体为：
根据PID算法，将角度偏移量eθ以及X轴和Y轴方向的距离偏移量ex、ey分别带入公式，得到两组PID输出；
其中，u(t)为偏差量修正量，kP为比例系数，kI为积分系数，kD为微分系数，当输入角度偏差eθ时输出为角度控制量，当输入为距离偏移量时输出为速度控制量。
5.根据权利要求1所述的方法，其特征在于：所述步骤20具体为：在机器人进行正常位移时，自适应蒙特卡洛算法在原有分布的基础上叠加运动学模型更新粒子位姿；在机器人发生跳机的情况下，自适应蒙特卡洛算法重新分布粒子，开始全新一轮的迭代计算，最终得到准确表示机器人真实坐标的结果。
6.一种误差矫正的地坪磨抛机器人轨迹纠偏装置，其特征在于，包括：初始化模块、位姿更新模块以及运动控制模块；
所述初始化模块，用于获取机器人覆盖的理想轨迹，将轨迹上机器人正前方距离为L的点设置为机器人的轨迹超前点；建立地坪磨抛机器人的运动控制模型；
所述位姿更新模块，用于在机器人沿着路径运动过程中，实时获取机器人的运动状态，然后通过自适应蒙特卡洛算法更新机器人的位姿；
所述运动控制模块，用于计算机器人当前位姿与轨迹超前点之间的偏差量，然后根据所述运动控制模型将偏差量转化为机器人的控制指令，控制机器人运动得到新的位姿，然后返回位姿更新模块。
7.根据权利要求6所述的装置，其特征在于：所述初始化模块中，当应用于两轮差速结构底盘的地坪磨抛机器人时，运动控制模型具体为：
其中，w表示机器人的转动角速度，v表示由驱动轮产生的前进方向速度分量，θ表示磨抛机器人在全局坐标下的方向角度，α表示地坪磨抛机器人受力方向与运动方向的夹角，F表示磨盘受到的来自地面的力，J表示地坪磨抛机器人的转动惯量，m表示地坪磨抛机器人的质量，L为超前距离。
8.根据权利要求7所述的装置，其特征在于：所述运动控制模块中，计算机器人当前位姿与轨迹超前点之间的偏差量，具体为：
ex＝xt-xn
ey＝yt-yn
其中，ex代表X方向距离偏差，ey代表Y方向距离偏差，eθ代表角度偏差，Pn(xn,yn,θn)为机器人当前位姿，Pt(xt,yt,θt)为轨迹超前点。
9.根据权利要求7或8所述的装置，其特征在于：所述运动控制模块中，将偏差量转化为机器人的控制指令，具体为：
根据PID算法，将角度偏移量eθ以及X轴和Y轴方向的距离偏移量ex、ey分别带入公式，得到两组PID输出；
其中，u(t)为偏差量修正量，kP为比例系数，kI为积分系数，kD为微分系数，当输入角度偏差eθ时输出为角度控制量，当输入为距离偏移量时输出为速度控制量。
10.根据权利要求6所述的装置，其特征在于：所述位姿更新模块具体为：在机器人进行正常位移时，自适应蒙特卡洛算法在原有分布的基础上叠加运动学模型更新粒子位姿；在机器人发生跳机的情况下，自适应蒙特卡洛算法重新分布粒子，开始全新一轮的迭代计算，最终得到准确表示机器人真实坐标的结果。
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111406310.XA CN114043479A (zh) | 2021-11-24 | 2021-11-24 | 一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置 |
| PCT/CN2022/132273 WO2023093594A1 (zh) | 2021-11-24 | 2022-11-16 | 一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111406310.XA CN114043479A (zh) | 2021-11-24 | 2021-11-24 | 一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114043479A true CN114043479A (zh) | 2022-02-15 |
Family
ID=80210618
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111406310.XA Pending CN114043479A (zh) | 2021-11-24 | 2021-11-24 | 一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置 |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN114043479A (zh) |
| WO (1) | WO2023093594A1 (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115014352A (zh) * | 2022-06-01 | 2022-09-06 | 浙江大学 | 一种基于建议分布地图的室内全局定位方法 |
| CN115990880A (zh) * | 2023-01-04 | 2023-04-21 | 佛山市顺德区一拓电气有限公司 | 机器人航向调整方法、机器人、装置及计算机存储介质 |
| WO2023093594A1 (zh) * | 2021-11-24 | 2023-06-01 | 泉州装备制造研究所 | 一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20050104548A1 (en) * | 2002-05-01 | 2005-05-19 | Toru Takenaka | Attitude control device of mobile robot |
| CN110286683A (zh) * | 2019-07-15 | 2019-09-27 | 北京科技大学 | 一种履带式移动机器人的自主行驶路径跟踪控制方法 |
| CN113211438A (zh) * | 2021-05-08 | 2021-08-06 | 东方红卫星移动通信有限公司 | 基于预瞄距离自适应的轮式机器人控制方法及控制系统 |
| CN113618743A (zh) * | 2021-08-27 | 2021-11-09 | 北京航空航天大学杭州创新研究院 | 一种针对多源干扰的无人机机械臂末端位姿控制方法 |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| RU2661964C2 (ru) * | 2016-04-12 | 2018-07-23 | Федеральное государственное бюджетное учреждение науки Институт автоматики и процессов управления Дальневосточного отделения Российской академии наук (ИАПУ ДВО РАН) | Способ автоматического формирования гладких траекторий движения мобильного робота в неизвестном окружении |
| CN111890350A (zh) * | 2020-06-12 | 2020-11-06 | 深圳先进技术研究院 | 机器人及其控制方法、计算机可读存储介质 |
| CN112356844A (zh) * | 2020-11-19 | 2021-02-12 | 苏州智加科技有限公司 | 一种控制车辆行驶方向的方法、装置及设备 |
| CN114043479A (zh) * | 2021-11-24 | 2022-02-15 | 泉州装备制造研究所 | 一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置 |
-
2021
- 2021-11-24 CN CN202111406310.XA patent/CN114043479A/zh active Pending
-
2022
- 2022-11-16 WO PCT/CN2022/132273 patent/WO2023093594A1/zh unknown
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20050104548A1 (en) * | 2002-05-01 | 2005-05-19 | Toru Takenaka | Attitude control device of mobile robot |
| CN110286683A (zh) * | 2019-07-15 | 2019-09-27 | 北京科技大学 | 一种履带式移动机器人的自主行驶路径跟踪控制方法 |
| CN113211438A (zh) * | 2021-05-08 | 2021-08-06 | 东方红卫星移动通信有限公司 | 基于预瞄距离自适应的轮式机器人控制方法及控制系统 |
| CN113618743A (zh) * | 2021-08-27 | 2021-11-09 | 北京航空航天大学杭州创新研究院 | 一种针对多源干扰的无人机机械臂末端位姿控制方法 |
Non-Patent Citations (1)
| Title |
|---|
| 王宁等: "一种改进的AMCL机器人定位方法", 导航定位学报, vol. 7, no. 3, 31 December 2019 (2019-12-31), pages 31 - 37 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2023093594A1 (zh) * | 2021-11-24 | 2023-06-01 | 泉州装备制造研究所 | 一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置 |
| CN115014352A (zh) * | 2022-06-01 | 2022-09-06 | 浙江大学 | 一种基于建议分布地图的室内全局定位方法 |
| CN115990880A (zh) * | 2023-01-04 | 2023-04-21 | 佛山市顺德区一拓电气有限公司 | 机器人航向调整方法、机器人、装置及计算机存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| WO2023093594A1 (zh) | 2023-06-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114043479A (zh) | 一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置 | |
| CN106041926B (zh) | 一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法 | |
| CN108381553B (zh) | 一种用于空间非合作目标捕获的相对导航近距离跟踪方法及系统 | |
| CN103009389B (zh) | 一种冗余空间机械臂在轨抓捕的轨迹规划方法 | |
| CN104483977B (zh) | 一种舵机云台的无速度反馈动态面控制方法及控制器 | |
| CN110955262B (zh) | 光伏组件清洁机器人的路径规划与跟踪的控制方法及系统 | |
| CN111578937A (zh) | 同时优化外参数的视觉惯性里程计系统 | |
| CN106354147B (zh) | 一种基于发动机预摆的大质心横移下的姿轨控耦合控制系统与方法 | |
| CN109960145B (zh) | 移动机器人混合视觉轨迹跟踪策略 | |
| CN107621266B (zh) | 基于特征点跟踪的空间非合作目标相对导航方法 | |
| CN106950999B (zh) | 一种采用自抗扰控制技术的移动舞台轨迹跟踪控制方法 | |
| CN112198885B (zh) | 一种满足机动平台自主降落需求的无人机控制方法 | |
| CN108519736A (zh) | 一种usv轨迹跟踪反步滑模控制方法 | |
| CN103878770A (zh) | 基于速度估计的空间机器人视觉时延误差补偿方法 | |
| Sanchez-Lopez et al. | Visual marker based multi-sensor fusion state estimation | |
| CN115480583B (zh) | 飞行作业机器人的视觉伺服跟踪与阻抗控制方法 | |
| Wang et al. | Vision-based tracking control of nonholonomic mobile robots without position measurement | |
| CN108681324A (zh) | 基于全局视觉的移动机器人轨迹跟踪控制方法 | |
| Liu et al. | High altitude monocular visual-inertial state estimation: Initialization and sensor fusion | |
| CN112650217B (zh) | 一种基于评估函数的机器人轨迹跟踪策略动态优化方法 | |
| CN111251303B (zh) | 一种周期性姿态调整的机器人运动控制方法 | |
| CN115562304A (zh) | 一种基于深度点云的移动机器人视觉伺服控制方法和系统 | |
| Srivastava et al. | Range estimation and visual servoing of a dynamic target using a monocular camera | |
| WO2022161271A1 (zh) | 斜坡定位校正方法、装置、机器人及可读存储介质 | |
| CN110231823B (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 |