CN107450308B - 存储设备、机器人 - Google Patents

存储设备、机器人 Download PDF

Info

Publication number
CN107450308B
CN107450308B CN201710474651.8A CN201710474651A CN107450308B CN 107450308 B CN107450308 B CN 107450308B CN 201710474651 A CN201710474651 A CN 201710474651A CN 107450308 B CN107450308 B CN 107450308B
Authority
CN
China
Prior art keywords
robot
time
control
moment
target
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.)
Active
Application number
CN201710474651.8A
Other languages
English (en)
Other versions
CN107450308A (zh
Inventor
韩勇
万海兵
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Jidao robot Co.,Ltd.
Original Assignee
Zhongdao Robot Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zhongdao Robot Technology Co ltd filed Critical Zhongdao Robot Technology Co ltd
Priority to CN201710474651.8A priority Critical patent/CN107450308B/zh
Publication of CN107450308A publication Critical patent/CN107450308A/zh
Application granted granted Critical
Publication of CN107450308B publication Critical patent/CN107450308B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B11/00—Automatic controllers
    • G05B11/01—Automatic controllers electric
    • G05B11/36—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential
    • G05B11/42—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential for obtaining a characteristic which is both proportional and time-dependent, e.g. P. I., P. I. D.
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D27/00—Simultaneous control of variables covered by two or more of main groups G05D1/00 - G05D25/00
    • G05D27/02—Simultaneous control of variables covered by two or more of main groups G05D1/00 - G05D25/00 characterised by the use of electric means

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本发明公开了一种存储设备、机器人。本技术方案在平面坐标系中建立运动学坐标系，以k时刻的指令轨迹速度为前馈速度，k‑1时刻机器人位置与运动学坐标系的原点之间沿指令轨迹的航向上的距离为位置偏差量，借助PID控制器辅提高了位置控制的精度并减小了对指令轨迹跟踪的时滞问题。引入曲率圆模型，根据k‑1时刻机器人偏离曲率圆的距离、机器人在k‑1时刻的控制线速度并结合PID控制器以得到k‑1时刻机器人角速度分量，并利用k‑1时刻机器人角速度分量对k‑1时刻的控制角速度进行调整以得到精确的控制角速度，解决了线速度与角速度不匹配的问题。

Description

存储设备、机器人
技术领域
本发明涉及导航控制技术领域，尤其涉及一种存储设备、机器人。
背景技术
《农业机械学报》(2012年04期)上刊登了一篇文献，题为《果园移动机器人曲线路径导航控制》，该文献公开了一种利用激光扫描获取果树位置信息，提出了一种果园环境下曲线导航路径的拟合算法，构建了以横向偏差和航向偏差为输入的模糊控制器，实现了移动机器人在曲线路径中的自动行走。机器人以0.54m/s的速度沿正弦曲线行走，最大横向偏差为0.40m，平均偏差为0.12m。试验结果表明，该系统路径拟合和导航控制算法性能良好。
该技术方案实现过程如下：
针对曲线路径的情况进行研究，采用二次曲线法拟合导航路径。取移动机器人行驶路径的切线作为导航路径，切线与y轴形成的夹角为移动机器人的航向偏差θ，车辆后轴中心与切线的距离为移动机器人的横向偏差 d。基于模糊控制理论设计导航控制算法，构建以移动机器人的航向偏差θ和后轮中心的横向偏差d作为输入量，以理论前轮偏转角φ为输出量的模糊控制器。通过计算与实际前轮偏转角的差值，计算出实际的前轮调整角度Δφ，使用脉冲宽度调制(Pulse Width Modulation，PWM)控制，将调整角度转变为调整电压增量Δu，控制方向盘转动。
但是上述技术方案有如下技术缺陷：
1、未考虑轨迹跟踪滞后问题，即线速度控制问题。如果轨迹跟踪滞后，那么在轨迹曲线变化率比较大时，轨迹跟踪滞后会导致更大的精度偏差。
2、未考虑控制角速度与线速度的关系问题。单独使用模糊控制器，不能解决机器人不同速度下角速度精确控制问题。
发明内容
本发明就是针对上述问题，提供了一种存储设备，该存储设备存储有多条指令，所述指令适于由处理器加载并执行如下步骤：
按照通信周期获取指令轨迹在k时刻的目标位置，并以该目标位置为原点，该指令轨迹的航向为X轴方向在平面坐标系内建立运动学坐标系；
以k时刻的指令轨迹速度为前馈速度，k-1时刻机器人位置与指令轨迹在k时刻的目标位置之间沿指令轨迹的航向上的距离为位置偏差量，并结合PID控制器确定机器人在k-1时刻的控制线速度；
确定指令轨迹在k时刻的目标位置在平面坐标系下的曲率半径；
根据机器人在k-1时刻的控制线速度和曲率半径得到机器人在k-1时刻的控制角速度；
根据k-1时刻机器人偏离曲率圆的距离、机器人在k-1时刻的控制线速度并结合PID控制器以得到k-1时刻机器人角速度分量；
利用所述k-1时刻机器人角速度分量对k-1时刻的控制角速度进行调整以得到k-1时刻机器人的角速度控制量。
可选的，所述指令适于由处理器加载并执行的步骤还包括：
基于所述机器人在k-1时刻的控制线速度和k-1时刻机器人的角速度控制量确定机器人的两轮电机转速；
以所述两轮电机转速作为指令控制机器人移动。
可选的，所述位置偏差量通过如下公式计算得到：
offset_x(k-1)＝target_x(k)*cos(-target_heading(k))-target_y(k)*sin(-target_heading(k)) +robot_y(k-1)*sin(-target_heading(k))-robot_x(k-1)*cos(-target_heading(k))
其中，offset_x(k-1)为k-1时刻机器人位置与指令轨迹在k时刻的目标位置之间沿指令轨迹的航向上的距离、(target_x(k)，target_y(k))为指令轨迹在k时刻的目标位置、target_heading(k)为该指令轨迹的航向、 (robot_x(k-1),robot_y(k-1))为k-1时刻机器人位置。
可选的，机器人在k-1时刻的控制线速度通过如下公式计算得到：
Figure GDA0002353954830000021
其中，control_velo(k-1)为机器人在k-1时刻的控制线速度，target_v(k)为k 时刻的指令轨迹速度，offset_x(k-1)为(k-1)时刻的位置偏差量，offset_x(k-2) 为(k-2)时刻的位置偏差量，P1、I1和D1为PID控制器的参数。
可选的，确定指令轨迹在k时刻的目标位置在平面坐标系下的曲率半径采用的公式如下：
Figure GDA0002353954830000022
其中，
Figure GDA0002353954830000023
Figure 1
heading(k)为k时刻指令轨迹的切向角，heading(k-1)为k-1时刻指令轨迹的切向角、(target_x(k)，target_y(k))为指令轨迹在k时刻的目标位置、(target_x(k-1)，target_y(k-1))为指令轨迹在k-1时刻的目标位置，s(k) 指令轨迹在k时刻与k-1时刻的偏移距离。
可选的，根据k-1时刻机器人位置偏离曲率圆的距离、机器人在k-1 时刻的控制线速度并结合PID控制器以得到k-1时刻机器人角速度分量包括：
1)建立曲率圆并确定该曲率圆的圆心坐标(center_x(k)，center_y(k))，(center_x(k-1)，center_y(k-1))；
2)依照如下公式确定k-1时刻机器人位置(robot_x(k-1)，robot_y(k-1)) 到曲率圆的圆心的距离r_robot(k-1)：
Figure GDA0002353954830000032
依照如下公式确定k-2时刻机器人位置(robot_x(k-2)，robot_y(k-2)) 到曲率圆的圆心的距离r_robot(k-2)：
Figure GDA0002353954830000033
3)k-1时刻机器人位置偏离曲率圆的距离offset_r(k-1)＝R(k)-r_robot(k-1)，
其中，R(k)为指令轨迹在k时刻的目标位置在平面坐标系下的曲率半径；
k-2时刻机器人位置偏离曲率圆的距离
offset_r(k-2)＝R(k-1)-r_robot(k-2)，其中，R(k-1)为指令轨迹在k-1时刻的目标位置在平面坐标系下的曲率半径；
4)依照如下公式确定k-1时刻机器人角速度分量：
Figure GDA0002353954830000034
其中，omg_offset_r(k-1)为k-1时刻机器人角速度分量，control_velo(k-1)为机器人在k-1时刻的控制线速度、P2、I2和D2为PID控制器参数。
可选的，利用所述k-1时刻机器人角速度分量对k-1时刻的控制角速度进行调整以得到k-1时刻机器人的角速度控制量的方式如下：
control_omg(k-1)＝omg_R(k-1)+omg_offset_r(k-1)
其中，control_omg(k-1)为k时刻机器人的角速度控制量、omg_R(k-1)为 k-1时刻的控制角速度。
可选的，所述通信周期为10毫秒。
可选的，基于所述机器人在k-1时刻的控制线速度和k-1时刻机器人的角速度控制量确定机器人的两轮电机转速包括：
左轮电机转速velo_motor_left＝
(2*control_velo(k-1)-control_omg(k-1)*WHEEL_DIS)/2.0*reduce_ratio；
右轮电机转速velo_motor_right＝
(2*control_velo(k-1)+control_omg(k-1)*WHEEL_DIS)/2.0*reduce_ratio；
其中，control_velo(k-1)为机器人在k时刻的控制线速度、control_omg(k-1)为k-1时刻机器人的角速度控制量、WHEEL_DIS为两轮间距、reduce_ratio为减速机的减速比。
本发明实施例还提供了一种机器人，包括：
上述存储设备；
处理器，适于处理存储设备存储的各种指令。
与现有技术相比，本发明技术方案至少具有以下有益效果：
通过在平面坐标系中建立运动学坐标系，以k时刻的指令轨迹速度为前馈速度，k-1时刻(即当前时刻)机器人位置与运动学坐标系的原点(即指令轨迹在k时刻的目标位置)之间沿指令轨迹的航向(运动学坐标系的 X轴方向)上的距离为位置偏差量，借助PID控制器辅提高了位置控制的精度并减小了对指令轨迹跟踪的时滞问题。
进一步，引入曲率圆模型，根据k-1时刻机器人偏离曲率圆的距离、机器人在k-1时刻的控制线速度并结合PID控制器以得到k-1时刻机器人角速度分量，并利用k-1时刻机器人角速度分量对k-1时刻的控制角速度进行调整以得到精确的控制角速度，解决了线速度与角速度不匹配的问题。
附图说明
图1是本发明实施例的一种存储设备存储的多条指令由处理器加载并执行时的步骤流程示意图；
图2是本发明实施例在平面坐标系中建立运动学坐标系的示意图；
图3是建立曲率圆和确定曲率圆的圆心的示意图；
图4是本发明实施例的信号控制流程示意图。
具体实施方式
为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施方式做详细的说明。
如图1所示的是本发明实施例的存储设备存储的多条指令由处理器加载并执行时的步骤流程示意图。参考图1，该步骤流程包括：
步骤S1：按照通信周期获取指令轨迹在k时刻的目标位置，并以该目标位置为原点，该指令轨迹的航向为X轴方向在平面坐标系内建立运动学坐标系；
步骤S2：以k时刻的指令轨迹速度为前馈速度，k-1时刻机器人位置与指令轨迹在k时刻的目标位置之间沿指令轨迹的航向上的距离为位置偏差量，并结合PID控制器确定机器人在k-1时刻的控制线速度；
步骤S3：确定指令轨迹在k时刻的目标位置在平面坐标系下的曲率半径；
步骤S4：根据机器人在k-1时刻的控制线速度和k时刻的曲率半径得到机器人在k-1时刻的控制角速度；
步骤S5：根据k-1时刻机器人偏离曲率圆的距离、机器人在k-1时刻的控制线速度并结合PID控制器以得到k-1时刻机器人角速度分量；
步骤S6：利用所述k-1时刻机器人角速度分量对k-1时刻的控制角速度进行调整以得到k-1时刻机器人的角速度控制量。
本实施例提供的存储设备中的指令可用于对机器人的移动轨迹进行控制，以使机器人根据指令轨迹来移动。机器人根据指令轨迹来移动是指：机器人按照通信周期在当前时刻接收下某一时刻指令轨迹所要求达到的目标位置，然后根据机器人当前时刻所在位置与指令轨迹下一时刻所要求达到的目标位置之间的位置偏差量以及其他相关参数，确定当前时刻对机器人的控制线速度和角速度控制量，使得在下一时刻时机器人所处位置能够接近指令轨迹中的下一时刻目标位置(控制精度在10厘米)或者与指令轨迹的目标位置完全吻合。其中，线速度和角速度是物理学上的概念，线速度是物体在单位时间内移动的弧长、角速度是物体在单位时间内转过的角度。机器人的移动轨迹是曲线，通过确定机器人在各个时刻的控制线速度和角速度控制量，可以控制机器人的移动轨迹。
在实际应用中，机器人在当前时刻接收到下一时刻的指令轨迹的目标位置后，即根据该指令轨迹的目标位置计算得到对机器人的指令控制量(即控制线速度和角速度控制量)，并依照该指令控制量控制机器人移动。如果机器人未按照指令轨迹在下一时刻到达目标位置，机器人可以通过调节 PID控制器的参数逐渐收敛到该指令轨迹的目标位置。
其中，针对PID控制器的作用，本领域技术人员理解，PID控制器是控制工程常用的一种使得被控对象(即本实施例中的机器人)收敛于目标 (即接收到的指令轨迹在下一时刻的目标位置)的一种方法。
PID控制器可以根据系统的误差，利用比例(P)、积分(I)、微分 (D)计算出控制量进行控制的。比例控制是一种最简单的控制方式，其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差。在积分控制中，控制器的输出与输入误差信号的积分成正比关系。在微分控制中，控制器的输出与输入误差信号的微分(即误差的变化率)成正比关系。
PID控制器的参数整定是控制系统设计的核心内容。它是根据被控过程的特性确定PID控制器的比例系数、积分时间和微分时间的大小。
PID控制器参数整定的方法很多，概括起来有两大类：一是理论计算整定法。它主要是依据系统的数学模型，经过理论计算确定控制器参数。这种方法所得到的计算数据未必可以直接用，还必须通过工程实际进行调整和修改。二是工程整定方法，它主要依赖工程经验，直接在控制系统的试验中进行，且方法简单、易于掌握，在工程实际中被广泛采用。
PID控制器参数的工程整定方法，主要有临界比例法、反应曲线法和衰减法。三种方法各有其特点，其共同点都是通过试验，然后按照工程经验公式对控制器参数进行整定。但无论采用哪一种方法所得到的控制器参数，都需要在实际运行中进行最后调整与完善。现在一般采用的是临界比例法。
利用该方法进行PID控制器参数的整定步骤如下：
1)首先预选择一个足够短的采样周期让系统工作；
2)仅加入比例控制环节，直到系统对输入的阶跃响应出现临界振荡，记下这时的比例放大系数和临界振荡周期；
3)在一定的控制度下通过公式计算得到PID控制器的参数。
本实施例中，机器人接收指令轨迹的通信周期与执行指令的执行周期相同。即在接收到当前时刻的指令轨迹后到接收下一时刻的指令轨迹前，执行完成对当前时刻的指令轨迹的处理和执行。
下面结合具体实施例对本技术方案详细描述。
如步骤S1所述，按照通信周期获取指令轨迹在k时刻的目标位置，并以该目标位置为原点，以该指令轨迹的航向(k时刻的指令轨迹航向？是的)为X轴方向在平面坐标系内建立运动学坐标系。
具体地，机器人在当前时刻可以按照设定的通信周期以及网络通信协议从服务器端获取指令轨迹在下一时刻的目标位置。在本实施例中，当前时刻是k-1时刻，下一时刻是k时刻，通信周期为10毫秒、网络通信协议为MQTT(Message Queuing Telemetry Transport，消息队列遥测传输)。
机器人在k-1时刻获取到的指令轨迹在k时刻的目标位置，机器人在k 时刻就需要到达(或者接近)该位置，最佳状态是机器人在k时刻所处位置与指令轨迹在k时刻的目标位置重合。
所述指令轨迹是存储于服务器端的一条曲线轨迹，曲线轨迹上包括多个目标位置。在本实施例中，通信周期为10毫秒，时间非常短，这些目标位置非常密集。并且，机器人获取到的指令轨迹在下一时刻的目标位置至少包括以下参数信息：时刻，k时刻的指令轨迹速度(是指线速度)，目标位置的坐标，k时刻指令轨迹航向。其中，目标位置的坐标是指该时刻目标位置在平面坐标系内的坐标。所述平面坐标系(即xoy坐标系)是机器人当前地图的坐标系，可以按照人为设定，所述航向概念上与现有技术相同。
根据获取到的上述信息，机器人在平面坐标系内建立运动学坐标系。
如图2所示的是本发明实施例在平面坐标系中建立运动学坐标系的示意图。参考图2，所示平面坐标系(即xoy坐标系)如上文所述，是机器人当前地图的坐标系，可以按照人为设定。该运动学坐标系是以指令轨迹在k时刻的目标位置为原点(图中的Q点)，该指令轨迹的航向(即图中 target_heading(k)方向)为运动学坐标系的X轴方向，Y轴方向是在以X轴逆时针90度方向，如图2中所示。在其他实施例中，所述Y轴方向也可以设置为是以X轴顺时针90度方向。
需要说明的是，本实施例中，公式里的各个坐标都是以平面坐标系中的坐标来表示和推导的。
如步骤S2所述，以k时刻的指令轨迹速度为前馈速度，k-1时刻机器人位置与指令轨迹在k时刻的目标位置之间沿指令轨迹的航向上的距离为位置偏差量，并结合PID控制器确定机器人在k-1时刻的控制线速度。
具体地，所述k时刻的指令轨迹速度是指线速度。继续参考图2，A 点为k-1时刻(即当前时刻)机器人位置，设A点在平面坐标系下的坐标为(robot_x(k-1),robot_y(k-1))，A点距离运动学坐标系的X轴和Y轴的偏移量分别为offset_x(k-1)和offset_y(k-1)。此时，以k-1时刻机器人位置与指令轨迹在k时刻的目标位置之间在沿指令轨迹的航向上的距离offset_x(k-1) 作为位置偏差量，即A点与Y轴的偏移距离。
所述位置偏差量通过如下公式计算得到：
offset_x(k-1)＝target_x(k)*cos(-target_heading(k))-target_y(k)*sin(-target_heading(k))+ robot_y(k-1)*sin(-target_heading(k))-robot_x(k-1)*cos(-target_heading(k))
其中，offset_x(k-1)为k-1时刻机器人位置与指令轨迹在k时刻的目标位置之间沿指令轨迹的航向上的距离、(target_x(k)，target_y(k))为指令轨迹在k时刻的目标位置、target_heading(k)为该指令轨迹的航向、 (robot_x(k-1),robot_y(k-1))为k-1时刻机器人位置。
机器人在k-1时刻的控制线速度通过如下公式计算得到：
Figure GDA0002353954830000081
其中，control_velo(k-1)为机器人在k-1时刻的控制线速度，target_v(k)为 k时刻的指令轨迹速度，offset_x(k-1)为(k-1)时刻的位置偏差量， offset_x(k-2)为(k-2)时刻的位置偏差量，P1、I1和D1为PID控制器的参数。
如步骤S3所述，确定指令轨迹在k时刻的目标位置在平面坐标系下的曲率半径。
曲率半径计算公式R＝lim|Δs/Δα|，当Δs趋向于0时，定义R就是曲率半径。Δα为当前时刻指令轨迹目标位置的切向角与上一时刻指令轨迹目标位置的切向角之差。Δs为当前时刻与下一时刻轨迹目标位置的偏移距离。
由于本实施例中通信周期为10毫秒(即两个时刻之间定义为10毫秒)，两时刻位置点距离足够短，默认为此时Δs为极限值，则R＝|Δs/Δα|。
在本实施例中，指令轨迹在k时刻的目标位置在平面坐标系下的曲率半径采用的公式如下：
Figure GDA0002353954830000082
其中，
Figure GDA0002353954830000083
Figure 2
heading(k)为k时刻指令轨迹的切向角，heading(k-1)为k-1时刻指令轨迹的切向角，(target_x(k)，target_y(k))为指令轨迹在k时刻的目标位置、(target_x(k-1)，target_y(k-1))为指令轨迹在k-1时刻的目标位置，s(k) 指令轨迹在k时刻与k-1时刻的偏移距离。其中，
Figure 4
Figure 13
由于k时刻和k-1时刻的指令轨迹的目标位置之间的距离足够短，则可将指令轨迹的目标位置点的切线方向角等效为k时刻与k-1时刻指令轨迹上两点连线的方向角。
如步骤S4所述，根据机器人在k-1时刻的控制线速度和曲率半径得到机器人在k-1时刻的控制角速度。
具体地，根据角速度与线速度的关系可知，机器人在k时刻的控制角速度的计算公式如下：
omg_R(k-1)＝control_velo(k-1)/R(k)
其中，omg_R(k-1)为机器人在k-1时刻的控制角速度、control_velo(k-1)为机器人在k-1时刻的控制线速度、R(k)为机器人在k时刻的曲率半径。
如步骤S5所述，根据k-1时刻机器人偏离曲率圆的距离、机器人在 k-1时刻的控制线速度并结合PID控制器以得到k-1时刻机器人角速度分量。
具体地，本步骤包括如下步骤：
1)建立曲率圆并确定该曲率圆的圆心坐标(center_x(k)，center_y(k))，
(center_x(k-1)，center_y(k-1))。
如图3所示的是本发明实施例建立曲率圆和确定曲率圆的圆心的示意图。参考图3，在曲率圆QR上，用直线分别连接曲率圆上的两点(A1和 A2)到圆心，分别求两条直线L1和L2的方程，结合两个方程求取两条直线的交点，该交点即为曲率圆QR的圆心。其中，A1和A2分别是k时刻和k-1时刻指令轨迹的目标位置。
假设tan(heading(k))≠0，根据公式：
Figure 10
根据上述L1和L2的方程可求得x、y，即为圆心坐标(center_x(k)， center_y(k))。
Figure 11
Figure 12
2)依照如下公式确定k-1时刻机器人位置(robot_x(k-1)，robot_y(k-1))
到曲率圆的圆心的距离r_robot(k-1)：
Figure GDA0002353954830000103
依照如下公式确定k-2时刻机器人位置(robot_x(k-2)，robot_y(k-2))
到曲率圆的圆心的距离r_robot(k-2)：
Figure GDA0002353954830000104
3)k-1时刻机器人位置偏离曲率圆的距离offset_r(k-1)＝R(k)-r_robot(k-1)，其中，R(k)为指令轨迹在k时刻的目标位置在平面坐标系下的曲率半径；
k-2时刻机器人位置偏离曲率圆的距离
offset_r(k-2)＝R(k-1)-r_robot(k-2)，其中，R(k-1)为指令轨迹在k-1时刻的目标位置在平面坐标系下的曲率半径。
4)依照如下公式确定机器人角速度分量：
Figure GDA0002353954830000105
其中，omg_offset_r(k-1)为k-1时刻机器人角速度分量，control_velo(k-1)为机器人在k-1时刻的控制线速度、P2、I2和D2为PID控制器参数。
如步骤S6所述，利用所述k-1时刻机器人角速度分量对k-1时刻的控制角速度进行调整以得到k-1时刻机器人的角速度控制量。
具体地，按照如下公式得到k-1时刻机器人的角速度控制量。
control_omg(k-1)＝omg_R(k-1)+omg_offset_r(k-1)
进一步地，在本实施例中，在得到了机器人在k-1时刻的控制线速度和k-1时刻机器人的角速度控制量后，将其用于控制机器人的两轮电机转速。
具体地，机器人的两轮电机转速的计算公式如下：
左轮电机转速velo_motor_left＝
(2*control_velo(k-1)-control_omg(k-1)*WHEEL_DIS)/2.0*reduce_ratio；
右轮电机转速velo_motor_right＝
(2*control_velo(k-1)+control_omg(k-1)*WHEEL_DIS)/2.0*reduce_ratio。
其中，control_velo(k-1)为机器人在k时刻的控制线速度、 control_omg(k-1)为k时刻机器人的角速度控制量、WHEEL_DIS为两轮间距、reduce_ratio为减速机的减速比。
然后，以所述两轮电机转速作为指令控制机器人移动，以使机器人在下一时刻(k时刻)能够尽量接近指令轨迹的k时刻的目标位置。
如图4所示的是本发明实施例的信号控制流程示意图。
参考图4，为了使机器人下一时刻(k时刻)能够尽量移动至指令轨迹在k时刻的目标位置，根据当前时刻(k-1时刻)获取到的下一时刻(k时刻)指令轨迹的目标位置与机器人反馈的在当前时刻(k-1时刻)位置之间的位置偏差量，以k时刻指令轨迹速度为前馈速度，结合PID控制器调节以确定当前时刻机器人的控制线速度。
进一步地，基于指令轨迹在k时刻的目标位置建立曲率圆，根据k-1 时刻机器人位置偏离曲率圆的距离、结合PID控制器调节以确定当前时刻机器人角速度分量。
可以看出，本实施例中，通过在平面坐标系中建立运动学坐标系，以 k时刻的指令轨迹速度为前馈速度，k-1时刻机器人位置与运动学坐标系的原点(即指令轨迹在k时刻的目标位置)之间在指令轨迹的航向(运动学坐标系的X轴方向)上的距离为位置偏差量，借助PID控制器辅提高了位置控制的精度并减小了对指令轨迹跟踪的时滞问题。
进一步，引入曲率圆模型，根据k-1时刻机器人偏离曲率圆的距离、机器人在k-1时刻的控制线速度并结合PID控制器以得到k-1时刻机器人角速度分量，并利用k-1时刻机器人角速度分量对k-1时刻的控制角速度进行调整以得到精确的控制角速度，解决了线速度与角速度不匹配的问题。
基于上述存储设备，本发明实施例还提供了一种机器人，该机器人包括如上述实施例中所述的存储设备，以及处理器，适于处理存储设备存储的各种指令。
本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (10)

