KR102492869B1

KR102492869B1 - Control Method for Balancing and Tracking of a Ballbot using Projected Task Space Dynamics with Inequality Constraints and Control System using the same

Info

Publication number: KR102492869B1
Application number: KR1020200140151A
Authority: KR
Inventors: 조준희; 오용환
Original assignee: 한국과학기술연구원
Priority date: 2020-10-27
Filing date: 2020-10-27
Publication date: 2023-01-31
Also published as: KR20220055738A

Abstract

본 발명은, 볼과 로봇을 구비하는 볼봇의 자세 균형 제어 방법으로서, 볼, 로봇 및 지면에 대한 제1운동방정식을 구성하는 단계; 상기 제1운동방정식으로부터 지면접촉 영공간 행렬을 이용하여 제2운동방정식을 도출하는 단계; 볼과 로봇 각각의 속도에 근거하여 작업 변수를 설정하는 단계; 상기 작업 변수 및 제2운동방정식에 근거하여, 제2운동방정식이 작업 변수에 의해 변환된 방정식인 제3방정식을 도출하는 단계; 및 상기 제3방정식으로부터 볼과 로봇 사이의 접촉힘 및 제어 토크 중 적어도 하나를 제어하는 단계를 포함하는 볼봇의 자세 균형 제어 방법 및 제어 시스템을 제공한다.The present invention provides a posture balance control method for a ball-bot having a ball and a robot, comprising the steps of constructing a first equation of motion for a ball, a robot and the ground; deriving a second equation of motion from the first equation of motion by using a ground contact null space matrix; Setting a work variable based on the speed of each ball and robot; Deriving a third equation, which is an equation in which the second equation of motion is converted by the working variable, based on the working variable and the second equation of motion; and controlling at least one of a contact force between the ball and the robot and a control torque based on the third equation.

Description

Control Method for Balancing and Tracking of a Ballbot using Projected Task Space Dynamics with Inequality Constraints and Control System using the same}

본 발명은 볼봇의 자세 균형 제어 방법 및 제어 시스템에 관한 것으로서, 보다 상세하게는 영공간으로 투영된 작업 공간을 활용한 볼봇의 자세 균형 제어 방법 및 제어 시스템에 관한 것이다.The present invention relates to a method and control system for controlling the posture balance of a robot, and more particularly, to a method and control system for controlling the posture balance of a robot using a workspace projected into a null space.

볼봇(ballbot)은 지면과 바로 접촉하지 않고, 지면 위의 공과 접촉한다. The ballbot does not make direct contact with the ground, but with a ball on the ground.

기존의 볼봇의 경우는, 공과 지면 사이의 속도나 마찰 등의 접촉 정보를 알기가 어려운 문제가 있었다. In the case of existing ballbots, it is difficult to know contact information such as speed or friction between the ball and the ground.

따라서, 볼봇 전체 시스템에 대한 동역학을 다루기 어려우므로 공과 지면에 대한 작업의 영공간으로 시스템 운동방정식을 투영하여 독립적인 운동방정식을 획득할 수 있는 방법이 요구된다.Therefore, since it is difficult to deal with the dynamics of the entire ballbot system, a method for acquiring independent equations of motion by projecting the system equations of motion into the null space of the ball and the ground is required.

특히, 영공간의 운동방정식을 이용한 작업공간 운동방정식을 통해 지면 접촉에 대한 정보 없이 자유 운동할 수 있도록 하는 제어 방법 및 제어 시스템의 개발이 요구된다.In particular, it is required to develop a control method and control system capable of free movement without information on ground contact through the workspace equation of motion using the null space equation of motion.

한국 특허 등록 공보 제10-0176249호(1999.5.1)Korean Patent Registration Publication No. 10-0176249 (May 1, 1999)

본 발명은 상기의 과제를 해결하기 위해 안출된 것으로서, 본 발명의 목적은 영공간의 운동방정식을 이용한 작업공간 운동방정식을 통해 지면 접촉에 대한 정보 없이 자유 운동할 수 있도록 하는 제어 방법 및 제어 시스템을 제공하는 것이다.The present invention has been made to solve the above problems, and an object of the present invention is to provide a control method and control system that allows free movement without information on ground contact through a workspace equation of motion using a null space equation of motion. is to provide

상기의 과제를 해결하기 위해, 본 발명의 볼봇의 자세 균형 제어 방법은, 볼과 로봇을 구비하는 볼봇의 자세 균형 제어 방법으로서, 볼, 로봇 및 지면에 대한 제1운동방정식을 구성하는 단계; 상기 제1운동방정식으로부터 지면접촉 영공간 행렬을 이용하여 제2운동방정식을 도출하는 단계; 볼과 로봇 각각의 속도에 근거하여 작업 변수를 설정하는 단계; 상기 작업 변수 및 제2운동방정식에 근거하여, 제2운동방정식이 작업 변수에 의해 변환된 방정식인 제3방정식을 도출하는 단계; 및 상기 제3방정식으로부터 볼과 로봇 사이의 접촉힘 및 제어 토크 중 적어도 하나를 제어하는 단계를 포함한다. In order to solve the above problems, a method for controlling the posture balance of a ball robot of the present invention is a method for controlling the posture balance of a ball robot having a ball and a robot, comprising the steps of constructing a first motion equation for the ball, the robot and the ground; deriving a second equation of motion from the first equation of motion by using a ground contact null space matrix; Setting a work variable based on the speed of each ball and robot; Deriving a third equation, which is an equation in which the second equation of motion is converted by the working variable, based on the working variable and the second equation of motion; and controlling at least one of a contact force between the ball and the robot and a control torque based on the third equation.

본 발명과 관련된 일 예에 따르면, 상기 제1운동방정식은 [수학식 1]에 의해 정의된다. According to an example related to the present invention, the first equation of motion is defined by [Equation 1].

[수학식 1] [Equation 1]

[수학식 1]에서, M'_B은 로봇의 관성 행렬, M'_K은 볼의 관성 행렬,

은 일반화 좌표계의 가속도, C'_B은 로봇에 대한 코리올리 힘 및 원심력, C'_K은 볼에 대한 코리올리 힘 및 원심력,

는

에 의해 표현되는 일반화 좌표계, ξ _B ^T는 로봇의 일반화 좌표계를 표현하는 전치행렬, ξ _K ^T는 볼의 일반화 좌표계를 표현하는 전치행렬,

은 일반화 좌표계의 속도,

은 일반화 좌표계의 가속도, g'_B는 로봇에 대한 중력 벡터, g'_K는 볼에 대한 중력 벡터, B'_B는 로봇 작업 선택 행렬,

는 로봇의 관절 토크, J ^T _CB는 로봇과 접촉점 사이의 자코비안(Jacobian)의 전치행렬, G ^T _CK는 볼과 접촉점 사이의 자코비안의 전치행렬, f _C는 휠과 볼 사이의 접촉힘, G ^T _NK는 볼과 지면 사이의 자코비안, f _N는 볼과 지면 사이의 접촉힘을 각각 나타낸다.In [Equation 1], M ' _B is the inertia matrix of the robot, M ' _K is the inertia matrix of the ball,

is the acceleration of the generalized coordinate system, C ' _B is the Coriolis force and centrifugal force on the robot, C ' _K is the Coriolis force and centrifugal force on the ball,

Is

The generalized coordinate system represented by , ξ _B ^T is the transposed matrix representing the generalized coordinate system of the robot, ξ _K ^T is the transposed matrix representing the generalized coordinate system of the ball,

is the velocity of the generalized coordinate system,

is the acceleration of the generalized coordinate system, g ' _B is the gravity vector for the robot, g ' _K is the gravity vector for the ball, B ' _B is the robot action selection matrix,

