CN103389087B - 一种轮式机器人位姿解算方法 - Google Patents

Abstract

本发明公开了一种轮式机器人位姿解算方法，该方法至少包含以下步骤：步骤1、测量机器人平台的结构参数、求取编码器的特性参数，并确定编码器脉冲输出与嵌入式控制板外部中断之间的连接关系；步骤2、确定四个轮子的转向；步骤3、置位表征四个轮子着地或打滑的状态标志；步骤4、确定左侧和右侧轮子编码器的有效脉冲计数值；步骤5、计算单元求取机器人的相对位移和转角；步骤6、计算单元求取后一时刻机器人相对于全局坐标下的位姿。本发明仅使用轮式机器人轮子上装配的编码器进行机器人的位姿解算，而不需要任何其它的惯性器件辅助，使用编码器能同时解算机器人的两个采样时刻间的位移和转角，增强了机器人的紧凑度和可靠性，节约了成本。

Description

一种轮式机器人位姿解算方法

技术领域

本发明涉及一种轮式机器人位姿解算方法。

背景技术

现有技术中对轮式机器人位姿的解算是使用电机编码器估算机器人在相邻时刻间的位移，还需要基于陀螺仪或电磁罗盘等惯性器件来估算相邻时刻的机器人相对转角。这样不仅增加了机器人平台的复杂度和成本，也使得机器人的故障率上升。

在室外、湿滑等复杂的地面环境中，机器人的轮子常会出现侧滑和空转，现有技术是使用固定在轮子上的编码器作为位姿估算基准，会使在这些复杂地面环境下解算的结果中混入误差，降低定位精度，进一步全局干扰了自治机器人的作业效果；

现有技术中对机器人的控制通常采用了主从机或上位机下位机模式，即，下位机对机器人运动实施控制并将位姿测算结果反馈到上位机。现有技术是将相对运动上发，对机器人全局坐标的解算在上位机部分实现。上位机端对移动机器人位姿的解算是基于航位推算原理，任一采样时刻相对运动数据的缺失都会改变全局的轨迹形状。当下位机到上位机的数据传输链路因干扰出现严重丢包和误码时，会让若干采样时刻的相对运动未能准确传至上位机，基于残缺的相对运动序列所解算的机器人全局位姿会出现关键性错误。

发明内容

本发明的目的在于提供一种轮式机器人位姿解算方法，仅使用轮式机器人轮子上装配的编码器进行机器人的位姿解算，而不需要任何其它的惯性器件辅助，使用编码器能同时解算机器人的两个采样时刻间的位移和转角，增强了机器人的紧凑度和可靠性，节约了成本。

为了达到上述目的，本发明通过以下技术方案实现：一种轮式机器人位姿解算方法，其特点是，位姿解算方法至少包含以下步骤：

步骤1、测量机器人平台的结构参数、求取编码器的特性参数，并确定编码器脉冲输出与嵌入式控制板外部中断之间的连接关系；

上述的机器人平台包含计算单元；

步骤2、计算单元记录一个采样周期内各轮子编码器的脉冲数，确定四个轮子的转向；

步骤3、计算单元根据一个采样周期内各轮子的编码器脉冲计数置位表征四个轮子着地或打滑的状态标志；

步骤4、计算单元根据一个采样周期内各轮子的编码器脉冲计数的原始数据确定左侧轮子编码器的有效脉冲计数值和右侧轮子编码器的有效脉冲计数值；

步骤5、计算单元求取机器人一个采样周期内前一时刻t-1和后一时刻t的相对位移和转角；

步骤6、计算单元根据前一时刻t-1机器人在全局坐标下的位姿、前一时刻t-1和后一时刻t下机器人的相对位移和转角，求取后一时刻t机器人相对于全局坐标下的位姿。

上述的步骤1中的机器人平台的结构参数包含机器人左右轮之间的轮距B、同侧前后轮的轮距L、机器人轮子的周长C、机器人减速电机的减速比m:1；

上述的编码器的特性参数包含编码器的分辨率n、用来读取编码器的微控制器的中断编程系数ω。

上述的步骤2中确定各轮子的转向还包含以下步骤：

步骤2.1、计算单元记录编码器中断发生时刻，并记录中断发生后编码器a路输出的电平值和b路输出的电平值；

步骤2.2、计算单元计算编码器a路输出电平值和b路输出电平值的相异或值；

步骤2.3、计算单元根据编码器a路输出电平值和b路输出电平值的相异或值判断四个轮子的转向。

上述的步骤3还包含以下步骤：

步骤3.1、设定阀度M_d,判断轮子转数之间的差值是否在阀度M_d内；

步骤3.2、计算单元分别求取编码器同侧轮子脉冲计数的绝对值的差值；

