空间插补算法大综合——四元数姿态插补、十五段轨迹规划、直线圆弧位置插补以及坐标标架实现
今天抽空把二月份宅家学习的空间插补算法进行整理，这里要感谢CSDN中的一些大咖们，有些算法是完全借鉴，有些算法是在之基础上发挥综合的，在这里先一并感谢！切入正题！
四元数基础
注意这里使用单位四元数来进行姿态插补，单位四元数具有比RPY更小运算量、便于插值的优势，更适合用于表达刚体的姿态变换。
① 四元数点乘绝对值越大，表示方向越接近；
② 四元数范数为1称为单位四元数；
③ 四元数的共轭，只要把向量部分取负即可；
④ 已知机械臂末端姿态R
得到各四元数如下：
空间三点圆弧算法（位置）
基本思想：将空间三维平面问题转化为二维平面的问题。工程上通常使用的方法，借助向量的点积和差积的方法实现。
基础：向量的点积与差积的计算及含义
（向量基础参考：《An introduction to Vectors, Vectors Operation and Vector Anslysis》, Author: Pramod S. Joag）
① 以P1为坐标原点构建坐标系，并令P1P2为U轴，P1P2与P1P3的vector Product 为W轴，即三点构成平面的法向量。再通过product获得V轴。
从而获得三轴。通过向量的点积vector dot 计算三点在P1-uvw坐标系下的位置坐标，从而将其转化为平面p1p2p3下的二维圆弧问题。
p1,p2,p3对应点设为A，B，C，B在U轴上，故而B(bx，0)，点C(cx,cy)在U,V轴上的投影由vector dot计算获取。
② 根据平面上的三点求圆心坐标，由于圆心坐标必然通过x=bx/2，因此设P1-uvw坐标系下的圆心的坐标为（bx/2,h）,对应标准方程为：
可以求解得到：
③ 将圆心坐标转化为O-xyz下的坐标，通过向量的加减获取圆心坐标，圆弧的半径由圆心与其中任意一点的坐标差求取。
由此得到空间三点形成的圆弧。
空间圆弧插补实现（坐标）
基本思想：将空间中的三点形成的平面转化为二维平面问题，然后计算圆弧角，并在该平面上进行插补，最后将该二维平面上的插补点坐标转化为空间三维点坐标
① 坐标变换
空间圆弧的本质是空间三点形成平面M上的圆弧，我们在求出在坐标系O-XYZ下的变换矩阵T，就可以进一步经过坐标变换将三维空间（O-XYZ）的圆弧变换到二维平面P0-UVW下的圆弧。平面方程的具体求解实现如上后单位化。
以平面M的法向量的方向作为新坐标系P0-UVW的W轴方向，W轴方向的方向余弦为：特别注意此处的p0为pc，即在上一步中求出的圆心坐标，这里是以二维平面中的圆心坐标center为P0点，以P0P1为U轴方向进行。
以P0P1方向作为新坐标系P0-UVW的U轴方向，U轴的方向余弦为：
以U轴的方向余弦与W轴的方向余弦的叉乘作为P0-UVW坐标系下V轴的方向余弦，V轴方向余弦为：
进一步求出P0-UVW在坐标系O-XYZ下的坐标变换矩阵：
同时根据酉矩阵的性质，其逆矩阵为：
进一步，可以求出P1,P2,P3点在新坐标系下对应的坐标值，其中i=1,2,3;
在新坐标系下对应的坐标为Q,则有矩阵变换：
插补角的方向
实际机械手在空间圆弧的任务操作中，圆弧具有确定的插补方向，此处约定在P0-UVW坐标系下UV平面内做逆时针方向为其插补方向，即由P3到P2再到P1运动的逆时针圆弧。
插补角的大小：
姿态插补
姿态插补主要研究在轨迹方向上的机械臂的姿态问题。分为普通线性插值（Lerp/Linear Interpolation）、正规化线性差值（Normalized Lerp）和 球面线性差值（Spherical Lerp）。
1.SLERP
四元数球面线性插补插值。SLERP类似于线性插值，不是沿着一条直线插入而是沿着球体表面上的一个弧进行插值,能够实现圆弧面上的等速插补，但在转接点时速度可能出现跳变。SLERP是对角度本身进行插值而不是弧长进行插值。
SLERP插值CSDN讲解的很多，但要注意两个问题：
① 旋转走最短路径；
处理方法：测试点积结果，结果为负时，将其中一个四元数取负；
② 夹角接近0时候退化为线性方程处理；
处理方法：线性方程处理：slerp(p,q,t)=(1-t)p+qt;
2.NLERP
NLerp是Lerp的改进，将Lerp的值单位化，即除以其模长后得到单位四元数，这种插补算法适用于插补角度接近0的情况。应当注意：NLerp插值算法在单位时间内，Vt扫过的角度是不同的,是一种非匀速插值算法。
MaAtlab代码实现(matlab 2019a)
基础知识补：
dist(quatA,quatB),
returns the angular distance in radians between the quaternion rotation operators for quatA and quatB
quaternion（）
Matlab中的四元数结构体。
1. 在matlab函数自带slerp（）函数中进行姿态插补函数运行。
clc;
q0 = quaternion([-80 10 0], 'eulerd', 'ZYX', 'frame');
q1 = quaternion([80 70 70], 'eulerd', 'ZYX', 'frame');
% p30 = slerp(q0, q1, 0.3);
% eulerd(p30, 'ZYX', 'frame');
%slerp球面插值
dt = 0.01;
h = (0:dt:1).';
trajSlerped = slerp(q0, q1, h);
T=2;stepN=100;
Qv0=[0 0 0];
myQplot(trajSlerped,T,Qv0,stepN);
%
%
%myQplot(trajSlerped,T,Qv0,stepN)函数功能：主要用于绘制四元数计算后的插补姿态位置曲线和速度曲线
%函数输入参数：
%接入初始速度Qv0,采样点数stepN,插补姿态序列trajSlerped,插补时间T
function myQplot(trajSlerped,T,Qv0,stepN)
%定义初始速度avSlerp0=qv0
% Qv0=[0,0,0];
avSlerp0=Qv0;
dt=T/stepN; %计算插补步长
h = (0:dt:T).';
%绘图程序
figure(1);
subplot(2,1,1);
%绘制姿态转动位置曲线
% avSlerp = eulerd(trajSlerped,'ZYX','frame'); %四元数转化为欧拉角deg
apSlerp = quat2eul(trajSlerped); %四元数转化为欧拉角rad
plot(h,apSlerp(:,1),'o','color','b');hold on;%获取转动姿态位置曲线
plot(h,apSlerp(:,2),'+','color','g');hold on;
plot(h,apSlerp(:,3),'*','color','r');grid on;
legend( 'X angular position (SLERP)', ...
'Y angular position (SLERP)', ...
'Z angular position (SLERP)');
xlabel('sampleTime/s');
ylabel('Angular Position(rad)');
title('Angular Velocity - SLERP');
%
%绘制姿态转动位置曲线
subplot(2,1,2);
avSlerp = myQuat2Qv(trajSlerped, dt); %计算姿态插补速度
%合成初始速度量qv0;
avSlerp=[avSlerp0;avSlerp];
pp=plot(h,avSlerp(:,1),h,avSlerp(:,2),h,avSlerp(:,3),...
'linewidth',0.5);grid on;%获取转动姿态位置曲线
pp(1).Color='r';pp(1).Marker='o';
pp(2).Color='g';pp(2).Marker='+';
pp(3).Color='b';pp(3).Marker='*';
legend( 'X angular position (SLERP)', ...
'Y angular position (SLERP)', ...
'Z angular position (SLERP)');
xlabel('sampleTime/s');
ylabel('Angular Position(rad/s)');
title('Angular Velocity - SLERP');
end
%
%
function av = myQuat2Qv(q, dt)
% Reference: Copyright 2018 The MathWorks, Inc.
% Computes angular velocity in the body frame from a sequence of
% quaternions
validateattributes(q, {'quaternion'}, {'finite', 'column', ...
'nonempty'});
validateattributes(dt, {'double', 'single'}, {'finite', 'real', ...
'nonempty', 'scalar'});
av4 = compact( (2* conj(q(1:end-1)) .* q(2:end))./dt);
av = av4(:,2:end);
end
2. 如果考虑角度非常小的情况下采用线性插值算法：
%myQSlerp插补demon
clc;clear all;
q0 = quaternion([-80 10 0], 'eulerd', 'ZYX', 'frame');
q1 = quaternion([80 70 70], 'eulerd', 'ZYX', 'frame');
q0=myParts(q0);
q1=myParts(q1);
stepN=100;
[pQi] = myQSlerp(q0,q1,stepN);
apSlerp = quat2eul(pQi);
%
%绘图
trajSlerped=pQi;T=1;Qv0=[0,0,0];stepN=100;
myQplot(trajSlerped,T,Qv0,stepN);
%
%
% 函数功能:对姿态插补，再现Slerp的插补算法。
% 同时考虑在极小角度插补时采用线性插值，其他
% 时间插补采用球面线性插补，实现等速度插补
% 输入参数:pQi，为将要插补两点姿态插补计算后单位化，
% 用单位四元数表示；
% stepN，姿态插补路径点数；
% 参考资料：Matlab函数 PreSlerp
function [pQi] = myQSlerp(pQ1,pQ3,stepN)
%分配插补后数据的存储空间
pQi=zeros(stepN,4);
% delta=T/stepN; %计算采样时间，用于线性插补计算
%计算姿态并判断插补最短路径
dp=pQ1(1)*pQ3(1)+pQ1(2)*pQ3(2)+pQ1(3)*pQ3(3)+pQ1(4)*pQ3(4);
if dp<0
pQ3=-pQ3; %两四元数的dot product表示两个Q之间的夹角余弦，取反后，选取最短路径
dp=-dp;
elseif dp>=1
dp=1;
end
%计算Q之间的夹角,并取最短路径
Q_theta=acos(dp);
sinv=1./sin(Q_theta);
for step=0:1:stepN
if dp>0.995 %Q之间夹角接近0采用线性插补
k1=step/stepN;
k0=1-k1;
else
k0=sin((1-step/stepN).*Q_theta);
k1=sin((step/stepN).*Q_theta);
end
pQi(step+1,:)=(pQ1.*k0+pQ3.*k1).*sinv;
pQi(step+1,:)=pQi(step+1,:)/norm( pQi(step+1,:));
end
end
%
%
%将四元数结构体转化为矩阵数组形式
%参考；Matlab 中的函数parts()
function Qa=myParts(qa)
[w,x,y,z]=parts(qa); %将四元数结构体转化为矩阵
Qa=[w,x,y,z];
end
%myQplot(trajSlerped,T,Qv0,stepN)函数功能：主要用于绘制四元数计算后的插补姿态位置曲线和速度曲线
%函数输入参数：
%接入初始速度Qv0,采样点数stepN,插补姿态四元数序列trajSlerped,插补时间T
function myQplot(trajSlerped,T,Qv0,stepN)
%定义初始速度avSlerp0=qv0
% Qv0=[0,0,0];
avSlerp0=Qv0;
dt=T/stepN; %计算插补步长
h = (0:dt:T).';
%绘图程序
figure(1);
subplot(2,1,1);
%绘制姿态转动位置曲线
% avSlerp = eulerd(trajSlerped,'ZYX','frame'); %四元数转化为欧拉角deg
apSlerp = quat2eul(trajSlerped) %四元数转化为欧拉角rad
plot(h,apSlerp(:,1),'o','color','b');hold on;%获取转动姿态位置曲线
plot(h,apSlerp(:,2),'+','color','g');hold on;
plot(h,apSlerp(:,3),'*','color','r');grid on;
legend( 'X angular position (SLERP)', ...
'Y angular position (SLERP)', ...
'Z angular position (SLERP)');
xlabel('sampleTime/s');
ylabel('Angular Position(rad)');
title('Angular Velocity - SLERP');
%
%绘制姿态转动速度曲线
subplot(2,1,2);
avSlerp = myQuat2Qv(trajSlerped, dt); %计算姿态插补速度
%合成初始速度量qv0;
avSlerp=[avSlerp0;avSlerp];
pp=plot(h,avSlerp(:,1),h,avSlerp(:,2),h,avSlerp(:,3),...
'linewidth',0.5);grid on;%获取转动姿态位置曲线
pp(1).Color='r';pp(1).Marker='o';
pp(2).Color='g';pp(2).Marker='+';
pp(3).Color='b';pp(3).Marker='*';
legend( 'X angular position (SLERP)', ...
'Y angular position (SLERP)', ...
'Z angular position (SLERP)');
xlabel('sampleTime/s');
ylabel('Angular Position(rad/s)');
title('Angular Velocity - SLERP');
end
function av = myQuat2Qv(q, dt)
% Reference: Copyright 2018 The MathWorks, Inc.
% Computes angular velocity in the body frame from a sequence of
% quaternions or array
% dt：sampleTime
if(isnumeric(q))
q=quaternion(q); %主要为了借用下面的函数而写
end
%
validateattributes(q, {'quaternion'}, {'finite', 'column', ...
'nonempty'});
validateattributes(dt, {'double', 'single'}, {'finite', 'real', ...
'nonempty', 'scalar'});
av4 = compact( (2* conj(q(1:end-1)) .* q(2:end))./dt);
av = av4(:,2:end); %四元数序列求速度
end
在上面程序中有一个地方是求插补速度的函数，这个函数要注意默认的输入是四元数类型，当不是四元数类型而是double数组情况下，程序内部将自动转化为四元数类型，使用的转化函数为quaternion（q）,该函数的使用在Matlab2018以上版本可使用。
位姿插补及标架实现
前文对直线和姿态插补进行了学习，这里将以一个直线位姿插补为例，并在matlab中实现其插补位姿的frame（标架）显示。
1.直线进行均匀线性插补，对其方位角度进行球面线性插补
% 函数demon:对一条直线进行均匀线性插补，对其方位角度进行球面线性插补
% 函数调用：插补函数与作图函数
% 插补函数：
% [stepDistance,p_i_LinePosition]=LineInsert(p1,p3,stepN)
% 获取直线插补点坐标
% [pQi] = myQSlerp(pQ1,pQ3,stepN)
% 获取角度插补点方位角度的四元数表示
% 作图函数：
% myQplot(trajSlerped,T,Qv0,stepN)
% 获取插补点的方位角度RPY变化和插补速度变化曲线
% myCoordinateFrame(Pi_position,Pi_orientation)
% 获取插补点位置及姿态变化并显示coordinate frame
%
% 输出参数：
% p1,p3，分别输入起始点坐标；
% pQ1,pQ3，分别输入起始点的姿态角度四元数表示；
% stepN，输入采样点数；
%
%
clc;clear all;
p1 = [1,0,1];p2=[1,4,2];p3=[-2,6,10]; %输入位置坐标
eq1 =[0,20,80];eq3=[-60,120,40]; %这里为了输入角度更直观输入为RPY角度，后转化为四元数；
stepN = 50;
%
q1 = quaternion(eq1, 'eulerd', 'ZYX', 'frame');%输入姿态坐标，角度转化为四元数计算
q3 = quaternion(eq3, 'eulerd', 'ZYX', 'frame');
q1=myParts(q1); %将四元数分离为数组形式
q3=myParts(q3);
%
% 插补计算直线上点为位置和姿态坐标
[stepDistance,p_i_LinePosition]=LineInsert(p1,p3,stepN); % 直线插补
[pQi] = myQSlerp(q1,q3,stepN); % QSlerp姿态插补
%
% 绘图程序
% 直线插补位置和姿态坐标显示
fg1=figure(1);
fg1.Name='直线插补位置和姿态坐标显示';
fg1.Color='White';
Pi_position=p_i_LinePosition;
myLineplot3(p1,p3,Pi_position);hold on;
Pi_orientation=pQi;
myCoordinateFrame(Pi_position,Pi_orientation);
%
% 姿态插补位置与速度变化曲线显示
fg2=figure (2);
fg2=figure(2);
fg2.Name='姿态插补位置与速度变化曲线显示';
fg2.Color='White';
trajSlerped=pQi;
T=1; %计算插补姿态速度时的步长使用
Qv0=[0,0,0]; %填充初始速度
myQplot(trajSlerped,T,Qv0,stepN);
%%
%%
%Coordinate Frame实现
% 函数功能：使用matlab自带函数quiver3绘制轨迹点的坐标标架；
% 函数输入：Pi_position,Pi_orientation
function myCoordinateFrame(Pi_position,Pi_orientation)
% Pi_position:Nx3 column array;
% Pi_orientation:Nx3 column array，euler angle
% or Nx4 quaternion with unitization;
%
if size(Pi_orientation,2)== 4 %if input is quaternion
Pi_orientation=quat2eul(Pi_orientation);
end
rotmZYX = eul2rotm(Pi_orientation); %if input is RPY
ox=Pi_position(:,1);
oy=Pi_position(:,2);
oz=Pi_position(:,3);
ux=squeeze(rotmZYX(1,1,:));vx=squeeze(rotmZYX(1,2,:));wx=squeeze(rotmZYX(1,3,:));
uy=squeeze(rotmZYX(2,1,:));vy=squeeze(rotmZYX(2,2,:));wy=squeeze(rotmZYX(2,3,:));
uz=squeeze(rotmZYX(3,1,:));vz=squeeze(rotmZYX(3,2,:));wz=squeeze(rotmZYX(3,3,:));
% AXhandle = quiver3(ox, oy, oz, ux, vx, wx, 'r', 'ShowArrowHead','on', 'MaxHeadSize', 0.999999, 'AutoScale', 'on');hold on;
% AYhandle = quiver3(ox, oy, oz, uy, vy, wy, 'g', 'ShowArrowHead','on', 'MaxHeadSize', 0.999999, 'AutoScale', 'on');hold on;
% AZhandle = quiver3(ox, oy, oz, uz, vz, wz, 'b', 'ShowArrowHead','on', 'MaxHeadSize', 0.999999, 'AutoScale', 'on');grid on;
% AXhandle.LineWidth=1.5;
% AYhandle.LineWidth=1.5;
% AZhandle.LineWidth=1.5;
% view (60,45);
%
% set the coordinate display one by one
Num=size(Pi_orientation);
for i=1:1:Num
AXhandle = quiver3(ox(i), oy(i), oz(i), ux(i), vx(i), wx(i), 'r', 'ShowArrowHead','on', 'MaxHeadSize', 0.39, 'AutoScale', 'on');hold on;
AYhandle = quiver3(ox(i), oy(i), oz(i), uy(i), vy(i), wy(i), 'g', 'ShowArrowHead','on', 'MaxHeadSize', 0.39, 'AutoScale', 'on');hold on;
AZhandle = quiver3(ox(i), oy(i), oz(i), uz(i), vz(i), wz(i), 'b', 'ShowArrowHead','on', 'MaxHeadSize', 0.39, 'AutoScale', 'on');
AXhandle.LineWidth=1.5;
AYhandle.LineWidth=1.5;
AZhandle.LineWidth=1.5;
pause(0.01);
end
fl=legend([ AXhandle, AYhandle, AZhandle],{'Y','P','R'},'FontAngle','italic','Box','off');
end
2.空间圆弧位姿插补算法实现
% 函数demon:对空间三点构成圆弧均匀线性插补，对其方位角度进行球面线性插补
% 函数调用：插补函数与作图函数
% 插补函数：
% [pc,rad,p_i_position]=CircleInsert(p1,p2,p3,stepN)
% 获取圆弧插补点坐标
% [pQi] = myQSlerp(pQ1,pQ3,stepN)
% 获取角度插补点方位角度的四元数表示
% 作图函数：
% myQplot(trajSlerped,T,Qv0,stepN)
% 获取插补点的方位角度RPY变化和插补速度变化曲线
% myCoordinateFrame(Pi_position,Pi_orientation)
% 获取插补点位置及姿态变化并显示coordinate frame
%
% 输出参数：
% p1,p2,p3，分别输入起始中间和末端位置点坐标；
% pQ1,pQ3，分别输入起始点的姿态角度四元数表示；
% stepN，输入采样点数；
%
%
clc;clear all;
p1 = [1,0,1];p2=[1,4,2];p3=[-2,6,10]; %输入位置坐标
eq1 =[0,20,80];eq3=[-60,120,40]; %这里为了输入角度更直观输入为RPY角度，后转化为四元数；
stepN = 50;
%
q1 = quaternion(eq1, 'eulerd', 'ZYX', 'frame');%输入姿态坐标，角度转化为四元数计算
q3 = quaternion(eq3, 'eulerd', 'ZYX', 'frame');
q1=myParts(q1); %将四元数分离为数组形式
q3=myParts(q3);
%
% 插补计算直线上点为位置和姿态坐标
[pc,rad,p_i_CirclePosition]=CircleInsert(p1,p2,p3,stepN); % 空间三点圆弧插补
[pQi] = myQSlerp(q1,q3,stepN); % QSlerp姿态插补
%
% 绘图程序
% 直线插补位置和姿态坐标显示
fg1=figure(1);
fg1.Name='空间圆弧插补位置和姿态坐标显示';
fg1.Color='White';
Pi_position=p_i_CirclePosition;
myCircleplot3(p1,p2,p3,pc,Pi_position);hold on;
Pi_orientation=pQi;
myCoordinateFrame(Pi_position,Pi_orientation);
%
% 姿态插补位置与速度变化曲线显示
fg2=figure (2);
fg2=figure(2);
fg2.Name='姿态插补位置与速度变化曲线显示';
fg2.Color='White';
trajSlerped=pQi;
T=1; %计算插补姿态速度时的步长使用
Qv0=[0,0,0]; %填充初始速度
myQplot(trajSlerped,T,Qv0,stepN);
3.任意空间三点位姿插补算法
%函数功能，对空间三点进行位姿插补，如果三点共线，采用直线插补，否则采用圆弧插补
function [pi,pQi]=my3PointsInterp(p1,p2,p3,eq1,eq3,stepN)
if nargin<4
stepN=50;
end
check_flag=my3PointsCheck(p1,p2,p3);
%位置插补
if ~check_flag
[pc,rad,p_i_CirclePosition]=CircleInsert(p1,p2,p3,stepN); % 空间三点圆弧插补
pi=p_i_CirclePosition;
else
[stepDistance,p_i_LinePosition]=LineInsert(p1,p3,stepN); % 直线插补
pi=p_i_LinePosition;
end
%姿态插补
q1 = quaternion(eq1, 'eulerd', 'ZYX', 'frame');%输入姿态坐标，角度转化为四元数计算
q3 = quaternion(eq3, 'eulerd', 'ZYX', 'frame');
q1=myParts(q1); %将四元数分离为数组形式
q3=myParts(q3);
[pQi] = myQSlerp(q1,q3,stepN); % QSlerp姿态插补
%插补位姿绘图
if ~check_flag
fg1=figure(1);
fg1.Name='空间圆弧插补位置和姿态坐标显示';
fg1.Color='White';
Pi_position=p_i_CirclePosition;
myCircleplot3(p1,p2,p3,pc,Pi_position);hold on;
Pi_orientation=pQi;
myCoordinateFrame(Pi_position,Pi_orientation);
else
fg1=figure(1);
fg1.Name='直线插补位置和姿态坐标显示';
fg1.Color='White';
Pi_position=p_i_LinePosition;
myLineplot3(p1,p3,Pi_position);hold on;
Pi_orientation=pQi;
myCoordinateFrame(Pi_position,Pi_orientation);
end
% 姿态插补位置与速度变化曲线显示
fg2=figure (2);
fg2=figure(2);
fg2.Name='姿态插补位置与速度变化曲线显示';
fg2.Color='White';
trajSlerped=pQi;
T=1; %计算插补姿态速度时的步长使用
Qv0=[0,0,0]; %填充初始速度
myQplot(trajSlerped,T,Qv0,stepN);
[pi]
[pQi]
end
% 函数功能：检查输入的空间三点是否共线
% 输入参数：p1,p2,p3
% 返回参数：check_flag
function check_flag=my3PointsCheck(p1,p2,p3)
fprintf("Please input the points :");
if size(p1,2)~=3|size(p2,2)~=3|size(p3,2)~=3
fprintf("Error!please input the points with 3 colum!");
check_flag=1;
return;
end
p12=p2-p1;
p13=p3-p1;
if find(norm(p12)==0)|find(norm(p13)==0)
fprintf("Error!please donot input the same datas!");
check_flag=1;
return;
end
p12_n=p12/norm(p12);
p13_n=p13/norm(p13);
p1213_n=cross(p12_n,p13_n);
if find(all(p1213_n)==0)|find(all(p1213_n)<1e-5)
fprintf("Error!The input datas are too closeing!nearly line!");
check_flag=1;
return;
end
check_flag=0;
end
%%
%%**测试：**
p1=[0.34 1.32 2.98];p2=[-0.2 2.4 4.6];p3=[-2 6 10];%直线
% p1=[1.409 0.3515 0.9858];p2=[0.5095 5.8044 3.6772];p3=[-3.1556 3.2142 12.1925];%圆弧
eq1 =[0,20,80];eq3=[-60,120,40]; %这里为了输入角度更直观输入为RPY角度，后转化为四元数；
stepN = 50;
[pi,pQi]=my3PointsInterp(p1,p2,p3,eq1,eq3,stepN);
路径规划与轨迹规划的一点探讨
路径规划解决的是从起始点到达终点的轨迹在几何上的描述问题，而轨迹规划要解决的是针对该路径在时间域内如何实现的问题。二者结合起来共同解决路径行走及避障问题。
轨迹规划：工程上主要使用的有S曲线规划轨迹，15段轨迹；
路径规划：针对问题规划，通过插补实现数据点的加密。
核心要素：直线、圆弧插补，轨迹规划算法实现，NURBS、BSpline实现。
1. 插补中的归一化处理问题
轨迹规划到路径规划的关键点在于：轨迹规划的单位采样时间增量δq的占比，即q(i)/pos的占比，注意这个占比是累计增加的。而路径插补的运算方式与匀速插补计算相同，区别在于将δt与δq相互映射即可。
在轨迹与路径相映射过程中涉及到一个“归一化处理”，核心思想就是计算轨迹规划曲线每段特征轨迹在整个轨迹运行中的占比，比如匀速时间、加减速时间分别归一化，即求每一段特征轨迹t/T=λ的值，然后将这个归一化参数映射到路径上，即每次插补增量为插补总量的λ倍。
2. 路径的圆滑过渡问题
转接模型
% 函数功能: 空间多直线插补圆弧过渡模型，预处理范围
% 输入参数: 两条直线端点交点p2,直线外端点p1,p3,
% 设置过渡半径r1,
% 输出参数: 过渡圆弧左右侧转接点坐标pt1，pt2,
% 过渡圆弧所在圆心坐标pc1,
% 过渡圆弧拐角corner1,
% 过渡分割后的直线段长度d1,圆弧长度d2;
%
%
function [pt1,pt2,pc1,corner1,d1,d2]=myAdapteModle(p0,p1,p2,r1)
% compute the corner between p1p0,p1p2
vec_p1p0=p0-p1; norm_p1p0=norm(vec_p1p0);unit_p1p0=vec_p1p0./norm_p1p0;
vec_p1p2=p2-p1; norm_p1p2=norm(vec_p1p2);unit_p1p2=vec_p1p2./norm_p1p2;
corner1=acos(dot(unit_p1p0,unit_p1p2));
%
% compute the coordinate of Adapter Points called pt1 and pt2
norm_p1pt1=r1/tan(corner1/2);vec_p1pt1=norm_p1pt1*unit_p1p0;
pt1=p1+vec_p1pt1;
norm_p1pt2=r1/tan(corner1/2);vec_p1pt2=norm_p1pt2*unit_p1p2;
pt2=p1+vec_p1pt2;
%
% coompute the coordinate of the centre of the circle
% set the middle point of pt1pt2 as M1
M1=pt1+0.5*(pt2-pt1);
vec_p1M1=M1-pt1;norm_p1M1=norm(vec_p1M1);unit_p1M1=vec_p1M1./norm_p1M1;
norm_p1pc1=sqrt(r1^2+norm_p1pt1^2);
pc1=p1+norm_p1pc1*unit_p1M1;
%
% compute the segments of corner befor and after
d1=norm(p0-pt1);
d2=r1*(pi-corner1);
%
end
% test：
p0=[1,1,3];p1=[4.5,9,13];p2=[3,7,5];r1=0.5;
[pt1,pt2,pc1,corner1,d1,d2]=myAdapteModle(p0,p1,p2,r1)
输出结果：
pt1 =
3.8930 7.6126 11.2657
pt2 =
4.0879 8.4506 10.8024
pc1 =
4.9700 11.0205 11.8829
corner1 =
0.4277
d1 =
10.9735
d2 =
1.3570
15段轨迹规划综合
15段轨迹的计算这里不做介绍，主要利用前面插补函数形式，实现位置姿态的插补。这里给出主要代码（matlb2019a）
clc;clear all;
p = csvread('pointsData20200517.csv');
% csvwrite('pointsData20200517.csv',p);
figure(1);
fg=plot3(p(:,1),p(:,2),p(:,3),'-o','MarkerSize',...
3,'MarkerFaceColor','#0000FF','MarkerIndices',1:51); %绘制一条直线方便取点
fg.Color='k';
fg.LineWidth=0.5;
% axis equal;
% axis([0,50,0,50,0,20]);
grid on;hold on;
%
% 对直线p0,p1进行直线插补，走15段规划轨迹
% p51,末点；p1,始点；
vec_p0p1=p(51,:)-p(1,:);
d1=norm(vec_p0p1); %计算整个直线的总长度
alpha=0.3;% 设定的速度在整个运动过程中增大或者减小的时间与整个运动时间的比例关系
beta= 0.25;% 设定的加速度增大或减小的时间与所在的非匀速段的时间比例关系
gamma=0.5;% 设定的snap非零时间与所在的非匀加速或者非匀减速段的时间比例关系
q0=0;q1=d1;
v0=0;v1=0;
n=50;T =2;
%
pQ1 = quaternion([-90 0 90], 'eulerd', 'ZYX', 'frame'); %RPY转化为四元数函数
pQ3 = quaternion([0 -70 120], 'eulerd', 'ZYX', 'frame');
pQ1=myParts(pQ1); %将四元数结构体转化为矩阵数组形式
pQ3=myParts(pQ3);
%
[uv,vel,acc,jerk,snap,dt,lamda]=myFifteenSegments(q0,q1,v0,v1,alpha,beta,gamma,T,n);
[p_i_LinePosition]=LineInsert15(p(1,:),p(51,:),lamda);
myLineplot3(p(1,:),p(2,:),p_i_LinePosition);hold on;
% 姿态插补
[pQi] = myQSlerp(pQ1,pQ3,lamda);
%frame
myCoordinateFrame(p_i_LinePosition,pQi);
figure(4)
myFifteenSegmentsPlots(uv,vel,acc,jerk,snap,dt,T);
% savefig('1.fig')
function [p_i_LinePosition]=LineInsert15(p1,p3,lamda)
% 计算插补步距stepDistance
% stepDistance=(sqrt((p3(1)-p1(1))^2+(p3(2)-p1(2))^2+(p3(3)-p1(3))^2))/stepN;
% %计算插值分量的步距,等间距插补
% delta_x=(p3(1)-p1(1))/stepN;
% delta_y=(p3(2)-p1(2))/stepN;
% delta_z=(p3(3)-p1(3))/stepN;
%计算插值分量的步距,等间距插补
delta_x=(p3(1)-p1(1));
delta_y=(p3(2)-p1(2));
delta_z=(p3(3)-p1(3));
lamda=lamda';
stepN=size(lamda,1);
p_i=zeros(stepN,3);%为存储矩阵分配空间
for t=1:1:stepN
p_i(t,1)=p1(1)+delta_x*lamda(t);
p_i(t,2)=p1(2)+delta_y*lamda(t);
p_i(t,3)=p1(3)+delta_z*lamda(t);
end
p_i_LinePosition=p_i;
end
%%
function [pQi] = myQSlerp(pQ1,pQ3,lamda)
%分配插补后数据的存储空间
lamda=lamda';
stepN=size(lamda,1);
pQi=zeros(stepN,4);
% delta=T/stepN; %计算采样时间，用于线性插补计算
%计算姿态并判断插补最短路径
dp=pQ1(1)*pQ3(1)+pQ1(2)*pQ3(2)+pQ1(3)*pQ3(3)+pQ1(4)*pQ3(4);
if dp<0
pQ3=-pQ3; %两四元数的dot product表示两个Q之间的夹角余弦，取反后，选取最短路径
dp=-dp;
elseif dp>=1
dp=1;
end
%计算Q之间的夹角,并取最短路径
Q_theta=acos(dp);
sinv=1./sin(Q_theta);
for step=1:1:stepN
if dp>0.995 %Q之间夹角接近0采用线性插补
k1=lamda(step);
k0=1-k1;
else
k0=sin((1-lamda(step)).*Q_theta);
k1=sin((lamda(step)).*Q_theta);
end
pQi(step,:)=(pQ1.*k0+pQ3.*k1).*sinv;
pQi(step,:)=pQi(step,:)/norm( pQi(step,:));
end
end
补充函数（matlab2019a）
15段归一化函数算法程序
% 函数功能：myFifteenSegments(q0,q1,v0,v1,alpha,beta,gamma,duration,n)十五段路径规划算法
% 输入参数：q0,q1,始末位置参数，rad
% v0,v1,始末速度参数，rad/s
% alpha,beta,gama,占空比参数，加速度，加加速度，加加加速度时间占空比
% duration,轨迹运行总时间
% n,轨迹点数，用于计算采样时间dt
% 输出参数：uv,vel,acc,jerk,snap,分别为规划位置、速度，加速度，加加速度和加加加速度
% dt,采样时间
% lamda,轨迹归一化参数，用于插补映射
%
%
%% 主函数程序
function [uv,vel,acc,jerk,snap,dt,lamda]=myFifteenSegments(q0,q1,v0,v1,alpha,beta,gamma,duration,n)
dt=duration/(n-1); % sampleTime
h=q1-q0; % desired displacement h
vmax = h/((1-alpha)*duration);
amax = h/(alpha*(1-alpha)*(1-beta)*duration^2);
jmax = h/(alpha^2*beta*(1-alpha)*(1-beta)*(1-gamma)*duration^3);
smax = h/(alpha^3*beta^2*gamma*(1-alpha)*(1-beta)*(1-gamma)*duration^4);
ts = jmax/smax;
tj = amax/jmax + ts;
td = tj + (vmax - v1)/amax;
ta = tj + (vmax - v0)/amax;
tv = (q1 - q0)/vmax - ta*(1 + v0/vmax)/2 - td*(1 + v1/vmax)/2;
% if tj>=2*ts && ta>=2*tj && td>=2*tj && tv>=0
% disp('ok');
% end
% 分配空间
uv=zeros(1,n);vel=zeros(1,n);acc=zeros(1,n);jerk=zeros(1,n);snap=zeros(1,n);
for i=1:1:n
t=duration*(i-1)/(n-1);
%1
if t<=ts
uv(i)= smax/24*t^4 + v0*t + q0;
vel(i)=smax/6*t^3 + v0;
acc(i)=smax/2*t^2;
jerk(i)=smax*t;
snap(i)=smax;
%2
elseif (t>ts) && (t<=tj - ts )
uv(i)= jmax*(2*t - ts)*(2*t*(t-ts) + ts^2)/24 + v0*t + q0;
vel(i)=jmax/6*ts^2 + jmax/2*t*(t-ts) + v0;
acc(i)=jmax*t -jmax/2*ts;
jerk(i)=jmax;
snap(i)=0;
%3
elseif ((t>tj-ts) && t<=tj )
uv(i)= smax/24*(-15*(ts^4) + 28*(ts^3)*(t + ts) - 18*(ts^2)*((t+ts)^2) + 4*ts*(t + ts)^3 - (t-tj+ts)^4) + v0*t + q0;
vel(i)=smax/6*(7*ts^3-9*ts^2*(t+ts)+3*ts*(t+ts)^2-(t-tj+ts)^3) + v0;
acc(i)=-smax/2*(t-tj)^2+amax;
jerk(i)=-smax*(t-tj);
snap(i)=-smax;
%4
elseif (t>tj) && (t<=ta-tj )
uv(i)= amax*(6*t^2 - 6*t*tj + 2*tj^2 - tj*ts + ts^2)/12 +v0*t + q0;
vel(i)=amax/2*(2*t-tj)+v0;
acc(i)=amax;
jerk(i)=0;
snap(i)=0;
%5
elseif (t>ta-tj) && (t<=ta-tj+ts)
uv(i)=-smax/24*(t-ta+tj)^4 + amax*(6*t^2 - 6*t*tj + 2*tj^2 - tj*ts + ts^2)/12 + v0*t + q0;
vel(i)=-smax/6*(t-ta+tj)^3+amax/2*(2*t-tj)+v0;
acc(i)=amax-smax/2*(t-ta+tj)^2;
jerk(i)=-smax*(t-ta+tj);
snap(i)=-smax;
%6
elseif (t>ta-tj+ts) && (t<=ta-ts)
uv(i)=-jmax*(4*(t-ta)^3 - 12*(2*t-ta)*ta*tj + 12*(2*t-ta)*tj^2 + 6*(t^2+2*t*(ta-2*tj) - ta*(ta-2*tj))*ts + 4*(t-ta)*ts^2 + ts^3)/24 + v0*t + q0;
vel(i)=-jmax/6*(3*(t-ta)^2-6*ta*tj+6*tj^2+3*(t+ta-2*tj)*ts+ts^2)+v0;
acc(i)=-jmax/2*(2*t-2*ta+ts);
jerk(i)=-jmax;
snap(i)=0;
%7
elseif (t>ta-ts) && (t<=ta)
uv(i)=smax/24*(t-ta)^4 + amax/2*(2*t-ta)*(ta-tj) + v0*t +q0;
vel(i)=smax/6*(t-ta)^3+amax*(ta-tj)+v0;
acc(i)=smax/2*(t-ta)^2;
jerk(i)=smax*(t-ta);
snap(i)=smax;
%8
elseif (t>ta) && (t<=ta+tv)
uv(i)=(vmax-v0)/2*(2*t-ta) + v0*t +q0;
vel(i)=vmax;
acc(i)=0;
jerk(i)=0;
snap(i)=0;
%9
elseif (t>ta+tv) && (t<=ta+tv+ts)
uv(i)=-smax/24*((duration-t)-td)^4 - amax/2*(2*(duration-t)-td)*(td-tj)-v1*(duration-t) + q1;
vel(i)=smax/6*((duration-t)-td)^3+amax*(td-tj)+v1;
acc(i)=-smax/2*((duration-t)-td)^2;
jerk(i)=smax*((duration-t)-td);
snap(i)=-smax;
%10
elseif (t>ta+tv+ts) && (t<=ta+tv+tj-ts)
uv(i)=jmax/24*(4*((duration-t)-td)^3 - 12*(2*(duration-t)-td)*td*tj + 12*(2*(duration-t)-td)*tj^2 + 6*((duration-t)^2 + 2*(duration-t)*(td-2*tj)-td*(td-2*tj))*ts + 4*((duration-t)-td)*ts^2 +ts^3) - v1*(duration-t) + q1;
vel(i)=-jmax/6*(3*((duration-t)-td)^2-6*td*tj+6*tj^2+3*((duration-t)+td-2*tj)*ts+ts^2)+v1;
acc(i)=jmax/2*(2*(duration-t)-2*td+ts);
jerk(i)=-jmax;
snap(i)=0;
%11
elseif (t>ta+tv+tj-ts) && (t<=ta+tv+tj)
uv(i)=smax/24*((duration-t)-td+tj)^4 - amax/12*(6*(duration-t)^2 - 6*(duration-t)*tj + 2*tj^2 - tj*ts + ts^2) - v1*(duration-t) + q1;
vel(i)=-smax/6*((duration-t)-td+tj)^3+amax/2*(2*(duration-t)-tj)+v1;
acc(i)=-amax+smax/2*((duration-t)-td+tj)^2;
jerk(i)=-smax*((duration-t)-td+tj);
snap(i)=smax;
%12
elseif (t>ta+tv+tj) && (t<=duration-tj)
uv(i)=-amax/12*(6*(duration-t)^2 - 6*(duration-t)*tj + 2*tj^2 - tj*ts + ts^2) - v1*(duration-t) + q1;
vel(i)=amax/2*(2*(duration-t)-tj)+v1;
acc(i)=-amax;
jerk(i)=0;
snap(i)=0;
%13
elseif (t>duration-tj) && (t<=duration-tj+ts)
uv(i)=-smax/24*(-15*ts^4 + 28*ts^3*((duration-t)+ts) - 18*ts^2*((duration-t)+ts)^2 + 4*ts*((duration-t)+ts)^3 - ((duration-t)-tj+ts)^4) - v1*(duration-t)+q1;
vel(i)=smax/6*(7*ts^3-9*ts^2*((duration-t)+ts)+3*ts*((duration-t)+ts)^2-((duration-t)-tj+ts)^3)+v1;
acc(i)=smax/2*((duration-t)-tj)^2-amax;
jerk(i)=-smax*((duration-t)-tj);
snap(i)=smax;
%14
elseif (t>duration-tj+ts) && (t<=duration-ts)
uv(i)=-jmax/24*(2*(duration-t)-ts)*(2*(duration-t)*((duration-t)-ts)+ts^2) - v1*(duration-t)+q1;
vel(i)=jmax/6*ts^2+jmax/2*(duration-t)*((duration-t)-ts)+v1;
acc(i)=-jmax*(duration-t)+jmax/2*ts;
jerk(i)=jmax;
snap(i)=0;
%15
elseif (t>duration-ts) && (t<=duration)
uv(i) = -smax/24*(duration-t)^4 - v1*(duration-t) + q1;
vel(i)=smax/6*(duration-t)^3+v1;
acc(i)=-smax/2*(duration-t)^2;
jerk(i)=smax*(duration-t);
snap(i)=-smax;
end
lamda(i)=uv(i)/h; %归一化处理参数
end
end
15段函数绘图函数
% 函数功能：myFifteenSegmentsPlots(uv,vel,acc,jerk,snap,dt,T)十五段路径规划算法图像显示
% 输入参数：uv,vel,acc,jerk,snap,分别为规划位置、速度，加速度，加加速度和加加加速度
% dt,采样时间
% T,轨迹运行时间
%
% 输出参数：绘图
%% 绘图程序
function myFifteenSegmentsPlots(uv,vel,acc,jerk,snap,dt,T)
t=0:dt:T;
subplot(2,2,1);plot(t,uv,'k')
% title('位移曲线');
xlabel('t [s]');ylabel('position [rad]');grid on;
subplot(2,2,2);plot(t,vel,'k');
% r2d=180/3.14159;csvwrite('outputData_Tp.csv',Tp); %存储位置曲线数据
% title('速度曲线');
xlabel('t [s]');ylabel('velocity [rad/s]');grid on;
subplot(2,2,3);plot(t,acc,'k');
% title('加速度曲线');
xlabel('t [s]');ylabel('acceleration [rad/s^2]');grid on;
subplot(2,2,4);plot(t,jerk,'k');
% title('jerk曲线');
xlabel('t [s]');ylabel('jerk [rad/s^3]');grid on;
%
% plot(t,snap,'k');
% % title('snap曲线');
% xlabel('t [s]');ylabel('snap [rad/s^4]');grid on;
%
end
%
计算平面方程系数函数
% 计算三点构成的平面方程系数ABC
clear all;
clc;
syms x1 y1 z1;
syms x2 y2 z2;
syms x3 y3 z3;
syms A B C
eq1=A*x1+B*y1+C*z1==1;
eq2=A*x2+B*y2+C*z2==1;
eq3=A*x3+B*y3+C*z3==1;
equ=[eq1,eq2,eq3];
val=[A,B,C];
D=solve(equ,val);
fprintf('输出平面方程的系数ABC:');
fprintf('A:');
D.A
fprintf('B:');
D.B
fprintf('C:');
D.C
NLERP姿态插补算法
%[pi_Q_rotation]=myCircleNlerp(p1_Q,p3_Q,stepN)函数
%采用正规化线性插值算法对圆弧上的两点进行姿态插补，返回
%代表姿态的单位四元数序列p_Q_rotation
%p1_Q,p3_Q为圆弧上需要插补的首末两点姿态的四元数表示
%stepN为在该圆弧上需要插补的点数
%返回参数：pi_Q_rotation
function [pi_Q_rotation]=myCircleNlerp(p1_Q,p3_Q,stepN)
pi_Q_rotation = zeros(stepN,4); %分配存储矩阵；
%计算,判断最短路径，对最短路径插补姿态
cos_theta=p1_Q(1)*p3_Q(1)+p1_Q(2)*p3_Q(2)+p1_Q(3)*p3_Q(3)+p1_Q(4)*p3_Q(4);
if cos_theta < 0
p3_Q = -p3_Q;
end
%
for step=1:1:stepN
k1=step/stepN;
k0=1-k1;
pi_Q_rotation(step,:)=(p1_Q*k0+p3_Q*k1)/norm(p1_Q*k0+p3_Q*k1);
end
end
SLERP插补算法
% 函数功能:对姿态插补，再现Slerp的插补算法。
% 同时考虑在极小角度插补时采用线性插值，其他
% 时间插补采用球面线性插补，实现等速度插补
% 输入参数:pQi，为将要插补两点姿态插补计算后单位化，
% 用单位四元数表示；
% stepN，姿态插补路径点数；
% 参考资料：Matlab函数 PreSlerp
function [pQi] = myQSlerp(pQ1,pQ3,stepN)
%分配插补后数据的存储空间
pQi=zeros(stepN,4);
% delta=T/stepN; %计算采样时间，用于线性插补计算
%计算姿态并判断插补最短路径
dp=pQ1(1)*pQ3(1)+pQ1(2)*pQ3(2)+pQ1(3)*pQ3(3)+pQ1(4)*pQ3(4);
if dp<0
pQ3=-pQ3; %两四元数的dot product表示两个Q之间的夹角余弦，取反后，选取最短路径
dp=-dp;
elseif dp>=1
dp=1;
end
%计算Q之间的夹角,并取最短路径
Q_theta=acos(dp);
sinv=1./sin(Q_theta);
for step=0:1:stepN
if dp>0.995 %Q之间夹角接近0采用线性插补
k1=step/stepN;
k0=1-k1;
else
k0=sin((1-step/stepN).*Q_theta);
k1=sin((step/stepN).*Q_theta);
end
pQi(step+1,:)=(pQ1.*k0+pQ3.*k1).*sinv;
pQi(step+1,:)=pQi(step+1,:)/norm( pQi(step+1,:));
end
end
绘制四元数计算后的插补姿态位置曲线和速度曲线
%myQplot(trajSlerped,T,Qv0,stepN)函数功能：主要用于绘制四元数计算后的插补姿态位置曲线和速度曲线
%函数输入参数：
%接入初始速度Qv0,采样点数stepN,插补姿态四元数序列trajSlerped,插补时间T
function myQplot(trajSlerped,T,Qv0,stepN)
%定义初始速度avSlerp0=qv0
% Qv0=[0,0,0];
avSlerp0=Qv0;
dt=T/stepN; %计算插补步长
h = (0:dt:T).';
%绘图程序
% figure(1);
subplot(2,1,1);
%绘制姿态转动位置曲线
% avSlerp = eulerd(trajSlerped,'ZYX','frame'); %四元数转化为欧拉角deg
apSlerp = quat2eul(trajSlerped); %四元数转化为欧拉角rad
plot(h,apSlerp(:,1),'o','color','b');hold on;%获取转动姿态位置曲线
plot(h,apSlerp(:,2),'+','color','g');hold on;
plot(h,apSlerp(:,3),'*','color','r');grid on;
legend( 'X angular position (SLERP)', ...
'Y angular position (SLERP)', ...
'Z angular position (SLERP)');
xlabel('sampleTime/s');
ylabel('Angular Position(rad)');
title('Angular Velocity - SLERP');
%
%绘制姿态转动速度曲线
subplot(2,1,2);
avSlerp = myQuat2Qv(trajSlerped, dt); %计算姿态插补速度
%合成初始速度量qv0;
avSlerp=[avSlerp0;avSlerp];
pp=plot(h,avSlerp(:,1),h,avSlerp(:,2),h,avSlerp(:,3),...
'linewidth',0.5);grid on;%获取转动姿态位置曲线
pp(1).Color='r';pp(1).Marker='o';
pp(2).Color='g';pp(2).Marker='+';
pp(3).Color='b';pp(3).Marker='*';
legend( 'X angular position (SLERP)', ...
'Y angular position (SLERP)', ...
'Z angular position (SLERP)');
xlabel('sampleTime/s');
ylabel('Angular Position(rad/s)');
title('Angular Velocity - SLERP');
end
三点圆弧插值轨迹及圆心坐标算法
%myCircleplot3(p1,p2,p3,p_i_position)函数主要绘制三点圆弧插值轨迹及圆心坐标
%输入：
%pi:圆弧上三个点；
%p_i_position:插值后的点坐标矩阵序列
%
function myCircleplot3(p1,p2,p3,pc,p_i_position)
plot3(p_i_position(:,1),p_i_position(:,2),p_i_position(:,3),'*','Color','b');grid on;
hold on;
plot3(p1(1),p1(2),p1(3),'-o','Color','r','MarkerSize',10);
hold on;
plot3(p2(1),p2(2),p2(3),'-o','Color','r','MarkerSize',10);
hold on;
plot3(p3(1),p3(2),p3(3),'-o','Color','r','MarkerSize',10);
plot3(pc(1),pc(2),pc(3),'*','Color','r','MarkerSize',15);
end
——————————————————————————————————————————
补充数据：pointsData20200517
0.34 1.32 2.98
0.2932 1.4136 3.1204
0.2464 1.5072 3.2608
0.1996 1.6008 3.4012
0.1528 1.6944 3.5416
0.106 1.788 3.682
0.0592 1.8816 3.8224
0.0124 1.9752 3.9628
-0.0344 2.0688 4.1032
-0.0812 2.1624 4.2436
-0.128 2.256 4.384
-0.1748 2.3496 4.5244
-0.2216 2.4432 4.6648
-0.2684 2.5368 4.8052
-0.3152 2.6304 4.9456
-0.362 2.724 5.086
-0.4088 2.8176 5.2264
-0.4556 2.9112 5.3668
-0.5024 3.0048 5.5072
-0.5492 3.0984 5.6476
-0.596 3.192 5.788
-0.6428 3.2856 5.9284
-0.6896 3.3792 6.0688
-0.7364 3.4728 6.2092
-0.7832 3.5664 6.3496
-0.83 3.66 6.49
-0.8768 3.7536 6.6304
-0.9236 3.8472 6.7708
-0.9704 3.9408 6.9112
-1.0172 4.0344 7.0516
-1.064 4.128 7.192
-1.1108 4.2216 7.3324