一种基于动态窗口法和动力学模型的移动机器人控制方法

技术领域

本发明涉及移动机器人运动规划与控制技术领域，尤其涉及一种基于动态窗口法和动力学模型的移动机器人控制方法。

背景技术

机器人是一种能够收集所处环境的信息并对其周围环境做出反应以安全地执行某个类型任务的机器。自主机器人可以自主感知并执行某种操作任务，整个过程中无需人类直接控制。自主机器人被广泛应用于医疗护理、安保、军事监视、航空航天、环境监测和清洁领域，尤其是在诸如矿场或核电站内部危险环境，更能体现自主移动机器人的优势和价值。与在环境完全已知并严格受控的制造工厂使用的机器人不同，由于无法预测机器人可能遇到的情况，自主机器人无法直接被程序化来执行预定义的动作。此外，环境可能是动态变化的，因此自主机器人的行为控制必须在线实时改变。由于移动机器人固有的自主性，需要动态响应变化并适应环境，其必须克服包括不确定性、传感误差、可靠性和实时响应问题挑战。

在常见的路径跟踪控制算法中，常将角速度和线速度分别求解。另一方面，现有的控制算法大多只能单独求解角速度或线速度，即横向控制和纵向控制。常见的横向控制算法包括纯跟踪算法、线性二次型调节器和模型预测控制。对于移动机器人来说，其对移动速度的要求较低，而纵向控制算法的引入会向机器人请求更多的计算资源，为平衡跟踪精度和计算成本，许多控制算法通常只做横向控制而将线速度设定为常量。显然，这至少会使机器人丢失跟踪精度或消耗更多的计算资源。同时，当机器人运行环境变大变复杂后，单一常量线速度无法保证运行安全性和跟踪精度。

更重要的是，路径规划算法和路径跟踪控制算法往往是分别完成的，路径跟踪控制算法作为下游任务，通常需要更改自身以适配路径规划算法，不可避免对控制性能造成损害，综上，现有技术存在控制精度不够、计算资源消耗过大以及因规划算法和控制算法不兼容而控制性能较低缺点。

发明内容

本发明提出的一种基于动态窗口法和动力学模型的移动机器人控制方法，解决了现有的问题。

为了实现上述目的，本发明采用了如下技术方案：一种基于动态窗口法和动力学模型的移动机器人控制方法，具体包括以下步骤：

步骤一、以动态窗口法为局部路径规划算法：动态窗口法以全局路径为参考路径，均匀地向参考路径两侧采样N条路径；

步骤二、移动机器人动力学模型建立LQR方程：对路径规划，根据各条路径与障碍物之间的距离、路径与参考路径之间的距离和各个路径之间的变道成本因素计算各条路径代价成本，选择代价成本最低的路径作为最终待跟踪路径，计算得到当前位置与跟踪路径中参考位置之间的稳态误差；

步骤三、引入前馈控制来消除LQR控制的稳态误差：代入前馈输入量，消除误差，最终得到横向角度控制量，即角速度；

步骤四、使用自适应的线速度控制方法：将局部路径规划算法所采样路径的路径成本和相对位置作为变量，自适应地给出控制线速度。

优选的，所述路径编号和路径代价成本为中间过程变量，记为RoadId和RoadCost。

优选的，所述路径跟踪用于控制机器人沿着路径规划算法所规划的路径行驶，并使机器人和参考路径之间的横向偏差和航向偏差达到最小，所述路径跟踪算法最终输出两个参数：使机器人转向的角速度和使机器人行进的线速度，所述角速度用于控制航向误差，角速度和线速度共同决定横向误差。

优选的，所述步骤二中，包括以下流程：

LQR中A和B分别为4x4矩阵和4x1向量，给定机器人参数为整机参数，所述整机参数包括前轮到质心距离a、后轮到质心距离b、前轮侧偏刚度cf、后轮侧偏刚度cr、质量和转动惯量Iz、机器人位置(x,y,theta)和状态(vx,vy)，则A以行优先的存储方式表示为：

0,1,0,0,

0,(cf+cr)/(m*vx),-(cf+cr)/m,(a*cf-b*cr)/(m*vx),

0,0,0,1,