步骤3.3、计算单元判断同侧轮子脉冲计数的绝对值的差值是否大于阀度M_d;

步骤3.4、计算单元根据不同侧轮子脉冲计数的绝对值的差值否大于阀度M_d，确定四个轮子着地或打滑的状态标志Λ。

上述的步骤3.4中的状态标志Λ：

当左前轮和右前轮完全着地，左后轮和右后轮打滑，状态标志Λ=0x11；

当左后轮和右后轮完全着地，左前轮和右前轮打滑，状态标志Λ=0x00；

当左后轮和右前轮完全着地，左前轮和右后轮打滑，状态标志Λ=0x01；

当左前轮和右后轮完全着地，左后轮和右前轮打滑，状态标志Λ=0x10；

当左前轮、右前轮和右后轮完全着地，左后轮打滑，状态标志Λ=0x11；

当左后轮、右前轮和右后轮完全着地，左前轮打滑，状态标志Λ=0x00；

当左前轮、左后轮和右前轮完全着地，右后轮打滑，状态标志Λ=0x11；

当左前轮、左后轮和右后轮完全着地，右前轮打滑，状态标志Λ=0x00；

当四个轮子都完全着地，状态标志Λ=0x11。

上述的步骤5还包含以下步骤：

步骤5.1、计算单元计算编码器的测量系数λ，公式为λ=mnω，

式中m为机器人减速电机的减速比，

n为编码器的分辨率，

ω为用来读取编码器的微控制器的中断编程系数；

步骤5.2、当左前轮和右前轮为有效轮时，即状态标志Λ=0x11，计算单元求出相对位移S_t和相对转角Φ_t如下：

${\mathrm{\Φ}}_t\⇐\frac{C(N_{\mathrm{right}}-N_{\mathrm{left}})}{\mathrm{\λB}},$

$S_t\⇐\frac{C(N_{\mathrm{right}}+N_{\mathrm{left}})}{2\mathrm{\λ}},$

式中λ为编码器的测量系数，

N_left为左侧轮子编码器的有效脉冲计数值，

N_right为右侧轮子编码器的有效脉冲计数值，

C为机器人轮子的周长；

步骤5.3、当左后轮和右后轮为有效轮时，即状态标志Λ=0x00，计算单元求出相对位移S_t和相对转角Φ_t如下：

${\mathrm{\Φ}}_t\⇐\frac{C(N_{\mathrm{rigbt}}-N_{\mathrm{Ieft}})}{\mathrm{\λB}},$

$S_t\⇐{\mathrm{\Φ}}_t\sqrt{{(R_{\mathrm{left}}+\frac{B}{2})}^2+L^2},$

式中λ为编码器的测量系数，

N_left为左侧轮子编码器的有效脉冲计数值，

N_right为右侧轮子编码器的有效脉冲计数值，

R_left为左轮距转弯圆心的半径，

C为机器人轮子的周长，

B为机器人左右轮之间的轮距，

L为同侧前后轮的轮距；

步骤5.4、当左后轮和右前轮为有效轮时，即状态标志Λ=0x01时，计算单元求出相对位移S_t和相对转角Φ_t如下：

${\mathrm{\Φ}}_t\⇐\frac{C(N_{\mathrm{right}}-N_{\mathrm{left}})}{\mathrm{\λ\ξ}},$

$R_{\mathrm{left}}\⇐\frac{N_{\mathrm{left}}\mathrm{\ξ}}{N_{\mathrm{right}}-N_{\mathrm{left}}},$

$\mathrm{\η}\⇐\frac{L(\mathrm{\ξ}+R_{\mathrm{left}})}{\mathrm{\ξ}},$

$\mathrm{\δ}\⇐\frac{B(\mathrm{\ξ}+R_{\mathrm{left}})}{\mathrm{\ξ}}-\frac{B}{2},$

$S_t\⇐{\mathrm{\Φ}}_t\sqrt{{\mathrm{\η}}^2+{\mathrm{\δ}}^2},$

式中λ为编码器的测量系数，

N_left为左侧轮子编码器的有效脉冲计数值，

N_right为右侧轮子编码器的有效脉冲计数值，

R_left为左轮距转弯圆心的半径，

C为机器人轮子的周长，

B为机器人左右轮之间的轮距，

L为同侧前后轮的轮距；

步骤5.5、当左前轮和右后轮为有效轮时，即状态标志Λ=0x10时，此时计算单元求出相对位移S_t和相对转角Φ_t如下：

$\mathrm{\ξ}\⇐\sqrt{B^2+L^2},$

${\mathrm{\Φ}}_t\⇐\frac{C(N_{\mathrm{right}}-N_{\mathrm{left}})}{\mathrm{\λ\ξ}},$

