4.1机动性

机动性，即讨论机器人如何实现空间中的运动。在机器人领域，一辆移动车体经常被描述为一个非完整系统。意思是车体运动受到一个或以上的非完整约束。一个完整约束可以用一个由位形变量x,y和θ组成的方程来描述。而一个非完整约束只能用位形变量的微分方程描述，而且无法积分成一个位形变量的约束方程。这样的系统也称为非可积系统。通过滑移转向的车辆，如坦克，可以在任意点实现原地转弯，但是要实现侧向平移，还是必须经过停止、转弯、前进、再停止、再转弯这个过程。这种操纵方式，或称时变控制策略，是非完整系统的重要标志。

4.2移动机器人小车

四轮车常用的模型是如图所示的双轮自行车模型，后轮固定在车体上，前轮可绕水平轴转动以实现车辆转向。

该车的位姿可以用坐标系{V}来表示，该车的速度依据定义为其在x方向上的速度v，而在y方向上的速度为零。在坐标系{V}中表示为：

其角速度为:

通过简单的几何知识可知转弯半径R₁=L/tanγ,其中L是该车辆或轮基的长度。转弯半径随着车辆长度的增加而增加。

机器人小车在世界坐标系中的速度是(vcosθ,vsinθ),可以将它的运动方程写为

这个模型被称为小车的运动学模型，因为它只描述了小车的运动速度，并未涉及导致速度变化的力和力矩。小车朝向的变化率θ也被称为转动速率、朝向转速或偏摆速率，该参数可以由陀螺仪直接测量，也可以由左右轮的角速度值推算出来。

在世界坐标系中，可以写出一个沿小车y方向的速度表达式：

这属于非完整约束，不能通过积分变成x，y，θ之间的关系。

通过MATLAB工具箱中的Simulink模型sl_lanechange，可以实现上述方程描述的运动关系。

>> r=sim('sl_lanechange');
>> y= r.find('y');
>> subplot(2,2,1);plot(y(:,1));title('y');
>> subplot(2,2,2);plot(y(:,3));title('θ');
>> subplot(2,2,3);plot(y(:,4));title('α');
>> subplot(2,2,4);plot(y(:,2));title('xy平面运动');

4.2.1移动到一个点

下面讨论机器人向平面上一个目标点（x，y）移动的问题。

移动过程中，控制机器人的速度与目标的远近成正比：

机器人转向目标角使用世界坐标系中的相对角度：

并使用比例控制器来调整车辆的转向角：

上述控制的Simulink模型为sl_drivepoint

>> xg=[5 5];
>> x0=[7 7 0];
>> r=sim('sl_drivepoint');
>> q=r.find('y');
>> plot(q(:,1),q(:,2));
>> hold on
>> x0=[8 5 pi/2];
r=sim('sl_drivepoint');
q=r.find('y');
plot(q(:,1),q(:,2));
>>hold on
>> x0=[3 1 pi];
r=sim('sl_drivepoint');
q=r.find('y');
plot(q(:,1),q(:,2));
>> hold on
x0=[0 9 0];
r=sim('sl_drivepoint');
q=r.find('y');
plot(q(:,1),q(:,2));


 生成(7,7),(8,5),(3,1),(0,9)四点，通过运动汇聚到(5,5)。

4.2.2跟踪一条直线

移动机器人另一种功能是跟踪平面上的一条线。这需要两个控制器来调整机器人运动的方向。一个控制器操纵机器人与直线的垂直距离最短。该距离可表示为

 采用以下比例控制器：

 它会控制机器人始终转向目标直线。

第二个控制器用于调整机器人的转向角或方向，使其与目标直线平行：

 同样采用比例控制器：

 上述控制方法的Simulink模型为sl_driveline

>> L=[1 -2 4];
>> x0=[8 5 pi/2];
>> r=sim('sl_driveline');
>> y=r.find('y');
>> plot(y(:,1),y(:,2));
>> hold on
>> x0=[7 1 0];
>> r=sim('sl_driveline');
>> y=r.find('y');
>> plot(y(:,1),y(:,2));
>> hold on
>> x0=[3 8 pi];
>> r=sim('sl_driveline');
>> y=r.find('y');
>> plot(y(:,1),y(:,2));

4.2.3跟踪一般路径