0,(a*cf-b*cr)/(Iz*vx),-(a*cf-b*cr)/Iz,(a*a*cf+b*b*cr)/(Iz*vx)；

B表示为

0,

-cf/m，

0,

-a*cf/Iz；

Q为4x1的向量(1，1，1，1)，R取1.0，采用前向欧拉法对LQR方程进行离散化，所求解的Riccati方程表示为：

P_new＝Ad.transpose()*P_old*Ad-Ad.transpose()*P_old*Bd*

(R+Bd.transpose()*P_old*Bd).inverse()*Bd.transpos

e()*P_old*Ad+Q；

初始化P_old为Q，Ad和Bd对应离散化之间的A和B，具体地，

Ad＝(eye-ts*0.5*A).inverse()*(eye+ts*0.5*A)；

Bd＝ts*B；

eye表示4维单位矩阵，ts为离散分辨率；

最终LQR输出：

K＝(R+Bd.transpose()*P_old*Bd).inverse()*Bd.transpose()*

P_old*Ad；

LQR得到的控制量为-K*errs,errs表示机器人当前位置与跟踪路径中参考位置之间的误差；

假设LQR模型的微分方程为dot(err)＝A*err+B*u，则当LQR稳定时，有dot(err)＝0,而0＝0+B*u不是上述微分方程的解，因此，LQR模型必然存在一个稳态误差。

优选的，所述步骤三中，包括以下流程：所述前馈输入量为

J＝curvature*(a+b-b*k3-((m*vx*vx)/(a+b))*(b/cf+a*k3/cr-a/cr))；

其中curvature是机器人参考路径点的曲率，k3是当前机器人状态下与参考路径点的朝向误差，因此，最终的横向角度控制量为-K*errs+J。

优选的，所述步骤四中，包括以下流程：所述控制线速度即为纵向控制量，所述纵向控制量为

maxspeed*(1-alpha*f(RoadID)-beta*RoadCost)；

其中，maxspeed是机器人所允许的最大线速度，alpha、beta为超参量，用于平衡换道成本和路径代价成本对线速度的影响，这是一种自适应的线速度控制方法，当换道成本和路径代价成本较低时，表明机器人所处环境障碍物较少，机器人离最优路径最近，这时机器人线速度最大，反之，机器人线速度减少，保证行驶安全和跟踪精度。

优选的，所述RoadId和RoadCost是路径规划的中间参数，因此线速度控制方法不对机器人产生计算资源消耗。

优选的，所述控制算法与规划算法完全兼容，不会因为适配规划算法而损失控制精度。

本发明的有益效果为：

1、本发明提出了一种基于动态窗口法、动力学线性二次型调节器和前馈控制的移动机器人路径跟踪控制算法，准确求解控制角速度，同时不占用任何新计算资源的情况下根据机器人所处环境自适应的给出线速度。

2、本发明综合考虑路径规划算法和路径跟踪算法，既巧妙地利用了路径规划算法的中间结果不占用新计算资源前提下求解自适应线速度，又解决了控制算法因为兼容规划算法而损失性能的问题。

附图说明

图1为本发明的方法流程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

参照图1所示，一种基于动态窗口法和动力学模型的移动机器人控制方法，具体包括以下步骤：

步骤一、以动态窗口法为局部路径规划算法：动态窗口法以全局路径为参考路径，均匀地向参考路径两侧采样N条路径；

步骤二、移动机器人动力学模型建立LQR方程：对路径规划，根据各条路径与障碍物之间的距离、路径与参考路径之间的距离和各个路径之间的变道成本因素计算各条路径代价成本，选择代价成本最低的路径作为最终待跟踪路径，计算得到当前位置与跟踪路径中参考位置之间的稳态误差；

步骤三、引入前馈控制来消除LQR控制的稳态误差：代入前馈输入量，消除误差，最终得到横向角度控制量，即角速度；

步骤四、使用自适应的线速度控制方法：将局部路径规划算法所采样路径的路径成本和相对位置作为变量，自适应地给出控制线速度。

