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

摘要

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

主权项

一种轮式机器人位姿解算方法，其特征在于，位姿解算方法至少包含以下步骤：步骤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如下：${\Phi }_t\Leftarrow \frac{C(N_{right}-N_{left})}{\lambda B},$$s_t\Leftarrow \frac{C(N_{right}+N_{left})}{2\lambda },$式中：λ为编码器的测量系数，N_left为左侧轮子编码器的有效脉冲计数值，N_right为右侧轮子编码器的有效脉冲计数值，B为机器人左右轮之间的轮距，C为机器人轮子的周长；步骤5.3、当左后轮和右后轮为有效轮时，即状态标志Λ＝0x00，计算单元求出相对位移s_t和相对转角Φ_t如下：${\Phi }_t\Leftarrow \frac{C(N_{right}-N_{left})}{\lambda B},$$s_t\Leftarrow {\Phi }_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如下：$\xi \Leftarrow \sqrt{B^2+L^2},$${\Phi }_t\Leftarrow \frac{C(N_{right}-N_{left})}{\lambda \xi },$$R_{left}\Leftarrow \frac{N_{left}\xi }{N_{right}-N_{left}},$$\eta \Leftarrow \frac{L(\xi +R_{left})}{\xi },$$\delta \Leftarrow \frac{B(\xi +R_{left})}{\xi }-\frac{B}{2},$$S_t\Leftarrow {\Phi }_t\sqrt{{\eta }^2+{\delta }^2},$式中：λ为编码器的测量系数，N_left为左侧轮子编码器的有效脉冲计数值，N_right为右侧轮子编码器的有效脉冲计数值，R_left为左轮距转弯圆心的半径，C为机器人轮子的周长，B为机器人左右轮之间的轮距，L为同侧前后轮的轮距；步骤5.5、当左前轮和右后轮为有效轮时，即状态标志Λ＝0x10时，此时计算单元求出相对位移s_t和相对转角Φ_t如下：$\xi \Leftarrow \sqrt{B^2+L^2},$${\Phi }_t\Leftarrow \frac{C(N_{right}-N_{left})}{\lambda \xi },$$R_{left}\Leftarrow \frac{N_{left}\xi }{N_{right}-N_{left}},$$\eta \Leftarrow \frac{B(\frac{\xi }{2}+R_{left})}{\xi },$$\delta \Leftarrow \frac{L(\frac{\xi }{2}+R_{left})}{\xi }-\frac{L}{2},$$S_t\Leftarrow {\Phi }_t\sqrt{{\eta }^2+{\delta }^2},$式中：λ为编码器的测量系数，N_left为左侧轮子编码器的有效脉冲计数值，N_right为右侧轮子编码器的有效脉冲计数值，R_left为左轮距转弯圆心的半径，C为机器人轮子的周长，B为机器人左右轮之间的轮距，L为同侧前后轮的轮距；步骤6、计算单元根据前一时刻t‑1机器人在全局坐标下的位姿、前一时刻t‑1和后一时刻t下机器人的相对位移和转角，求取后一时刻t机器人相对于全局坐标下的位姿；步骤6.1、计算单元求取机器人在全局坐标系下后一时刻t，机器人位姿的横坐标公式为：$x_w^t\Leftarrow S_t\cos \left({\Phi }_t\right)\cos \left({\theta }_w^{t-1}\right)-S_t\sin \left({\Phi }_t\right)\sin \left({\theta }_w^{t-1}\right)+x_w^{t-1},$式中：为航向角，s_t为位移，为前一时刻t‑1计算机器人在全局坐标系下位姿的横坐标Φ_t为相对转角；步骤6.2、计算单元求取机器人在全局坐标系下后一时刻t，机器人位姿的纵坐标公式为：$y_w^t\Leftarrow S_t\cos \left({\Phi }_t\right)\sin \left({\theta }_w^{t-1}\right)+S_t\sin \left({\Phi }_t\right)\cos \left({\theta }_w^{t-1}\right)+y_w^{t-1},$式中：为前一时刻t‑1计算机器人在全局坐标系下位姿的航向角，s_t为位移，为前一时刻t‑1计算机器人在全局坐标系下位姿的横坐标；Φ_t为相对转角；步骤6.3、计算单元求取机器人在全局坐标系下后一时刻t，机器人位姿的航向角公式为：${\theta }_w^t\Leftarrow {\theta }_w^{t-1}+{\Phi }_t,$式中：为前一时刻t‑1计算机器人在全局坐标系下位姿的航向角，Φ_t为相对转角；步骤6.4、将后一时刻t航向角限定在定义域(‑π，π]中。