路径跟踪问题的一个简单有效的算法是纯追踪。最简单的情况是目标点(x(t),y(t))沿着路径以恒定速度运动，而机器人总是朝向目标点运动——有点类似于驴追胡萝卜。

 设机器人与被跟踪点的距离为d*，则跟踪误差可定义为：

 比例积分控制器来控制机器人的速度使跟踪误差趋近于零：

 第二控制器则使机器人转向目标，相对角形式的目标值为

 可以只用一个简单的比例控制器：

 它将控制转向轮使机器人朝向目标点。

上述控制方法的模型为sl_pursuit

>> r=sim('sl_pursuit')

h = 1.2730e+03
r = Simulink.SimulationOutput:
                      t: [773x1 double] 
                      y: [773x6 double] 
     SimulationMetadata: [1x1 Simulink.SimulationMetadata] 
           ErrorMessage: [0x0 char] 
>> hold on
>> y=r.find('y');
>> plot(y(:,1),y(:,2),'LineWidth',2);

 虚线为被追踪轨迹，实线是机器人的实际轨迹。

4.2.4运动到一个位姿

为了控制机器人最终的姿态，首先将小车的运动方程重写成矩阵形式：

 我们对变量做如下转换：

 可以得到

 采用以下线性控制方法：

 它将控制机器人到达一个唯一的平衡点,即(p,α,β)=(0,0,0)。该控制器的设计思路是用kp和kα项控制机器人沿一条直线向目标{G}运动，而kβ项则用来转动该直线从而使β→0。

 至此，我们讨论了一种能驱动机器人达到姿态(0,0,0)的调节器。为使机器人运动到任意姿势，需要进行坐标变换：

 位姿控制器采用sl_drivepose

xg=[5 5 pi/2];
x0=[7 7 0];
r=sim('sl_drivepose');
q=r.find('y');
plot(q(:,1),q(:,2));
hold on
x0=[8 5 pi/2];
r=sim('sl_drivepose');
q=r.find('y');
plot(q(:,1),q(:,2));
>>hold on
x0=[3 1 pi];
r=sim('sl_drivepose');
q=r.find('y');
plot(q(:,1),q(:,2));
hold on
x0=[0 9 0];
r=sim('sl_drivepose');
q=r.find('y');
plot(q(:,1),q(:,2));

 结果如图

4.3飞行机器人

 如图展示了四旋翼飞行器的模型。机体坐标系{B}的z轴向下。该四旋翼飞行器有4个转动桨叶(旋翼),标记为1至4,安装在每个交叉臂的两端。设旋翼转速为w,产生一个向上的推力向量，指向z轴的负方向：

 其中b为升力常数。由牛顿第二定律，可以给出该飞行器在世界坐标系中的移动动力学方程（4.3）：

 v是飞行器在世界坐标系中的速度，g是重力加速度，m是该飞行器的总质量，T是总的向上推力。

 每对旋翼之间的推力差异就会导致飞行器旋转。在x轴上产生的转矩即为横滚力矩，其大小为

 其中，d是电机到飞行器质心的距离。我们可以将方程(4.3)代入，得到该力矩关于旋翼转速的表达式：

 类似地，俯仰力矩的大小是

 电机施加到每个旋翼桨上的转矩与空气阻力力矩方向相反：

 该转矩会对飞行器机体施加一个反转力矩，其作用是使整个机体产生一个绕旋翼轴的转动，但方向与旋翼转向相反。因此在z轴上总的转矩是

 由欧拉运动方程可以给出飞行器机体的旋转加速度：

 通过整合计算机体上作用力的动力学方程和作用力矩的方程,可以得到四旋翼飞行器的运动模型：

 如果d>0,则矩阵A是满秩的，可以通过求逆得到

 利用上式可以求出为给机体施加特定的力和力矩所需要的各旋翼转速。

sl_quadrotor模型可以实现四旋翼飞行器的仿真:

 它在一个单独的窗口中显示仿真动画。飞行器起飞升空，然后飞行一圈，同时缓慢绕自己的z轴旋转。

>> sim('sl_quadrotor');

 结果如图：

本章小结

本章主要内容可以总结如下：

1.讨论了机动性、位形空间和任务空间等基本概念，为理解机器人运动和控制提供了基础。

2.针对类似汽车的地面机器人平台建立了运动学模型，并开发了多种控制器，使其能够完成移动、跟踪路径和调整位姿等任务。

3.介绍了四旋翼飞行器并建立了动力学模型，实现了飞行器完成一圈飞行的任务。

4.通过这些模型和控制器的应用，展示了不同机器人平台在完成各种任务时的运动规划和控制方法。