├── LICENSE ├── README.md ├── documents ├── MDH表格.docx ├── 单节连续体运动学模型.tif └── 单节连续体运动学模型.vsdx └── src ├── Rope_test.m ├── Test_theta2rope.mlx ├── forwardKinematicsFor3SegmentsDH.m ├── objectiveFunctionFor3SegmentsDH.m ├── solveInverseKinematicsFor3SegmentsDH.m ├── testInverseKinematicsFor3Segments.m ├── test_eulerd.m └── theta2rope.m /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Lawrencehh 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # continuum_robot_matlab 2 | 一个用matlab来实现连续体机器人正逆运动学、动力学以及其他算法的仓库 3 | -------------------------------------------------------------------------------- /documents/MDH表格.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lawrencehh/continuum_robot_matlab/6a3959531285b26766b847dcb9c93c2d51250c22/documents/MDH表格.docx -------------------------------------------------------------------------------- /documents/单节连续体运动学模型.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lawrencehh/continuum_robot_matlab/6a3959531285b26766b847dcb9c93c2d51250c22/documents/单节连续体运动学模型.tif -------------------------------------------------------------------------------- /documents/单节连续体运动学模型.vsdx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lawrencehh/continuum_robot_matlab/6a3959531285b26766b847dcb9c93c2d51250c22/documents/单节连续体运动学模型.vsdx -------------------------------------------------------------------------------- /src/Rope_test.m: -------------------------------------------------------------------------------- 1 | Rope_intial=theta2rope ([0,0,0,0,0,0]) 2 | Rope_current =theta2rope([pi*5/180,pi*5/180,pi*5/180,pi*5/180,pi*5/180,pi*5/180]) 3 | Rope_current - Rope_intial -------------------------------------------------------------------------------- /src/Test_theta2rope.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lawrencehh/continuum_robot_matlab/6a3959531285b26766b847dcb9c93c2d51250c22/src/Test_theta2rope.mlx -------------------------------------------------------------------------------- /src/forwardKinematicsFor3SegmentsDH.m: -------------------------------------------------------------------------------- 1 | function T_end = forwardKinematicsFor3SegmentsDH(q) 2 | % 给变量赋值 3 | 4 | 5 | % DH方法一起赋同样的值 6 | 7 | % 确保 q 是一个大小为 7 的数组, q(1)为直线模组的进给，q(2)到q(7)为关节空间中的角度 8 | assert(length(q) == 7, 'Theta must be a 7-element array'); 9 | 10 | Shaft_Distance = 45; % 轴间距为45mm 11 | 12 | N = 5; %表示每节有5组正交轴，一共10个转轴 13 | T={}; %用来存放T0-1至T30-31变换矩阵 14 | T_end_theta_DH = eye(4); %初始化变换矩阵存放值，最后得到基坐标系到工具坐标系的T(theta) 15 | 16 | % 第1个关节的DH计算（直线模组） 17 | T{end+1} = [ 1 0 0 q(1) ;... 18 | 0 1 0 0 ;... 19 | 0 0 1 0 ;... 20 | 0 0 0 1 ]; 21 | T_end_theta_DH = T{end}; 22 | 23 | 24 | % 第1节的运动学 25 | for i = 1 : N 26 | if i == 1 27 | T{end+1} = [ cos(q(2)) -sin(q(2)) 0 0 ;... 28 | sin(q(2)) cos(q(2)) 0 0 ;... 29 | 0 0 1 0 ;... 30 | 0 0 0 1 ]; 31 | end 32 | if i > 1 33 | T{end+1} = [ cos(q(2)) -sin(q(2)) 0 Shaft_Distance;... 34 | 0 0 -1 0;... 35 | sin(q(2)) cos(q(2)) 0 0;... 36 | 0 0 0 1]; 37 | 38 | end 39 | T{end+1} = [ cos(q(3)) -sin(q(3)) 0 Shaft_Distance;... 40 | 0 0 1 0;... 41 | -sin(q(3)) -cos(q(3)) 0 0;... 42 | 0 0 0 1]; 43 | T_end_theta_DH = T_end_theta_DH*T{end-1}*T{end}; 44 | end 45 | 46 | % 第2节的运动学 47 | for i = 1 : N 48 | T{end+1} = [ cos(q(4)) -sin(q(4)) 0 Shaft_Distance;... 49 | 0 0 -1 0;... 50 | sin(q(4)) cos(q(4)) 0 0;... 51 | 0 0 0 1]; 52 | 53 | T{end+1} = [ cos(q(5)) -sin(q(5)) 0 Shaft_Distance;... 54 | 0 0 1 0;... 55 | -sin(q(5)) -cos(q(5)) 0 0;... 56 | 0 0 0 1]; 57 | T_end_theta_DH = T_end_theta_DH*T{end-1}*T{end}; 58 | end 59 | 60 | % 第3节的运动学 61 | for i = 1 : N 62 | T{end+1} = [ cos(q(6)) -sin(q(6)) 0 Shaft_Distance;... 63 | 0 0 -1 0;... 64 | sin(q(6)) cos(q(6)) 0 0;... 65 | 0 0 0 1]; 66 | 67 | T{end+1} = [ cos(q(7)) -sin(q(7)) 0 Shaft_Distance;... 68 | 0 0 1 0;... 69 | -sin(q(7)) -cos(q(7)) 0 0;... 70 | 0 0 0 1]; 71 | T_end_theta_DH = T_end_theta_DH*T{end-1}*T{end}; 72 | end 73 | 74 | % fprintf('DH参数方法得到的位姿变换矩阵为：') 75 | T_end = T_end_theta_DH; 76 | end -------------------------------------------------------------------------------- /src/objectiveFunctionFor3SegmentsDH.m: -------------------------------------------------------------------------------- 1 | function error = objectiveFunctionFor3SegmentsDH(q, desired_position, desired_orientation) 2 | % 调用正运动学函数 3 | T_end = forwardKinematicsFor3SegmentsDH(q); 4 | 5 | % 提取实际位置和方向 6 | actual_position = T_end(1:3, 4); 7 | actual_orientation = rotm2eul(T_end(1:3, 1:3), 'ZYX'); 8 | 9 | % 计算位置和方向的误差 10 | position_error = norm(desired_position - actual_position); 11 | orientation_error = norm(desired_orientation - actual_orientation); 12 | 13 | % 总误差 14 | error = position_error + 100* orientation_error; 15 | end 16 | -------------------------------------------------------------------------------- /src/solveInverseKinematicsFor3SegmentsDH.m: -------------------------------------------------------------------------------- 1 | function q = solveInverseKinematicsFor3SegmentsDH(desired_position, desired_orientation) 2 | % 初始猜测值 3 | initial_guess = zeros(7, 1); % 你可能需要提供一个更好的初始猜测值 4 | lb = [0, -pi*25/360, -pi*25/360, -pi*25/360, -pi*25/360, -pi*25/360, -pi*25/360]; % Lower bounds 5 | ub = [2000, pi*25/360, pi*255/360, pi*255/360, pi*25/360, pi*25/360, pi*25/360]; % Upper bounds 6 | % 调用 fmincon 函数求解优化问题 7 | options = optimoptions('fmincon','Algorithm','interior-point'); % 使用内点法 8 | q = fmincon(@(x) objectiveFunctionFor3SegmentsDH(x, desired_position, desired_orientation), initial_guess, [], [], [], [], lb, ub, [], options); 9 | end 10 | -------------------------------------------------------------------------------- /src/testInverseKinematicsFor3Segments.m: -------------------------------------------------------------------------------- 1 | % Test Instance for Inverse Kinematics 2 | 3 | 4 | % 定义初始 q 值, 关节空间变量 5 | q = [0, pi*-2/180, pi*3/180, pi*2/180, pi*1/180, pi*5/180, pi*6/180]; 6 | 7 | % 使用正运动学得到期望的位置和方向 8 | T_end_desired = forwardKinematicsFor3SegmentsDH(q); 9 | desired_position = T_end_desired(1:3, 4); 10 | desired_orientation = rotm2eul(T_end_desired(1:3, 1:3), 'ZYX'); 11 | 12 | tic; 13 | % 调用逆运动学求解器 14 | q_output = solveInverseKinematicsFor3SegmentsDH(desired_position, desired_orientation); 15 | T_end_output = forwardKinematicsFor3SegmentsDH(q_output); 16 | output_orientation = rotm2eul(T_end_output(1:3, 1:3), 'ZYX'); 17 | toc; 18 | 19 | % 显示输出结果 20 | disp('desired_orientation'); 21 | disp(desired_orientation); 22 | 23 | disp('output_orientation'); 24 | disp(rotm2eul(T_end_output(1:3, 1:3), 'ZYX')); 25 | 26 | 27 | disp('Input q Values:'); 28 | disp(q); 29 | 30 | disp('Output q Values:'); 31 | disp(q_output'); 32 | 33 | disp('T_end_desired:'); 34 | disp(T_end_desired); 35 | 36 | disp('T_end_output:'); 37 | disp(T_end_output); 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /src/test_eulerd.m: -------------------------------------------------------------------------------- 1 | % 输入 theta 和 displacement 的值求解正运动学的姿态矩阵 2 | theta = [5*pi/180,5*pi/180,5*pi/180,5*pi/180,5*pi/180,5*pi/180]; 3 | displacement = 10; 4 | T_end = forwardKinematicsFor3SegmentsDH(theta, displacement); 5 | disp(T_end); 6 | 7 | %从姿态矩阵中获取旋转矩阵R(3x3) 8 | R = T_end(1:3, 1:3) 9 | 10 | % 导入齐次变换矩阵到欧拉角的函数 11 | import robotics.* 12 | 13 | % 确定欧拉角的旋转顺序以及单位 14 | rotationOrder = 'ZYX'; % 按照需要设置旋转顺序 15 | rotationUnit = 'rad'; % 设置欧拉角的单位，可以是 'rad' 或者 'deg' 16 | eulerAngles = rotm2eul(R, rotationOrder)%由旋转矩阵输出得到的欧拉角 17 | -------------------------------------------------------------------------------- /src/theta2rope.m: -------------------------------------------------------------------------------- 1 | function Rope_length = theta2rope (theta) 2 | 3 | %定义各间隙变量，相应地赋予固定值 4 | Gap_1(1:12) = [0,0,0,0,0,0,0,0,0,0,0,0]; %第1个间隙 5 | Gap_2(1:12) = [29.63,29.63,29.09,29.09,29.63,29.63,29.63,29.63,29.09,29.09,29.63,29.63]; %第2个间隙, 固定值 6 | Gap_3(1:12) = [0,0,0,0,0,0,0,0,0,0,0,0]; %第3个间隙 7 | Gap_4(1:12) = [29.63,29.63,29.09,29.09,29.63,29.63,29.63,29.63,29.09,29.09,29.63,29.63]; %第4个间隙, 固定值 8 | Gap_base(1:12) = [24.57,24.57,26.11,26.11,28.12,28.12,28.12,28.12,26.11,26.11,24.57,24.57];%基座圆盘处绳长，固定值 9 | 10 | Rope_length=zeros(1, 12); %12根绳长的当前长度 11 | beta = pi*50/180; % gap1和gap3的初始角度 12 | R_Gap_1(1:12) = [23.03,23.03,18.82,18.82,13.33,13.33,13.33,13.33,18.82,18.82,23.03,23.03]; % 代表每个孔心在Gap1处到旋转轴的垂线长度 13 | R_Gap_3(1:12) = [13.33,13.33,18.82,18.82,23.03,23.03,23.03,23.03,18.82,18.82,13.33,13.33]; % 代表每个孔心在Gap3处到旋转轴的垂线长度 14 | 15 | % 第一节代表根部关节； 16 | % 1，2,11,12 17 | 18 | % 第一节的 19 | %对12根绳都需计算 20 | for N = 1:5 21 | for i = 1:12 22 | if i > 6 23 | Gap_1(i) = sin((beta-theta(1))/2) * 2*R_Gap_1(i); 24 | end 25 | if i <= 6 26 | Gap_1(i) = sin((beta+theta(1))/2) * 2*R_Gap_1(i); 27 | end 28 | 29 | if mod(i,2) ==1 30 | Gap_3(i) = sin((beta-theta(2))/2) * 2*R_Gap_3(i); 31 | end 32 | if mod(i,2) ==0 33 | Gap_3(i) = sin((beta+theta(2))/2) * 2*R_Gap_3(i); 34 | end 35 | 36 | Rope_length(i) = Rope_length(i) + Gap_1(i) + Gap_2(i) + Gap_3(i) + Gap_4(i)+Gap_base(i); 37 | end 38 | end 39 | 40 | % 第二节的 41 | % 除了1,2,11,12 42 | for N = 1:5 43 | for i = 1:12 44 | if(i==1 | i==2 | i ==11 | i ==12) 45 | Rope_length(i) = Rope_length(i) +0; 46 | 47 | else 48 | if i > 6 49 | Gap_1(i) = sin((beta-theta(3))/2) * 2*R_Gap_1(i); 50 | end 51 | if i <= 6 52 | Gap_1(i) = sin((beta+theta(3))/2) * 2*R_Gap_1(i); 53 | end 54 | 55 | if mod(i,2) ==1 56 | Gap_3(i) = sin((beta-theta(4))/2) * 2*R_Gap_3(i); 57 | end 58 | if mod(i,2) ==0 59 | Gap_3(i) = sin((beta+theta(4))/2) * 2*R_Gap_3(i); 60 | end 61 | 62 | Rope_length(i) = Rope_length(i) + Gap_1(i) + Gap_2(i) + Gap_3(i) + Gap_4(i); 63 | end 64 | end 65 | end 66 | 67 | % 第三节的 68 | % 除了1,2,11,12, 3,4,9,10 69 | for N = 1:5 70 | for i = 1:12 71 | if(i == 1 | i == 2 | i == 11 | i == 12 | i == 3 | i == 4 | i == 9 | i == 10 ) 72 | Rope_length(i) = Rope_length(i) +0; 73 | else 74 | if i > 6 75 | Gap_1(i) = sin((beta-theta(5))/2) * 2*R_Gap_1(i); 76 | end 77 | if i <= 6 78 | Gap_1(i) = sin((beta+theta(5))/2) * 2*R_Gap_1(i); 79 | end 80 | 81 | if mod(i,2) ==1 82 | Gap_3(i) = sin((beta-theta(6))/2) * 2*R_Gap_3(i); 83 | end 84 | if mod(i,2) ==0 85 | Gap_3(i) = sin((beta+theta(6))/2) * 2*R_Gap_3(i); 86 | end 87 | 88 | Rope_length(i) = Rope_length(i) + Gap_1(i) + Gap_2(i) + Gap_3(i) + Gap_4(i); 89 | end 90 | end 91 | end 92 | 93 | 94 | 95 | % 特殊情况，每个关节和下一个关节连接处的圆盘内绳长（即绿色块内的绳长）需减掉一截Gap_4 96 | for i = 1:12 97 | Rope_length(i) = Rope_length(i) - Gap_4(i); 98 | end 99 | 100 | end 101 | 102 | --------------------------------------------------------------------------------