u014077947的博客

原创 雅克比矩阵

原创 Matlab T型速度规划

%% 梯形速度曲线%% 假定初始速度和最终速度为零，给定最大的加速度、运行时间、开始角度 、结束角度function trapezoid()%给定初始条件t0=0;tf=10;q0=0;qf=20;a_max=1;if a_max>= 4*(qf-q0)/(tf-t0)^2% 先判断能否求解到t_f，给定的最大加速度是否满足条件 % t_f 加速至匀速阶段的...

原创 三点求圆公式Matlab 和C++版程序

三点求圆公式Matlab 和C++版程序%% 给定三个点做圆弧轨迹规划function P_P=plot_circle(p1,p2,p3)%% 利用这三个点做一个平面方程k_11=( p1(2)-p3(2) )*( p2(3)-p3(3) ) - ( p2(2)-p3(2) )*( p1(3)-p3(3) ) ;k_12=( p2(1)-p3(1) )*( p1(3)-p3(3) )...

原创 求饶不经过原点的旋转轴的旋转矩阵

看《机器人导论》中关于齐次变换的内容中，发现饶轴旋转的时候，分为两种情况。一、饶经过原点的轴进行旋转；二、饶不经过原点的旋转轴进行旋转。其实这两种情况下，采用旋转向量 和 旋转矩阵的相互转换的几种方法 这里面的方法都是可以求到旋转矩阵的，但是这两种旋转方式中的变换矩阵中的位置矩阵是不一样的。饶经过原点的轴进行旋转的变换矩阵，由于原点的位置没有发生改变，所以变换矩阵中的位置矩阵是零，而第二种方法中...

原创 空间中位姿矩阵的插补算法代码，旋转中采用四元数进行插补

// 五次多项式生成void Five_ploynomial(double t0, double tf, double q0, double qf, double d_q0, double d_qf, double dd_q0, double dd_qf, VectorXd & qt){ double A, B, C, D, E, F; A = (double...

原创 旋转向量 和 旋转矩阵的相互转换的几种方法

最近在做旋转向量转旋转矩阵的，尝试了几种方法，现在总结一下，方便以后使用。pi=3.1415926;qq_1= - pi/6;p1=[0.8;0.6;0.5];p2=[1;2;3];PP=p2-p1;PP=PP/sqrt( PP(1)^2 + PP(2)^2 + PP(3)^2 );%% 采用罗德里格斯公式， 需要进行单位化，不进行单位化的话，算出来的结果是不一样的R...