CN109571467B - 双轮差速机器人运动模型标定方法、装置及里程计系统 - Google Patents

Abstract

本发明涉及一种双轮差速机器人运动模型标定方法、装置及里程计系统，属于人工智能领域。所述方法包括获取用户输入的第一参数并实时获取与第一参数对应的机器人左、右轮电机转动次数，第一参数包括机器人运动时的4条轨迹信息；根据第一参数及对应的机器人左、右轮电机转动次数，确定机器人的姿态标定参数；获取用户输入的第二参数，并实时获取与第二参数对应的机器人左、右轮电机转动次数，第二参数包括两条轨迹信息；根据第二参数、第二参数对应的机器人左、右轮电机转动次数以及姿态标定参数，确定机器人的位置标定参数；根据机器人的姿态标定参数及机器人的位置标定参数，标定理论运动模型。本发明对工作人员专业技术要求低，节约了标定时间。

Description

双轮差速机器人运动模型标定方法、装置及里程计系统

技术领域

本发明涉及一种双轮差速机器人运动模型标定方法、装置及里程计系统，属于人工智能领域。

背景技术

双轮差速机器人因结构简单、控制方便，是目前大多数轮式机器人采用的一种形式，通常通过不同的电机控制左、右轮转动实现机器人的运动，例如，控制双轮差速移动实现机器人旋转，控制双轮等速移动实现机器人直行。

双轮差速机器人的控制需要依赖于运动模型，通常需要根据机器人的结构参数进行运动模型的建立，但是在机器人实际运行过程中存在“地面接触摩擦系数不确定”、“不相等的轮直径”、“轮距的不确定”和“两轮子直径的实际平均值与标称平均值不相等”等问题，导致建立的机器人理论运动模型误差较大，不能满足实际使用需求，因而在机器人投入使用前需要对运动理论模型进行标定。

目前，机器人在完成搭建后，通常只包含硬件部分及理论模型，需要机器人开发者根据机器人实际使用环境，结合所述理论模型人工编写标定代码并调试参数，实现对机器人理论运动模型的标定，该方法对工作人员专业技术要求非常高，且需要耗费大量开发工时。

发明内容

本发明的技术解决问题是：针对现有技术中存在的问题，提供一种双轮差速机器人运动模型标定方法、装置及里程计系统，通过用户控制或推行机器人运动6条简单轨迹，并输入轨迹信息即可实现对机器人双轮差速运动模型的参数标定，对工作人员专业技术要求低，节约了标定时间。

本发明的技术解决方案是：

一种双轮差速机器人运动模型标定方法，包括：

获取用户输入的第一参数，并实时获取与所述第一参数对应的机器人左、右轮电机转动次数，所述第一参数包括机器人运动时的第一直行前进轨迹信息、第一直行后退轨迹信息、原地左转轨迹信息及原地右转轨迹信息；

根据所述第一参数及对应的机器人左、右轮电机转动次数，确定机器人的姿态标定参数；

获取所述用户输入的第二参数，并实时获取与所述第二参数对应的机器人左、右轮电机转动次数，所述第二参数包括第二直行前进轨迹信息和第二直行后退轨迹信息；

根据所述第二参数、所述第二参数对应的机器人左、右轮电机转动次数以及所述姿态标定参数，确定机器人的位置标定参数；

根据所述机器人的姿态标定参数及机器人的位置标定参数，标定理论运动模型。

在一可选实施例中，所述获取用户输入的第一参数，包括：

显示第一参数输入界面，并获取用户输入的所述第一参数。

在一可选实施例中，所述获取所述用户输入的第二参数，包括：

显示第二参数输入界面，并获取所述用户输入的所述第二参数。

在一可选实施例中，还包括：

显示开始按钮，当检测到用户按下所述开始按钮时，确定为机器人运动轨迹的起始点。

在一可选实施例中，还包括：

显示完成按钮，当检测到用户按下所述完成按钮时，确定为所述机器人运动轨迹的结束点。

在一可选实施例中，所述轨迹信息包含轨迹标识以及机器人x、y方向运动距离和/或机器人运动角度。

在一可选实施例中，所述根据所述第一参数及对应的机器人左、右轮电机转动次数，确定机器人的姿态标定参数，包括：

根据下式确定机器人的姿态标定参数：

其中，C_2,1和C_2,2是机器人的姿态标定参数，

是由机器人运动时的第一直行前进轨迹、第一直行后退轨迹、原地左转轨迹及原地右转轨迹4条轨迹生成的广义姿态耦合矩阵，θ_N,m为第m条轨迹结束点处机器人的运动角度，θ_0,m为第m条轨迹起始点处机器人的运动角度，m＝1-4。

