机器人的轨迹规划与自动导引

轨迹生成

轨迹规划（Trajectory Planning）包括两个方面：对于移动机器人（mobile robot）偏向于指移动的路径轨迹规划（path planning），如机器人是在有地图条件或是没有地图的条件下，按什么样的路径轨迹来行走；对于工业机器人/操作臂（manipulator）则意指两个方向，机械臂末端行走的曲线轨迹，或是操作臂在运动过程中的位移、速度和加速度的曲线轮廓。

操作臂最常用的轨迹规划方法有两种：第一种是要求对于选定的轨迹结点（插值点）上的位姿、速度和加速度给出一组显式约束（例如连续性和光滑程度等），轨迹规划器从一类函数（例如n次多项式）选取参数化轨迹，对结点进行插值，并满足约束条件。第二种方法要求给出运动路径的解析式。

一般来讲，移动机器人有三个自由度（X，Y，θ），机械臂有6个自由度（3个位置自由度和3个姿态自由度）。

运动规划（motion planning）由路径规划和轨迹规划组成，连接起点位置和终点位置的序列点或曲线称之为路径，构成路径的策略称之为路径规划。路径是机器人位姿的一定序列，而不考虑机器人位姿参数随时间变化的因素。路径规划（一般指位置规划）是找到一系列要经过的路径点，路径点是空间中的位置或关节角度，而轨迹规划是赋予路径时间信息，对机器人执行任务时的速度与加速度进行规划，以满足光滑性和速度可控性等要求。

运动规划是在给定的路径端点之间插入用于控制的中间点序列从而实现沿给定轨迹的平稳运动。运动控制则是主要解决如何控制目标系统准确跟踪指令轨迹的问题。即对于给定的指令轨迹，选择适合的控制算法和参数，产生输出，控制目标实时，准确地跟踪给定的指令轨迹。

Ref.

https://blog.csdn.net/wx545644217/article/details/54175035

https://en.wikipedia.org/wiki/Motion_planning

Motion planning (also known as the navigation problem) is a term used in robotics for the process of breaking down a desired movement task into discrete motions that satisfy movement constraints and possibly optimize some aspect of the movement.
For example, consider navigating a mobile robot inside a building to a distant waypoint. It should execute this task while avoiding walls and not falling down stairs. A motion planning algorithm would take a description of these tasks as input, and produce the speed and turning commands sent to the robot's wheels. Motion planning algorithms might address robots with a larger number of joints (e.g., industrial manipulators), more complex tasks (e.g. manipulation of objects), different constraints (e.g., a car that can only drive forward), and uncertainty (e.g. imperfect models of the environment or robot).

多项式曲线轨迹

轨迹规划的约束条件

• 初始位置：
  • 位置（给定）
  • 速度（给定，通常为零）
  • 加速度（给定，通常为零）
• 中间位置：
  • 中间点位置（给定）
  • 中间点位置（与前一段轨迹连续）
  • 速度（与前一段轨迹连续）
  • 加速度（与前一段轨迹连续）
• 终止位置：
  • 位置（给定）
  • 速度（给定，通常为零）
  • 加速度（给定，通常为零）

在上述约束条件下，我们要研究一种n次（或小于n次）的多项式函数，使得在各结点上满足对位置、速度和加速度的要求，并使机器人位置、速度和加速度在整个时间间隔[t0,tf]中保持连续。

方法一：规定一个七次多项式函数：

式中ai由一致的位置和连续条件确定。

方法二：将整个轨迹分割为几段，在每段轨迹中用不同的低次多项式来插值。常用的分割轨迹的方法:

• 4-3-4轨迹 每个关节有如下三段轨迹：第一段由提升点的轨迹用四次多项式表示。第二段有提升点到下放点的轨迹用三次多项式表示。最后一段是由下放点到终止点的轨迹由四次多项式表示。
• 3-5-3轨迹 与4-3-4轨迹相同，但是每一段所用的多项式次数不同。第一段用三次多项式，第二段用五次多项式，最后一段用三次多项式。
• 5段3次轨迹 对五段轨迹都使用三次多项式样条函数。

Example.

这四个已知信息可用来求解下列三次多项式方程中的四个未知量。