is the joint torque of the robot, J ^T _CB is the Jacobian transposition matrix between the robot and the contact point, G ^T _CK is the Jacobian transpose matrix between the ball and the contact point, f _C is the contact force between the wheel and the ball, G ^T _NK represents the Jacobian between the ball and the ground, and f _N represents the contact force between the ball and the ground, respectively.

또한, 상기 제2운동방정식은 [수학식 2]에 의해 도출될 수 있다. In addition, the second equation of motion may be derived by [Equation 2].

[수학식 2][Equation 2]

[수학식 2]에서 ,

는 지면접촉 영공간으로 투영된 관성을 나타내는 행렬,

은 지면접촉 영공간으로 투영된 코리올리힘 및 원심력을 나타내는 행렬,

는

에 의해 표현되는 일반화 좌표계, ξ _B는 로봇의 일반화 좌표계를 표현하는 벡터, ξ _K는 볼의 일반화 좌표계를 표현하는 벡터,

은 일반화 좌표계의 속도,

은 일반화 좌표계의 가속도,

는 지면접촉 영공간으로 투영된 중력 벡터,

는 지면접촉 영공간으로 투영된 선택 행렬,

은 지면접촉 영공간으로 투영된 휠과 볼 사이의 접촉 자코비안,

은 휠과 볼 사이의 접촉힘을 나타낸다.In [Equation 2],

is a matrix representing the inertia projected into the ground contact null space,

is a matrix representing the Coriolis force and centrifugal force projected into the ground contact null space,

Is

The generalized coordinate system represented by ξ _B is a vector representing the robot's generalized coordinate system, ξ _K is a vector representing the ball's generalized coordinate system,

is the velocity of the generalized coordinate system,

is the acceleration of the generalized coordinate system,

is the gravity vector projected into the ground contact null space,

is the selection matrix projected into the ground contact null space,

is the contact Jacobian between the wheel and the ball projected into the ground contact null space,

represents the contact force between the wheel and the ball.

또한, 상기 작업 변수의 설정은 [수학식 3]에 의해 이루어질 수 있다. In addition, the setting of the working variable may be performed by [Equation 3].

[수학식 3][Equation 3]

[수학식 3]에서,

는 작업 변수,

은 로봇의 무게 중심에서의 x방향으로의 속도,

은 로봇의 무게 중심에서의 y방향으로의 속도,

은 공의 중심에서의 x방향으로의 속도, 윗첨자 T는 전치 행렬(transposed matrix),

은 공의 중심에서의 y방향으로의 속도,

은 로봇의 무게중심 작업 공간 행렬,

은 볼 작업 공간 행렬,

은 일반화 좌표계의 속도, T

에서 T는 작업 공간 행렬이다.In [Equation 3],

is the working variable,

is the velocity in the x direction from the center of gravity of the robot,

is the velocity in the y direction from the center of gravity of the robot,

is the velocity in the x direction at the center of the ball, the superscript T is the transposed matrix,

is the velocity in the y direction from the center of the ball,

is the barycentric workspace matrix of the robot,

is the ball workspace matrix,

is the velocity of the generalized coordinate system, T

where T is the working space matrix.

본 발명과 관련된 다른 일 예에 따르면, 상기 제3방정식은 [수학식 4]에 의해 표현될 수 있다. According to another example related to the present invention, the third equation may be expressed by [Equation 4].

[수학식 4][Equation 4]

[수학식 4]에서,

은 영공간에서 작업공간으로 투영된 로봇의 무게 중심 관련 관성행렬,

은 영공간에서 작업공간으로 투영된 볼 관련 관성행렬,

는 로봇의 무게중심과 공의 중심에서의 x 및 y 방향으로의 가속도를 포함하는 작업 변수이고,

는 영공간에서 작업공간으로 투영된 로봇의 무게 중심 관련 코리올리 힘 및 원심력 행렬,

는 영공간에서 작업공간으로 투영된 볼 관련 코리올리 힘 및 원심력 행렬,

는 전술한 바와 같이 로봇의 무게중심과 공의 중심에서의 x 및 y 방향으로의 속도를 포함하는 작업 변수이고,

는 영공간에서 작업공간으로 투영된 로봇의 무게 중심 관련 중력 벡터,

는 영공간에서 작업공간으로 투영된 볼 관련 중력 벡터,

는 영공간에서 작업공간으로 투영된 로봇의 무게 중심 관련 선택 행렬,

는 로봇의 관절 토크,

는 영공간에서 작업공간으로 투영된 휠과 볼 사이의 자코비안의 전치행렬,

는 영공간에서 작업공간으로 투영된 볼과 지면 사이의 자코비안의 전치행렬,

은 휠과 볼 사이의 접촉힘을 나타낸다.In [Equation 4],

is the inertia matrix related to the center of gravity of the robot projected from the null space to the workspace,

is the inertia matrix related to the ball projected from the null space to the workspace,

is a work variable including the acceleration in the x and y directions at the center of gravity of the robot and the center of the ball,

is the Coriolis force and centrifugal force matrix related to the center of gravity of the robot projected from the null space to the workspace,

is the ball-related Coriolis force and centrifugal force matrix projected from the null space to the workspace,

As described above, is a work variable including the velocity in the x and y directions at the center of gravity of the robot and the center of the ball,

is the gravity vector related to the center of gravity of the robot projected from the null space to the workspace,

is the ball-related gravity vector projected from null space to workspace,

is the selection matrix related to the center of gravity of the robot projected from the null space to the workspace,

is the joint torque of the robot,

is the Jacobian transposition matrix between the wheel and the ball projected from the null space to the workspace,

is the Jacobian transposition matrix between the ball and the ground projected from the null space to the workspace,

represents the contact force between the wheel and the ball.

또한, 상기 접촉힘은 [수학식 5]에 의해 표현될 수 있다. In addition, the contact force can be expressed by [Equation 5].

[수학식 5][Equation 5]

[수학식 5]에서,

는 영공간에서 작업공간으로 투영된 볼에 가해지는 접촉힘,

는 영공간에서 작업공간으로 투영된 볼관련 관성행렬,

은 레퍼런스 작업 가속도, G ^T _C,K는 휠과 볼 사이의 자코비안의 전치행렬,

은 휠과 볼사이의 접촉힘이다.In [Equation 5],

is the contact force applied to the ball projected from the null space to the workspace,

is the inertia matrix related to the ball projected from the null space to the work space,

is the reference work acceleration, G ^T _C,K is the Jacobian transposition matrix between wheel and ball,

is the contact force between the wheel and the ball.

본 발명과 관련된 또 다른 일 예에 따르면, 상기 제어 토크는 [수학식 6]에 의해 표현될 수 있다. According to another example related to the present invention, the control torque may be expressed by [Equation 6].

[수학식 6][Equation 6]

[수학식 6]에서,

는 로봇의 관절 토크,

는 영공간에서 작업공간으로 투영된 로봇의 무게 중심 관련 선택 행렬의 일반화 역행렬이고,

은 레퍼런스 작업 가속도이고,

이고,

은 로봇 작업 레퍼런스 가속도,

은 볼 작업 레퍼런스 가속도,

는 스케일링 이득,

은 모멘트 중심 반사 운동(Com reflex), K _K,p는 볼 작업 비례이득, K _K,d는 볼 작업 감쇠이득,

는 작업 속도,

은 Quadratic Programming(QP)로부터 계산된 접촉힘이다.In [Equation 6],

is the joint torque of the robot,

is the generalized inverse matrix of the selection matrix related to the center of gravity of the robot projected from the null space to the workspace,

is the reference work acceleration,

ego,

is the robot work reference acceleration,

is the ball work reference acceleration,

is the scaling gain,

is the moment centered reflex (Com reflex), K _K,p is the ball work proportional gain, K _K,d is the ball work damping gain,

is the work rate,

is the contact force calculated from Quadratic Programming (QP).