$R_{\mathrm{left}}\⇐\frac{N_{\mathrm{left}}\mathrm{\ξ}}{N_{\mathrm{right}}-N_{\mathrm{left}}},$

$\mathrm{\η}\⇐\frac{B(\frac{\mathrm{\ξ}}{2}+R_{\mathrm{left}})}{\mathrm{\ξ}},$

$\mathrm{\δ}\⇐\frac{L(\frac{\mathrm{\ξ}}{2}+R_{\mathrm{left}})}{\mathrm{\ξ}}-\frac{L}{2},$

$S_t\⇐{\mathrm{\Φ}}_t\sqrt{{\mathrm{\η}}^2+{\mathrm{\δ}}^2},$

式中λ为编码器的测量系数，

N_left为左侧轮子编码器的有效脉冲计数值，

N_right为右侧轮子编码器的有效脉冲计数值，

R_left为左轮距转弯圆心的半径，

C为机器人轮子的周长，

B为机器人左右轮之间的轮距，

L为同侧前后轮的轮距。

上述的步骤6还包含以下步骤：

步骤6.1、计算单元求取机器人在全局坐标系下后一时刻t，机器人位姿的横坐标公式为： $x_w^t\⇐S_t\cos \left({\mathrm{\Φ}}_t\right)\cos \left({\mathrm{\θ}}_w^{t-1}\right)-S_t\sin \left({\mathrm{\Φ}}_t\right)\sin \left({\mathrm{\θ}}_w^{t-1}\right)+x_w^{t-1},$

式中：为航向角，

S_t为位移，

为前一时刻t-1计算机器人在全局坐标系下位姿的横坐标

Φ_t为相对转角；

步骤6.2、计算单元求取机器人在全局坐标系下后一时刻t，机器人位姿的纵坐标公式为： $y_w^t\⇐S_t\cos \left({\mathrm{\Φ}}_t\right)\sin \left({\mathrm{\θ}}_w^{t-1}\right)+S_t\sin \left({\mathrm{\Φ}}_t\right)\cos \left({\mathrm{\θ}}_w^{t-1}\right)+y_w^{t-1},$

式中：为前一时刻t-1计算机器人在全局坐标系下位姿的航向角，

S_t为位移，

为前一时刻t-1计算机器人在全局坐标系下位姿的横坐标；

Φ_t为相对转角；

步骤6.3、计算单元求取机器人在全局坐标系下后一时刻t，机器人位姿的航向角公式为： ${\mathrm{\θ}}_w^t\⇐{\mathrm{\θ}}_w^{t-1}+{\mathrm{\Φ}}_t,$

式中：为前一时刻t-1计算机器人在全局坐标系下位姿的航向角，

Φ_t为相对转角；

步骤6.4、将后一时刻t航向角限定在定义域(-π，π]中。

上述的步骤6.4还包含以下步骤：

步骤6.4.1、将除以2π得到余数e；

步骤6.4.2、判断e是否小于π，若否，则若是，则

若e在(-π,π]之间，则

本发明一种轮式机器人位姿解算方法与现有技术相比具有以下优点：本发明仅使用轮式机器人轮子上装配的编码器进行机器人的位姿解算，而不需要任何其它的惯性器件辅助，使用编码器能同时解算机器人的两个采样时刻间的位移和转角，增强了机器人的紧凑度和可靠性，节约了成本。

本发明同时利用四驱轮式机器人的四个轮子上的四个编码器的脉冲输出数据，针对不同的路况，通过实时判断空转或打滑程度大于所设定的阈值的轮子，灵活配置状态机的状态，并根据状态机取值切换不同的运动模型，完成相邻采样时刻间机器人的位移和转角的解算，有效提高了对于不同路况的适应性和位姿解算精度。

本发明对机器人全局位姿的解算在下位机完成，同时依据不同机器人中心坐标系的空间映射关系，将所有时刻机器人的位姿映射到初始时刻的机器人中心坐标系中。下位机仅向上位机传送该全局笛卡尔坐标而并不上传相对运动，即使在下位机和上位机通信过程中由于误码造成某采样时刻的相对运动数据丢失，也不会损伤上位机端全局航位推算的精度，而仅反应为该时刻机器人位姿在全局路径中的丢失，这在长时间大范围的移动机器人作业应用中是可以忽略的。

附图说明

图1为本发明一种轮式机器人位姿解算方法的流程图。

图2为实施例中机器人平台的左右轮距和前后轮距的示意图。

图3为实施例中一个周期内编码器的两项输出的脉冲时序示意图。

图4为本发明机器人轮子着地或打滑状态标志的置位流程示意图。

