CN115718494A - 移动机器人的参数校准方法和移动机器人 - Google Patents
移动机器人的参数校准方法和移动机器人 Download PDFInfo
- Publication number
- CN115718494A CN115718494A CN202211461016.3A CN202211461016A CN115718494A CN 115718494 A CN115718494 A CN 115718494A CN 202211461016 A CN202211461016 A CN 202211461016A CN 115718494 A CN115718494 A CN 115718494A
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- coordinates
- encoder
- path
- coordinate
- 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
- 238000000034 method Methods 0.000 title claims abstract description 59
- 238000009434 installation Methods 0.000 claims abstract description 39
- 230000008569 process Effects 0.000 claims abstract description 15
- 238000001914 filtration Methods 0.000 claims abstract description 10
- 238000005259 measurement Methods 0.000 claims description 11
- 238000004590 computer program Methods 0.000 claims description 7
- 238000001514 detection method Methods 0.000 claims description 4
- 238000004422 calculation algorithm Methods 0.000 abstract description 13
- 230000006870 function Effects 0.000 description 9
- 239000011159 matrix material Substances 0.000 description 8
- 238000010586 diagram Methods 0.000 description 7
- 230000008859 change Effects 0.000 description 3
- 238000004519 manufacturing process Methods 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 238000012795 verification Methods 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000012549 training Methods 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 235000019800 disodium phosphate Nutrition 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000011900 installation process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种移动机器人的参数校准方法和移动机器人，移动机器人上设有转向编码器和激光雷达，方法包括：控制移动机器人直线行走，并记录移动机器人在行走过程中N个位置点的第一坐标；根据N个位置点的第一坐标对转向编码器的偏移参数进行校准；待转向编码器校准完成后，控制移动机器人按照预设路径行走，并记录移动机器人行走过程中的编码器增量和激光雷达的位姿观测数据，其中，预设路径包括前进路径、后退路径和调头路径；利用扩展卡尔曼滤波算法根据增量和位姿观测数据得到位姿目标数据，并记录两次经过预设路径中目标站点时的第二坐标，根据第二坐标对激光雷达的安装参数进行校准。该方法简单易实现，可实现自动化校准，人工成本低。
Description
技术领域
本发明涉及机器人技术领域，尤其涉及一种移动机器人的参数校准方法和移动机器人。
背景技术
机器人(Robot)是一种能够半自主或全自主工作的智能机器，能够通过编程和自动控制来执行诸如作业或移动等任务。移动机器人如AGV小车(Automated GuidedVehicle，自动导引小车)，作为机器人的一种，是指装备有电磁或光学等自动导航装置，能够沿规定的导航路径行驶，具有安全保护以及各种移载功能的机器人。
移动机器人的转向编码器偏移参数即零位偏移值(Steer Encoder Offset)、激光雷达的安装角度(称Laser Angle)、激光雷达的安装横向值(Laser X)和激光雷达的安装纵向值(Laser Y)等的准确性直接影响移动机器人的稳定性和可靠性。Steer EncoderOffset与转向编码器实际安装零位偏移偏差过大将会导致移动机器人高速直线运行时走S型；Laser Angle与激光雷达实际安装角度偏差过大时将会导致移动机器人追踪样条曲线时超出安全区域，偏离预设轨道；Laser X和Laser Y与激光雷达实际安装横纵向位置偏差过大时将影响多移动机器人运行精度和停止精度。并且，受制造和安装工艺等的影响，制造出的移动机器人实际物理位姿与设计存在偏差，因此，移动机器人出厂前需要对上述参数进行校准。
发明内容
本发明的目的在于提出一种移动机器人的参数校准方法和移动机器人，以简单方便的实现对移动机器人参数的自动化校准。
第一方面，本发明提出了一种移动机器人的参数校准方法，所述移动机器人上设有转向编码器和激光雷达，所述方法包括：控制所述移动机器人直线行走，并记录所述移动机器人在行走过程中N个位置点的第一坐标，其中，N为大于等于3的整数；根据所述N个位置点的第一坐标对所述转向编码器的偏移参数进行校准；待所述转向编码器校准完成后，控制所述移动机器人按照预设路径行走，并记录所述移动机器人行走过程中的编码器增量和所述激光雷达的位姿观测数据，其中，所述预设路径包括前进路径、后退路径和调头路径；利用扩展卡尔曼滤波算法根据所述增量和所述位姿观测数据得到位姿目标数据，基于所述位姿目标数据记录两次经过所述预设路径中目标站点时的第二坐标，以及根据所述第二坐标对所述激光雷达的安装参数进行校准。
第二方面，本发明提出了一种移动机器人，包括控制器，所述控制器包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述第一方面的方法。
本发明实施例的移动机器人的参数校准方法和移动机器人，控制移动机器人直线运动，通过N次停车位置校准转向编码器的偏移参数；在转向编码器的偏移参数校准完成后，控制移动机器人按照预设路径运动，基于扩展卡尔曼滤波算法，在定位的同时对激光雷达的安装偏差进行迭代校准，通过激光雷达不断地测量更新运动结果来校准激光雷达的安装参数。整个过程操作简单，可实现自动化校准，人工成本低。
本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
图1是本发明实施例的移动机器人的参数校准方法的流程图；
图2是本发明一些实施例的移动机器人行走路线的示意图；
图3是本发明一个示例的预设路径的示意图；
图4是本发明一些实施例的移动机器人转向编码器状态的示意图；
图5是本发明实施例的移动机器人的结构框图。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
考虑到影响移动机器人定位的原因包括Steer Encoder Offset、Laser Angle、Laser Y、Laser X的偏差影响，如果单纯采用EKF(Extended Kalman Filter，扩展卡尔曼滤波)方法校准这四个参数，则由于待估计参数过多，且不同的误差组合可能导致相同的观测量，即通过观测信息不能区分多组不同组合偏差，将导致有限的观测信息不足以对这些误差进行估计。考虑到Steer Encoder Offset的校准不依赖激光雷达的安装偏差，但激光雷达的安装偏差校准依赖于转向编码器的零位偏移，本发明提供了一种采用控制移动机器人走直线，通过判断移动机器人三个停止点是否在同一条直线上来校准Steer EncoderOffset，然后使用EKF算法校准激光雷达的安装偏差。下面参考附图描述本发明实施例的移动机器人的参数校准方法和移动机器人。
图1是本发明实施例的移动机器人的参数校准方法。
在该实施例中，移动机器人上设有转向编码器和激光雷达。如图1所示，方法包括：
S1，控制移动机器人直线行走，以及记录移动机器人在行走过程中上N个位置点的第一坐标，其中，N为大于等于3的整数。
具体地，移动机器人的转向编码器可包括舵轮的转向编码器，还可包括差速驱动轮的两行走电机的行走转向编码器。其中，若转向编码器为舵轮的转向编码器时，在控制移动机器人直线行走之前，需将舵轮的角度设为零度。
S2，根据N个位置点的第一坐标对转向编码器的偏移参数进行校准。
其中，转向编码器的偏移参数可包括舵轮的转向编码器的零位偏移量，还可包括差速轮两个驱动轮的行走转向编码器的比例偏移量。
具体地，可判断前N-1个位置点的实际坐标与第N个位置点的测量坐标是否在同一条直线上，若是则说明移动机器人的实际行走路线是直线，此时转向编码器没有偏移量，无需进行校准，否则需重复执行S1。
在一个示例中，如图2所示，N＝3，虚线为移动机器人的实际行走路线，实线为N个位置点的实际坐标所在的目标路线(为直线)。控制移动机器人直线行走时，根据第3个位置点可能会判断移动机器人存在三种情况：情况一，移动机器人左偏；情况一，移动机器人右偏；情况三，移动机器人不偏。
S3，待转向编码器校准完成后，控制移动机器人按照预设路径行走，并记录移动机器人行走过程中的编码器增量和激光雷达的位姿观测数据，其中，预设路径包括前进路径、后退路径和调头路径。
其中，编码器增量包括转向编码器增量和行走编码器增量。
在一些实施例中，如图3所示，控制移动机器人按照预设路径行走，可包括：控制移动机器人直线前进第一路径段(即图3中的Seg2)，直线后退第二路径段(即图3中的Seg7)，第一方向曲线前进第三路径段(即图3中的Seg4)，第二方向曲线后退第四路径段(即图3中的Seg5)，直线后退第五路径段(即图3中的Seg6)，直线前进第六路径段(即图3中的Seg9)，直线前进第七路径段(即图3中的Seg8)，其中，第一路径段和第二路径段为同一路径段，第五路径段和第六路径段为同一路径段，第七路径段为连接在第一路径段和第六路径段之间的路径段；其中，两个第二坐标包括移动机器人直线前进第一路径段停止时的测量坐标和直线前进第七路径段停止时的测量坐标。
S4，利用扩展卡尔曼滤波算法根据增量和位姿观测数据得到位姿目标数据，基于位姿目标数据记录两次经过预设路径中目标站点时的第二坐标，以及根据第二坐标对激光雷达的安装参数进行校准。
具体地，可根据当前时刻的编码器增量，推算出移动机器人当前时刻的世界坐标系位姿，从而推算出激光雷达当前时刻的位姿，得到位姿预测值，根据激光雷达当前的观测值(即位姿检测数据)，运行EKF算法，通过观测值对预测值不断进行修正，得到修正结果(即位姿目标数据)。在该过程中，状态空间中的激光雷达的安装误差参数也会跟着变化，并最终收敛。
该方法操作简单，可减少人工手动校准参数生产的培训和时间成本，避免人工误分析导致校准的数据随机误差大，且适用于单舵轮、双舵轮、单雷达、多雷达等移动机器人的校准，适用范围广。
在一些示例中，如图2所示，N＝3，控制移动机器人直线行走，以及记录移动机器人在行走过程中N个位置点的第一坐标，可包括：控制移动机器人从第一位置点开始直线行走，并记录第一位置点(即图2中的站点11)的实际坐标(x11,y11)；在移动机器人的行走参数满足第一预设条件行时，控制移动机器人停止，并记录第一预设条件对应的在第二位置点(即图2中的站点21)的实际坐标(x21,y21)；控制移动机器人从第二位置点继续直线行走，并在移动机器人的行走参数满足第二预设条件时，控制移动机器人停止，并记录第二预设条件对应的第三位置点(即图2中的站点30)的实际坐标(x30,y30)和测量坐标(x31,y31)。其中，N个位置点包括第一位置点、第二位置点和第三位置点，N个位置点的第一坐标包括第一位置点的实际坐标、第二位置点的实际坐标、第三位置点的实际坐标和测量坐标。其中，参见图2，站点11的实际坐标(x11,y11)、站点21的实际坐标(x21,y21)和站点30的实际坐标(x30,y30)在同一条直线(即图2中的实线)上，该直线为控制移动机器人直线行走的目标路线，站点30的测量坐标(x31,y31)为移动机器人实际行走时，最后停止位置(即图2中点31)的坐标。
在一些实施方式中，移动机器人上设有行走编码器，行走参数包括行走编码器的当前值，第一预设条件为当前值与行走编码器的初始值之间的差值大于第一差值阈值(如5000)，此时理论上应直线行走至第二位置点；第二预设条件为当前值与行走编码器的初始值之间的差值大于第二差值阈值(如10000)，此时理论上应直线行走至第三位置点。其中，第二差值阈值大于第一差值阈值。
具体地，根据上述的行走编码器的当前值和初始值可计算出移动机器人的行走距离，第一差值阈值可对应移动机器人行走第一预设距离(如5000mm)，第二差值阈值可对应移动机器人的行走距离达到第二预设距离(如10000mm)。
在另一些实施例中，行走参数可包括行走时间，第一预设条件为行走时间达到第一时间阈值，第二预设条件为行走时间达到第二时间阈值，其中，第二时间阈值大于第一时间阈值。
在一些实施例中，根据N个位置点的第一坐标对转向编码器的偏移参数进行校准，可包括：前N-1个位置点的实际坐标与第N个位置点的测量坐标是否在同一条直线上；若否，则根据第N个位置点的实际坐标和测量坐标得到转向编码器的目标偏移量。
具体地，根据第N个位置点的实际坐标和测量坐标得到转向编码器的目标偏移量，可包括：识别所述第N个位置点的测量坐标相对第N个位置点的实际坐标的偏移情况；若偏移情况为向左偏移，则确定目标偏移量为当前偏移量与|yN1-yN0|/2的和值，其中，yN1为第N个位置点的测量坐标中的y坐标，yN0为第N个位置点的实际坐标中的y坐标；若偏移情况为向右偏移，则确定目标偏移量为当前偏移量与|yN1-yN0|/2的差值。
在一些实施例中，得到目标偏移量之后，方法可还包括：若|yN1-yN0|小于或等于偏移量阈值，则确定目标偏移量为最终偏移量；若|yN1-yN0|大于偏移量阈值，则根据目标偏移量再次控制移动机器人直线行走，直至|yN1-yN0|小于或等于偏移量阈值。
具体而言，如图2所示，站点11、站点21是给定舵轮角度为0°及给定行走电机一定速度(如100mm/s)控制移动机器人直线运动的停止点。如果Steer Encoder Offset不精准，将导致移动机器人直线运动时，测量坐标(x31，y31)与实际坐标(x11,y11)、(x21,y21)不在一条直线上。若坐标(x31，y31)与坐标(x11,y11)、(x21,y21)不在一条直线上，则可通过测量坐标(x31，y31)与其对应的实际坐标(x30，y30)的偏向来调整Steer Encoder Offset值，并可通过该偏差的大小来判断Steer Encoder Offset值的准确性。
下面分别对不同类型的移动机器人的转向编码器的偏移参数校准过程进行阐述：
类型1、单舵轮Steer Encoder Offset校准
强制给定舵轮角度为0°，强制给定行走速度100mm/s控制移动机器人直线行走。如图2所示，移动机器人起始点为11，记录实际坐标为(x11,y11),行走第一预设距离(如5000mm)后在停止点21停止，记录实际坐标为(x21,y21)，然后继续行走一定距离(即第二预设距离与第一预设距离的差值，如5000mm)到停止点为31停止，记录移动机器人的测量坐标为(x31,y31)。若上述记录的三点在同一条直线上，则有下式成立：
如果坐标(x31,y31)如图2中情况一所示，移动机器人直线行走左偏，则将SteerEncoder Offset值增加|y31-y30|/2。
Steer Encoder Offset＝Steer Encoder Offset+|y31-y30|/2
如果坐标为(x31,y31)如图2中情况二所示，移动机器人直线行走右偏，则将SteerEncoder Offset值减去|y31-y30|/2。
Steer Encoder Offset＝Steer Encoder Offset-|y31-y30|/2
如果坐标为(x31,y31)如图2中情况三所示，停止点31与30坐标几乎重合，说明移动机器人的Steer Encoder Offset已经校准好了。
直线行走直到|y31-y30|小于偏移量阈值时，停止更新Steer Encoder Offset的值。可重复验证控制发送0°给舵轮，100mm/s给行走驱动器，控制移动机器人在站点11、站点21、站点30停止，若多次如三次|y31-y30|的值小于偏移量阈值，则可将当前的SteerEncoder Offset作为最终校准的值。
类型2、差速轮两个驱动轮行走编码器比例关系校准
差速驱动轮移动机器人转向是通过两个轮子的速度差来控制的，影响差速移动机器人高速直线运行的参数是左右轮行走编码器比例之间的比例关系。受工艺影响两个轮子在机械上不可能完全一致的。校准方法是控制器分别发送100mm/s的速度给左右轮驱动器，控制移动机器人前进运行。如图2所示，移动机器人起始点为11，记录实际坐标为(x11,y11),行走5000mm后在停止点为21停止，记录实际坐标为(x21,y21)，然后继续行走5000mm到停止点31停止，记录移动机器人的测量坐标为(x31,y31)。假设Left Driver EncoderScal＝k*Righ Driver Encoder Scal，若上述记录的三点在同一直线上，则有下式成立：
如果坐标为(x31,y31)如图2中情况一所示，移动机器人直线行走左偏，则适当降低左轮转向编码器比例，将k值减去k1*(|y31-y30|/2)。
k＝(k-k1*|y31-y30|)
其中，k1为经验值，如取值为0.0001。
如果坐标为(x31,y31)如图2中情况二所示，移动机器人直线行走右偏，则适当增加左轮行走编码器比例，将k值增加k1*(|y31-y30|/2)。
k＝(k+k1*|y31-y30|)
如果坐标为(x31,y31)如图2中情况三所示，停止点31与30坐标几乎重合，说明移动机器人的k值已经校准好了。直线行走直到|y31-y30|小于偏移量阈值时，停止更新k值。同理，可重复验证，若多次如三次|y31-y30|的值小于偏移量阈值，则可将当前的k值作为最终的左右驱动轮的关系比例值。
类型3、双舵轮的Steer Encoder Offset校准
双舵轮涉及到两个舵轮，有两个Steer Encoder Offset值需要校准。可以先校准前轮的Steer Encoder Offset值，再校准后轮Steer Encoder Offset的值。
强制给定舵轮角度为0°，强制给定行走速度100mm/s控制移动机器人直线行走。如图2所示，移动机器人起始点为11，记录实际坐标为(x11,y11),行走5000mm后在停止点21停止，记录实际坐标为(x21,y21)，然后继续行走5000mm到停止点31停止，记录移动机器人的测量坐标为(x31,y31)。上述记录的三点在同一条直线上，则有下式成立：
如果坐标为(x31,y31)如图2中情况一所示，移动机器人直线行走左偏，则存在前轮Steer Encoder Offset值过大或过小两种可能。此时，先保持后轮的Steer EncoderOffset不变，将前轮Steer Encoder Offset值增加|y31-y30|/2。
Steer Encoder Offset＝Steer Encoder Offset+|y31-y30|/2
如果下次|y31-y30|的值比上次增大说明调整反了，此时将Steer EncoderOffset减|y31-y30|/2。
如果坐标为(x31,y31)如图2中情况二所示，移动机器人直线行走右偏，则存在前轮Steer Encoder Offset值过大或过小两种可能。先保持后轮的Steer Encoder Offset不变，将前轮Steer Encoder Offset值减小|y31-y30|/2。
Steer Encoder Offset＝Steer Encoder Offset-|y31-y30|/2
如果下次|y31-y30|的值比上次增大说明调整反了，此时将Steer EncoderOffset增加|y31-y30|/2。待完成前轮调整后，再同样的方式调整后轮。最终使得控制移动机器人走直线时，能如图2中的情况三所示。
需要说明的是，移动机器人能走直线，存在图4中的两种情况，图4左图中为前后轮均没有偏移的情况，图4右图中为前后轮偏移角度相同的情况。针对图4右图中的情况，需用测试横移功能验证前后轮Steer Encoder Offset的正确性，如果能正常横移说明移动机器人的前后两个Steer Encoder Offset校准完成。
上述Steer Encoder Offset的校准是为了解决由于Steer Encoder Offset的值与转向编码器实际安装零位偏移偏差过大导致移动机器人高速直线运行时走S型的问题。并且，本发明提供的是直接控制移动机器人直线运动，不需要导航来控制，避免了激光雷达未校准给校准Steer Encoder Offset带来的影响。
在一些实施例中，根据增量和位姿观测数据得到位姿目标数据，可包括：构建扩展卡尔曼滤波模型，其中，扩展卡尔曼滤波模型包括状态方程(Sxk,Syk,Sθk,lxk,lyk,lθk)T＝(Sx(k-1),Sy(k-1),Sθ(k-1),lx(k-1),ly(k-1),lθ(k-1))T+(A,B,lθ,0,0,0)T和观测方程Zk＝[sxk，syk，sθk]+vk，(Sxk,Syk,Sθk)表示世界坐标系下移动机器人在k时刻的位姿，vk表示k时刻的噪声，(lxk,lyk,lθk)表示k时刻激光雷达的安装位置与参考点的位姿偏差，x,y,θ分别表示安装横向值、安装纵向值和安装角度，
δ表示预设系数；将增量和位姿检测数据输入至扩展卡尔曼滤波模型，得到位姿目标数据。
在一些实施例中，根据第二坐标对激光雷达的安装参数进行校准，包括：计算两个第二坐标的横向偏差和纵向偏差；若横向偏差和纵向偏差均小于偏差阈值，则确定所述位姿目标数据对应的所述激光雷达的安装参数为最终安装参数。
具体地，无论是单舵轮、双舵轮，还是差速轮，激光雷达的Laser Angle，Laser X和Laser Y的校准方法都一样。对于装有多个激光雷达的移动机器人需要自动屏蔽其它激光雷达，逐个自动测试。最后再开启多个雷达，验证校准后的参数融合后能否满足要求。
首先，把含有测试场地反光柱(如图3所示的R8-R16)的坐标信息，绘制的路径信息、站点信息的文件下载到控制器中。校准激光雷达的方法通过控制移动机器人前进到一个控制点和调头停止同一个控制点，运行EKF算法，根据激光雷达的观测值对预测值不断进行修正，让激光雷达的Laser Angle、Laser X和Laser Y参数逐渐逼近到真值(即激光雷达的实际安装位置)。例如：激光雷达的实际安装位置(103,103,3)，表示激光雷达的实际安装位置距离参考点的横向值为103，激光雷达的实际安装位置距离参考点的纵向值为103，激光雷达的实际安装角度与车身姿态位姿偏差为3°。
具体方法是：如图3所示，控制移动机器人插入系统(即移动机器人定位成功，并且进入到规划好的路径上。当发送一个目标点时，移动机器人能够追踪规划的路径到终点)，行走到初始站点10进入自动模式。控制移动机器人前进路径段2后到达站点20，记录坐标(x20,y20)；控制移动机器人跟踪路径段7后退，跟踪路径段4前进，跟踪路径段5后退，跟踪路径段6后退，跟踪路径段9前进，跟踪路径段8前进到达站点20停止，记录坐标(x21,y21)。移动机器人运行过程中运行EKF算法，停止后判断(x20,y20)与(x21,y21)的对应坐标差值是否在阈值内，若是，则判断更新的Laser Angle、Laser X和Laser Y逼近激光雷达的实际安装位置。
运行EKF算法时，首先建立移动机器人运动的状态方程，状态方程应该满足上一时刻与当前时刻的位姿关系。
设k-1时刻移动机器人在世界坐标系下的位姿为qk(xk-1,yk-1,θk-1)T，存在
其中，
是引入Censi的记号运算符，表征2维平面位姿的列矩阵组成的粒群，Sk表示激光雷达在世界坐标系下的位姿检测数据。
由k时间段内编码器增量、k-1时刻的状态，预测k时刻状态系统状态方程表达式为：
可缩写成Sxk＝Sx(k-1)+A；
可缩写成Syk＝Sy(k-1)+B；Sθk＝(sθ(k-1)-lθ(k-1)+Slθ+lθ)。由此可得状态方程：
(Sxk,Syk,Sθk,lxk,lyk,lθk)T＝(Sx(k-1),Sy(k-1),Sθ(k-1),lx(k-1),ly(k-1),lθ(k-1))T+(A,B,lθ,0,0,0)T
观测方程Zk＝[sxk，syk，sθk]+vk中的位姿观测数据(即激光雷达输出的坐标信息)由激光雷达采集得到。
假设vk中只包含高斯白噪声，根据EKF公式可向前推算状态变量为：
其中，f()泛指函数，具体的函数关系是上述的状态方程，也就是通过移动机器人k-1时刻位姿和编码器增量预测出k时刻的位姿；
是k时刻系统状态的最优估计，
是k-1时刻的最优状态估计，Uk-1是k-1时刻输入系统的编码器增量。
向前推算误差协方差：
其中，
为k时刻先验估计协方差，体现实际值依赖预测值所占的比例；Pk是k时刻系统状态的最优估计
的协方差；Qk-1为过程噪声的协方差矩阵，Qk-1受编码器增量的影响；
表示函数f对系统状态变量的雅克比矩阵；
表示函数f对系统控制输入量的雅克比矩阵。
根据EKF公式测量更新时，计算卡尔曼增益：
其中，kk为卡尔曼增益，体现预测的位姿与通过激光雷达采集的位姿信息，哪个置信度更大；Hk是观测方程的雅克比矩阵，Rk为k时刻测量噪声的协方差。
由观测变量更新估计：
其中，
是k时刻系统状态的最优估计，
是k-1时刻的最优状态估计。
更新误差协方差：
其中，Dk是k时刻系统状态的最优估计
对应的协方差，I是单位矩阵。
EKF算法是一个递推算法，状态空间在k时刻的估计以及估计的协方差矩阵的获得只需要通过k-1时刻状态的估计及其协方差矩阵以及k时间段内的输入量即可计算得到，与k-1时刻之前的信息无关。在EKF预测－更新融合迭代过程中，
状态空间中的Laser Angle，Laser X和Laser Y误差参数也会跟着变化，并最终收敛。
具体而言，对于某一时刻，移动机器人位姿有两个值，预测值(通过k-1时刻和编码器增量预测出k时刻移动机器人的位姿)和雷达的测量值。预测值与观测值都存在误差，可通过协方差来判断两者的置信度。用这两个值不断更新迭代，观测值对预测值不断进行修正，状态空间中的激光雷达安装误差参数也会跟着改变，并且lxk,lyk,lθK最终收敛。通过控制移动机器人前进以及调头到同一个点，实际停车位置是否在阈值内判断lxk,lyk,lθK的准确性。
例如，要估算k时刻的位姿值，首先根据k-1时刻的移动机器人的位姿值和编码器增量预测出k时刻的位姿值，假设是(103,103,3)，同时该值的高斯噪声的偏差是(5,5,5)。其中，(5,5,5)是这样得到的：如果k-1时刻估算出的最优位姿值的偏差是(3,3,3)，预测的不确定度是(4,4,4)，两者平方相加再开方，就是(5,5,5))。然后，从激光雷达得到k时刻的移动机器人的位姿值，假设是(105,105,5)，同时该值的偏差是(4,4,4)。
为确定(103,103,3)和(105,105,5)的置信度，可以用他们的协方差来判断。卡尔曼增益Kk^2＝5^2/(5^2+4^2)＝0.78，可以估算出k时刻的移动机器人的位姿值是：(103,103,3)+(0.78*(105-103),0.78*(105 -103),0.78*(105-103))＝(104.56,104.56，4.56)。可以看出，因为激光雷达测量的的协方差比较小(比较相信激光雷达)，所以估算出的最优位姿值偏向激光雷达测量值。
在得到k时刻的最优位姿值后，进入k+1时刻，进行新的最优估算。在进入k+1时刻之前，还要算出k时刻那个最优值(104.56,104.56，4.56)的偏差。算法如下：((1-Kk)*5^2)^0.5＝2.35。这里的(5,5,5)就是上面的k时刻预测的(103,103,3)位姿值的偏差，得出的(2.35,2.35,2.35)就是进入k+1时刻以后k时刻估算出的移动机器人最优位姿值的偏差(对应于上面的(3,3,3))。
扩展卡尔曼滤波器不断把协方差递归，从而估算出最优的移动机器人位姿值。该过程运行很快，且只保留了上一时刻的协方差。
通过以上方法校准的Laser Angle，Laser X和Laser Y的值，最终为了解决：1)由于Laser Angle与激光雷达实际安装角度偏差过大时会导致移动机器人追踪样条曲线时超出安全区域，偏离预设轨道的问题；2)由于Laser X和Laser Y与激光雷达实际安装横纵向位置偏差过大时影响移动机器人运行精度和停止精度的问题。
图5是本发明实施例的移动机器人的结构框图。
如图5所示，移动机器人1000包括控制器500，控制器500包括：处理器501和存储器503。其中，处理器501和存储器503相连，如通过总线502相连。可选地，控制器500还可以包括收发器504。需要说明的是，实际应用中收发器504不限于一个，该控制器500的结构并不构成对本发明实施例的限定。
处理器501可以是CPU(Central Processing Unit，中央处理器)，通用处理器，DSP(Digital Signal Processor，数据信号处理器)，ASIC(Application SpecificIntegrated Circuit，专用集成电路)，FPGA(Field Programmable Gate Array，现场可编程门阵列)或者其他可编程逻辑器件、晶体管逻辑器件、硬件部件或者其任意组合。其可以实现或执行结合本发明公开内容所描述的各种示例性的逻辑方框、模块和电路。处理器501也可以是实现计算功能的组合，例如包含一个或多个微处理器组合，DSP和微处理器的组合等。
总线502可包括一通路，在上述组件之间传送信息。总线502可以是PCI(Peripheral Component Interconnect，外设部件互连标准)总线或EISA(ExtendedIndustry Standard Architecture，扩展工业标准结构)总线等。总线502可以分为地址总线、数据总线、控制总线等。为便于表示，图5中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
存储器503用于存储与本发明上述实施例的方法对应的计算机程序，该计算机程序由处理器501来控制执行。处理器501用于执行存储器503中存储的计算机程序，以实现前述方法实施例所示的内容。
图5示出的控制器500仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。
综上，本发明实施例的移动机器人的参数校准方法和移动机器人，不仅能校准出单舵轮的Steer Encoder Offset值，还能校准出双舵轮的Steer Encoder Offset值，同样可以校准出差速轮左右轮行走编码器的比例关系；且不仅能校准出单激光雷达的LaserAngle、LaserX和LaserY，还能够校准出多激光雷达的LaserAngle、LaserX和LaserY，适应范围广。同时，通过重复验证，校准精度高；且操作简单，减少人工手动校准参数生产的培训和时间成本，避免了人工误分析导致的校准随机误差大。进而，通过校准后的参数，能够在移动机器人行走直线时减少纠偏量，增加行走稳定性和可靠性；在控制移动机器人转弯时，解决因参数不准确原因偏离预设轨道的问题；解决多移动机器人运行精度和停止精度问题。
需要说明的是，在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”、“顺时针”、“逆时针”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。而且，第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第一特征在第二特征正下方或斜下方，或仅仅表示第一特征水平高度小于第二特征。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
Claims (10)
1.一种移动机器人的参数校准方法，其特征在于，所述移动机器人上设有转向编码器和激光雷达，所述方法包括：
控制所述移动机器人直线行走，并记录所述移动机器人在行走过程中N个位置点的第一坐标，其中，N为大于等于3的整数；
根据所述N个位置点的第一坐标对所述转向编码器的偏移参数进行校准；
待所述转向编码器校准完成后，控制所述移动机器人按照预设路径行走，并记录所述移动机器人行走过程中的转向编码器增量和所述激光雷达的位姿观测数据，其中，所述预设路径包括前进路径、后退路径和调头路径；
根据所述增量和所述位姿观测数据得到位姿目标数据，并记录两次经过所述预设路径中目标站点时的第二坐标，以及根据所述第二坐标对所述激光雷达的安装参数进行校准。
2.根据权利要求1所述的方法，其特征在于，N＝3，所述控制所述移动机器人直线行走，以及记录所述移动机器人在行走过程中N个位置点的第一坐标，包括：
控制所述移动机器人从第一位置点开始直线行走，并记录所述第一位置点的实际坐标；
在所述移动机器人的行走参数满足第一预设条件时，控制所述移动机器人停止，并记录所述第一预设条件对应的第二位置点的实际坐标；
控制所述移动机器人从停止位置继续直线行走，并在所述移动机器人的行走参数满足第二预设条件时，控制所述移动机器人停止，并记录所述第二预设条件对应的第三位置点的实际坐标和测量坐标；
其中，所述N个位置点包括所述第一位置点、所述第二位置点和所述第三位置点，所述N个位置点的第一坐标包括所述第一位置点的实际坐标、所述第二位置点的实际坐标、所述第三位置点的实际坐标和测量坐标。
3.根据权利要求2所述的方法，其特征在于，所述移动机器人上还设有行走编码器，所述行走参数包括所述行走编码器的当前值，所述第一预设条件为所述当前值与所述行走编码器的初始值之间的差值大于第一差值阈值，所述第二预设条件为所述当前值与所述行走编码器的初始值之间的差值大于第二差值阈值，其中，所述第二差值阈值大于所述第一差值阈值。
4.根据权利要求2所述的方法，其特征在于，所述根据N个所述第一坐标对所述转向编码器的偏移参数进行校准，包括：
判断前N-1个位置点的实际坐标与第N个位置点的测量坐标是否在同一条直线上；
若否，则根据第N个位置点的实际坐标和测量坐标得到所述转向编码器的目标偏移量。
5.根据权利要求1所述的方法，其特征在于，所述根据第N个位置点的实际坐标和测量坐标得到所述转向编码器的目标偏移量，包括：
识别所述第N个位置点的测量坐标相对第N个位置点的实际坐标的偏移情况；
若所述偏移情况为向左偏移，则确定所述目标偏移量为当前偏移量与|yN1-yN0|/2的和值，其中，yN1为所述第N个位置点的测量坐标中的y坐标，yN0为所述第N个位置点的实际坐标中的y坐标；
若所述偏移情况为向右偏移，则确定所述目标偏移量为当前偏移量与|yN1-yN0|/2的差值。
6.根据权利要求5所述的方法，其特征在于，得到所述目标偏移量之后，所述方法还包括：
若|yN1-yN0|小于或等于偏移量阈值，则确定所述目标偏移量为最终偏移量；
若|yN1-yN0|大于所述偏移量阈值，则根据所述目标偏移量再次控制所述移动机器人直线行走，直至|yN1-yN0|小于或等于所述偏移量阈值。
7.根据权利要求5所述的方法，其特征在于，所述根据所述增量和所述位姿观测数据得到位姿目标数据，包括：
构建扩展卡尔曼滤波模型，其中，所述扩展卡尔曼滤波模型包括状态方程(Sxk,Syk,Sθk,lxk,lyk,lθk)T＝(Sx(k-1),Sy(k-1),Sθ(k-1),lx(k-1),ly(k-1),lθ(k-1))T+(A,B,lθ,0,0,0)T和观测方程Zk＝[sxk，syk，sθk]+vk，(Sxk,Syk,Sθk)表示世界坐标系下所述移动机器人在k时刻的位姿，vk表示k时刻的噪声，(lxk,lyk,lθk)表示k时刻所述激光雷达的安装位置与参考点的位姿偏差，x,y,θ分别表示安装横向值、安装纵向值和安装角度，
δ表示预设系数；
将所述增量和所述位姿检测数据输入至所述扩展卡尔曼滤波模型，得到所述位姿目标数据。
8.根据权利要求7所述的方法，其特征在于，所述根据所述第二坐标对所述激光雷达的安装参数进行校准，包括：
计算两个所述第二坐标的横向偏差和纵向偏差；
若所述横向偏差和所述纵向偏差均小于偏差阈值，则确定所述位姿目标数据对应的所述激光雷达的安装参数为最终安装参数。
9.根据权利要求1所述的方法，其特征在于，所述控制所述移动机器人按照预设路径行走，包括：
控制所述移动机器人直线前进第一路径段，直线后退第二路径段，第一方向曲线前进第三路径段，第二方向曲线后退第四路径段，直线后退第五路径段，直线前进第六路径段，直线前进第七路径段，其中，所述第一路径段和所述第二路径段为同一路径段，所述第五路径段和所述第六路径段为同一路径段，所述第七路径段为连接在所述第一路径段和所述第六路径段之间的路径段；
其中，两个所述第二坐标包括所述移动机器人直线前进所述第一路径段停止时的测量坐标和直线前进所述第七路径段停止时的测量坐标。
10.一种移动机器人，其特征在于，包括控制器，所述控制器包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现权利要求1-9中任一项所述方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211461016.3A CN115718494A (zh) | 2022-11-17 | 2022-11-17 | 移动机器人的参数校准方法和移动机器人 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211461016.3A CN115718494A (zh) | 2022-11-17 | 2022-11-17 | 移动机器人的参数校准方法和移动机器人 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115718494A true CN115718494A (zh) | 2023-02-28 |
Family
ID=85255858
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211461016.3A Pending CN115718494A (zh) | 2022-11-17 | 2022-11-17 | 移动机器人的参数校准方法和移动机器人 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115718494A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116148824A (zh) * | 2023-04-17 | 2023-05-23 | 机科发展科技股份有限公司 | 一种激光无人叉车导航参数自动校准系统及方法 |
-
2022
- 2022-11-17 CN CN202211461016.3A patent/CN115718494A/zh active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116148824A (zh) * | 2023-04-17 | 2023-05-23 | 机科发展科技股份有限公司 | 一种激光无人叉车导航参数自动校准系统及方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108146424B (zh) | 自动泊车路径控制方法和控制系统以及车辆 | |
| CN108640040B (zh) | 一种智能叉车的位姿调整路径规划方法 | |
| CN113581167B (zh) | 一种基于直线圆弧路径的全自动泊车控制方法及控制器 | |
| CN112379351A (zh) | Agv小车参数标定方法、装置、设备及存储介质 | |
| CN112884842B (zh) | 双舵轮零位和采集装置外参同步标定方法及可移动平台 | |
| US9821847B2 (en) | Method for guiding an off-road vehicle along a curved path | |
| CN115718494A (zh) | 移动机器人的参数校准方法和移动机器人 | |
| WO2024193405A1 (zh) | 移动机器人舵轮安装位置的标定方法及系统、存储介质 | |
| CN112278071A (zh) | 方向盘角度标定装置及方法 | |
| CN114442054B (zh) | 一种移动机器人的传感器与底盘联合标定系统及方法 | |
| JP2019215773A (ja) | 無人搬送車の走行制御装置及び走行制御方法 | |
| KR20160010175A (ko) | 방향각 오차를 이용한 차량형 이동 로봇의 오도메트리 오차 보정 방법 | |
| CN113334387B (zh) | 焊接机器人的控制方法、装置、存储介质及焊接机器人 | |
| CN112009567B (zh) | 一种农机及其转向角度标定方法、转向控制方法 | |
| CN114089694B (zh) | 位置校正方法与装置、存储介质、伺服驱动器、伺服系统 | |
| CN111880538B (zh) | 一种agv激光导航线路标定中弯道调整方法 | |
| CN113465590A (zh) | 路径规划方法和装置、自动驾驶方法和装置及作业机械 | |
| CN117415858B (zh) | 单舵轮移动机器人舵角补偿标定方法及系统、存储介质 | |
| CN114089730A (zh) | 机器人的运动规划方法和自动引导车 | |
| CN111942470A (zh) | 一种关于Forklift AGV的提高差速轮转向系统纠偏能力的控制方法 | |
| CN117093001B (zh) | 自动导向车的纠偏方法及系统 | |
| CN111164533A (zh) | 移动体 | |
| CN116974290B (zh) | 一种双舵轮agv舵轮角度校准方法及装置 | |
| CN114834441B (zh) | 自动泊车的轨迹跟踪控制方法、装置、系统及存储介质 | |
| CN118687566A (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 |