求导得
联立得
通过联立求解这四个方程，得到方程中四个未知的数值。如果要求机器人依次通过两个以上的点，那么每一段末端求解出的边界速度和位置都可以做下一段的初始条件，每一段的轨迹均可用类似的三次多项式加以规划。然而，尽管位置和速度都是连续的，但加速度并不连续，在实际应用中，这也可能会产生问题。

贝塞尔曲线轨迹

贝塞尔曲线是依据任意位置的点坐标绘制出的一条光滑曲线。其设计思路是按照规律u从0到1的移动过程中，在各个控制点连线的相应位置取点，并对相邻两条线上的点再次连线，重复以上过程直到没有可连接的两个点。

如图所示，设A点坐标为 (x0,y0) ，B点坐标为 (x1,y1)，C点坐标为 (x2,y2)，D点坐标为 (x'0(u),y'0(u))，E点坐标为 (x'1(u),y'1(u))，F点坐标为 (x(u),y(u))，则

以此类推
通过连接D、E两点得到线段DE，求点F的x轴坐标
其中的与坐标轴无关项被称为贝塞尔多项式函数(贝塞尔方程)

自动导引

机器人的自动导引（automated guiding）是指通过设计控制器，使得机器人沿着所设定的轨迹前进（自主跟踪设定轨迹），即实际位姿与期待位姿的误差e(t)满足

Background: LQR Control

对于线性二次型（linear quadratic，LQ）优化问题

其中F,Q为对称半正定矩阵，R为对称正定矩阵，解为 其中P是代数Riccati方程 的对称正定解，最优轨线
渐近稳定，目标函数值为

对于非时变系统，以上解出非时变状态调节器，即状态反馈

显然，对于我们建立的模型有x(tf)=0，并且可取Q=R=I；可以设计无限时间状态调节器，即令tf→∞，这样，离散化的目标函数可以写成

虽然MATLAB中有直接求取LQR问题的函数，但是，了解方程(4)的求解方法仍然是必要的。设λ是系统(3)的特征值，那么

设
代入式(4)有
即
这里，我们定义哈密顿矩阵
取变换矩阵
则
这表明H的特征值集合为
也就是说，对于闭环系统(3)的每一个特征值λ，满足±λ都是H的特征值。

我们只考虑H没有重特征值的情况，关于H有重特征值的情况可以参见：

MRTENSSONA K. On the matrix riccati equation. Information Sciences, 1970, 3(1) : 17 – 49.

特征值λi对应的特征向量可以写成

设A的阶数为n，则H的特征值为
然后根据式(1)解出反馈矩阵K，可以编程实现一个函数lqr(A,B,Q,R)，返回反馈矩阵K的值。

建模

如图所示，当机器人在平面上运动时，在实际位置C和目标位置D之间，会产生距离误差和角度误差，设叉车左右轮线速度分别为vl,vr，两轮间距为L，需求线速度和角速度分别为vd,ωd，x方向和y方向上的距离误差分别为x,y，角度误差为θ，记vc=(vl+vr)/2，则显然

为了求解
故瞬时半径
于是
又
当θ很小时有sinθ≈θ，cosθ≈1，于是(6)式可以写成
稳定状态下(6)式左边=0，于是
写成矩阵形式为
如果把(7)看做

设采样周期为T，离散系统为

通过
和需求速度
然后开始新一轮迭代。

伪代码如下。

REQUIRE：θ(0),x(0),y(0),vd,ωd，采样时间T，最大迭代次数km，形如式(5)的目标函数J
ENSURE：位姿x,y,θ
初始化：设置vl(0)=vr(0)=vc(0)=0
FOR k∈[0,km)
根据(10)式计算出θ(k+1),x(k+1),y(k+1)的值
计算反馈矩阵K的值
通过(11)式计算反馈修正速度

通过(12)式计算需求速度

RETURN 位姿θ(k),x(k),y(k)
END FOR

在第k次迭代时，目标位置

实际位置

测试不同的θ(0),x(0),y(0),vd,ωd下误差关于时间的变化规律，以及运动情况如图所示，其中“Goal”和“Position”分别表示目标路径和实际路径。可以看到，对于不同的初始参数，控制器都给出了良好的结果，在短时间内实现了有效地跟踪。