图5a为实施例中构建的左前轮和右前轮完全着地时的相对运动模型等效示意图。

图5b为实施例中构建的左前轮和右前轮完全着地时的相对运动模型计算示意图。

图6a为实施例中构建的左后轮和右后轮完全着地时的相对运动模型等效示意图。

图6b为实施例中构建的左后轮和右后轮完全着地时的相对运动模型计算示意图。

图7a为实施例中构建的左前轮和右后轮完全着地时的相对运动模型等效示意图。

图7b为实施例中构建的左前轮和右后轮完全着地时的相对运动模型计算示意图。

图8a为实施例中构建的左后轮和右前轮完全着地时的相对运动模型等效示意图。

图8b为实施例中构建的左后轮和右前轮完全着地时的相对运动模型计算示意图。

具体实施方式

以下结合附图，通过详细说明一个较佳的具体实施例，对本发明做进一步阐述。

如图1所示，步骤1、测量机器人平台的结构参数、求取编码器的特性参数，并确定编码器脉冲输出与嵌入式控制板外部中断之间的连接关系。

机器人平台包含计算单元及驱动电机；计算单元包含上位机、下位机，上位机为计算机，用于处理数据；下位机中设有嵌入式控制板；嵌入式控制板上设有微控制器；微控制器包含多路外部中断；驱动电机上设有编码器；编码器包含多个a路输出和多个b路输出；编码器的多个a路输出和多个b路输出分别与微控制器的多路外部中断连接；机器人平台的结构参数包含机器人左右轮之间的轮距B、同侧前后轮的轮距L、机器人轮子的周长C、机器人减速电机的减速比m:1；编码器的特性参数包含编码器的分辨率n、用来读取编码器的微控制器的中断编程系数ω。

如图2所示，机器人包含四个驱动轮，左右轮之间的轮距B、同侧前后轮的轮距L。如果微控制器编程中使用了单沿中断触发时，ω取值为1，如果微控制器编程中使用了双沿中断触发时，ω取值为2。

在本实施例中，B为29cm，C为40cm，L为18cm，m为43.7，n为16，ω为2。本实施例中机器人编码器包含的a路输出和b路输出，它们的时序错开半个周期，两相及其组合求异或的时序示于图2。本实施例中的嵌入式控制板包含6路外部中断，分别通过微控制板的4路外部中断读取4个轮子编码器的A路输出，其计数值分别记为N_frontleft、N_rearleft、N_frontright、N_rearright;其余的2路外部中断分别与左侧任一个轮子和右侧的任一个轮子的B路相连，在本实施例中，上述2路外部中断分别与左前轮和右前轮的编码器的B路相连。

如图3和表1所示，步骤2、上位机记录一个采样周期内各轮子编码器的脉冲数，确定四个轮子的转向，即确定4个轮子编码器的A路输出计数值N_frontleft、N_rearleft、N_frontright、N_rearright的正负号。

表1通过编码器两路信号输出电平判断轮子的正反转

	在A相外部中断操作中判断	在B相外部中断操作中判断
			正转	pinA⊕pinB=1	pinA⊕pinB=0
反转	pinA⊕pinB=0	pinA⊕pinB=1

步骤2.1、微控制器记录编码器中断发生位置，并记录中断发生后编码器a路输出的电平值和b路输出的电平值；

步骤2.2、上位机求取编码器a路输出电平值和b路输出电平值的相异或值；

步骤2.3、上位机根据编码器a路输出电平值和b路输出电平值的相异或值判断四个轮子的转向。

本实施例以判断右后轮的转向为例：

记录中断发生于A路还是B路，在每一次编码器脉冲中断发生后分别记录右后轮编码器的A路电平和B路电平

若A路发生中断则计算和相异或的结果，如果为1则正转，N_frontright和N_rearright符号为正；如果为0则反转，N_frontright和N_rearright符号为负；

若B路发生中断则计算和相异或的结果，如果为1则反转，N_frontright和N_rearright符号为负；如果为0则正转，N_frontright和N_rearright符号为正。

步骤3、上位机根据一个采样周期内各轮子的编码器脉冲计数置位表征各轮子着地或打滑的状态。

本实施例通过一个十六进制的状态标志Λ表征机器人四个轮子的着地或打滑状态。在湿滑的路况下，可能出现如表2所示的轮子着地与打滑状态的9种组合。

表1车轮着地情况与状态标志间的映射关系

轮子着地和打滑状态	状态标志Λ取值
		左前轮和右前轮完全着地，左后轮和右后轮打滑	0x11
左后轮和右后轮完全着地，左前轮和右前轮打滑	0x00
		左后轮和右前轮完全着地，左前轮和右后轮打滑	0x01