在一可选实施例中，所述根据所述第二参数、所述第二参数对应的机器人左、右轮电机转动次数以及所述姿态标定参数，确定机器人的位置标定参数，包括：

根据下式确定机器人的位置标定参数：

其中，C_1,1和C_1,2是机器人的位置标定参数，

是由机器人的第二直行前进轨迹和第二直行后退轨迹2条轨迹生成的广义位置耦合矩阵，x_N,n为第n条轨迹结束点处机器人的x方向运动距离，y_N,n为第n条轨迹结束点处机器人的y方向运动距离，x_0,n为第n条轨迹起始点处机器人的x方向运动距离，y_0,n为第n条轨迹起始点处机器人的y方向运动距离，n＝1-2。

在一可选实施例中，所述标定运动模型之后，还包括：

基于标定后的运动模型，输出机器人航迹推测信息，所述航迹推测信息包括位置信息和姿态信息。

在一可选实施例中，所述标定运动模型之后，还包括：

接收用户下发的机器人期望速度信息；

基于标定后的运动模型，根据所述期望速度信息和机器人当前速度信息，对机器人进行闭环控制，其中所述速度信息包括角速度和/或线速度。

在一可选实施例中，根据所述期望速度信息和当前速度信息，对机器人进行闭环控制，包括：

根据下式分别确定机器人左轮和右轮的PWM输出占空比：

其中，υ_obj为期望线速度、ω_obj为期望角速度；υ_cur为机器人当前线速度、ω_cur为机器人当前角速度；C^-1 _1,1、C^-1 _1,2、C^-1 _2,1、C^-1 _2,2分别为所述机器人的姿态标定参数和位置标定参数的逆；Δw_L为机器人左轮期望旋转速度和当前旋转速度的差、表示左轮P控制量，Δw_R为机器人右轮期望旋转速度和实际旋转速度的差、表示右轮P控制量；Σw_L(t)表示当前控制周期机器人左轮I控制量、Σw_R(t)表示当前控制周期机器人右轮I控制量，Σw_L(t-1)表示上一控制周期机器人左轮I控制量、Σw_R(t-1)表示上一控制周期机器人右轮I控制量；PWM_L为控制机器人左轮速度的PWM输出占空比，PWM_R为控制机器人右轮速度的PWM输出占空比；K_P,L和K_I,L是左轮PI控制参数，K_P,R和K_I,R是右轮的PI控制参数；

根据确定的左、右轮PWM输出占空比，分别对机器人左、右轮进行闭环控制。

一种双轮差速机器人运动模型标定装置，包括：

第一获取模块，用于获取用户输入的第一参数并实时获取与所述第一参数对应的机器人左、右轮电机转动次数，所述第一参数包括机器人运动时的第一直行前进轨迹信息、第一直行后退轨迹信息、原地左转轨迹信息及原地右转轨迹信息；

第一确定模块，用于根据所述第一参数及对应的机器人左、右轮电机转动次数，确定机器人的姿态标定参数；

第二获取模块，用于获取所述用户输入的第二参数，并实时获取与所述第二参数对应的机器人左、右轮电机转动次数，所述第二参数包括第二直行前进轨迹信息和第二直行后退轨迹信息；

第二确定模块，用于根据所述第二参数、所述第二参数对应的机器人左、右轮电机转动次数以及所述姿态标定参数，确定机器人的位置标定参数；

标定模块，用于根据所述机器人的姿态标定参数及机器人的位置标定参数，标定理论运动模型。

在一可选实施例中，还包括显示模块，用于显示第一参数输入界面，所述第一获取模块用于获取用户输入的所述第一参数。

在一可选实施例中，所述显示模块还用于显示第二参数输入界面，所述第二获取模块用于获取所述用户输入的所述第二参数。

在一可选实施例中，还包括起始点确定模块：

所述显示模块，还用于显示开始按钮，所述起始点确定模块，用于当检测到用户按下所述开始按钮时，确定为机器人运动轨迹的起始点。

在一可选实施例中，还包括：

所述显示模块，还用于显示完成按钮，所述结束点确定模块，用于当检测到用户按下所述完成按钮时，确定为所述机器人运动轨迹的结束点。

在一可选实施例中，还包括：

航迹推测模块，用于基于标定后的运动模型，输出机器人航迹推测信息，所述航迹推测信息包括位置信息和姿态信息。

在一可选实施例中，还包括：

接收模块，用于接收用户下发的机器人期望速度信息；

控制模块，用于基于标定后的运动模型，根据所述期望速度信息和机器人当前速度信息，对机器人进行闭环控制，其中所述速度信息包括角速度和/或线速度。

一种双轮差速机器人里程计系统，包括壳体及设置在壳体内的由权利要求12-18任一项所述的运动模型标定装置。