또 다른 과제를 해결하기 위해, 전술한 볼봇의 자세 균형 제어 방법을 이용하는 볼봇의 자세 균형 제어 시스템으로서, 볼봇의 자세 균형 제어 시스템은, 정의된 작업들을 이용해 레퍼런스 값을 생성하는 목표치 생성부; 목표치 생성부로부터 생성된 로봇의 레퍼런스 값을 제공받아서 제어 입력과 접촉힘을 계산하는 제어부; 지면에 놓여지는 볼과, 볼에 접촉하는 로봇을 포함하고, 외부힘에 의해 운동하는 볼봇; 및 볼봇의 운동으로부터 얻어진 변수들을 계산하고, 계산된 변수들을 제어부로 제공하는 산출부를 포함한다.In order to solve another problem, as a posture balance control system for a ball-bot using the above-described posture balance control method for a ball-bot, the system for controlling the posture balance of a ball-bot includes: a target value generator for generating reference values using defined tasks; a controller that calculates a control input and a contact force by receiving the reference value of the robot generated from the target value generator; A ball robot that includes a ball placed on the ground and a robot that contacts the ball and moves by an external force; and a calculating unit that calculates variables obtained from the movement of the ballbot and provides the calculated variables to a controller.

본 발명의 제어 방법 및 제어 시스템은, Quadratic Programming(QP)을 이용한 균형 제어가 적용될 수 있으며, 볼과 지면의 공간의 영공간에서의 작업을 기반으로 수행될 수 있다. In the control method and control system of the present invention, balance control using quadratic programming (QP) may be applied and may be performed based on work in the null space of the space between the ball and the ground.

또한, 본 발명의 제어 방법 및 제어 시스템은, 영공간으로의 투영을 통해 볼과 지면의 접촉에 대한 정보 없이 로봇 시스템 운동방정식을 다룰 수 있다. In addition, the control method and control system of the present invention can handle the equation of motion of the robot system without information on contact between the ball and the ground through projection into the null space.

또한, 본 발명의 제어 방법 및 제어 시스템은, 지면 접촉에 대한 정보 없이 제어가 요구되는 모바일 로봇 등의 분야에도 적용될 수 있다.In addition, the control method and control system of the present invention can be applied to fields such as mobile robots requiring control without ground contact information.

도 1은 본 발명의 볼봇의 자세 균형 제어 방법을 도시하는 순서도.
도 2는 일반화 좌표 내에 볼봇이 배치된 예를 도시하는 개념도.
도 3은 본 발명의 볼봇의 자세 균형 제어 시스템을 도시하는 블록도.
도 4는 일반화 좌표 내에서 볼봇이 이동하는 예를 도시하는 개념도.
도 5a는 일반화 좌표 내에 볼봇이 배치된 예를 3차원으로 도시하는 개념도.
도 5b는 도 5a에서 볼봇의 질량 및 길이를 표시한 개념도.
도 6은 로봇의 작업 이동을 나타내는 그래프.
도 7은 로봇의 작업 속도를 나타내는 그래프.
도 8은 로봇의 작업 이동을 나타내는 그래프.1 is a flow chart showing a method for controlling the posture balance of a ball-bot according to the present invention.
2 is a conceptual diagram showing an example in which a ball-bot is placed in generalized coordinates;
3 is a block diagram showing a posture balance control system for a ball-bot according to the present invention;
4 is a conceptual diagram illustrating an example of a ballbot moving within a generalized coordinate;
5A is a conceptual diagram illustrating an example in which a ballbot is disposed within generalized coordinates in three dimensions;
Figure 5b is a conceptual diagram showing the mass and length of the robot in Figure 5a.
Figure 6 is a graph showing the work movement of the robot.
7 is a graph showing the working speed of the robot.
Figure 8 is a graph showing the work movement of the robot.

이하, 첨부된 도면을 참조하여 본 명세서에 개시된 실시 예를 상세히 설명하되, 동일하거나 유사한 구성요소에는 동일, 유사한 도면 부호를 부여하고 이에 대한 중복되는 설명은 생략하기로 한다. 이하의 설명에서 사용되는 구성요소에 대한 접미사 "부"는 명세서 작성의 용이함만이 고려되어 부여되거나 혼용되는 것으로서, 그 자체로 서로 구별되는 의미 또는 역할을 갖는 것은 아니다. 또한, 본 명세서에 개시된 실시 예를 설명함에 있어서 관련된 공지 기술에 대한 구체적인 설명이 본 명세서에 개시된 실시 예의 요지를 흐릴 수 있다고 판단되는 경우 그 상세한 설명을 생략한다. 또한, 첨부된 도면은 본 명세서에 개시된 실시 예를 쉽게 이해할 수 있도록 하기 위한 것일 뿐, 첨부된 도면에 의해 본 명세서에 개시된 기술적 사상이 제한되지 않으며, 본 발명의 사상 및 기술 범위에 포함되는 모든 변경, 균등물 내지 대체물을 포함하는 것으로 이해되어야 한다.Hereinafter, the embodiments disclosed in this specification will be described in detail with reference to the accompanying drawings, but the same or similar reference numerals are given to the same or similar components, and overlapping descriptions thereof will be omitted. The suffix "part" for components used in the following description is given or used interchangeably in consideration of ease of writing the specification, and does not itself have a meaning or role distinct from each other. In addition, in describing the embodiments disclosed in this specification, if it is determined that a detailed description of a related known technology may obscure the gist of the embodiment disclosed in this specification, the detailed description thereof will be omitted. In addition, the accompanying drawings are only for easy understanding of the embodiments disclosed in this specification, the technical idea disclosed in this specification is not limited by the accompanying drawings, and all changes included in the spirit and technical scope of the present invention , it should be understood to include equivalents or substitutes.

제1, 제2 등과 같이 서수를 포함하는 용어는 다양한 구성요소들을 설명하는데 사용될 수 있지만, 상기 구성요소들은 상기 용어들에 의해 한정되지는 않는다. 상기 용어들은 하나의 구성요소를 다른 구성요소로부터 구별하는 목적으로만 사용된다.Terms including ordinal numbers, such as first and second, may be used to describe various components, but the components are not limited by the terms. These terms are only used for the purpose of distinguishing one component from another.

어떤 구성요소가 다른 구성요소에 "연결되어" 있다고 언급된 때에는, 그 다른 구성요소에 직접적으로 연결되어 있을 수도 있지만, 중간에 다른 구성요소가 존재할 수도 있다고 이해되어야 할 것이다. It should be understood that when an element is referred to as being “connected” to another element, it may be directly connected to the other element, but other elements may exist in the middle.

단수의 표현은 문맥상 명백하게 다르게 뜻하지 않는 한, 복수의 표현을 포함한다.Singular expressions include plural expressions unless the context clearly dictates otherwise.

본 출원에서, "포함한다" 또는 "가지다" 등의 용어는 명세서상에 기재된 특징, 숫자, 단계, 동작, 구성요소, 부품 또는 이들을 조합한 것이 존재함을 지정하려는 것이지, 하나 또는 그 이상의 다른 특징들이나 숫자, 단계, 동작, 구성요소, 부품 또는 이들을 조합한 것들의 존재 또는 부가 가능성을 미리 배제하지 않는 것으로 이해되어야 한다.In this application, terms such as "comprise" or "have" are intended to designate that there is a feature, number, step, operation, component, part, or combination thereof described in the specification, but one or more other features It should be understood that the presence or addition of numbers, steps, operations, components, parts, or combinations thereof is not precluded.