路径编号和路径代价成本为中间过程变量，记为RoadId和RoadCost，路径跟踪用于控制机器人沿着路径规划算法所规划的路径行驶，并使机器人和参考路径之间的横向偏差和航向偏差达到最小，路径跟踪算法最终输出两个参数：使机器人转向的角速度和使机器人行进的线速度，角速度用于控制航向误差，角速度和线速度共同决定横向误差。

步骤二中，包括以下流程：

LQR中A和B分别为4x4矩阵和4x1向量，给定机器人参数为整机参数，LQR为线性二次型调节器，整机参数包括前轮到质心距离a、后轮到质心距离b、前轮侧偏刚度cf、后轮侧偏刚度cr、质量和转动惯量Iz、机器人位置(x,y,theta)和状态(vx,vy)，则A以行优先的存储方式表示为：

0,1,0,0,

0,(cf+cr)/(m*vx),-(cf+cr)/m,(a*cf-b*cr)/(m*vx),

0,0,0,1,

0,(a*cf-b*cr)/(Iz*vx),-(a*cf-b*cr)/Iz,(a*a*cf+b*b*cr)/(Iz*vx)；

B表示为

0,

-cf/m，

0,

-a*cf/Iz；

Q为4x1的向量(1，1，1，1)，R取1.0。采用前向欧拉法对LQR方程进行离散化，所求解的Riccati方程表示为：

P_new＝Ad.transpose()*P_old*Ad-Ad.transpose()*P_old*Bd*

(R+Bd.transpose()*P_old*Bd).inverse()*Bd.transpos

e()*P_old*Ad+Q；

初始化P_old为Q，Ad和Bd对应离散化之间的A和B，具体地，

Ad＝(eye-ts*0.5*A).inverse()*(eye+ts*0.5*A)；

Bd＝ts*B；

eye表示4维单位矩阵，ts为离散分辨率。

最终LQR输出：

K＝(R+Bd.transpose()*P_old*Bd).inverse()*Bd.transpose()*

P_old*Ad；

LQR得到的控制量为-K*errs,errs表示机器人当前位置与跟踪路径中参考位置之间的误差，假设LQR模型的微分方程为dot(err)＝A*err+B*u，则当LQR稳定时，有dot(err)＝0,而0＝0+B*u不是上述微分方程的解，因此，LQR模型必然存在一个稳态误差。

步骤三中，包括以下流程：前馈输入量为

J＝curvature*(a+b-b*k3-((m*vx*vx)/(a+b))*(b/cf+a*k3/cr-a/cr))；

其中curvature是机器人参考路径点的曲率，k3是当前机器人状态下与参考路径点的朝向误差，因此，最终的横向角度控制量为-K*errs+J，引入前馈控制消除了LQR的稳态误差，提升了横向控制精度，角速度即横向控制，线速度即使纵向控制。

步骤四中，包括以下流程：控制线速度即为纵向控制量，纵向控制量为

maxspeed*(1-alpha*f(RoadID)-beta*RoadCost)；

其中，maxspeed是机器人所允许的最大线速度，alpha、beta为超参量，用于平衡换道成本和路径代价成本对线速度的影响，这是一种自适应的线速度控制方法，当换道成本和路径代价成本较低时，表明机器人所处环境障碍物较少，机器人离最优路径最近，这时机器人线速度最大，反之，机器人线速度减少，保证行驶安全和跟踪精度，利用规划算法中间过程变量，在不消耗任何计算资源的情况下给出自适应线速度，RoadId和RoadCost是路径规划的中间参数，因此线速度控制方法不对机器人产生计算资源消耗，控制算法与规划算法完全兼容，不会因为适配规划算法而损失控制精度，综合考虑规划算法和控制算法，降低了计算资源成本，解决了兼容带来的性能损害问题。

使用动态窗口法以全局路径为参考路径，均匀地向参考路径两侧采样N条路径，移动机器人动力学模型建立线性二次型调节器方程求解控制角速度，同时，引入前馈控制来消除线性二次型调节器控制的稳态误差，提升了横向控制精度。此外，将局部路径规划算法所采样路径的路径成本和相对位置作为变量，自适应地给出控制线速度，综合考虑路径规划算法和路径跟踪算法，既巧妙地利用了路径规划算法的中间结果，不占用新计算资源前提下求解自适应线速度，又解决了控制算法因为兼容规划算法而损失性能的问题。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。