在一可选实施例中，所述壳体上设有标准接口，用于将所述运动模型标定装置的各功能引脚与对应的外部设备连接。

在一可选实施例中，所述标准接口包括：

电源接口，用于为里程计系统提供5V安全电压；

人机通信接口，用于连接所述接收模块和外部控制系统以使用户下发机器人的期望速度信息，和/或连接所述航迹推测模块和所述外部控制系统以为用户提供机器人航迹推测信息；

测量和控制接口，用于连接所述获取模块与机器人的外部测量装置；和/或连接所述控制模块与机器人的控制系统；和/或

烧录接口，用于连接用户PC和所述运动模型标定装置以实现算法更新。

本发明与现有技术相比具有如下优点：

本发明实施例提供的一种双轮差速机器人运动模型标定方法，通过用户控制或推行机器人运动6条简单轨迹，并输入轨迹信息即可实现对机器人双轮差速运动模型的参数标定，对工作人员专业技术要求低，节约了标定时间；

本发明通过显示输入界面，引导用户根据输入界面显示内容输入相应参数，进一步降低了标定难度，使从无标定经验的用户也能根据显示提示完成参数的输入，进一步节约了标定成本及标定时间；

通过显示开始和/或起始按钮，使用户可以通过按钮输入机器人运动轨迹的起始和/或终止点位置，进一步提高了标定效率，且用户可以根据实际情况随时起始或终止运动轨迹，用户体验更好。

附图说明

图1为本发明实施例提供的一种双轮差速机器人运动模型标定方法流程图；

图2为本发明实施例提供的运动模型示意图；

图3为本发明实施例提供的一种双轮差速机器人运动模型标定装置组成图；

图4为本发明实施例提供的一种双轮差速机器人里程计系统外观示意图

图5为本发明一具体实施例提供的编码器信号采集流程图；

图6为本发明一具体实施例提供的机器人速度PI闭环控制流程图。

具体实施方式

以下将结合附图及具体实施例，对本发明的具体实施方式做进一步详细说明。

参见图1，本发明实施例提供了一种双轮差速机器人运动模型标定方法，包括：

步骤101：获取用户输入的第一参数并实时获取与所述第一参数对应的机器人左、右轮电机转动次数，所述第一参数包括机器人运动时的第一直行前进轨迹信息、第一直行后退轨迹信息、原地左转轨迹信息及原地右转轨迹信息；

具体地，本发明实施例中所述的轨迹信息可以包含轨迹标识以及机器人x、y方向运动距离，还可以包含机器人运动角度等；

在本发明实施例中，可以通过用户推行或控制机器人行走得到上述轨迹信息；各轨迹的长度、起始点、结束点均可以由用户设置；

步骤102：根据所述第一参数及对应的机器人左、右轮电机转动次数，确定机器人的姿态标定参数；

本发明实施例中，机器人左轮电机转动参数及右轮电机转动参数可以通过设置在机器人左、右两轮上的编码器获取；其中，第一参数对应的机器人左、右轮电机转动次数包括：机器人从第一直行前进轨迹的起始点到结束点时每一采样时刻对应的左轮转动次数及右轮转动次数、从第一直行后退轨迹的起始点到结束点时每一采样时刻对应的左轮转动次数及右轮转动次数、从原地左转轨迹起始点到结束点时每一采样时刻对应的左轮转动次数及右轮转动次数，以及从原地右转轨迹起始点到结束点时每一采样时刻对应的左轮转动次数及右轮转动次数；

所述姿态标定参数通常为左、右轮转动速度与机器人角速度之间的标定后的转换系数。

步骤103：获取所述用户输入的第二参数，并实时获取与所述第二参数对应的机器人左、右轮电机转动次数，所述第二参数包括第二直行前进轨迹信息和第二直行后退轨迹信息；

步骤104：根据所述第二参数、所述第二参数对应的机器人左、右轮电机转动次数以及所述姿态标定参数，确定机器人的位置标定参数；

具体地，第二参数对应的机器人的左、右轮电机转动次数包括机器人从第二直行前进轨迹的起始点到结束点时每一采样时刻对应的左轮转动次数及右轮转动次数，以及从第二直行后退轨迹的起始点到结束点时每一采样时刻对应的左轮转动次数及右轮转动次数；

所述位置标定参数通常为左、右轮转动速度与机器人线速度之间的标定后的转换系数。

步骤105：根据所述机器人的姿态标定参数及机器人的位置标定参数，标定理论运动模型。

具体地，本发明实施例中，双轮差速移动机器人满足方程

在笛卡尔坐标系下，机器人的位姿(位置和姿态)、速度(线速度和角速度)分别用(x,y,θ)和(υ,ω)表示，模型如图2所示，则机器人运动方程如式(1)所示：