도 1을 참조하면, 본 발명의 볼봇의 자세 균형 제어 방법(S100)은 볼, 로봇 및 지면에 대한 제1운동방정식을 구성하는 단계(S10), 상기 제1운동방정식으로부터 지면접촉 영공간 행렬을 이용하여 제2운동방정식을 도출하는 단계(S20), 볼과 로봇 각각의 속도에 근거하여 작업 변수를 설정하는 단계(S30), 상기 작업 변수 및 제2운동방정식에 근거하여, 제2운동방정식이 작업 변수에 의해 변환된 방정식인 제3운동방정식을 도출하는 단계(S40) 및 상기 제3운동방정식으로부터 볼과 로봇 사이의 접촉힘 및 제어 토크 중 적어도 하나를 제어하는 단계(S50)를 포함한다. Referring to FIG. 1, the method for controlling the posture balance of a robot of the present invention (S100) comprises the step of constructing a first equation of motion for a ball, a robot, and the ground (S10), and a ground contact null space matrix from the first equation of motion Deriving a second equation of motion using (S20), setting a work variable based on the speed of each ball and robot (S30), based on the work variable and the second equation of motion, the second equation of motion is Deriving a third equation of motion, which is an equation converted by a work variable (S40), and controlling at least one of a contact force between a ball and a robot and a control torque from the third equation of motion (S50).

또한, 본 발명에서, 볼봇(ballbot, 30)은, 볼(31)과 로봇(35)을 구비하는데(도 2), 볼봇의 구조에 대해 보다 자세한 설명은 후술한다. Further, in the present invention, the ballbot 30 includes a ball 31 and a robot 35 (FIG. 2), and a more detailed description of the structure of the ballbot will be described later.

볼, 로봇 및 지면에 대한 제1운동방정식을 구성하는 단계(S10)에서, 제1운동방정식은, 로봇과 볼에 대한 정보, 및 지면과 볼 사이의 구속 조건을 포함하는 로봇과 볼 시스템에 대한 운동방정식으로 이해될 수 있다.In the step of constructing the first equation of motion for the ball, the robot, and the ground (S10), the first equation of motion is for the robot and the ball system including information about the robot and the ball, and constraint conditions between the ground and the ball. can be understood in terms of the equations of motion.

볼, 로봇 및 지면에 대한 제1운동방정식을 구성하는 단계(S10)에서, 볼, 로봇 및 지면에 대한 제1운동방정식은 [수학식 1]에 의해 표현될 수 있다.In the step of constructing the first equations of motion for the ball, the robot, and the ground (S10), the first equations of motion for the ball, the robot, and the ground may be expressed by [Equation 1].

[수학식 1] [Equation 1]

는

은 일반화 좌표계의 속도,

Is

is the velocity of the generalized coordinate system,

또한, [수학식 1]에서의 좌변은

으로 나타낼 수 있고, 우변은

으로 나타낼 수 있는데, M'는 관성 행렬, C'는 코리올리 힘 및 원심력 행렬, g'는 중력 벡터, B'는 작업 선택 행렬, G ^T' _C는 휠과 볼 사이의 자코비안, G ^T' _N는 볼과 지면 사이의 자코비안(Jacobian)이다.In addition, the left side in [Equation 1] is

, and the right side is

where M 'is the inertia matrix, C ' is the Coriolis force and centrifugal force matrix, g ' is the gravity vector, B ' is the work selection matrix, G ^T' _C is the Jacobian between the wheel and the ball, G ^T' _N is the Jacobian between the ball and the ground.

또한, [수학식 1]에서 구속조건은,

및

식을 포함할 수 있고,

는 휠과 볼 사이의 접촉점 속도, J _CB는 로봇과 접촉점 사이의 자코비안,

는 로봇의 일반화 좌표계의 속도, G _CK는 볼과 접촉점 사이의 자코비안,

는 볼의 일반화 좌표계의 속도,

은, G _NK는 볼과 지면 사이의 자코비안이다. In addition, in [Equation 1], the constraint is,

and

can contain an expression,

is the velocity of the contact point between the wheel and the ball, J _CB is the Jacobian between the robot and the contact point,

is the velocity of the generalized coordinate system of the robot, G _CK is the Jacobian between the ball and the contact point,

is the velocity of the generalized coordinate system of the ball,

Silver, G _NK is the Jacobian between the ball and the ground.

볼, 로봇 및 지면에 대한 제1운동방정식으로부터 지면접촉 영공간 행렬을 이용하여 제2운동 방정식을 도출해내는 단계(S20)에서, 제2운동 방정식은 볼봇이 지면과 접촉하는 영공간을 나타내는 투영 운동 방정식으로 이해될 수 있다. In the step of deriving the second equation of motion using the ground contact null space matrix from the first equation of motion for the ball, the robot and the ground (S20), the second equation of motion is the projection motion representing the null space in which the ball robot contacts the ground can be understood as an equation.

볼과 지면 사이의 정보를 측정할 수 없으므로 제2운동방정식의 도출에 의해 볼과 지면 사이의 접촉 정보와 무관한 운동 방정식을 얻을 수 있다. 제2운동방정식은 볼과 지면 사이의 공간에 대해 독립적인 운동방정식으로, 볼과 지면 사이의 접촉힘 관련 정보들을 포함하지 않는다. Since the information between the ball and the ground cannot be measured, an equation of motion unrelated to the contact information between the ball and the ground can be obtained by deriving the second equation of motion. The second equation of motion is an independent equation of motion for the space between the ball and the ground, and does not include information related to the contact force between the ball and the ground.

또한, 제2운동방정식은 [수학식 2]에 의해 도출될 수 있다.In addition, the second equation of motion can be derived by [Equation 2].

[수학식 2][Equation 2]

[수학식 2]에서,

는 지면접촉 영공간으로 투영된 관성을 나타내는 행렬,

는

은 일반화 좌표계의 속도,

은 일반화 좌표계의 가속도,

는 지면접촉 영공간으로 투영된 중력 벡터,

는 지면접촉 영공간으로 투영된 선택 행렬,

은 휠과 볼 사이의 접촉힘을 나타낸다. In [Equation 2],

Is

is the velocity of the generalized coordinate system,

is the acceleration of the generalized coordinate system,

is the gravity vector projected into the ground contact null space,

is the selection matrix projected into the ground contact null space,

represents the contact force between the wheel and the ball.

또한, [수학식 2]에서,

은

로 나타낼 수 있는데, P _N은 지면접촉 영공간 행렬로서

이고, I는 단위 행렬,

은 볼과 지면 사이의 자코비안의 유사역행렬,

은 볼과 지면 사이의 자코비안,

은 관성행렬,

은

로 나타낼 수 있는데, P _N은 전술한 지면접촉 영공간 행렬,

은 코리올리 힘 및 원심력 행렬,

은

로 나타낼 수 있는데, P _N은 전술한 지면접촉 영공간 행렬, g'는 중력 벡터,

는

로 나타낼 수 있는데, P _N은 전술한 지면접촉 영공간 행렬, B'는 작업 선택 행렬,

는

로 나타낼 수 있고, P _N은 전술한 지면접촉 영공간 행렬, G ^T' _C는 볼과 접촉점 사이의 자코비안의 전치행렬이다.Also, in [Equation 2],

silver

, where P _N is a ground contact null space matrix

, and I is the identity matrix,

is the pseudoinverse Jacobian matrix between the ball and the ground,

The Jacobian between the silver ball and the ground,

is the inertia matrix,

silver

can be expressed as P _N is the aforementioned ground contact null space matrix,

The Coriolis force and centrifugal force matrix,

silver

, where P _N is the ground contact null space matrix, g 'is the gravity vector,

Is

, where P _N is the aforementioned ground contact null space matrix, B 'is the operation selection matrix,

Is

, where P _N is the above-mentioned ground contact null space matrix, and G ^T' _C is a Jacobian transposition matrix between the ball and the contact point.

한편, [수학식 2]에서 구속조건은,

로 표현될 수 있고,

는 볼의 일반화 좌표계의 속도이다. On the other hand, in [Equation 2], the constraint is,

can be expressed as

is the velocity of the generalized coordinate system of the ball.

볼과 로봇 각각의 속도에 근거하여 작업 변수를 설정하는 단계(S30)에서, 작업 변수의 설정은 [수학식 3]에 의해 이루어질 수 있다. In the step of setting the work variable based on the speed of each ball and the robot (S30), the work variable may be set by [Equation 3].