左前轮和右后轮完全着地，左后轮和右前轮打滑	0x10

左前轮、右前轮和右后轮完全着地，左后轮打滑	0x11
		左后轮、右前轮和右后轮完全着地，左前轮打滑	0x00
左前轮、左后轮和右前轮完全着地，右后轮打滑	0x11
		左前轮、左后轮和右后轮完全着地，右前轮打滑	0x00
四个轮子都完全着地	0x11

如图4所示，对状态标志Λ的置位过程详述于下列步骤：

步骤3.1、设定阀度M_d，判断轮子转数之间的差值是否在阀度M_d内；

步骤3.2、上位机计算编码器测得右前轮和右后轮的脉冲计数的绝对值的差值，即ΔN_right=|N_frontright|-|N_rearright｜,求取编码器测得左前轮和左后轮的脉冲计数的绝对值的差值，即ΔN_left=|N_frontleft|-|N_rearleft|;

步骤3.3、判断门限M_d与ΔN_right的大小关系，门限M_d与ΔN_left的大小关系；

步骤3.4、若ΔN_right大于门限M_d，则状态标志Λ=0x00；

若ΔN_right小于门限-M_d，则设置Λ=0x01；

若ΔN_right处于-M_d和M_d之间，则设置Λ=0x02；

步骤3.5、若ΔN_left大于门限M_d，则基于步骤3.4所设置状态标志Λ的值执行下述操作：若Λ=0x02，则设置Λ=0x00；

若Λ不等于0x02，则设置Λ=Λ+0x00；

步骤3.6、若ΔN_left小于门限-M_d，则基于步骤3.4所设置状态标志Λ的值执行下述操作：如果Λ=0x02，则设置Λ=0x11；

如果Λ不等于0x02，则设置Λ=Λ+0x10；

步骤3.7、若ΔN_left处于-M_d和M_d之间，则基于步骤3.4所设置状态标志Λ的值执行下述操作：如果Λ=0x02，则设置Λ=0x11；

如果Λ不等于0x02，则设置Λ为Λ与步骤3.4得到的Λ左移8位后的和。

步骤4、上位机根据一个采样周期内各轮子的编码器脉冲计数的原始数据确定左侧和右侧轮子编码器的有效脉冲计数值。

根据状态标志Λ和编码器脉冲计数N_frontleft、N_rearleft、N_frontright、N_rearright确定有效的编码器计数值N_left和N_right，具体的步骤如下：

步骤4.1、若状态标志Λ与0x0F按位取与的结果等于0x01，则设置N_right=N_frontright;

若不是，设置N_right=N_rearright;

步骤4.1、若状态标志Λ与0xF0按位取与的结果等于0x10，则设置N_left=N_frontleft;

若不是，设置N_left=N_rearleft。

步骤5、上位机求取一个采样周期内前一时刻t-1和后一时刻t下机器人的相对位移S_t和转角Φ_t。

本实施例中两个采样时刻相隔为0.2s。具体步骤描述如下：

步骤5.1、上位机计算编码器的测量系数λ，公式为λ=mnω，

式中m为机器人减速电机的减速比，

n为编码器的分辨率，

ω为用来读取编码器的微控制器的中断编程系数；

步骤5.2、当左前轮和右前轮为有效轮时，即状态标志Λ=0x11，上位机求出相对位移S_t和相对转角Φ_t如下：

${\mathrm{\Φ}}_t\⇐\frac{C(N_{\mathrm{right}}-N_{\mathrm{left}})}{\mathrm{\λB}},$

此时的模型示意如图5a和图5b所示，

式中λ为编码器的测量系数，

N_left为左侧轮子编码器的有效脉冲计数值，

N_right为右侧轮子编码器的有效脉冲计数值，

C为机器人轮子的周长；

步骤5.3、当左后轮和右后轮为有效轮时，即状态标志Λ=0x00，上位机求出相对位移S_t和相对转角Φ_t如下：

${\mathrm{\Φ}}_t\⇐\frac{C(N_{\mathrm{right}}-N_{\mathrm{left}})}{\mathrm{\λB}},$

此时的模型示意如图6a和图6b所示，

式中λ为编码器的测量系数，

N_left为左侧轮子编码器的有效脉冲计数值，

N_right为右侧轮子编码器的有效脉冲计数值，

R_left为左轮距转弯圆心的半径，

C为机器人轮子的周长，

B为机器人左右轮之间的轮距，

L为同侧前后轮的轮距；

步骤5.4、当左后轮和右前轮为有效轮时，即状态标志Λ=0x01时，上位机求出相对位移S_t和相对转角Φ_t如下：