其中，

表示机器人位姿的变化量，即只要给定期望的线速度和角速度，就可以控制当前移动机器人的位姿。

双轮差速移动机器人的运动模型如式(2)所示：

其中，w_R是右轮的旋转速度，w_L是左轮的旋转速度，r_R是右轮半径，r_L是左轮半径，b是轮轴的轴距，C是w_R、w_L转为υ和ω的耦合矩阵，C^-1是υ、ω转为w_R和w_L的解耦矩阵。

本发明实施例中，将所述机器人的姿态标定参数及位置标定参数，代入式(2)中，即以标定后的参数替代理论计算的C矩阵，即可完成模型的标定。

本发明实施例提供的一种双轮差速机器人运动模型标定方法，通过用户控制或推行机器人运动6条简单轨迹，并输入轨迹信息即可实现对机器人双轮差速运动模型的参数标定，对工作人员专业技术要求低，节约了标定时间。

在一可选实施例中，所述获取用户输入的第一参数，包括：

显示第一参数输入界面，并获取用户输入的所述第一参数。

在一可选实施例中，所述获取用户输入的第二参数，包括：

显示第二参数输入界面，并获取用户输入的所述第二参数。

本发明通过显示输入界面，引导用户根据输入界面显示内容输入相应参数，进一步降低了标定难度，使从无标定经验的用户也能根据显示提示完成参数的输入，进一步节约了标定成本及标定时间。

在一可选实施例中，所述的双轮差速机器人运动模型标定方法，还包括：

显示开始按钮，当检测到用户按下所述开始按钮时，确定为机器人运动轨迹的起始点。

显示完成按钮，当检测到用户按下所述完成按钮时，确定为所述机器人运动轨迹的结束点。

通过显示开始和/或起始按钮，使用户可以通过按钮输入机器人运动轨迹的起始和/或结束点位置，进一步提高了标定效率，且用户可以根据实际情况随时起始或终止运动轨迹，用户体验更好。

在一可选实施例中，所述根据所述第一参数及对应的机器人左、右轮电机转动次数，确定机器人的姿态标定参数，包括：

根据下式(3)确定机器人的姿态标定参数：

其中，C_2,1和C_2,2是机器人的姿态标定参数，

是由机器人运动时的第一直行前进轨迹、第一直行后退轨迹、原地左转轨迹及原地右转轨迹4条轨迹生成的广义姿态耦合矩阵，θ_N,m为第m条轨迹结束点处机器人的运动角度，θ_0,m为第m条轨迹起始点处机器人的运动角度，m＝1-4。

在一可选实施例中，所述根据所述第二参数、所述第二参数对应的机器人左、右轮电机转动次数以及所述姿态标定参数(用于求取

详见后文)，确定机器人的位置标定参数，包括：

根据下式(4)确定机器人的位置标定参数：

其中，C_1,1和C_1,2是机器人的位置标定参数，

是由机器人的第二直行前进轨迹和第二直行后退轨迹2条轨迹生成的广义位置耦合矩阵，x_N,n为第n条轨迹结束点处机器人的x方向运动距离，y_N,n为第n条轨迹结束点处机器人的y方向运动距离，x_0,n为第n条轨迹起始点处机器人的x方向运动距离，y_0,n为第n条轨迹起始点处机器人的y方向运动距离，n＝1-2。

在一可选实施例中，所述标定运动模型之后，还包括：

基于标定后的运动模型，输出机器人航迹推测信息，所述航迹推测信息包括位置信息和姿态信息。

具体地，以时间间隔T采集机器人左、右轮的转动次数，经过理论计算可得到右轮的旋转速度w_R和左轮的旋转速度w_L，通过耦合矩阵C计算出当前移动机器人的实际线速度υ_k和角速度ω_k，采用下列公式(5)增量式算法，得到当前航迹推测信息。

其中，k表示第k个采样时间。

在一可选实施例中，所述标定运动模型之后，还包括：

接收用户下发的机器人的期望速度信息；

基于标定后的运动模型，根据所述期望速度信息和机器人当前速度信息，对机器人的速度进行闭环控制，其中所述速度信息包括角速度和/或线速度。

在一可选实施例中，根据所述期望速度信息和当前速度信息，对机器人的速度进行闭环控制，包括：

根据下式(6)-(8)，将机器人左、右轮分别作为独立系统，分别进行闭环控制：