[수학식 3][Equation 3]

[수학식 3]에서,

는 작업 변수,

은 로봇의 무게 중심에서의 x방향으로의 속도,

은 로봇의 무게 중심에서의 y방향으로의 속도,

은 공의 중심에서의 y방향으로의 속도,

은 로봇의 무게중심 작업 공간 행렬,

은 볼 작업 공간 행렬,

은 일반화 좌표계의 속도, T

에서 T는 작업 공간 행렬이다. In [Equation 3],

is the working variable,

is the velocity in the x direction from the center of gravity of the robot,

is the velocity in the y direction from the center of gravity of the robot,

is the velocity in the y direction from the center of the ball,

is the barycentric workspace matrix of the robot,

is the ball workspace matrix,

is the velocity of the generalized coordinate system, T

where T is the working space matrix.

또한, [수학식 3]에서,

는 작업 변수로서, 로봇의 무게중심과 공의 중심에서의 x 및 y 방향으로의 속도를 포함하는 개념으로 4차원(R⁴)인 작업 공간을 포함할 수 있다. Also, in [Equation 3],

As a work variable, is a concept including the center of gravity of the robot and the velocity in the x and y directions from the center of the ball, and may include a 4-dimensional (R ⁴ ) work space.

상기 작업 변수 및 제2운동방정식에 근거하여, 제2운동방정식이 작업 변수에 의해 변환된 방정식인 제3운동방정식을 도출하는 단계(S40)에서, 제3운동방정식은 작업 공간으로 투영된 운동방정식으로 이해될 수 있다. Based on the work variables and the second equation of motion, in the step (S40) of deriving a third equation of motion, which is an equation in which the second equation of motion is converted by the work variable, the third equation of motion is the projected equation of motion into the work space. can be understood as

제3운동방정식의 도출함으로써, 볼-지면 접촉 정보 없는 공간에서 작업 공간을 구성하여 로봇과 볼의 접촉 정보만으로 로봇과 볼의 운동 방정식을 얻을 수 있다. By deriving the third equation of motion, it is possible to construct a workspace in a space without ball-ground contact information and obtain the motion equation of the robot and the ball only with the contact information between the robot and the ball.

제3운동방정식의 도출은, 로봇과 볼의 자유 운동을 위한 작업 공간을 설정하고, 지면접촉 영공간으로 투영된 운동 방정식을 작업 공간으로 투영하여 작업공간 운동방정식으로 변환한다. In order to derive the third equation of motion, a work space for free motion of the robot and the ball is set, and the equation of motion projected into the ground contact null space is projected into the work space to be converted into a work space equation of motion.

또한, 제3운동방정식은 로봇과 볼 사이의 접촉힘 등의 구속 조건을 포함하는 운동방정식으로 이해될 수 있다. In addition, the third equation of motion may be understood as an equation of motion including constraint conditions such as contact force between the robot and the ball.

일례로, 제3운동방정식을 도출하는 단계(S40)에서, [수학식 3]에 의해 설정된 작업 변수 및 제2운동방정식에 근거하여, 제3운동방정식은 [수학식 4]에 의해 표현될 수 있다. For example, in the step of deriving the third equation of motion (S40), based on the work variable and the second equation of motion set by [Equation 3], the third equation of motion can be expressed by [Equation 4] there is.

[수학식 4][Equation 4]

[수학식 4]에서,

은 영공간에서 작업공간으로 투영된 볼 관련 관성행렬,

는 영공간에서 작업공간으로 투영된 볼 관련 중력 벡터,

는 로봇의 관절 토크,

은 휠과 볼 사이의 접촉힘을 나타낸다.In [Equation 4],

is the ball-related gravity vector projected from null space to workspace,

is the joint torque of the robot,

represents the contact force between the wheel and the ball.

또한, 제3운동방정식을 도출하기 위해, [보조식 1]이 이용될 수 있다.In addition, [Supplementary Equation 1] may be used to derive the third equation of motion.

[보조식 1][Supplementary Formula 1]

[보조식 1]은 [수학식 2]의 좌변에

을 곱하고, [수학식 3]에서,

및

을

및

에 대한 식으로 변환한 식이고,

는 볼-작업 공간 행렬의 역전치행렬,

는 볼-작업 공간 행렬의 유사역행렬,

는

이고,

는 영공간에서 작업공간으로 투영된 관성행렬,

는 C이고, C는 영공간에서 작업공간으로 투영된 코리올리 힘 및 원심력 행렬이고,

는 g이고, g는 중력 벡터이고,

는 B이고, B는 영공간에서 작업공간으로 투영된 선택 행렬이고,

는 G ^T _C이고, G ^T _C는 휠과 볼 사이의 자코비안이다. [Supplementary Equation 1] is on the left side of [Equation 2]

Multiply by , and in [Equation 3],

and

second

and

It is an expression converted into an expression for ,

is the inverse transpose matrix of the ball-workspace matrix,

is the pseudoinverse matrix of the ball-workspace matrix,

Is

ego,

is the inertia matrix projected from the null space to the workspace,

is C , C is the Coriolis force and centrifugal force matrix projected from null space to workspace,

is g , g is the gravity vector,

is B , where B is the selection matrix projected from the null space to the workspace,

is G ^T _C , where G ^T _C is the Jacobian between the wheel and the ball.

본 발명에서, 볼과 로봇 사이의 접촉힘은 [수학식 5]에 의해 표현될 수 있다.In the present invention, the contact force between the ball and the robot can be expressed by [Equation 5].

[수학식 5][Equation 5]

[수학식 5]에서,

는 영공간에서 작업공간으로 투영된 볼에 가해지는 접촉힘,

는 영공간에서 작업공간으로 투영된 볼관련 관성행렬,

은 휠과 볼사이의 접촉힘이다.In [Equation 5],

is the contact force between the wheel and the ball.

[수학식 5]의 도출을 위해 이차프로그래밍이 사용될 수 있으며, Quadratic Programming은 이차프로그래밍의 일례로 이해될 수 있다. Quadratic programming can be used to derive [Equation 5], and quadratic programming can be understood as an example of quadratic programming.

[수학식 5]의 도출을 위해 이차프로그래밍이 사용됨으로써, 구속 조건을 만족하는 로봇과 볼 사이의 접촉힘을 구할 수 있다. By using secondary programming to derive [Equation 5], the contact force between the robot and the ball that satisfies the constraint conditions can be obtained.

전술한 제3방정식은, 로봇 관련 운동 방정식 파트({B})와 볼 관련 운동 방정식 파트({K})로 나뉘는데, 이차프로그래밍에 의해, 볼 관련 운동 방정식 파트를 만족하는 동시에, 구속 조건들을 만족하는 로봇과 볼 사이의 접촉힘이 계산된다.The above-described third equation is divided into a robot-related motion equation part ({B}) and a ball-related motion equation part ({K}). Through secondary programming, it satisfies the ball-related motion equation part and at the same time satisfies constraint conditions. The contact force between the robot and the ball is calculated.

제3운동방정식으로부터 볼과 로봇 사이의 접촉힘 및 제어 토크 중 적어도 하나를 제어하는 단계(S50)에서, 이차프로그래밍에 의해 계산된 접촉힘을 포함하여 일반화 제어 입력 토크를 구하고 시뮬레이터를 통해 구현 및 결과가 확인된다.In the step of controlling at least one of the contact force and control torque between the ball and the robot from the third equation of motion (S50), a generalized control input torque is obtained including the contact force calculated by secondary programming and implemented and results through a simulator is confirmed

[수학식 5]의 도출을 위해, [보조식 2] 내지 [보조식 6]이 이용된다. For the derivation of [Equation 5], [Supplementary Equation 2] to [Supplementary Equation 6] are used.

[보조식 2][Supplementary Expression 2]

[보조식 3][Supplementary Expression 3]