${\mathrm{\Φ}}_t\⇐\frac{C(N_{\mathrm{right}}-N_{\mathrm{left}})}{\mathrm{\λ\ξ}},$

$R_{\mathrm{left}}\⇐\frac{N_{\mathrm{left}}\mathrm{\ξ}}{N_{\mathrm{right}}-N_{\mathrm{left}}},$

$\mathrm{\η}\⇐\frac{L(\mathrm{\ξ}+R_{\mathrm{left}})}{\mathrm{\ξ}},$

$\mathrm{\δ}\⇐\frac{B(\mathrm{\ξ}+R_{\mathrm{left}})}{\mathrm{\ξ}}-\frac{B}{2},$

此时的模型示意如图7a和图7b所示，

式中λ为编码器的测量系数，

N_left为左侧轮子编码器的有效脉冲计数值，

N_right为右侧轮子编码器的有效脉冲计数值，

R_left为左轮距转弯圆心的半径，

C为机器人轮子的周长，

B为机器人左右轮之间的轮距，

L为同侧前后轮的轮距；

步骤5.5、当左前轮和右后轮为有效轮时，即状态标志Λ=0x10时，此时上位机求出相对位移S_t和相对转角Φ_t如下：

$\mathrm{\ξ}\⇐\sqrt{B^2+L^2},$

${\mathrm{\Φ}}_t\⇐\frac{C(N_{\mathrm{right}}-N_{\mathrm{left}})}{\mathrm{\λ\ξ}},$

$R_{\mathrm{left}}\⇐\frac{N_{\mathrm{left}}\mathrm{\ξ}}{N_{\mathrm{right}}-N_{\mathrm{left}}},$

$\mathrm{\η}\⇐\frac{B(\frac{\mathrm{\ξ}}{2}+R_{\mathrm{left}})}{\mathrm{\ξ}},$

$\mathrm{\δ}\⇐\frac{L(\frac{\mathrm{\ξ}}{2}+R_{\mathrm{left}})}{\mathrm{\ξ}}-\frac{L}{2},$

此时的模型示意如图8a和图8b所示，

式中λ为编码器的测量系数，

N_left为左侧轮子编码器的有效脉冲计数值，

N_right为右侧轮子编码器的有效脉冲计数值，

R_left为左轮距转弯圆心的半径，

C为机器人轮子的周长，

B为机器人左右轮之间的轮距，

L为同侧前后轮的轮距。

步骤6、上位机根据前一时刻t-1机器人在全局坐标下的位姿、前一时刻t-1和后一时刻t下机器人的相对位移和转角，求取后一时刻t机器人相对于全局坐标下的位姿。

上位机利用步骤5求得的t时刻相对于t-1时刻机器人的相对位移S_t和相对转角Φ_t，及t-1时刻机器人处于全局坐标系w下的笛卡尔坐标解算t时刻机器人在全局坐标系w下的笛卡尔坐标即t时刻机器人的全局位姿，具体步骤为：

步骤6.1、上位机求取机器人在全局坐标系下后一时刻t，机器人位姿的横坐标公式为： $x_w^t\⇐S_t\cos \left({\mathrm{\Φ}}_t\right)\cos \left({\mathrm{\θ}}_w^{t-1}\right)-S_t\sin \left({\mathrm{\Φ}}_t\right)\sin \left({\mathrm{\θ}}_w^{t-1}\right)+x_w^{t-1},$

式中：为航向角，

S_t为位移，

为前一时刻t-1计算机器人在全局坐标系下位姿的横坐标

Φ_t为相对转角；

步骤6.2、上位机求取机器人在全局坐标系下后一时刻t，机器人位姿的纵坐标公式为： $y_w^t\⇐S_t\cos \left({\mathrm{\Φ}}_t\right)\sin \left({\mathrm{\θ}}_w^{t-1}\right)+S_t\sin \left({\mathrm{\Φ}}_t\right)\cos \left({\mathrm{\θ}}_w^{t-1}\right)+y_w^{t-1},$

式中：为前一时刻t-1计算机器人在全局坐标系下位姿的航向角，

S_t为位移，

为前一时刻t-1计算机器人在全局坐标系下位姿的横坐标；

Φ_t为相对转角；

步骤6.3、上位机求取机器人在全局坐标系下后一时刻t，机器人位姿的航向角公式为： ${\mathrm{\θ}}_w^t\⇐{\mathrm{\θ}}_w^{t-1}+{\mathrm{\Φ}}_t,$

式中：为前一时刻t-1计算机器人在全局坐标系下位姿的航向角，

Φ_t为相对转角；

步骤6.4、将后一时刻t航向角限定在定义域(-π,π]中；

步骤6.4.1、将除以2π得到余数e；