其中，υ_obj为期望线速度、ω_obj为期望角速度；υ_cur为机器人当前线速度、ω_cur为机器人当前角速度；C^-1 _1,1、C^-1 _1,2、C^-1 _2,1、C^-1 _2,2分别为所述机器人的姿态标定参数和位置标定参数的逆；Δw_L为机器人左轮期望旋转速度和当前旋转速度的差、表示左轮P控制量，Δw_R为机器人右轮期望旋转速度和当前旋转速度的差、表示右轮P控制量；Σw_L(t)表示当前控制周期机器人左轮I控制量、Σw_R(t)表示当前控制周期机器人右轮I控制量，Σw_L(t-1)表示上一控制周期机器人左轮I控制量、Σw_R(t-1)表示上一控制周期机器人右轮I控制量；PWM_L为控制机器人左轮速度的PWM输出占空比，PWM_R为控制机器人右轮速度的PWM输出占空比；K_P,L和K_I,L是左轮PI控制参数，K_P,R和K_I,R是右轮的PI控制参数。

参见图3，本发明实施例还提供了一种双轮差速机器人运动模型标定装置，包括：

第一获取模块10，用于获取用户输入的第一参数，并实时获取与所述第一参数对应的机器人左、右轮电机转动次数，所述第一参数包括机器人运动时的第一直行前进轨迹信息、第一直行后退轨迹信息、原地左转轨迹信息及原地右转轨迹信息；

第一确定模块20，用于根据所述第一参数及对应的机器人左、右轮电机转动次数，确定机器人的姿态标定参数；

第二获取模块30，用于获取用户输入的第二参数，并实时获取与所述第二参数对应的机器人左、右轮电机转动次数，所述第二参数包括第二直行前进轨迹信息和第二直行后退轨迹信息；

第二确定模块40，用于根据所述第二参数、所述第二参数对应的机器人左、右轮电机转动次数以及所述姿态标定参数，确定机器人的位置标定参数；

标定模块50，用于根据所述机器人的姿态标定参数和位置标定参数，标定理论运动模型。

在一可选实施例中，所述的双轮差速机器人运动模型标定装置，还包括显示模块，用于显示第一参数输入界面，所述第一获取模块用于获取用户输入的所述第一参数。

在一可选实施例中，所述显示模块还用于显示第二参数输入界面，所述第二获取模块用于获取用户输入的所述第二参数。

在一可选实施例中，所述的双轮差速机器人运动模型标定装置，还包括起始点确定模块：

所述显示模块，还用于显示开始按钮，所述起始点确定模块，用于当检测到用户按下所述开始按钮时，确定为所述机器人运动轨迹的起始点。

在一可选实施例中，所述的双轮差速机器人运动模型标定装置，还包括结束点确定模块：

所述显示模块，还用于显示完成按钮，所述结束点确定模块，用于当检测到用户按下所述完成按钮时，确定为所述机器人运动轨迹的结束点。

在一可选实施例中，所述的双轮差速机器人运动模型标定装置，还包括：

航迹推测模块，用于基于标定后的运动模型，输出机器人航迹推测信息，所述航迹推测信息包括位置信息和姿态信息。

在一可选实施例中，所述的双轮差速机器人运动模型标定装置，还包括：

接收模块，用于接收用户下发的机器人期望速度信息；

控制模块，用于基于标定后的运动模型，根据所述期望速度信息和机器人当前速度信息，对机器人的速度进行闭环控制，其中所述速度信息包括角速度和/或线速度。

本发明装置实施例与方法实施例一一对应，具体描述及效果参见方法实施例在此不再赘述。

参见图4，本发明实施例还提供了一种双轮差速机器人里程计系统，包括壳体1及设置在壳体1内的由上述运动模型标定装置实施例提供的标定装置。

本发明实施例提供的一种双轮差速机器人里程计系统，通过用户控制或推行机器人运动6条简单轨迹，并输入轨迹信息即可实现对机器人双轮差速运动模型的参数标定，对工作人员专业技术要求低，节约了标定时间；通过显示输入界面，引导用户根据输入界面显示内容输入相应参数，进一步降低了标定难度，使从无标定经验的用户也能根据显示提示完成参数的输入，进一步节约了标定成本及标定时间；通过显示开始和/或完成按钮，使用户可以通过按钮输入机器人运动轨迹的起始和/或结束点位置，进一步提高了标定效率，且用户可以根据实际情况随时起始或结束运动轨迹，用户体验更好；通过将装置设置在壳体内，确保在机器人运动过程中装置不易受损。

如图4所示，所述壳体上设有标准接口，用于通过快插线缆将所述运动模型标定装置的各功能引脚与对应的外部设备连接。通过设置标准接口既便于与外部设备连接，又便于将接线隐于壳体内，保护各路信号的接线。

如图4所示，所述标准接口包括：

所述电源接口，用于为里程计系统提供5V安全电压；

所述人机通信接口，用于连接所述接收模块和外部控制系统以使用户下发机器人的期望速度信息，和/或连接所述航迹推测模块和所述外部控制系统以为用户提供机器人航迹推测信息；