[보조식 4][Supplementary Expression 4]

[보조식 5][Supplementary Expression 5]

[보조식 6][Supplementary Expression 6]

[보조식 2] 내지 [보조식 6]에서,

는 볼-접촉 힘 오차 최소화 항목이고,

는 접촉힘 최소화 항목이며,

는 영공간에서 작업공간으로 투영된 볼 힘,

은 휠과 볼 사이의 접촉힘,

은 x축 방향의 i번째 접촉힘,

은 y축 방향의 i번째 접촉힘,

은 z축 방향의 i번째 접촉힘,

은 설정된 i번째 접촉힘의 최소값이고, [보조식 5]의 우변은 마찰력 근사치이고, [보조식 6]은 옴니휠 구속조건을 나타낸다.In [Supplemental Formula 2] to [Supplemental Formula 6],

is the ball-contact force error minimization item,

is the contact force minimization item,

is the ball force projected from the null space to the workspace,

is the contact force between the wheel and the ball,

is the ith contact force in the x-axis direction,

is the ith contact force in the y-axis direction,

is the ith contact force in the z-axis direction,

is the minimum value of the set ith contact force, the right side of [Supplementary Equation 5] is an approximate value of frictional force, and [Supplementary Equation 6] represents the omni-wheel restraint condition.

제3운동방정식으로부터 볼과 로봇 사이의 접촉힘 및 제어 토크 중 적어도 하나를 제어하는 단계(S50)에서, 전술한 [수학식 4]에 의해 표현된, 작업 변수에 근거한 제3운동방정식을 이용하여, 제어 토크가 [수학식 6]에 의해 표현될 수 있다.In the step of controlling at least one of the contact force and control torque between the ball and the robot from the third equation of motion (S50), using the third equation of motion based on the work variable, expressed by the above-described [Equation 4], , the control torque can be expressed by [Equation 6].

[수학식 6][Equation 6]

[수학식 6]에서,

는 로봇의 관절 토크,

은 레퍼런스 작업 가속도이고,

이고,

은 로봇 작업 레퍼런스 가속도,

은 볼 작업 레퍼런스 가속도,

는 스케일링 이득,

는 작업 속도,

is the joint torque of the robot,

is the reference work acceleration,

ego,

is the robot work reference acceleration,

is the ball work reference acceleration,

is the scaling gain,

is the work rate,

is the contact force calculated from Quadratic Programming (QP).

[수학식 6]에서,

은 하강 모멘트(falling moment)인

을 이용한 최적화 값(cost)

을 이용해 구비강화법(gradient descent method)으로부터 구해진 모멘트 중심 반사 운동(Com reflex)을 통해 얻어질 수 있고,

은 xy 평면에서 볼 위치에서 로봇의 무게 중심까지의 벡터이다. In [Equation 6],

is the falling moment

Optimization value (cost) using

It can be obtained through the moment centered reflex (Com reflex) obtained from the gradient descent method using

is the vector from the ball position to the center of gravity of the robot in the xy plane.

또한, [수학식 6]은 [보조식 7]에 근거하여 도출될 수 있다.Also, [Equation 6] can be derived based on [Supplementary Equation 7].

[보조식 7][Supplementary Expression 7]

[보조식 7]에서,

은 레퍼런스 작업 가속도이고,

는 작업 가속도 목표값이고, K _p는 비례 이득이고,

는 작업 위치 목표값이고, x는 작업 위치이고, K _p는 감쇠 이득이고,

는 작업 속도 목표값이고

는 작업 속도이다. In [Supplementary Formula 7],

is the reference work acceleration,

Is the work acceleration target value, K _p is the proportional gain,

is the work position target value, x is the work position, K _p is the damping gain,

is the work rate target value, and

is the work rate.

또한, 영공간 힘을 고려한, 일반화 제어 토크가 [수학식 7]에 의해 표현된다. 일반화 제어 토크는 일반화 좌표계의 제어에 이용된다.In addition, the generalized control torque considering the null space force is expressed by [Equation 7]. The generalized control torque is used to control the generalized coordinate system.

[수학식 7][Equation 7]

[수학식 7]에서,

은 레퍼런스 작업 가속도이고,

는 작업 속도,

은 Quadratic Programming(QP)로부터 계산된 접촉힘,

는 영공간 작업힘으로서, f ₀의 로봇 부분의 힘을 나타내며, 작업 공간에 독립적인 힘으로 로봇의 기울기를 제어하기 위해 사용된다. In [Equation 7],

is the reference work acceleration,

is the work rate,

is the contact force calculated from Quadratic Programming (QP),

is the null space work force, which represents the force of the robot part of f ₀ , and is used to control the inclination of the robot as a force independent of the work space.

또한, [수학식 7]은 [보조식 8]에 근거하여 도출될 수 있다.Also, [Equation 7] can be derived based on [Supplementary Equation 8].

[보조식 8][Supplementary Expression 8]

[보조식 8]에서, f ₀는 작업 영공간 힘이고, P _T는 작업 행렬의 영공간 투영자이고,

이고, K ₀는 포텐셜 이득이고,

는 볼-작업 공간 행렬의 역전치행렬이고,

는 일반화 좌표계 함수로서,

이고,

는

는 목표 일반화 좌표계,

은 영공간의 힘이다.In [Supplementary Equation 8], f ₀ is the working null space force, P _T is the null space projector of the working matrix,

, K ₀ is the potential gain,

is the inverse transpose matrix of the ball-workspace matrix,

is a generalized coordinate system function,

ego,

Is

is the target generalized coordinate system,

is the power of the null space.

한편, [보조식 8]에서, 영공간 제어를 위한 일반화 좌표계의 포텐셜 레퍼런스

는,

을 제외하고는, 모든 i에 대해 ξ _i,d = 0 이고

이며_,

는 관절 선택 변수이다.On the other hand, in [Supplementary Equation 8], the potential reference of the generalized coordinate system for null space control

Is,

ξ _i,d for all i, except for= 0 and

_and

is a joint selection variable.

도 3은 본 발명의 볼봇의 자세 균형 제어 방법(S100)을 수행하는 볼봇의 자세 균형 제어 시스템(100)을 도시하는 블록도이다.3 is a block diagram showing a posture balance control system 100 of a ball-bot performing the posture balance control method (S100) of the present invention.

본 발명의 볼봇의 자세 균형 제어 시스템(100)은 목표치 생성부(10), 제어부(20), 볼봇(30) 및 산출부(40)를 포함한다. The posture balance control system 100 of a robot according to the present invention includes a target value generator 10, a controller 20, a robot 30, and a calculation unit 40.

도 3을 참조하여 이하, 볼봇의 자세 균형 제어 시스템(100)에 대하여 서술한다. Referring to FIG. 3 , the posture balance control system 100 of the robot will be described below.

목표치 생성부(10)는, 도 3의 제어기(22)의 좌측에 배치된 예가 도시된다. An example in which the target value generation unit 10 is disposed on the left side of the controller 22 in FIG. 3 is shown.

목표치 생성부(10)는, 정의된 작업들을 이용해 레퍼런스 값을 생성한다. The target value generation unit 10 generates reference values using defined tasks.

목표치 생성부(10)는, 기준 계획부(11), 무게 중심 작업 생성부(13) 및 볼 작업 생성부(15)를 포함할 수 있다. The target value generating unit 10 may include a standard planning unit 11 , a center of gravity work generating unit 13 , and a ball work generating unit 15 .

기준 계획부(11)는 로봇의 무게 중심에서의 x 방향과 y 방향의 기준 변위값(x _G,ref , y _G,ref)을 제공받아서 로봇의 무게 중심에서의 x 방향과 y 방향의 목표 변위값(x _G,d , y _G,d)과 공의 중심에서의 x 방향과 y 방향의 목표 변위값(x _K,d , y _K,d)을 생성한다. The reference planning unit 11 receives the reference displacement values ( x _G,ref , y _G,ref ) in the x and y directions from the center of gravity of the robot, and calculates the target displacement in the x and y directions from the center of gravity of the robot values ( x _G,d , y _G,d ) and target displacement values ( x _K,d , y _K,d ) in the x and y directions from the center of the ball are created.