1.一种存储设备，其特征在于，该存储设备存储有多条指令，所述指令适于由处理器加载并执行如下步骤：
按照通信周期获取指令轨迹在k时刻的目标位置，并以该目标位置为原点，该指令轨迹的航向为X轴方向在平面坐标系内建立运动学坐标系；
以k时刻的指令轨迹速度为前馈速度，k-1时刻机器人位置与指令轨迹在k时刻的目标位置之间沿指令轨迹的航向上的距离为位置偏差量，并结合PID控制器确定机器人在k-1时刻的控制线速度；
确定指令轨迹在k时刻的目标位置在平面坐标系下的曲率半径；
根据机器人在k-1时刻的控制线速度和曲率半径得到机器人在k-1时刻的控制角速度；
根据k-1时刻机器人偏离曲率圆的距离、机器人在k-1时刻的控制线速度并结合PID控制器以得到k-1时刻机器人角速度分量；
利用所述k-1时刻机器人角速度分量对k-1时刻的控制角速度进行调整以得到k-1时刻机器人的角速度控制量。
2.如权利要求1所述的存储设备，其特征在于，所述指令适于由处理器加载并执行的步骤还包括：
基于所述机器人在k-1时刻的控制线速度和k-1时刻机器人的角速度控制量确定机器人的两轮电机转速；
以所述两轮电机转速作为指令控制机器人移动。
3.如权利要求1所述的存储设备，其特征在于，所述位置偏差量通过如下公式计算得到：
offset_x(k-1)＝target_x(k)*cos(-target_heading(k))-target_y(k)*sin(-target_heading(k))+robot_y(k-1)*sin(-target_heading(k))-robot_x(k-1)*cos(-target_heading(k))
其中，offset_x(k-1)为k-1时刻机器人位置与指令轨迹在k时刻的目标位置之间沿指令轨迹的航向上的距离、(target_x(k)，target_y(k))为指令轨迹在k时刻的目标位置、target_heading(k)为该指令轨迹的航向、(robot_x(k-1)，robot_y(k-1))为k-1时刻机器人位置。
4.如权利要求3所述的存储设备，其特征在于，机器人在k-1时刻的控制线速度通过如下公式计算得到：
Figure FDA0002353954820000011
其中，control_velo(k-1)为机器人在k-1时刻的控制线速度，target_v(k)为k时刻的指令轨迹速度，offset_x(k-1)为(k-1)时刻的位置偏差量，offset_x(k-2)为(k-2)时刻的位置偏差量，P1、I1和D1为PID控制器的参数。
5.如权利要求1所述的存储设备，其特征在于，确定指令轨迹在k时刻的目标位置在平面坐标系下的曲率半径采用的公式如下：
Figure FDA0002353954820000021
其中，
Figure FDA0002353954820000022
Figure 3
heading(k)为k时刻指令轨迹的切向角，heading(k-1)为k-1时刻指令轨迹的切向角、(target_x(k)，target_y(k))为指令轨迹在k时刻的目标位置、(target_x(k-1)，target_y(k-1))为指令轨迹在k-1时刻的目标位置，s(k)指令轨迹在k时刻与k-1时刻的偏移距离。
6.如权利要求5所述的存储设备，其特征在于，根据k-1时刻机器人位置偏离曲率圆的距离、机器人在k-1时刻的控制线速度并结合PID控制器以得到k-1时刻机器人角速度分量包括：
1)建立曲率圆并确定该曲率圆的圆心坐标(center_x(k)，center_y(k))，(center_x(k-1)，center_y(k-1))；
2)依照如下公式确定k-1时刻机器人位置(robot_x(k-1)，robot_y(k-1))
到曲率圆的圆心的距离r_robot(k-1)：
Figure FDA0002353954820000024
依照如下公式确定k-2时刻机器人位置(robot_x(k-2)，robot_y(k-2))
到曲率圆的圆心的距离r_robot(k-2)：
Figure FDA0002353954820000025
3)k-1时刻机器人位置偏离曲率圆的距离offset_r(k-1)＝R(k)-r_robot(k-1)，
其中，R(k)为指令轨迹在k时刻的目标位置在平面坐标系下的曲率半径；
k-2时刻机器人位置偏离曲率圆的距离
offset_r(k-2)＝R(k-1)-r_robot(k-2)，其中，R(k-1)为指令轨迹在k-1时刻的目标位置在平面坐标系下的曲率半径；
4)依照如下公式确定k-1时刻机器人角速度分量：
Figure FDA0002353954820000026
其中，omg_offset_r(k-1)为k-1时刻机器人角速度分量，control_velo(k-1)为机器人在k-1时刻的控制线速度、P2、I2和D2为PID控制器参数。
7.如权利要求6所述的存储设备，其特征在于，利用所述k-1时刻机器人角速度分量对k-1时刻的控制角速度进行调整以得到k-1时刻机器人的角速度控制量的方式如下：
control_omg(k-1)＝omg_R(k-1)+omg_offset_r(k-1)；
其中，control_omg(k-1)为k时刻机器人的角速度控制量、omg_R(k-1)为k-1时刻的控制角速度。
8.如权利要求1所述的存储设备，其特征在于，所述通信周期为10毫秒。
9.如权利要求2所述的存储设备，其特征在于，基于所述机器人在k-1时刻的控制线速度和k-1时刻机器人的角速度控制量确定机器人的两轮电机转速包括：
左轮电机转速velo_motor_left＝(2*control_velo(k-1)-control_omg(k-1)*WHEEL_DIS)/2.0*reduce_ratio；
右轮电机转速velo_motor_right＝(2*control_velo(k-1)+control_omg(k-1)*WHEEL_DIS)/2.0*reduce_ratio；
其中，control_velo(k-1)为机器人在k时刻的控制线速度、control_omg(k-1)为k-1时刻机器人的角速度控制量、WHEEL_DIS为两轮间距、reduce_ratio为减速机的减速比。
10.一种机器人，其特征在于，包括：
如权利要求1至9中任一项所述的存储设备；
处理器，适于处理存储设备存储的各种指令。
CN201710474651.8A 2017-06-21 2017-06-21 存储设备、机器人 Active CN107450308B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710474651.8A CN107450308B (zh) 2017-06-21 2017-06-21 存储设备、机器人

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710474651.8A CN107450308B (zh) 2017-06-21 2017-06-21 存储设备、机器人