所述测量和控制接口，用于连接所述获取模块与机器人的外部测量装置；和/或连接所述控制模块与机器人的控制系统；和/或

所述烧录接口，用于连接用户PC和所述运动模型标定装置以实现算法更新。

以下为本发明的一具体实施例：

本实施例提供了一种双轮差速机器人里程计系统，如图4所示，壳体由6面亚克力板制作而成，底板左右两边略宽，各面设计有公或母拼装螺丝接口，通过螺丝拼装在一起，将电路板(运动模型标定装置)和各路信号连接线保护并隐藏；在里程计外观正面设计有2个标准USB接口(USB-A和USB-B)，其中，USB-A为电源接口，主要为里程计系统提供5v稳定电源，USB-B为烧录接口，通过该接口可将板载算法烧录到里程计系统中；设计有2个标准DB28接口(DB28-A和DB28-B)，DB28-A为左轮电机信号测量和控制接口，DB28-B为右轮电机信号测量和控制接口，通过线缆分别与对应电机的编码器和驱动器相连接；设计有3个标准DB9接口(DB9-A、DB9-B和DB9-C)，其中，DB9-A为人机通信接口，里程计通过该接口与上位计算机进行通讯，用户通过上位计算机对机器人下发速度指令，里程计收到后计算闭环控制PWM输出占空比，最终通过两个DB28接口控制左、右轮电机运动，同时通过DB9-A，还将航迹推测信息发送给上位计算机，DB9-B和DB9-C为预留扩展接口；在里程计外观左右两侧设计有散热口3，散热口3处安装有散热风扇，用于里程计工作期间的散热；在里程计外观底面设计有设备法兰安装接口2，用于设备的固定和安装。

本实施例提供的里程计的标定原理及流程如下：

增量式编码器的信号计数寄存器仅是一个双向的增减计数器，而且寄存器存储空间为16bit，超过上界(下届)会自动跳转，所以需要判断并记录编码器计数方向，信号采集流程图如图5所示，其中“编码状态”包含递增、递减、超过上界和超过下界4种情况。

将双轮差速机器人的运动模型(2)和航迹推测计算公式(5)联立，可以得到关于矩阵C的四个参数C_i,j的线性化等式，i、j为参数标号。对姿态标定参数C_2,1与C_2,2有方程：

其中，N表示轨迹运动期间的采样总次数。

记

则可得等式：

让机器人行走4条轨迹(直线前进、直线后退、原地左转、原地右转)，记录下每一采样时刻的编码器增量和机器人姿态改变绝对值，然后通过最小二乘法转化的广义逆矩阵求解下述方程(10)和前述公式(3)就可得到C_2,1和C_2,2。

对C_1,1和C_1,2，记

则有：

同理可得前述关系式(4)。需要说明的是，Φ_xy中的参数θ_k、ω_k不能直接测得，需间接使用上一步辨识出来的参数C_2,1、C_2,2，并结合运动学方程计算得到。

由上，给出标定流程如下：

1)使机器人行走一段轨迹，记录下每一采样时刻的编码器增量，并更新回归子Φ_θ，记录轨迹始末点上机器人的姿态θ_N与θ₀，保存回归子Φ_θ和姿态改变量θ_N-θ₀；

2)重复流程1四次，按公式(3)计算出C_2,1和C_2,2；

3)使机器人行走一段轨迹，记录下每一采样时刻的编码器增量，并通过已辨识获得的参数C_2,1、C_2,2和运动学方程，计算每一采样时刻的姿态值，并更新回归子Φ_xy，记录轨迹始末点上机器人的位置(x_N,y_N)与(x₀,y₀)，保存回归子Φ_xy和位置改变量x_N-x₀、y_N-y₀；

4)重复流程3两次，按公式(4)计算出参数C_1,1和C_1,2。

参数标定辨识完毕。

本实施例提供的里程计速度PI闭环控制包括：

移动机器人的左轮和右轮被作为独立系统，分别进行PI控制，如图6所示，其中υ_obj、ω_obj为期望线速度和角速度；υ_cur、ω_cur为移动机器人实际线速度和角速度；PWM_L、PWM_R为控制机器人左、右轮速度的PWM输出占空比；解耦、耦合矩阵如式(2)所示；标定后的C矩阵由式(3)和(4)求得。

控制算法如下：

根据前述公式(6)-(8)计算左、右轮PWM输出占空比。板载算法以周期1ms的中断执行上述PI控制算法，上、下位机通讯速率是50ms，即每50ms更新一次期望的控制量，在每50ms中按给定的期望控制量执行50次PI算法，实现动态平衡。