步骤6.4.2、判断e是否小于π，若否，则若是，则

若e在(-π,π]之间，则

尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。

Claims (3)

1.一种轮式机器人位姿解算方法，其特征在于，位姿解算方法至少包含以下步骤：

步骤1、测量机器人平台的结构参数、求取编码器的特性参数，并确定编码器脉冲输出与嵌入式控制板外部中断之间的连接关系；

所述的机器人平台包含计算单元；

步骤2、计算单元记录一个采样周期内四个轮子编码器的脉冲数，得到四个轮子的转向；

步骤2.1、计算单元记录编码器中断发生的时刻，并记录中断发生后编码器a路输出的电平值和b路输出的电平值；

步骤2.2、计算单元计算编码器a路输出电平值和b路输出电平值的相异或值；

步骤2.3、上位机根据编码器a路输出电平值和b路输出电平值的相异或值判断四个轮子的转向；

步骤3、计算单元根据一个采样周期内各轮子的编码器脉冲计数置位表征四个轮子着地或打滑的状态标志；

步骤3.1、设定阀度M_d，判断轮子转数之间的差值是否在阀度M_d内；

步骤3.2、计算单元分别求取编码器同侧轮子脉冲计数的绝对值的差值；

步骤3.3、计算单元判断同侧轮子脉冲计数的绝对值的差值是否大于阀度M_d；

步骤3.4、计算单元根据不同侧轮子脉冲计数的绝对值的差值否大于阀度M_d，确定各轮子着地或打滑的状态标志Λ；

步骤4、计算单元根据一个采样周期内四个轮子的编码器脉冲计数的原始数据确定左侧轮子编码器的有效脉冲计数值和右侧轮子编码器的有效脉冲计数值；

步骤5、计算单元求取机器人一个采样周期内前一时刻t-1和后一时刻t的相对位移和转角；

步骤5.1、计算单元计算编码器的测量系数λ，公式为λ＝mnω，

式中m为机器人减速电机的减速比，

n为编码器的分辨率，

ω为用来读取编码器的微控制器的中断编程系数；

步骤5.2、当左前轮和右前轮为有效轮时，即状态标志Λ＝0x11，计算单元求出相对位移s_t和相对转角Φ_t如下：

${\mathrm{\Φ}}_t\⇐\frac{C(N_{right}-N_{left})}{\mathrm{\λ}B},$

$s_t\⇐\frac{C(N_{right}+N_{left})}{2\mathrm{\λ}},$

式中：λ为编码器的测量系数，

N_left为左侧轮子编码器的有效脉冲计数值，

N_right为右侧轮子编码器的有效脉冲计数值，

B为机器人左右轮之间的轮距，

C为机器人轮子的周长；

步骤5.3、当左后轮和右后轮为有效轮时，即状态标志Λ＝0x00，计算单元求出相对位移s_t和相对转角Φ_t如下：

${\mathrm{\Φ}}_t\⇐\frac{C(N_{right}-N_{left})}{\mathrm{\λ}B},$

$s_t\⇐{\mathrm{\Φ}}_t\sqrt{{(R_{left}+\frac{B}{2})}^2+L^2},$

式中：λ为编码器的测量系数，

N_left为左侧轮子编码器的有效脉冲计数值，

N_right为右侧轮子编码器的有效脉冲计数值，

R_left为左轮距转弯圆心的半径，

C为机器人轮子的周长，

B为机器人左右轮之间的轮距，

L为同侧前后轮的轮距；

步骤5.4、当左后轮和右前轮为有效轮时，即状态标志Λ＝0x01时，计算单元求出相对位移s_t和相对转角Φ_t如下：

$\mathrm{\ξ}\⇐\sqrt{B^2+L^2},$

${\mathrm{\Φ}}_t\⇐\frac{C(N_{right}-N_{left})}{\mathrm{\λ}\mathrm{\ξ}},$

$R_{left}\⇐\frac{N_{left}\mathrm{\ξ}}{N_{right}-N_{left}},$

$\mathrm{\η}\⇐\frac{L(\mathrm{\ξ}+R_{left})}{\mathrm{\ξ}},$

$\mathrm{\δ}\⇐\frac{B(\mathrm{\ξ}+R_{left})}{\mathrm{\ξ}}-\frac{B}{2},$

$S_t\⇐{\mathrm{\Φ}}_t\sqrt{{\mathrm{\η}}^2+{\mathrm{\δ}}^2},$

式中：λ为编码器的测量系数，

N_left为左侧轮子编码器的有效脉冲计数值，

N_right为右侧轮子编码器的有效脉冲计数值，

R_left为左轮距转弯圆心的半径，

C为机器人轮子的周长，