Publications (2)

Publication Number Publication Date
CN107450308A CN107450308A (zh) 2017-12-08
CN107450308B true CN107450308B (zh) 2020-05-05

Family

ID=60486322

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710474651.8A Active CN107450308B (zh) 2017-06-21 2017-06-21 存储设备、机器人

Country Status (1)

Country Link
CN (1) CN107450308B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110000792B (zh) * 2019-04-26 2020-08-21 西湖大学 船壁清洗机器人的路径调整方法、装置、设备及存储介质
CN110440806A (zh) * 2019-08-12 2019-11-12 苏州寻迹智行机器人技术有限公司 一种激光与二维码融合的agv精确定位方法
CN116300409B (zh) * 2023-05-17 2023-08-15 季华实验室 一种轨迹跟踪控制方法、装置、电子设备及存储介质

Family Cites Families (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0725307B2 (ja) * 1987-09-25 1995-03-22 本田技研工業株式会社 前輪転舵角の制御方法
JP4294286B2 (ja) * 2002-09-24 2009-07-08 富士重工業株式会社 車両の差動制限制御装置
JP3911492B2 (ja) * 2003-06-26 2007-05-09 トヨタ自動車株式会社 車両用走行支援装置
US8983765B2 (en) * 2006-10-11 2015-03-17 GM Global Technology Operations LLC Method and system for lane centering control
CN101859147A (zh) * 2010-06-11 2010-10-13 哈尔滨工程大学 船舶航向智能协调控制方法
CN102506860B (zh) * 2011-11-26 2015-08-26 中国科学院光电技术研究所 一种基于加速度反馈与前馈的惯性稳定装置及其控制方法
CN102929282A (zh) * 2012-11-30 2013-02-13 天津市中环电子计算机有限公司 基于运动控制卡的智能物流车
CN104881025B (zh) * 2015-04-10 2018-11-27 北京科技大学 一种地下矿用车辆的反应式导航控制方法
CN105549597B (zh) * 2016-02-04 2018-06-26 同济大学 一种基于环境不确定性的无人车动态路径规划方法
CN106482735A (zh) * 2016-10-14 2017-03-08 中国科学院光电技术研究所 一种提高快反镜稳定平台扰动抑制能力的控制方法
CN106527119B (zh) * 2016-11-03 2019-07-23 东华大学 基于模糊控制的微分先行pid控制系统
CN106527459B (zh) * 2016-11-24 2020-01-14 北京星网卫通科技开发有限公司 一种稳定平台以及其控制方法

Also Published As

Publication number Publication date
CN107450308A (zh) 2017-12-08

Similar Documents

Publication Publication Date Title
CN108673496B (zh) 一种基于纯追踪模型的工业机器人路径跟踪方法
CN107450308B (zh) 存储设备、机器人
WO2018209864A1 (zh) 移动控制方法、装置、机器人及存储介质
KR101145112B1 (ko) 자율이동차량의 조향제어장치, 이를 구비하는 자율이동차량 및 자율이동차량의 조향제어방법
AU2016253152A1 (en) System, apparatus, and method for controlling a vehicle
US11024178B2 (en) System and method for autonomously steering a vehicle
Wu et al. Path following for a tractor-trailer system using model predictive control
Krisna et al. Speed control system design using fuzzy-pid for load variation of automated guided vehicle (agv)
JP2002215239A (ja) 車両系の走行制御方法及び装置
CN113428218A (zh) 一种车辆转向控制方法、装置、设备及存储介质
CN208384428U (zh) 基于反向位移补偿的稳定转台系统
CN113525366A (zh) 一种针对钢轮压路机液压横向控制器的横向控制方法
CN111830980B (zh) 一种激光导航路径跟随方法
CN112925327B (zh) 一种二维码惯导导航控制算法
KR102563074B1 (ko) 차동 구동형 이동로봇의 운동역학을 고려한 장애물 회피 및 경로 추종방법
McNinch et al. Application of a coordinated trajectory planning and real-time obstacle avoidance algorithm
JP6894595B2 (ja) 移動体
CN112666986A (zh) 一种天线伺服轨迹规划方法和系统
CN112947487A (zh) 一种自动引导车辆及其曲线路径跟踪方法、控制装置
JP2010149637A (ja) 車両
CN114089730A (zh) 机器人的运动规划方法和自动引导车
CN114253241B (zh) 用于工业智能小车的路径跟踪方法
CN113467460B (zh) 一种基于双圆形前视距离的农业机械路径跟踪方法及系统
CN113885514B (zh) 基于模糊控制和几何追踪的agv路径跟踪方法及系统
Cartade et al. Adaptive and predictive control of a mobile robots fleet: Application to off-road formation regulation

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
GR01 Patent grant
GR01 Patent grant
CP03 Change of name, title or address
CP03 Change of name, title or address

Address after: 322000 2nd Floor, Building 14, Kechuang Garden, 968 Xuefeng West Road, Beiyuan Street, Yiwu City, Jinhua City, Zhejiang Province

Patentee after: Zhejiang Zhong Dao Robot Technology Co.,Ltd.

Address before: 230601 Haiheng Building 543 No. 6 Cuiwei Road, Hefei Economic and Technological Development Zone, Anhui Province

Patentee before: ZHONGDAO ROBOT TECHNOLOGY Co.,Ltd.

TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20210209

Address after: 201700 block B, room 126, area C, building 6, No. 500, Huapu Road, Qingpu District, Shanghai

Patentee after: Shanghai Jidao robot Co.,Ltd.

Address before: 322000 2nd Floor, Building 14, Kechuang Garden, 968 Xuefeng West Road, Beiyuan Street, Yiwu City, Jinhua City, Zhejiang Province

Patentee before: Zhejiang Zhong Dao Robot Technology Co.,Ltd.