本发明通过简单采集电机左、右轮编码反馈数值即可实现里程计功能，硬件电路的设计非常简单，系统工作稳定可靠。实际应用过程中，用户仅通过推行机器人直行和旋转若干次，即可完成整个标定过程，且需要的场地范围不大，标定的参数精确。基于标定的参数，通过速度PI闭环控制，实现了机器人运行过程中短时间内航迹的准确推算，以及对机器人速度的准确控制。本发明使标定后的机器人运动模型参数更为适配机器人本体和工作地面环境，可广泛应用于双轮差速移动机器人导航控制任务中。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (21)

1.一种双轮差速机器人运动模型标定方法，其特征在于，包括：

获取用户输入的第一参数，并实时获取与所述第一参数对应的机器人左、右轮电机转动次数，所述第一参数包括机器人运动时的第一直行前进轨迹信息、第一直行后退轨迹信息、原地左转轨迹信息及原地右转轨迹信息；

根据所述第一参数及对应的机器人左、右轮电机转动次数，确定机器人的姿态标定参数；

获取所述用户输入的第二参数，并实时获取与所述第二参数对应的机器人左、右轮电机转动次数，所述第二参数包括第二直行前进轨迹信息和第二直行后退轨迹信息；

根据所述第二参数、所述第二参数对应的机器人左、右轮电机转动次数以及所述姿态标定参数，确定机器人的位置标定参数；

根据所述机器人的姿态标定参数及机器人的位置标定参数，标定理论运动模型。

2.根据权利要求1所述的双轮差速机器人运动模型标定方法，其特征在于，所述获取用户输入的第一参数，包括：

显示第一参数输入界面，并获取用户输入的所述第一参数。

3.根据权利要求2所述的双轮差速机器人运动模型标定方法，其特征在于，所述获取所述用户输入的第二参数，包括：

显示第二参数输入界面，并获取所述用户输入的所述第二参数。

4.根据权利要求1所述的双轮差速机器人运动模型标定方法，其特征在于，还包括：

显示开始按钮，当检测到用户按下所述开始按钮时，确定为机器人运动轨迹的起始点。

5.根据权利要求4所述的双轮差速机器人运动模型标定方法，其特征在于，还包括：

显示完成按钮，当检测到用户按下所述完成按钮时，确定为所述机器人运动轨迹的结束点。

6.根据权利要求1-5任一项所述的双轮差速机器人运动模型标定方法，其特征在于，所述轨迹信息包含轨迹标识以及机器人x、y方向运动距离和/或机器人运动角度。

7.根据权利要求6所述的双轮差速机器人运动模型标定方法，其特征在于，所述根据所述第一参数及对应的机器人左、右轮电机转动次数，确定机器人的姿态标定参数，包括：

根据下式确定机器人的姿态标定参数：

其中，C_2,1和C_2,2是机器人的姿态标定参数，

是由机器人运动时的第一直行前进轨迹、第一直行后退轨迹、原地左转轨迹及原地右转轨迹4条轨迹生成的广义姿态耦合矩阵，θ_N,m为第m条轨迹结束点处机器人的运动角度，θ_0,m为第m条轨迹起始点处机器人的运动角度，m＝1-4。

8.根据权利要求7所述的双轮差速机器人运动模型标定方法，其特征在于，所述根据所述第二参数、所述第二参数对应的机器人左、右轮电机转动次数以及所述姿态标定参数，确定机器人的位置标定参数，包括：

根据下式确定机器人的位置标定参数：

其中，C_1,1和C_1,2是机器人的位置标定参数，

是由机器人的第二直行前进轨迹和第二直行后退轨迹2条轨迹生成的广义位置耦合矩阵，x_N,n为第n条轨迹结束点处机器人的x方向运动距离，y_N,n为第n条轨迹结束点处机器人的y方向运动距离，x_0,n为第n条轨迹起始点处机器人的x方向运动距离，y_0,n为第n条轨迹起始点处机器人的y方向运动距离，n＝1-2。

9.根据权利要求8所述的双轮差速机器人运动模型标定方法，其特征在于，所述标定运动模型之后，还包括：

基于标定后的运动模型，输出机器人航迹推测信息，所述航迹推测信息包括位置信息和姿态信息。

10.根据权利要求9所述的双轮差速机器人运动模型标定方法，其特征在于，所述标定运动模型之后，还包括：

接收用户下发的机器人期望速度信息；

基于标定后的运动模型，根据所述期望速度信息和机器人当前速度信息，对机器人进行闭环控制，其中所述速度信息包括角速度和/或线速度。

11.根据权利要求10所述的双轮差速机器人运动模型标定方法，其特征在于，根据所述期望速度信息和当前速度信息，对机器人进行闭环控制，包括：