무게 중심 작업 생성부(13)는 기준 계획부(11)로부터 로봇의 무게 중심에서의 x 방향과 y 방향의 목표 변위값(x _G,d, y _G,d)을 제공받아서 로봇의 무게 중심에서의 x 방향과 y 방향의 목표힘(f _Gx,d, f _Gy,d)을 생성하여 제어부(20)로 제공한다. The center of gravity task generation unit 13 receives target displacement values ( x _G,d , y _G,d ) in the x and y directions from the center of gravity of the robot from the reference planning unit 11, and Target forces ( f _Gx,d , f _Gy,d ) in the x and y directions of are generated and provided to the control unit 20 .

볼 작업 생성부(15)는 기준 계획부(11)로부터 공의 중심에서의 x 방향과 y 방향의 목표 변위값(x _K,d, y _K,d)을 제공받아서, 볼의 중심에서의 x 방향과 y 방향의 목표힘(f _Kx,d, f _Ky,d)을 생성하여 제어부(20)로 제공한다.The ball work generating unit 15 receives target displacement values ( x _K,d , y _K,d ) in the x and y directions at the center of the ball from the reference planning unit 11, and calculates x at the center of the ball. Target forces ( f _Kx,d , f _Ky,d ) in the direction and y direction are generated and provided to the control unit 20 .

제어부(20)는 목표치 생성부(10)로부터 로봇의 무게 중심과 볼의 중심 각각에서의 x 방향과 y 방향의 목표힘(f _Gx,d, f _Gy,d , f _Kx,d, f _Ky,d)을 제공받아서 제어 입력과 접촉힘을 계산한다. The control unit 20 calculates target forces ( f _Gx,d , f _{Gy,d, f Kx,d, f Ky, f Gx,d, f Gy,d} , f _Kx,d , f _{Ky, d} ) is provided and the control input and contact force are calculated.

제어부(20)는 제어 입력을 계산하는 제어기(22) 및 접촉힘을 계산하는 접촉힘산출부(25)를 포함할 수 있다. 접촉힘산출부(25)는 일례로, Quadratic Programming(QP)을 수행하도록 이루어질 수 있다. The controller 20 may include a controller 22 that calculates a control input and a contact force calculator 25 that calculates a contact force. The contact force calculation unit 25 may, for example, perform Quadratic Programming (QP).

접촉힘산출부(25)에 의해 계산된 접촉힘(f ^QP _C)은 제어기(22)로 제공되어 로봇의 관절 토크(

)를 도출한다.The contact force f ^QP _C calculated by the contact force calculation unit 25 is provided to the controller 22 and the joint torque of the robot (

) is derived.