B为机器人左右轮之间的轮距，

L为同侧前后轮的轮距；

步骤5.5、当左前轮和右后轮为有效轮时，即状态标志Λ＝0x10时，此时计算单元求出相对位移s_t和相对转角Φ_t如下：

$\mathrm{\ξ}\⇐\sqrt{B^2+L^2},$

${\mathrm{\Φ}}_t\⇐\frac{C(N_{right}-N_{left})}{\mathrm{\λ}\mathrm{\ξ}},$

$R_{left}\⇐\frac{N_{left}\mathrm{\ξ}}{N_{right}-N_{left}},$

$\mathrm{\η}\⇐\frac{B(\frac{\mathrm{\ξ}}{2}+R_{left})}{\mathrm{\ξ}},$

$\mathrm{\δ}\⇐\frac{L(\frac{\mathrm{\ξ}}{2}+R_{left})}{\mathrm{\ξ}}-\frac{L}{2},$

$S_t\⇐{\mathrm{\Φ}}_t\sqrt{{\mathrm{\η}}^2+{\mathrm{\δ}}^2},$

式中：λ为编码器的测量系数，

N_left为左侧轮子编码器的有效脉冲计数值，

N_right为右侧轮子编码器的有效脉冲计数值，

R_left为左轮距转弯圆心的半径，

C为机器人轮子的周长，

B为机器人左右轮之间的轮距，

L为同侧前后轮的轮距；

步骤6、计算单元根据前一时刻t-1机器人在全局坐标下的位姿、前一时刻t-1和后一时刻t下机器人的相对位移和转角，求取后一时刻t机器人相对于全局坐标下的位姿；

步骤6.1、计算单元求取机器人在全局坐标系下后一时刻t，机器人位姿的横坐标公式为：

$x_w^t\⇐S_t\cos \left({\mathrm{\Φ}}_t\right)\cos \left({\mathrm{\θ}}_w^{t-1}\right)-S_t\sin \left({\mathrm{\Φ}}_t\right)\sin \left({\mathrm{\θ}}_w^{t-1}\right)+x_w^{t-1},$

式中：为航向角，

s_t为位移，

为前一时刻t-1计算机器人在全局坐标系下位姿的横坐标

Φ_t为相对转角；

步骤6.2、计算单元求取机器人在全局坐标系下后一时刻t，机器人位姿的纵坐标公式为：

$y_w^t\⇐S_t\cos \left({\mathrm{\Φ}}_t\right)\sin \left({\mathrm{\θ}}_w^{t-1}\right)+S_t\sin \left({\mathrm{\Φ}}_t\right)\cos \left({\mathrm{\θ}}_w^{t-1}\right)+y_w^{t-1},$

式中：为前一时刻t-1计算机器人在全局坐标系下位姿的航向角，

s_t为位移，

为前一时刻t-1计算机器人在全局坐标系下位姿的横坐标；

Φ_t为相对转角；

步骤6.3、计算单元求取机器人在全局坐标系下后一时刻t，机器人位姿的航向角公式为：

${\mathrm{\θ}}_w^t\⇐{\mathrm{\θ}}_w^{t-1}+{\mathrm{\Φ}}_t,$

式中：为前一时刻t-1计算机器人在全局坐标系下位姿的航向角，

Φ_t为相对转角；

步骤6.4、将后一时刻t航向角限定在定义域(-π，π]中。

2.如权利要求1所述的轮式机器人位姿解算方法，其特征在于，所述的步骤3.4中的状态标志Λ：

当左前轮和右前轮完全着地，左后轮和右后轮打滑，状态标志Λ＝0x11；

当左后轮和右后轮完全着地，左前轮和右前轮打滑，状态标志Λ＝0x00；

当左后轮和右前轮完全着地，左前轮和右后轮打滑，状态标志Λ＝0x01；

当左前轮和右后轮完全着地，左后轮和右前轮打滑，状态标志Λ＝0x10；

当左前轮、右前轮和右后轮完全着地，左后轮打滑，状态标志Λ＝0x11；

当左后轮、右前轮和右后轮完全着地，左前轮打滑，状态标志Λ＝0x00；

当左前轮、左后轮和右前轮完全着地，右后轮打滑，状态标志Λ＝0x11；

当左前轮、左后轮和右后轮完全着地，右前轮打滑，状态标志Λ＝0x00；

当四个轮子都完全着地，状态标志Λ＝0x11。

3.如权利要求1所述的轮式机器人位姿解算方法，其特征在于，所述的步骤6.4还包含以下步骤：

步骤6.4.1、将除以2π得到余数e；

步骤6.4.2、判断e是否小于π，若否，则若是，则

若e在(-π，π]之间，则