根据下式分别确定机器人左轮和右轮的PWM输出占空比：

其中，υ_obj为期望线速度、ω_obj为期望角速度；υ_cur为机器人当前线速度、ω_cur为机器人当前角速度；C^-1 _1,1、C^-1 _1,2、C^-1 _2,1、C^-1 _2,2分别为所述机器人的姿态标定参数和位置标定参数的逆；Δw_L为机器人左轮期望旋转速度和当前旋转速度的差、表示左轮P控制量，Δw_R为机器人右轮期望旋转速度和实际旋转速度的差、表示右轮P控制量；Σw_L(t)表示当前控制周期机器人左轮I控制量、Σw_R(t)表示当前控制周期机器人右轮I控制量，Σw_L(t-1)表示上一控制周期机器人左轮I控制量、Σw_R(t-1)表示上一控制周期机器人右轮I控制量；PWM_L为控制机器人左轮速度的PWM输出占空比，PWM_R为控制机器人右轮速度的PWM输出占空比；K_P,L和K_I,L是左轮PI控制参数，K_P,R和K_I,R是右轮的PI控制参数；

根据确定的左、右轮PWM输出占空比，分别对机器人左、右轮进行闭环控制。

12.一种双轮差速机器人运动模型标定装置，其特征在于，包括：

第一获取模块，用于获取用户输入的第一参数并实时获取与所述第一参数对应的机器人左、右轮电机转动次数，所述第一参数包括机器人运动时的第一直行前进轨迹信息、第一直行后退轨迹信息、原地左转轨迹信息及原地右转轨迹信息；

第一确定模块，用于根据所述第一参数及对应的机器人左、右轮电机转动次数，确定机器人的姿态标定参数；

第二获取模块，用于获取所述用户输入的第二参数，并实时获取与所述第二参数对应的机器人左、右轮电机转动次数，所述第二参数包括第二直行前进轨迹信息和第二直行后退轨迹信息；

第二确定模块，用于根据所述第二参数、所述第二参数对应的机器人左、右轮电机转动次数以及所述姿态标定参数，确定机器人的位置标定参数；

标定模块，用于根据所述机器人的姿态标定参数及机器人的位置标定参数，标定理论运动模型。

13.根据权利要求12所述的双轮差速机器人运动模型标定装置，其特征在于，还包括显示模块，用于显示第一参数输入界面，所述第一获取模块用于获取用户输入的所述第一参数。

14.根据权利要求13所述的双轮差速机器人运动模型标定装置，其特征在于，所述显示模块还用于显示第二参数输入界面，所述第二获取模块用于获取所述用户输入的所述第二参数。

15.根据权利要求13所述的双轮差速机器人运动模型标定装置，其特征在于，还包括起始点确定模块：

所述显示模块，还用于显示开始按钮，所述起始点确定模块，用于当检测到用户按下所述开始按钮时，确定为机器人运动轨迹的起始点。

16.根据权利要求15所述的双轮差速机器人运动模型标定装置，其特征在于，还包括结束点确定模块：

所述显示模块，还用于显示完成按钮，所述结束点确定模块，用于当检测到用户按下所述完成按钮时，确定为所述机器人运动轨迹的结束点。

17.根据权利要求12-16任一项所述的双轮差速机器人运动模型标定装置，其特征在于，还包括：

航迹推测模块，用于基于标定后的运动模型，输出机器人航迹推测信息，所述航迹推测信息包括位置信息和姿态信息。

18.根据权利要求17所述的双轮差速机器人运动模型标定装置，其特征在于，还包括：

接收模块，用于接收用户下发的机器人期望速度信息；

控制模块，用于基于标定后的运动模型，根据所述期望速度信息和机器人当前速度信息，对机器人进行闭环控制，其中所述速度信息包括角速度和/或线速度。

19.一种双轮差速机器人里程计系统，其特征在于，包括壳体及设置在壳体内的由权利要求18所述的运动模型标定装置。

20.根据权利要求19所述的双轮差速机器人里程计系统，其特征在于，所述壳体上设有标准接口，用于将所述运动模型标定装置的各功能引脚与对应的外部设备连接。

21.根据权利要求20所述的双轮差速机器人里程计系统，其特征在于，所述标准接口包括：

电源接口，用于为里程计系统提供5V安全电压；

人机通信接口，用于连接所述接收模块和外部控制系统以使用户下发机器人的期望速度信息，和/或连接所述航迹推测模块和所述外部控制系统以为用户提供机器人航迹推测信息；

测量和控制接口，用于连接所述获取模块与机器人的外部测量装置；和/或连接所述控制模块与机器人的控制系统；和/或

烧录接口，用于连接用户PC和所述运动模型标定装置以实现算法更新。

Images