한편, 접촉힘산출부(25)는 후술하는 산출부(40)로부터 산출된 값들(로봇의 질량중심위치값(p _G ), 몸통회전 목표값(R _B), 관절변위(q), 일반화 좌표계(ξ), 작업공간에서의 운동 자코비안(J), 볼-작업 공간 행렬(T), 질량 및 관성모멘트(M), 코리올리의 힘 및 원심력(C), 중력값(g))을 제공받을 수 있다. On the other hand, the contact force calculation unit 25 calculates the values calculated from the calculation unit 40 to be described later (the robot's center of mass position value (p _G ), the body rotation target value ( RB ), the joint displacement ( _q ), and the generalized coordinate system). ( ξ ), the Jacobian of motion in the workspace ( J ), the ball-workspace matrix ( T ), the mass and moment of inertia ( M ), the Coriolis force and centrifugal force ( C ), and the gravity value ( g )). can

목표치 생성부(10) 및 산출부(40)로부터 얻어진 변수들을 이용해 이차 프로그래밍 기법으로 접촉힘을 구하고 제어 입력을 계산함으로써, 볼봇(30)의 자세 균형을 제어한다. The posture balance of the robot 30 is controlled by obtaining the contact force and calculating the control input using the secondary programming technique using the variables obtained from the target value generating unit 10 and the calculating unit 40 .

도 2를 참조하면, 볼봇(30)은, 지면에 놓여지는 볼(31)과, 볼(31)에 접촉하는 로봇(35)을 포함할 수 있다. 또한, 로봇(35)은, 볼(31)과 접촉하는 휠(wheel, 36)을 구비할 수 있다. 휠(36)은 일례로, 옴니휠(omnidirectional wheel)일 수 있다. 볼봇(30)은 외부힘에 의해 운동한다.Referring to FIG. 2 , the ballbot 30 may include a ball 31 placed on the ground and a robot 35 contacting the ball 31 . In addition, the robot 35 may have a wheel 36 in contact with the ball 31 . The wheel 36 may be, for example, an omnidirectional wheel. The ballbot 30 is moved by an external force.

도 3에 도시된 바와 같이, 볼봇(30)은 제어부(20) 및 산출부(40)와 전기적으로 연결될 수 있다.As shown in FIG. 3 , the ballbot 30 may be electrically connected to the control unit 20 and the calculation unit 40 .

볼봇(30)에는 외부힘(f _c)과 제어부(20)로부터 도출된 로봇의 관절 토크(

)가 입력되어 질량중심위치값(p _G ), 몸통회전 목표값(R _B), 관절변위(q), 로봇의 일반화 좌표계(ξ _B)를 산출하여 산출부(40)로 제공한다. In the ballbot 30, the joint torque of the robot derived from the external force ( f _c ) and the control unit 20 (

) is input, and the center of mass position value (p _G ), trunk rotation target value ( _RB ), joint displacement (q), and generalized coordinate system ( ξ B ) of the _robot are calculated and provided to the calculation unit 40 .

산출부(40)는 볼봇(30)으로부터 얻어진 상태 변수들(질량중심위치값(p _G ), 몸통회전 목표값(R _B), 관절변위(q), 로봇의 일반화 좌표계(ξ _B))을 이용하여 여러 운동 방정식들을 계산하고 이를 이용해 작업 공간 변수들을 계산하여, 계산된 변수들(로봇의 질량중심위치값(p _G ), 몸통회전 목표값(R _B), 관절변위(q), 일반화 좌표계(ξ), 작업공간에서의 운동 자코비안(J), 볼-작업 공간 행렬(T), 질량 및 관성모멘트(M), 코리올리의 힘 및 원심력(C), 중력값(g), 작업 공간 T 위치(x _T), 작업 공간 T 속도(

_T), 로봇의 코리올리의 힘 및 원심력(

), 코리올리의 힘 및 원심력(C), 중력값(g))을 제어부(20)로 제공하도록 한다. The calculation unit 40 calculates the state variables obtained from the ballbot 30 (center of mass position value (p _G ), trunk rotation target value ( _RB ), joint displacement (q), generalized coordinate system ( ξ B ) of the _robot ) By calculating various equations of motion and using them to calculate workspace variables, the calculated variables (robot center of mass position value (p _G ), body rotation target value ( R _B ), joint displacement (q), generalized coordinate system ( ξ ), motion Jacobian in workspace ( J ), ball-workspace matrix ( T ), mass and moment of inertia ( M ), Coriolis force and centrifugal force ( C ), gravity value ( g ), workspace T position ( x _T ), workspace T velocity (

_T ), Coriolis force and centrifugal force of the robot (

), Coriolis force, centrifugal force ( C ), and gravity value ( g )) are provided to the controller 20.

산출부(40)는 시스템 운동방정식과 지면접촉 영공간 운동방정식을 계산할 수 있으며, 작업공간 운동방정식을 또한 계산할 수 있다. The calculation unit 40 may calculate a system motion equation and a ground contact null space motion equation, and may also calculate a workspace equation of motion.

본 발명의 볼봇의 자세 균형 제어 방법(S100) 및 볼봇의 자세 균형 제어 시스템(100)은, Quadratic Programming(QP)을 이용한 균형 제어가 적용될 수 있으며, 볼과 지면의 공간의 영공간에서의 작업을 기반으로 수행될 수 있다. The ballbot posture balance control method (S100) and the ballbot posture balance control system 100 of the present invention can apply balance control using Quadratic Programming (QP), and work in the null space of the space between the ball and the ground can be performed based on

또한, 본 발명의 볼봇의 자세 균형 제어 방법(S100) 및 볼봇의 자세 균형 제어 시스템(100)은, 영공간으로의 투영을 통해 볼과 지면의 접촉에 대한 정보 없이 로봇 시스템 운동방정식을 다룰 수 있다. In addition, the ball-bot posture balance control method (S100) and the ball-bot posture balance control system 100 of the present invention can handle the equation of motion of the robot system without information on contact between the ball and the ground through projection into a null space. .

또한, 본 발명의 볼봇의 자세 균형 제어 방법(S100) 및 볼봇의 자세 균형 제어 시스템(100)은, 지면 접촉에 대한 정보 없이 제어가 요구되는 모바일 로봇 등의 분야에도 적용될 수 있다.In addition, the posture balance control method of the robot (S100) and the posture balance control system 100 of the robot according to the present invention can be applied to fields such as mobile robots requiring control without ground contact information.

이상에서 설명한 볼봇의 자세 균형 제어 방법(S100) 및 볼봇의 자세 균형 제어 시스템(100)은 위에서 설명된 실시예들의 구성과 방법에 한정되는 것이 아니라, 실시예들은 다양한 변형이 이루어질 수 있도록 각 실시예들의 전부 또는 일부가 선택적으로 조합되어 구성될 수도 있다. The posture balance control method (S100) and the posture balance control system 100 of the ballbot described above are not limited to the configuration and method of the above-described embodiments, but the embodiments are variously modified so that each embodiment All or part of these may be selectively combined.

본 발명은 본 발명의 정신 및 필수적 특징을 벗어나지 않는 범위에서 다른 특정한 형태로 구체화될 수 있음은 당업자에게 자명하다. 따라서, 상기의 상세한 설명은 모든 면에서 제한적으로 해석되어서는 아니되고 예시적인 것으로 고려되어야 한다. 본 발명의 범위는 첨부된 청구항의 합리적 해석에 의해 결정되어야 하고, 본 발명의 등가적 범위 내에서의 모든 변경은 본 발명의 범위에 포함된다.It is apparent to those skilled in the art that the present invention can be embodied in other specific forms without departing from the spirit and essential characteristics of the present invention. Accordingly, the above detailed description should not be construed as limiting in all respects and should be considered illustrative. The scope of the present invention should be determined by reasonable interpretation of the appended claims, and all changes within the equivalent scope of the present invention are included in the scope of the present invention.

S100:볼봇의 자세 균형 제어 방법
S10:볼, 로봇 및 지면에 대한 제1운동방정식을 구성하는 단계
S20:제1운동방정식으로부터 지면접촉 영공간 행렬을 이용하여 제2운동방정식을 도출하는 단계
S30:볼과 로봇 각각의 속도에 근거하여 작업 변수를 설정하는 단계
S40:상기 작업 변수 및 제2운동방정식에 근거하여, 제2운동방정식이 작업 변수에 의해 변환된 방정식인 제3운동방정식을 도출하는 단계
S50:상기 제3운동방정식으로부터 볼과 로봇 사이의 접촉힘 및 제어 토크 중 적어도 하나를 제어하는 단계
100:볼봇의 자세 균형 제어 시스템
10:목표치 생성부 11:기준 계획부 13:무게 중심 작업 생성부 15:볼 작업 생성부
20:제어부 22:제어기 25:접촉힘산출부
30:볼봇 31:볼 35:로봇 36:휠
40:산출부S100: How to control the posture balance of a ballbot
S10: Step of constructing the first equation of motion for the ball, robot and ground
S20: Deriving a second equation of motion from the first equation of motion using a ground contact null space matrix
S30: Step of setting work variables based on the speed of each ball and robot
S40: Deriving a third equation of motion, which is an equation in which the second equation of motion is converted by the working variable, based on the working variable and the second equation of motion.
S50: Controlling at least one of the contact force and control torque between the ball and the robot from the third equation of motion
100: Ballbot's posture balance control system
Reference Numerals 10: target value generation unit 11: standard planning unit 13: center of gravity work generation unit 15: ball operation generation unit
20: control unit 22: controller 25: contact force calculation unit
30: ball bot 31: ball 35: robot 36: wheel
40: calculation unit

Claims

As a method for controlling the posture balance of a ball robot having a ball and a robot,
constructing first equations of motion for the ball, the robot, and the ground;
deriving a second equation of motion from the first equation of motion by using a ground contact null space matrix;
Setting a work variable based on the speed of each ball and robot;
Deriving a third equation, which is an equation in which the second equation of motion is converted by the working variable, based on the working variable and the second equation of motion; and
Controlling at least one of a contact force and a control torque calculated by quadratic programming between the ball and the robot from the third equation,
The first equation of motion is defined by [Equation 1].
[Equation 1]

In [Equation 1], M ' _B is the inertia matrix of the robot, M ' _K is the inertia matrix of the ball,

Is

The generalized coordinate system represented by ξ _B ^T is the transposed matrix representing the generalized coordinate system of the robot, ξ _K ^T is the transposed matrix representing the generalized coordinate system of the ball,

is the velocity of the generalized coordinate system,

delete

According to claim 1,
The posture balance control method of the ballbot, characterized in that the second equation of motion is derived by [Equation 2].
[Equation 2]

In [Equation 2],

Is

is the velocity of the generalized coordinate system,

is the acceleration of the generalized coordinate system,

is the gravity vector projected into the ground contact null space,

is the selection matrix projected into the ground contact null space,

represents the contact force between the wheel and the ball.

According to claim 3,
The posture balance control method of the ballbot, characterized in that the setting of the work variable is performed by [Equation 3].
[Equation 3]

In [Equation 3],

is the working variable,

is the velocity in the x direction from the center of gravity of the robot,

is the velocity in the y direction from the center of gravity of the robot,

is the velocity in the y direction from the center of the ball,

is the barycentric workspace matrix of the robot,

is the ball workspace matrix,

is the velocity of the generalized coordinate system, T

where T is the working space matrix.

According to claim 4,
The posture balance control method of the ballbot, characterized in that the third equation is expressed by [Equation 4].
[Equation 4]

In [Equation 4],

is the ball-related gravity vector projected from null space to workspace,

is a selection matrix related to the center of gravity of the robot projected from the null space to the workspace,

is the joint torque of the robot,

represents the contact force between the wheel and the ball.

According to claim 5,
The posture balance control method of the robot, characterized in that the contact force is expressed by [Equation 5].
[Equation 5]

In [Equation 5],

is the contact force between the wheel and the ball.

According to claim 6,
The control torque is expressed by [Equation 6].
[Equation 6]

In [Equation 6],

is the joint torque of the robot,

is the reference work acceleration,

ego,

is the robot work reference acceleration,

is the ball work reference acceleration,

is the scaling gain,

is the work rate,

is the contact force calculated from Quadratic Programming (QP).

A posture balance control system for a robot using the posture balance control method of any one of claims 1 and 3 to 7,
a target value generating unit generating a reference value using defined tasks;
a controller that calculates a control input and a contact force by receiving the reference value of the robot generated from the target value generator;
A ball robot that includes a ball placed on the ground and a robot that contacts the ball and moves by an external force; and
A posture balance control system for a ball-bot, comprising a calculation unit for calculating variables obtained from the movement of the ball-bot and providing the calculated variables to a control unit.